mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
FixCollisionObjects stage
This commit is contained in:
parent
f006f0f3d6
commit
f35a6bc05d
@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <geometry_msgs/Pose.h>
|
||||
|
||||
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_;
|
||||
|
||||
};
|
||||
|
||||
} } }
|
||||
@ -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})
|
||||
|
||||
89
core/src/stages/fix_collision_objects.cpp
Normal file
89
core/src/stages/fix_collision_objects.cpp
Normal file
@ -0,0 +1,89 @@
|
||||
#include <moveit/task_constructor/stages/fix_collision_objects.h>
|
||||
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
|
||||
|
||||
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"<<std::endl;
|
||||
if(collision_result.contacts.begin()->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!!!!!!!!!!!!!"<<std::endl;
|
||||
}
|
||||
|
||||
void FixCollisionObjects::moveCollisionObject(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object, const std::string& base_frame)
|
||||
{
|
||||
moveit_msgs::CollisionObject collision_obj;
|
||||
collision_obj.header.frame_id = base_frame;
|
||||
collision_obj.id = object;
|
||||
collision_obj.operation = moveit_msgs::CollisionObject::MOVE;
|
||||
|
||||
collision_obj.primitive_poses.resize(1);
|
||||
collision_obj.primitive_poses[0] = pose;
|
||||
|
||||
if(!scene.processCollisionObjectMsg(collision_obj))
|
||||
std::cout<<"Moving FAILED"<<std::endl;
|
||||
}
|
||||
|
||||
planning_scene::PlanningScenePtr FixCollisionObjects::apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert) const
|
||||
{
|
||||
planning_scene::PlanningScenePtr result = scene->diff();
|
||||
|
||||
//Check collision of objects on the table in planning scene
|
||||
(const_cast<FixCollisionObjects*>(this))->checkCollisions(*result, "object");
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
} } }
|
||||
Loading…
Reference in New Issue
Block a user