From c8d0ad0c2c1e679b7b65f70bc412f99c2f9d8898 Mon Sep 17 00:00:00 2001
From: Mathieu Geisert <mgeisert@laas.fr>
Date: Tue, 1 Jul 2014 18:04:56 +0200
Subject: [PATCH] Change graspsMap key from string to DifferentialFunction.
add virtual function resetConstraints to deal with collisions.
add virtual function addConstraintToConfigProjector to deal with gripper
disable collisions in the ProblemSolver.
---
include/hpp/manipulation/fwd.hh | 5 +++
include/hpp/manipulation/problem-solver.hh | 21 ++++++----
include/hpp/manipulation/robot.hh | 3 --
src/problem-solver.cc | 49 +++++++++++++++++++++-
4 files changed, 66 insertions(+), 12 deletions(-)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index bf9bdf53..e0afbb2b 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -66,6 +66,11 @@ namespace hpp {
typedef std::vector <ObjectPtr_t> Objects_t;
typedef std::map <JointConstPtr_t, JointPtr_t> JointMap_t;
typedef core::ConstraintPtr_t ConstraintPtr_t;
+ typedef core::ConstraintSet ConstraintSet;
+
+ typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
+ typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
+ typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t;
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 337dd61f..33dda8ae 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -31,9 +31,6 @@ 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 ()
{
@@ -88,12 +85,13 @@ namespace hpp {
/// \}
/// Add grasp
- void addGrasp( std::string graspName, model::GripperPtr_t gripper,
- HandlePtr_t handle)
+ void addGrasp( const DifferentiableFunctionPtr_t& constraint,
+ const model::GripperPtr_t& gripper,
+ const HandlePtr_t& handle)
{
Grasp_t* ptr = new Grasp_t (gripper, handle);
GraspPtr_t shPtr (ptr);
- graspsMap_[graspName] = shPtr;
+ graspsMap_[constraint] = shPtr;
}
/// get grapsMap
@@ -105,8 +103,17 @@ namespace hpp {
/// get graps by name
///
/// return NULL if no grasp named graspName
- GraspPtr_t grasp(const std::string& graspName) const;
+ GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const;
+ /// Reset constraint set and put back the disable collisions
+ /// between gripper and handle
+ virtual void resetConstraints ();
+
+ /// Add differentialFunction to the config projector
+ /// Build the config projector if not constructed
+ virtual void addConstraintToConfigProjector(
+ const std::string& constraintName,
+ const DifferentiableFunctionPtr_t& constraint);
/// Build a composite robot from several robots and objects
/// \param robotName Name of the composite robot,
diff --git a/include/hpp/manipulation/robot.hh b/include/hpp/manipulation/robot.hh
index 39aadb3d..3643b8fd 100644
--- a/include/hpp/manipulation/robot.hh
+++ b/include/hpp/manipulation/robot.hh
@@ -160,9 +160,6 @@ namespace hpp {
/// 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/problem-solver.cc b/src/problem-solver.cc
index 7de4c9e3..501b9ea2 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -20,6 +20,8 @@
#include <hpp/manipulation/object.hh>
#include <hpp/manipulation/problem-solver.hh>
#include <hpp/manipulation/robot.hh>
+#include <hpp/model/gripper.hh>
+
namespace hpp {
namespace manipulation {
@@ -67,15 +69,58 @@ namespace hpp {
return object;
}
- ProblemSolver::GraspPtr_t ProblemSolver::grasp (const std::string& graspName) const
+ GraspPtr_t ProblemSolver::grasp (
+ const DifferentiableFunctionPtr_t& constraint) const
{
GraspsMap_t::const_iterator it =
- graspsMap_.find (graspName);
+ graspsMap_.find (constraint);
if (it == graspsMap_.end ()) {
GraspPtr_t grasp;
return grasp;
}
return it->second;
}
+
+ void ProblemSolver::resetConstraints ()
+ {
+ if (robot_)
+ constraints_ = ConstraintSet::create (robot_,
+ "Default constraint set");
+ GraspsMap_t graspsMap = grasps();
+ for (GraspsMap_t::const_iterator itGrasp = graspsMap.begin() ;
+ itGrasp != graspsMap.end() ; itGrasp++) {
+ GraspPtr_t grasp = itGrasp->second;
+ GripperPtr_t gripper = grasp->first;
+ HandlePtr_t handle = grasp->second;
+ JointPtr_t joint = handle->joint();
+ model::JointVector_t joints = gripper->getDisabledCollisions();
+ for (model::JointVector_t::iterator itJoint = joints.begin() ;
+ itJoint != joints.end() ; itJoint++ ) {
+ robot()->addCollisionPairs(joint, *itJoint, hpp::model::COLLISION);
+ robot()->addCollisionPairs(joint, *itJoint, hpp::model::DISTANCE);
+ }
+ }
+ }
+
+ void ProblemSolver::addConstraintToConfigProjector (
+ const std::string& constraintName,
+ const DifferentiableFunctionPtr_t& constraint)
+ {
+ core::ProblemSolver::addConstraintToConfigProjector(constraintName,
+ constraint);
+ if ( grasp(constraint) ) {
+ GripperPtr_t gripper = grasp(constraint)->first;
+ HandlePtr_t handle = grasp(constraint)->second;
+ JointPtr_t joint1 = handle->joint();
+ model::JointVector_t joints = gripper->getDisabledCollisions();
+ for (model::JointVector_t::iterator itJoint = joints.begin() ;
+ itJoint != joints.end() ; itJoint++ ) {
+ robot()->removeCollisionPairs(joint1, *itJoint,
+ hpp::model::COLLISION);
+ robot()->removeCollisionPairs(joint1, *itJoint,
+ hpp::model::DISTANCE);
+ }
+ }
+ }
} // namespace manipulation
} // namespace hpp
--
GitLab