diff --git a/core/include/moveit/task_constructor/stages/fix_collision_objects.h b/core/include/moveit/task_constructor/stages/fix_collision_objects.h new file mode 100644 index 00000000..42bae0ea --- /dev/null +++ b/core/include/moveit/task_constructor/stages/fix_collision_objects.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +namespace moveit { namespace planning_interface { +MOVEIT_CLASS_FORWARD(MoveGroupInterface) +} } + +namespace moveit { namespace task_constructor { namespace stages { + +class FixCollisionObjects : public PropagatingEitherWay { +public: + FixCollisionObjects(std::string name = "fix collisions of objects"); + + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + void setDistanceThreshold(double penetration); + +protected: + // apply stored modifications to scene + void moveCollisionObject(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object, const std::string& base_frame); + void checkCollisions(planning_scene::PlanningScene &scene, const std::string& object); + planning_scene::PlanningScenePtr apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert) const; + +private: + double penetration_; + +}; + +} } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index b2cd5d8c..292bb362 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/gripper.h ${PROJECT_INCLUDE}/stages/modify_planning_scene.h + ${PROJECT_INCLUDE}/stages/fix_collision_objects.h ${PROJECT_INCLUDE}/stages/move.h cartesian_position_motion.cpp @@ -13,6 +14,7 @@ add_library(${PROJECT_NAME}_stages generate_grasp_pose.cpp gripper.cpp modify_planning_scene.cpp + fix_collision_objects.cpp move.cpp ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp new file mode 100644 index 00000000..e06816d4 --- /dev/null +++ b/core/src/stages/fix_collision_objects.cpp @@ -0,0 +1,89 @@ +#include + +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +FixCollisionObjects::FixCollisionObjects(std::string name) + : PropagatingEitherWay(name) +{} + +bool FixCollisionObjects::computeForward(const InterfaceState &from){ + planning_scene::PlanningScenePtr to = apply(from.scene(), false); + sendForward(from, InterfaceState(to), robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +bool FixCollisionObjects::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from = apply(to.scene(), true); + sendBackward(InterfaceState(from), to, robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +void FixCollisionObjects::setDistanceThreshold(double penetration) +{ + penetration_ = penetration; +} + +void FixCollisionObjects::checkCollisions(planning_scene::PlanningScene &scene, const std::string& object) +{ + collision_detection::AllowedCollisionMatrix acm = scene.getAllowedCollisionMatrix(); + + collision_detection::CollisionRequest collision_request; + collision_detection::CollisionResult collision_result; + collision_request.group_name = object; + collision_request.contacts = true; + collision_request.max_contacts = 100; + collision_request.max_contacts_per_pair = 5; + collision_request.verbose = false; + collision_request.distance = true; + + scene.checkCollision(collision_request, collision_result, scene.getCurrentState()); + + geometry_msgs::Pose pose; + + if(collision_result.collision) + { + std::cout<<"COLLIDING"<second.begin()->depth < penetration_) + { + pose.position.x = 0.3 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[0]); + pose.position.y = 0.23 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[2]); + pose.position.z = -0.02 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[1]); + + //correct collision + moveCollisionObject(scene, pose, object, "world"); + } + } + else + std::cout<<"No Collision!!!!!!!!!!!!!"<diff(); + + //Check collision of objects on the table in planning scene + (const_cast(this))->checkCollisions(*result, "object"); + + return result; +} + +} } }