Dectect pairs (constraint, complement) and replace by relative transformation.

  - add methods Graph::registerConstraints, isComplement
  - modify Edge::builConfigConstraints and Edge::buildPathConstraints in order
    to replace pairs of constraint and complement by full explicit relative
    transformations.

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

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);

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
 

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 ());

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 ();

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

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