From 12d9b5f9184b2fe49b041a7335a3d56591add487 Mon Sep 17 00:00:00 2001
From: Mathieu Geisert <mgeisert@laas.fr>
Date: Mon, 30 Jun 2014 18:12:34 +0200
Subject: [PATCH] Add map of grasp (=pair<gripper,handle>) to disable the
collisions between gripper and handle when a grasping constraint is set.
---
include/hpp/manipulation/problem-solver.hh | 27 ++++++++++-
src/problem-solver.cc | 10 ++++
src/robot.cc | 54 +++-------------------
3 files changed, 42 insertions(+), 49 deletions(-)
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 42a9a392..337dd61f 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -31,12 +31,15 @@ namespace hpp {
public:
typedef core::ProblemSolver parent_t;
typedef std::vector <std::string> Names_t;
+ typedef std::pair< model::GripperPtr_t, HandlePtr_t> Grasp_t;
+ typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
+ typedef std::map <const std::string, GraspPtr_t> GraspsMap_t;
/// Destructor
virtual ~ProblemSolver ()
{
}
ProblemSolver () : core::ProblemSolver (), robot_ (),
- robotsAndObjects_ ()
+ robotsAndObjects_ (), graspsMap_()
{
}
/// Set robot
@@ -84,6 +87,27 @@ namespace hpp {
ObjectPtr_t object (const std::string& name) const;
/// \}
+ /// Add grasp
+ void addGrasp( std::string graspName, model::GripperPtr_t gripper,
+ HandlePtr_t handle)
+ {
+ Grasp_t* ptr = new Grasp_t (gripper, handle);
+ GraspPtr_t shPtr (ptr);
+ graspsMap_[graspName] = shPtr;
+ }
+
+ /// get grapsMap
+ GraspsMap_t& grasps()
+ {
+ return graspsMap_;
+ }
+
+ /// get graps by name
+ ///
+ /// return NULL if no grasp named graspName
+ GraspPtr_t grasp(const std::string& graspName) const;
+
+
/// 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
@@ -97,6 +121,7 @@ namespace hpp {
RobotPtr_t robot_;
/// Map of single robots to store before building a composite robot.
RobotsandObjects_t robotsAndObjects_;
+ GraspsMap_t graspsMap_;
}; // class ProblemSolver
} // namespace manipulation
} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index d97d4b14..7de4c9e3 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -67,5 +67,15 @@ namespace hpp {
return object;
}
+ ProblemSolver::GraspPtr_t ProblemSolver::grasp (const std::string& graspName) const
+ {
+ GraspsMap_t::const_iterator it =
+ graspsMap_.find (graspName);
+ if (it == graspsMap_.end ()) {
+ GraspPtr_t grasp;
+ return grasp;
+ }
+ return it->second;
+ }
} // namespace manipulation
} // namespace hpp
diff --git a/src/robot.cc b/src/robot.cc
index efa09cdd..c42f304c 100644
--- a/src/robot.cc
+++ b/src/robot.cc
@@ -196,60 +196,18 @@ namespace hpp {
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);
- }
+ 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