diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 605f43688a7728de5cfb9a6e851835cbde0340b1..9ea966fd85ff41db76ae4a47a0a0ec56fcd7507c 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -64,13 +64,6 @@ namespace hpp {
virtual NumericalConstraintPtr_t createGraspComplement
(const GripperPtr_t& gripper, std::string name) const;
- /// Create constraint composed of grasp constraint and its complement
- /// \param gripper object containing the gripper information
- /// \return the composition of grasp constraint and its complement.
- /// \note the 6 degrees of freedom are constrained
- virtual NumericalConstraintPtr_t createGraspAndComplement
- (const GripperPtr_t& gripper, std::string name) const;
-
/// Create constraint corresponding to a pregrasping task.
/// \param gripper object containing the gripper information
/// \return the constraint of relative transformation between the handle and
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index c076ca844aeef8129d9cad97e29a963db246726a..2a80e4cd801dfdeb32d462d149861b084b6fb42a 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -100,10 +100,16 @@ namespace hpp {
typedef std::vector <ObjectPtr_t> Objects_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
+ typedef core::ExplicitNumericalConstraintPtr_t
+ ExplicitNumericalConstraintPtr_t;
typedef core::LockedJoint LockedJoint;
typedef core::LockedJointPtr_t LockedJointPtr_t;
typedef core::NumericalConstraint NumericalConstraint;
typedef core::NumericalConstraintPtr_t NumericalConstraintPtr_t;
+ typedef core::ComparisonTypesPtr_t ComparisonTypesPtr_t;
+ typedef core::EqualToZero EqualToZero;
+ typedef core::Equality Equality;
+ typedef core::ComparisonTypes ComparisonTypes;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
HPP_PREDEF_CLASS (ConstraintSet);
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 56ba1109eeb24aae8c129f74320ae34e18783aa5..a7314ea42dad02b780e189f2476a0eb8ba3a85da 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -17,6 +17,7 @@
#ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
# define HPP_MANIPULATION_GRAPH_GRAPH_HH
+# include <boost/tuple/tuple.hpp>
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
@@ -104,6 +105,38 @@ namespace hpp {
/// Select randomly outgoing edge of the given node.
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
+ /// Register a triple of constraints to be inserted in nodes and edges
+ /// \param constraint a constraint (grasp of placement)
+ /// \param complement the complement constraint
+ /// \param both combination of the constraint and its complement. Both
+ /// constraints together corresponds to a full relative
+ /// transformation constraint
+ /// When inserting constraints in transitions of the graph,
+ /// in many cases, a constraint is associated to a state and
+ /// the complement constraint is associated to the
+ /// transition itself. Registering those constraints
+ /// priorly to graph construction makes possible to replace
+ /// the constraint and its complement by the combination of
+ /// both that is an explicit constraint.
+ void registerConstraints (const NumericalConstraintPtr_t& constraint,
+ const NumericalConstraintPtr_t& complement,
+ const NumericalConstraintPtr_t& both);
+
+ /// Test whether two constraints are complement of one another
+ ///
+ /// \param constraint, complement two constraints to test
+ /// \retval combinationOfBoth constraint corresponding to combining
+ /// constraint and complement if result is true,
+ /// unchanged otherwise.
+ /// \return whether complement is the complement of constraint.
+ /// Two constraints are complement of one another if and only if
+ /// combined they constitute a complement relative transformation
+ /// constraint. \sa Graph::registerConstraints
+ /// \warning argument order matters.
+ bool isComplement (const NumericalConstraintPtr_t& constraint,
+ const NumericalConstraintPtr_t& complement,
+ NumericalConstraintPtr_t& combinationOfBoth) const;
+
/// Constraint to project onto the Node.
/// \param state the state on which to project.
/// \return The initialized projector.
@@ -265,6 +298,20 @@ namespace hpp {
value_type errorThreshold_;
size_type maxIterations_;
+ struct ConstraintAndComplement_t {
+ NumericalConstraintPtr_t constraint;
+ NumericalConstraintPtr_t complement;
+ NumericalConstraintPtr_t both;
+ ConstraintAndComplement_t (const NumericalConstraintPtr_t& constr,
+ const NumericalConstraintPtr_t& comp,
+ const NumericalConstraintPtr_t& b) :
+ constraint (constr), complement (comp), both (b)
+ {
+ }
+ };
+ typedef std::vector <ConstraintAndComplement_t>
+ ConstraintsAndComplements_t;
+ ConstraintsAndComplements_t constraintsAndComplements_;
friend class GraphComponent;
}; // Class Graph
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 8c7fdd8863c3d2366641cd996ffc95e834afe943..31ee35142167db40899715d5aaef75c8f42e3cb2 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -165,6 +165,62 @@ namespace hpp {
return configConstraints_;
}
+ // Merge constraints of several graph components into a config projectors
+ // Replace constraints and complement by combination of both when
+ // necessary.
+ static void mergeConstraintsIntoConfigProjector
+ (const ConfigProjectorPtr_t& proj,
+ const std::vector <GraphComponentPtr_t>& components,
+ const GraphPtr_t& graph)
+ {
+ NumericalConstraints_t nc;
+ std::vector <segments_t> pdof;
+ for (std::vector <GraphComponentPtr_t>::const_iterator it
+ (components.begin ()); it != components.end (); ++it) {
+ nc.insert (nc.end (), (*it)->numericalConstraints ().begin (),
+ (*it)->numericalConstraints ().end ());
+ pdof.insert (pdof.end (), (*it)->passiveDofs ().begin (),
+ (*it)->passiveDofs ().end ());
+ }
+ assert (nc.size () == pdof.size ());
+ NumericalConstraints_t::iterator itnc1 (nc.begin ());
+ std::vector <segments_t>::iterator itpdof1 (pdof.begin ());
+ while (itnc1 != nc.end ()) {
+ NumericalConstraints_t::iterator itnc2 (nc.begin ());
+ std::vector <segments_t>::iterator itpdof2 (pdof.begin ());
+ while (itnc2 != nc.end ()) {
+ bool increment (true);
+ NumericalConstraintPtr_t combination;
+ // Do not check that a constraint is its own complement
+ if (itnc1 != itnc2) {
+ // Remove duplicate constraints
+ if (*itnc1 == *itnc2) {
+ nc.erase (itnc2);
+ pdof.erase (itpdof2);
+ increment = false;
+ } else if (graph->isComplement (*itnc1, *itnc2, combination)) {
+ // Replace constraint by combination of both and remove
+ // complement.
+ *itnc1 = combination;
+ if (itnc1 > itnc2) --itnc1;
+ nc.erase (itnc2);
+ pdof.erase (itpdof2);
+ break;
+ }
+ }
+ if (increment) ++itnc2; ++itpdof2;
+ }
+ ++itnc1; ++itpdof1;
+ }
+ assert (nc.size () == pdof.size ());
+ NumericalConstraints_t::iterator itnc (nc.begin ());
+ std::vector <segments_t>::iterator itpdof (pdof.begin ());
+ while (itnc != nc.end ()) {
+ proj->add (*itnc, *itpdof);
+ ++itnc; ++itpdof;
+ }
+ }
+
ConstraintSetPtr_t Edge::buildConfigConstraint()
{
std::string n = "(" + name () + ")";
@@ -180,12 +236,14 @@ namespace hpp {
state ()->insertLockedJoints (proj);
}
- g->insertNumericalConstraints (proj);
- insertNumericalConstraints (proj);
- to ()->insertNumericalConstraints (proj);
+ std::vector <GraphComponentPtr_t> components;
+ components.push_back (g);
+ components.push_back (wkPtr_.lock ());
+ components.push_back (to ());
if (state () != to ()) {
- state ()->insertNumericalConstraints (proj);
+ components.push_back (state ());
}
+ mergeConstraintsIntoConfigProjector (proj, components, parentGraph ());
constraint->addConstraint (proj);
constraint->edge (wkPtr_.lock ());
@@ -210,9 +268,11 @@ namespace hpp {
insertLockedJoints (proj);
state ()->insertLockedJoints (proj);
- g->insertNumericalConstraints (proj);
- insertNumericalConstraints (proj);
- state ()->insertNumericalConstraintsForPath (proj);
+ std::vector <GraphComponentPtr_t> components;
+ components.push_back (g);
+ components.push_back (wkPtr_.lock ());
+ components.push_back (state ());
+ mergeConstraintsIntoConfigProjector (proj, components, parentGraph ());
constraint->addConstraint (proj);
constraint->edge (wkPtr_.lock ());
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index c3812b75acc4ee07b9a5e4d41a8f20eb82ef7d9b..7d397a98e0dbb0495093f0ce5c9d2b61be1932a1 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -150,6 +150,32 @@ namespace hpp {
return stateSelector_->chooseEdge (from);
}
+ void Graph::registerConstraints
+ (const NumericalConstraintPtr_t& constraint,
+ const NumericalConstraintPtr_t& complement,
+ const NumericalConstraintPtr_t& both)
+ {
+ constraintsAndComplements_.push_back (ConstraintAndComplement_t
+ (constraint, complement, both));
+ }
+
+ bool Graph::isComplement (const NumericalConstraintPtr_t& constraint,
+ const NumericalConstraintPtr_t& complement,
+ NumericalConstraintPtr_t& combinationOfBoth)
+ const
+ {
+ for (ConstraintsAndComplements_t::const_iterator it =
+ constraintsAndComplements_.begin ();
+ it != constraintsAndComplements_.end (); ++it) {
+ if ((it->constraint->functionPtr () == constraint->functionPtr ()) &&
+ (it->complement->functionPtr () == complement->functionPtr ())) {
+ combinationOfBoth = it->both;
+ return true;
+ }
+ }
+ return false;
+ }
+
ConstraintSetPtr_t Graph::configConstraint (const StatePtr_t& state) const
{
return state->configConstraint ();
diff --git a/src/handle.cc b/src/handle.cc
index 446fc9b920590a039c92670fbbfdaff158d0c374..797db73157630daeb80791c0819ec04bce607aaf 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -164,12 +164,25 @@ namespace hpp {
if (n.empty()) {
n = gripper->name() + "_holds_" + name();
}
+ // Create the comparison operator
+ assert (mask_.size () == 6);
+ ComparisonTypes_t comp (6);
+ for (std::size_t i=0; i<6;++i) {
+ if (mask_ [i]) {
+ comp [i] = constraints::EqualToZero;
+ } else {
+ comp [i] = constraints::Equality;
+ }
+ }
// If handle is on a freeflying object, create an explicit constraint
if (isHandleOnFreeflyer (*this)) {
- return ExplicitRelativeTransformation::create
- (n, gripper->joint ()->robot (), gripper->joint (), joint (),
- gripper->objectPositionInJoint (),
- localPosition())->createNumericalConstraint();
+ ExplicitNumericalConstraintPtr_t enc
+ (ExplicitRelativeTransformation::create
+ (n, gripper->joint ()->robot (), gripper->joint (), joint (),
+ gripper->objectPositionInJoint (),
+ localPosition())->createNumericalConstraint());
+ enc->comparisonType (comp);
+ return enc;
}
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
@@ -179,7 +192,7 @@ namespace hpp {
gripper->objectPositionInJoint (),
localPosition(),
list_of (true)(true)(true)(true)(true)
- (true))));
+ (true)), comp));
}
NumericalConstraintPtr_t Handle::createPreGrasp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 694ea363be0f880e561cfdc47d3e50288c8e579e..fd688e57a01d0f21a1502326c6cf723f349d4fe7 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -301,15 +301,22 @@ namespace hpp {
(const std::string& name, const std::string& gripper,
const std::string& handle)
{
+ if (!constraintGraph ()) {
+ throw std::runtime_error ("The graph is not defined.");
+ }
GripperPtr_t g = robot_->get <GripperPtr_t> (gripper);
if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
HandlePtr_t h = robot_->get <HandlePtr_t> (handle);
if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
const std::string cname = name + "/complement";
+ const std::string bname = name + "/hold";
NumericalConstraintPtr_t constraint (h->createGrasp (g, name));
NumericalConstraintPtr_t complement (h->createGraspComplement (g, cname));
+ NumericalConstraintPtr_t both (h->createGraspAndComplement (g, bname));
addNumericalConstraint ( name, constraint);
addNumericalConstraint (cname, complement);
+ addNumericalConstraint (bname, both);
+ constraintGraph ()->registerConstraints (constraint, complement, both);
}
void ProblemSolver::createPreGraspConstraint