mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
FixCollisions stage
This commit is contained in:
parent
a7b74673ce
commit
e5e291d86f
@ -38,10 +38,10 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <geometry_msgs/Pose.h>
|
||||
#include <deque>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
#include <moveit/collision_detection/collision_common.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
@ -52,11 +52,11 @@ public:
|
||||
void computeForward(const InterfaceState& from) override;
|
||||
void computeBackward(const InterfaceState& to) override;
|
||||
|
||||
void setMaxPenetration(double penetration);
|
||||
void setDirection(const geometry_msgs::Vector3& dir) { setProperty("direction", dir); }
|
||||
void setMaxPenetration(double penetration) { setProperty("max_penetration", penetration); }
|
||||
|
||||
private:
|
||||
bool fixCollisions(planning_scene::PlanningScene &scene, std::deque<visualization_msgs::Marker>& markers) const;
|
||||
void fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const;
|
||||
SubTrajectory fixCollisions(planning_scene::PlanningScene &scene) const;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -46,6 +46,7 @@
|
||||
#include <ros/console.h>
|
||||
|
||||
namespace vm = visualization_msgs;
|
||||
namespace cd = collision_detection;
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
@ -54,86 +55,104 @@ FixCollisionObjects::FixCollisionObjects(const std::string &name)
|
||||
{
|
||||
auto& p = properties();
|
||||
p.declare<double>("max_penetration", "maximally corrected penetration depth");
|
||||
p.declare<geometry_msgs::Vector3>("direction", "direction vector to use for corrections");
|
||||
}
|
||||
|
||||
void FixCollisionObjects::setMaxPenetration(double penetration)
|
||||
{
|
||||
setProperty("max_penetration", penetration);
|
||||
}
|
||||
|
||||
|
||||
void FixCollisionObjects::computeForward(const InterfaceState &from)
|
||||
{
|
||||
planning_scene::PlanningScenePtr to = from.scene()->diff();
|
||||
SubTrajectory solution;
|
||||
bool success = fixCollisions(*to, solution.markers());
|
||||
if (!success) solution.markAsFailure();
|
||||
sendForward(from, InterfaceState(to), std::move(solution));
|
||||
sendForward(from, InterfaceState(to), fixCollisions(*to));
|
||||
}
|
||||
|
||||
void FixCollisionObjects::computeBackward(const InterfaceState &to)
|
||||
{
|
||||
planning_scene::PlanningScenePtr from = to.scene()->diff();
|
||||
SubTrajectory solution;
|
||||
bool success = fixCollisions(*from, solution.markers());
|
||||
if (!success) solution.markAsFailure();
|
||||
sendBackward(InterfaceState(from), to, std::move(solution));
|
||||
sendBackward(InterfaceState(from), to, fixCollisions(*from));
|
||||
}
|
||||
|
||||
bool FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene, std::deque<visualization_msgs::Marker> &markers) const
|
||||
bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d& correction, double max_penetration)
|
||||
{
|
||||
const auto& props = properties();
|
||||
double penetration = props.get<double>("max_penetration");
|
||||
(void) penetration; // not used (yet)
|
||||
for (const cd::Contact& c : contacts) {
|
||||
if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT &&
|
||||
c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) {
|
||||
ROS_WARN_STREAM_NAMED("FixCollisionObjects", "Cannot fix collision between "
|
||||
<< c.body_name_1 << " and " << c.body_name_2);
|
||||
return false;
|
||||
}
|
||||
if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT)
|
||||
correction -= c.depth * c.normal;
|
||||
else
|
||||
correction += c.depth * c.normal;
|
||||
}
|
||||
// average and add tolerance
|
||||
double norm = correction.norm();
|
||||
double rounded = norm / contacts.size() + 1.e-3;
|
||||
correction *= rounded / norm;
|
||||
return true;
|
||||
}
|
||||
|
||||
collision_detection::CollisionRequest req;
|
||||
collision_detection::CollisionResult res;
|
||||
SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene) const
|
||||
{
|
||||
SubTrajectory result;
|
||||
const auto& props = properties();
|
||||
double max_penetration = props.get<double>("max_penetration");
|
||||
const boost::any& dir = props.get("direction");
|
||||
|
||||
cd::CollisionRequest req;
|
||||
cd::CollisionResult res;
|
||||
req.group_name = ""; // check collisions for complete robot
|
||||
req.contacts = true;
|
||||
req.max_contacts = 100;
|
||||
req.max_contacts_per_pair = 5;
|
||||
req.max_contacts_per_pair = 100;
|
||||
req.verbose = false;
|
||||
req.distance = false;
|
||||
|
||||
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(),
|
||||
scene.getAllowedCollisionMatrix());
|
||||
|
||||
geometry_msgs::Pose pose;
|
||||
|
||||
if (!res.collision)
|
||||
return true;
|
||||
|
||||
vm::Marker m;
|
||||
m.header.frame_id = scene.getPlanningFrame();
|
||||
m.ns = "collisions";
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED);
|
||||
|
||||
ROS_INFO_THROTTLE(1, "collision(s) detected");
|
||||
for (const auto& info : res.contacts) {
|
||||
for (const auto& contact : info.second) {
|
||||
tf::poseEigenToMsg(Eigen::Translation3d(contact.pos) * Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), contact.normal), m.pose);
|
||||
rviz_marker_tools::makeArrow(m, contact.depth);
|
||||
markers.push_back(m);
|
||||
}
|
||||
}
|
||||
|
||||
// check again
|
||||
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(),
|
||||
bool failure = false;
|
||||
while (!failure) {
|
||||
res.clear();
|
||||
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(),
|
||||
scene.getCurrentState(),
|
||||
scene.getAllowedCollisionMatrix());
|
||||
return !res.collision;
|
||||
if (!res.collision)
|
||||
return result;
|
||||
|
||||
for (const auto& info : res.contacts) {
|
||||
Eigen::Vector3d correction;
|
||||
failure = !computeCorrection(info.second, correction, max_penetration);
|
||||
if (failure)
|
||||
break;
|
||||
double depth = correction.norm();
|
||||
failure = depth > max_penetration;
|
||||
|
||||
// marker indicating correction
|
||||
const cd::Contact &c = info.second.front();
|
||||
rviz_marker_tools::setColor(m.color, failure ? rviz_marker_tools::RED : rviz_marker_tools::GREEN);
|
||||
tf::poseEigenToMsg(Eigen::Translation3d(c.pos) *
|
||||
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), correction), m.pose);
|
||||
rviz_marker_tools::makeArrow(m, depth, true);
|
||||
result.markers().push_back(m);
|
||||
if (failure)
|
||||
break;
|
||||
|
||||
// fix collision by shifting object along correction direction
|
||||
if (!dir.empty()) // if explicitly given, use this correction direction
|
||||
tf::vectorMsgToEigen(boost::any_cast<geometry_msgs::Vector3>(dir), correction);
|
||||
|
||||
const std::string& name = c.body_type_1 == cd::BodyTypes::WORLD_OBJECT ? c.body_name_1 : c.body_name_2;
|
||||
scene.getWorldNonConst()->moveObject(name, Eigen::Affine3d(Eigen::Translation3d(correction)));
|
||||
}
|
||||
}
|
||||
|
||||
void FixCollisionObjects::fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const
|
||||
{
|
||||
moveit_msgs::CollisionObject collision_obj;
|
||||
collision_obj.header.frame_id = scene.getPlanningFrame();
|
||||
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;
|
||||
// failure
|
||||
result.markAsFailure();
|
||||
return result;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -25,7 +25,7 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) {
|
||||
o.primitive_poses.resize(1);
|
||||
o.primitive_poses[0].position.x= 0.3;
|
||||
o.primitive_poses[0].position.y= 0.23;
|
||||
o.primitive_poses[0].position.z= 0.12;
|
||||
o.primitive_poses[0].position.z= 0.10;
|
||||
o.primitive_poses[0].orientation.w= 1.0;
|
||||
o.primitives.resize(1);
|
||||
o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER;
|
||||
@ -59,14 +59,14 @@ TEST(PA10, pick) {
|
||||
spawnObject(scene);
|
||||
|
||||
auto initial = std::make_unique<stages::FixedState>();
|
||||
initial_stage = initial.get();
|
||||
initial->setState(scene);
|
||||
t.add(std::move(initial));
|
||||
}
|
||||
{
|
||||
auto stage = std::make_unique<stages::FixCollisionObjects>();
|
||||
stage->restrictDirection(stages::MoveTo::FORWARD);
|
||||
stage->setMaxPenetration(0.1);
|
||||
stage->setMaxPenetration(0.04);
|
||||
initial_stage = stage.get();
|
||||
t.add(std::move(stage));
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user