Remove constraint member function.

include/hpp/manipulation/graph/edge.hh

@@ -36,19 +36,15 @@ namespace hpp {
           /// Create a new empty Edge.
           static EdgePtr_t create (const NodeWkPtr_t& from, const NodeWkPtr_t& to);
 
-          /// Set the constraints of the Edge.
-          void constraints (const ConstraintPtr_t& constraints)
-            throw (Bad_function_call);
-
-          /// Projector to project onto the same leaf as config.
+          /// Constraint to project onto the same leaf as config.
           /// \return The initialized projector.
           /// \param config Configuration that will initialize the projector.
-          ConfigProjectorPtr_t configurationProjector(const Configuration_t config);
+          ConstraintPtr_t configConstraint(ConfigurationIn_t config);
 
-          /// Projector to project a path.
-          /// \return The initialized projector.
-          /// \param config Configuration that will initialize the projector.
-          ConfigProjectorPtr_t pathProjector(const Configuration_t config);
+          /// Constraint to project a path.
+          /// \return The initialized constraint.
+          /// \param config Configuration that will initialize the constraint.
+          ConstraintPtr_t pathConstraint(ConfigurationIn_t config);
 
           /// Print the object in a stream.
           std::ostream& print (std::ostream& os) const;
@@ -66,16 +62,12 @@ namespace hpp {
           /// The two ends of the transition.
           NodeWkPtr_t from_, to_;
 
-          /// Set of constraints to be statisfied.
-          ConstraintPtr_t constraints_;
-
-          /// Projectors ensuring that a path between q_near and q_proj is
-          /// valid regarding the manipulation rules.
-          ConfigProjectorPtr_t configurationProjector_;
+          /// See pathConstraint member function.
+          ConstraintPtr_t pathConstraints_;
 
-          /// Projectors ensuring that a path between two configurations in
-          /// the same leaf lies in the leaf.
-          ConfigProjectorPtr_t pathProjector_;
+          /// Constraint ensuring that a q_proj will be in to_ and in the
+          /// same leaf of to_ as the configuration used for initialization.
+          ConstraintPtr_t configConstraints_;
 
           /// Weak pointer to itself.
           EdgeWkPtr_t wkPtr_;

include/hpp/manipulation/graph/fwd.hh

@@ -37,9 +37,20 @@ namespace hpp {
       typedef std::vector < EdgePtr_t > Edges_t;
       typedef std::vector < NodeSelectorPtr_t > NodeSelectors_t;
 
-      typedef hpp::core::Constraint Constraint_t;
+      typedef hpp::core::value_type value_type;
+      typedef hpp::model::ConfigurationIn_t ConfigurationIn_t;
+
+      typedef hpp::core::Constraint Constraint;
       typedef hpp::core::ConstraintPtr_t ConstraintPtr_t;
+      typedef hpp::core::LockedDof LockedDof;
+      typedef hpp::core::LockedDofPtr_t LockedDofPtr_t;
+      typedef hpp::core::ConfigProjector ConfigProjector;
       typedef hpp::core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
+      typedef hpp::core::ConstraintSet ConstraintSet;
+      typedef hpp::core::ConstraintSetPtr_t ConstraintSetPtr_t;
+      typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
+      typedef std::list < DifferentiableFunctionPtr_t > DifferentiableFunctions_t;
+      typedef std::list < LockedDofPtr_t > LockedDofs_t;
     } // namespace graph
   } // namespace manipulation
 } // namespace hpp

include/hpp/manipulation/graph/graph.hh

@@ -74,11 +74,9 @@ namespace hpp {
             return os;
           }
 
-          /// Set the constraints of the component.
-          virtual void constraints (const ConstraintPtr_t& /* constraints */)
-            throw (Bad_function_call) = 0;
 
         protected:
