From 5b9b39a59d99a04692521eaf37b50d0d0ddda28f Mon Sep 17 00:00:00 2001
From: Mathieu Geisert <mgeisert@laas.fr>
Date: Thu, 19 Jun 2014 14:05:09 +0200
Subject: [PATCH] Add collisions between devices when buildcomposite robot and
also deal with the disabled collisions of grippers.
---
include/hpp/manipulation/problem-solver.hh | 4 +-
include/hpp/manipulation/robot.hh | 8 +-
src/robot.cc | 87 ++++++++++++++++++++++
3 files changed, 95 insertions(+), 4 deletions(-)
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 3cc9116a..42a9a392 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -86,8 +86,8 @@ namespace hpp {
/// Build a composite robot from several robots and objects
/// \param robotName Name of the composite robot,
- /// \param robotNames Names of the robots stored internally that have been
- /// added by method addRobot.
+ /// \param robotNames Names of the robots stored internally that have
+ /// been added by method addRobot.
///
/// Objects are detected by dynamic cast.
void buildCompositeRobot (const std::string& robotName,
diff --git a/include/hpp/manipulation/robot.hh b/include/hpp/manipulation/robot.hh
index 1214309c..e271076e 100644
--- a/include/hpp/manipulation/robot.hh
+++ b/include/hpp/manipulation/robot.hh
@@ -104,7 +104,6 @@ namespace hpp {
throw std::runtime_error ("no gripper with name " + name);
return it->second;
}
-
/// Print object in a stream
virtual std::ostream& print (std::ostream& os) const;
@@ -142,7 +141,12 @@ namespace hpp {
/// Copy grippers of the device into composite robot.
void copyGrippers (const DeviceConstPtr_t& device);
-
+ /// Add Collision pairs between the robots
+ void addCollisions(const DeviceConstPtr_t& device);
+
+ /// Check if joint1 joint2 is a couple gripper-handle
+ bool isCollisionPairDisabled(JointPtr_t& joint1, JointPtr_t& joint2);
+
Devices_t robots_;
/// Set of objects
Objects_t objects_;
diff --git a/src/robot.cc b/src/robot.cc
index 6174a6ae..efa09cdd 100644
--- a/src/robot.cc
+++ b/src/robot.cc
@@ -18,12 +18,17 @@
// <http://www.gnu.org/licenses/>.
#include <hpp/util/debug.hh>
+#include <hpp/manipulation/fwd.hh>
#include <hpp/model/object-factory.hh>
#include <hpp/manipulation/robot.hh>
#include <hpp/model/gripper.hh>
namespace hpp {
namespace manipulation {
+ typedef std::map <std::string, HandlePtr_t> Handles_t;
+ typedef std::map <std::string, GripperPtr_t> Grippers_t;
+ typedef std::vector<JointPtr_t> JointVector_t;
+
void Robot::copyKinematicChain (const JointPtr_t& parentJoint,
const JointConstPtr_t& joint)
{
@@ -45,6 +50,7 @@ namespace hpp {
{
copyKinematicChain (rootJoint, object->rootJoint ());
copyHandles (object);
+ addCollisions(object);
}
void Robot::copyHandles (const ObjectConstPtr_t& object)
@@ -64,6 +70,7 @@ namespace hpp {
{
copyKinematicChain (rootJoint, device->rootJoint ());
copyGrippers (device);
+ addCollisions(device);
}
void Robot::copyGrippers (const DeviceConstPtr_t& device)
@@ -74,6 +81,12 @@ namespace hpp {
GripperPtr_t gripper = (*itGripper)->clone ();
gripper->name (device->name () + "/" + (*itGripper)->name ());
gripper->joint (jointMap_ [(*itGripper)->joint ()]);
+ gripper->removeAllDisabledCollisions();
+ JointVector_t joints = (*itGripper)->getDisabledCollisions();
+ for (model::JointVector_t::const_iterator itJoint = joints.begin() ;
+ itJoint != joints.end() ; itJoint++ ) {
+ gripper->addDisabledCollision(jointMap_[*itJoint]);
+ }
addGripper (gripper->name (), gripper);
}
}
@@ -163,6 +176,80 @@ namespace hpp {
updateDistances ();
}
+ void Robot::addCollisions (const DeviceConstPtr_t& device)
+ {
+ JointVector_t joints = device->getJointVector ();
+ // Cycle through all joint pairs
+ for (JointVector_t::const_iterator it1 = joints.begin ();
+ it1 != joints.end (); it1++) {
+ JointPtr_t joint1 = jointMap_[*it1];
+ hpp::model::Body* body1 = joint1->linkedBody ();
+ if (!body1) {
+ hppDout (info, "Joint " + joint1->name () <<
+ " has no hpp::model::Body.");
+ } else {
+ for (JointVector_t::const_iterator it2 = getJointVector().begin ();
+ *it2 != jointMap_[*(joints.begin())]; it2++) {
+ JointPtr_t joint2 = *it2;
+ hpp::model::Body* body2 = joint2->linkedBody ();
+ if (!body2) {
+ hppDout (info, "Joint " + joint2->name () <<
+ " has no hpp::model::Body.");
+ } else {
+ if (!isCollisionPairDisabled (joint1, joint2)) {
+ hppDout (info, "add collision pairs: (" << joint1->name() <<
+ "," << joint2->name() << ")");
+ hpp::model::Device::addCollisionPairs (joint1, joint2,
+ hpp::model::COLLISION);
+ hpp::model::Device::addCollisionPairs (joint1, joint2,
+ hpp::model::DISTANCE);
+ }
+ }
+ }
+ }
+ }
+ }
+
+ bool Robot::isCollisionPairDisabled(JointPtr_t& joint1, JointPtr_t& joint2)
+ {
+ // Disable collision if a joint is an handle and the other is a
+ // disabledCollision from a gripper
+ for (Handles_t::iterator itHandle = handles_.begin();
+ itHandle != handles_.end() ; itHandle++) {
+ if ( itHandle->second->joint() == joint1 ) {
+ for (Grippers_t::iterator itGripper = grippers_.begin();
+ itGripper != grippers_.end() ; itGripper++) {
+ model::JointVector_t joints = itGripper->second
+ ->getDisabledCollisions();
+ for (model::JointVector_t::iterator itJoint = joints.begin() ;
+ itJoint != joints.end() ; itJoint++ ) {
+ if ( joint2 == *(itJoint) ) {
+ hppDout (info, "Disabled collision between "<<
+ joint1->name() << " and " << joint2->name());
+ return true;
+ }
+ }
+ }
+ }
+ if ( itHandle->second->joint() == joint2 ) {
+ for (Grippers_t::iterator itGripper = grippers_.begin();
+ itGripper != grippers_.end() ; itGripper++) {
+ model::JointVector_t joints = itGripper->second
+ ->getDisabledCollisions();
+ for (model::JointVector_t::iterator itJoint = joints.begin() ;
+ itJoint != joints.end() ; itJoint++ ) {
+ if ( joint1 == *(itJoint) ) {
+ hppDout (info, "Disabled collision between "<<
+ joint1->name() << " and " << joint2->name());
+ return true;
+ }
+ }
+ }
+ }
+ }
+ return false;
+ }
+
std::ostream& Robot::print (std::ostream& os) const
{
parent_t::print (os);
--
GitLab