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
|
#pragma once
|
||||||
|
|
||||||
|
#include <moveit/task_constructor/storage.h>
|
||||||
#include <moveit/task_constructor/stage.h>
|
#include <moveit/task_constructor/stage.h>
|
||||||
#include <visualization_msgs/Marker.h>
|
#include <geometry_msgs/Vector3.h>
|
||||||
#include <geometry_msgs/Pose.h>
|
#include <moveit/collision_detection/collision_common.h>
|
||||||
#include <deque>
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
@ -52,11 +52,11 @@ public:
|
|||||||
void computeForward(const InterfaceState& from) override;
|
void computeForward(const InterfaceState& from) override;
|
||||||
void computeBackward(const InterfaceState& to) 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:
|
private:
|
||||||
bool fixCollisions(planning_scene::PlanningScene &scene, std::deque<visualization_msgs::Marker>& markers) const;
|
SubTrajectory fixCollisions(planning_scene::PlanningScene &scene) const;
|
||||||
void fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -46,6 +46,7 @@
|
|||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
|
|
||||||
namespace vm = visualization_msgs;
|
namespace vm = visualization_msgs;
|
||||||
|
namespace cd = collision_detection;
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
@ -54,86 +55,104 @@ FixCollisionObjects::FixCollisionObjects(const std::string &name)
|
|||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("max_penetration", "maximally corrected penetration depth");
|
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)
|
void FixCollisionObjects::computeForward(const InterfaceState &from)
|
||||||
{
|
{
|
||||||
planning_scene::PlanningScenePtr to = from.scene()->diff();
|
planning_scene::PlanningScenePtr to = from.scene()->diff();
|
||||||
SubTrajectory solution;
|
sendForward(from, InterfaceState(to), fixCollisions(*to));
|
||||||
bool success = fixCollisions(*to, solution.markers());
|
|
||||||
if (!success) solution.markAsFailure();
|
|
||||||
sendForward(from, InterfaceState(to), std::move(solution));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void FixCollisionObjects::computeBackward(const InterfaceState &to)
|
void FixCollisionObjects::computeBackward(const InterfaceState &to)
|
||||||
{
|
{
|
||||||
planning_scene::PlanningScenePtr from = to.scene()->diff();
|
planning_scene::PlanningScenePtr from = to.scene()->diff();
|
||||||
SubTrajectory solution;
|
sendBackward(InterfaceState(from), to, fixCollisions(*from));
|
||||||
bool success = fixCollisions(*from, solution.markers());
|
|
||||||
if (!success) solution.markAsFailure();
|
|
||||||
sendBackward(InterfaceState(from), to, std::move(solution));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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();
|
for (const cd::Contact& c : contacts) {
|
||||||
double penetration = props.get<double>("max_penetration");
|
if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT &&
|
||||||
(void) penetration; // not used (yet)
|
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;
|
SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene) const
|
||||||
collision_detection::CollisionResult res;
|
{
|
||||||
|
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.group_name = ""; // check collisions for complete robot
|
||||||
req.contacts = true;
|
req.contacts = true;
|
||||||
req.max_contacts = 100;
|
req.max_contacts = 100;
|
||||||
req.max_contacts_per_pair = 5;
|
req.max_contacts_per_pair = 100;
|
||||||
req.verbose = false;
|
req.verbose = false;
|
||||||
req.distance = 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;
|
vm::Marker m;
|
||||||
m.header.frame_id = scene.getPlanningFrame();
|
m.header.frame_id = scene.getPlanningFrame();
|
||||||
m.ns = "collisions";
|
m.ns = "collisions";
|
||||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED);
|
|
||||||
|
|
||||||
ROS_INFO_THROTTLE(1, "collision(s) detected");
|
bool failure = false;
|
||||||
for (const auto& info : res.contacts) {
|
while (!failure) {
|
||||||
for (const auto& contact : info.second) {
|
res.clear();
|
||||||
tf::poseEigenToMsg(Eigen::Translation3d(contact.pos) * Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), contact.normal), m.pose);
|
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(),
|
||||||
rviz_marker_tools::makeArrow(m, contact.depth);
|
scene.getCurrentState(),
|
||||||
markers.push_back(m);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// check again
|
|
||||||
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(),
|
|
||||||
scene.getAllowedCollisionMatrix());
|
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
|
// failure
|
||||||
{
|
result.markAsFailure();
|
||||||
moveit_msgs::CollisionObject collision_obj;
|
return result;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -25,7 +25,7 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) {
|
|||||||
o.primitive_poses.resize(1);
|
o.primitive_poses.resize(1);
|
||||||
o.primitive_poses[0].position.x= 0.3;
|
o.primitive_poses[0].position.x= 0.3;
|
||||||
o.primitive_poses[0].position.y= 0.23;
|
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.primitive_poses[0].orientation.w= 1.0;
|
||||||
o.primitives.resize(1);
|
o.primitives.resize(1);
|
||||||
o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER;
|
o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER;
|
||||||
@ -59,14 +59,14 @@ TEST(PA10, pick) {
|
|||||||
spawnObject(scene);
|
spawnObject(scene);
|
||||||
|
|
||||||
auto initial = std::make_unique<stages::FixedState>();
|
auto initial = std::make_unique<stages::FixedState>();
|
||||||
initial_stage = initial.get();
|
|
||||||
initial->setState(scene);
|
initial->setState(scene);
|
||||||
t.add(std::move(initial));
|
t.add(std::move(initial));
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::FixCollisionObjects>();
|
auto stage = std::make_unique<stages::FixCollisionObjects>();
|
||||||
stage->restrictDirection(stages::MoveTo::FORWARD);
|
stage->restrictDirection(stages::MoveTo::FORWARD);
|
||||||
stage->setMaxPenetration(0.1);
|
stage->setMaxPenetration(0.04);
|
||||||
|
initial_stage = stage.get();
|
||||||
t.add(std::move(stage));
|
t.add(std::move(stage));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user