+          /// Initialize the component
           void init (const GraphComponentWkPtr_t& weak)
           {
             wkPtr_ = weak;
@@ -94,8 +92,11 @@ namespace hpp {
           /// easily.
           static std::vector < GraphComponentWkPtr_t > components;
 
+          /// Name of the component.
           std::string name_;
+          /// Weak pointer to itself.
           GraphComponentWkPtr_t wkPtr_;
+          /// ID of the component (index in components vector).
           int id_;
       };
 
@@ -131,12 +132,6 @@ namespace hpp {
           /// Print the object in a stream.
           std::ostream& print (std::ostream& os) const;
 
-          /// Should never be called.
-          virtual void constraints (const ConstraintPtr_t& /* constraints */)
-            throw (Bad_function_call)
-          {
-            HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
-          }
 
         protected:
           /// Initialization of the object.

include/hpp/manipulation/graph/node-selector.hh

@@ -36,9 +36,6 @@ namespace hpp {
           /// Create an empty node
           NodePtr_t createNode ();
 
-          /// Create a node with the constraints
-          NodePtr_t createNode (const ConstraintPtr_t& constraints);
-
           /// Returns the state of a configuration.
           virtual NodePtr_t getNode(const Configuration_t config);
 
@@ -49,11 +46,6 @@ namespace hpp {
           std::ostream& print (std::ostream& os) const;
 
           /// Should never be called.
-          virtual void constraints (const ConstraintPtr_t& /* constraints */)
-            throw (Bad_function_call)
-          {
-            HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
-          }
         protected:
           /// Initialization of the object.
           void init (const NodeSelectorPtr_t& weak);

include/hpp/manipulation/graph/node.hh

@@ -17,7 +17,10 @@
 #ifndef HPP_MANIPULATION_GRAPH_NODE_HH
 # define HPP_MANIPULATION_GRAPH_NODE_HH
 
+
+#include <hpp/core/locked-dof.hh>
 #include <hpp/core/constraint-set.hh>
+#include <hpp/core/config-projector.hh>
 
 #include "hpp/manipulation/fwd.hh"
 #include "hpp/manipulation/graph/fwd.hh"
@@ -36,9 +39,6 @@ namespace hpp {
           /// Create a new node.
           static NodePtr_t create ();
 
-          /// Create a new node with the specified constraints
-          static NodePtr_t create (const ConstraintPtr_t& constraints);
-
           /// Create a link from this node to the given node.
           EdgePtr_t linkTo(const NodePtr_t& to);
 
@@ -50,16 +50,6 @@ namespace hpp {
           /// the constraints. Instead, use the class NodeSelector.
           virtual bool contains (const Configuration_t config) const;
 
-          /// Get the constraint set associated to the node.
-          const ConstraintPtr_t constraints () const
-          {
-            return constraints_;
-          }
-
-          /// Set the constraint set associated to the node.
-          void constraints (const ConstraintPtr_t& constraints)
-            throw (Bad_function_call);
-
           /// Get the parent NodeSelector.
           NodeSelectorWkPtr_t nodeSelector ()
           {
@@ -85,9 +75,6 @@ namespace hpp {
           /// Initialize the object.
           void init (const NodeWkPtr_t& self);
 
-          /// Initialize the object.
-          void init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints);
-
           /// Constructor
           Node()
           {}
@@ -98,7 +85,7 @@ namespace hpp {
           Edges_t neighbors_;
 
           /// Set of constraints to be statisfied.
-          ConstraintPtr_t constraints_;
+          ConstraintPtr_t configConstraints_;
 
           /// A selector that will implement the selection of the next state.
           NodeSelectorWkPtr_t selector_;
@@ -106,6 +93,42 @@ namespace hpp {
           /// Weak pointer to itself.
           NodeWkPtr_t wkPtr_;
       }; // class Node
+
+      template <typename T>
+        static inline void insertListIn (const T& l, ConstraintSetPtr_t cs)
+      {
+        typename T::const_iterator it;
+        for (it = l.begin(); it != l.end(); it++)
+          cs->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, *it));
+      }
+      template <typename T>
+        static inline void insertListIn (const T& l, ConfigProjectorPtr_t cs)
+      {
+        typename T::const_iterator it;
+        for (it = l.begin(); it != l.end(); it++)
+          cs->addConstraint (*it);
+      }
+      static ConfigProjectorPtr_t buildConfigProjector (GraphWkPtr_t graph, const std::string& name)
+      {
+        GraphPtr_t g = graph.lock ();
+        if (!g) {
+          HPP_THROW_EXCEPTION(Bad_function_call, "Invalid weak_ptr to the Graph.");
+        }
+
+        ConfigProjectorPtr_t ret = ConfigProjector::
+          create(g->robot(), name, g->errorThreshold(), g->maxIterations());
+        return ret;
+      }
+      static ConstraintSetPtr_t buildConstraintSet (GraphWkPtr_t graph, const std::string& name)
+      {
+        GraphPtr_t g = graph.lock ();
+        if (!g) {
+          HPP_THROW_EXCEPTION(Bad_function_call, "Invalid weak_ptr to the Graph.");
+        }
+
+        ConstraintSetPtr_t ret = ConstraintSet::create(g->robot(), name);
+        return ret;
+      }
     } // namespace graph
   } // namespace manipulation
 } // namespace hpp

src/graph/edge.cc

@@ -27,12 +27,6 @@ namespace hpp {
         return shPtr;
       }
 
-      void Edge::constraints (const ConstraintPtr_t& constraint)
-        throw (Bad_function_call)
-      {
-        constraints_ = constraint;
-      }
-
       void Edge::init (const EdgeWkPtr_t& weak, const NodeWkPtr_t& from,
           const NodeWkPtr_t& to)
       {

src/graph/node-selector.cc

@@ -43,14 +43,6 @@ namespace hpp {
         return newNode;
       }
 
-      NodePtr_t NodeSelector::createNode (const ConstraintPtr_t& constraints)
-      {
-        NodePtr_t newNode = Node::create(constraints);
-        newNode->nodeSelector(wkPtr_);
-        orderedStates_.push_back(newNode);
-        return newNode;
-      }
-
       NodePtr_t NodeSelector::getNode(const Configuration_t config)
       {
         for (Nodes_t::iterator it = orderedStates_.begin();

src/graph/node.cc

@@ -27,32 +27,12 @@ namespace hpp {
         return shPtr;
       }
 
-      NodePtr_t Node::create (const ConstraintPtr_t& constraints)
-      {
-        Node* node = new Node;
-        NodePtr_t shPtr(node);
-        shPtr->init(shPtr, constraints);
-        return shPtr;
-      }
-
       void Node::init (const NodeWkPtr_t& weak)
       {
         GraphComponent::init (weak);
         wkPtr_ = weak;
       }
 
-      void Node::init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraint)
-      {
-        init(self);
-        constraints(constraint);
-      }
-
-      void Node::constraints (const ConstraintPtr_t& constraint)
-        throw (Bad_function_call)
-      {
-        constraints_ = constraint;
-      }
-
       EdgePtr_t Node::linkTo(const NodePtr_t& to)
       {
         EdgePtr_t newEdge = Edge::create(wkPtr_, to);
@@ -66,7 +46,7 @@ namespace hpp {
         // compute the value of the constraint instead of apllying
         // the constraint.
         Configuration_t cfg = config;
-        return constraints_->apply(cfg) && ( cfg == config );
+        return configConstraints_->apply(cfg) && ( cfg == config );
       }
 
       std::ostream& Node::print (std::ostream& os) const