Implement method to create manipulation graph

include/hpp/manipulation/graph/edge.hh

@@ -31,6 +31,16 @@ namespace hpp {
       class HPP_MANIPULATION_DLLAPI Edge
       {
         public:
+          /// Create a new Edge.
+          static EdgePtr_t create (const NodeWkPtr_t& from, const NodeWkPtr_t& to.
+              const ConstraintPtr_t& constraints)
+          {
+            Edge* ptr = new Edge;
+            EdgePtr_t shPtr (ptr);
+            ptr->init(shPtr, from, to, constraints);
+            return shPtr;
+          }
+
           /// Projector to project onto the same leaf as config.
           /// \return The initialized projector.
           /// \param config Configuration that will initialize the projector.
@@ -41,9 +51,24 @@ namespace hpp {
           /// \param config Configuration that will initialize the projector.
           ConfigProjectorPtr_t pathProjector(const Configuration_t config);
 
+        protected:
+          /// Initialization of the object.
+          void init (const EdgeWkPtr_t& weak, const NodeWkPtr_t& from,
+              const NodeWkPtr_t& to, const ConstraintPtr_t& constraints)
+          {
+            wkPtr_ = weak;
+            from_ = from;
+            to_ = to;
+            constraints_ = constraints;
+          }
+
+          /// Constructor
+          Edge()
+          {}
+
         private:
           /// The two ends of the transition.
-          NodeWkPtr_t startState, endState;
+          NodeWkPtr_t from_, to_;
 
           /// Set of constraints to be statisfied.
           ConstraintPtr_t constraints_;
@@ -55,6 +80,9 @@ namespace hpp {
           /// Projectors ensuring that a path between two configurations in
           /// the same leaf lies in the leaf.
           ConfigProjectorPtr_t pathProjector_;
+
+          /// Weak pointer to itself.
+          EdgeWkPtr_t wkPtr_;
       }; // class Edge
     } // namespace graph
   } // namespace manipulation

include/hpp/manipulation/graph/graph.hh

@@ -26,6 +26,13 @@ namespace hpp {
       /// Description of the constraint graph
       /// This class contains a graph representing a robot with several
       /// end-effectors.
+      /// 
+      /// One must make sure not to create loop with shared pointers.
+      /// To ensure that, the classes are defined as follow:
+      /// - A Graph owns (i.e. has a shared pointer to) the NodeSelector s
+      /// - A NodeSelector owns the Node s related to one gripper.
+      /// - A Node owns its outgoing Edge s.
+      /// - An Edge does not own anything.
       class HPP_MANIPULATION_DLLAPI Graph
       {
         public:

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

@@ -28,6 +28,14 @@ namespace hpp {
       class HPP_MANIPULATION_DLLAPI NodeSelector
       {
         public:
+          /// Create a nodes with the constraints
+          NodePtr_t createNode (const ConstraintPtr_t& constraints)
+          {
+            NodePtr_t newNode = Node::create(constraints);
+            orderedStates_.push_back(newNode);
+            return newNode;
+          }
+
           /// Returns the state of a configuration.
           virtual NodePtr_t getNode(const Configuration_t config) const;
 
@@ -36,7 +44,7 @@ namespace hpp {
 
         private:
           /// List of the states of one end-effector, ordered by priority.
-          std::vector< NodeWkPtr_t > orderedStates_;
+          std::vector< NodePtr_t > orderedStates_;
       }; // Class NodeSelector
     } // namespace graph
   } // namespace manipulation

include/hpp/manipulation/graph/node.hh

@@ -33,13 +33,60 @@ namespace hpp {
       class HPP_MANIPULATION_DLLAPI Node
       {
         public:
+          /// Create a new node.
+          static NodePtr_t create (const ConstraintPtr_t& constraints)
+          {
+            Node* node = new Node;
+            NodePtr_t shPtr(node);
+            shPtr->init(shPtr, constraints);
+            return shPtr;
+          }
+
+          /// Create a link from this node to the given node.
+          EdgePtr_t linkTo(const EdgePtr_t& to, const ConstraintPtr_t& constraints)
+          {
+            EdgePtr_t newEdge = Edge::create(wkPtr_, to, constraints);
+            neighbors_.push_back(newEdge);
+            newEdge->
+          }
+
           /// Check whether the configuration is in this state.
           /// \return True if this state contains this configuration
           /// \param config The configuration to be tested.
           /// \note You should note use that to know in which states a
           /// configuration is. This only checks if the configuration satisfies
           /// the constraints. Instead, use the class NodeSelector.
-          virtual bool contains(const Configuration_t config) const;
+          virtual bool contains (const Configuration_t config) const
+          {
+            // TODO: This is not the most efficient way. We should
+            // compute the value of the constraint instead of apllying
+            // the constraint.
+            ConfigurationOut_t cfg = config;
+            return constraints_->apply(cfg) && ( cfg == config );
+          }
+
+          /// 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)
+          {
+            constraints_ = constraints;
+          }
+
+        protected:
+          /// Initialize the object.
+          void init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints)
+          {
+            wkPtr_ = self;
+            constraints_ = constraints;
+          }
+
+          Node()
+          {}
 
         private:
           /// List of possible motions from this state (i.e. the outgoing
@@ -51,6 +98,9 @@ namespace hpp {
 
           /// A selector that will implement the selection of the next state.
           NodeSelectorPtr_t selector_;
+
+          /// Weak pointer to itself.
+          NodeWkPtr_t wkPtr_;
       }; // class Node
     } // namespace graph
   } // namespace manipulation