diff --git a/NEWS b/NEWS
index 2c7e97af0f3001b48a3f404f09c6563be6edc782..1004ee6a703869421e14f454a3724a9378880841 100644
--- a/NEWS
+++ b/NEWS
@@ -24,7 +24,7 @@ New in 4.10.0
   - call the problem PathValidation instead of the edge one.
 * In class ProblemSolver
   - register contact constraints and complement in constraint graph.
-  
+
 New in 4.8.0
 * Rewrite steering method CrossStateOptimization.
 * In graph component classes (State, Edge, Graph) locked joints are handled as other numerical constraints.
diff --git a/include/hpp/manipulation/connected-component.hh b/include/hpp/manipulation/connected-component.hh
index 72b003f6b71d2ad2d241f3a11743976fa79554bc..a3f47ffdb1101c972612a5f364f685055b7537a3 100644
--- a/include/hpp/manipulation/connected-component.hh
+++ b/include/hpp/manipulation/connected-component.hh
@@ -36,42 +36,45 @@
 #include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// Extension of hpp::core::connected-component. Adds a list of roadmap nodes for
-    /// every contraint graph state within the connected component. Thus every roadmap
-    /// node is assigned to a grahp state, which minimises computation time.
-class HPP_MANIPULATION_DLLAPI ConnectedComponent : public core::ConnectedComponent 
-{ 
-  public:
-      /// Map of graph states within the connected component
-      typedef std::map <graph::StatePtr_t, RoadmapNodes_t> GraphStates_t;
-     
-      ConnectedComponent() {}
+namespace manipulation {
+/// Extension of hpp::core::connected-component. Adds a list of roadmap nodes
+/// for every contraint graph state within the connected component. Thus every
+/// roadmap node is assigned to a grahp state, which minimises computation time.
+class HPP_MANIPULATION_DLLAPI ConnectedComponent
+    : public core::ConnectedComponent {
+ public:
+  /// Map of graph states within the connected component
+  typedef std::map<graph::StatePtr_t, RoadmapNodes_t> GraphStates_t;
 
-      /// return a shared pointer to new instance of manipulation::ConnectedComponent 
-      static ConnectedComponentPtr_t create (const RoadmapWkPtr_t& roadmap);
+  ConnectedComponent() {}
 
-      /// Merge two connected components (extension of core::ConnectedComponent::merge)
-      /// \param other manipulation connected component to merge into this one.
-      /// \note other will be empty after calling this method.
-      void merge (const core::ConnectedComponentPtr_t& otherCC); 
-         
-      /// Add roadmap node to connected component
-      /// \param roadmap node to be added
-      void addNode (const core::NodePtr_t& node);
+  /// return a shared pointer to new instance of
+  /// manipulation::ConnectedComponent
+  static ConnectedComponentPtr_t create(const RoadmapWkPtr_t& roadmap);
 
-      const RoadmapNodes_t& getRoadmapNodes (const graph::StatePtr_t graphState) const;
-     
-  protected:
-  private:
-      bool check () const;
-	GraphStates_t graphStateMap_;
-	// a RoadmapWkPtr_t so that memory can be released ?
-	RoadmapWkPtr_t roadmap_;
-        static RoadmapNodes_t empty_;
+  /// Merge two connected components (extension of
+  /// core::ConnectedComponent::merge) \param other manipulation connected
+  /// component to merge into this one. \note other will be empty after calling
+  /// this method.
+  void merge(const core::ConnectedComponentPtr_t& otherCC);
 
-      HPP_SERIALIZABLE();
-    }; // class ConnectedComponent
-  } //   namespace manipulation
-} // namespace hpp
-#endif // HPP_MANIPULATION_CONNECTED_COMPONENT_HH
+  /// Add roadmap node to connected component
+  /// \param roadmap node to be added
+  void addNode(const core::NodePtr_t& node);
+
+  const RoadmapNodes_t& getRoadmapNodes(
+      const graph::StatePtr_t graphState) const;
+
+ protected:
+ private:
+  bool check() const;
+  GraphStates_t graphStateMap_;
+  // a RoadmapWkPtr_t so that memory can be released ?
+  RoadmapWkPtr_t roadmap_;
+  static RoadmapNodes_t empty_;
+
+  HPP_SERIALIZABLE();
+};  // class ConnectedComponent
+}  //   namespace manipulation
+}  // namespace hpp
+#endif  // HPP_MANIPULATION_CONNECTED_COMPONENT_HH
diff --git a/include/hpp/manipulation/constraint-set.hh b/include/hpp/manipulation/constraint-set.hh
index 972914363411ca229abc8359b37aba49107b3391..ba9215c9454e17ce39ce69c2f7d5482ee760489f 100644
--- a/include/hpp/manipulation/constraint-set.hh
+++ b/include/hpp/manipulation/constraint-set.hh
@@ -27,74 +27,68 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_CONSTRAINT_SET_HH
-# define HPP_MANIPULATION_CONSTRAINT_SET_HH
+#define HPP_MANIPULATION_CONSTRAINT_SET_HH
 
-# include <hpp/core/constraint-set.hh>
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/graph/fwd.hh>
-# include <hpp/manipulation/config.hh>
+#include <hpp/core/constraint-set.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup constraints
-    /// \{
-
-    /// a core::ConstraintSet remembering which edge created it
-    class HPP_MANIPULATION_DLLAPI ConstraintSet : public core::ConstraintSet
-    {
-    public:
-      typedef core::ConstraintSet Parent_t;
-
-      /// Return shared pointer to new object
-      static ConstraintSetPtr_t create (const DevicePtr_t& robot,
-					const std::string& name);
-
-      /// Return shared pointer to new object
-      static ConstraintSetPtr_t createCopy (const ConstraintSetPtr_t& cs);
-
-      /// return shared pointer to copy
-      virtual ConstraintPtr_t copy () const;
-
-      void edge (graph::EdgePtr_t edge);
-
-      graph::EdgePtr_t edge () const;
-
-    protected:
-      /// Constructor
-      ConstraintSet (const DevicePtr_t& robot, const std::string& name);
-      /// Copy constructor
-      ConstraintSet (const ConstraintSet& other);
-      /// Store weak pointer to itself.
-      void init (const ConstraintSetPtr_t& self);
-
-      virtual std::ostream& print (std::ostream& os) const;
-
-    private:
-      graph::EdgePtr_t edge_;
-      ConstraintSetWkPtr_t weak_;
-
-      ConstraintSet() {}
-      HPP_SERIALIZABLE();
-    }; // class ConstraintSet
-
-    struct ConstraintAndComplement_t {
-      ImplicitPtr_t constraint;
-      ImplicitPtr_t complement;
-      ImplicitPtr_t both;
-      ConstraintAndComplement_t (const ImplicitPtr_t& constr,
-          const ImplicitPtr_t& comp,
-          const ImplicitPtr_t& b) :
-        constraint (constr), complement (comp), both (b)
-      {
-      }
-    };
-    typedef std::vector <ConstraintAndComplement_t>
-      ConstraintsAndComplements_t;
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+/// \addtogroup constraints
+/// \{
+
+/// a core::ConstraintSet remembering which edge created it
+class HPP_MANIPULATION_DLLAPI ConstraintSet : public core::ConstraintSet {
+ public:
+  typedef core::ConstraintSet Parent_t;
+
+  /// Return shared pointer to new object
+  static ConstraintSetPtr_t create(const DevicePtr_t& robot,
+                                   const std::string& name);
+
+  /// Return shared pointer to new object
+  static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t& cs);
+
+  /// return shared pointer to copy
+  virtual ConstraintPtr_t copy() const;
+
+  void edge(graph::EdgePtr_t edge);
+
+  graph::EdgePtr_t edge() const;
+
+ protected:
+  /// Constructor
+  ConstraintSet(const DevicePtr_t& robot, const std::string& name);
+  /// Copy constructor
+  ConstraintSet(const ConstraintSet& other);
+  /// Store weak pointer to itself.
+  void init(const ConstraintSetPtr_t& self);
+
+  virtual std::ostream& print(std::ostream& os) const;
+
+ private:
+  graph::EdgePtr_t edge_;
+  ConstraintSetWkPtr_t weak_;
+
+  ConstraintSet() {}
+  HPP_SERIALIZABLE();
+};  // class ConstraintSet
+
+struct ConstraintAndComplement_t {
+  ImplicitPtr_t constraint;
+  ImplicitPtr_t complement;
+  ImplicitPtr_t both;
+  ConstraintAndComplement_t(const ImplicitPtr_t& constr,
+                            const ImplicitPtr_t& comp, const ImplicitPtr_t& b)
+      : constraint(constr), complement(comp), both(b) {}
+};
+typedef std::vector<ConstraintAndComplement_t> ConstraintsAndComplements_t;
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
 
 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::ConstraintSet)
 
-#endif // HPP_MANIPULATION_CONSTRAINT_SET_HH
+#endif  // HPP_MANIPULATION_CONSTRAINT_SET_HH
diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index 7cf231d9bd936846449acf48ac5e713524af00fb..c46916423ffcb58e84f270988ce4f9a352081c83 100644
--- a/include/hpp/manipulation/device.hh
+++ b/include/hpp/manipulation/device.hh
@@ -29,91 +29,84 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_DEVICE_HH
-# define HPP_MANIPULATION_DEVICE_HH
+#define HPP_MANIPULATION_DEVICE_HH
 
-# include <hpp/pinocchio/humanoid-robot.hh>
+#include <hpp/core/container.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/pinocchio/humanoid-robot.hh>
 
-# include <hpp/core/container.hh>
+namespace hpp {
+namespace manipulation {
+/// Device with handles.
+///
+/// As a deriving class of hpp::pinocchio::HumanoidRobot,
+/// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot
+///
+/// This class also contains pinocchio::Gripper, Handle and \ref
+/// JointAndShapes_t
+class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot {
+ public:
+  typedef pinocchio::HumanoidRobot Parent_t;
 
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/config.hh>
+  /// Constructor
+  /// \param name of the new instance,
+  static DevicePtr_t create(const std::string& name) {
+    Device* ptr = new Device(name);
+    DevicePtr_t shPtr(ptr);
+    ptr->init(shPtr);
+    return shPtr;
+  }
 
-namespace hpp {
-  namespace manipulation {
-    /// Device with handles.
-    ///
-    /// As a deriving class of hpp::pinocchio::HumanoidRobot,
-    /// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot
-    ///
-    /// This class also contains pinocchio::Gripper, Handle and \ref JointAndShapes_t
-    class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot
-    {
-      public:
-        typedef pinocchio::HumanoidRobot Parent_t;
-
-        /// Constructor
-        /// \param name of the new instance,
-        static DevicePtr_t create (const std::string& name)
-        {
-          Device* ptr = new Device (name);
-          DevicePtr_t shPtr (ptr);
-          ptr->init (shPtr);
-          return shPtr;
-        }
-
-        DevicePtr_t self () const { return self_.lock(); }
-
-        /// Print object in a stream
-        virtual std::ostream& print (std::ostream& os) const;
-
-        void setRobotRootPosition (const std::string& robotName,
-                                   const Transform3f& positionWRTParentJoint);
-
-        virtual pinocchio::DevicePtr_t clone () const;
-
-        std::vector<std::string> robotNames () const;
-
-        FrameIndices_t robotFrames (const std::string& robotName) const;
-
-        void removeJoints(const std::vector<std::string>& jointNames,
-            Configuration_t referenceConfig);
-
-        core::Container <HandlePtr_t> handles;
-        core::Container <GripperPtr_t> grippers;
-        core::Container <JointAndShapes_t> jointAndShapes;
-
-      protected:
-        /// Constructor
-        /// \param name of the new instance,
-        /// \param robot Robots that manipulate objects,
-        /// \param objects Set of objects manipulated by the robot.
-        Device (const std::string& name) :
-          Parent_t (name)
-        {}
-
-        void init (const DeviceWkPtr_t& self)
-        {
-          Parent_t::init (self);
-          self_ = self;
-        }
-
-        void initCopy (const DeviceWkPtr_t& self, const Device& other)
-        {
-          Parent_t::initCopy (self, other);
-          self_ = self;
-        }
-
-        /// For serialization only
-        Device() {}
-
-      private:
-        DeviceWkPtr_t self_;
-
-        HPP_SERIALIZABLE();
-    }; // class Device
-  } // namespace manipulation
-} // namespace hpp
+  DevicePtr_t self() const { return self_.lock(); }
+
+  /// Print object in a stream
+  virtual std::ostream& print(std::ostream& os) const;
+
+  void setRobotRootPosition(const std::string& robotName,
+                            const Transform3f& positionWRTParentJoint);
+
+  virtual pinocchio::DevicePtr_t clone() const;
+
+  std::vector<std::string> robotNames() const;
+
+  FrameIndices_t robotFrames(const std::string& robotName) const;
+
+  void removeJoints(const std::vector<std::string>& jointNames,
+                    Configuration_t referenceConfig);
+
+  core::Container<HandlePtr_t> handles;
+  core::Container<GripperPtr_t> grippers;
+  core::Container<JointAndShapes_t> jointAndShapes;
+
+ protected:
+  /// Constructor
+  /// \param name of the new instance,
+  /// \param robot Robots that manipulate objects,
+  /// \param objects Set of objects manipulated by the robot.
+  Device(const std::string& name) : Parent_t(name) {}
+
+  void init(const DeviceWkPtr_t& self) {
+    Parent_t::init(self);
+    self_ = self;
+  }
+
+  void initCopy(const DeviceWkPtr_t& self, const Device& other) {
+    Parent_t::initCopy(self, other);
+    self_ = self;
+  }
+
+  /// For serialization only
+  Device() {}
+
+ private:
+  DeviceWkPtr_t self_;
+
+  HPP_SERIALIZABLE();
+};  // class Device
+}  // namespace manipulation
+}  // namespace hpp
 
 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::Device)
 
-#endif // HPP_MANIPULATION_DEVICE_HH
+#endif  // HPP_MANIPULATION_DEVICE_HH
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 96d12d85d7d77cab68a2400d18f39a9ce1479692..5bd1a21d64ff85bca71c724b249a52c0add0783d 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -29,134 +29,131 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_FWD_HH
-# define HPP_MANIPULATION_FWD_HH
+#define HPP_MANIPULATION_FWD_HH
 
-# include <map>
-# include <hpp/core/fwd.hh>
+#include <hpp/core/fwd.hh>
+#include <map>
 
 namespace hpp {
-  namespace manipulation {
-    HPP_PREDEF_CLASS (Device);
-    typedef shared_ptr <Device> DevicePtr_t;
-    typedef shared_ptr <const Device> DeviceConstPtr_t;
-    typedef pinocchio::Joint Joint;
-    typedef pinocchio::JointPtr_t JointPtr_t;
-    typedef pinocchio::JointIndex JointIndex;
-    typedef std::vector<JointIndex> JointIndices_t;
-    typedef pinocchio::FrameIndex FrameIndex;
-    typedef std::vector<pinocchio::FrameIndex> FrameIndices_t;
-    typedef pinocchio::Configuration_t Configuration_t;
-    typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
-    typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
-    typedef core::ConfigurationPtr_t ConfigurationPtr_t;
-    typedef pinocchio::GripperPtr_t GripperPtr_t;
-    typedef pinocchio::LiegroupElement LiegroupElement;
-    typedef pinocchio::LiegroupSpace LiegroupSpace;
-    typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
-    HPP_PREDEF_CLASS (AxialHandle);
-    typedef shared_ptr <AxialHandle> AxialHandlePtr_t;
-    HPP_PREDEF_CLASS (Handle);
-    typedef shared_ptr <Handle> HandlePtr_t;
-    HPP_PREDEF_CLASS (Object);
-    typedef shared_ptr <Object> ObjectPtr_t;
-    typedef shared_ptr <const Object> ObjectConstPtr_t;
-    HPP_PREDEF_CLASS (ProblemSolver);
-    typedef ProblemSolver* ProblemSolverPtr_t;
-    HPP_PREDEF_CLASS (Problem);
-    typedef shared_ptr <Problem> ProblemPtr_t;
-    typedef shared_ptr <const Problem> ProblemConstPtr_t;
-    HPP_PREDEF_CLASS (Roadmap);
-    typedef shared_ptr <Roadmap> RoadmapPtr_t;
-    HPP_PREDEF_CLASS (RoadmapNode);
-    typedef RoadmapNode* RoadmapNodePtr_t;
-    typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
-    HPP_PREDEF_CLASS (ConnectedComponent);
-    typedef shared_ptr<ConnectedComponent> ConnectedComponentPtr_t; 
-    HPP_PREDEF_CLASS (LeafConnectedComp);
-    typedef shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
-    typedef shared_ptr<const LeafConnectedComp>
-    LeafConnectedCompConstPtr_t;
-    typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
-    HPP_PREDEF_CLASS (WeighedLeafConnectedComp);
-    typedef shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
-    HPP_PREDEF_CLASS (WeighedDistance);
-    typedef shared_ptr<WeighedDistance> WeighedDistancePtr_t;
-    typedef constraints::RelativeOrientation RelativeOrientation;
-    typedef constraints::RelativePosition RelativePosition;
-    typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
-    typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
-    typedef constraints::RelativeTransformation RelativeTransformation;
-    typedef constraints::RelativeTransformationR3xSO3
-    RelativeTransformationR3xSO3;
-    typedef constraints::RelativeTransformationPtr_t
-    RelativeTransformationPtr_t;
-    typedef core::value_type value_type;
-    typedef core::size_type size_type;
-    typedef core::Transform3f Transform3f;
-    typedef core::vector_t vector_t;
-    typedef core::vectorIn_t vectorIn_t;
-    typedef core::vectorOut_t vectorOut_t;
-    HPP_PREDEF_CLASS (ManipulationPlanner);
-    typedef shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
-    HPP_PREDEF_CLASS (GraphPathValidation);
-    typedef shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
-    HPP_PREDEF_CLASS (SteeringMethod);
-    typedef shared_ptr < SteeringMethod > SteeringMethodPtr_t;
-    typedef core::PathOptimizer PathOptimizer;
-    typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
-    HPP_PREDEF_CLASS (GraphOptimizer);
-    typedef shared_ptr < GraphOptimizer > GraphOptimizerPtr_t;
-    HPP_PREDEF_CLASS (GraphNodeOptimizer);
-    typedef shared_ptr < GraphNodeOptimizer > GraphNodeOptimizerPtr_t;
-    typedef core::PathProjectorPtr_t PathProjectorPtr_t;
+namespace manipulation {
+HPP_PREDEF_CLASS(Device);
+typedef shared_ptr<Device> DevicePtr_t;
+typedef shared_ptr<const Device> DeviceConstPtr_t;
+typedef pinocchio::Joint Joint;
+typedef pinocchio::JointPtr_t JointPtr_t;
+typedef pinocchio::JointIndex JointIndex;
+typedef std::vector<JointIndex> JointIndices_t;
+typedef pinocchio::FrameIndex FrameIndex;
+typedef std::vector<pinocchio::FrameIndex> FrameIndices_t;
+typedef pinocchio::Configuration_t Configuration_t;
+typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
+typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
+typedef core::ConfigurationPtr_t ConfigurationPtr_t;
+typedef pinocchio::GripperPtr_t GripperPtr_t;
+typedef pinocchio::LiegroupElement LiegroupElement;
+typedef pinocchio::LiegroupSpace LiegroupSpace;
+typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
+HPP_PREDEF_CLASS(AxialHandle);
+typedef shared_ptr<AxialHandle> AxialHandlePtr_t;
+HPP_PREDEF_CLASS(Handle);
+typedef shared_ptr<Handle> HandlePtr_t;
+HPP_PREDEF_CLASS(Object);
+typedef shared_ptr<Object> ObjectPtr_t;
+typedef shared_ptr<const Object> ObjectConstPtr_t;
+HPP_PREDEF_CLASS(ProblemSolver);
+typedef ProblemSolver* ProblemSolverPtr_t;
+HPP_PREDEF_CLASS(Problem);
+typedef shared_ptr<Problem> ProblemPtr_t;
+typedef shared_ptr<const Problem> ProblemConstPtr_t;
+HPP_PREDEF_CLASS(Roadmap);
+typedef shared_ptr<Roadmap> RoadmapPtr_t;
+HPP_PREDEF_CLASS(RoadmapNode);
+typedef RoadmapNode* RoadmapNodePtr_t;
+typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
+HPP_PREDEF_CLASS(ConnectedComponent);
+typedef shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
+HPP_PREDEF_CLASS(LeafConnectedComp);
+typedef shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
+typedef shared_ptr<const LeafConnectedComp> LeafConnectedCompConstPtr_t;
+typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
+HPP_PREDEF_CLASS(WeighedLeafConnectedComp);
+typedef shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
+HPP_PREDEF_CLASS(WeighedDistance);
+typedef shared_ptr<WeighedDistance> WeighedDistancePtr_t;
+typedef constraints::RelativeOrientation RelativeOrientation;
+typedef constraints::RelativePosition RelativePosition;
+typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
+typedef constraints::RelativePositionPtr_t RelativePositionPtr_t;
+typedef constraints::RelativeTransformation RelativeTransformation;
+typedef constraints::RelativeTransformationR3xSO3 RelativeTransformationR3xSO3;
+typedef constraints::RelativeTransformationPtr_t RelativeTransformationPtr_t;
+typedef core::value_type value_type;
+typedef core::size_type size_type;
+typedef core::Transform3f Transform3f;
+typedef core::vector_t vector_t;
+typedef core::vectorIn_t vectorIn_t;
+typedef core::vectorOut_t vectorOut_t;
+HPP_PREDEF_CLASS(ManipulationPlanner);
+typedef shared_ptr<ManipulationPlanner> ManipulationPlannerPtr_t;
+HPP_PREDEF_CLASS(GraphPathValidation);
+typedef shared_ptr<GraphPathValidation> GraphPathValidationPtr_t;
+HPP_PREDEF_CLASS(SteeringMethod);
+typedef shared_ptr<SteeringMethod> SteeringMethodPtr_t;
+typedef core::PathOptimizer PathOptimizer;
+typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
+HPP_PREDEF_CLASS(GraphOptimizer);
+typedef shared_ptr<GraphOptimizer> GraphOptimizerPtr_t;
+HPP_PREDEF_CLASS(GraphNodeOptimizer);
+typedef shared_ptr<GraphNodeOptimizer> GraphNodeOptimizerPtr_t;
+typedef core::PathProjectorPtr_t PathProjectorPtr_t;
 
-    typedef std::vector <pinocchio::DevicePtr_t> Devices_t;
-    typedef std::vector <ObjectPtr_t> Objects_t;
-    typedef core::Constraint Constraint;
-    typedef core::ConstraintPtr_t ConstraintPtr_t;
-    typedef constraints::Explicit Explicit;
-    typedef constraints::ExplicitPtr_t ExplicitPtr_t;
-    typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-    typedef constraints::LockedJoint LockedJoint;
-    typedef constraints::LockedJointPtr_t LockedJointPtr_t;
-    typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
-    typedef core::ConfigProjector ConfigProjector;
-    typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
-    HPP_PREDEF_CLASS (ConstraintSet);
-    typedef shared_ptr <ConstraintSet> ConstraintSetPtr_t;
-    typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
-    typedef core::ConfigurationShooter ConfigurationShooter;
-    typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
-    typedef core::ValidationReport ValidationReport;
-    typedef core::NumericalConstraints_t NumericalConstraints_t;
-    typedef core::PathValidationPtr_t PathValidationPtr_t;
-    typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
-    typedef core::matrix_t matrix_t;
-    typedef core::matrixIn_t matrixIn_t;
-    typedef core::matrixOut_t matrixOut_t;
-    typedef core::size_type size_type;
-    typedef core::value_type value_type;
-    typedef core::vector3_t vector3_t;
-    typedef core::matrix3_t matrix3_t;
+typedef std::vector<pinocchio::DevicePtr_t> Devices_t;
+typedef std::vector<ObjectPtr_t> Objects_t;
+typedef core::Constraint Constraint;
+typedef core::ConstraintPtr_t ConstraintPtr_t;
+typedef constraints::Explicit Explicit;
+typedef constraints::ExplicitPtr_t ExplicitPtr_t;
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+typedef constraints::LockedJoint LockedJoint;
+typedef constraints::LockedJointPtr_t LockedJointPtr_t;
+typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
+typedef core::ConfigProjector ConfigProjector;
+typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
+HPP_PREDEF_CLASS(ConstraintSet);
+typedef shared_ptr<ConstraintSet> ConstraintSetPtr_t;
+typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
+typedef core::ConfigurationShooter ConfigurationShooter;
+typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
+typedef core::ValidationReport ValidationReport;
+typedef core::NumericalConstraints_t NumericalConstraints_t;
+typedef core::PathValidationPtr_t PathValidationPtr_t;
+typedef core::PathValidationReportPtr_t PathValidationReportPtr_t;
+typedef core::matrix_t matrix_t;
+typedef core::matrixIn_t matrixIn_t;
+typedef core::matrixOut_t matrixOut_t;
+typedef core::size_type size_type;
+typedef core::value_type value_type;
+typedef core::vector3_t vector3_t;
+typedef core::matrix3_t matrix3_t;
 
-    typedef core::Shape_t Shape_t;
-    typedef core::JointAndShape_t JointAndShape_t;
-    typedef core::JointAndShapes_t JointAndShapes_t;
+typedef core::Shape_t Shape_t;
+typedef core::JointAndShape_t JointAndShape_t;
+typedef core::JointAndShapes_t JointAndShapes_t;
 
-    typedef std::list <std::string> StringList_t;
+typedef std::list<std::string> StringList_t;
 
-    namespace pathOptimization {
-      HPP_PREDEF_CLASS (SmallSteps);
-      typedef shared_ptr < SmallSteps > SmallStepsPtr_t;
-      HPP_PREDEF_CLASS (Keypoints);
-      typedef shared_ptr < Keypoints > KeypointsPtr_t;
-    } // namespace pathOptimization
+namespace pathOptimization {
+HPP_PREDEF_CLASS(SmallSteps);
+typedef shared_ptr<SmallSteps> SmallStepsPtr_t;
+HPP_PREDEF_CLASS(Keypoints);
+typedef shared_ptr<Keypoints> KeypointsPtr_t;
+}  // namespace pathOptimization
 
-    namespace problemTarget {
-      HPP_PREDEF_CLASS (State);
-      typedef shared_ptr < State > StatePtr_t;
-    } // namespace problemTarget
-  } // namespace manipulation
-} // namespace hpp
+namespace problemTarget {
+HPP_PREDEF_CLASS(State);
+typedef shared_ptr<State> StatePtr_t;
+}  // namespace problemTarget
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_FWD_HH
+#endif  // HPP_MANIPULATION_FWD_HH
diff --git a/include/hpp/manipulation/graph-node-optimizer.hh b/include/hpp/manipulation/graph-node-optimizer.hh
index 887a6953275e071d76765f001b46cc2c7ffc946d..654dacda8f46f57052c6520d586bbf6af21c53c1 100644
--- a/include/hpp/manipulation/graph-node-optimizer.hh
+++ b/include/hpp/manipulation/graph-node-optimizer.hh
@@ -27,52 +27,48 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
-# define HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
+#define HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
 
-# include <hpp/core/path.hh>
-# include <hpp/core/path-vector.hh>
-# include <hpp/core/path-optimizer.hh>
-# include <hpp/core/problem.hh>
-# include <hpp/core/problem-solver.hh>
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/graph/fwd.hh>
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/constraint-set.hh>
+#include <hpp/core/path-optimizer.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/path.hh>
+#include <hpp/core/problem-solver.hh>
+#include <hpp/core/problem.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    using hpp::core::Path;
-    using hpp::core::PathPtr_t;
-    using hpp::core::PathVector;
-    using hpp::core::PathVectorPtr_t;
+namespace manipulation {
+using hpp::core::Path;
+using hpp::core::PathPtr_t;
+using hpp::core::PathVector;
+using hpp::core::PathVectorPtr_t;
 
-    /// \addtogroup path_optimization
-    /// \{
+/// \addtogroup path_optimization
+/// \{
 
-    /// Path optimizer that recompute the edge parameter of the constraints
-    ///
-    /// This class encapsulates another path optimizer class. This optimizer
-    /// calls the inner optimizer on every subpaths with the same set of
-    /// constraints.
-    class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer
-    {
-      public:
-        static GraphNodeOptimizerPtr_t create
-	  (const core::ProblemConstPtr_t& problem);
+/// Path optimizer that recompute the edge parameter of the constraints
+///
+/// This class encapsulates another path optimizer class. This optimizer
+/// calls the inner optimizer on every subpaths with the same set of
+/// constraints.
+class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer {
+ public:
+  static GraphNodeOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
 
-        virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
+  virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
 
-      protected:
-        /// Constructor
-        GraphNodeOptimizer (const core::ProblemConstPtr_t& problem) :
-          PathOptimizer (problem)
-        {}
+ protected:
+  /// Constructor
+  GraphNodeOptimizer(const core::ProblemConstPtr_t& problem)
+      : PathOptimizer(problem) {}
 
-      private:
-    };
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
+ private:
+};
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
+#endif  // HPP_MANIPULATION_GRAPH_NODE_OPTIMIZER_HH
diff --git a/include/hpp/manipulation/graph-optimizer.hh b/include/hpp/manipulation/graph-optimizer.hh
index 6ed4dae80bf07303337e32f41a480e525e393e81..6aac4d13f65143b13b42f1655c2f5bc7d4c9097a 100644
--- a/include/hpp/manipulation/graph-optimizer.hh
+++ b/include/hpp/manipulation/graph-optimizer.hh
@@ -26,75 +26,65 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-
 #ifndef HPP_MANIPULATION_GRAPHOPTIMIZER_HH
-# define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
-
-# include <hpp/core/path-optimizer.hh>
-# include <hpp/core/problem-solver.hh> // PathOptimizerBuilder_t
+#define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
 
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/graph/fwd.hh>
+#include <hpp/core/path-optimizer.hh>
+#include <hpp/core/problem-solver.hh>  // PathOptimizerBuilder_t
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    using hpp::core::Path;
-    using hpp::core::PathPtr_t;
-    using hpp::core::PathVector;
-    using hpp::core::PathVectorPtr_t;
-
-    /// \addtogroup path_optimization
-    /// \{
-
-    /// Path optimizer for paths created with the constraint graph
-    ///
-    /// This class encapsulates another path optimizer class. This optimizer
-    /// calls the inner optimizer on every subpaths with the same set of
-    /// constraints.
-    class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer
-    {
-      public:
-        typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
-
-        template <typename TraitsOrInnerType>
-          static GraphOptimizerPtr_t create
-	  (const core::ProblemConstPtr_t& problem);
-
-        virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
-
-        /// Get the encapsulated optimizer
-        const PathOptimizerPtr_t& innerOptimizer ()
-        {
-          return pathOptimizer_;
-        }
-
-      protected:
-        /// Constructor
-        GraphOptimizer (const core::ProblemConstPtr_t& problem,
-			PathOptimizerBuilder_t factory) :
-          PathOptimizer (problem), factory_ (factory), pathOptimizer_ ()
-        {}
-
-      private:
-        PathOptimizerBuilder_t factory_;
-
-        /// The encapsulated PathOptimizer
-        PathOptimizerPtr_t pathOptimizer_;
-    };
-    /// \}
-
-    /// Member function definition
-    template <typename TraitsOrInnerType>
-      GraphOptimizerPtr_t GraphOptimizer::create
-      (const core::ProblemConstPtr_t& problem)
-    {
-      return GraphOptimizerPtr_t (
-          new GraphOptimizer (problem, TraitsOrInnerType::create)
-          );
-    }
-
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
+namespace manipulation {
+using hpp::core::Path;
+using hpp::core::PathPtr_t;
+using hpp::core::PathVector;
+using hpp::core::PathVectorPtr_t;
+
+/// \addtogroup path_optimization
+/// \{
+
+/// Path optimizer for paths created with the constraint graph
+///
+/// This class encapsulates another path optimizer class. This optimizer
+/// calls the inner optimizer on every subpaths with the same set of
+/// constraints.
+class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer {
+ public:
+  typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
+
+  template <typename TraitsOrInnerType>
+  static GraphOptimizerPtr_t create(const core::ProblemConstPtr_t& problem);
+
+  virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
+
+  /// Get the encapsulated optimizer
+  const PathOptimizerPtr_t& innerOptimizer() { return pathOptimizer_; }
+
+ protected:
+  /// Constructor
+  GraphOptimizer(const core::ProblemConstPtr_t& problem,
+                 PathOptimizerBuilder_t factory)
+      : PathOptimizer(problem), factory_(factory), pathOptimizer_() {}
+
+ private:
+  PathOptimizerBuilder_t factory_;
+
+  /// The encapsulated PathOptimizer
+  PathOptimizerPtr_t pathOptimizer_;
+};
+/// \}
+
+/// Member function definition
+template <typename TraitsOrInnerType>
+GraphOptimizerPtr_t GraphOptimizer::create(
+    const core::ProblemConstPtr_t& problem) {
+  return GraphOptimizerPtr_t(
+      new GraphOptimizer(problem, TraitsOrInnerType::create));
+}
+
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
index e024216dbbccfdaf7e4d2141517e35b4c6889f30..46754f7890257bb22fb3962bd719a3aa305bf6aa 100644
--- a/include/hpp/manipulation/graph-path-validation.hh
+++ b/include/hpp/manipulation/graph-path-validation.hh
@@ -26,182 +26,167 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-
 #ifndef HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
-# define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
-
-# include <hpp/core/path-validation.hh>
-# include <hpp/core/obstacle-user.hh>
+#define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
 
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/graph/fwd.hh>
+#include <hpp/core/obstacle-user.hh>
+#include <hpp/core/path-validation.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    using hpp::core::PathValidation;
-    using hpp::core::PathValidationPtr_t;
-    using hpp::core::Path;
-    using hpp::core::PathPtr_t;
-    using hpp::core::PathVector;
-    using hpp::core::PathVectorPtr_t;
-
-    /// \addtogroup validation
-    /// \{
-
-    /// Path validation for a constraint graph
-    ///
-    /// This class encapsulates another path validation class.
-    /// The encapsulated path validation is responsible for collision
-    /// checking, whereas this class checks if a path is valid regarding
-    /// the constraint graph.
-    class HPP_MANIPULATION_DLLAPI GraphPathValidation :
-      public PathValidation,
-      public core::ObstacleUserInterface
-    {
-      public:
-	/// Check that path is valid regarding the constraint graph.
-	///
-	/// \param path the path to check for validity,
-	/// \param reverse if true check from the end,
-	/// \retval the extracted valid part of the path, pointer to path if
-	///         path is valid,
-	/// \retval report information about the validation process. unused in
-	///         this case,
-	/// \return whether the whole path is valid.
-        ///
-        /// \notice Call the encapsulated PathValidation::validate.
-	virtual bool validate (const PathPtr_t& path, bool reverse,
-			       PathPtr_t& validPart,
-			       PathValidationReportPtr_t& report);
-
-        /// Set the encapsulated path validator.
-        void innerValidation (const PathValidationPtr_t& pathValidation)
-        {
-          pathValidation_ = pathValidation;
-        }
-
-        /// Get the encapsulated path validator.
-        const PathValidationPtr_t& innerValidation ()
-        {
-          return pathValidation_;
-        }
-
-        /// Set the graph of constraints
-        void constraintGraph (const graph::GraphPtr_t& graph)
-        {
-          constraintGraph_ = graph;
-        }
-
-        /// Get the graph of constraints
-        graph::GraphPtr_t constraintGraph () const
-        {
-          return constraintGraph_;
-        }
-
-        /// Create a new instance of this class.
-        /// \param pathValidation a PathValidation that is responsible for collision
-        //         checking.
-        //  \param graph A pointer to the constraint graph.
-        static GraphPathValidationPtr_t create (
-            const PathValidationPtr_t& pathValidation);
-
-        template <typename T>
-          static GraphPathValidationPtr_t create (
-              const pinocchio::DevicePtr_t& robot, const value_type& stepSize);
-
-        /// Add obstacle in the environment
-        ///
-        /// Dynamic cast inner path validation into
-        /// hpp::core::ObstacleUserInterface and calls
-        /// hpp::core::ObstacleUserInterface::addObstacle in case of success.
-        void addObstacle (const hpp::core::CollisionObjectConstPtr_t& object)
-        {
-          shared_ptr<core::ObstacleUserInterface> oui =
-            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
-          if (oui) oui->addObstacle (object);
-        }
-
-        /// Remove a collision pair between a joint and an obstacle
-        ///
-        /// Dynamic cast inner path validation into
-        /// hpp::core::ObstacleUserInterface and calls
-        /// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
-        /// of success.
-        void removeObstacleFromJoint (const JointPtr_t& joint,
-            const pinocchio::CollisionObjectConstPtr_t& obstacle)
-        {
-          shared_ptr<core::ObstacleUserInterface> oui =
-            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
-          if (oui) oui->removeObstacleFromJoint (joint, obstacle);
-        }
-
-        /// \brief Filter collision pairs.
-        ///
-        /// Dynamic cast inner path validation into
-        /// hpp::core::ObstacleUserInterface and calls
-        /// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
-        /// success.
-        void filterCollisionPairs (const core::RelativeMotion::matrix_type& relMotion)
-        {
-          shared_ptr<core::ObstacleUserInterface> oui =
-            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
-          if (oui) oui->filterCollisionPairs (relMotion);
-        }
-
-        /// Set different security margins for collision pairs
-        ///
-        /// Dynamic cast inner path validation into
-        /// hpp::core::ObstacleUserInterface and calls
-        /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
-        /// success.
-        void setSecurityMargins(const matrix_t& securityMargins)
-        {
-          shared_ptr<core::ObstacleUserInterface> oui =
-            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
-          if (oui) oui->setSecurityMargins(securityMargins);
-        }
-
-        /// \copydoc hpp::core::ObstacleUserInterface::setSecurityMarginBetweenBodies
-        ///
-        /// Dynamic cast inner path validation into
-        /// hpp::core::ObstacleUserInterface and calls
-        /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
-        /// success.
-        void setSecurityMarginBetweenBodies(const std::string& body_a,
-                                            const std::string& body_b,
-                                            const value_type& margin)
-        {
-          shared_ptr<core::ObstacleUserInterface> oui =
-            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
-          if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin);
-        }
-
-      protected:
-        /// Constructor
-        GraphPathValidation (const PathValidationPtr_t& pathValidation);
-
-      private:
-        /// Do validation regarding the constraint graph for PathVector
-        bool impl_validate (const PathVectorPtr_t& path, bool reverse,
-            PathPtr_t& validPart, PathValidationReportPtr_t& report);
-        /// Do validation regarding the constraint graph for Path 
-        bool impl_validate (const PathPtr_t& path, bool reverse,
-            PathPtr_t& validPart, PathValidationReportPtr_t& report);
-        /// The encapsulated PathValidation.
-        PathValidationPtr_t pathValidation_;
-        /// Pointer to the constraint graph.
-        graph::GraphPtr_t constraintGraph_;
-    };
-
-    template <typename T>
-      GraphPathValidationPtr_t GraphPathValidation::create
-      (const pinocchio::DevicePtr_t& robot, const value_type& stepSize)
-    {
-      return GraphPathValidation::create (T::create (robot, stepSize));
-    }
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
+namespace manipulation {
+using hpp::core::Path;
+using hpp::core::PathPtr_t;
+using hpp::core::PathValidation;
+using hpp::core::PathValidationPtr_t;
+using hpp::core::PathVector;
+using hpp::core::PathVectorPtr_t;
+
+/// \addtogroup validation
+/// \{
+
+/// Path validation for a constraint graph
+///
+/// This class encapsulates another path validation class.
+/// The encapsulated path validation is responsible for collision
+/// checking, whereas this class checks if a path is valid regarding
+/// the constraint graph.
+class HPP_MANIPULATION_DLLAPI GraphPathValidation
+    : public PathValidation,
+      public core::ObstacleUserInterface {
+ public:
+  /// Check that path is valid regarding the constraint graph.
+  ///
+  /// \param path the path to check for validity,
+  /// \param reverse if true check from the end,
+  /// \retval the extracted valid part of the path, pointer to path if
+  ///         path is valid,
+  /// \retval report information about the validation process. unused in
+  ///         this case,
+  /// \return whether the whole path is valid.
+  ///
+  /// \notice Call the encapsulated PathValidation::validate.
+  virtual bool validate(const PathPtr_t& path, bool reverse,
+                        PathPtr_t& validPart,
+                        PathValidationReportPtr_t& report);
+
+  /// Set the encapsulated path validator.
+  void innerValidation(const PathValidationPtr_t& pathValidation) {
+    pathValidation_ = pathValidation;
+  }
+
+  /// Get the encapsulated path validator.
+  const PathValidationPtr_t& innerValidation() { return pathValidation_; }
+
+  /// Set the graph of constraints
+  void constraintGraph(const graph::GraphPtr_t& graph) {
+    constraintGraph_ = graph;
+  }
+
+  /// Get the graph of constraints
+  graph::GraphPtr_t constraintGraph() const { return constraintGraph_; }
+
+  /// Create a new instance of this class.
+  /// \param pathValidation a PathValidation that is responsible for collision
+  //         checking.
+  //  \param graph A pointer to the constraint graph.
+  static GraphPathValidationPtr_t create(
+      const PathValidationPtr_t& pathValidation);
+
+  template <typename T>
+  static GraphPathValidationPtr_t create(const pinocchio::DevicePtr_t& robot,
+                                         const value_type& stepSize);
+
+  /// Add obstacle in the environment
+  ///
+  /// Dynamic cast inner path validation into
+  /// hpp::core::ObstacleUserInterface and calls
+  /// hpp::core::ObstacleUserInterface::addObstacle in case of success.
+  void addObstacle(const hpp::core::CollisionObjectConstPtr_t& object) {
+    shared_ptr<core::ObstacleUserInterface> oui =
+        HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+    if (oui) oui->addObstacle(object);
+  }
+
+  /// Remove a collision pair between a joint and an obstacle
+  ///
+  /// Dynamic cast inner path validation into
+  /// hpp::core::ObstacleUserInterface and calls
+  /// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
+  /// of success.
+  void removeObstacleFromJoint(
+      const JointPtr_t& joint,
+      const pinocchio::CollisionObjectConstPtr_t& obstacle) {
+    shared_ptr<core::ObstacleUserInterface> oui =
+        HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+    if (oui) oui->removeObstacleFromJoint(joint, obstacle);
+  }
+
+  /// \brief Filter collision pairs.
+  ///
+  /// Dynamic cast inner path validation into
+  /// hpp::core::ObstacleUserInterface and calls
+  /// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
+  /// success.
+  void filterCollisionPairs(
+      const core::RelativeMotion::matrix_type& relMotion) {
+    shared_ptr<core::ObstacleUserInterface> oui =
+        HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+    if (oui) oui->filterCollisionPairs(relMotion);
+  }
+
+  /// Set different security margins for collision pairs
+  ///
+  /// Dynamic cast inner path validation into
+  /// hpp::core::ObstacleUserInterface and calls
+  /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
+  /// success.
+  void setSecurityMargins(const matrix_t& securityMargins) {
+    shared_ptr<core::ObstacleUserInterface> oui =
+        HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+    if (oui) oui->setSecurityMargins(securityMargins);
+  }
+
+  /// \copydoc hpp::core::ObstacleUserInterface::setSecurityMarginBetweenBodies
+  ///
+  /// Dynamic cast inner path validation into
+  /// hpp::core::ObstacleUserInterface and calls
+  /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
+  /// success.
+  void setSecurityMarginBetweenBodies(const std::string& body_a,
+                                      const std::string& body_b,
+                                      const value_type& margin) {
+    shared_ptr<core::ObstacleUserInterface> oui =
+        HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+    if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin);
+  }
+
+ protected:
+  /// Constructor
+  GraphPathValidation(const PathValidationPtr_t& pathValidation);
+
+ private:
+  /// Do validation regarding the constraint graph for PathVector
+  bool impl_validate(const PathVectorPtr_t& path, bool reverse,
+                     PathPtr_t& validPart, PathValidationReportPtr_t& report);
+  /// Do validation regarding the constraint graph for Path
+  bool impl_validate(const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
+                     PathValidationReportPtr_t& report);
+  /// The encapsulated PathValidation.
+  PathValidationPtr_t pathValidation_;
+  /// Pointer to the constraint graph.
+  graph::GraphPtr_t constraintGraph_;
+};
+
+template <typename T>
+GraphPathValidationPtr_t GraphPathValidation::create(
+    const pinocchio::DevicePtr_t& robot, const value_type& stepSize) {
+  return GraphPathValidation::create(T::create(robot, stepSize));
+}
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
diff --git a/include/hpp/manipulation/graph/dot.hh b/include/hpp/manipulation/graph/dot.hh
index 66151918e7e32da4298bd068529941ca764dfcdf..7bde131c07d438c2736aaa545745546a9855609d 100644
--- a/include/hpp/manipulation/graph/dot.hh
+++ b/include/hpp/manipulation/graph/dot.hh
@@ -27,67 +27,62 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_DOT_HH
-# define HPP_MANIPULATION_GRAPH_DOT_HH
+#define HPP_MANIPULATION_GRAPH_DOT_HH
 
-# include <ostream>
-# include <sstream>
-# include <map>
-# include <list>
+#include <list>
+#include <map>
+#include <ostream>
+#include <sstream>
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      namespace dot {
+namespace manipulation {
+namespace graph {
+namespace dot {
 
-        struct DrawingAttributes {
-          typedef std::pair <std::string, std::string> Pair;
-          typedef std::map <std::string, std::string> Map;
+struct DrawingAttributes {
+  typedef std::pair<std::string, std::string> Pair;
+  typedef std::map<std::string, std::string> Map;
 
-          std::string separator, openSection, closeSection;
-          Map attr;
+  std::string separator, openSection, closeSection;
+  Map attr;
 
-          inline void insertWithQuote (const std::string& K, const std::string& V) {
-            attr.insert (Pair (K, "\"" + V + "\""));
-          }
-          inline void insert (const std::string& K, const std::string& V) {
-            attr.insert (Pair (K, V));
-          }
-          std::string& operator [] (const std::string& K) {
-            return attr [K];
-          }
-          DrawingAttributes () :
-            separator (", "), openSection ("["), closeSection ("]"),
-            attr () {};
-        };
+  inline void insertWithQuote(const std::string& K, const std::string& V) {
+    attr.insert(Pair(K, "\"" + V + "\""));
+  }
+  inline void insert(const std::string& K, const std::string& V) {
+    attr.insert(Pair(K, V));
+  }
+  std::string& operator[](const std::string& K) { return attr[K]; }
+  DrawingAttributes()
+      : separator(", "), openSection("["), closeSection("]"), attr(){};
+};
 
-        struct Tooltip {
-          static const std::string tooltipendl;
-          typedef std::list <std::string> TooltipLineVector;
-          TooltipLineVector v;
+struct Tooltip {
+  static const std::string tooltipendl;
+  typedef std::list<std::string> TooltipLineVector;
+  TooltipLineVector v;
 
-          Tooltip () : v() {};
-          inline std::string toStr () const {
-            std::stringstream ss;
-            size_t i = v.size ();
-            for (TooltipLineVector::const_iterator
-                it = v.begin (); it != v.end (); ++it ) {
-              ss << *it;
-              i--;
-              if (i > 0) ss << tooltipendl;
-            }
-            return ss.str ();
-          }
-          inline void addLine (const std::string& l) {
-            v.push_back (l);
-          }
-        };
+  Tooltip() : v(){};
+  inline std::string toStr() const {
+    std::stringstream ss;
+    size_t i = v.size();
+    for (TooltipLineVector::const_iterator it = v.begin(); it != v.end();
+         ++it) {
+      ss << *it;
+      i--;
+      if (i > 0) ss << tooltipendl;
+    }
+    return ss.str();
+  }
+  inline void addLine(const std::string& l) { v.push_back(l); }
+};
 
-        std::ostream& insertComments (std::ostream& os, const std::string& c);
+std::ostream& insertComments(std::ostream& os, const std::string& c);
 
-        std::ostream& operator<< (std::ostream& os, const DrawingAttributes& da);
-      } // namespace dot
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+std::ostream& operator<<(std::ostream& os, const DrawingAttributes& da);
+}  // namespace dot
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_GRAPH_DOT_HH
+#endif  // HPP_MANIPULATION_GRAPH_DOT_HH
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 4ec0ec4b5cc42b4c6842ccfb12ae0a84d517d0dc..f5bacb62138881a432ea5709940d4cbfd001880c 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -27,596 +27,570 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_EDGE_HH
-# define HPP_MANIPULATION_GRAPH_EDGE_HH
+#define HPP_MANIPULATION_GRAPH_EDGE_HH
 
 #include <hpp/core/constraint-set.hh>
-#include <hpp/core/steering-method.hh>
-#include <hpp/core/relative-motion.hh>
 #include <hpp/core/path.hh>
+#include <hpp/core/relative-motion.hh>
+#include <hpp/core/steering-method.hh>
 
 #include "hpp/manipulation/config.hh"
 #include "hpp/manipulation/fwd.hh"
 #include "hpp/manipulation/graph/graph.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      /// \addtogroup constraint_graph
-      /// \{
-
-      /// Transition between two states of a constraint graph
-      ///
-      /// An edge stores two types of constraints.
-
-      /// \li <b> Path constraints </b> should be safisfied by paths belonging
-      /// to the edge. Along any path, the right hand side of the
-      /// constraint is constant, but can differ between paths. For
-      /// instance if an edge represents a transit path of a robot
-      /// that can grasp an object, the right hand side of the
-      /// constraint represents the position of the object. Along any
-      /// transit path, the object does not move, but for different paths
-      /// the object can be at different positions.
-      /// \sa method pathConstraint.
-      /// \li <b> Configuration constraints </b> are constraints that
-      /// configurations in the destination state should satisfy and
-      /// the constraints that paths should satisfy. For instance, if
-      /// the edge links a state where the robot does not hold the
-      /// object to a state where the robot holds the object, the
-      /// configuration constraints represent a fixed relative
-      /// position of the object with respect to the gripper and a
-      /// stable position of the object. Configuration constraints are
-      /// necessary to generate a configuration in the destination
-      /// state of the edge that is reachable from a given
-      /// configuration in the start state by an admissible path.
-      class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent
-      {
-        friend class WaypointEdge;
-        public:
-          typedef core::RelativeMotion RelativeMotion;
-
-          /// Destructor
-          virtual ~Edge ();
-
-          /// Create a new empty Edge.
-          static EdgePtr_t create
-	    (const std::string& name,
-	     const GraphWkPtr_t& graph,
-	     const StateWkPtr_t& from,
-	     const StateWkPtr_t& to);
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param nStart node containing the configuration defining the right
-	  ///        hand side of the edge constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear
-          /// \deprecated Use generateTargetConfig instead.
-          virtual bool applyConstraints (core::NodePtr_t nStart,
-                                         ConfigurationOut_t q) const
-            HPP_MANIPULATION_DEPRECATED;
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param qStart node containing the configuration defining the right
-	  ///        hand side of the edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear.
-          /// \deprecated Use generateTargetConfig instead.
-          virtual bool applyConstraints (ConfigurationIn_t qStart,
-                                         ConfigurationOut_t q) const
-            HPP_MANIPULATION_DEPRECATED;
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param nStart node containing the configuration defining the right
-	  ///        hand side of the edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nStart.
-          virtual bool generateTargetConfig(core::NodePtr_t nStart,
-                                            ConfigurationOut_t q) const;
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param qStart node containing the configuration defining the right
-	  ///        hand side of the edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear.
-          virtual bool generateTargetConfig (ConfigurationIn_t qStart,
-                                             ConfigurationOut_t q) const;
-
-          virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
-          virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
-              ConfigurationIn_t q2) const;
-
-          /// Get the destination
-          StatePtr_t stateTo () const;
-
-          /// Get the origin
-          StatePtr_t stateFrom () const;
-
-          /// Get the state in which path is.
-          StatePtr_t state () const
-          {
-            return state_.lock();
-          }
-
-          void state (StatePtr_t state)
-          {
-            state_ = state;
-          }
-
-	  /// Get steering method associated to the edge.
-	  const core::SteeringMethodPtr_t& steeringMethod () const
-	  {
-	    return steeringMethod_;
-	  }
-
-	  /// Get path validation associated to the edge.
-	  const core::PathValidationPtr_t& pathValidation () const
-	  {
-	    return pathValidation_;
-	  }
-
-          const RelativeMotion::matrix_type& relativeMotion () const
-          {
-            return relMotion_;
-          }
-
-          /// Update the relative motion matrix
-          void relativeMotion(const RelativeMotion::matrix_type & m);
-
-          /// Set Security margin for a pair of joints
-          ///
-          /// \param row index of joint1 + 1 in robot,
-          /// \param col index of joint2 + 1 in robot,
-          /// \param margin security margin for collision checking between
-          ///        those joints.
-          ///
-          /// \note set value to matrix [row, col] and matrix [col, row].
-          void securityMarginForPair(const size_type& row, const size_type& col,
-                                     const value_type& margin);
-
-          /// Accessor to the security margin.
-          const matrix_t& securityMargins() const
-          {
-            return securityMargins_;
-          }
-
-          /// Get direction of the path compare to the edge
-          /// \return true is reverse
-          virtual bool direction (const core::PathPtr_t& path) const;
-
-          /// Populate a ConfigProjector with constraints required to generate
-          /// a path at the intersection of two edges.
-          virtual bool intersectionConstraint (const EdgePtr_t& other,
-              ConfigProjectorPtr_t projector) const;
-
-          /// Print the object in a stream.
-          virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
-          /// Constraint of the destination state and of the path
-          /// \deprecated Use targetConstraint instead
-          ConstraintSetPtr_t configConstraint() const
-            HPP_MANIPULATION_DEPRECATED;
-
-          /// Constraint of the destination state and of the path
-          ConstraintSetPtr_t targetConstraint() const;
-
-          void setShort (bool isShort) {
-            isShort_ = isShort;
-          }
-
-          bool isShort () const {
-            return isShort_;
-          }
-	   /// Constraint to project a path.
-          /// \return The initialized constraint.
-          ConstraintSetPtr_t pathConstraint() const;
-
-        protected:
-          /// Initialization of the object.
-          void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
-              const StateWkPtr_t& to);
-
-          /// Constructor
-          Edge (const std::string& name);
-
-          virtual ConstraintSetPtr_t buildConfigConstraint()
-            HPP_MANIPULATION_DEPRECATED;
-
-          /// Build path and target state constraint set.
-          virtual ConstraintSetPtr_t buildTargetConstraint();
-
-          /// Build path constraints
-          virtual ConstraintSetPtr_t buildPathConstraint();
-
-          virtual void initialize ();
-
-          /// Print the object in a stream.
-          virtual std::ostream& print (std::ostream& os) const;
-
-          bool isShort_;
-
-        private:
-          /// See pathConstraint member function.
-          ConstraintSetPtr_t pathConstraints_;
-
-          /// Constraint ensuring that a q_proj will be in to_ and in the
-          /// same leaf of to_ as the configuration used for initialization.
-          ConstraintSetPtr_t targetConstraints_;
-
-          /// The two ends of the transition.
-          StateWkPtr_t from_, to_;
-
-          /// True if this path is in state from, False if in state to
-          StateWkPtr_t state_;
-
-	  /// Steering method used to create paths associated to the edge
-          core::SteeringMethodPtr_t steeringMethod_;
-
-	  /// Path validation associated to the edge
-          mutable RelativeMotion::matrix_type relMotion_;
-          /// matrix of security margins for collision checking between joints
-          matrix_t securityMargins_;
-
-          core::PathValidationPtr_t pathValidation_;
-
-          /// Weak pointer to itself.
-          EdgeWkPtr_t wkPtr_;
-
-          friend class Graph;
-      }; // class Edge
-
-      /// Edge with intermediate waypoint states.
-      ///
-      /// This class implements a transition from one state to another state
-      /// with intermediate states in-between. This feature is particularly
-      /// interesting when manipulating objects. Between a state where a gripper
-      /// does not grasp an object and a state where the same gripper grasps
-      /// the object, it is useful to add an intermediate state where the
-      /// gripper is close to the object.
-      ///
-      /// Waypoints are handled recursively, i.e.\ class WaypointEdge
-      /// contains only a State and an Edge, the second Edge being
-      /// itself. In this package, the State in a WaypointEdge is
-      /// semantically different from other State because it does not
-      /// correspond to a state with different manipulation rules. It
-      /// has the same rules as another State (either Edge::stateFrom() or
-      /// Edge::stateTo()).
-      ///
-      /// Semantically, a waypoint State is fully part of the WaypointEdge.
-      /// When a corresponding path reaches it, no planning is required to know
-      /// what to do next. To the contrary, when a path reaches
-      /// Edge::stateFrom() or Edge::stateTo(), there may be several
-      /// possibilities.
-      ///
-      /// \note
-      ///   Implementation details: let's say, between the two states \f$N_f\f$
-      ///   and \f$N_t\f$, two waypoints are required:
-      ///   \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1
-      ///   \xrightarrow{e_2} N_t\f$.
-      ///   The WaypointEdge contains:
-      ///   \li from: \f$N_f\f$,
-      ///   \li to: \f$N_t\f$,
-      ///   \li states: \f$(n_0, n_1)\f$
-      ///   \li transitions: \f$(e_0, e_1, e_2)\f$
-      ///   \li constraints: any calls to the constraints throw,
-      class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge
-      {
-        public:
-          /// Create a new WaypointEdge.
-	static WaypointEdgePtr_t create
-	  (const std::string& name,
-	   const GraphWkPtr_t& graph, const StateWkPtr_t& from,
-	   const StateWkPtr_t& to);
-
-          virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
-          virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param qStart node containing the configuration defining the right
-	  ///        hand side of the edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear.
-          /// deprecated Used generateTargetConfig instead.
-          virtual bool applyConstraints (ConfigurationIn_t qStart,
-                                         ConfigurationOut_t q) const
-            HPP_MANIPULATION_DEPRECATED;
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param qStart node containing the configuration defining the right
-	  ///        hand side of the edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear.
-          virtual bool generateTargetConfig (ConfigurationIn_t qStart,
-                                             ConfigurationOut_t q) const;
-
-          /// Return the index-th edge.
-          const EdgePtr_t& waypoint (const std::size_t index) const;
-
-          /// Print the object in a stream.
-          virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
-          /// Set the number of waypoints
-          void nbWaypoints (const size_type number);
-
-          std::size_t nbWaypoints () const
-          {
-            return edges_.size () - 1;
-          }
-
-          /// Set waypoint index with wEdge and wTo.
-          /// \param wTo is the destination state of wEdge
-          void setWaypoint (const std::size_t index, const EdgePtr_t wEdge, const StatePtr_t wTo);
-
-        protected:
-	  WaypointEdge (const std::string& name) :
-	    Edge (name),
-            lastSucceeded_ (false)
-	    {
-	    }
-          /// Initialization of the object.
-          void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
-              const StateWkPtr_t& to);
-
-          /// Initialize each of the internal edges
-          virtual void initialize ();
-          /// Print the object in a stream.
-          virtual std::ostream& print (std::ostream& os) const;
-
-        private:
-          Edges_t edges_;
-          States_t states_;
-
-          mutable matrix_t configs_;
-          mutable bool lastSucceeded_;
-
-          WaypointEdgeWkPtr_t wkPtr_;
-      }; // class WaypointEdge
-
-      /// Edge that handles crossed foliations
-      ///
-      /// Let us consider the following simple constraint graph
-      /// corresponding to a robot grasping an object with one gripper.
-      ///
-      /// \image html constraint-graph.png "Simple constraint graph corresponding to a robot grasping an object."
-      ///
-      /// In order to disambiguate, we assume here that
-      /// \li transition <b>Grasp object</b> is in \b Placement state,
-      /// \li transition <b>Release object</b> is in \b Grasp state.
-      ///
-      /// If state \b Placement is defined by the object lying on a planar
-      /// polygonal surface, then
-      /// \li state \b Placement,
-      /// \li transition \b Transit, and
-      /// \li transition <b>Grasp object</b>
-      ///
-      /// are all constrained in a foliated manifold parameterized by the
-      /// position of the box on the surface.
-      ///
-      /// Likewise, if the object is cylindrical the grasp may have a degree
-      /// of freedom corresponding to the angle around z-axis of the gripper
-      /// with respect to the object. See classes
-      /// \link hpp::manipulation::Handle Handle\endlink and
-      /// \link hpp::pinocchio::Gripper Gripper\endlink for details.
-      /// In this latter case,
-      /// \li state \b Grasp,
-      /// \li transition \b Transfer, and
-      /// \li transition <b>Release object</b>
-      ///
-      /// are all constrained in a foliated manifold parameterized by the
-      /// angle around z-axis of the gripper with respect to the object.
-      ///
-      /// Let us denote
-      /// \li \c grasp the numerical constraint defining state \b Grasp,
-      /// \li \c placement the numerical constraint defining state \b Placement,
-      /// \li \c grasp_comp the parameterized constraint defining a leaf
-      ///     of \c Transfer (the angle between the gripper and the
-      ///     object),
-      /// \li \c placement_comp the parameterized constraint defining a leaf
-      ///     of \b Placement (the position of the object on the contact
-      ///     surface).
-      ///
-      /// As explained in <a
-      /// href="https://hal.archives-ouvertes.fr/hal-01358767">this
-      /// paper </a>, we are in the crossed foliation case and manipulation RRT
-      /// will never be able to connect trees expanding in different leaves of
-      /// the foliation.
-      ///
-      /// This class solves this issue in the following way by creating an
-      /// instance of LevelSetEdge between \b Placement and \b Grasp.
-      ///
-      /// When extending a configuration \f$\mathbf{q}_{start}\f$ in state
-      /// \b Placement, this transition will produce a target configuration
-      /// (method \link LevelSetEdge::generateTargetConfig generateTargetConfig)
-      /// \endlink as follows.
-      ///
-      /// \li pick a random configuration \f$\mathbf{q}_rand\f$, in the edge
-      /// histogram (see method \link LevelSetEdge::histogram histogram\endlink)
-      /// \li compute right hand side of \c grasp_comp with
-      ///     \f$\mathbf{q}_{rand}\f$,
-      /// \li compute right hand side of \c placement_comp with
-      ///     \f$\mathbf{q}_{start}\f$,
-      /// \li solve (\c grasp, \c placement, \c placement_comp, \c grasp_comp)
-      ///     using input configuration \f$\mathbf{q}\f$. Note that the
-      /// parent method Edge::generateTargetConfig does the same without
-      /// adding \c grasp_comp.
-      ///
-      /// The constraints parameterizing the target state foliation
-      /// (\c graps_comp in our example) are passed to class instances
-      /// using method \link LevelSetEdge::insertParamConstraint
-      /// insertParamConstraint\endlink.
-      class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge
-      {
-        public:
-          virtual ~LevelSetEdge ();
-
-          /// Create a new LevelSetEdge.
-          static LevelSetEdgePtr_t create
-	    (const std::string& name,
-	     const GraphWkPtr_t& graph, const StateWkPtr_t& from,
-	     const StateWkPtr_t& to);
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param nStart node containing the configuration defining the right
-	  ///        hand side of the edge constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear
-          /// \deprecated Use generateTargetConfig instead.
-          virtual bool applyConstraints (core::NodePtr_t nStart,
-                                         ConfigurationOut_t q) const
-            HPP_MANIPULATION_DEPRECATED;
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param qStart node containing the configuration defining the right
-	  ///        hand side of the edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear.
-          /// \deprecated Use generateTargetConfig instead.
-          virtual bool applyConstraints (ConfigurationIn_t qStart,
-                                         ConfigurationOut_t q) const
-            HPP_MANIPULATION_DEPRECATED;
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param nStart node containing the configuration defining the right
-	  ///        hand side of the edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nStart.
-          virtual bool generateTargetConfig(core::NodePtr_t nStart,
-                                            ConfigurationOut_t q) const;
-
-	  /// Generate a reachable configuration in the target state
-	  ///
-	  /// \param qStart configuration defining the right hand side of the
-          ///               edge path constraint,
-	  /// \param[in,out] q input configuration used to initialize the
-          ///                numerical solver and output configuration lying
-          ///                in the target state and reachable along the edge
-          ///                from nnear.
-          virtual bool generateTargetConfig (ConfigurationIn_t qStart,
-                                             ConfigurationOut_t q) const;
-
-          /// Generate a reachable configuration in leaf of target state
-	  /// \param qStart configuration defining the right hand side of the
-          ///               edge path constraint,
-          /// \param qLeaf configuration used to set the right hand side of
-          ///        the target state foliation. See method
-          ///        \link LevelSetEdge::insertParamConstraint
-          ///        insertParamConstraint\endlink.
-          bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
-                                          ConfigurationIn_t qLeaf,
-                                          ConfigurationOut_t q) const;
-
-          /// \deprecated Use buildTargetConstraint instead
-          virtual ConstraintSetPtr_t buildConfigConstraint()
-            HPP_MANIPULATION_DEPRECATED;
-
-          /// Build path and target state constraints
-          virtual ConstraintSetPtr_t buildTargetConstraint();
-
-          /// Build the histogram
-          /// \sa LevelSetEdge::histogram.
-          void buildHistogram ();
-
-          /// Return pointer on histogram of the edge
-          ///
-          /// The edge histogram is a container of configurations defined by
-          /// a set of constraints called the <b>condition constraints</b>
-          /// that a configuration should satisfy to be inserted in the
-          /// histogram.
-          ///
-          /// The histogram is passed to the Roadmap via the graph (method
-          /// Graph::insertHistogram). The roadmap then populates the histogram
-          /// with all new configurations satisfying the condition constraints.
-          ///
-          /// The condition constraints should therefore be the constraints of
-          /// the target state of the level set edge.
-          ///
-          /// \sa LevelSetEdge::conditionConstraints
-          ///     LevelSetEdge::insertConditionConstraint
-          LeafHistogramPtr_t histogram () const;
-
-          /// \name Foliation definition
-          /// \{
-
-          /// Insert a constraints parameterizing the target state foliation
-          /// \param nm the numerical constraint,
-          void insertParamConstraint (const ImplicitPtr_t& nm);
-
-          /// Get constraints parameterizing the target state foliation
-          const NumericalConstraints_t& paramConstraints() const;
-
-          /// Insert a condition constraint
-          /// \sa LevelSetEdge::histogram
-          void insertConditionConstraint (const ImplicitPtr_t& nm);
-
-          /// Get constraints parameterizing the target state foliation
-          /// \sa LevelSetEdge::histogram
-          const NumericalConstraints_t& conditionConstraints() const;
-          /// \}
-
-          /// Print the object in a stream.
-          virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
-        protected:
-          /// Initialization of the object.
-          void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
-              const StateWkPtr_t& to);
-
-	  LevelSetEdge (const std::string& name);
-
-          /// Print the object in a stream.
-          virtual std::ostream& print (std::ostream& os) const;
-
-          /// Populate DrawingAttributes tooltip
-          virtual void populateTooltip (dot::Tooltip& tp) const;
-
-          virtual void initialize ();
-
-        private:
-          // Parametrizer
-          // NumericalConstraints_t
-          NumericalConstraints_t paramNumericalConstraints_;
-
-          // Condition
-          // NumericalConstraints_t
-          NumericalConstraints_t condNumericalConstraints_;
-
-          /// This histogram will be used to find a good level set.
-          LeafHistogramPtr_t hist_;
-
-          LevelSetEdgeWkPtr_t wkPtr_;
-      }; // class LevelSetEdge
-
-      /// \}
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPH_EDGE_HH
+namespace manipulation {
+namespace graph {
+/// \addtogroup constraint_graph
+/// \{
+
+/// Transition between two states of a constraint graph
+///
+/// An edge stores two types of constraints.
+
+/// \li <b> Path constraints </b> should be safisfied by paths belonging
+/// to the edge. Along any path, the right hand side of the
+/// constraint is constant, but can differ between paths. For
+/// instance if an edge represents a transit path of a robot
+/// that can grasp an object, the right hand side of the
+/// constraint represents the position of the object. Along any
+/// transit path, the object does not move, but for different paths
+/// the object can be at different positions.
+/// \sa method pathConstraint.
+/// \li <b> Configuration constraints </b> are constraints that
+/// configurations in the destination state should satisfy and
+/// the constraints that paths should satisfy. For instance, if
+/// the edge links a state where the robot does not hold the
+/// object to a state where the robot holds the object, the
+/// configuration constraints represent a fixed relative
+/// position of the object with respect to the gripper and a
+/// stable position of the object. Configuration constraints are
+/// necessary to generate a configuration in the destination
+/// state of the edge that is reachable from a given
+/// configuration in the start state by an admissible path.
+class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent {
+  friend class WaypointEdge;
+
+ public:
+  typedef core::RelativeMotion RelativeMotion;
+
+  /// Destructor
+  virtual ~Edge();
+
+  /// Create a new empty Edge.
+  static EdgePtr_t create(const std::string& name, const GraphWkPtr_t& graph,
+                          const StateWkPtr_t& from, const StateWkPtr_t& to);
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param nStart node containing the configuration defining the right
+  ///        hand side of the edge constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear
+  /// \deprecated Use generateTargetConfig instead.
+  virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
+      const HPP_MANIPULATION_DEPRECATED;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param qStart node containing the configuration defining the right
+  ///        hand side of the edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear.
+  /// \deprecated Use generateTargetConfig instead.
+  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
+      const HPP_MANIPULATION_DEPRECATED;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param nStart node containing the configuration defining the right
+  ///        hand side of the edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nStart.
+  virtual bool generateTargetConfig(core::NodePtr_t nStart,
+                                    ConfigurationOut_t q) const;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param qStart node containing the configuration defining the right
+  ///        hand side of the edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear.
+  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
+                                    ConfigurationOut_t q) const;
+
+  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
+                     ConfigurationIn_t q2) const;
+
+  /// Get the destination
+  StatePtr_t stateTo() const;
+
+  /// Get the origin
+  StatePtr_t stateFrom() const;
+
+  /// Get the state in which path is.
+  StatePtr_t state() const { return state_.lock(); }
+
+  void state(StatePtr_t state) { state_ = state; }
+
+  /// Get steering method associated to the edge.
+  const core::SteeringMethodPtr_t& steeringMethod() const {
+    return steeringMethod_;
+  }
+
+  /// Get path validation associated to the edge.
+  const core::PathValidationPtr_t& pathValidation() const {
+    return pathValidation_;
+  }
+
+  const RelativeMotion::matrix_type& relativeMotion() const {
+    return relMotion_;
+  }
+
+  /// Update the relative motion matrix
+  void relativeMotion(const RelativeMotion::matrix_type& m);
+
+  /// Set Security margin for a pair of joints
+  ///
+  /// \param row index of joint1 + 1 in robot,
+  /// \param col index of joint2 + 1 in robot,
+  /// \param margin security margin for collision checking between
+  ///        those joints.
+  ///
+  /// \note set value to matrix [row, col] and matrix [col, row].
+  void securityMarginForPair(const size_type& row, const size_type& col,
+                             const value_type& margin);
+
+  /// Accessor to the security margin.
+  const matrix_t& securityMargins() const { return securityMargins_; }
+
+  /// Get direction of the path compare to the edge
+  /// \return true is reverse
+  virtual bool direction(const core::PathPtr_t& path) const;
+
+  /// Populate a ConfigProjector with constraints required to generate
+  /// a path at the intersection of two edges.
+  virtual bool intersectionConstraint(const EdgePtr_t& other,
+                                      ConfigProjectorPtr_t projector) const;
+
+  /// Print the object in a stream.
+  virtual std::ostream& dotPrint(
+      std::ostream& os,
+      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
+
+  /// Constraint of the destination state and of the path
+  /// \deprecated Use targetConstraint instead
+  ConstraintSetPtr_t configConstraint() const HPP_MANIPULATION_DEPRECATED;
+
+  /// Constraint of the destination state and of the path
+  ConstraintSetPtr_t targetConstraint() const;
+
+  void setShort(bool isShort) { isShort_ = isShort; }
+
+  bool isShort() const { return isShort_; }
+  /// Constraint to project a path.
+  /// \return The initialized constraint.
+  ConstraintSetPtr_t pathConstraint() const;
+
+ protected:
+  /// Initialization of the object.
+  void init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
+            const StateWkPtr_t& from, const StateWkPtr_t& to);
+
+  /// Constructor
+  Edge(const std::string& name);
+
+  virtual ConstraintSetPtr_t buildConfigConstraint()
+      HPP_MANIPULATION_DEPRECATED;
+
+  /// Build path and target state constraint set.
+  virtual ConstraintSetPtr_t buildTargetConstraint();
+
+  /// Build path constraints
+  virtual ConstraintSetPtr_t buildPathConstraint();
+
+  virtual void initialize();
+
+  /// Print the object in a stream.
+  virtual std::ostream& print(std::ostream& os) const;
+
+  bool isShort_;
+
+ private:
+  /// See pathConstraint member function.
+  ConstraintSetPtr_t pathConstraints_;
+
+  /// Constraint ensuring that a q_proj will be in to_ and in the
+  /// same leaf of to_ as the configuration used for initialization.
+  ConstraintSetPtr_t targetConstraints_;
+
+  /// The two ends of the transition.
+  StateWkPtr_t from_, to_;
+
+  /// True if this path is in state from, False if in state to
+  StateWkPtr_t state_;
+
+  /// Steering method used to create paths associated to the edge
+  core::SteeringMethodPtr_t steeringMethod_;
+
+  /// Path validation associated to the edge
+  mutable RelativeMotion::matrix_type relMotion_;
+  /// matrix of security margins for collision checking between joints
+  matrix_t securityMargins_;
+
+  core::PathValidationPtr_t pathValidation_;
+
+  /// Weak pointer to itself.
+  EdgeWkPtr_t wkPtr_;
+
+  friend class Graph;
+};  // class Edge
+
+/// Edge with intermediate waypoint states.
+///
+/// This class implements a transition from one state to another state
+/// with intermediate states in-between. This feature is particularly
+/// interesting when manipulating objects. Between a state where a gripper
+/// does not grasp an object and a state where the same gripper grasps
+/// the object, it is useful to add an intermediate state where the
+/// gripper is close to the object.
+///
+/// Waypoints are handled recursively, i.e.\ class WaypointEdge
+/// contains only a State and an Edge, the second Edge being
+/// itself. In this package, the State in a WaypointEdge is
+/// semantically different from other State because it does not
+/// correspond to a state with different manipulation rules. It
+/// has the same rules as another State (either Edge::stateFrom() or
+/// Edge::stateTo()).
+///
+/// Semantically, a waypoint State is fully part of the WaypointEdge.
+/// When a corresponding path reaches it, no planning is required to know
+/// what to do next. To the contrary, when a path reaches
+/// Edge::stateFrom() or Edge::stateTo(), there may be several
+/// possibilities.
+///
+/// \note
+///   Implementation details: let's say, between the two states \f$N_f\f$
+///   and \f$N_t\f$, two waypoints are required:
+///   \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1
+///   \xrightarrow{e_2} N_t\f$.
+///   The WaypointEdge contains:
+///   \li from: \f$N_f\f$,
+///   \li to: \f$N_t\f$,
+///   \li states: \f$(n_0, n_1)\f$
+///   \li transitions: \f$(e_0, e_1, e_2)\f$
+///   \li constraints: any calls to the constraints throw,
+class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge {
+ public:
+  /// Create a new WaypointEdge.
+  static WaypointEdgePtr_t create(const std::string& name,
+                                  const GraphWkPtr_t& graph,
+                                  const StateWkPtr_t& from,
+                                  const StateWkPtr_t& to);
+
+  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
+                     ConfigurationIn_t q2) const;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param qStart node containing the configuration defining the right
+  ///        hand side of the edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear.
+  /// deprecated Used generateTargetConfig instead.
+  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
+      const HPP_MANIPULATION_DEPRECATED;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param qStart node containing the configuration defining the right
+  ///        hand side of the edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear.
+  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
+                                    ConfigurationOut_t q) const;
+
+  /// Return the index-th edge.
+  const EdgePtr_t& waypoint(const std::size_t index) const;
+
+  /// Print the object in a stream.
+  virtual std::ostream& dotPrint(
+      std::ostream& os,
+      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
+
+  /// Set the number of waypoints
+  void nbWaypoints(const size_type number);
+
+  std::size_t nbWaypoints() const { return edges_.size() - 1; }
+
+  /// Set waypoint index with wEdge and wTo.
+  /// \param wTo is the destination state of wEdge
+  void setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
+                   const StatePtr_t wTo);
+
+ protected:
+  WaypointEdge(const std::string& name) : Edge(name), lastSucceeded_(false) {}
+  /// Initialization of the object.
+  void init(const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
+            const StateWkPtr_t& from, const StateWkPtr_t& to);
+
+  /// Initialize each of the internal edges
+  virtual void initialize();
+  /// Print the object in a stream.
+  virtual std::ostream& print(std::ostream& os) const;
+
+ private:
+  Edges_t edges_;
+  States_t states_;
+
+  mutable matrix_t configs_;
+  mutable bool lastSucceeded_;
+
+  WaypointEdgeWkPtr_t wkPtr_;
+};  // class WaypointEdge
+
+/// Edge that handles crossed foliations
+///
+/// Let us consider the following simple constraint graph
+/// corresponding to a robot grasping an object with one gripper.
+///
+/// \image html constraint-graph.png "Simple constraint graph corresponding to a
+/// robot grasping an object."
+///
+/// In order to disambiguate, we assume here that
+/// \li transition <b>Grasp object</b> is in \b Placement state,
+/// \li transition <b>Release object</b> is in \b Grasp state.
+///
+/// If state \b Placement is defined by the object lying on a planar
+/// polygonal surface, then
+/// \li state \b Placement,
+/// \li transition \b Transit, and
+/// \li transition <b>Grasp object</b>
+///
+/// are all constrained in a foliated manifold parameterized by the
+/// position of the box on the surface.
+///
+/// Likewise, if the object is cylindrical the grasp may have a degree
+/// of freedom corresponding to the angle around z-axis of the gripper
+/// with respect to the object. See classes
+/// \link hpp::manipulation::Handle Handle\endlink and
+/// \link hpp::pinocchio::Gripper Gripper\endlink for details.
+/// In this latter case,
+/// \li state \b Grasp,
+/// \li transition \b Transfer, and
+/// \li transition <b>Release object</b>
+///
+/// are all constrained in a foliated manifold parameterized by the
+/// angle around z-axis of the gripper with respect to the object.
+///
+/// Let us denote
+/// \li \c grasp the numerical constraint defining state \b Grasp,
+/// \li \c placement the numerical constraint defining state \b Placement,
+/// \li \c grasp_comp the parameterized constraint defining a leaf
+///     of \c Transfer (the angle between the gripper and the
+///     object),
+/// \li \c placement_comp the parameterized constraint defining a leaf
+///     of \b Placement (the position of the object on the contact
+///     surface).
+///
+/// As explained in <a
+/// href="https://hal.archives-ouvertes.fr/hal-01358767">this
+/// paper </a>, we are in the crossed foliation case and manipulation RRT
+/// will never be able to connect trees expanding in different leaves of
+/// the foliation.
+///
+/// This class solves this issue in the following way by creating an
+/// instance of LevelSetEdge between \b Placement and \b Grasp.
+///
+/// When extending a configuration \f$\mathbf{q}_{start}\f$ in state
+/// \b Placement, this transition will produce a target configuration
+/// (method \link LevelSetEdge::generateTargetConfig generateTargetConfig)
+/// \endlink as follows.
+///
+/// \li pick a random configuration \f$\mathbf{q}_rand\f$, in the edge
+/// histogram (see method \link LevelSetEdge::histogram histogram\endlink)
+/// \li compute right hand side of \c grasp_comp with
+///     \f$\mathbf{q}_{rand}\f$,
+/// \li compute right hand side of \c placement_comp with
+///     \f$\mathbf{q}_{start}\f$,
+/// \li solve (\c grasp, \c placement, \c placement_comp, \c grasp_comp)
+///     using input configuration \f$\mathbf{q}\f$. Note that the
+/// parent method Edge::generateTargetConfig does the same without
+/// adding \c grasp_comp.
+///
+/// The constraints parameterizing the target state foliation
+/// (\c graps_comp in our example) are passed to class instances
+/// using method \link LevelSetEdge::insertParamConstraint
+/// insertParamConstraint\endlink.
+class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge {
+ public:
+  virtual ~LevelSetEdge();
+
+  /// Create a new LevelSetEdge.
+  static LevelSetEdgePtr_t create(const std::string& name,
+                                  const GraphWkPtr_t& graph,
+                                  const StateWkPtr_t& from,
+                                  const StateWkPtr_t& to);
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param nStart node containing the configuration defining the right
+  ///        hand side of the edge constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear
+  /// \deprecated Use generateTargetConfig instead.
+  virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
+      const HPP_MANIPULATION_DEPRECATED;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param qStart node containing the configuration defining the right
+  ///        hand side of the edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear.
+  /// \deprecated Use generateTargetConfig instead.
+  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
+      const HPP_MANIPULATION_DEPRECATED;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param nStart node containing the configuration defining the right
+  ///        hand side of the edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nStart.
+  virtual bool generateTargetConfig(core::NodePtr_t nStart,
+                                    ConfigurationOut_t q) const;
+
+  /// Generate a reachable configuration in the target state
+  ///
+  /// \param qStart configuration defining the right hand side of the
+  ///               edge path constraint,
+  /// \param[in,out] q input configuration used to initialize the
+  ///                numerical solver and output configuration lying
+  ///                in the target state and reachable along the edge
+  ///                from nnear.
+  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
+                                    ConfigurationOut_t q) const;
+
+  /// Generate a reachable configuration in leaf of target state
+  /// \param qStart configuration defining the right hand side of the
+  ///               edge path constraint,
+  /// \param qLeaf configuration used to set the right hand side of
+  ///        the target state foliation. See method
+  ///        \link LevelSetEdge::insertParamConstraint
+  ///        insertParamConstraint\endlink.
+  bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
+                                  ConfigurationIn_t qLeaf,
+                                  ConfigurationOut_t q) const;
+
+  /// \deprecated Use buildTargetConstraint instead
+  virtual ConstraintSetPtr_t buildConfigConstraint()
+      HPP_MANIPULATION_DEPRECATED;
+
+  /// Build path and target state constraints
+  virtual ConstraintSetPtr_t buildTargetConstraint();
+
+  /// Build the histogram
+  /// \sa LevelSetEdge::histogram.
+  void buildHistogram();
+
+  /// Return pointer on histogram of the edge
+  ///
+  /// The edge histogram is a container of configurations defined by
+  /// a set of constraints called the <b>condition constraints</b>
+  /// that a configuration should satisfy to be inserted in the
+  /// histogram.
+  ///
+  /// The histogram is passed to the Roadmap via the graph (method
+  /// Graph::insertHistogram). The roadmap then populates the histogram
+  /// with all new configurations satisfying the condition constraints.
+  ///
+  /// The condition constraints should therefore be the constraints of
+  /// the target state of the level set edge.
+  ///
+  /// \sa LevelSetEdge::conditionConstraints
+  ///     LevelSetEdge::insertConditionConstraint
+  LeafHistogramPtr_t histogram() const;
+
+  /// \name Foliation definition
+  /// \{
+
+  /// Insert a constraints parameterizing the target state foliation
+  /// \param nm the numerical constraint,
+  void insertParamConstraint(const ImplicitPtr_t& nm);
+
+  /// Get constraints parameterizing the target state foliation
+  const NumericalConstraints_t& paramConstraints() const;
+
+  /// Insert a condition constraint
+  /// \sa LevelSetEdge::histogram
+  void insertConditionConstraint(const ImplicitPtr_t& nm);
+
+  /// Get constraints parameterizing the target state foliation
+  /// \sa LevelSetEdge::histogram
+  const NumericalConstraints_t& conditionConstraints() const;
+  /// \}
+
+  /// Print the object in a stream.
+  virtual std::ostream& dotPrint(
+      std::ostream& os,
+      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
+
+ protected:
+  /// Initialization of the object.
+  void init(const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
+            const StateWkPtr_t& from, const StateWkPtr_t& to);
+
+  LevelSetEdge(const std::string& name);
+
+  /// Print the object in a stream.
+  virtual std::ostream& print(std::ostream& os) const;
+
+  /// Populate DrawingAttributes tooltip
+  virtual void populateTooltip(dot::Tooltip& tp) const;
+
+  virtual void initialize();
+
+ private:
+  // Parametrizer
+  // NumericalConstraints_t
+  NumericalConstraints_t paramNumericalConstraints_;
+
+  // Condition
+  // NumericalConstraints_t
+  NumericalConstraints_t condNumericalConstraints_;
+
+  /// This histogram will be used to find a good level set.
+  LeafHistogramPtr_t hist_;
+
+  LevelSetEdgeWkPtr_t wkPtr_;
+};  // class LevelSetEdge
+
+/// \}
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPH_EDGE_HH
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index 0ad9fd2912a01b62dccb45d71dc02499c35ede11..1d935cfb7101728160b87f17944b9ae38000c4ed 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -27,56 +27,55 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_FWD_HH
-# define HPP_MANIPULATION_GRAPH_FWD_HH
+#define HPP_MANIPULATION_GRAPH_FWD_HH
 
-#include <hpp/util/pointer.hh>
 #include <hpp/statistics/distribution.hh>
+#include <hpp/util/pointer.hh>
 #include <vector>
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      HPP_PREDEF_CLASS (Graph);
-      HPP_PREDEF_CLASS (Edge);
-      HPP_PREDEF_CLASS (State);
-      HPP_PREDEF_CLASS (WaypointEdge);
-      HPP_PREDEF_CLASS (LevelSetEdge);
-      HPP_PREDEF_CLASS (StateSelector);
-      HPP_PREDEF_CLASS (GraphComponent);
-      HPP_PREDEF_CLASS (GuidedStateSelector);
-      typedef shared_ptr < Graph > GraphPtr_t;
-      typedef shared_ptr < State > StatePtr_t;
-      typedef shared_ptr < Edge > EdgePtr_t;
-      typedef shared_ptr < WaypointEdge > WaypointEdgePtr_t;
-      typedef shared_ptr < LevelSetEdge > LevelSetEdgePtr_t;
-      typedef shared_ptr < StateSelector > StateSelectorPtr_t;
-      typedef shared_ptr < GuidedStateSelector >
-      GuidedStateSelectorPtr_t;
-      typedef shared_ptr < GraphComponent > GraphComponentPtr_t;
-      typedef std::vector < GraphComponentWkPtr_t > GraphComponents_t;
-      typedef std::vector < StatePtr_t > States_t;
-      typedef std::vector < EdgePtr_t > Edges_t;
-      typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t >::Weight_t Weight_t;
-      typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t > Neighbors_t;
-      typedef std::vector < StateSelectorPtr_t > StateSelectors_t;
+namespace manipulation {
+namespace graph {
+HPP_PREDEF_CLASS(Graph);
+HPP_PREDEF_CLASS(Edge);
+HPP_PREDEF_CLASS(State);
+HPP_PREDEF_CLASS(WaypointEdge);
+HPP_PREDEF_CLASS(LevelSetEdge);
+HPP_PREDEF_CLASS(StateSelector);
+HPP_PREDEF_CLASS(GraphComponent);
+HPP_PREDEF_CLASS(GuidedStateSelector);
+typedef shared_ptr<Graph> GraphPtr_t;
+typedef shared_ptr<State> StatePtr_t;
+typedef shared_ptr<Edge> EdgePtr_t;
+typedef shared_ptr<WaypointEdge> WaypointEdgePtr_t;
+typedef shared_ptr<LevelSetEdge> LevelSetEdgePtr_t;
+typedef shared_ptr<StateSelector> StateSelectorPtr_t;
+typedef shared_ptr<GuidedStateSelector> GuidedStateSelectorPtr_t;
+typedef shared_ptr<GraphComponent> GraphComponentPtr_t;
+typedef std::vector<GraphComponentWkPtr_t> GraphComponents_t;
+typedef std::vector<StatePtr_t> States_t;
+typedef std::vector<EdgePtr_t> Edges_t;
+typedef ::hpp::statistics::DiscreteDistribution<EdgePtr_t>::Weight_t Weight_t;
+typedef ::hpp::statistics::DiscreteDistribution<EdgePtr_t> Neighbors_t;
+typedef std::vector<StateSelectorPtr_t> StateSelectors_t;
 
-      typedef hpp::core::segments_t segments_t;
-      typedef std::vector <segments_t> IntervalsContainer_t;
-      typedef hpp::core::NumericalConstraints_t NumericalConstraints_t;
-      typedef hpp::core::LockedJoints_t LockedJoints_t;
+typedef hpp::core::segments_t segments_t;
+typedef std::vector<segments_t> IntervalsContainer_t;
+typedef hpp::core::NumericalConstraints_t NumericalConstraints_t;
+typedef hpp::core::LockedJoints_t LockedJoints_t;
 
-      class Histogram;
-      class StateHistogram;
-      class LeafHistogram;
-      typedef shared_ptr <Histogram> HistogramPtr_t;
-      typedef shared_ptr <StateHistogram> StateHistogramPtr_t;
-      typedef shared_ptr <LeafHistogram> LeafHistogramPtr_t;
-      typedef std::list < HistogramPtr_t > Histograms_t;
+class Histogram;
+class StateHistogram;
+class LeafHistogram;
+typedef shared_ptr<Histogram> HistogramPtr_t;
+typedef shared_ptr<StateHistogram> StateHistogramPtr_t;
+typedef shared_ptr<LeafHistogram> LeafHistogramPtr_t;
+typedef std::list<HistogramPtr_t> Histograms_t;
 
-      class Validation;
-      typedef shared_ptr < Validation > ValidationPtr_t;
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+class Validation;
+typedef shared_ptr<Validation> ValidationPtr_t;
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_GRAPH_FWD_HH
+#endif  // HPP_MANIPULATION_GRAPH_FWD_HH
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 29832a7c65bb18831e3fd9f6298e04ce2626430c..45b057f1b0d02a9ae05678ac969c074a1d9c24bc 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -27,127 +27,119 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
-# define HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
+#define HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
 
-# include <string>
-# include <ostream>
+#include <ostream>
+#include <string>
 
-# include "hpp/manipulation/config.hh"
-# include "hpp/manipulation/deprecated.hh"
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/graph/fwd.hh"
-# include "hpp/manipulation/graph/dot.hh"
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/deprecated.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/dot.hh"
+#include "hpp/manipulation/graph/fwd.hh"
 
 namespace hpp {
-  namespace manipulation {
-    typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-    namespace graph {
-      /// \defgroup constraint_graph Constraint Graph
+namespace manipulation {
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+namespace graph {
+/// \defgroup constraint_graph Constraint Graph
 
-      /// \addtogroup constraint_graph
-      /// \{
+/// \addtogroup constraint_graph
+/// \{
 
-      /// Define common methods of the graph components.
-      class HPP_MANIPULATION_DLLAPI GraphComponent
-      {
-        public:
-          virtual ~GraphComponent () {};
+/// Define common methods of the graph components.
+class HPP_MANIPULATION_DLLAPI GraphComponent {
+ public:
+  virtual ~GraphComponent(){};
 
-          /// Get the component name.
-          const std::string& name() const;
+  /// Get the component name.
+  const std::string& name() const;
 
-          /// Return the component id.
-	  const std::size_t& id () const
-          {
-            return id_;
-          }
+  /// Return the component id.
+  const std::size_t& id() const { return id_; }
 
-          /// Add constraint to the component.
-          virtual void addNumericalConstraint (
-              const ImplicitPtr_t& numConstraint);
+  /// Add constraint to the component.
+  virtual void addNumericalConstraint(const ImplicitPtr_t& numConstraint);
 
-          /// Add a cost function Implicit to the component.
-          virtual void addNumericalCost (const ImplicitPtr_t& numCost);
+  /// Add a cost function Implicit to the component.
+  virtual void addNumericalCost(const ImplicitPtr_t& numCost);
 
-	  /// Reset the numerical constraints stored in the component.
-	  virtual void resetNumericalConstraints ();
+  /// Reset the numerical constraints stored in the component.
+  virtual void resetNumericalConstraints();
 
-          /// Insert the numerical constraints in a ConfigProjector
-          /// \return true is at least one ImplicitPtr_t was inserted.
-          bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
+  /// Insert the numerical constraints in a ConfigProjector
+  /// \return true is at least one ImplicitPtr_t was inserted.
+  bool insertNumericalConstraints(ConfigProjectorPtr_t& proj) const;
 
-          /// Get a reference to the NumericalConstraints_t
-          const NumericalConstraints_t& numericalConstraints() const;
+  /// Get a reference to the NumericalConstraints_t
+  const NumericalConstraints_t& numericalConstraints() const;
 
-          /// Get a reference to the NumericalConstraints_t
-          const NumericalConstraints_t& numericalCosts() const;
+  /// Get a reference to the NumericalConstraints_t
+  const NumericalConstraints_t& numericalCosts() const;
 
-          /// Set the parent graph.
-          void parentGraph(const GraphWkPtr_t& parent);
+  /// Set the parent graph.
+  void parentGraph(const GraphWkPtr_t& parent);
 
-          /// Set the parent graph.
-          GraphPtr_t parentGraph() const;
+  /// Set the parent graph.
+  GraphPtr_t parentGraph() const;
 
-          /// Print the component in DOT language.
-          virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+  /// Print the component in DOT language.
+  virtual std::ostream& dotPrint(
+      std::ostream& os,
+      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
 
-          /// Invalidate the component
-          /// The component needs to be initialized again.
-          virtual void invalidate()
-          {
-            isInit_ = false;
-          }
+  /// Invalidate the component
+  /// The component needs to be initialized again.
+  virtual void invalidate() { isInit_ = false; }
 
-          /// Declare a component as dirty
-          /// \deprecated call invalidate instead
-          void setDirty() HPP_MANIPULATION_DEPRECATED;
+  /// Declare a component as dirty
+  /// \deprecated call invalidate instead
+  void setDirty() HPP_MANIPULATION_DEPRECATED;
 
-        protected:
-          /// Initialize the component
-          void init (const GraphComponentWkPtr_t& weak);
+ protected:
+  /// Initialize the component
+  void init(const GraphComponentWkPtr_t& weak);
 
-          GraphComponent(const std::string& name) : isInit_(false)
-                                                    , name_ (name)
-                                                    , id_(-1)
-          {}
+  GraphComponent(const std::string& name)
+      : isInit_(false), name_(name), id_(-1) {}
 
-          /// Stores the numerical constraints.
-          NumericalConstraints_t numericalConstraints_;
-          /// Stores the numerical costs.
-          NumericalConstraints_t numericalCosts_;
-          /// A weak pointer to the parent graph.
-          GraphWkPtr_t graph_;
+  /// Stores the numerical constraints.
+  NumericalConstraints_t numericalConstraints_;
+  /// Stores the numerical costs.
+  NumericalConstraints_t numericalCosts_;
+  /// A weak pointer to the parent graph.
+  GraphWkPtr_t graph_;
 
-          bool isInit_;
+  bool isInit_;
 
-          void throwIfNotInitialized () const;
+  void throwIfNotInitialized() const;
 
-          /// Print the object in a stream.
-          virtual std::ostream& print (std::ostream& os) const;
-          friend std::ostream& operator<< (std::ostream&, const GraphComponent&);
+  /// Print the object in a stream.
+  virtual std::ostream& print(std::ostream& os) const;
+  friend std::ostream& operator<<(std::ostream&, const GraphComponent&);
 
-          /// Populate DrawingAttributes tooltip
-          virtual void populateTooltip (dot::Tooltip& tp) const;
+  /// Populate DrawingAttributes tooltip
+  virtual void populateTooltip(dot::Tooltip& tp) const;
 
-          virtual void initialize() = 0;
+  virtual void initialize() = 0;
 
-        private:
-          /// Name of the component.
-          std::string name_;
-          /// Weak pointer to itself.
-          GraphComponentWkPtr_t wkPtr_;
-          /// ID of the component (index in components vector).
-	  std::size_t id_;
+ private:
+  /// Name of the component.
+  std::string name_;
+  /// Weak pointer to itself.
+  GraphComponentWkPtr_t wkPtr_;
+  /// ID of the component (index in components vector).
+  std::size_t id_;
 
-          friend class Graph;
-      };
+  friend class Graph;
+};
 
-      std::ostream& operator<< (std::ostream& os, const GraphComponent& graphComp);
+std::ostream& operator<<(std::ostream& os, const GraphComponent& graphComp);
 
-      /// \}
-    } // namespace graph
-  } // namespace manipulation
+/// \}
+}  // namespace graph
+}  // namespace manipulation
 
-} // namespace hpp
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
+#endif  // HPP_MANIPULATION_GRAPH_GRAPHCOMPONENT_HH
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index c0f9d4939504b79ff460e9ea2b420295b0f6fdfc..a51dc274affdf443567e85edd6b23949f412776a 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -27,298 +27,291 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
-# define HPP_MANIPULATION_GRAPH_GRAPH_HH
+#define HPP_MANIPULATION_GRAPH_GRAPH_HH
 
-# include <tuple>
-# include "hpp/manipulation/config.hh"
-# include "hpp/manipulation/constraint-set.hh"
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/graph/fwd.hh"
-# include "hpp/manipulation/graph/graph-component.hh"
+#include <tuple>
+
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/constraint-set.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+#include "hpp/manipulation/graph/graph-component.hh"
 
 namespace hpp {
-  namespace manipulation {
-    typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-    namespace graph {
-      /// \addtogroup constraint_graph
-      /// \{
-
-      /// 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 StateSelector s
-      /// - A StateSelector owns the Node s related to one gripper.
-      /// - A State owns its outgoing Edge s.
-      /// - An Edge does not own anything.
-      class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent
-      {
-        public:
-          /// Create a new Graph.
-	  ///
-	  /// \param robot a manipulation robot
-	  /// \param problem a pointer to the problem
-	  static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
-				   const ProblemPtr_t& problem);
-
-          GraphPtr_t self () const { return wkPtr_.lock(); }
-
-          /// Create and insert a state selector inside the graph.
-          StateSelectorPtr_t createStateSelector (const std::string& name);
-
-          /// Set the state selector
-          /// \warning This should be done before adding nodes to the node
-          /// selector otherwise the pointer to the parent graph will NOT be
-          /// valid.
-          void stateSelector (StateSelectorPtr_t ns);
-
-          /// Get the state selector
-          StateSelectorPtr_t stateSelector () const
-          {
-            return stateSelector_;
-          }
-
-          /// Returns the state of a configuration.
-          StatePtr_t getState (ConfigurationIn_t config) const;
-
-          /// Returns the state of a roadmap node
-          StatePtr_t getState (RoadmapNodePtr_t node) const;
-
-          /// Get possible edges between two nodes.
-          Edges_t getEdges (const StatePtr_t& from, const StatePtr_t& to) const;
-
-          /// Select randomly outgoing edge of the given node.
-          EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
-
-          /// Clear the vector of constraints and complements
-          /// \sa registerConstraints
-          void clearConstraintsAndComplement();
-
-          /// 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 ImplicitPtr_t& constraint,
-                                    const ImplicitPtr_t& complement,
-                                    const ImplicitPtr_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 ImplicitPtr_t& constraint,
-                             const ImplicitPtr_t& complement,
-                             ImplicitPtr_t& combinationOfBoth) const;
-
-          /// Return the vector of tuples as registered in registerConstraints
-          /// \return a vector of tuples (c, c/complement, c/both) each of them
-          ///         corresponding to a constraint, the complement constraint
-          ///         and the combination of boths.
-          const ConstraintsAndComplements_t& constraintsAndComplements () const;
-
-          /// Constraint to project onto the Node.
-          /// \param state the state on which to project.
-          /// \return The initialized projector.
-          ConstraintSetPtr_t configConstraint (const StatePtr_t& state) const;
-
-          /// Constraints of path and target state of an edge
-          /// \deprecated Use tagetConstraint instead.
-          ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const
-            HPP_MANIPULATION_DEPRECATED;
-
-          /// Constraints a configuration in target state should satisfy
-          /// \param edge a transition
-          /// \return The set of constraints a configuration lying in the
-          ///         target state of the edge should satisfy. This set
-          ///         includes the paths constraints of the edge.
-          /// \sa Edge::targetConstraint.
-          ConstraintSetPtr_t targetConstraint (const EdgePtr_t& edge) const;
-
-	  /// Get error of a config with respect to a state constraint
-	  ///
-	  /// \param config Configuration,
-	  /// \param state state containing the constraint to check config against
-	  /// \retval error the error of the state constraint for the
-	  ///         configuration
-	  /// \return whether the configuration belongs to the state.
-	  /// Call method core::ConstraintSet::isSatisfied for the state
-	  /// constraints.
-	  bool getConfigErrorForState (ConfigurationIn_t config,
-				      const StatePtr_t& state, vector_t& error) const;
-
-	  /// Get error of a config with respect to an edge constraint
-	  ///
-	  /// \param config Configuration,
-	  /// \param edge edge containing the constraint to check config against
-	  /// \retval error the error of the edge constraint for the
-	  ///         configuration
-	  /// \return whether the configuration can be a start point of a path
-	  //          of the edge
-	  /// Call core::ConfigProjector::rightHandSideFromConfig with
-	  /// input configuration and method core::ConstraintSet::isSatisfied
-	  /// for the edge constraint.
-	  bool getConfigErrorForEdge (ConfigurationIn_t config,
-				      const EdgePtr_t& edge, vector_t& error) const;
-
-	  /// Get error of a config with respect to an edge foliation leaf
-	  ///
-	  /// \param leafConfig Configuration that determines the foliation leaf
-	  /// \param config Configuration the error of which is computed
-	  /// \retval error the error
-	  /// \return whether config can be the end point of a path of the edge
-	  ///         starting at leafConfig
-	  /// Call methods core::ConfigProjector::rightHandSideFromConfig with
-	  /// leafConfig and then core::ConstraintSet::isSatisfied with config.
-	  /// on the edge constraints.
-	  bool getConfigErrorForEdgeLeaf
-	    (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-	     const EdgePtr_t& edge, vector_t& error) const;
-
-	  /// Get error of a config with respect to the target of an edge foliation leaf
-	  ///
-	  /// \param leafConfig Configuration that determines the foliation leaf
-	  /// \param config Configuration the error of which is computed
-	  /// \retval error the error
-	  /// \return whether config can be the end point of a path of the edge
-	  ///         starting at leafConfig
-	  /// Call methods core::ConfigProjector::rightHandSideFromConfig with
-	  /// leafConfig and then core::ConstraintSet::isSatisfied with config.
-	  /// on the edge constraints.
-	  bool getConfigErrorForEdgeTarget
-	    (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-	     const EdgePtr_t& edge, vector_t& error) const;
-
-          /// Constraint to project a path.
-          /// \param edge a list of edges defining the foliation.
-          /// \return The constraint.
-          ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge) const;
-
-          /// Set maximal number of iterations
-          void maxIterations (size_type iterations);
-
-          /// Get maximal number of iterations in config projector
-          size_type maxIterations () const;
-
-          /// Set error threshold
-          void errorThreshold (const value_type& threshold);
-
-          /// Get error threshold in config projector
-          value_type errorThreshold () const;
-
-          /// Get the robot.
-          const DevicePtr_t& robot () const;
-
-	  /// Get the problem
-	  const ProblemPtr_t& problem () const;
-
-	  /// Set the problem
-          void problem (const ProblemPtr_t& problem);
-
-          /// Register an histogram representing a foliation
-          void insertHistogram (const graph::HistogramPtr_t& hist)
-          {
-            hists_.push_back (hist);
-          }
-
-          /// Get the histograms
-          const Histograms_t& histograms () const
-          {
-            return hists_;
-          }
-
-          /// Get the component by its ID.
-          GraphComponentWkPtr_t get(std::size_t id) const;
-
-          std::size_t nbComponents () const
-          {
-            return components_.size();
-          }
-
-          /// Print the component in DOT language.
-          virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
-          /// Initialize all components of the graph (edges and states)
-          virtual void initialize ();
-
-          /// Invalidate all states and edges of the graph
-          virtual void invalidate();
-
-        protected:
-          /// Initialization of the object.
-          void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
-
-          /// Constructor
-	  /// \param sm a steering method to create paths from edges
-          Graph (const std::string& name, const ProblemPtr_t& problem);
-
-          /// Print the object in a stream.
-          std::ostream& print (std::ostream& os) const;
-
-        private:
-          /// The list of elements
-          GraphComponents_t& components ();
-
-          /// Keep track of the created components
-          GraphComponents_t components_;
-
-          /// This list contains a state selector for each end-effector.
-          StateSelectorPtr_t stateSelector_;
-
-          /// A set of constraints that will always be used, for example
-          /// stability constraints.
-          ConstraintPtr_t constraints_;
-
-          /// Keep a pointer to the composite robot.
-          DevicePtr_t robot_;
+namespace manipulation {
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+namespace graph {
+/// \addtogroup constraint_graph
+/// \{
+
+/// 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 StateSelector s
+/// - A StateSelector owns the Node s related to one gripper.
+/// - A State owns its outgoing Edge s.
+/// - An Edge does not own anything.
+class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
+ public:
+  /// Create a new Graph.
+  ///
+  /// \param robot a manipulation robot
+  /// \param problem a pointer to the problem
+  static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
+                           const ProblemPtr_t& problem);
+
+  GraphPtr_t self() const { return wkPtr_.lock(); }
+
+  /// Create and insert a state selector inside the graph.
+  StateSelectorPtr_t createStateSelector(const std::string& name);
+
+  /// Set the state selector
+  /// \warning This should be done before adding nodes to the node
+  /// selector otherwise the pointer to the parent graph will NOT be
+  /// valid.
+  void stateSelector(StateSelectorPtr_t ns);
+
+  /// Get the state selector
+  StateSelectorPtr_t stateSelector() const { return stateSelector_; }
+
+  /// Returns the state of a configuration.
+  StatePtr_t getState(ConfigurationIn_t config) const;
+
+  /// Returns the state of a roadmap node
+  StatePtr_t getState(RoadmapNodePtr_t node) const;
+
+  /// Get possible edges between two nodes.
+  Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
+
+  /// Select randomly outgoing edge of the given node.
+  EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
+
+  /// Clear the vector of constraints and complements
+  /// \sa registerConstraints
+  void clearConstraintsAndComplement();
+
+  /// 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 ImplicitPtr_t& constraint,
+                           const ImplicitPtr_t& complement,
+                           const ImplicitPtr_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 ImplicitPtr_t& constraint,
+                    const ImplicitPtr_t& complement,
+                    ImplicitPtr_t& combinationOfBoth) const;
+
+  /// Return the vector of tuples as registered in registerConstraints
+  /// \return a vector of tuples (c, c/complement, c/both) each of them
+  ///         corresponding to a constraint, the complement constraint
+  ///         and the combination of boths.
+  const ConstraintsAndComplements_t& constraintsAndComplements() const;
+
+  /// Constraint to project onto the Node.
+  /// \param state the state on which to project.
+  /// \return The initialized projector.
+  ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
+
+  /// Constraints of path and target state of an edge
+  /// \deprecated Use tagetConstraint instead.
+  ConstraintSetPtr_t configConstraint(const EdgePtr_t& edge) const
+      HPP_MANIPULATION_DEPRECATED;
+
+  /// Constraints a configuration in target state should satisfy
+  /// \param edge a transition
+  /// \return The set of constraints a configuration lying in the
+  ///         target state of the edge should satisfy. This set
+  ///         includes the paths constraints of the edge.
+  /// \sa Edge::targetConstraint.
+  ConstraintSetPtr_t targetConstraint(const EdgePtr_t& edge) const;
+
+  /// Get error of a config with respect to a state constraint
+  ///
+  /// \param config Configuration,
+  /// \param state state containing the constraint to check config against
+  /// \retval error the error of the state constraint for the
+  ///         configuration
+  /// \return whether the configuration belongs to the state.
+  /// Call method core::ConstraintSet::isSatisfied for the state
+  /// constraints.
+  bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t& state,
+                              vector_t& error) const;
+
+  /// Get error of a config with respect to an edge constraint
+  ///
+  /// \param config Configuration,
+  /// \param edge edge containing the constraint to check config against
+  /// \retval error the error of the edge constraint for the
+  ///         configuration
+  /// \return whether the configuration can be a start point of a path
+  //          of the edge
+  /// Call core::ConfigProjector::rightHandSideFromConfig with
+  /// input configuration and method core::ConstraintSet::isSatisfied
+  /// for the edge constraint.
+  bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t& edge,
+                             vector_t& error) const;
+
+  /// Get error of a config with respect to an edge foliation leaf
+  ///
+  /// \param leafConfig Configuration that determines the foliation leaf
+  /// \param config Configuration the error of which is computed
+  /// \retval error the error
+  /// \return whether config can be the end point of a path of the edge
+  ///         starting at leafConfig
+  /// Call methods core::ConfigProjector::rightHandSideFromConfig with
+  /// leafConfig and then core::ConstraintSet::isSatisfied with config.
+  /// on the edge constraints.
+  bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
+                                 ConfigurationIn_t config,
+                                 const EdgePtr_t& edge, vector_t& error) const;
+
+  /// Get error of a config with respect to the target of an edge foliation leaf
+  ///
+  /// \param leafConfig Configuration that determines the foliation leaf
+  /// \param config Configuration the error of which is computed
+  /// \retval error the error
+  /// \return whether config can be the end point of a path of the edge
+  ///         starting at leafConfig
+  /// Call methods core::ConfigProjector::rightHandSideFromConfig with
+  /// leafConfig and then core::ConstraintSet::isSatisfied with config.
+  /// on the edge constraints.
+  bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
+                                   ConfigurationIn_t config,
+                                   const EdgePtr_t& edge,
+                                   vector_t& error) const;
+
+  /// Constraint to project a path.
+  /// \param edge a list of edges defining the foliation.
+  /// \return The constraint.
+  ConstraintSetPtr_t pathConstraint(const EdgePtr_t& edge) const;
+
+  /// Set maximal number of iterations
+  void maxIterations(size_type iterations);
+
+  /// Get maximal number of iterations in config projector
+  size_type maxIterations() const;
+
+  /// Set error threshold
+  void errorThreshold(const value_type& threshold);
+
+  /// Get error threshold in config projector
+  value_type errorThreshold() const;
+
+  /// Get the robot.
+  const DevicePtr_t& robot() const;
+
+  /// Get the problem
+  const ProblemPtr_t& problem() const;
+
+  /// Set the problem
+  void problem(const ProblemPtr_t& problem);
+
+  /// Register an histogram representing a foliation
+  void insertHistogram(const graph::HistogramPtr_t& hist) {
+    hists_.push_back(hist);
+  }
+
+  /// Get the histograms
+  const Histograms_t& histograms() const { return hists_; }
+
+  /// Get the component by its ID.
+  GraphComponentWkPtr_t get(std::size_t id) const;
+
+  std::size_t nbComponents() const { return components_.size(); }
+
+  /// Print the component in DOT language.
+  virtual std::ostream& dotPrint(
+      std::ostream& os,
+      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
+
+  /// Initialize all components of the graph (edges and states)
+  virtual void initialize();
+
+  /// Invalidate all states and edges of the graph
+  virtual void invalidate();
+
+ protected:
+  /// Initialization of the object.
+  void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
+
+  /// Constructor
+  /// \param sm a steering method to create paths from edges
+  Graph(const std::string& name, const ProblemPtr_t& problem);
+
+  /// Print the object in a stream.
+  std::ostream& print(std::ostream& os) const;
+
+ private:
+  /// The list of elements
+  GraphComponents_t& components();
+
+  /// Keep track of the created components
+  GraphComponents_t components_;
+
+  /// This list contains a state selector for each end-effector.
+  StateSelectorPtr_t stateSelector_;
+
+  /// A set of constraints that will always be used, for example
+  /// stability constraints.
+  ConstraintPtr_t constraints_;
+
+  /// Keep a pointer to the composite robot.
+  DevicePtr_t robot_;
 
-          /// Weak pointer to itself.
-          GraphWkPtr_t wkPtr_;
+  /// Weak pointer to itself.
+  GraphWkPtr_t wkPtr_;
 
-          /// Map of constraint sets (from State).
-          typedef std::map  < StatePtr_t, ConstraintSetPtr_t > MapFromState;
-          typedef std::pair < StatePtr_t, ConstraintSetPtr_t > PairStateConstraints;
-          MapFromState constraintSetMapFromState_;
+  /// Map of constraint sets (from State).
+  typedef std::map<StatePtr_t, ConstraintSetPtr_t> MapFromState;
+  typedef std::pair<StatePtr_t, ConstraintSetPtr_t> PairStateConstraints;
+  MapFromState constraintSetMapFromState_;
 
-          /// List of histograms
-          Histograms_t hists_;
+  /// List of histograms
+  Histograms_t hists_;
 
-          /// Map of constraint sets (from Edge).
-          typedef std::map  < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
-          typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
-          MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
-	  ProblemPtr_t problem_;
-          value_type errorThreshold_;
-          size_type maxIterations_;
+  /// Map of constraint sets (from Edge).
+  typedef std::map<EdgePtr_t, ConstraintSetPtr_t> MapFromEdge;
+  typedef std::pair<EdgePtr_t, ConstraintSetPtr_t> PairEdgeConstraints;
+  MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
+  ProblemPtr_t problem_;
+  value_type errorThreshold_;
+  size_type maxIterations_;
 
-          ConstraintsAndComplements_t constraintsAndComplements_;
-          friend class GraphComponent;
-      }; // Class Graph
+  ConstraintsAndComplements_t constraintsAndComplements_;
+  friend class GraphComponent;
+};  // Class Graph
 
-      /// \}
-    } // namespace graph
-  } // namespace manipulation
+/// \}
+}  // namespace graph
+}  // namespace manipulation
 
-} // namespace hpp
+}  // namespace hpp
 
 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
 
-#endif // HPP_MANIPULATION_GRAPH_GRAPH_HH
+#endif  // HPP_MANIPULATION_GRAPH_GRAPH_HH
diff --git a/include/hpp/manipulation/graph/guided-state-selector.hh b/include/hpp/manipulation/graph/guided-state-selector.hh
index 900ddee7e2a55ebf67ea63d328336010fdeea4e8..2f5c42721c36cfdfaef6ffbc16bc1b9441d4031a 100644
--- a/include/hpp/manipulation/graph/guided-state-selector.hh
+++ b/include/hpp/manipulation/graph/guided-state-selector.hh
@@ -27,56 +27,54 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
-# define HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
+#define HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
 
 #include "hpp/manipulation/fwd.hh"
 #include "hpp/manipulation/graph/fwd.hh"
 #include "hpp/manipulation/graph/state-selector.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      class HPP_MANIPULATION_DLLAPI GuidedStateSelector : public StateSelector
-      {
-        public:
-          /// Create a new GuidedStateSelector.
-          static GuidedStateSelectorPtr_t create(const std::string& name,
-              const core::RoadmapPtr_t& roadmap);
+namespace manipulation {
+namespace graph {
+class HPP_MANIPULATION_DLLAPI GuidedStateSelector : public StateSelector {
+ public:
+  /// Create a new GuidedStateSelector.
+  static GuidedStateSelectorPtr_t create(const std::string& name,
+                                         const core::RoadmapPtr_t& roadmap);
 
-          /// Set the target
-          void setStateList (const States_t& stateList);
+  /// Set the target
+  void setStateList(const States_t& stateList);
 
-          /// Select randomly an outgoing edge of the given node.
-          virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
+  /// Select randomly an outgoing edge of the given node.
+  virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
 
-          /// Print the object in a stream.
-          std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+  /// Print the object in a stream.
+  std::ostream& dotPrint(std::ostream& os, dot::DrawingAttributes da =
+                                               dot::DrawingAttributes()) const;
 
-        protected:
-          /// Initialization of the object.
-          void init (const GuidedStateSelectorPtr_t& weak);
+ protected:
+  /// Initialization of the object.
+  void init(const GuidedStateSelectorPtr_t& weak);
 
-          /// Constructor
-          GuidedStateSelector (const std::string& name,
-              const core::RoadmapPtr_t roadmap) :
-            StateSelector (name), roadmap_ (roadmap)
-          {}
+  /// Constructor
+  GuidedStateSelector(const std::string& name, const core::RoadmapPtr_t roadmap)
+      : StateSelector(name), roadmap_(roadmap) {}
 
-          /// Print the object in a stream.
-          std::ostream& print (std::ostream& os) const;
+  /// Print the object in a stream.
+  std::ostream& print(std::ostream& os) const;
 
-        private:
-          /// The target
-          States_t stateList_;
+ private:
+  /// The target
+  States_t stateList_;
 
-          /// The roadmap
-          core::RoadmapPtr_t roadmap_;
+  /// The roadmap
+  core::RoadmapPtr_t roadmap_;
 
-          /// Weak pointer to itself.
-          GuidedStateSelectorWkPtr_t wkPtr_;
-      }; // Class StateSelector
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+  /// Weak pointer to itself.
+  GuidedStateSelectorWkPtr_t wkPtr_;
+};  // Class StateSelector
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
+#endif  // HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index f6a78e4d852d9376230cb2c31b039230d1372e1e..52b55f595262daa3d0e5398f2fd880102591495e 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -27,185 +27,169 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_HELPER_HH
-# define HPP_MANIPULATION_GRAPH_HELPER_HH
+#define HPP_MANIPULATION_GRAPH_HELPER_HH
 
-# include <string>
-# include <algorithm>
+#include <algorithm>
+#include <string>
+#include <tuple>
 
-# include <tuple>
-
-# include "hpp/manipulation/config.hh"
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/graph/fwd.hh"
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
 
 namespace hpp {
-  namespace manipulation {
-    typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-    namespace graph {
-      namespace helper {
-        /// \defgroup helper Helpers to build the graph of constraints
-        /// \addtogroup helper
-        /// \{
-
-        NumericalConstraints_t merge_nc
-          (const NumericalConstraints_t& a, const NumericalConstraints_t& b) {
-            NumericalConstraints_t nc;
-            nc.reserve (a.size() + b.size());
-            std::copy (a.begin(), a.end(), nc.begin());
-            std::copy (b.begin(), b.end(), nc.begin());
-            return nc;
-          }
-
-        struct FoliatedManifold {
-          // Manifold definition
-          NumericalConstraints_t nc;
-          LockedJoints_t lj;
-          NumericalConstraints_t nc_path;
-          // Foliation definition
-          NumericalConstraints_t nc_fol;
-          LockedJoints_t lj_fol;
-
-          FoliatedManifold merge (const FoliatedManifold& other) {
-            FoliatedManifold both;
-            both.nc = merge_nc(nc, other.nc);
-            both.nc_path = merge_nc(nc_path, other.nc_path);
-
-            std::copy (lj.begin (), lj.end (), both.lj.end ());
-            std::copy (other.lj.begin (), other.lj.end (), both.lj.end ());
-            return both;
-          }
-
-          void addToState (StatePtr_t comp) const;
-          void addToEdge (EdgePtr_t comp) const;
-          void specifyFoliation (LevelSetEdgePtr_t lse) const;
-
-          bool foliated () const {
-            return !lj_fol.empty () || !nc_fol.empty ();
-          }
-          bool empty () const {
-            return lj.empty () && nc.empty ();
-          }
-        };
-
-        enum GraspingCase {
-          NoGrasp = 1 << 0,
-          GraspOnly = 1 << 1,
-          WithPreGrasp = 1 << 2
-        };
-        enum PlacementCase {
-          NoPlace = 1 << 3,
-          PlaceOnly = 1 << 4,
-          WithPrePlace = 1 << 5
-        };
-
-	struct Rule {
-          std::vector<std::string> grippers_;
-          std::vector<std::string> handles_;
-	  bool link_;
-	  Rule() : grippers_(), handles_(), link_(false) {}
-	};
-
-        typedef std::vector<Rule> Rules_t;
-
-        /// Create edges according to the case.
-        /// gCase is a logical OR combination of GraspingCase and PlacementCase
-        ///
-        /// When an argument is not relevant, use the default constructor
-        /// of FoliatedManifold
-        template < int gCase >
-          Edges_t createEdges (
-              const std::string& forwName,   const std::string& backName,
-              const StatePtr_t& from,         const StatePtr_t& to,
-              const size_type& wForw,        const size_type& wBack,
-              const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
-              const FoliatedManifold& place, const FoliatedManifold& preplace,
-              const bool levelSetGrasp,      const bool levelSetPlace,
-              const FoliatedManifold& submanifoldDef = FoliatedManifold ()
-              );
-
-        EdgePtr_t createLoopEdge (
-              const std::string& loopName,
-              const StatePtr_t& state,
-              const size_type& w,
-              const bool levelSet,
-              const FoliatedManifold& submanifoldDef = FoliatedManifold ()
-              );
-
-        /// Create a waypoint edge taking into account:
-        /// \li grasp
-        /// \li placement
-        /// \li preplacement
-
-        /// Create a waypoint edge taking into account
-        /// \li grasp
-        /// \li pregrasp
-        /// \li placement
-
-        void graspManifold (
-            const GripperPtr_t& gripper, const HandlePtr_t& handle,
-            FoliatedManifold& grasp, FoliatedManifold& pregrasp);
-
-        /// The placement foliation constraint is built using
-        /// hpp::constraints::ConvexShapeMatcherComplement
-        void strictPlacementManifold (
-            const ImplicitPtr_t placement,
-            const ImplicitPtr_t preplacement,
-            const ImplicitPtr_t placementComplement,
-            FoliatedManifold& place, FoliatedManifold& preplace);
-
-        /// The placement foliation constraint is built locked joints
-        /// It is faster than strictPlacementManifold but the foliation
-        /// parametrisation is redundant.
-        void relaxedPlacementManifold (
-            const ImplicitPtr_t placement,
-            const ImplicitPtr_t preplacement,
-            const LockedJoints_t objectLocks,
-            FoliatedManifold& place, FoliatedManifold& preplace);
-
-        typedef std::tuple <ImplicitPtr_t,
-                            ImplicitPtr_t,
-                            LockedJoints_t>
-                            PlacementConstraint_t;
-        typedef std::vector <HandlePtr_t> Handles_t;
-        typedef std::vector <GripperPtr_t> Grippers_t;
-        /// Tuple representing an object as follows:
-        /// \li PlacementConstraint_t constraint to place the object
-        /// \li Handles_t             list of handles of the object
-        /// \li std::size_t           the index of this tuple in Objects_t.
-        /// \note the index must be unique, as object equallity is checked using this index.
-        typedef std::tuple <PlacementConstraint_t, Handles_t, std::size_t> Object_t;
-        typedef std::vector <Object_t> Objects_t;
-
-        /// Fill a Graph 
-        ///
-        /// \note It is assumed that a gripper can grasp only one handle and each
-        /// handle cannot be grasped by several grippers at the same time.
-        ///
-        /// \param[in,out] graph must be an initialized empty Graph.
-        void graphBuilder (
-            const ProblemSolverPtr_t& ps,
-            const Objects_t& objects,
-            const Grippers_t& grippers,
-            GraphPtr_t graph,
-            const Rules_t& rules = Rules_t ());
-
-        struct ObjectDef_t {
-          std::string name;
-          StringList_t handles, shapes;
-        };
-
-        GraphPtr_t graphBuilder (
-            const ProblemSolverPtr_t& ps,
-            const std::string& graphName,
-            const StringList_t& griNames,
-            const std::list <ObjectDef_t>& objs,
-            const StringList_t& envNames,
-	    const Rules_t& rules,
-            const value_type& prePlaceWidth = 0.05);
-        /// \}
-      } // namespace helper
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPH_HELPER_HH
+namespace manipulation {
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+namespace graph {
+namespace helper {
+/// \defgroup helper Helpers to build the graph of constraints
+/// \addtogroup helper
+/// \{
+
+NumericalConstraints_t merge_nc(const NumericalConstraints_t& a,
+                                const NumericalConstraints_t& b) {
+  NumericalConstraints_t nc;
+  nc.reserve(a.size() + b.size());
+  std::copy(a.begin(), a.end(), nc.begin());
+  std::copy(b.begin(), b.end(), nc.begin());
+  return nc;
+}
+
+struct FoliatedManifold {
+  // Manifold definition
+  NumericalConstraints_t nc;
+  LockedJoints_t lj;
+  NumericalConstraints_t nc_path;
+  // Foliation definition
+  NumericalConstraints_t nc_fol;
+  LockedJoints_t lj_fol;
+
+  FoliatedManifold merge(const FoliatedManifold& other) {
+    FoliatedManifold both;
+    both.nc = merge_nc(nc, other.nc);
+    both.nc_path = merge_nc(nc_path, other.nc_path);
+
+    std::copy(lj.begin(), lj.end(), both.lj.end());
+    std::copy(other.lj.begin(), other.lj.end(), both.lj.end());
+    return both;
+  }
+
+  void addToState(StatePtr_t comp) const;
+  void addToEdge(EdgePtr_t comp) const;
+  void specifyFoliation(LevelSetEdgePtr_t lse) const;
+
+  bool foliated() const { return !lj_fol.empty() || !nc_fol.empty(); }
+  bool empty() const { return lj.empty() && nc.empty(); }
+};
+
+enum GraspingCase {
+  NoGrasp = 1 << 0,
+  GraspOnly = 1 << 1,
+  WithPreGrasp = 1 << 2
+};
+enum PlacementCase {
+  NoPlace = 1 << 3,
+  PlaceOnly = 1 << 4,
+  WithPrePlace = 1 << 5
+};
+
+struct Rule {
+  std::vector<std::string> grippers_;
+  std::vector<std::string> handles_;
+  bool link_;
+  Rule() : grippers_(), handles_(), link_(false) {}
+};
+
+typedef std::vector<Rule> Rules_t;
+
+/// Create edges according to the case.
+/// gCase is a logical OR combination of GraspingCase and PlacementCase
+///
+/// When an argument is not relevant, use the default constructor
+/// of FoliatedManifold
+template <int gCase>
+Edges_t createEdges(
+    const std::string& forwName, const std::string& backName,
+    const StatePtr_t& from, const StatePtr_t& to, const size_type& wForw,
+    const size_type& wBack, const FoliatedManifold& grasp,
+    const FoliatedManifold& pregrasp, const FoliatedManifold& place,
+    const FoliatedManifold& preplace, const bool levelSetGrasp,
+    const bool levelSetPlace,
+    const FoliatedManifold& submanifoldDef = FoliatedManifold());
+
+EdgePtr_t createLoopEdge(
+    const std::string& loopName, const StatePtr_t& state, const size_type& w,
+    const bool levelSet,
+    const FoliatedManifold& submanifoldDef = FoliatedManifold());
+
+/// Create a waypoint edge taking into account:
+/// \li grasp
+/// \li placement
+/// \li preplacement
+
+/// Create a waypoint edge taking into account
+/// \li grasp
+/// \li pregrasp
+/// \li placement
+
+void graspManifold(const GripperPtr_t& gripper, const HandlePtr_t& handle,
+                   FoliatedManifold& grasp, FoliatedManifold& pregrasp);
+
+/// The placement foliation constraint is built using
+/// hpp::constraints::ConvexShapeMatcherComplement
+void strictPlacementManifold(const ImplicitPtr_t placement,
+                             const ImplicitPtr_t preplacement,
+                             const ImplicitPtr_t placementComplement,
+                             FoliatedManifold& place,
+                             FoliatedManifold& preplace);
+
+/// The placement foliation constraint is built locked joints
+/// It is faster than strictPlacementManifold but the foliation
+/// parametrisation is redundant.
+void relaxedPlacementManifold(const ImplicitPtr_t placement,
+                              const ImplicitPtr_t preplacement,
+                              const LockedJoints_t objectLocks,
+                              FoliatedManifold& place,
+                              FoliatedManifold& preplace);
+
+typedef std::tuple<ImplicitPtr_t, ImplicitPtr_t, LockedJoints_t>
+    PlacementConstraint_t;
+typedef std::vector<HandlePtr_t> Handles_t;
+typedef std::vector<GripperPtr_t> Grippers_t;
+/// Tuple representing an object as follows:
+/// \li PlacementConstraint_t constraint to place the object
+/// \li Handles_t             list of handles of the object
+/// \li std::size_t           the index of this tuple in Objects_t.
+/// \note the index must be unique, as object equallity is checked using this
+/// index.
+typedef std::tuple<PlacementConstraint_t, Handles_t, std::size_t> Object_t;
+typedef std::vector<Object_t> Objects_t;
+
+/// Fill a Graph
+///
+/// \note It is assumed that a gripper can grasp only one handle and each
+/// handle cannot be grasped by several grippers at the same time.
+///
+/// \param[in,out] graph must be an initialized empty Graph.
+void graphBuilder(const ProblemSolverPtr_t& ps, const Objects_t& objects,
+                  const Grippers_t& grippers, GraphPtr_t graph,
+                  const Rules_t& rules = Rules_t());
+
+struct ObjectDef_t {
+  std::string name;
+  StringList_t handles, shapes;
+};
+
+GraphPtr_t graphBuilder(const ProblemSolverPtr_t& ps,
+                        const std::string& graphName,
+                        const StringList_t& griNames,
+                        const std::list<ObjectDef_t>& objs,
+                        const StringList_t& envNames, const Rules_t& rules,
+                        const value_type& prePlaceWidth = 0.05);
+/// \}
+}  // namespace helper
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPH_HELPER_HH
diff --git a/include/hpp/manipulation/graph/state-selector.hh b/include/hpp/manipulation/graph/state-selector.hh
index c25d74c0dd1d808eed1468a11b053700472c4112..aca693619fdec2ab5115d6cbff6963dbab912311 100644
--- a/include/hpp/manipulation/graph/state-selector.hh
+++ b/include/hpp/manipulation/graph/state-selector.hh
@@ -27,7 +27,7 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
-# define HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
+#define HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
 
 #include "hpp/manipulation/config.hh"
 #include "hpp/manipulation/fwd.hh"
@@ -35,85 +35,81 @@
 #include "hpp/manipulation/graph/state.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      /// This class is used to get the state of a configuration. States have to
-      /// be ordered because a configuration can be in several states.
-      class HPP_MANIPULATION_DLLAPI StateSelector
-      {
-        public:
-          virtual ~StateSelector () {};
-
-          /// Create a new StateSelector.
-          static StateSelectorPtr_t create(const std::string& name);
-
-          const std::string& name() const
-          {
-            return name_;
-          }
-
-          /// Create an empty state
-          StatePtr_t createState (const std::string& name, bool waypoint = false,
-              const int w = 0);
-
-          /// Returns the state of a configuration.
-          StatePtr_t getState(ConfigurationIn_t config) const;
-
-          /// Returns the state of a roadmap state
-          StatePtr_t getState(RoadmapNodePtr_t node) const;
-
-          /// Returns a list of all the states
-          States_t getStates () const;
-
-          /// Returns a list of all the states
-          States_t getWaypointStates () const;
-
-          /// Select randomly an outgoing edge of the given state.
-          virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
-
-          /// Print the object in a stream.
-          virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
-          /// Set the parent graph.
-          void parentGraph(const GraphWkPtr_t& parent);
-
-          /// Set the parent graph.
-          GraphPtr_t parentGraph() const;
-
-        protected:
-          /// Initialization of the object.
-          void init (const StateSelectorPtr_t& weak);
-
-          /// Constructor
-          StateSelector (const std::string& name) : name_ (name)
-          {}
-
-          /// Print the object in a stream.
-          virtual std::ostream& print (std::ostream& os) const;
-
-          /// List of the states of one end-effector, ordered by priority.
-          typedef std::pair <int, StatePtr_t> WeighedState_t;
-          typedef std::list <WeighedState_t> WeighedStates_t;
-          WeighedStates_t orderedStates_;
-          States_t waypoints_;
-
-        private:
-          /// Name of the component.
-          std::string name_;
-          /// A weak pointer to the parent graph.
-          GraphWkPtr_t graph_;
-          /// Weak pointer to itself.
-          StateSelectorPtr_t wkPtr_;
-
-          friend std::ostream& operator<< (std::ostream& os, const StateSelector& ss);
-      }; // Class StateSelector
-
-      inline std::ostream& operator<< (std::ostream& os, const StateSelector& ss)
-      {
-        return ss.print(os);
-      }
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
+namespace manipulation {
+namespace graph {
+/// This class is used to get the state of a configuration. States have to
+/// be ordered because a configuration can be in several states.
+class HPP_MANIPULATION_DLLAPI StateSelector {
+ public:
+  virtual ~StateSelector(){};
+
+  /// Create a new StateSelector.
+  static StateSelectorPtr_t create(const std::string& name);
+
+  const std::string& name() const { return name_; }
+
+  /// Create an empty state
+  StatePtr_t createState(const std::string& name, bool waypoint = false,
+                         const int w = 0);
+
+  /// Returns the state of a configuration.
+  StatePtr_t getState(ConfigurationIn_t config) const;
+
+  /// Returns the state of a roadmap state
+  StatePtr_t getState(RoadmapNodePtr_t node) const;
+
+  /// Returns a list of all the states
+  States_t getStates() const;
+
+  /// Returns a list of all the states
+  States_t getWaypointStates() const;
+
+  /// Select randomly an outgoing edge of the given state.
+  virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
+
+  /// Print the object in a stream.
+  virtual std::ostream& dotPrint(
+      std::ostream& os,
+      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
+
+  /// Set the parent graph.
+  void parentGraph(const GraphWkPtr_t& parent);
+
+  /// Set the parent graph.
+  GraphPtr_t parentGraph() const;
+
+ protected:
+  /// Initialization of the object.
+  void init(const StateSelectorPtr_t& weak);
+
+  /// Constructor
+  StateSelector(const std::string& name) : name_(name) {}
+
+  /// Print the object in a stream.
+  virtual std::ostream& print(std::ostream& os) const;
+
+  /// List of the states of one end-effector, ordered by priority.
+  typedef std::pair<int, StatePtr_t> WeighedState_t;
+  typedef std::list<WeighedState_t> WeighedStates_t;
+  WeighedStates_t orderedStates_;
+  States_t waypoints_;
+
+ private:
+  /// Name of the component.
+  std::string name_;
+  /// A weak pointer to the parent graph.
+  GraphWkPtr_t graph_;
+  /// Weak pointer to itself.
+  StateSelectorPtr_t wkPtr_;
+
+  friend std::ostream& operator<<(std::ostream& os, const StateSelector& ss);
+};  // Class StateSelector
+
+inline std::ostream& operator<<(std::ostream& os, const StateSelector& ss) {
+  return ss.print(os);
+}
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
index 1a671870a881ae4da433c4e2d38e9b480d5b52f3..35be28d1cfa1526613180efdb439a1de838e1055 100644
--- a/include/hpp/manipulation/graph/state.hh
+++ b/include/hpp/manipulation/graph/state.hh
@@ -27,187 +27,158 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_STATE_HH
-# define HPP_MANIPULATION_GRAPH_STATE_HH
+#define HPP_MANIPULATION_GRAPH_STATE_HH
 
 #include <functional>
-
-#include <hpp/core/constraint-set.hh>
-#include <hpp/core/config-projector.hh>
 #include <hpp/constraints/implicit.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/constraint-set.hh>
 
 #include "hpp/manipulation/config.hh"
 #include "hpp/manipulation/fwd.hh"
-#include "hpp/manipulation/graph/fwd.hh"
 #include "hpp/manipulation/graph/edge.hh"
+#include "hpp/manipulation/graph/fwd.hh"
 #include "hpp/manipulation/graph/graph-component.hh"
 
 namespace hpp {
-  namespace manipulation {
-    using constraints::Implicit;
-    using constraints::ImplicitPtr_t;
-    namespace graph {
-      /// \addtogroup constraint_graph
-      /// \{
-
-      /// State of an end-effector.
-      ///
-      /// States of the graph of constraints. There is one
-      /// graph for each end-effector.
-      class HPP_MANIPULATION_DLLAPI State : public GraphComponent
-      {
-        public:
-	typedef std::function < EdgePtr_t
-				(const std::string&,
-				 const GraphWkPtr_t&,
-				 const StateWkPtr_t&, const StateWkPtr_t&) >
-	EdgeFactory;
-          /// Destructor
-          ~State ();
-
-          /// Create a new state.
-          static StatePtr_t create (const std::string& name);
-
-          /// Create a link from this state to the given state.
-          /// \param w if strictly negative, the edge is not included in the neighbor
-          ///          list. Otherwise, it is included with Weight_t w
-          EdgePtr_t linkTo (const std::string& name, const StatePtr_t& to,
-			    const size_type& w = 1,
-			    EdgeFactory create = Edge::create);
-
-          /// Check whether the configuration is in this state.
-          /// \code
-          ///   return configConstraint()->isSatisfied (config);
-          /// \endcode
-          /// \note You should not use this method to know in which states a
-          /// configuration is. This only checks if the configuration satisfies
-          /// the constraints. Instead, use the class StateSelector.
-          virtual bool contains (ConfigurationIn_t config) const;
-
-          inline bool isWaypoint () const
-          {
-            return isWaypoint_;
-          }
-
-          inline void isWaypoint (bool isWaypoint)
-          {
-            isWaypoint_ = isWaypoint;
-          }
-
-          /// Get the parent StateSelector.
-          StateSelectorWkPtr_t stateSelector () const
-          {
-            return selector_;
-          }
-
-          /// Set the StateSelector containing this state.
-          void stateSelector (const StateSelectorWkPtr_t& parent)
-          {
-            selector_ = parent;
-          };
-
-          /// Get the neighbors
-          const Neighbors_t& neighbors() const
-          {
-            return neighbors_;
-          }
-
-          /// Get the neighbors
-          Edges_t neighborEdges() const
-          {
-            return neighbors_.values();
-          }
-
-          /// Get the hidden neighbors
-          /// It is a vector of transitions outgoing from this state and that are
-          /// included in a waypoint edge.
-          const Edges_t& hiddenNeighbors() const
-          {
-            return hiddenNeighbors_;
-          }
-
-          /// Set weight of edge starting from this state.
-          void updateWeight (const EdgePtr_t&edge, const Weight_t& w);
-
-          /// Get weight of edge starting from this state.
-          Weight_t getWeight (const EdgePtr_t&edge);
-
-          /// Constraint to project onto this state.
-          ConstraintSetPtr_t configConstraint() const
-          {
-            throwIfNotInitialized ();
-            return configConstraints_;
-          }
-
-          /// Add constraint to the state
-	  /// Call the parent implementation.
-	  /// \throw std::logic_error if the constraint is parameterizable
-	  /// (contains at least one Equality comparison type).
-          virtual void addNumericalConstraint (
-              const ImplicitPtr_t& numConstraint);
-
-          /// Add a constraint for paths that lie in this state.
-          virtual void addNumericalConstraintForPath (const ImplicitPtr_t& nm)
-          {
-            invalidate();
-            numericalConstraintsForPath_.push_back (nm);
-          }
-
-          /// Insert the numerical constraints in a ConfigProjector
-          /// \return true is at least one ImplicitPtr_t was inserted.
-          bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
-          {
-            for (const auto& nc : numericalConstraintsForPath_)
-              proj->add (nc);
-            return !numericalConstraintsForPath_.empty ();
-          }
-
-          /// Get a reference to the NumericalConstraints_t
-          const NumericalConstraints_t& numericalConstraintsForPath () const
-          {
-            return numericalConstraintsForPath_;
-          }
-
-          /// Print the object in a stream.
-          std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
-        protected:
-          /// Initialize the object.
-          void init (const StateWkPtr_t& self);
-
-          /// Constructor
-          State(const std::string& name);
-
-          /// Print the object in a stream.
-          std::ostream& print (std::ostream& os) const;
-
-          virtual void populateTooltip (dot::Tooltip& tp) const;
-
-          virtual void initialize ();
-
-        private:
-          /// List of possible motions from this state (i.e. the outgoing
-          /// vertices).
-          Neighbors_t neighbors_;
-          Edges_t hiddenNeighbors_;
-
-          /// Set of constraints to be statisfied.
-          ConstraintSetPtr_t configConstraints_;
-
-          /// Stores the numerical constraints for path.
-          NumericalConstraints_t numericalConstraintsForPath_;
-
-          /// A selector that will implement the selection of the next state.
-          StateSelectorWkPtr_t selector_;
-
-          /// Weak pointer to itself.
-          StateWkPtr_t wkPtr_;
-
-          bool isWaypoint_;
-      }; // class State
-
-      /// \}
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPH_STATE_HH
+namespace manipulation {
+using constraints::Implicit;
+using constraints::ImplicitPtr_t;
+namespace graph {
+/// \addtogroup constraint_graph
+/// \{
+
+/// State of an end-effector.
+///
+/// States of the graph of constraints. There is one
+/// graph for each end-effector.
+class HPP_MANIPULATION_DLLAPI State : public GraphComponent {
+ public:
+  typedef std::function<EdgePtr_t(const std::string&, const GraphWkPtr_t&,
+                                  const StateWkPtr_t&, const StateWkPtr_t&)>
+      EdgeFactory;
+  /// Destructor
+  ~State();
+
+  /// Create a new state.
+  static StatePtr_t create(const std::string& name);
+
+  /// Create a link from this state to the given state.
+  /// \param w if strictly negative, the edge is not included in the neighbor
+  ///          list. Otherwise, it is included with Weight_t w
+  EdgePtr_t linkTo(const std::string& name, const StatePtr_t& to,
+                   const size_type& w = 1, EdgeFactory create = Edge::create);
+
+  /// Check whether the configuration is in this state.
+  /// \code
+  ///   return configConstraint()->isSatisfied (config);
+  /// \endcode
+  /// \note You should not use this method to know in which states a
+  /// configuration is. This only checks if the configuration satisfies
+  /// the constraints. Instead, use the class StateSelector.
+  virtual bool contains(ConfigurationIn_t config) const;
+
+  inline bool isWaypoint() const { return isWaypoint_; }
+
+  inline void isWaypoint(bool isWaypoint) { isWaypoint_ = isWaypoint; }
+
+  /// Get the parent StateSelector.
+  StateSelectorWkPtr_t stateSelector() const { return selector_; }
+
+  /// Set the StateSelector containing this state.
+  void stateSelector(const StateSelectorWkPtr_t& parent) {
+    selector_ = parent;
+  };
+
+  /// Get the neighbors
+  const Neighbors_t& neighbors() const { return neighbors_; }
+
+  /// Get the neighbors
+  Edges_t neighborEdges() const { return neighbors_.values(); }
+
+  /// Get the hidden neighbors
+  /// It is a vector of transitions outgoing from this state and that are
+  /// included in a waypoint edge.
+  const Edges_t& hiddenNeighbors() const { return hiddenNeighbors_; }
+
+  /// Set weight of edge starting from this state.
+  void updateWeight(const EdgePtr_t& edge, const Weight_t& w);
+
+  /// Get weight of edge starting from this state.
+  Weight_t getWeight(const EdgePtr_t& edge);
+
+  /// Constraint to project onto this state.
+  ConstraintSetPtr_t configConstraint() const {
+    throwIfNotInitialized();
+    return configConstraints_;
+  }
+
+  /// Add constraint to the state
+  /// Call the parent implementation.
+  /// \throw std::logic_error if the constraint is parameterizable
+  /// (contains at least one Equality comparison type).
+  virtual void addNumericalConstraint(const ImplicitPtr_t& numConstraint);
+
+  /// Add a constraint for paths that lie in this state.
+  virtual void addNumericalConstraintForPath(const ImplicitPtr_t& nm) {
+    invalidate();
+    numericalConstraintsForPath_.push_back(nm);
+  }
+
+  /// Insert the numerical constraints in a ConfigProjector
+  /// \return true is at least one ImplicitPtr_t was inserted.
+  bool insertNumericalConstraintsForPath(ConfigProjectorPtr_t& proj) const {
+    for (const auto& nc : numericalConstraintsForPath_) proj->add(nc);
+    return !numericalConstraintsForPath_.empty();
+  }
+
+  /// Get a reference to the NumericalConstraints_t
+  const NumericalConstraints_t& numericalConstraintsForPath() const {
+    return numericalConstraintsForPath_;
+  }
+
+  /// Print the object in a stream.
+  std::ostream& dotPrint(std::ostream& os, dot::DrawingAttributes da =
+                                               dot::DrawingAttributes()) const;
+
+ protected:
+  /// Initialize the object.
+  void init(const StateWkPtr_t& self);
+
+  /// Constructor
+  State(const std::string& name);
+
+  /// Print the object in a stream.
+  std::ostream& print(std::ostream& os) const;
+
+  virtual void populateTooltip(dot::Tooltip& tp) const;
+
+  virtual void initialize();
+
+ private:
+  /// List of possible motions from this state (i.e. the outgoing
+  /// vertices).
+  Neighbors_t neighbors_;
+  Edges_t hiddenNeighbors_;
+
+  /// Set of constraints to be statisfied.
+  ConstraintSetPtr_t configConstraints_;
+
+  /// Stores the numerical constraints for path.
+  NumericalConstraints_t numericalConstraintsForPath_;
+
+  /// A selector that will implement the selection of the next state.
+  StateSelectorWkPtr_t selector_;
+
+  /// Weak pointer to itself.
+  StateWkPtr_t wkPtr_;
+
+  bool isWaypoint_;
+};  // class State
+
+/// \}
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPH_STATE_HH
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index c8b27f45d003a9d50e5feba4c0ed14f86fbe603b..24e62da631ef19f6eb6209a05ecbb128121e54a8 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -26,193 +26,187 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-
 #ifndef HPP_MANIPULATION_GRAPH_STATISTICS_HH
-# define HPP_MANIPULATION_GRAPH_STATISTICS_HH
-
-# include <hpp/util/debug.hh>
+#define HPP_MANIPULATION_GRAPH_STATISTICS_HH
 
-# include <hpp/statistics/bin.hh>
+#include <hpp/manipulation/roadmap-node.hh>
+#include <hpp/statistics/bin.hh>
+#include <hpp/util/debug.hh>
 
-# include "hpp/manipulation/config.hh"
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/graph/graph.hh"
-# include "hpp/manipulation/graph/state.hh"
-# include <hpp/manipulation/roadmap-node.hh>
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/graph/state.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      /// This class is used to do statistics on the roadmap.
-      /// It keeps a track of which leaves of a foliation have been visited.
-      class HPP_MANIPULATION_DLLAPI LeafBin : public ::hpp::statistics::Bin
-      {
-        public :
-          typedef ::hpp::statistics::Bin Parent;
-          typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t;
+namespace manipulation {
+namespace graph {
+/// This class is used to do statistics on the roadmap.
+/// It keeps a track of which leaves of a foliation have been visited.
+class HPP_MANIPULATION_DLLAPI LeafBin : public ::hpp::statistics::Bin {
+ public:
+  typedef ::hpp::statistics::Bin Parent;
+  typedef std::list<RoadmapNodePtr_t> RoadmapNodes_t;
+
+  LeafBin(const vector_t& v, value_type* threshold_);
+
+  void push_back(const RoadmapNodePtr_t& n);
+
+  bool operator<(const LeafBin& rhs) const;
+
+  bool operator==(const LeafBin& rhs) const;
 
-          LeafBin(const vector_t& v, value_type* threshold_);
+  const vector_t& value() const;
 
-          void push_back(const RoadmapNodePtr_t& n);
+  std::ostream& print(std::ostream& os) const;
 
-          bool operator<(const LeafBin& rhs) const;
+  unsigned int numberOfObsOutOfConnectedComponent(
+      const core::ConnectedComponentPtr_t& cc) const;
 
-          bool operator==(const LeafBin& rhs) const;
+  const RoadmapNodes_t& nodes() const;
 
-          const vector_t& value () const;
+ private:
+  vector_t value_;
 
-          std::ostream& print (std::ostream& os) const;
+  RoadmapNodes_t nodes_;
 
-          unsigned int numberOfObsOutOfConnectedComponent (const core::ConnectedComponentPtr_t& cc) const;
+  value_type* thr_;
 
-          const RoadmapNodes_t& nodes () const;
+  std::ostream& printValue(std::ostream& os) const;
+};
 
-        private:
-          vector_t value_;
+/// This class is used to do statistics on the roadmap.
+/// It keeps a track of which nodes of the constraint graph have been visited.
+class HPP_MANIPULATION_DLLLOCAL NodeBin : public ::hpp::statistics::Bin {
+ public:
+  typedef ::hpp::statistics::Bin Parent;
+  NodeBin(const StatePtr_t& n);
 
-          RoadmapNodes_t nodes_;
+  void push_back(const RoadmapNodePtr_t& n);
 
-          value_type* thr_;
+  bool operator<(const NodeBin& rhs) const;
 
-          std::ostream& printValue (std::ostream& os) const;
-      };
+  bool operator==(const NodeBin& rhs) const;
 
-      /// This class is used to do statistics on the roadmap.
-      /// It keeps a track of which nodes of the constraint graph have been visited.
-      class HPP_MANIPULATION_DLLLOCAL NodeBin : public ::hpp::statistics::Bin
-      {
-        public :
-          typedef ::hpp::statistics::Bin Parent;
-          NodeBin(const StatePtr_t& n);
+  const StatePtr_t& state() const;
 
-          void push_back(const RoadmapNodePtr_t& n);
+  std::ostream& print(std::ostream& os) const;
 
-          bool operator<(const NodeBin& rhs) const;
+ private:
+  StatePtr_t state_;
 
-          bool operator==(const NodeBin& rhs) const;
+  typedef std::list<RoadmapNodePtr_t> RoadmapNodes_t;
+  RoadmapNodes_t roadmapNodes_;
 
-          const StatePtr_t& state () const;
+  std::ostream& printValue(std::ostream& os) const;
+};
 
-          std::ostream& print (std::ostream& os) const;
+class HPP_MANIPULATION_DLLLOCAL Histogram {
+ public:
+  virtual ~Histogram(){};
 
-        private:
-          StatePtr_t state_;
+  virtual void add(const RoadmapNodePtr_t& node) = 0;
 
-          typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t;
-          RoadmapNodes_t roadmapNodes_;
+  virtual HistogramPtr_t clone() const = 0;
 
-          std::ostream& printValue (std::ostream& os) const;
-      };
+  virtual void clear() = 0;
+};
 
-      class HPP_MANIPULATION_DLLLOCAL Histogram
-      {
-        public:
-          virtual ~Histogram () {};
+/// This class represents a foliation of a submanifold of the configuration
+/// space.
+/// Such a foliation is defined by the two following functions:
+/// \li a condition $f$ such that the submanifold is
+///                     $\mathcal{M}= \left{q \in \mathbb{C} | f(q)=0 \right}$
+/// \li a parametrizer $g$ such that the leaf of this foliation is a level
+///      set of $g$.
+class HPP_MANIPULATION_DLLAPI Foliation {
+ public:
+  /// Whether the configuration is the submanifold $\mathcal{M}$
+  bool contains(ConfigurationIn_t q) const;
+  /// Whether the configuration is the submanifold $\mathcal{M}$
+  vector_t parameter(ConfigurationIn_t q) const;
 
-          virtual void add (const RoadmapNodePtr_t& node) = 0;
+  void condition(const ConstraintSetPtr_t c);
+  ConstraintSetPtr_t condition() const;
+  void parametrizer(const ConstraintSetPtr_t p);
+  ConstraintSetPtr_t parametrizer() const;
 
-          virtual HistogramPtr_t clone () const = 0;
+ private:
+  // condition_ contains the constraints defining the submanifold
+  // containing all the leaf.
+  // parametrizer_ contains the constraints providing a parametrization
+  // of the foliation.
+  ConstraintSetPtr_t condition_, parametrizer_;
+};
 
-          virtual void clear () = 0;
-      };
+class HPP_MANIPULATION_DLLAPI LeafHistogram
+    : public ::hpp::statistics::Statistics<LeafBin>,
+      public Histogram {
+ public:
+  typedef ::hpp::statistics::Statistics<LeafBin> Parent;
 
-      /// This class represents a foliation of a submanifold of the configuration
-      /// space.
-      /// Such a foliation is defined by the two following functions:
-      /// \li a condition $f$ such that the submanifold is
-      ///                     $\mathcal{M}= \left{q \in \mathbb{C} | f(q)=0 \right}$
-      /// \li a parametrizer $g$ such that the leaf of this foliation is a level
-      ///      set of $g$.
-      class HPP_MANIPULATION_DLLAPI Foliation
-      {
-        public:
-          /// Whether the configuration is the submanifold $\mathcal{M}$
-          bool contains (ConfigurationIn_t q) const;
-          /// Whether the configuration is the submanifold $\mathcal{M}$
-          vector_t parameter (ConfigurationIn_t q) const;
+  static LeafHistogramPtr_t create(const Foliation f);
 
-          void condition (const ConstraintSetPtr_t c);
-          ConstraintSetPtr_t condition () const;
-          void parametrizer (const ConstraintSetPtr_t p);
-          ConstraintSetPtr_t parametrizer () const;
+  /// Insert an occurence of a value in the histogram
+  void add(const RoadmapNodePtr_t& n);
 
-        private:
-          // condition_ contains the constraints defining the submanifold
-          // containing all the leaf.
-          // parametrizer_ contains the constraints providing a parametrization
-          // of the foliation.
-          ConstraintSetPtr_t condition_, parametrizer_;
-      };
+  std::ostream& print(std::ostream& os) const;
 
-      class HPP_MANIPULATION_DLLAPI LeafHistogram : public ::hpp::statistics::Statistics < LeafBin >
-                                                      , public Histogram
-      {
-        public:
-          typedef ::hpp::statistics::Statistics < LeafBin > Parent;
+  virtual HistogramPtr_t clone() const;
 
-          static LeafHistogramPtr_t create (const Foliation f);
+  statistics::DiscreteDistribution<RoadmapNodePtr_t>
+  getDistribOutOfConnectedComponent(
+      const core::ConnectedComponentPtr_t& cc) const;
 
-          /// Insert an occurence of a value in the histogram
-          void add (const RoadmapNodePtr_t& n);
+  statistics::DiscreteDistribution<RoadmapNodePtr_t> getDistrib() const;
 
-          std::ostream& print (std::ostream& os) const;
+  void clear() { Parent::clear(); }
 
-          virtual HistogramPtr_t clone () const;
+  const Foliation& foliation() const { return f_; }
 
-          statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistribOutOfConnectedComponent (
-              const core::ConnectedComponentPtr_t& cc) const;
+ protected:
+  /// Constructor
+  /// \param state defines the submanifold containing the foliation.
+  /// \param constraint The constraint that create the foliation being
+  ///        studied.
+  LeafHistogram(const Foliation f);
 
-          statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistrib () const;
+ private:
+  Foliation f_;
 
-          void clear () { Parent::clear(); }
+  /// Threshold used for equality between offset values.
+  value_type threshold_;
+};
 
-          const Foliation& foliation () const {
-            return f_;
-          }
+class HPP_MANIPULATION_DLLLOCAL StateHistogram
+    : public ::hpp::statistics::Statistics<NodeBin>,
+      public Histogram {
+ public:
+  typedef ::hpp::statistics::Statistics<NodeBin> Parent;
+  /// Constructor
+  /// \param graph The constraint graph used to get the states from
+  ///        a configuration.
+  StateHistogram(const graph::GraphPtr_t& graph);
 
-        protected:
-          /// Constructor
-          /// \param state defines the submanifold containing the foliation.
-          /// \param constraint The constraint that create the foliation being
-          ///        studied.
-          LeafHistogram (const Foliation f);
+  /// Insert an occurence of a value in the histogram
+  void add(const RoadmapNodePtr_t& n);
 
-        private:
-          Foliation f_;
+  std::ostream& print(std::ostream& os) const;
 
-          /// Threshold used for equality between offset values.
-          value_type threshold_;
-      };
+  const graph::GraphPtr_t& constraintGraph() const;
 
-      class HPP_MANIPULATION_DLLLOCAL StateHistogram : public ::hpp::statistics::Statistics < NodeBin >
-                                                      , public Histogram
-      {
-        public:
-          typedef ::hpp::statistics::Statistics < NodeBin > Parent;
-          /// Constructor
-          /// \param graph The constraint graph used to get the states from
-          ///        a configuration.
-          StateHistogram (const graph::GraphPtr_t& graph);
+  virtual HistogramPtr_t clone() const;
 
-          /// Insert an occurence of a value in the histogram
-          void add (const RoadmapNodePtr_t& n);
-
-          std::ostream& print (std::ostream& os) const;
-
-          const graph::GraphPtr_t& constraintGraph () const;
-
-          virtual HistogramPtr_t clone () const;
-
-          void clear () { Parent::clear(); }
-
-        private:
-          /// The constraint graph
-          graph::GraphPtr_t graph_;
-      };
-      typedef StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED;
-      typedef shared_ptr <StateHistogram> NodeHistogramPtr_t;
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPH_STATISTICS_HH
+  void clear() { Parent::clear(); }
+
+ private:
+  /// The constraint graph
+  graph::GraphPtr_t graph_;
+};
+typedef StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED;
+typedef shared_ptr<StateHistogram> NodeHistogramPtr_t;
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPH_STATISTICS_HH
diff --git a/include/hpp/manipulation/graph/validation.hh b/include/hpp/manipulation/graph/validation.hh
index f56ef62c53cb1b053339c413d748e177396e47e0..5cd6d77a0ae921227fa6571f2f950876fb413aba 100644
--- a/include/hpp/manipulation/graph/validation.hh
+++ b/include/hpp/manipulation/graph/validation.hh
@@ -27,111 +27,101 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
-# define HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
+#define HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
 
-# include <string>
-# include <vector>
-# include <hpp/core/validation-report.hh>
-
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/graph/fwd.hh>
-# include <hpp/manipulation/graph/graph.hh>
+#include <hpp/core/validation-report.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <string>
+#include <vector>
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      /// \addtogroup constraint_graph
-      /// \{
-
-      /// Check that graph components are valid.
-      ///
-      /// A stringified validation report can be obtained via
-      /// Validation::print or operator<< (std::ostream&, const Validation&).
-      class HPP_MANIPULATION_DLLAPI Validation
-      {
-        public:
-          typedef std::vector<std::string> Collision;
-          typedef std::vector<Collision> CollisionList;
-          typedef std::map<std::string, CollisionList> CollisionMap;
-          Validation(const core::ProblemPtr_t& problem)
-            : problem_ (problem) {}
-
-          void clear ()
-          {
-            warnings_.clear();
-            errors_.clear();
-          }
-
-          bool hasWarnings () const { return !warnings_.empty(); }
-
-          bool hasErrors () const { return !errors_.empty(); }
-
-          virtual std::ostream& print (std::ostream& os) const;
-
-          /// Validate a graph component.
-          /// It dynamically casts in order to call the right function among
-          /// the validation method below.
-          ///
-          /// \return true if the component could not be proven infeasible.
-          /// \note Even if true is returned, the report can contain warnings.
-          bool validate (const GraphComponentPtr_t& comp);
-
-          /// Validate a state
-          /// \return true if the state could not be proven infeasible.
-          /// \note Even if true is returned, the report can contain warnings.
-          bool validateState (const StatePtr_t& state);
-
-          /// Validate an edge
-          /// \return true if the edge could not be proven infeasible.
-          /// \note Even if true is returned, the report can contain warnings.
-          bool validateEdge  (const EdgePtr_t & edge);
-
-          /// Validate an graph
-          /// \return true if no component of the graph could not be proven infeasible.
-          /// \note Even if true is returned, the report can contain warnings.
-          bool validateGraph (const GraphPtr_t& graph);
-
-          CollisionList getCollisionsForNode (const std::string& nodeName)
-          {
-            return collisions_[nodeName];
-          }
-
-
-        private:
-          void addWarning (const GraphComponentPtr_t& c, const std::string& w)
-          {
-            warnings_.push_back (Message (c, w));
-          }
-
-          void addError (const GraphComponentPtr_t& c, const std::string& w)
-          {
-            errors_.push_back (Message (c, w));
-          }
-
-          void addCollision (const GraphComponentPtr_t& c, const std::string& obj1,
-                  const std::string& obj2)
-          {
-              Collision coll = Collision{obj1, obj2};
-              collisions_[c->name()].push_back(coll);
-          }
-
-          typedef std::pair<GraphComponentPtr_t, std::string> Message;
-          std::vector<Message> warnings_, errors_;
-          CollisionMap collisions_;
-
-          core::ProblemPtr_t problem_;
-      };
-
-      inline std::ostream& operator<< (std::ostream& os, const Validation& v)
-      {
-        return v.print(os);
-      }
-
-      /// \}
-    } // namespace graph
-  } // namespace manipulation
-
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
+namespace manipulation {
+namespace graph {
+/// \addtogroup constraint_graph
+/// \{
+
+/// Check that graph components are valid.
+///
+/// A stringified validation report can be obtained via
+/// Validation::print or operator<< (std::ostream&, const Validation&).
+class HPP_MANIPULATION_DLLAPI Validation {
+ public:
+  typedef std::vector<std::string> Collision;
+  typedef std::vector<Collision> CollisionList;
+  typedef std::map<std::string, CollisionList> CollisionMap;
+  Validation(const core::ProblemPtr_t& problem) : problem_(problem) {}
+
+  void clear() {
+    warnings_.clear();
+    errors_.clear();
+  }
+
+  bool hasWarnings() const { return !warnings_.empty(); }
+
+  bool hasErrors() const { return !errors_.empty(); }
+
+  virtual std::ostream& print(std::ostream& os) const;
+
+  /// Validate a graph component.
+  /// It dynamically casts in order to call the right function among
+  /// the validation method below.
+  ///
+  /// \return true if the component could not be proven infeasible.
+  /// \note Even if true is returned, the report can contain warnings.
+  bool validate(const GraphComponentPtr_t& comp);
+
+  /// Validate a state
+  /// \return true if the state could not be proven infeasible.
+  /// \note Even if true is returned, the report can contain warnings.
+  bool validateState(const StatePtr_t& state);
+
+  /// Validate an edge
+  /// \return true if the edge could not be proven infeasible.
+  /// \note Even if true is returned, the report can contain warnings.
+  bool validateEdge(const EdgePtr_t& edge);
+
+  /// Validate an graph
+  /// \return true if no component of the graph could not be proven infeasible.
+  /// \note Even if true is returned, the report can contain warnings.
+  bool validateGraph(const GraphPtr_t& graph);
+
+  CollisionList getCollisionsForNode(const std::string& nodeName) {
+    return collisions_[nodeName];
+  }
+
+ private:
+  void addWarning(const GraphComponentPtr_t& c, const std::string& w) {
+    warnings_.push_back(Message(c, w));
+  }
+
+  void addError(const GraphComponentPtr_t& c, const std::string& w) {
+    errors_.push_back(Message(c, w));
+  }
+
+  void addCollision(const GraphComponentPtr_t& c, const std::string& obj1,
+                    const std::string& obj2) {
+    Collision coll = Collision{obj1, obj2};
+    collisions_[c->name()].push_back(coll);
+  }
+
+  typedef std::pair<GraphComponentPtr_t, std::string> Message;
+  std::vector<Message> warnings_, errors_;
+  CollisionMap collisions_;
+
+  core::ProblemPtr_t problem_;
+};
+
+inline std::ostream& operator<<(std::ostream& os, const Validation& v) {
+  return v.print(os);
+}
+
+/// \}
+}  // namespace graph
+}  // namespace manipulation
+
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index 2db63fe03364cc119cc62bf28029c9071d7a2621..00a12c37cc1738c7bca0f037103bfc133334f623 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -29,209 +29,182 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_HANDLE_HH
-# define HPP_MANIPULATION_HANDLE_HH
+#define HPP_MANIPULATION_HANDLE_HH
 
-# include <pinocchio/spatial/se3.hpp>
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <pinocchio/spatial/se3.hpp>
 
 namespace hpp {
-  namespace manipulation {
-    typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-    /// Frame attached to an object that is aimed at being grasped
-    ///
-    /// Together with a hpp::pinocchio::Gripper, a handle defines a grasp.
-    /// A vector of 6 Boolean values called a mask can be passed to the
-    /// constructor to define the symmetries of the handle. For example,
-    /// {True,True,True,False,True,True} means that the handle can be
-    /// grasped with free orientation around x-axis.
-    /// See https://hal.laas.fr/hal-02995125v2 for details.
-    /// The setter method \c mask allows users to define the mask.
-    ///
-    /// Along motions where the handle is grasped by a gripper, an additional
-    /// constraint is enforced, called the complement constraint. This latter
-    /// constraint ensures that the object is rigidly fixed to the gripper.
-    ///
-    /// However, for some applications, the complement constraint can be
-    /// customized using setter \c maskComp. Note that calling setter method
-    /// \c mask reinitializes the mask complement.
-    class HPP_MANIPULATION_DLLAPI Handle
-    {
-    public:
-      static std::string className;
-      virtual ~Handle () {};
-
-      /// Create constraint corresponding to a gripper grasping this object
-      /// \param robot the robot that grasps the handle,
-      /// \param grasp object containing the grasp information
-      /// \return the constraint of relative position between the handle and
-      ///         the gripper.
-      static HandlePtr_t create (const std::string& name,
-				 const Transform3f& localPosition,
-				 const DeviceWkPtr_t& robot,
-				 const JointPtr_t& joint)
-      {
-	Handle* ptr = new Handle (name, localPosition, robot, joint);
-	HandlePtr_t shPtr (ptr);
-	ptr->init (shPtr);
-	return shPtr;
-      }
-      /// Return a pointer to the copy of this
-      virtual HandlePtr_t clone () const;
-
-      /// \name Name
-      /// \{
-
-      /// Get name
-      const std::string& name () const
-      {
-	return name_;
-      }
-      /// Set name
-      void name (const std::string& n)
-      {
-	name_ = n;
-      }
-      /// \}
-
-      /// \name Joint
-      /// \{
-
-      /// Get joint to which the handle is linked
-      const JointPtr_t& joint () const
-      {
-	return joint_;
-      }
-      /// Set joint to which the handle is linked
-      void joint (const JointPtr_t& joint)
-      {
-	joint_ = joint;
-      }
-
-      DevicePtr_t robot () const
-      {
-        return robot_.lock();
-      }
-      /// \}
-
-      /// Get local position in joint frame
-      const Transform3f& localPosition () const
-      {
-	return localPosition_;
-      }
-
-      /// Set constraint mask
-      void mask (const std::vector<bool>& mask);
-
-      /// Get constraint mask
-      /// See mask(const std::vector<bool>&)
-      const std::vector<bool>& mask () const
-      { return mask_; }
-
-      /// Set mask of complement constraint
-      void maskComp (const std::vector<bool>& mask);
-
-      /// Get mask of complement constraint
-      const std::vector<bool>& maskComp () const
-      { return maskComp_; }
-
-      /// Create constraint corresponding to a gripper grasping this handle
-      /// \param gripper object containing the gripper information
-      /// \return the constraint of relative transformation between the handle
-      ///         and the gripper.
-      /// The degrees of freedom of the relative transformation that are
-      /// constrained are determined by the mask.
-      /// \sa constraints::Implicit::mask.
-      /// The constraint is not parameterizable (has constant right hand side).
-      virtual ImplicitPtr_t createGrasp
-      (const GripperPtr_t& gripper, std::string name) const;
-
-      /// Create complement constraint of gripper grasping this handle
-      /// \param gripper object containing the gripper information
-      /// \return complement constraint: constraint combined with its complement
-      ///         constitute a full relative transformation constraint.
-      /// The complement constraint is parameterizable (has non constant right
-      /// hand side).
-      virtual ImplicitPtr_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, that
-      ///         that is a full relative transformation constraint.
-      virtual ImplicitPtr_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
-      ///         the gripper.
-      /// \note 6 DOFs of the relative transformation between the handle and the gripper
-      ///       are constrained. The transformation is shifted along x-axis of
-      ///       value shift.
-      virtual ImplicitPtr_t createPreGrasp
-      (const GripperPtr_t& gripper, const value_type& shift, std::string name) const;
-
-      /// Get the clearance
-      ///
-      /// The clearance is a distance, from the center of the gripper and along
-      /// the x-aixs, that "ensures" an object being at that distance is not
-      /// colliding with this gripper.
-      /// It also gives an order of magnitude of the size of the gripper.
-      value_type clearance () const
-      {
-        return clearance_;
-      }
-
-      /// Set the clearance
-      /// \sa clearance()
-      void clearance (const value_type& clearance)
-      {
-        clearance_ = clearance;
-      }
-
-    protected:
-      /// Constructor
-      /// \param robot the robot that grasps the handle,
-      /// \param grasp object containing the grasp information
-      /// \return the constraint of relative position between the handle and
-      ///         the gripper.
-      Handle (const std::string& name, const Transform3f& localPosition,
-              const DeviceWkPtr_t& robot, const JointPtr_t& joint) :
-        name_ (name), localPosition_ (localPosition), joint_ (joint),
-        robot_ (robot), clearance_ (0), mask_ (6, true), weakPtr_ ()
-      {
-      }
-      void init (HandleWkPtr_t weakPtr)
-      {
-	weakPtr_ = weakPtr;
-      }
-
-      virtual std::ostream& print (std::ostream& os) const;
-
-    private:
-      std::string name_;
-      /// Position of the handle in the joint frame.
-      Transform3f localPosition_;
-      /// Joint to which the handle is linked.
-      JointPtr_t joint_;
-      /// Pointer to the robot
-      DeviceWkPtr_t robot_;
-      /// Clearance
-      value_type clearance_;
-      /// Mask
-      std::vector<bool> mask_;
-      /// Mask of complement constraint
-      std::vector<bool> maskComp_;
-      /// Weak pointer to itself
-      HandleWkPtr_t weakPtr_;
-
-      friend std::ostream& operator<< (std::ostream&, const Handle&);
-    }; // class Handle
-
-    std::ostream& operator<< (std::ostream& os, const Handle& handle);
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_HANDLE_HH
+namespace manipulation {
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+/// Frame attached to an object that is aimed at being grasped
+///
+/// Together with a hpp::pinocchio::Gripper, a handle defines a grasp.
+/// A vector of 6 Boolean values called a mask can be passed to the
+/// constructor to define the symmetries of the handle. For example,
+/// {True,True,True,False,True,True} means that the handle can be
+/// grasped with free orientation around x-axis.
+/// See https://hal.laas.fr/hal-02995125v2 for details.
+/// The setter method \c mask allows users to define the mask.
+///
+/// Along motions where the handle is grasped by a gripper, an additional
+/// constraint is enforced, called the complement constraint. This latter
+/// constraint ensures that the object is rigidly fixed to the gripper.
+///
+/// However, for some applications, the complement constraint can be
+/// customized using setter \c maskComp. Note that calling setter method
+/// \c mask reinitializes the mask complement.
+class HPP_MANIPULATION_DLLAPI Handle {
+ public:
+  static std::string className;
+  virtual ~Handle(){};
+
+  /// Create constraint corresponding to a gripper grasping this object
+  /// \param robot the robot that grasps the handle,
+  /// \param grasp object containing the grasp information
+  /// \return the constraint of relative position between the handle and
+  ///         the gripper.
+  static HandlePtr_t create(const std::string& name,
+                            const Transform3f& localPosition,
+                            const DeviceWkPtr_t& robot,
+                            const JointPtr_t& joint) {
+    Handle* ptr = new Handle(name, localPosition, robot, joint);
+    HandlePtr_t shPtr(ptr);
+    ptr->init(shPtr);
+    return shPtr;
+  }
+  /// Return a pointer to the copy of this
+  virtual HandlePtr_t clone() const;
+
+  /// \name Name
+  /// \{
+
+  /// Get name
+  const std::string& name() const { return name_; }
+  /// Set name
+  void name(const std::string& n) { name_ = n; }
+  /// \}
+
+  /// \name Joint
+  /// \{
+
+  /// Get joint to which the handle is linked
+  const JointPtr_t& joint() const { return joint_; }
+  /// Set joint to which the handle is linked
+  void joint(const JointPtr_t& joint) { joint_ = joint; }
+
+  DevicePtr_t robot() const { return robot_.lock(); }
+  /// \}
+
+  /// Get local position in joint frame
+  const Transform3f& localPosition() const { return localPosition_; }
+
+  /// Set constraint mask
+  void mask(const std::vector<bool>& mask);
+
+  /// Get constraint mask
+  /// See mask(const std::vector<bool>&)
+  const std::vector<bool>& mask() const { return mask_; }
+
+  /// Set mask of complement constraint
+  void maskComp(const std::vector<bool>& mask);
+
+  /// Get mask of complement constraint
+  const std::vector<bool>& maskComp() const { return maskComp_; }
+
+  /// Create constraint corresponding to a gripper grasping this handle
+  /// \param gripper object containing the gripper information
+  /// \return the constraint of relative transformation between the handle
+  ///         and the gripper.
+  /// The degrees of freedom of the relative transformation that are
+  /// constrained are determined by the mask.
+  /// \sa constraints::Implicit::mask.
+  /// The constraint is not parameterizable (has constant right hand side).
+  virtual ImplicitPtr_t createGrasp(const GripperPtr_t& gripper,
+                                    std::string name) const;
+
+  /// Create complement constraint of gripper grasping this handle
+  /// \param gripper object containing the gripper information
+  /// \return complement constraint: constraint combined with its complement
+  ///         constitute a full relative transformation constraint.
+  /// The complement constraint is parameterizable (has non constant right
+  /// hand side).
+  virtual ImplicitPtr_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, that
+  ///         that is a full relative transformation constraint.
+  virtual ImplicitPtr_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
+  ///         the gripper.
+  /// \note 6 DOFs of the relative transformation between the handle and the
+  /// gripper
+  ///       are constrained. The transformation is shifted along x-axis of
+  ///       value shift.
+  virtual ImplicitPtr_t createPreGrasp(const GripperPtr_t& gripper,
+                                       const value_type& shift,
+                                       std::string name) const;
+
+  /// Get the clearance
+  ///
+  /// The clearance is a distance, from the center of the gripper and along
+  /// the x-aixs, that "ensures" an object being at that distance is not
+  /// colliding with this gripper.
+  /// It also gives an order of magnitude of the size of the gripper.
+  value_type clearance() const { return clearance_; }
+
+  /// Set the clearance
+  /// \sa clearance()
+  void clearance(const value_type& clearance) { clearance_ = clearance; }
+
+ protected:
+  /// Constructor
+  /// \param robot the robot that grasps the handle,
+  /// \param grasp object containing the grasp information
+  /// \return the constraint of relative position between the handle and
+  ///         the gripper.
+  Handle(const std::string& name, const Transform3f& localPosition,
+         const DeviceWkPtr_t& robot, const JointPtr_t& joint)
+      : name_(name),
+        localPosition_(localPosition),
+        joint_(joint),
+        robot_(robot),
+        clearance_(0),
+        mask_(6, true),
+        weakPtr_() {}
+  void init(HandleWkPtr_t weakPtr) { weakPtr_ = weakPtr; }
+
+  virtual std::ostream& print(std::ostream& os) const;
+
+ private:
+  std::string name_;
+  /// Position of the handle in the joint frame.
+  Transform3f localPosition_;
+  /// Joint to which the handle is linked.
+  JointPtr_t joint_;
+  /// Pointer to the robot
+  DeviceWkPtr_t robot_;
+  /// Clearance
+  value_type clearance_;
+  /// Mask
+  std::vector<bool> mask_;
+  /// Mask of complement constraint
+  std::vector<bool> maskComp_;
+  /// Weak pointer to itself
+  HandleWkPtr_t weakPtr_;
+
+  friend std::ostream& operator<<(std::ostream&, const Handle&);
+};  // class Handle
+
+std::ostream& operator<<(std::ostream& os, const Handle& handle);
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_HANDLE_HH
diff --git a/include/hpp/manipulation/leaf-connected-comp.hh b/include/hpp/manipulation/leaf-connected-comp.hh
index 4ba08c30c62fa3f7d5873dbf9bea39f801fd0b2f..8bc55c212292298e6e16debe979c62379bc329ff 100644
--- a/include/hpp/manipulation/leaf-connected-comp.hh
+++ b/include/hpp/manipulation/leaf-connected-comp.hh
@@ -33,128 +33,108 @@
 #include <hpp/manipulation/config.hh>
 #include <hpp/manipulation/fwd.hh>
 #include <hpp/manipulation/graph/fwd.hh>
-
 #include <hpp/manipulation/roadmap-node.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// Set of configurations accessible to each others by a single transition,
-    /// with the same right hand side.
-    ///
-    /// This assumes the roadmap is not directed.
-    class HPP_MANIPULATION_DLLAPI LeafConnectedComp
-    {
-      public:
-        typedef LeafConnectedComp* RawPtr_t;
-        typedef std::set <RawPtr_t> LeafConnectedComps_t;
-        /// return a shared pointer to new instance
-        static LeafConnectedCompPtr_t create (const RoadmapPtr_t& roadmap);
-
-        /// Merge two connected components
-        /// \param other manipulation symbolic component to merge into this one.
-        /// \note other will be empty after calling this method.
-        virtual void merge (const LeafConnectedCompPtr_t& otherCC);
-
-        /// Whether this connected component can reach cc
-        /// \param cc a connected component
-        bool canReach (const LeafConnectedCompPtr_t& cc);
-
-        /// Whether this connected component can reach cc
-        /// \param cc a connected component
-        /// \retval cc2Tocc1 list of connected components between cc2 and cc1
-        ///         that should be merged.
-        bool canReach (const LeafConnectedCompPtr_t& cc,
-                       LeafConnectedComp::LeafConnectedComps_t& cc2Tocc1);
-
-        /// Add roadmap node to connected component
-        /// \param roadmap node to be added
-        void addNode (const RoadmapNodePtr_t& node);
-
-        virtual void setFirstNode (const RoadmapNodePtr_t& node);
-
-        core::ConnectedComponentPtr_t connectedComponent () const
-        {
-          assert (!nodes_.empty());
-          return nodes_.front()->connectedComponent();
-        };
-
-        const RoadmapNodes_t& nodes() const
-        {
-          return nodes_;
-        }
-
-        LeafConnectedCompPtr_t self ()
-        {
-          return weak_.lock ();
-        }
-
-        const LeafConnectedComp::LeafConnectedComps_t& from () const
-        {
-          return from_;
-        }
-
-        const LeafConnectedComp::LeafConnectedComps_t& to () const
-        {
-          return to_;
-        }
-    protected:
-        LeafConnectedComp(const RoadmapPtr_t& r)
-          : roadmap_(r) {}
-
-        void init (const LeafConnectedCompWkPtr_t& shPtr)
-        {
-          weak_ = shPtr;
-        }
-
-        graph::StatePtr_t state_;
-        RoadmapNodes_t nodes_;
-
-        /// For serialization only.
-        LeafConnectedComp() {}
-
-      private:
-        static void clean (LeafConnectedComps_t& set);
-        // status variable to indicate whether or not CC has been visited
-        mutable bool explored_;
-        RoadmapWkPtr_t roadmap_;
-        LeafConnectedComps_t to_, from_;
-        LeafConnectedCompWkPtr_t weak_;
-        friend class Roadmap;
-
-        HPP_SERIALIZABLE();
-    }; // class LeafConnectedComp
-
-    class HPP_MANIPULATION_DLLAPI WeighedLeafConnectedComp :
-      public LeafConnectedComp
-    {
-      public:
-        void merge (const LeafConnectedCompPtr_t& otherCC);
-
-        void setFirstNode (const RoadmapNodePtr_t& node);
-
-        static WeighedLeafConnectedCompPtr_t create (const RoadmapPtr_t& roadmap);
-
-        std::size_t indexOf (const graph::EdgePtr_t e) const;
-
-        void normalizeProba ()
-        {
-          const value_type s = p_.sum();
-          p_ /= s;
-        }
-
-        value_type weight_;
-        /// Transition probabilities
-        vector_t p_;
-        std::vector<graph::EdgePtr_t> edges_;
-
-      protected:
-        WeighedLeafConnectedComp(const RoadmapPtr_t& r)
-          : LeafConnectedComp(r), weight_(1) {}
-
-      private:
-        WeighedLeafConnectedComp() {}
-        HPP_SERIALIZABLE();
-    }; // class LeafConnectedComp
-  } //   namespace manipulation
-} // namespace hpp
-#endif // HPP_MANIPULATION_LEAF_CONNECTED_COMP_HH
+namespace manipulation {
+/// Set of configurations accessible to each others by a single transition,
+/// with the same right hand side.
+///
+/// This assumes the roadmap is not directed.
+class HPP_MANIPULATION_DLLAPI LeafConnectedComp {
+ public:
+  typedef LeafConnectedComp* RawPtr_t;
+  typedef std::set<RawPtr_t> LeafConnectedComps_t;
+  /// return a shared pointer to new instance
+  static LeafConnectedCompPtr_t create(const RoadmapPtr_t& roadmap);
+
+  /// Merge two connected components
+  /// \param other manipulation symbolic component to merge into this one.
+  /// \note other will be empty after calling this method.
+  virtual void merge(const LeafConnectedCompPtr_t& otherCC);
+
+  /// Whether this connected component can reach cc
+  /// \param cc a connected component
+  bool canReach(const LeafConnectedCompPtr_t& cc);
+
+  /// Whether this connected component can reach cc
+  /// \param cc a connected component
+  /// \retval cc2Tocc1 list of connected components between cc2 and cc1
+  ///         that should be merged.
+  bool canReach(const LeafConnectedCompPtr_t& cc,
+                LeafConnectedComp::LeafConnectedComps_t& cc2Tocc1);
+
+  /// Add roadmap node to connected component
+  /// \param roadmap node to be added
+  void addNode(const RoadmapNodePtr_t& node);
+
+  virtual void setFirstNode(const RoadmapNodePtr_t& node);
+
+  core::ConnectedComponentPtr_t connectedComponent() const {
+    assert(!nodes_.empty());
+    return nodes_.front()->connectedComponent();
+  };
+
+  const RoadmapNodes_t& nodes() const { return nodes_; }
+
+  LeafConnectedCompPtr_t self() { return weak_.lock(); }
+
+  const LeafConnectedComp::LeafConnectedComps_t& from() const { return from_; }
+
+  const LeafConnectedComp::LeafConnectedComps_t& to() const { return to_; }
+
+ protected:
+  LeafConnectedComp(const RoadmapPtr_t& r) : roadmap_(r) {}
+
+  void init(const LeafConnectedCompWkPtr_t& shPtr) { weak_ = shPtr; }
+
+  graph::StatePtr_t state_;
+  RoadmapNodes_t nodes_;
+
+  /// For serialization only.
+  LeafConnectedComp() {}
+
+ private:
+  static void clean(LeafConnectedComps_t& set);
+  // status variable to indicate whether or not CC has been visited
+  mutable bool explored_;
+  RoadmapWkPtr_t roadmap_;
+  LeafConnectedComps_t to_, from_;
+  LeafConnectedCompWkPtr_t weak_;
+  friend class Roadmap;
+
+  HPP_SERIALIZABLE();
+};  // class LeafConnectedComp
+
+class HPP_MANIPULATION_DLLAPI WeighedLeafConnectedComp
+    : public LeafConnectedComp {
+ public:
+  void merge(const LeafConnectedCompPtr_t& otherCC);
+
+  void setFirstNode(const RoadmapNodePtr_t& node);
+
+  static WeighedLeafConnectedCompPtr_t create(const RoadmapPtr_t& roadmap);
+
+  std::size_t indexOf(const graph::EdgePtr_t e) const;
+
+  void normalizeProba() {
+    const value_type s = p_.sum();
+    p_ /= s;
+  }
+
+  value_type weight_;
+  /// Transition probabilities
+  vector_t p_;
+  std::vector<graph::EdgePtr_t> edges_;
+
+ protected:
+  WeighedLeafConnectedComp(const RoadmapPtr_t& r)
+      : LeafConnectedComp(r), weight_(1) {}
+
+ private:
+  WeighedLeafConnectedComp() {}
+  HPP_SERIALIZABLE();
+};  // class LeafConnectedComp
+}  //   namespace manipulation
+}  // namespace hpp
+#endif  // HPP_MANIPULATION_LEAF_CONNECTED_COMP_HH
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index 3ec17f42bd10639971275a894aada80d99d103f3..ef5937f5c9b97795f8672e387691afc4a310528a 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -27,114 +27,110 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_MANIPULATION_PLANNER_HH
-# define HPP_MANIPULATION_MANIPULATION_PLANNER_HH
+#define HPP_MANIPULATION_MANIPULATION_PLANNER_HH
 
 #include <hpp/core/path-planner.hh>
-
 #include <hpp/statistics/success-bin.hh>
 
-#include "hpp/manipulation/graph/statistics.hh"
-
 #include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/fwd.hh"
 #include "hpp/manipulation/graph/fwd.hh"
 #include "hpp/manipulation/graph/graph.hh"
-#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/statistics.hh"
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup path_planning
-    /// \{
-
-    class HPP_MANIPULATION_DLLAPI ManipulationPlanner :
-      public hpp::core::PathPlanner
-    {
-      public:
-        typedef std::list<std::size_t> ErrorFreqs_t;
-
-        /// Create an instance and return a shared pointer to the instance
-        static ManipulationPlannerPtr_t create
-	  (const core::ProblemConstPtr_t& problem,
-            const core::RoadmapPtr_t& roadmap);
-
-        /// One step of extension.
-        ///
-        /// A set of constraints is chosen using the graph of constraints.
-        /// A constraint extension is done using a chosen set.
-        ///
-        virtual void oneStep ();
-
-        /// Extend configuration q_near toward q_rand.
-        /// \param q_near the configuration to be extended.
-        /// \param q_rand the configuration toward extension is performed.
-        /// \retval validPath the longest valid path (possibly of length 0),
-        ///         resulting from the extension.
-        /// \return True if the returned path is valid.
-        bool extend (RoadmapNodePtr_t q_near,
-            const ConfigurationPtr_t &q_rand, core::PathPtr_t& validPath);
-
-        /// Get the number of occurrence of each errors.
-        ///
-        /// \sa ManipulationPlanner::errorList
-        ErrorFreqs_t getEdgeStat (const graph::EdgePtr_t& edge) const;
-
-        /// Get the list of possible outputs of the extension step.
-        ///
-        /// \sa ManipulationPlanner::getEdgeStat
-        static StringList_t errorList ();
-
-      protected:
-        /// Protected constructor
-        ManipulationPlanner (const ProblemConstPtr_t& problem,
-            const RoadmapPtr_t& roadmap);
-
-        /// Store weak pointer to itself
-        void init (const ManipulationPlannerWkPtr_t& weak);
-
-      private:
-        /// Try to connect nodes of the roadmap to other connected components.
-        /// \return the number of connection made.
-        std::size_t tryConnectToRoadmap (const core::Nodes_t nodes);
-        /// Try to connect nodes in a list between themselves.
-        /// \return the number of connection made.
-        std::size_t tryConnectNewNodes (const core::Nodes_t nodes);
-
-        /// Configuration shooter
-        ConfigurationShooterPtr_t shooter_;
-        /// Pointer to the problem
-        ProblemConstPtr_t problem_;
-        /// Pointer to the roadmap
-        RoadmapPtr_t roadmap_;
-        /// weak pointer to itself
-        ManipulationPlannerWkPtr_t weakPtr_;
-
-        /// Keep track of success and failure for method
-        /// extend.
-        typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
-        typedef ::hpp::statistics::SuccessBin SuccessBin;
-        typedef ::hpp::statistics::SuccessBin::Reason Reason;
-        SuccessStatistics& edgeStat (const graph::EdgePtr_t& edge);
-        std::vector<size_type> indexPerEdgeStatistics_;
-        std::vector<SuccessStatistics> perEdgeStatistics_;
-
-        /// A Reason is associated to each EdgePtr_t that generated a failure.
-        enum TypeOfFailure {
-          PATH_PROJECTION_SHORTER = 0,
-          PATH_VALIDATION_SHORTER = 1,
-	  REACHED_DESTINATION_NODE = 2,
-	  FAILURE = 3,
-          PROJECTION = 4,
-          STEERING_METHOD = 5,
-          PATH_VALIDATION_ZERO = 6,
-          PATH_PROJECTION_ZERO = 7
-        };
-        static const std::vector<Reason> reasons_;
-
-        value_type extendStep_;
-
-        mutable Configuration_t qProj_;
-    };
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_MANIPULATION_PLANNER_HH
+namespace manipulation {
+/// \addtogroup path_planning
+/// \{
+
+class HPP_MANIPULATION_DLLAPI ManipulationPlanner
+    : public hpp::core::PathPlanner {
+ public:
+  typedef std::list<std::size_t> ErrorFreqs_t;
+
+  /// Create an instance and return a shared pointer to the instance
+  static ManipulationPlannerPtr_t create(const core::ProblemConstPtr_t& problem,
+                                         const core::RoadmapPtr_t& roadmap);
+
+  /// One step of extension.
+  ///
+  /// A set of constraints is chosen using the graph of constraints.
+  /// A constraint extension is done using a chosen set.
+  ///
+  virtual void oneStep();
+
+  /// Extend configuration q_near toward q_rand.
+  /// \param q_near the configuration to be extended.
+  /// \param q_rand the configuration toward extension is performed.
+  /// \retval validPath the longest valid path (possibly of length 0),
+  ///         resulting from the extension.
+  /// \return True if the returned path is valid.
+  bool extend(RoadmapNodePtr_t q_near, const ConfigurationPtr_t& q_rand,
+              core::PathPtr_t& validPath);
+
+  /// Get the number of occurrence of each errors.
+  ///
+  /// \sa ManipulationPlanner::errorList
+  ErrorFreqs_t getEdgeStat(const graph::EdgePtr_t& edge) const;
+
+  /// Get the list of possible outputs of the extension step.
+  ///
+  /// \sa ManipulationPlanner::getEdgeStat
+  static StringList_t errorList();
+
+ protected:
+  /// Protected constructor
+  ManipulationPlanner(const ProblemConstPtr_t& problem,
+                      const RoadmapPtr_t& roadmap);
+
+  /// Store weak pointer to itself
+  void init(const ManipulationPlannerWkPtr_t& weak);
+
+ private:
+  /// Try to connect nodes of the roadmap to other connected components.
+  /// \return the number of connection made.
+  std::size_t tryConnectToRoadmap(const core::Nodes_t nodes);
+  /// Try to connect nodes in a list between themselves.
+  /// \return the number of connection made.
+  std::size_t tryConnectNewNodes(const core::Nodes_t nodes);
+
+  /// Configuration shooter
+  ConfigurationShooterPtr_t shooter_;
+  /// Pointer to the problem
+  ProblemConstPtr_t problem_;
+  /// Pointer to the roadmap
+  RoadmapPtr_t roadmap_;
+  /// weak pointer to itself
+  ManipulationPlannerWkPtr_t weakPtr_;
+
+  /// Keep track of success and failure for method
+  /// extend.
+  typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
+  typedef ::hpp::statistics::SuccessBin SuccessBin;
+  typedef ::hpp::statistics::SuccessBin::Reason Reason;
+  SuccessStatistics& edgeStat(const graph::EdgePtr_t& edge);
+  std::vector<size_type> indexPerEdgeStatistics_;
+  std::vector<SuccessStatistics> perEdgeStatistics_;
+
+  /// A Reason is associated to each EdgePtr_t that generated a failure.
+  enum TypeOfFailure {
+    PATH_PROJECTION_SHORTER = 0,
+    PATH_VALIDATION_SHORTER = 1,
+    REACHED_DESTINATION_NODE = 2,
+    FAILURE = 3,
+    PROJECTION = 4,
+    STEERING_METHOD = 5,
+    PATH_VALIDATION_ZERO = 6,
+    PATH_PROJECTION_ZERO = 7
+  };
+  static const std::vector<Reason> reasons_;
+
+  value_type extendStep_;
+
+  mutable Configuration_t qProj_;
+};
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_MANIPULATION_PLANNER_HH
diff --git a/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
index ec0a6f3d44bc01bf7a1a0c0bccc018fc880966d4..2991e528aa7e8a09fafa28679dc4bbae238305b9 100644
--- a/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
+++ b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
@@ -29,45 +29,44 @@
 #ifndef HPP_MANIPULATION_PATHOPTIMIZATION_ENFORCE_TRANSITION_SEMANTIC_HH
 #define HPP_MANIPULATION_PATHOPTIMIZATION_ENFORCE_TRANSITION_SEMANTIC_HH
 
-# include <hpp/core/path-optimizer.hh>
-
-# include <hpp/manipulation/problem.hh>
+#include <hpp/core/path-optimizer.hh>
+#include <hpp/manipulation/problem.hh>
 
 namespace hpp {
-  namespace manipulation {
-    namespace pathOptimization {
-      using hpp::core::Path;
-      using hpp::core::PathPtr_t;
-      using hpp::core::PathVector;
-      /// \addtogroup path_optimization
-      /// \{
+namespace manipulation {
+namespace pathOptimization {
+using hpp::core::Path;
+using hpp::core::PathPtr_t;
+using hpp::core::PathVector;
+/// \addtogroup path_optimization
+/// \{
 
-      class HPP_MANIPULATION_DLLAPI EnforceTransitionSemantic : public core::PathOptimizer
-      {
-        public:
-          typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
-          typedef shared_ptr<EnforceTransitionSemantic> Ptr_t;
+class HPP_MANIPULATION_DLLAPI EnforceTransitionSemantic
+    : public core::PathOptimizer {
+ public:
+  typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
+  typedef shared_ptr<EnforceTransitionSemantic> Ptr_t;
 
-          static Ptr_t create (const core::ProblemConstPtr_t& problem) {
-            ProblemConstPtr_t p (HPP_DYNAMIC_PTR_CAST(const Problem, problem));
-            if (!p) throw std::invalid_argument("This is not a manipulation problem.");
-            return Ptr_t (new EnforceTransitionSemantic (p));
-          }
+  static Ptr_t create(const core::ProblemConstPtr_t& problem) {
+    ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+    if (!p) throw std::invalid_argument("This is not a manipulation problem.");
+    return Ptr_t(new EnforceTransitionSemantic(p));
+  }
 
-          virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
+  virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
 
-        protected:
-          /// Constructor
-          EnforceTransitionSemantic (const ProblemConstPtr_t& problem) :
-            PathOptimizer (problem), problem_ (problem) {}
+ protected:
+  /// Constructor
+  EnforceTransitionSemantic(const ProblemConstPtr_t& problem)
+      : PathOptimizer(problem), problem_(problem) {}
 
-        private:
-          ProblemConstPtr_t problem_;
-      };
+ private:
+  ProblemConstPtr_t problem_;
+};
 
-      /// \}
-    } // namespace pathOptimization
-  } // namespace manipulation
-} // namespace hpp
+/// \}
+}  // namespace pathOptimization
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_PATHOPTIMIZATION_ENFORCE_TRANSITION_SEMANTIC_HH
+#endif  // HPP_MANIPULATION_PATHOPTIMIZATION_ENFORCE_TRANSITION_SEMANTIC_HH
diff --git a/include/hpp/manipulation/path-optimization/random-shortcut.hh b/include/hpp/manipulation/path-optimization/random-shortcut.hh
index b74f6bf4f1c7c22f2f00ff526cb1b7d1a484a14b..c8a0031070daf10d67f8fada415810f767d511f4 100644
--- a/include/hpp/manipulation/path-optimization/random-shortcut.hh
+++ b/include/hpp/manipulation/path-optimization/random-shortcut.hh
@@ -27,48 +27,41 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
-# define HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
+#define HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
 
 #include <hpp/core/path-optimization/random-shortcut.hh>
-
-#include <hpp/manipulation/fwd.hh>
 #include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup path_optimization
-    /// \{
-    namespace pathOptimization {
-      HPP_PREDEF_CLASS (RandomShortcut);
-      typedef shared_ptr<RandomShortcut> RandomShortcutPtr_t;
+namespace manipulation {
+/// \addtogroup path_optimization
+/// \{
+namespace pathOptimization {
+HPP_PREDEF_CLASS(RandomShortcut);
+typedef shared_ptr<RandomShortcut> RandomShortcutPtr_t;
 
-      class HPP_MANIPULATION_DLLAPI RandomShortcut :
-        public core::pathOptimization::RandomShortcut
-      {
-        public:
-          /// Return shared pointer to new object.
-          static RandomShortcutPtr_t create
-	    (const core::ProblemConstPtr_t problem)
-          {
-            return RandomShortcutPtr_t (new RandomShortcut (problem));
-          }
+class HPP_MANIPULATION_DLLAPI RandomShortcut
+    : public core::pathOptimization::RandomShortcut {
+ public:
+  /// Return shared pointer to new object.
+  static RandomShortcutPtr_t create(const core::ProblemConstPtr_t problem) {
+    return RandomShortcutPtr_t(new RandomShortcut(problem));
+  }
 
-        protected:
-          RandomShortcut (const core::ProblemConstPtr_t& problem)
-            : core::pathOptimization::RandomShortcut (problem)
-          {}
+ protected:
+  RandomShortcut(const core::ProblemConstPtr_t& problem)
+      : core::pathOptimization::RandomShortcut(problem) {}
 
-          /// Sample times along currentOpt.
-          /// t1 and t2 will not be on a path whose transition was short.
-          virtual bool shootTimes (const core::PathVectorPtr_t& currentOpt,
-              const value_type& t0,
-              value_type& t1,
-              value_type& t2,
-              const value_type& t3);
-      }; // class RandomShortcut
-    /// \}
-    } // namespace pathOptimization
-  }  // namespace manipulation
-} // namespace hpp
+  /// Sample times along currentOpt.
+  /// t1 and t2 will not be on a path whose transition was short.
+  virtual bool shootTimes(const core::PathVectorPtr_t& currentOpt,
+                          const value_type& t0, value_type& t1, value_type& t2,
+                          const value_type& t3);
+};  // class RandomShortcut
+/// \}
+}  // namespace pathOptimization
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
+#endif  // HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
diff --git a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
index 68231d5238e323773c6aae2fcf7139255c4ef851..f52d6d71916e1e83db127029a3cd1906c36717b8 100644
--- a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
+++ b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
@@ -27,75 +27,78 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_HH
-# define HPP_MANIPULATION_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_HH
+#define HPP_MANIPULATION_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_HH
 
 #include <hpp/core/path-optimization/spline-gradient-based.hh>
-
 #include <hpp/manipulation/config.hh>
 #include <hpp/manipulation/problem.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup path_optimization
-    /// \{
-    namespace pathOptimization {
-      template <int _PolynomeBasis, int _SplineOrder>
-      class HPP_MANIPULATION_DLLAPI SplineGradientBased :
-        public core::pathOptimization::SplineGradientBased<_PolynomeBasis, _SplineOrder>
-      {
-        public:
-          enum {
-            PolynomeBasis = _PolynomeBasis,
-            SplineOrder = _SplineOrder
-          };
-          typedef core::pathOptimization::SplineGradientBased<PolynomeBasis, SplineOrder>
-            Parent_t;
-          typedef shared_ptr<SplineGradientBased> Ptr_t;
+namespace manipulation {
+/// \addtogroup path_optimization
+/// \{
+namespace pathOptimization {
+template <int _PolynomeBasis, int _SplineOrder>
+class HPP_MANIPULATION_DLLAPI SplineGradientBased
+    : public core::pathOptimization::SplineGradientBased<_PolynomeBasis,
+                                                         _SplineOrder> {
+ public:
+  enum { PolynomeBasis = _PolynomeBasis, SplineOrder = _SplineOrder };
+  typedef core::pathOptimization::SplineGradientBased<PolynomeBasis,
+                                                      SplineOrder>
+      Parent_t;
+  typedef shared_ptr<SplineGradientBased> Ptr_t;
 
-          using typename Parent_t::Spline;
-          using typename Parent_t::SplinePtr_t;
-          using typename Parent_t::Splines_t;
+  using typename Parent_t::Spline;
+  using typename Parent_t::SplinePtr_t;
+  using typename Parent_t::Splines_t;
 
-          /// Return shared pointer to new object.
-          static Ptr_t create (const ProblemConstPtr_t& problem);
+  /// Return shared pointer to new object.
+  static Ptr_t create(const ProblemConstPtr_t& problem);
 
-          /// This is only for compatibility purpose (with ProblemSolver).
-          /// problem is statically casted to an object of type
-          /// const manipulation::Problem& and method create(const Problem&)
-          /// is called.
-          static Ptr_t createFromCore (const core::ProblemConstPtr_t& problem);
+  /// This is only for compatibility purpose (with ProblemSolver).
+  /// problem is statically casted to an object of type
+  /// const manipulation::Problem& and method create(const Problem&)
+  /// is called.
+  static Ptr_t createFromCore(const core::ProblemConstPtr_t& problem);
 
-        protected:
-          typedef typename hpp::core::pathOptimization::LinearConstraint LinearConstraint;
-          using typename Parent_t::SplineOptimizationDatas_t;
+ protected:
+  typedef
+      typename hpp::core::pathOptimization::LinearConstraint LinearConstraint;
+  using typename Parent_t::SplineOptimizationDatas_t;
 
-          SplineGradientBased(const ProblemConstPtr_t& problem);
+  SplineGradientBased(const ProblemConstPtr_t& problem);
 
-          /// Get path validation for each spline
-          ///
-          /// \param splines, vector of splines
-          ///
-          /// for each spline in the input vector, retrieve the path validation
-          /// method of the transition the spline comes from.
-          /// If no edge is found, use path validation in problem.
-          ///
-          /// \note path validation methods are stored in member
-          /// hpp::core::pathOptimization::SplineGradientBasedAbstract::validations_
-          virtual void initializePathValidation(const Splines_t& splines);
+  /// Get path validation for each spline
+  ///
+  /// \param splines, vector of splines
+  ///
+  /// for each spline in the input vector, retrieve the path validation
+  /// method of the transition the spline comes from.
+  /// If no edge is found, use path validation in problem.
+  ///
+  /// \note path validation methods are stored in member
+  /// hpp::core::pathOptimization::SplineGradientBasedAbstract::validations_
+  virtual void initializePathValidation(const Splines_t& splines);
 
-          virtual void addProblemConstraints (const core::PathVectorPtr_t& init,
-              const Splines_t& splines, LinearConstraint& lc, SplineOptimizationDatas_t& sods) const;
+  virtual void addProblemConstraints(const core::PathVectorPtr_t& init,
+                                     const Splines_t& splines,
+                                     LinearConstraint& lc,
+                                     SplineOptimizationDatas_t& sods) const;
 
-          virtual void constrainEndIntoState (const core::PathPtr_t& path,
-              const size_type& idxSpline, const SplinePtr_t& spline,
-              const graph::StatePtr_t state, LinearConstraint& lc) const;
+  virtual void constrainEndIntoState(const core::PathPtr_t& path,
+                                     const size_type& idxSpline,
+                                     const SplinePtr_t& spline,
+                                     const graph::StatePtr_t state,
+                                     LinearConstraint& lc) const;
 
-          virtual void constraintDerivativesAtEndOfSpline (const size_type& idxSpline,
-              const SplinePtr_t& spline, LinearConstraint& lc) const;
-      }; // SplineGradientBased
-    } // namespace pathOptimization
-    /// \}
-  }  // namespace manipulation
-} // namespace hpp
+  virtual void constraintDerivativesAtEndOfSpline(const size_type& idxSpline,
+                                                  const SplinePtr_t& spline,
+                                                  LinearConstraint& lc) const;
+};  // SplineGradientBased
+}  // namespace pathOptimization
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_PATH_OPTIMIZATION_GRADIENT_BASED_HH
+#endif  // HPP_MANIPULATION_PATH_OPTIMIZATION_GRADIENT_BASED_HH
diff --git a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
index 759aac1ef2746d7ae922906ad92c91fe5b3da6ea..098de5447ceb5fef03ecc157b2321b74d013f519 100644
--- a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
@@ -26,126 +26,111 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-
 #ifndef HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
-# define HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
-
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-
-# include <pinocchio/spatial/se3.hpp>
+#define HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
 
-# include <hpp/pinocchio/frame.hh>
-# include <hpp/core/path-planner.hh>
+#include <hpp/core/path-planner.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/pinocchio/frame.hh>
+#include <pinocchio/spatial/se3.hpp>
 
 namespace hpp {
-  namespace manipulation {
-    namespace pathPlanner {
-      class HPP_MANIPULATION_DLLAPI IkSolverInitialization
-      {
-        public:
-          typedef std::vector<Configuration_t> Configurations_t;
-
-          Configurations_t solve (vectorIn_t target)
-          {
-            return impl_solve (target);
-          }
-
-        protected:
-          virtual Configurations_t impl_solve (vectorIn_t target) = 0;
-      };
-      typedef shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
-
-      HPP_PREDEF_CLASS (EndEffectorTrajectory);
-      typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
-
-      class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner
-      {
-      public:
-        /// Return shared pointer to new instance
-        /// \param problem the path planning problem
-        static EndEffectorTrajectoryPtr_t create
-	  (const core::ProblemConstPtr_t& problem);
-        /// Return shared pointer to new instance
-        /// \param problem the path planning problem
-        /// \param roadmap previously built roadmap
-        static EndEffectorTrajectoryPtr_t createWithRoadmap
-          (const core::ProblemConstPtr_t& problem,
-	   const core::RoadmapPtr_t& roadmap);
-
-        /// Initialize the problem resolution
-        ///  \li call parent implementation
-        ///  \li get number nodes in problem parameter map
-        virtual void startSolve ();
-
-        /// One step of the algorithm
-        virtual void oneStep ();
-
-        /// Get the number of random configurations shoot (after using init
-        /// config) in order to generate the initial config of the final path.
-        int nRandomConfig () const
-        { return nRandomConfig_; }
-
-        void nRandomConfig (int n)
-        {
-          assert (n >= 0);
-          nRandomConfig_ = n;
-        }
-
-        /// Number of steps to generate goal config (successive projections).
-        int nDiscreteSteps () const
-        { return nDiscreteSteps_; }
-
-        void nDiscreteSteps (int n)
-        {
-          assert (n > 0);
-          nDiscreteSteps_ = n;
-        }
-
-        /// If enabled, only add one solution to the roadmap.
-        /// Otherwise add all solution.
-        void checkFeasibilityOnly (bool enable);
-
-        bool checkFeasibilityOnly () const
-        {
-          return feasibilityOnly_;
-        }
-
-        void ikSolverInitialization (IkSolverInitializationPtr_t solver)
-        {
-          ikSolverInit_ = solver;
-        }
-
-        void tryConnectInitAndGoals ();
-
-      protected:
-        /// Protected constructor
-        /// \param problem the path planning problem
-        EndEffectorTrajectory (const core::ProblemConstPtr_t& problem);
-        /// Protected constructor
-        /// \param problem the path planning problem
-        /// \param roadmap previously built roadmap
-        EndEffectorTrajectory (const core::ProblemConstPtr_t& problem,
-			       const core::RoadmapPtr_t& roadmap);
-        /// Store weak pointer to itself
-        void init (const EndEffectorTrajectoryWkPtr_t& weak);
-
-      private:
-        std::vector<core::Configuration_t> configurations(const core::Configuration_t& q_init);
-
-        /// Weak pointer to itself
-        EndEffectorTrajectoryWkPtr_t weak_;
-        /// Number of random config.
-        int nRandomConfig_;
-        /// Number of steps to generate goal config.
-        int nDiscreteSteps_;
-        /// Ik solver initialization. An external Ik solver can be plugged here.
-        IkSolverInitializationPtr_t ikSolverInit_;
-        /// Feasibility
-        bool feasibilityOnly_;
-      }; // class EndEffectorTrajectory
-    } // namespace pathPlanner
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
+namespace manipulation {
+namespace pathPlanner {
+class HPP_MANIPULATION_DLLAPI IkSolverInitialization {
+ public:
+  typedef std::vector<Configuration_t> Configurations_t;
+
+  Configurations_t solve(vectorIn_t target) { return impl_solve(target); }
+
+ protected:
+  virtual Configurations_t impl_solve(vectorIn_t target) = 0;
+};
+typedef shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
+
+HPP_PREDEF_CLASS(EndEffectorTrajectory);
+typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+
+class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner {
+ public:
+  /// Return shared pointer to new instance
+  /// \param problem the path planning problem
+  static EndEffectorTrajectoryPtr_t create(
+      const core::ProblemConstPtr_t& problem);
+  /// Return shared pointer to new instance
+  /// \param problem the path planning problem
+  /// \param roadmap previously built roadmap
+  static EndEffectorTrajectoryPtr_t createWithRoadmap(
+      const core::ProblemConstPtr_t& problem,
+      const core::RoadmapPtr_t& roadmap);
+
+  /// Initialize the problem resolution
+  ///  \li call parent implementation
+  ///  \li get number nodes in problem parameter map
+  virtual void startSolve();
+
+  /// One step of the algorithm
+  virtual void oneStep();
+
+  /// Get the number of random configurations shoot (after using init
+  /// config) in order to generate the initial config of the final path.
+  int nRandomConfig() const { return nRandomConfig_; }
+
+  void nRandomConfig(int n) {
+    assert(n >= 0);
+    nRandomConfig_ = n;
+  }
+
+  /// Number of steps to generate goal config (successive projections).
+  int nDiscreteSteps() const { return nDiscreteSteps_; }
+
+  void nDiscreteSteps(int n) {
+    assert(n > 0);
+    nDiscreteSteps_ = n;
+  }
+
+  /// If enabled, only add one solution to the roadmap.
+  /// Otherwise add all solution.
+  void checkFeasibilityOnly(bool enable);
+
+  bool checkFeasibilityOnly() const { return feasibilityOnly_; }
+
+  void ikSolverInitialization(IkSolverInitializationPtr_t solver) {
+    ikSolverInit_ = solver;
+  }
+
+  void tryConnectInitAndGoals();
+
+ protected:
+  /// Protected constructor
+  /// \param problem the path planning problem
+  EndEffectorTrajectory(const core::ProblemConstPtr_t& problem);
+  /// Protected constructor
+  /// \param problem the path planning problem
+  /// \param roadmap previously built roadmap
+  EndEffectorTrajectory(const core::ProblemConstPtr_t& problem,
+                        const core::RoadmapPtr_t& roadmap);
+  /// Store weak pointer to itself
+  void init(const EndEffectorTrajectoryWkPtr_t& weak);
+
+ private:
+  std::vector<core::Configuration_t> configurations(
+      const core::Configuration_t& q_init);
+
+  /// Weak pointer to itself
+  EndEffectorTrajectoryWkPtr_t weak_;
+  /// Number of random config.
+  int nRandomConfig_;
+  /// Number of steps to generate goal config.
+  int nDiscreteSteps_;
+  /// Ik solver initialization. An external Ik solver can be plugged here.
+  IkSolverInitializationPtr_t ikSolverInit_;
+  /// Feasibility
+  bool feasibilityOnly_;
+};  // class EndEffectorTrajectory
+}  // namespace pathPlanner
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_PATH_PLANNER_END_EFFECTOR_TRAJECTORY_HH
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 0fd9a121b0f04eeaf992739f3bb4a0d4a5d9eddd..ee3e01d1390f53dd829ca8cb2d436567bad0ca60 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -27,143 +27,134 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_PROBLEM_SOLVER_HH
-# define HPP_MANIPULATION_PROBLEM_SOLVER_HH
-
-# include <map>
-# include <hpp/pinocchio/device.hh>
-# include <hpp/core/problem-solver.hh>
-# include <hpp/core/container.hh>
-# include "hpp/manipulation/constraint-set.hh"
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/deprecated.hh"
-# include "hpp/manipulation/device.hh"
-# include "hpp/manipulation/graph/fwd.hh"
+#define HPP_MANIPULATION_PROBLEM_SOLVER_HH
+
+#include <hpp/core/container.hh>
+#include <hpp/core/problem-solver.hh>
+#include <hpp/pinocchio/device.hh>
+#include <map>
+
+#include "hpp/manipulation/constraint-set.hh"
+#include "hpp/manipulation/deprecated.hh"
+#include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
 
 namespace hpp {
-  namespace manipulation {
-    class HPP_MANIPULATION_DLLAPI ProblemSolver :
-      public core::ProblemSolver
-    {
-      public:
-        typedef core::ProblemSolver parent_t;
-        typedef std::vector <std::string> Names_t;
-
-        /// Destructor
-        virtual ~ProblemSolver ()
-        {}
-
-        ProblemSolver ();
-
-        static ProblemSolverPtr_t create ();
-
-        /// Set robot
-        /// Check that robot is of type hpp::manipulation::Device
-        virtual void robot (const core::DevicePtr_t& robot)
-        {
-          robot_ = HPP_DYNAMIC_PTR_CAST (Device, robot);
-          assert (robot_);
-          parent_t::robot (robot);
-        }
-
-        /// Get robot
-        const DevicePtr_t& robot () const
-        {
-          return robot_;
-        }
-
-        /// \name Constraint graph
-        /// \{
-
-        /// Set the constraint graph
-        void constraintGraph (const std::string& graph);
-
-        /// Get the constraint graph
-        graph::GraphPtr_t constraintGraph () const;
-
-        /// Should be called before any call on the graph is made.
-        void initConstraintGraph ();
-        /// \}
-
-	/// Create placement constraint
-	/// \param name name of the placement constraint,
-	/// \param triangleName name of the first list of triangles,
-	/// \param envContactName name of the second list of triangles.
-        /// \param margin see hpp::constraints::ConvexShapeContact::setNormalMargin
-	/// 
-	void createPlacementConstraint (const std::string& name,
-					const StringList_t& surface1,
-					const StringList_t& surface2,
-                                        const value_type& margin = 1e-4);
-
-	/// Create pre-placement constraint
-	/// \param name name of the placement constraint,
-	/// \param triangleName name of the first list of triangles,
-	/// \param envContactName name of the second list of triangles.
-	/// \param width approaching distance.
-        /// \param margin see hpp::constraints::ConvexShapeContact::setNormalMargin
-	/// 
-	void createPrePlacementConstraint (const std::string& name,
-					   const StringList_t& surface1,
-					   const StringList_t& surface2,
-                                           const value_type& width,
-                                           const value_type& margin = 1e-4);
-
-	/// Create the grasp constraint and its complement
-	/// \param name name of the grasp constraint,
-	/// \param gripper gripper's name
-	/// \param handle handle's name
-	/// 
-        /// Two constraints are created:
-        /// - "name" corresponds to the grasp constraint.
-        /// - "name/complement" corresponds to the complement.
-        void createGraspConstraint (const std::string& name,
-                                    const std::string& gripper,
-                                    const std::string& handle);
-
-	/// Create pre-grasp constraint
-	/// \param name name of the grasp constraint,
-	/// \param gripper gripper's name
-	/// \param handle handle's name
-	/// 
-        void createPreGraspConstraint (const std::string& name,
-                                       const std::string& gripper,
-                                       const std::string& handle);
-
-        virtual void pathValidationType (const std::string& type,
-                                         const value_type& tolerance);
-
-        /// Create a new problem.
-        virtual void resetProblem ();
-
-        /// Create a new Roadmap
-        virtual void resetRoadmap ();
-
-        /// Get pointer to problem
-        ProblemPtr_t problem () const
-        {
-          return problem_;
-        }
-
-        void setTargetState (const graph::StatePtr_t state);
-
-        core::Container <graph::GraphPtr_t> graphs;
-
-        ConstraintsAndComplements_t constraintsAndComplements;
-
-      protected:
-        virtual void initializeProblem (ProblemPtr_t problem);
-
-      private:
-        /// Keep track of the created components in order to retrieve them
-        /// easily.
-        graph::GraphComponents_t components_;
-
-        DevicePtr_t robot_;
-        /// The pointer should point to the same object as core::Problem.
-        ProblemPtr_t problem_;
-        graph::GraphPtr_t constraintGraph_;
-    }; // class ProblemSolver
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_PROBLEM_SOLVER_HH
+namespace manipulation {
+class HPP_MANIPULATION_DLLAPI ProblemSolver : public core::ProblemSolver {
+ public:
+  typedef core::ProblemSolver parent_t;
+  typedef std::vector<std::string> Names_t;
+
+  /// Destructor
+  virtual ~ProblemSolver() {}
+
+  ProblemSolver();
+
+  static ProblemSolverPtr_t create();
+
+  /// Set robot
+  /// Check that robot is of type hpp::manipulation::Device
+  virtual void robot(const core::DevicePtr_t& robot) {
+    robot_ = HPP_DYNAMIC_PTR_CAST(Device, robot);
+    assert(robot_);
+    parent_t::robot(robot);
+  }
+
+  /// Get robot
+  const DevicePtr_t& robot() const { return robot_; }
+
+  /// \name Constraint graph
+  /// \{
+
+  /// Set the constraint graph
+  void constraintGraph(const std::string& graph);
+
+  /// Get the constraint graph
+  graph::GraphPtr_t constraintGraph() const;
+
+  /// Should be called before any call on the graph is made.
+  void initConstraintGraph();
+  /// \}
+
+  /// Create placement constraint
+  /// \param name name of the placement constraint,
+  /// \param triangleName name of the first list of triangles,
+  /// \param envContactName name of the second list of triangles.
+  /// \param margin see hpp::constraints::ConvexShapeContact::setNormalMargin
+  ///
+  void createPlacementConstraint(const std::string& name,
+                                 const StringList_t& surface1,
+                                 const StringList_t& surface2,
+                                 const value_type& margin = 1e-4);
+
+  /// Create pre-placement constraint
+  /// \param name name of the placement constraint,
+  /// \param triangleName name of the first list of triangles,
+  /// \param envContactName name of the second list of triangles.
+  /// \param width approaching distance.
+  /// \param margin see hpp::constraints::ConvexShapeContact::setNormalMargin
+  ///
+  void createPrePlacementConstraint(const std::string& name,
+                                    const StringList_t& surface1,
+                                    const StringList_t& surface2,
+                                    const value_type& width,
+                                    const value_type& margin = 1e-4);
+
+  /// Create the grasp constraint and its complement
+  /// \param name name of the grasp constraint,
+  /// \param gripper gripper's name
+  /// \param handle handle's name
+  ///
+  /// Two constraints are created:
+  /// - "name" corresponds to the grasp constraint.
+  /// - "name/complement" corresponds to the complement.
+  void createGraspConstraint(const std::string& name,
+                             const std::string& gripper,
+                             const std::string& handle);
+
+  /// Create pre-grasp constraint
+  /// \param name name of the grasp constraint,
+  /// \param gripper gripper's name
+  /// \param handle handle's name
+  ///
+  void createPreGraspConstraint(const std::string& name,
+                                const std::string& gripper,
+                                const std::string& handle);
+
+  virtual void pathValidationType(const std::string& type,
+                                  const value_type& tolerance);
+
+  /// Create a new problem.
+  virtual void resetProblem();
+
+  /// Create a new Roadmap
+  virtual void resetRoadmap();
+
+  /// Get pointer to problem
+  ProblemPtr_t problem() const { return problem_; }
+
+  void setTargetState(const graph::StatePtr_t state);
+
+  core::Container<graph::GraphPtr_t> graphs;
+
+  ConstraintsAndComplements_t constraintsAndComplements;
+
+ protected:
+  virtual void initializeProblem(ProblemPtr_t problem);
+
+ private:
+  /// Keep track of the created components in order to retrieve them
+  /// easily.
+  graph::GraphComponents_t components_;
+
+  DevicePtr_t robot_;
+  /// The pointer should point to the same object as core::Problem.
+  ProblemPtr_t problem_;
+  graph::GraphPtr_t constraintGraph_;
+};  // class ProblemSolver
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_PROBLEM_SOLVER_HH
diff --git a/include/hpp/manipulation/problem-target/state.hh b/include/hpp/manipulation/problem-target/state.hh
index d8d715065a90c8ce5b3dac28dc987de1794b3f40..ebef0dc2eb42580afff8d813b7dcd6572978fcbe 100644
--- a/include/hpp/manipulation/problem-target/state.hh
+++ b/include/hpp/manipulation/problem-target/state.hh
@@ -28,52 +28,46 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_PROBLEM_TARGET_STATE_HH
-# define HPP_MANIPULATION_PROBLEM_TARGET_STATE_HH
+#define HPP_MANIPULATION_PROBLEM_TARGET_STATE_HH
 
-# include <hpp/core/problem-target.hh>
-
-# include <hpp/core/fwd.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/graph/fwd.hh>
-# include <hpp/manipulation/config.hh>
+#include <hpp/core/fwd.hh>
+#include <hpp/core/problem-target.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    namespace problemTarget {
-      /// \addtogroup path_planning
-      /// \{
+namespace manipulation {
+namespace problemTarget {
+/// \addtogroup path_planning
+/// \{
 
-      /// State
-      ///
-      /// This class defines a goal using state of the constraint graph.
-      class HPP_MANIPULATION_DLLAPI State : public core::ProblemTarget {
-        public:
-          static StatePtr_t create (const core::ProblemPtr_t& problem);
+/// State
+///
+/// This class defines a goal using state of the constraint graph.
+class HPP_MANIPULATION_DLLAPI State : public core::ProblemTarget {
+ public:
+  static StatePtr_t create(const core::ProblemPtr_t& problem);
 
-          /// Check if the problem target is well specified.
-          void check (const core::RoadmapPtr_t& roadmap) const;
+  /// Check if the problem target is well specified.
+  void check(const core::RoadmapPtr_t& roadmap) const;
 
-          /// Check whether the problem is solved.
-          bool reached (const core::RoadmapPtr_t& roadmap) const;
+  /// Check whether the problem is solved.
+  bool reached(const core::RoadmapPtr_t& roadmap) const;
 
-          core::PathVectorPtr_t computePath(const core::RoadmapPtr_t& roadmap) const;
+  core::PathVectorPtr_t computePath(const core::RoadmapPtr_t& roadmap) const;
 
-          void target (const graph::StatePtr_t& state)
-          {
-            state_ = state;
-          }
+  void target(const graph::StatePtr_t& state) { state_ = state; }
 
-        protected:
-          /// Constructor
-          State (const core::ProblemPtr_t& problem)
-            : ProblemTarget (problem)
-          {}
+ protected:
+  /// Constructor
+  State(const core::ProblemPtr_t& problem) : ProblemTarget(problem) {}
 
-        private:
-          graph::StatePtr_t state_;
-      }; // class State
-      /// \}
-    } // namespace problemTarget
-  } //   namespace manipulation
-} // namespace hpp
-#endif // HPP_MANIPULATION_PROBLEM_TARGET_STATE_HH
+ private:
+  graph::StatePtr_t state_;
+};  // class State
+/// \}
+}  // namespace problemTarget
+}  //   namespace manipulation
+}  // namespace hpp
+#endif  // HPP_MANIPULATION_PROBLEM_TARGET_STATE_HH
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index fedd70e863fbc295e974ca671e335e1a345b6243..5761678946302ed928caa913f202607d75ab9371 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -27,77 +27,71 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_PROBLEM_HH
-# define HPP_MANIPULATION_PROBLEM_HH
+#define HPP_MANIPULATION_PROBLEM_HH
 
-# include <hpp/core/problem.hh>
-# include <hpp/core/problem-solver.hh> // PathValidationBuilder_t
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/device.hh>
-# include <hpp/manipulation/graph/fwd.hh>
+#include <hpp/core/problem-solver.hh>  // PathValidationBuilder_t
+#include <hpp/core/problem.hh>
+#include <hpp/manipulation/device.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup path_planning
-    /// \{
+namespace manipulation {
+/// \addtogroup path_planning
+/// \{
 
-    class HPP_MANIPULATION_DLLAPI Problem : public core::Problem
-    {
-      public:
-        typedef core::Problem Parent;
+class HPP_MANIPULATION_DLLAPI Problem : public core::Problem {
+ public:
+  typedef core::Problem Parent;
 
-        /// Constructor
-        static ProblemPtr_t create (DevicePtr_t robot);
+  /// Constructor
+  static ProblemPtr_t create(DevicePtr_t robot);
 
-        /// Set the graph of constraints
-        void constraintGraph (const graph::GraphPtr_t& graph);
+  /// Set the graph of constraints
+  void constraintGraph(const graph::GraphPtr_t& graph);
 
-        /// Get the graph of constraints
-        graph::GraphPtr_t constraintGraph () const
-        {
-          return graph_;
-        }
+  /// Get the graph of constraints
+  graph::GraphPtr_t constraintGraph() const { return graph_; }
 
-        /// Check whether the problem is well formulated.
-        virtual void checkProblem () const;
+  /// Check whether the problem is well formulated.
+  virtual void checkProblem() const;
 
-        /// Expose parent method.
-        PathValidationPtr_t pathValidation () const;
+  /// Expose parent method.
+  PathValidationPtr_t pathValidation() const;
 
-        /// \param pathValidation if of type GraphPathValidation, sets 
-        ///        its constraint graph to Problem::constraintGraph()
-        void pathValidation (const PathValidationPtr_t& pathValidation);
+  /// \param pathValidation if of type GraphPathValidation, sets
+  ///        its constraint graph to Problem::constraintGraph()
+  void pathValidation(const PathValidationPtr_t& pathValidation);
 
-        /// Get the steering method as a SteeringMethod
-        SteeringMethodPtr_t manipulationSteeringMethod () const;
+  /// Get the steering method as a SteeringMethod
+  SteeringMethodPtr_t manipulationSteeringMethod() const;
 
-        /// Build a new path validation
-        /// \note Current obstacles are added to the created object.
-        /// \todo Keep a pointer to this value to update it when a new obstacle
-        /// is added.
-        PathValidationPtr_t pathValidationFactory () const;
+  /// Build a new path validation
+  /// \note Current obstacles are added to the created object.
+  /// \todo Keep a pointer to this value to update it when a new obstacle
+  /// is added.
+  PathValidationPtr_t pathValidationFactory() const;
 
-        void setPathValidationFactory (
-            const core::PathValidationBuilder_t& factory,
-            const value_type& tol);
+  void setPathValidationFactory(const core::PathValidationBuilder_t& factory,
+                                const value_type& tol);
 
-      protected:
-        /// Constructor
-        Problem (DevicePtr_t robot);
+ protected:
+  /// Constructor
+  Problem(DevicePtr_t robot);
 
-        void init (ProblemWkPtr_t wkPtr);
+  void init(ProblemWkPtr_t wkPtr);
 
-      private:
-        ProblemWkPtr_t wkPtr_;
+ private:
+  ProblemWkPtr_t wkPtr_;
 
-        /// The graph of constraints
-        graph::GraphPtr_t graph_;
+  /// The graph of constraints
+  graph::GraphPtr_t graph_;
 
-        core::PathValidationBuilder_t pvFactory_;
-        value_type pvTol_;
-    }; // class Problem
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
+  core::PathValidationBuilder_t pvFactory_;
+  value_type pvTol_;
+};  // class Problem
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_PROBLEM_HH
+#endif  // HPP_MANIPULATION_PROBLEM_HH
diff --git a/include/hpp/manipulation/roadmap-node.hh b/include/hpp/manipulation/roadmap-node.hh
index 4307c567603acb3315e2d2f8775bbc25d52fbde5..f281285f4e9c223c425857dd32d610f2a578c9c8 100644
--- a/include/hpp/manipulation/roadmap-node.hh
+++ b/include/hpp/manipulation/roadmap-node.hh
@@ -28,69 +28,52 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_ROADMAP_NODE_HH
-# define HPP_MANIPULATION_ROADMAP_NODE_HH
+#define HPP_MANIPULATION_ROADMAP_NODE_HH
 
-# include <hpp/core/node.hh>
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/deprecated.hh>
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/graph/fwd.hh>
-# include <hpp/manipulation/connected-component.hh>
+#include <hpp/core/node.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/connected-component.hh>
+#include <hpp/manipulation/deprecated.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    class HPP_MANIPULATION_DLLAPI RoadmapNode : public core::Node
-    {
-      public:
-        RoadmapNode (const ConfigurationPtr_t& configuration) :
-          core::Node (configuration),
-          state_ ()
-        {}
+namespace manipulation {
+class HPP_MANIPULATION_DLLAPI RoadmapNode : public core::Node {
+ public:
+  RoadmapNode(const ConfigurationPtr_t& configuration)
+      : core::Node(configuration), state_() {}
 
-        RoadmapNode (const ConfigurationPtr_t& configuration,
-            ConnectedComponentPtr_t cc);
+  RoadmapNode(const ConfigurationPtr_t& configuration,
+              ConnectedComponentPtr_t cc);
 
-        /// \name Cache
-        /// \{
+  /// \name Cache
+  /// \{
 
-        /// Get the caching system being used.
-        bool cacheUpToDate () const
-        {
-          return static_cast<bool>(graphState());
-        }
+  /// Get the caching system being used.
+  bool cacheUpToDate() const { return static_cast<bool>(graphState()); }
 
-        /// Getter for the graph::State.
-        graph::StatePtr_t graphState () const
-        {
-          return state_.lock();
-        }
+  /// Getter for the graph::State.
+  graph::StatePtr_t graphState() const { return state_.lock(); }
 
-        /// Setter for the graph::State.
-        void graphState (const graph::StatePtr_t& state)
-        {
-          state_ = state;
-        }
-        /// \}
+  /// Setter for the graph::State.
+  void graphState(const graph::StatePtr_t& state) { state_ = state; }
+  /// \}
 
-        void leafConnectedComponent (const LeafConnectedCompPtr_t& sc)
-        {
-          leafCC_ = sc;
-        }
+  void leafConnectedComponent(const LeafConnectedCompPtr_t& sc) {
+    leafCC_ = sc;
+  }
 
-        LeafConnectedCompPtr_t leafConnectedComponent () const
-        {
-          return leafCC_;
-        }
+  LeafConnectedCompPtr_t leafConnectedComponent() const { return leafCC_; }
 
-      private:
-        graph::StateWkPtr_t state_;
-        LeafConnectedCompPtr_t leafCC_;
+ private:
+  graph::StateWkPtr_t state_;
+  LeafConnectedCompPtr_t leafCC_;
 
-        RoadmapNode() {}
-        HPP_SERIALIZABLE();
-    };
-  } // namespace manipulation
-} // namespace hpp
+  RoadmapNode() {}
+  HPP_SERIALIZABLE();
+};
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_ROADMAP_NODE_HH
+#endif  // HPP_MANIPULATION_ROADMAP_NODE_HH
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index d355aeb924fb4cb3d75c7c6837020cab8d62148a..cc95360f88f4c247e6c29a82c42063352ae639fc 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -27,112 +27,110 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_ROADMAP_HH
-# define HPP_MANIPULATION_ROADMAP_HH
+#define HPP_MANIPULATION_ROADMAP_HH
 
-# include <hpp/core/roadmap.hh>
+#include <hpp/core/roadmap.hh>
+#include <hpp/manipulation/deprecated.hh>
+#include <hpp/manipulation/leaf-connected-comp.hh>
 
-# include "hpp/manipulation/config.hh"
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/graph/fwd.hh"
-# include <hpp/manipulation/deprecated.hh>
-# include <hpp/manipulation/leaf-connected-comp.hh>
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup roadmap
-    /// \{
-
-    /// Extension of hpp::core::Roadmap. It adds the ability of doing
-    /// statistics on the graph
-    class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap
-    {
-      public:
-        typedef core::Roadmap Parent;
-
-        /// Return a shared pointer to a new instance
-        static RoadmapPtr_t create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
-
-        /// Register histogram so that each time a node is added to the roadmap,
-        /// it is also added to the histogram
-        void insertHistogram (const graph::HistogramPtr_t hist);
-
-        /// Register the constraint graph to do statistics.
-        void constraintGraph (const graph::GraphPtr_t& graph);
-
-        /// Clear the histograms and call parent implementation.
-        void clear ();
-
-        /// Catch event 'New node added'
-        void push_node (const core::NodePtr_t& n);
-
-        /// Get the nearest neighbor in a given graph::Node and in a given
-        /// ConnectedComponent.
-        RoadmapNodePtr_t nearestNodeInState (const ConfigurationPtr_t& configuration,
-            const ConnectedComponentPtr_t& connectedComponent,
-            const graph::StatePtr_t& state,
-            value_type& minDistance) const;
-
-	/// Get graph state corresponding to given roadmap node
-	/// \deprecated use getState instead
-	graph::StatePtr_t getNode(RoadmapNodePtr_t node) HPP_MANIPULATION_DEPRECATED;
-
-        /// Update the graph of connected components after new connection
-        /// \param cc1, cc2 the two connected components that have just been
-        /// connected.
-        void connect (const LeafConnectedCompPtr_t& cc1,
-                      const LeafConnectedCompPtr_t& cc2);
-
-        /// Merge two connected components
-        /// \param cc1 the connected component to merge into
-        /// \param the connected components to merge into cc1.
-        void merge (const LeafConnectedCompPtr_t& cc1,
-                    LeafConnectedComp::LeafConnectedComps_t& ccs);
-
-	/// Get graph state corresponding to given roadmap node
-	graph::StatePtr_t getState(RoadmapNodePtr_t node);
-
-        /// Get leaf connected components
-        ///
-        /// Leaf connected components are composed of nodes
-        /// \li belonging to the same connected component of the roadmap and,
-        /// \li lying in the same leaf of a transition.
-        const LeafConnectedComps_t& leafConnectedComponents () const
-        {
-          return leafCCs_;
-        }
-
-      protected:
-        /// Register a new configuration.
-        void statInsert (const RoadmapNodePtr_t& n);
-
-        /// Constructor
-        Roadmap (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
-
-        /// Node factory
-        core::NodePtr_t createNode (const ConfigurationPtr_t& config) const;
-
-        void init (const RoadmapPtr_t& shPtr)
-	{
-          Parent::init (shPtr);
-	  weak_ = shPtr;
-	}
-
-        virtual void impl_addEdge (const core::EdgePtr_t& edge);
-
-      private:
-        typedef graph::Histograms_t Histograms_t;
-        /// Keep track of the leaf that are explored.
-        /// There should be one histogram per foliation.
-        Histograms_t histograms_;
-        graph::GraphPtr_t graph_;
-        RoadmapWkPtr_t weak_;
-        LeafConnectedComps_t leafCCs_;
-
-        Roadmap() {}
-        HPP_SERIALIZABLE();
-    };
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_ROADMAP_HH
+namespace manipulation {
+/// \addtogroup roadmap
+/// \{
+
+/// Extension of hpp::core::Roadmap. It adds the ability of doing
+/// statistics on the graph
+class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap {
+ public:
+  typedef core::Roadmap Parent;
+
+  /// Return a shared pointer to a new instance
+  static RoadmapPtr_t create(const core::DistancePtr_t& distance,
+                             const core::DevicePtr_t& robot);
+
+  /// Register histogram so that each time a node is added to the roadmap,
+  /// it is also added to the histogram
+  void insertHistogram(const graph::HistogramPtr_t hist);
+
+  /// Register the constraint graph to do statistics.
+  void constraintGraph(const graph::GraphPtr_t& graph);
+
+  /// Clear the histograms and call parent implementation.
+  void clear();
+
+  /// Catch event 'New node added'
+  void push_node(const core::NodePtr_t& n);
+
+  /// Get the nearest neighbor in a given graph::Node and in a given
+  /// ConnectedComponent.
+  RoadmapNodePtr_t nearestNodeInState(
+      const ConfigurationPtr_t& configuration,
+      const ConnectedComponentPtr_t& connectedComponent,
+      const graph::StatePtr_t& state, value_type& minDistance) const;
+
+  /// Get graph state corresponding to given roadmap node
+  /// \deprecated use getState instead
+  graph::StatePtr_t getNode(RoadmapNodePtr_t node) HPP_MANIPULATION_DEPRECATED;
+
+  /// Update the graph of connected components after new connection
+  /// \param cc1, cc2 the two connected components that have just been
+  /// connected.
+  void connect(const LeafConnectedCompPtr_t& cc1,
+               const LeafConnectedCompPtr_t& cc2);
+
+  /// Merge two connected components
+  /// \param cc1 the connected component to merge into
+  /// \param the connected components to merge into cc1.
+  void merge(const LeafConnectedCompPtr_t& cc1,
+             LeafConnectedComp::LeafConnectedComps_t& ccs);
+
+  /// Get graph state corresponding to given roadmap node
+  graph::StatePtr_t getState(RoadmapNodePtr_t node);
+
+  /// Get leaf connected components
+  ///
+  /// Leaf connected components are composed of nodes
+  /// \li belonging to the same connected component of the roadmap and,
+  /// \li lying in the same leaf of a transition.
+  const LeafConnectedComps_t& leafConnectedComponents() const {
+    return leafCCs_;
+  }
+
+ protected:
+  /// Register a new configuration.
+  void statInsert(const RoadmapNodePtr_t& n);
+
+  /// Constructor
+  Roadmap(const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
+
+  /// Node factory
+  core::NodePtr_t createNode(const ConfigurationPtr_t& config) const;
+
+  void init(const RoadmapPtr_t& shPtr) {
+    Parent::init(shPtr);
+    weak_ = shPtr;
+  }
+
+  virtual void impl_addEdge(const core::EdgePtr_t& edge);
+
+ private:
+  typedef graph::Histograms_t Histograms_t;
+  /// Keep track of the leaf that are explored.
+  /// There should be one histogram per foliation.
+  Histograms_t histograms_;
+  graph::GraphPtr_t graph_;
+  RoadmapWkPtr_t weak_;
+  LeafConnectedComps_t leafCCs_;
+
+  Roadmap() {}
+  HPP_SERIALIZABLE();
+};
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_ROADMAP_HH
diff --git a/include/hpp/manipulation/serialization.hh b/include/hpp/manipulation/serialization.hh
index 953a68904c15588e6fa19be06a60947350dad981..ef780d399a2a7972db45743903db852c48c200be 100644
--- a/include/hpp/manipulation/serialization.hh
+++ b/include/hpp/manipulation/serialization.hh
@@ -28,36 +28,34 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_SERIALIZATION_HH
-# define HPP_MANIPULATION_SERIALIZATION_HH
-
-# include <boost/serialization/split_free.hpp>
-# include <boost/serialization/shared_ptr.hpp>
-# include <boost/serialization/weak_ptr.hpp>
-
-# include <hpp/util/serialization.hh>
-# include <hpp/pinocchio/serialization.hh>
-
-# include <hpp/manipulation/fwd.hh>
-
-# include <hpp/manipulation/graph/graph.hh>
-# include <hpp/manipulation/graph/state.hh>
-# include <hpp/manipulation/graph/edge.hh>
+#define HPP_MANIPULATION_SERIALIZATION_HH
+
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/weak_ptr.hpp>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/graph/state.hh>
+#include <hpp/pinocchio/serialization.hh>
+#include <hpp/util/serialization.hh>
 
 namespace hpp {
 namespace serialization {
-template<typename Archive>
-manipulation::graph::GraphPtr_t getGraphFromArchive(Archive& ar, const std::string& name)
-{
+template <typename Archive>
+manipulation::graph::GraphPtr_t getGraphFromArchive(Archive& ar,
+                                                    const std::string& name) {
   auto* har = hpp::serialization::cast(&ar);
   if (!har || !har->contains(name))
-    throw std::runtime_error("Cannot deserialize edges with a provided graph with correct name.");
+    throw std::runtime_error(
+        "Cannot deserialize edges with a provided graph with correct name.");
   return har->template get<manipulation::graph::Graph>(name, true)->self();
 }
 
-template<class Archive, class GraphCompT>
-inline void serializeGraphComponent(Archive & ar, shared_ptr<GraphCompT>& c, const unsigned int version)
-{
-  (void) version;
+template <class Archive, class GraphCompT>
+inline void serializeGraphComponent(Archive& ar, shared_ptr<GraphCompT>& c,
+                                    const unsigned int version) {
+  (void)version;
 
   std::size_t id;
   std::string name;
@@ -65,15 +63,15 @@ inline void serializeGraphComponent(Archive & ar, shared_ptr<GraphCompT>& c, con
     id = (c ? c->id() : -1);
     if (c && c->parentGraph()) name = c->parentGraph()->name();
   }
-  ar & BOOST_SERIALIZATION_NVP(id);
-  ar & BOOST_SERIALIZATION_NVP(name);
+  ar& BOOST_SERIALIZATION_NVP(id);
+  ar& BOOST_SERIALIZATION_NVP(name);
   if (!Archive::is_saving::value) {
     auto graph = getGraphFromArchive(ar, name);
     c = HPP_DYNAMIC_PTR_CAST(GraphCompT, graph->get(id).lock());
   }
 }
-} // namespace manipulation
-} // namespace hpp
+}  // namespace serialization
+}  // namespace hpp
 
 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::RoadmapNode)
 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::ConnectedComponent)
@@ -82,67 +80,70 @@ BOOST_CLASS_EXPORT_KEY(hpp::manipulation::Roadmap)
 
 namespace boost {
 namespace serialization {
-template<class Archive>
-inline void serialize(Archive & ar, hpp::manipulation::graph::GraphPtr_t& g, const unsigned int version)
-{
+template <class Archive>
+inline void serialize(Archive& ar, hpp::manipulation::graph::GraphPtr_t& g,
+                      const unsigned int version) {
   using hpp::serialization::getGraphFromArchive;
-  (void) version;
+  (void)version;
 
   std::string name;
   if (Archive::is_saving::value) name = g->name();
-  ar & BOOST_SERIALIZATION_NVP(name);
-  if (!Archive::is_saving::value)
-    g = getGraphFromArchive(ar, name);
+  ar& BOOST_SERIALIZATION_NVP(name);
+  if (!Archive::is_saving::value) g = getGraphFromArchive(ar, name);
 }
 
-template<class Archive>
-inline void serialize(Archive & ar, hpp::manipulation::graph::EdgePtr_t& e, const unsigned int version)
-{
-  hpp::serialization::serializeGraphComponent (ar, e, version);
+template <class Archive>
+inline void serialize(Archive& ar, hpp::manipulation::graph::EdgePtr_t& e,
+                      const unsigned int version) {
+  hpp::serialization::serializeGraphComponent(ar, e, version);
 }
 
-template<class Archive>
-inline void serialize(Archive & ar, hpp::manipulation::graph::StatePtr_t& s, const unsigned int version)
-{
-  hpp::serialization::serializeGraphComponent (ar, s, version);
+template <class Archive>
+inline void serialize(Archive& ar, hpp::manipulation::graph::StatePtr_t& s,
+                      const unsigned int version) {
+  hpp::serialization::serializeGraphComponent(ar, s, version);
 }
 
-template<class Archive>
-inline void serialize(Archive & ar, hpp::manipulation::graph::EdgeWkPtr_t& e, const unsigned int version)
-{
+template <class Archive>
+inline void serialize(Archive& ar, hpp::manipulation::graph::EdgeWkPtr_t& e,
+                      const unsigned int version) {
   auto e_ = e.lock();
   serialize(ar, e_, version);
   e = e_;
 }
 
-template<class Archive>
-inline void serialize(Archive & ar, hpp::manipulation::graph::StateWkPtr_t& s, const unsigned int version)
-{
+template <class Archive>
+inline void serialize(Archive& ar, hpp::manipulation::graph::StateWkPtr_t& s,
+                      const unsigned int version) {
   auto s_ = s.lock();
   serialize(ar, s_, version);
   s = s_;
 }
 
-template<class Archive>
-inline void load (Archive& ar, hpp::manipulation::DevicePtr_t& d, const unsigned int version)
-{
-  load<Archive, hpp::manipulation::Device> (ar, d, version);
+template <class Archive>
+inline void load(Archive& ar, hpp::manipulation::DevicePtr_t& d,
+                 const unsigned int version) {
+  load<Archive, hpp::manipulation::Device>(ar, d, version);
   auto* har = hpp::serialization::cast(&ar);
   if (d && har && har->contains(d->name()))
-    d = har->template getChildClass<hpp::pinocchio::Device, hpp::manipulation::Device>(d->name(), true)->self();
+    d = har->template getChildClass<hpp::pinocchio::Device,
+                                    hpp::manipulation::Device>(d->name(), true)
+            ->self();
 }
 
-template<class Archive>
-inline void load (Archive& ar, hpp::manipulation::DeviceWkPtr_t& d, const unsigned int version)
-{
-  load<Archive, hpp::manipulation::Device> (ar, d, version);
+template <class Archive>
+inline void load(Archive& ar, hpp::manipulation::DeviceWkPtr_t& d,
+                 const unsigned int version) {
+  load<Archive, hpp::manipulation::Device>(ar, d, version);
   auto* har = hpp::serialization::cast(&ar);
   auto dd = d.lock();
   if (!dd) return;
   if (har && har->contains(dd->name()))
-    d = har->template getChildClass<hpp::pinocchio::Device, hpp::manipulation::Device>(dd->name(), true)->self();
+    d = har->template getChildClass<hpp::pinocchio::Device,
+                                    hpp::manipulation::Device>(dd->name(), true)
+            ->self();
 }
-} // namespace serialization
-} // namespace boost
+}  // namespace serialization
+}  // namespace boost
 
-#endif // HPP_MANIPULATION_SERIALIZATION_HH
+#endif  // HPP_MANIPULATION_SERIALIZATION_HH
diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
index 794ae9b23bf44fb40fa3401d78e3d4ca3a6db247..5edfca060bcd07fa431f2bb6aa92a0577950b85d 100644
--- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh
+++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -28,167 +28,163 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
-# define HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
+#define HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
 
-# include <hpp/core/steering-method.hh>
-# include <hpp/core/config-projector.hh>
-
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/problem.hh>
-# include <hpp/manipulation/steering-method/fwd.hh>
-# include <hpp/manipulation/steering-method/graph.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/steering-method.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/steering-method/fwd.hh>
+#include <hpp/manipulation/steering-method/graph.hh>
 
 namespace hpp {
-  namespace manipulation {
-    namespace steeringMethod {
-      /// \addtogroup steering_method
-      /// \{
-
-      /// Optimization-based steering method.
-      ///
-      /// #### Sketch of the method
-      ///
-      /// Given two configuration \f$ (q_1,q_2) \f$, this class formulates and
-      /// solves the problem as follows.
-      /// - Compute the corresponding states \f$ (s_1, s_2) \f$.
-      /// - For a each path \f$ (e_0, ... e_n) \f$ of length not more than
-      ///   parameter "CrossStateOptimization/maxDepth" between
-      ///   \f$ (s_1, s_2)\f$ in the constraint graph, do:
-      ///   - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$,
-      ///   - initialize the optimization problem, as explained below,
-      ///   - solve the optimization problem, which gives \f$ p^*_i \f$,
-      ///   - in case of failure, continue the loop.
-      ///   - call the Edge::build of each \f$ e_j \f$ for each consecutive
-      ///     \f$ (p^*_i, p^*_{i+1}) \f$.
-      ///
-      /// #### Problem formulation
-      /// Find \f$ (p_i) \f$ such that:
-      /// - \f$ p_0 = q_1 \f$,
-      /// - \f$ p_{n+1} = q_2 \f$,
-      /// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref StateFunction)
-      /// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref EdgeFunction).
-      ///
-      /// #### Problem resolution
-      ///
-      /// One solver (hpp::constraints::solver::BySubstitution) is created
-      /// for each waypoint \f$p_i\f$.
-      /// - method buildOptimizationProblem builds a matrix the rows of which
-      ///   are the parameterizable numerical constraints present in the
-      ///   graph, and the columns of which are the waypoints. Each value in the
-      ///   matrix defines the status of each constraint right hand side for
-      ///   this waypoint, among {absent from the solver,
-      ///                         equal to value for previous waypoint,
-      ///                         equal to value for start configuration,
-      ///                         equal to value for end configuration}.
-      /// - method CrossStateOptimization::solveOptimizationProblem loops over
-      ///   the waypoint solvers, solves for each waypoint after
-      ///   initializing the right hand sides with the proper values.
-      /// - eventually method buildPath build paths between waypoints with
-      ///   the constraints of the transition in which the path lies.
-      ///
-      /// #### Current status
-      ///
-      /// The method has been successfully tested with romeo holding a placard
-      /// and the construction set benchmarks. The result is satisfactory
-      /// except between pregrasp and grasp waypoints that may be far
-      /// away from each other if the transition between those state does
-      /// not contain the grasp complement constraint. The same holds
-      /// between placement and pre-placement.
-      class HPP_MANIPULATION_DLLAPI CrossStateOptimization :
-        public SteeringMethod
-      {
-        public:
-          struct OptimizationData;
-
-          static CrossStateOptimizationPtr_t create
-	    (const ProblemConstPtr_t& problem);
-
-          /// \warning core::Problem will be casted to Problem
-          static CrossStateOptimizationPtr_t create
-            (const core::ProblemConstPtr_t& problem);
-
-          template <typename T>
-            static CrossStateOptimizationPtr_t create
-            (const core::ProblemConstPtr_t& problem);
-
-          core::SteeringMethodPtr_t copy () const;
-
-        protected:
-          CrossStateOptimization (const ProblemConstPtr_t& problem) :
-            SteeringMethod (problem),
-            sameRightHandSide_ ()
-          {
-            gatherGraphConstraints ();
-          }
-
-          CrossStateOptimization (const CrossStateOptimization& other) :
-            SteeringMethod (other), constraints_ (other.constraints_),
-            index_ (other.index_), sameRightHandSide_
-            (other.sameRightHandSide_), weak_ ()
-          {}
-
-          core::PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
-          void init (CrossStateOptimizationWkPtr_t weak)
-          {
-            SteeringMethod::init (weak);
-            weak_ = weak;
-          }
-
-        private:
-          typedef constraints::solver::BySubstitution Solver_t;
-          struct GraphSearchData;
-
-          /// Gather constraints of all edges
-          void gatherGraphConstraints ();
-
-          /// Step 1 of the algorithm
-          /// \return whether the max depth was reached.
-          bool findTransitions (GraphSearchData& data) const;
-
-          /// Step 2 of the algorithm
-          graph::Edges_t getTransitionList (GraphSearchData& data, const std::size_t& i) const;
-
-          /// Step 3 of the algorithm
-          bool buildOptimizationProblem
-            (OptimizationData& d, const graph::Edges_t& transitions) const;
-
-          /// Step 4 of the algorithm
-          bool solveOptimizationProblem (OptimizationData& d) const;
-
-          bool checkConstantRightHandSide (OptimizationData& d,
-                                           size_type index) const;
-
-          core::PathVectorPtr_t buildPath (OptimizationData& d, const graph::Edges_t& edges) const;
-
-          bool contains (const Solver_t& solver, const ImplicitPtr_t& c) const;
-
-          /// Vector of parameterizable edge numerical constraints
-          NumericalConstraints_t constraints_;
-          /// Map of indexes in constraints_
-          std::map < std::string, std::size_t > index_;
-
-          /// associative map that stores pairs of constraints of the form
-          /// (constraint, constraint/hold)
-          std::map <ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
-
-          /// Weak pointer to itself
-          CrossStateOptimizationWkPtr_t weak_;
-      }; // class CrossStateOptimization
-      /// \}
-
-      template <typename T>
-        CrossStateOptimizationPtr_t CrossStateOptimization::create
-        (const core::ProblemConstPtr_t& problem)
-      {
-        CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create
-	  (problem);
-        gsm->innerSteeringMethod (T::create (problem));
-        return gsm;
-      }
-    } // namespace steeringMethod
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
+namespace manipulation {
+namespace steeringMethod {
+/// \addtogroup steering_method
+/// \{
+
+/// Optimization-based steering method.
+///
+/// #### Sketch of the method
+///
+/// Given two configuration \f$ (q_1,q_2) \f$, this class formulates and
+/// solves the problem as follows.
+/// - Compute the corresponding states \f$ (s_1, s_2) \f$.
+/// - For a each path \f$ (e_0, ... e_n) \f$ of length not more than
+///   parameter "CrossStateOptimization/maxDepth" between
+///   \f$ (s_1, s_2)\f$ in the constraint graph, do:
+///   - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$,
+///   - initialize the optimization problem, as explained below,
+///   - solve the optimization problem, which gives \f$ p^*_i \f$,
+///   - in case of failure, continue the loop.
+///   - call the Edge::build of each \f$ e_j \f$ for each consecutive
+///     \f$ (p^*_i, p^*_{i+1}) \f$.
+///
+/// #### Problem formulation
+/// Find \f$ (p_i) \f$ such that:
+/// - \f$ p_0 = q_1 \f$,
+/// - \f$ p_{n+1} = q_2 \f$,
+/// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref
+/// StateFunction)
+/// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref
+/// EdgeFunction).
+///
+/// #### Problem resolution
+///
+/// One solver (hpp::constraints::solver::BySubstitution) is created
+/// for each waypoint \f$p_i\f$.
+/// - method buildOptimizationProblem builds a matrix the rows of which
+///   are the parameterizable numerical constraints present in the
+///   graph, and the columns of which are the waypoints. Each value in the
+///   matrix defines the status of each constraint right hand side for
+///   this waypoint, among {absent from the solver,
+///                         equal to value for previous waypoint,
+///                         equal to value for start configuration,
+///                         equal to value for end configuration}.
+/// - method CrossStateOptimization::solveOptimizationProblem loops over
+///   the waypoint solvers, solves for each waypoint after
+///   initializing the right hand sides with the proper values.
+/// - eventually method buildPath build paths between waypoints with
+///   the constraints of the transition in which the path lies.
+///
+/// #### Current status
+///
+/// The method has been successfully tested with romeo holding a placard
+/// and the construction set benchmarks. The result is satisfactory
+/// except between pregrasp and grasp waypoints that may be far
+/// away from each other if the transition between those state does
+/// not contain the grasp complement constraint. The same holds
+/// between placement and pre-placement.
+class HPP_MANIPULATION_DLLAPI CrossStateOptimization : public SteeringMethod {
+ public:
+  struct OptimizationData;
+
+  static CrossStateOptimizationPtr_t create(const ProblemConstPtr_t& problem);
+
+  /// \warning core::Problem will be casted to Problem
+  static CrossStateOptimizationPtr_t create(
+      const core::ProblemConstPtr_t& problem);
+
+  template <typename T>
+  static CrossStateOptimizationPtr_t create(
+      const core::ProblemConstPtr_t& problem);
+
+  core::SteeringMethodPtr_t copy() const;
+
+ protected:
+  CrossStateOptimization(const ProblemConstPtr_t& problem)
+      : SteeringMethod(problem), sameRightHandSide_() {
+    gatherGraphConstraints();
+  }
+
+  CrossStateOptimization(const CrossStateOptimization& other)
+      : SteeringMethod(other),
+        constraints_(other.constraints_),
+        index_(other.index_),
+        sameRightHandSide_(other.sameRightHandSide_),
+        weak_() {}
+
+  core::PathPtr_t impl_compute(ConfigurationIn_t q1,
+                               ConfigurationIn_t q2) const;
+
+  void init(CrossStateOptimizationWkPtr_t weak) {
+    SteeringMethod::init(weak);
+    weak_ = weak;
+  }
+
+ private:
+  typedef constraints::solver::BySubstitution Solver_t;
+  struct GraphSearchData;
+
+  /// Gather constraints of all edges
+  void gatherGraphConstraints();
+
+  /// Step 1 of the algorithm
+  /// \return whether the max depth was reached.
+  bool findTransitions(GraphSearchData& data) const;
+
+  /// Step 2 of the algorithm
+  graph::Edges_t getTransitionList(GraphSearchData& data,
+                                   const std::size_t& i) const;
+
+  /// Step 3 of the algorithm
+  bool buildOptimizationProblem(OptimizationData& d,
+                                const graph::Edges_t& transitions) const;
+
+  /// Step 4 of the algorithm
+  bool solveOptimizationProblem(OptimizationData& d) const;
+
+  bool checkConstantRightHandSide(OptimizationData& d, size_type index) const;
+
+  core::PathVectorPtr_t buildPath(OptimizationData& d,
+                                  const graph::Edges_t& edges) const;
+
+  bool contains(const Solver_t& solver, const ImplicitPtr_t& c) const;
+
+  /// Vector of parameterizable edge numerical constraints
+  NumericalConstraints_t constraints_;
+  /// Map of indexes in constraints_
+  std::map<std::string, std::size_t> index_;
+
+  /// associative map that stores pairs of constraints of the form
+  /// (constraint, constraint/hold)
+  std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
+
+  /// Weak pointer to itself
+  CrossStateOptimizationWkPtr_t weak_;
+};  // class CrossStateOptimization
+/// \}
+
+template <typename T>
+CrossStateOptimizationPtr_t CrossStateOptimization::create(
+    const core::ProblemConstPtr_t& problem) {
+  CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create(problem);
+  gsm->innerSteeringMethod(T::create(problem));
+  return gsm;
+}
+}  // namespace steeringMethod
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index b0513efd847d0440b15f36cdc3c2faf0cbe8502c..6454a1b134c08326e33115ceef823fd6ab2f4ae9 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -30,107 +30,95 @@
 #define HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH
 
 #include <hpp/core/steering-method.hh>
-
-#include <hpp/manipulation/fwd.hh>
 #include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup steering_method
-    /// \{
-    namespace steeringMethod {
-      HPP_PREDEF_CLASS (EndEffectorTrajectory);
-      typedef shared_ptr < EndEffectorTrajectory > EndEffectorTrajectoryPtr_t;
-
-      using core::PathPtr_t;
-
-      /// Build StraightPath constrained by a varying right hand side constraint.
-      class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
-        : public core::SteeringMethod
-      {
-        public:
-          typedef core::interval_t interval_t;
-
-          static EndEffectorTrajectoryPtr_t create
-	    (const core::ProblemConstPtr_t& problem)
-          {
-            EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory (problem));
-            ptr->init(ptr);
-            return ptr;
-          }
-
-          /// Build a trajectory in SE(3).
-          /// \param points a Nx7 matrix whose rows corresponds to a pose.
-          /// \param weights a 6D vector, weights to be applied when computing
-          ///        the distance between two SE3 points.
-          static PathPtr_t makePiecewiseLinearTrajectory (matrixIn_t points,
-              vectorIn_t weights);
-
-          /// Set the constraint whose right hand side will vary.
-          void trajectoryConstraint (const constraints::ImplicitPtr_t& ic);
-
-          const constraints::ImplicitPtr_t& trajectoryConstraint ()
-          {
-            return constraint_;
-          }
-
-          /// Set the right hand side of the function from a path
-          /// \param se3Output set to True if the output of path must be
-          ///                  understood as SE3.
-          void trajectory (const PathPtr_t& eeTraj, bool se3Output);
-
-          /// Set the right hand side of the function from another function.
-          /// \param eeTraj a function whose input space is of dimension 1.
-          /// \param timeRange the input range of eeTraj.
-          void trajectory (const DifferentiableFunctionPtr_t& eeTraj, const interval_t& timeRange);
-
-          const DifferentiableFunctionPtr_t& trajectory () const
-          {
-            return eeTraj_;
-          }
-
-          const interval_t& timeRange () const
-          {
-            return timeRange_;
-          }
-
-          core::SteeringMethodPtr_t copy () const
-          {
-            EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory (*this));
-            ptr->init (ptr);
-            return ptr;
-          }
-
-          /// Computes an core::InterpolatedPath from the provided interpolation
-          /// points.
-          /// \param times the time of each configuration
-          /// \param configs each column correspond to a configuration
-          PathPtr_t projectedPath (vectorIn_t times, matrixIn_t configs) const;
-
-        protected:
-          EndEffectorTrajectory (const core::ProblemConstPtr_t& problem)
-            : core::SteeringMethod (problem)
-          {}
-
-          EndEffectorTrajectory (const EndEffectorTrajectory& other)
-            : core::SteeringMethod (other),
-            eeTraj_ (other.eeTraj_),
-            timeRange_ (other.timeRange_),
-            constraint_ (other.constraint_)
-          {}
-
-          PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
-        private:
-          core::ConstraintSetPtr_t getUpdatedConstraints () const;
-
-          DifferentiableFunctionPtr_t eeTraj_;
-          interval_t timeRange_;
-          constraints::ImplicitPtr_t constraint_;
-      };
-    } // namespace steeringMethod
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH
+namespace manipulation {
+/// \addtogroup steering_method
+/// \{
+namespace steeringMethod {
+HPP_PREDEF_CLASS(EndEffectorTrajectory);
+typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+
+using core::PathPtr_t;
+
+/// Build StraightPath constrained by a varying right hand side constraint.
+class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory
+    : public core::SteeringMethod {
+ public:
+  typedef core::interval_t interval_t;
+
+  static EndEffectorTrajectoryPtr_t create(
+      const core::ProblemConstPtr_t& problem) {
+    EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory(problem));
+    ptr->init(ptr);
+    return ptr;
+  }
+
+  /// Build a trajectory in SE(3).
+  /// \param points a Nx7 matrix whose rows corresponds to a pose.
+  /// \param weights a 6D vector, weights to be applied when computing
+  ///        the distance between two SE3 points.
+  static PathPtr_t makePiecewiseLinearTrajectory(matrixIn_t points,
+                                                 vectorIn_t weights);
+
+  /// Set the constraint whose right hand side will vary.
+  void trajectoryConstraint(const constraints::ImplicitPtr_t& ic);
+
+  const constraints::ImplicitPtr_t& trajectoryConstraint() {
+    return constraint_;
+  }
+
+  /// Set the right hand side of the function from a path
+  /// \param se3Output set to True if the output of path must be
+  ///                  understood as SE3.
+  void trajectory(const PathPtr_t& eeTraj, bool se3Output);
+
+  /// Set the right hand side of the function from another function.
+  /// \param eeTraj a function whose input space is of dimension 1.
+  /// \param timeRange the input range of eeTraj.
+  void trajectory(const DifferentiableFunctionPtr_t& eeTraj,
+                  const interval_t& timeRange);
+
+  const DifferentiableFunctionPtr_t& trajectory() const { return eeTraj_; }
+
+  const interval_t& timeRange() const { return timeRange_; }
+
+  core::SteeringMethodPtr_t copy() const {
+    EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory(*this));
+    ptr->init(ptr);
+    return ptr;
+  }
+
+  /// Computes an core::InterpolatedPath from the provided interpolation
+  /// points.
+  /// \param times the time of each configuration
+  /// \param configs each column correspond to a configuration
+  PathPtr_t projectedPath(vectorIn_t times, matrixIn_t configs) const;
+
+ protected:
+  EndEffectorTrajectory(const core::ProblemConstPtr_t& problem)
+      : core::SteeringMethod(problem) {}
+
+  EndEffectorTrajectory(const EndEffectorTrajectory& other)
+      : core::SteeringMethod(other),
+        eeTraj_(other.eeTraj_),
+        timeRange_(other.timeRange_),
+        constraint_(other.constraint_) {}
+
+  PathPtr_t impl_compute(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+ private:
+  core::ConstraintSetPtr_t getUpdatedConstraints() const;
+
+  DifferentiableFunctionPtr_t eeTraj_;
+  interval_t timeRange_;
+  constraints::ImplicitPtr_t constraint_;
+};
+}  // namespace steeringMethod
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_STEERING_METHOD_END_EFFECTOR_TRAJECTORY_HH
diff --git a/include/hpp/manipulation/steering-method/fwd.hh b/include/hpp/manipulation/steering-method/fwd.hh
index 91e73eac99865b5760de9b3bed7062601af8e0c5..167c1e440cbb6718ca5834b63bbb5d57ac3e1458 100644
--- a/include/hpp/manipulation/steering-method/fwd.hh
+++ b/include/hpp/manipulation/steering-method/fwd.hh
@@ -29,20 +29,20 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_STEERING_METHOD_FWD_HH
-# define HPP_MANIPULATION_STEERING_METHOD_FWD_HH
+#define HPP_MANIPULATION_STEERING_METHOD_FWD_HH
 
-# include <map>
-# include <hpp/core/fwd.hh>
+#include <hpp/core/fwd.hh>
+#include <map>
 
 namespace hpp {
-  namespace manipulation {
-    namespace steeringMethod {
-      HPP_PREDEF_CLASS (Graph);
-      typedef shared_ptr < Graph > GraphPtr_t;
-      HPP_PREDEF_CLASS (CrossStateOptimization);
-      typedef shared_ptr < CrossStateOptimization > CrossStateOptimizationPtr_t;
-    } // namespace steeringMethod
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+namespace steeringMethod {
+HPP_PREDEF_CLASS(Graph);
+typedef shared_ptr<Graph> GraphPtr_t;
+HPP_PREDEF_CLASS(CrossStateOptimization);
+typedef shared_ptr<CrossStateOptimization> CrossStateOptimizationPtr_t;
+}  // namespace steeringMethod
+}  // namespace manipulation
+}  // namespace hpp
 
-#endif // HPP_MANIPULATION_STEERING_METHOD_FWD_HH
+#endif  // HPP_MANIPULATION_STEERING_METHOD_FWD_HH
diff --git a/include/hpp/manipulation/steering-method/graph.hh b/include/hpp/manipulation/steering-method/graph.hh
index d95aa1f818dcb9e8a8237e299be0bec77141e474..6eaf0b2abb710d0b1641cdbe2cfe65235bd33051 100644
--- a/include/hpp/manipulation/steering-method/graph.hh
+++ b/include/hpp/manipulation/steering-method/graph.hh
@@ -26,111 +26,96 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-
 #ifndef HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
-# define HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
-
-# include <hpp/core/problem-solver.hh> // SteeringMethodBuilder_t
-# include <hpp/core/steering-method.hh>
+#define HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
 
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/graph/fwd.hh>
-# include <hpp/manipulation/steering-method/fwd.hh>
+#include <hpp/core/problem-solver.hh>  // SteeringMethodBuilder_t
+#include <hpp/core/steering-method.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
+#include <hpp/manipulation/steering-method/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup steering_method
-    /// \{
-    class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod
-    {
-      public:
-        const core::SteeringMethodPtr_t& innerSteeringMethod () const
-        {
-          return steeringMethod_;
-        }
-
-        void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
-        {
-          steeringMethod_ = sm;
-        }
-
-      protected:
-        /// Constructor
-        SteeringMethod (const ProblemConstPtr_t& problem);
-
-        /// Copy constructor
-        SteeringMethod (const SteeringMethod& other);
-
-        void init (SteeringMethodWkPtr_t weak)
-        {
-          core::SteeringMethod::init (weak);
-        }
-
-        /// A pointer to the manipulation problem
-        ProblemConstPtr_t problem_;
-        /// The encapsulated steering method
-        core::SteeringMethodPtr_t steeringMethod_;
-    };
-
-    namespace steeringMethod {
-      using core::PathPtr_t;
-
-      class HPP_MANIPULATION_DLLAPI Graph : public SteeringMethod
-      {
-        typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
-
-        public:
-          /// Create instance and return shared pointer
-          /// \warning core::Problem will be casted to Problem
-          static GraphPtr_t create
-            (const core::ProblemConstPtr_t& problem);
-
-          template <typename T>
-            static GraphPtr_t create
-            (const core::ProblemConstPtr_t& problem);
-
-          /// Create copy and return shared pointer
-          static GraphPtr_t createCopy
-            (const GraphPtr_t& other);
-
-          /// Copy instance and return shared pointer
-          virtual core::SteeringMethodPtr_t copy () const
-          {
-            return createCopy (weak_.lock ());
-          }
-
-        protected:
-          /// Constructor
-          Graph (const ProblemConstPtr_t& problem);
-
-          /// Copy constructor
-          Graph (const Graph&);
-
-          virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
-          void init (GraphWkPtr_t weak)
-          {
-            SteeringMethod::init (weak);
-            weak_ = weak;
-          }
-
-        private:
-          /// Weak pointer to itself
-          GraphWkPtr_t weak_;
-      };
-
-      template <typename T>
-        GraphPtr_t Graph::create
-        (const core::ProblemConstPtr_t& problem)
-      {
-        GraphPtr_t gsm = Graph::create (problem);
-        gsm->innerSteeringMethod (T::create (problem));
-        return gsm;
-      }
-    } // namespace steeringMethod
-    /// \}
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
+namespace manipulation {
+/// \addtogroup steering_method
+/// \{
+class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod {
+ public:
+  const core::SteeringMethodPtr_t& innerSteeringMethod() const {
+    return steeringMethod_;
+  }
+
+  void innerSteeringMethod(const core::SteeringMethodPtr_t& sm) {
+    steeringMethod_ = sm;
+  }
+
+ protected:
+  /// Constructor
+  SteeringMethod(const ProblemConstPtr_t& problem);
+
+  /// Copy constructor
+  SteeringMethod(const SteeringMethod& other);
+
+  void init(SteeringMethodWkPtr_t weak) { core::SteeringMethod::init(weak); }
+
+  /// A pointer to the manipulation problem
+  ProblemConstPtr_t problem_;
+  /// The encapsulated steering method
+  core::SteeringMethodPtr_t steeringMethod_;
+};
+
+namespace steeringMethod {
+using core::PathPtr_t;
+
+class HPP_MANIPULATION_DLLAPI Graph : public SteeringMethod {
+  typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
+
+ public:
+  /// Create instance and return shared pointer
+  /// \warning core::Problem will be casted to Problem
+  static GraphPtr_t create(const core::ProblemConstPtr_t& problem);
+
+  template <typename T>
+  static GraphPtr_t create(const core::ProblemConstPtr_t& problem);
+
+  /// Create copy and return shared pointer
+  static GraphPtr_t createCopy(const GraphPtr_t& other);
+
+  /// Copy instance and return shared pointer
+  virtual core::SteeringMethodPtr_t copy() const {
+    return createCopy(weak_.lock());
+  }
+
+ protected:
+  /// Constructor
+  Graph(const ProblemConstPtr_t& problem);
+
+  /// Copy constructor
+  Graph(const Graph&);
+
+  virtual PathPtr_t impl_compute(ConfigurationIn_t q1,
+                                 ConfigurationIn_t q2) const;
+
+  void init(GraphWkPtr_t weak) {
+    SteeringMethod::init(weak);
+    weak_ = weak;
+  }
+
+ private:
+  /// Weak pointer to itself
+  GraphWkPtr_t weak_;
+};
+
+template <typename T>
+GraphPtr_t Graph::create(const core::ProblemConstPtr_t& problem) {
+  GraphPtr_t gsm = Graph::create(problem);
+  gsm->innerSteeringMethod(T::create(problem));
+  return gsm;
+}
+}  // namespace steeringMethod
+/// \}
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
diff --git a/include/hpp/manipulation/weighed-distance.hh b/include/hpp/manipulation/weighed-distance.hh
index da8ebce82ee3ca38e9eec76882897e684b9e89b3..fa4e8b110e7fee21582fef1b37f58ddc33f8eb82 100644
--- a/include/hpp/manipulation/weighed-distance.hh
+++ b/include/hpp/manipulation/weighed-distance.hh
@@ -27,64 +27,55 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_DISTANCE_HH
-# define HPP_MANIPULATION_DISTANCE_HH
+#define HPP_MANIPULATION_DISTANCE_HH
 
-# include <hpp/core/weighed-distance.hh>
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/graph/fwd.hh>
+#include <hpp/core/weighed-distance.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
-  namespace manipulation {
-    /// \addtogroup steering_method
-    /// \{
+namespace manipulation {
+/// \addtogroup steering_method
+/// \{
 
-    /// Class for distance between configurations
-    class HPP_MANIPULATION_DLLAPI WeighedDistance : public core::WeighedDistance
-    {
-    public:
-      static WeighedDistancePtr_t create (const DevicePtr_t& robot,
-          const graph::GraphPtr_t& graph);
+/// Class for distance between configurations
+class HPP_MANIPULATION_DLLAPI WeighedDistance : public core::WeighedDistance {
+ public:
+  static WeighedDistancePtr_t create(const DevicePtr_t& robot,
+                                     const graph::GraphPtr_t& graph);
 
-      static WeighedDistancePtr_t createCopy
-	(const WeighedDistancePtr_t& distance);
+  static WeighedDistancePtr_t createCopy(const WeighedDistancePtr_t& distance);
 
-      virtual core::DistancePtr_t clone () const;
+  virtual core::DistancePtr_t clone() const;
 
-      /// Set the graph of constraints
-      void constraintGraph (const graph::GraphPtr_t& graph)
-      {
-        graph_ = graph;
-      }
+  /// Set the graph of constraints
+  void constraintGraph(const graph::GraphPtr_t& graph) { graph_ = graph; }
 
-      /// Get the graph of constraints
-      graph::GraphPtr_t constraintGraph () const
-      {
-        return graph_;
-      }
+  /// Get the graph of constraints
+  graph::GraphPtr_t constraintGraph() const { return graph_; }
 
-    protected:
-      WeighedDistance (const DevicePtr_t& robot, const graph::GraphPtr_t graph);
+ protected:
+  WeighedDistance(const DevicePtr_t& robot, const graph::GraphPtr_t graph);
 
-      WeighedDistance (const WeighedDistance& distance);
+  WeighedDistance(const WeighedDistance& distance);
 
-      /// Derived class should implement this function
-      virtual value_type impl_distance (
-          ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-      virtual value_type impl_distance (
-          core::NodePtr_t n1, core::NodePtr_t n2) const;
+  /// Derived class should implement this function
+  virtual value_type impl_distance(ConfigurationIn_t q1,
+                                   ConfigurationIn_t q2) const;
+  virtual value_type impl_distance(core::NodePtr_t n1,
+                                   core::NodePtr_t n2) const;
 
-      void init (WeighedDistanceWkPtr_t self);
+  void init(WeighedDistanceWkPtr_t self);
 
-    private:
-      graph::GraphPtr_t graph_;
-      WeighedDistanceWkPtr_t weak_;
+ private:
+  graph::GraphPtr_t graph_;
+  WeighedDistanceWkPtr_t weak_;
 
-      WeighedDistance() {}
-      HPP_SERIALIZABLE();
-    }; // class Distance
-    /// \}
-  } //   namespace manipulation
-} // namespace hpp
-#endif // HPP_MANIPULATION_DISTANCE_HH
+  WeighedDistance() {}
+  HPP_SERIALIZABLE();
+};  // class Distance
+/// \}
+}  //   namespace manipulation
+}  // namespace hpp
+#endif  // HPP_MANIPULATION_DISTANCE_HH
diff --git a/plugins/end-effector-trajectory.cc b/plugins/end-effector-trajectory.cc
index 77ebb0e87172083861787cda5b7563749858319c..66e1d8cb6649a450513810c9b752a980c75734e7 100644
--- a/plugins/end-effector-trajectory.cc
+++ b/plugins/end-effector-trajectory.cc
@@ -28,30 +28,26 @@
 
 #include <hpp/core/plugin.hh>
 #include <hpp/core/problem-solver.hh>
-
-#include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
 #include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
+#include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
 
 namespace hpp {
-  namespace manipulation {
-    class EndEffectorTrajectoryPlugin : public core::ProblemSolverPlugin
-    {
-      public:
-        EndEffectorTrajectoryPlugin ()
-          : ProblemSolverPlugin ("EndEffectorTrajectoryPlugin", "0.0")
-        {}
+namespace manipulation {
+class EndEffectorTrajectoryPlugin : public core::ProblemSolverPlugin {
+ public:
+  EndEffectorTrajectoryPlugin()
+      : ProblemSolverPlugin("EndEffectorTrajectoryPlugin", "0.0") {}
 
-      protected:
-        virtual bool impl_initialize (core::ProblemSolverPtr_t ps)
-        {
-          ps->pathPlanners.add ("EndEffectorTrajectory",
-              pathPlanner::EndEffectorTrajectory::createWithRoadmap);
-          ps->steeringMethods.add ("EndEffectorTrajectory",
-              steeringMethod::EndEffectorTrajectory::create);
-          return true;
-        }
-    };
-  } // namespace manipulation
-} // namespace hpp
+ protected:
+  virtual bool impl_initialize(core::ProblemSolverPtr_t ps) {
+    ps->pathPlanners.add("EndEffectorTrajectory",
+                         pathPlanner::EndEffectorTrajectory::createWithRoadmap);
+    ps->steeringMethods.add("EndEffectorTrajectory",
+                            steeringMethod::EndEffectorTrajectory::create);
+    return true;
+  }
+};
+}  // namespace manipulation
+}  // namespace hpp
 
 HPP_CORE_DEFINE_PLUGIN(hpp::manipulation::EndEffectorTrajectoryPlugin)
diff --git a/plugins/spline-gradient-based.cc b/plugins/spline-gradient-based.cc
index d7e114b158cd1bad32e1338d95135c80aebbb4f7..5ba32c350068db807ecda35724e7f60d40b515ed 100644
--- a/plugins/spline-gradient-based.cc
+++ b/plugins/spline-gradient-based.cc
@@ -28,32 +28,40 @@
 
 #include <hpp/core/plugin.hh>
 #include <hpp/core/problem-solver.hh>
-
 #include <hpp/manipulation/path-optimization/spline-gradient-based.hh>
 
 namespace hpp {
-  namespace manipulation {
-    class SplineGradientBasedPlugin : public core::ProblemSolverPlugin
-    {
-      public:
-        SplineGradientBasedPlugin ()
-          : ProblemSolverPlugin ("SplineGradientBasedPlugin", "0.0")
-        {}
+namespace manipulation {
+class SplineGradientBasedPlugin : public core::ProblemSolverPlugin {
+ public:
+  SplineGradientBasedPlugin()
+      : ProblemSolverPlugin("SplineGradientBasedPlugin", "0.0") {}
 
-      protected:
-        virtual bool impl_initialize (core::ProblemSolverPtr_t ps)
-        {
-          // ps->pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
-          // ps->pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
-          // ps->pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
-          ps->pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore);
-          // ps->pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
-          ps->pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
+ protected:
+  virtual bool impl_initialize(core::ProblemSolverPtr_t ps) {
+    // ps->pathOptimizers.add
+    // ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis,
+    // 1>::createFromCore); ps->pathOptimizers.add
+    // ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis,
+    // 2>::createFromCore); ps->pathOptimizers.add
+    // ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis,
+    // 3>::createFromCore);
+    ps->pathOptimizers.add(
+        "SplineGradientBased_bezier1",
+        pathOptimization::SplineGradientBased<core::path::BernsteinBasis,
+                                              1>::createFromCore);
+    // ps->pathOptimizers.add
+    // ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis,
+    // 2>::createFromCore);
+    ps->pathOptimizers.add(
+        "SplineGradientBased_bezier3",
+        pathOptimization::SplineGradientBased<core::path::BernsteinBasis,
+                                              3>::createFromCore);
 
-          return true;
-        }
-    };
-  } // namespace manipulation
-} // namespace hpp
+    return true;
+  }
+};
+}  // namespace manipulation
+}  // namespace hpp
 
 HPP_CORE_DEFINE_PLUGIN(hpp::manipulation::SplineGradientBasedPlugin)
diff --git a/src/astar.hh b/src/astar.hh
index ea6f32199dbba64118c11a9d7493f5e96d46fc16..a680f7f3ab61ba42180b17e477975eac0c7a4b5c 100644
--- a/src/astar.hh
+++ b/src/astar.hh
@@ -28,152 +28,141 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_ASTAR_HH
-# define HPP_MANIPULATION_ASTAR_HH
-
-# include <limits>
-# include <hpp/core/distance.hh>
-# include <hpp/core/node.hh>
-# include <hpp/core/edge.hh>
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/roadmap-node.hh>
-# include <hpp/manipulation/graph/state-selector.hh>
+#define HPP_MANIPULATION_ASTAR_HH
+
+#include <hpp/core/distance.hh>
+#include <hpp/core/edge.hh>
+#include <hpp/core/node.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/state-selector.hh>
+#include <hpp/manipulation/roadmap-node.hh>
+#include <limits>
 //# include <hpp/core/path-vector.hh>
 
 namespace hpp {
-  namespace manipulation {
-    class Astar
-    {
-    public:
-      typedef std::map <RoadmapNodePtr_t, value_type> CostMap_t;
-      struct CostMapCompFunctor {
-	CostMap_t& cost_;
-	CostMapCompFunctor (CostMap_t& cost) : cost_ (cost) {}
-	bool operator () (const RoadmapNodePtr_t& n1, const RoadmapNodePtr_t& n2)
-	{ return cost_ [n1] < cost_ [n2]; }
-	bool operator () (const RoadmapNodePtr_t& n1, const value_type& val)
-	{ return cost_ [n1] < val; }
-      }; // struc CostMapCompFunctor
-
-      typedef std::list <graph::StatePtr_t> States_t;
-      typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t;
-      typedef std::list <core::EdgePtr_t> RoadmapEdges_t;
-      typedef std::map <RoadmapNodePtr_t, core::EdgePtr_t> Parent_t;
-
-      Astar (const core::DistancePtr_t distance,
-          const graph::StateSelectorPtr_t& stateSelector, RoadmapNodePtr_t from) :
-	distance_ (distance), selector_ (stateSelector),
-        from_ (from)
-      {
-        open_.push_back (from);
-        costFromStart_ [from] = 0;
+namespace manipulation {
+class Astar {
+ public:
+  typedef std::map<RoadmapNodePtr_t, value_type> CostMap_t;
+  struct CostMapCompFunctor {
+    CostMap_t& cost_;
+    CostMapCompFunctor(CostMap_t& cost) : cost_(cost) {}
+    bool operator()(const RoadmapNodePtr_t& n1, const RoadmapNodePtr_t& n2) {
+      return cost_[n1] < cost_[n2];
+    }
+    bool operator()(const RoadmapNodePtr_t& n1, const value_type& val) {
+      return cost_[n1] < val;
+    }
+  };  // struc CostMapCompFunctor
+
+  typedef std::list<graph::StatePtr_t> States_t;
+  typedef std::list<RoadmapNodePtr_t> RoadmapNodes_t;
+  typedef std::list<core::EdgePtr_t> RoadmapEdges_t;
+  typedef std::map<RoadmapNodePtr_t, core::EdgePtr_t> Parent_t;
+
+  Astar(const core::DistancePtr_t distance,
+        const graph::StateSelectorPtr_t& stateSelector, RoadmapNodePtr_t from)
+      : distance_(distance), selector_(stateSelector), from_(from) {
+    open_.push_back(from);
+    costFromStart_[from] = 0;
+  }
+
+  States_t solution(RoadmapNodePtr_t to) {
+    if (parent_.find(to) != parent_.end() || findPath(to)) {
+      RoadmapNodePtr_t node = to;
+      States_t states;
+
+      states.push_front(selector_->getState(to));
+      while (node) {
+        Parent_t::const_iterator itNode = parent_.find(node);
+        if (itNode != parent_.end()) {
+          node = static_cast<RoadmapNodePtr_t>(itNode->second->from());
+          states.push_front(selector_->getState(node));
+        } else
+          node = RoadmapNodePtr_t(0);
       }
-
-      States_t solution (RoadmapNodePtr_t to)
-      {
-	if (parent_.find (to) != parent_.end () ||
-            findPath (to))
-        {
-          RoadmapNodePtr_t node = to;
-          States_t states;
-
-          states.push_front (selector_->getState (to));
-          while (node) {
-            Parent_t::const_iterator itNode = parent_.find (node);
-            if (itNode != parent_.end ()) {
-              node = static_cast <RoadmapNodePtr_t> (itNode->second->from ());
-              states.push_front (selector_->getState (node));
+      // We may want to clean it a little
+      // std::unique (states.begin(), states.end ());
+
+      states.push_front(selector_->getState(from_));
+      return states;
+    }
+    return States_t();
+  }
+
+ private:
+  bool findPath(RoadmapNodePtr_t to) {
+    // Recompute the estimated cost to goal
+    for (CostMap_t::iterator it = estimatedCostToGoal_.begin();
+         it != estimatedCostToGoal_.end(); ++it) {
+      it->second = getCostFromStart(it->first) + heuristic(it->first, to);
+    }
+    open_.sort(CostMapCompFunctor(estimatedCostToGoal_));
+
+    while (!open_.empty()) {
+      RoadmapNodes_t::iterator itv = open_.begin();
+      RoadmapNodePtr_t current(*itv);
+      if (current == to) {
+        return true;
+      }
+      open_.erase(itv);
+      closed_.push_back(current);
+      for (RoadmapEdges_t::const_iterator itEdge = current->outEdges().begin();
+           itEdge != current->outEdges().end(); ++itEdge) {
+        RoadmapNodePtr_t child = static_cast<RoadmapNodePtr_t>((*itEdge)->to());
+        if (std::find(closed_.begin(), closed_.end(), child) == closed_.end()) {
+          // node is not in closed set
+          value_type transitionCost = edgeCost(*itEdge);
+          value_type tmpCost = getCostFromStart(current) + transitionCost;
+          bool childNotInOpenSet =
+              (std::find(open_.begin(), open_.end(), child) == open_.end());
+          if ((childNotInOpenSet) || (tmpCost < getCostFromStart(child))) {
+            parent_[child] = *itEdge;
+            costFromStart_[child] = tmpCost;
+            value_type estimatedCost = tmpCost + heuristic(child, to);
+            estimatedCostToGoal_[child] = estimatedCost;
+            if (childNotInOpenSet) {
+              // Find the first element not strictly smaller than child
+              RoadmapNodes_t::iterator pos =
+                  std::lower_bound(open_.begin(), open_.end(), estimatedCost,
+                                   CostMapCompFunctor(estimatedCostToGoal_));
+              open_.insert(pos, child);
             }
-            else node = RoadmapNodePtr_t (0);
           }
-          // We may want to clean it a little
-          // std::unique (states.begin(), states.end ());
-
-          states.push_front (selector_->getState (from_));
-          return states;
         }
-        return States_t();
-      }
-
-    private:
-      bool findPath (RoadmapNodePtr_t to)
-      {
-        // Recompute the estimated cost to goal
-        for (CostMap_t::iterator it = estimatedCostToGoal_.begin ();
-            it != estimatedCostToGoal_.end (); ++it) {
-          it->second = getCostFromStart (it->first) + heuristic (it->first, to);
-        }
-        open_.sort (CostMapCompFunctor (estimatedCostToGoal_));
-
-	while (!open_.empty ()) {
-	  RoadmapNodes_t::iterator itv = open_.begin ();
-          RoadmapNodePtr_t current (*itv);
-	  if (current == to) {
-	    return true;
-	  }
-	  open_.erase (itv);
-	  closed_.push_back (current);
-	  for (RoadmapEdges_t::const_iterator itEdge = current->outEdges ().begin ();
-	       itEdge != current->outEdges ().end (); ++itEdge) {
-            RoadmapNodePtr_t child = static_cast <RoadmapNodePtr_t> ((*itEdge)->to ());
-	    if (std::find (closed_.begin(), closed_.end(),
-                  child) == closed_.end ()) {
-	      // node is not in closed set
-              value_type transitionCost = edgeCost (*itEdge);
-              value_type tmpCost = getCostFromStart (current) + transitionCost;
-	      bool childNotInOpenSet = (std::find (open_.begin (),
-						   open_.end (),
-						   child) == open_.end ());
-	      if ((childNotInOpenSet) || (tmpCost < getCostFromStart (child))) {
-		parent_ [child] = *itEdge;
-		costFromStart_ [child] = tmpCost;
-                value_type estimatedCost = tmpCost + heuristic (child, to);
-                estimatedCostToGoal_ [child] = estimatedCost;
-                if (childNotInOpenSet) {
-                  // Find the first element not strictly smaller than child
-                  RoadmapNodes_t::iterator pos =
-                    std::lower_bound (open_.begin (), open_.end (),
-                        estimatedCost, CostMapCompFunctor (estimatedCostToGoal_));
-                  open_.insert (pos, child);
-                }
-	      }
-	    }
-	  }
-	}
-        return false;
-      }
-
-      inline value_type heuristic (RoadmapNodePtr_t node, RoadmapNodePtr_t to) const
-      {
-	const ConfigurationPtr_t& config = node->configuration ();
-	return (*distance_) (*config, *to->configuration ());
-      }
-
-      inline value_type edgeCost (const core::EdgePtr_t& edge) const
-      {
-	return edge->path ()->length ();
-      }
-
-      value_type getCostFromStart (RoadmapNodePtr_t to) const
-      {
-        CostMap_t::const_iterator it = costFromStart_.find (to);
-        if (it == costFromStart_.end())
-          return std::numeric_limits <value_type>::max();
-        return it->second;
       }
-
-      RoadmapNodes_t closed_;
-      RoadmapNodes_t open_;
-      std::map <RoadmapNodePtr_t, value_type> costFromStart_;
-      std::map <RoadmapNodePtr_t, value_type> estimatedCostToGoal_;
-      Parent_t parent_;
-      core::DistancePtr_t distance_;
-      graph::StateSelectorPtr_t selector_;
-      RoadmapNodePtr_t from_;
-
-    }; // class Astar
-  } // namespace manipulation
-} // namespace hpp
-
-
-#endif // HPP_MANIPULATION_ASTAR_HH
+    }
+    return false;
+  }
+
+  inline value_type heuristic(RoadmapNodePtr_t node,
+                              RoadmapNodePtr_t to) const {
+    const ConfigurationPtr_t& config = node->configuration();
+    return (*distance_)(*config, *to->configuration());
+  }
+
+  inline value_type edgeCost(const core::EdgePtr_t& edge) const {
+    return edge->path()->length();
+  }
+
+  value_type getCostFromStart(RoadmapNodePtr_t to) const {
+    CostMap_t::const_iterator it = costFromStart_.find(to);
+    if (it == costFromStart_.end())
+      return std::numeric_limits<value_type>::max();
+    return it->second;
+  }
+
+  RoadmapNodes_t closed_;
+  RoadmapNodes_t open_;
+  std::map<RoadmapNodePtr_t, value_type> costFromStart_;
+  std::map<RoadmapNodePtr_t, value_type> estimatedCostToGoal_;
+  Parent_t parent_;
+  core::DistancePtr_t distance_;
+  graph::StateSelectorPtr_t selector_;
+  RoadmapNodePtr_t from_;
+
+};  // class Astar
+}  // namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_ASTAR_HH
diff --git a/src/connected-component.cc b/src/connected-component.cc
index fe8f1e790debf648c29c3607fa4f5920946ed1d5..8efe8444c36ce2788f75efd292e01d42531fbfe9 100644
--- a/src/connected-component.cc
+++ b/src/connected-component.cc
@@ -29,86 +29,84 @@
 
 #include <hpp/manipulation/connected-component.hh>
 
-#include "hpp/manipulation/roadmap.hh"
 #include "hpp/manipulation/roadmap-node.hh"
+#include "hpp/manipulation/roadmap.hh"
 
 namespace hpp {
-  namespace manipulation {
-    RoadmapNodes_t ConnectedComponent::empty_ = RoadmapNodes_t();
+namespace manipulation {
+RoadmapNodes_t ConnectedComponent::empty_ = RoadmapNodes_t();
 
-    bool ConnectedComponent::check () const
-    {
-      std::set <core::NodePtr_t> s1;
-      for (core::NodeVector_t::const_iterator it = nodes ().begin ();
-	   it != nodes ().end (); ++it) {
-	s1.insert (*it);
-      }
-      std::set <core::NodePtr_t> s2;
-      for (GraphStates_t::const_iterator it = graphStateMap_.begin();
-	   it != graphStateMap_.end(); ++it ) {
-	for (RoadmapNodes_t::const_iterator itNodes = it->second.begin ();
-	     itNodes != it->second.end (); ++itNodes) {
-	  s2.insert (*itNodes);
-	}
-      }
-      if (s1.size () == 0) return false;
-      if (s1 == s2) return true;
-      return false;
+bool ConnectedComponent::check() const {
+  std::set<core::NodePtr_t> s1;
+  for (core::NodeVector_t::const_iterator it = nodes().begin();
+       it != nodes().end(); ++it) {
+    s1.insert(*it);
+  }
+  std::set<core::NodePtr_t> s2;
+  for (GraphStates_t::const_iterator it = graphStateMap_.begin();
+       it != graphStateMap_.end(); ++it) {
+    for (RoadmapNodes_t::const_iterator itNodes = it->second.begin();
+         itNodes != it->second.end(); ++itNodes) {
+      s2.insert(*itNodes);
     }
+  }
+  if (s1.size() == 0) return false;
+  if (s1 == s2) return true;
+  return false;
+}
 
-    ConnectedComponentPtr_t ConnectedComponent::create(const RoadmapWkPtr_t& roadmap)
-    {
-      ConnectedComponent* ptr = new ConnectedComponent ();
-      ConnectedComponentPtr_t shPtr (ptr);
-      // calls init function in core::ConnectedComponent that saves 
-      // shPtr into the class variable weak_ (weak pointer). Reimplement?
-      ptr->init (shPtr);
-      shPtr->roadmap_ = roadmap.lock();
-      return shPtr;
-    }
+ConnectedComponentPtr_t ConnectedComponent::create(
+    const RoadmapWkPtr_t& roadmap) {
+  ConnectedComponent* ptr = new ConnectedComponent();
+  ConnectedComponentPtr_t shPtr(ptr);
+  // calls init function in core::ConnectedComponent that saves
+  // shPtr into the class variable weak_ (weak pointer). Reimplement?
+  ptr->init(shPtr);
+  shPtr->roadmap_ = roadmap.lock();
+  return shPtr;
+}
 
-    void ConnectedComponent::merge (const core::ConnectedComponentPtr_t& otherCC)
-    {
-      core::ConnectedComponent::merge(otherCC);
-      const ConnectedComponentPtr_t other = static_pointer_cast <ConnectedComponent> (otherCC);
-      /// take all graph states in other->graphStateMap_ and put them in this->graphStateMap_
-      /// if they already exist in this->graphStateMap_, append roadmap nodes from other graph state
-      /// to graph state in this. 
-      for (GraphStates_t::iterator otherIt = other->graphStateMap_.begin(); 
-	otherIt != other->graphStateMap_.end(); otherIt++)
-      {
-	// find other graph state in this-graphStateMap_ -> merge their roadmap nodes
-	GraphStates_t::iterator mapIt = this->graphStateMap_.find(otherIt->first);
-	if (mapIt != this->graphStateMap_.end())	{
-	  mapIt->second.insert(mapIt->second.end(), otherIt->second.begin(), otherIt->second.end());
-	} else {
-	  this->graphStateMap_.insert(*otherIt);
-	}
-      }
-      other->graphStateMap_.clear();
-      assert (check ());
-    } 
-
-    void ConnectedComponent::addNode(const core::NodePtr_t& node)      
-    {
-      core::ConnectedComponent::addNode(node);
-      // Find right graph state in map and add roadmap node to corresponding vector
-      const RoadmapNodePtr_t& n = static_cast <const RoadmapNodePtr_t> (node);
-      RoadmapPtr_t roadmap = roadmap_.lock();
-      if (!roadmap) throw std::logic_error("The roadmap of this ConnectedComponent as been deleted.");
-      graphStateMap_[roadmap->getState(n)].push_back(n);
-      assert (check ());
+void ConnectedComponent::merge(const core::ConnectedComponentPtr_t& otherCC) {
+  core::ConnectedComponent::merge(otherCC);
+  const ConnectedComponentPtr_t other =
+      static_pointer_cast<ConnectedComponent>(otherCC);
+  /// take all graph states in other->graphStateMap_ and put them in
+  /// this->graphStateMap_ if they already exist in this->graphStateMap_, append
+  /// roadmap nodes from other graph state to graph state in this.
+  for (GraphStates_t::iterator otherIt = other->graphStateMap_.begin();
+       otherIt != other->graphStateMap_.end(); otherIt++) {
+    // find other graph state in this-graphStateMap_ -> merge their roadmap
+    // nodes
+    GraphStates_t::iterator mapIt = this->graphStateMap_.find(otherIt->first);
+    if (mapIt != this->graphStateMap_.end()) {
+      mapIt->second.insert(mapIt->second.end(), otherIt->second.begin(),
+                           otherIt->second.end());
+    } else {
+      this->graphStateMap_.insert(*otherIt);
     }
+  }
+  other->graphStateMap_.clear();
+  assert(check());
+}
 
-    const RoadmapNodes_t& ConnectedComponent::getRoadmapNodes (
-        const graph::StatePtr_t graphState) const
-    {
-      GraphStates_t::const_iterator mapIt = graphStateMap_.find(graphState);
-      if (mapIt != graphStateMap_.end())
-        return mapIt->second;
-      return empty_;
-    }
+void ConnectedComponent::addNode(const core::NodePtr_t& node) {
+  core::ConnectedComponent::addNode(node);
+  // Find right graph state in map and add roadmap node to corresponding vector
+  const RoadmapNodePtr_t& n = static_cast<const RoadmapNodePtr_t>(node);
+  RoadmapPtr_t roadmap = roadmap_.lock();
+  if (!roadmap)
+    throw std::logic_error(
+        "The roadmap of this ConnectedComponent as been deleted.");
+  graphStateMap_[roadmap->getState(n)].push_back(n);
+  assert(check());
+}
 
-  } // namespace manipulation
-} // namespace hpp
+const RoadmapNodes_t& ConnectedComponent::getRoadmapNodes(
+    const graph::StatePtr_t graphState) const {
+  GraphStates_t::const_iterator mapIt = graphStateMap_.find(graphState);
+  if (mapIt != graphStateMap_.end()) return mapIt->second;
+  return empty_;
+}
 
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/constraint-set.cc b/src/constraint-set.cc
index bc6686cb7c4bb9b26e5acdfc0be91fedae667bd2..7192b770e69ec89fc60230887c7efc82dcb9b984 100644
--- a/src/constraint-set.cc
+++ b/src/constraint-set.cc
@@ -35,74 +35,56 @@
 #include "hpp/manipulation/serialization.hh"
 
 namespace hpp {
-  namespace manipulation {
-    ConstraintSetPtr_t ConstraintSet::create
-      (const DevicePtr_t& robot, const std::string& name)
-      {
-        ConstraintSet* ptr = new ConstraintSet (robot, name);
-        ConstraintSetPtr_t shPtr (ptr);
-        ptr->init (shPtr);
-        return shPtr;
-      }
-
-    ConstraintSetPtr_t ConstraintSet::createCopy (const ConstraintSetPtr_t& cs)
-    {
-      ConstraintSet* ptr = new ConstraintSet (*cs);
-      ConstraintSetPtr_t shPtr (ptr);
-      ptr->init (shPtr);
-      return shPtr;
-    }
-
-    ConstraintPtr_t ConstraintSet::copy () const
-    {
-      return createCopy (weak_.lock ());
-    }
-
-    void ConstraintSet::edge (graph::EdgePtr_t edge)
-    {
-      edge_ = edge;
-    }
-
-    graph::EdgePtr_t ConstraintSet::edge () const
-    {
-      return edge_;
-    }
-
-    ConstraintSet::ConstraintSet (const DevicePtr_t& robot, const std::string& name) :
-      Parent_t (robot, name),
-      edge_ ()
-    {}
-
-    ConstraintSet::ConstraintSet (const ConstraintSet& other) :
-      Parent_t (other),
-      edge_ (other.edge ())
-    {}
-
-    void ConstraintSet::init (const ConstraintSetPtr_t& self)
-    {
-      Parent_t::init (self);
-      weak_ = self;
-    }
-
-    std::ostream& ConstraintSet::print (std::ostream& os) const
-    {
-      Parent_t::print (os);
-      if (edge_) os << iendl << "Built by edge " << edge_->name();
-      return os;
-    }
-
-    template<class Archive>
-    void ConstraintSet::serialize(Archive & ar, const unsigned int version)
-    {
-      using namespace boost::serialization;
-      (void) version;
-      ar & make_nvp("base", base_object<core::ConstraintSet>(*this));
-      ar & BOOST_SERIALIZATION_NVP(edge_);
-      ar & BOOST_SERIALIZATION_NVP(weak_);
-    }
-
-    HPP_SERIALIZATION_IMPLEMENT(ConstraintSet);
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+ConstraintSetPtr_t ConstraintSet::create(const DevicePtr_t& robot,
+                                         const std::string& name) {
+  ConstraintSet* ptr = new ConstraintSet(robot, name);
+  ConstraintSetPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+ConstraintSetPtr_t ConstraintSet::createCopy(const ConstraintSetPtr_t& cs) {
+  ConstraintSet* ptr = new ConstraintSet(*cs);
+  ConstraintSetPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+ConstraintPtr_t ConstraintSet::copy() const { return createCopy(weak_.lock()); }
+
+void ConstraintSet::edge(graph::EdgePtr_t edge) { edge_ = edge; }
+
+graph::EdgePtr_t ConstraintSet::edge() const { return edge_; }
+
+ConstraintSet::ConstraintSet(const DevicePtr_t& robot, const std::string& name)
+    : Parent_t(robot, name), edge_() {}
+
+ConstraintSet::ConstraintSet(const ConstraintSet& other)
+    : Parent_t(other), edge_(other.edge()) {}
+
+void ConstraintSet::init(const ConstraintSetPtr_t& self) {
+  Parent_t::init(self);
+  weak_ = self;
+}
+
+std::ostream& ConstraintSet::print(std::ostream& os) const {
+  Parent_t::print(os);
+  if (edge_) os << iendl << "Built by edge " << edge_->name();
+  return os;
+}
+
+template <class Archive>
+void ConstraintSet::serialize(Archive& ar, const unsigned int version) {
+  using namespace boost::serialization;
+  (void)version;
+  ar& make_nvp("base", base_object<core::ConstraintSet>(*this));
+  ar& BOOST_SERIALIZATION_NVP(edge_);
+  ar& BOOST_SERIALIZATION_NVP(weak_);
+}
+
+HPP_SERIALIZATION_IMPLEMENT(ConstraintSet);
+}  // namespace manipulation
+}  // namespace hpp
 
 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::manipulation::ConstraintSet)
diff --git a/src/device.cc b/src/device.cc
index 0568521eda09b82399e46198de402228ad603a96..9d203f6fc57f2900cd920475218d8aba7d711293 100644
--- a/src/device.cc
+++ b/src/device.cc
@@ -28,160 +28,146 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/device.hh>
-
 #include <boost/serialization/weak_ptr.hpp>
-
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/multibody/geometry.hpp>
-
-#include <hpp/util/serialization.hh>
-
-#include <hpp/pinocchio/joint.hh>
-#include <hpp/pinocchio/joint-collection.hh>
-#include <hpp/pinocchio/gripper.hh>
-
+#include <hpp/manipulation/device.hh>
 #include <hpp/manipulation/handle.hh>
 #include <hpp/manipulation/serialization.hh>
+#include <hpp/pinocchio/gripper.hh>
+#include <hpp/pinocchio/joint-collection.hh>
+#include <hpp/pinocchio/joint.hh>
+#include <hpp/util/serialization.hh>
+#include <pinocchio/multibody/geometry.hpp>
+#include <pinocchio/multibody/model.hpp>
 
 namespace hpp {
-  namespace manipulation {
-    using ::pinocchio::Frame;
-
-    pinocchio::DevicePtr_t Device::clone () const
-    {
-      Device* ptr = new Device(*this);
-      DevicePtr_t shPtr (ptr);
-      ptr->initCopy (shPtr, *this);
-      return shPtr;
-    }
-
-    void Device::setRobotRootPosition (const std::string& rn,
-        const Transform3f& t)
-    {
-      FrameIndices_t idxs = robotFrames (rn);
-      if (idxs.size() == 0)
-        throw std::invalid_argument ("No frame for robot name " + rn);
-
-      pinocchio::Model& m = model();
-      pinocchio::GeomModel& gm = geomModel();
-      // The root frame should be the first frame.
-      Frame& rootFrame = m.frames[idxs[0]];
-      if (rootFrame.type == ::pinocchio::JOINT) {
-        JointIndex jid = m.getJointId (rootFrame.name);
-        m.jointPlacements[jid] = t;
-        return;
-      }
-
-      Transform3f shift (t * rootFrame.placement.inverse());
-      // Find all the frames that have the same parent joint.
-      for (std::size_t i = 1; i < idxs.size(); ++i) {
-        Frame& frame = m.frames[idxs[i]];
-        if (frame.parent == rootFrame.parent) {
-          // frame is between rootFrame and next moving joints.
-          frame.placement = shift * frame.placement;
-          if (frame.type == ::pinocchio::BODY) {
-            // Update the geometry object placement.
-            for (std::size_t k = 0; k < gm.geometryObjects.size(); ++k)
-            {
-              ::pinocchio::GeometryObject& go = gm.geometryObjects[k];
-              if (go.parentFrame == idxs[i])
-                go.placement = shift * go.placement;
-            }
-          }
-        } else if ((frame.type == ::pinocchio::JOINT) && (rootFrame.parent == m.parents[frame.parent])) {
-          // frame corresponds to a child joint of rootFrame.parent
-          m.jointPlacements[frame.parent] = shift * m.jointPlacements[frame.parent];
+namespace manipulation {
+using ::pinocchio::Frame;
+
+pinocchio::DevicePtr_t Device::clone() const {
+  Device* ptr = new Device(*this);
+  DevicePtr_t shPtr(ptr);
+  ptr->initCopy(shPtr, *this);
+  return shPtr;
+}
+
+void Device::setRobotRootPosition(const std::string& rn, const Transform3f& t) {
+  FrameIndices_t idxs = robotFrames(rn);
+  if (idxs.size() == 0)
+    throw std::invalid_argument("No frame for robot name " + rn);
+
+  pinocchio::Model& m = model();
+  pinocchio::GeomModel& gm = geomModel();
+  // The root frame should be the first frame.
+  Frame& rootFrame = m.frames[idxs[0]];
+  if (rootFrame.type == ::pinocchio::JOINT) {
+    JointIndex jid = m.getJointId(rootFrame.name);
+    m.jointPlacements[jid] = t;
+    return;
+  }
+
+  Transform3f shift(t * rootFrame.placement.inverse());
+  // Find all the frames that have the same parent joint.
+  for (std::size_t i = 1; i < idxs.size(); ++i) {
+    Frame& frame = m.frames[idxs[i]];
+    if (frame.parent == rootFrame.parent) {
+      // frame is between rootFrame and next moving joints.
+      frame.placement = shift * frame.placement;
+      if (frame.type == ::pinocchio::BODY) {
+        // Update the geometry object placement.
+        for (std::size_t k = 0; k < gm.geometryObjects.size(); ++k) {
+          ::pinocchio::GeometryObject& go = gm.geometryObjects[k];
+          if (go.parentFrame == idxs[i]) go.placement = shift * go.placement;
         }
       }
-      invalidate();
+    } else if ((frame.type == ::pinocchio::JOINT) &&
+               (rootFrame.parent == m.parents[frame.parent])) {
+      // frame corresponds to a child joint of rootFrame.parent
+      m.jointPlacements[frame.parent] = shift * m.jointPlacements[frame.parent];
     }
-
-    std::vector<std::string> Device::robotNames () const
-    {
-      const pinocchio::Model& model = this->model();
-      std::vector<std::string> names;
-
-      for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi)
-      {
-        const Frame& frame = model.frames[fi];
-        std::size_t sep = frame.name.find ('/');
-        if (sep == std::string::npos) {
-          hppDout (warning, "Frame " << frame.name << " does not belong to any robots.");
-          continue;
-        }
-        std::string name = frame.name.substr(0,sep);
-
-        if (std::find(names.rbegin(), names.rend(), name) != names.rend())
-          names.push_back (name);
-      }
-      return names;
+  }
+  invalidate();
+}
+
+std::vector<std::string> Device::robotNames() const {
+  const pinocchio::Model& model = this->model();
+  std::vector<std::string> names;
+
+  for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi) {
+    const Frame& frame = model.frames[fi];
+    std::size_t sep = frame.name.find('/');
+    if (sep == std::string::npos) {
+      hppDout(warning,
+              "Frame " << frame.name << " does not belong to any robots.");
+      continue;
     }
-
-    FrameIndices_t Device::robotFrames (const std::string& robotName) const
-    {
-      const pinocchio::Model& model = this->model();
-      FrameIndices_t frameIndices;
-
-      for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi)
-      {
-        const std::string& name = model.frames[fi].name;
-        if (   name.size() > robotName.size()
-            && name.compare (0, robotName.size(), robotName) == 0
-            && name[robotName.size()] == '/') {
-          frameIndices.push_back (fi);
-        }
-      }
-      return frameIndices;
+    std::string name = frame.name.substr(0, sep);
+
+    if (std::find(names.rbegin(), names.rend(), name) != names.rend())
+      names.push_back(name);
+  }
+  return names;
+}
+
+FrameIndices_t Device::robotFrames(const std::string& robotName) const {
+  const pinocchio::Model& model = this->model();
+  FrameIndices_t frameIndices;
+
+  for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi) {
+    const std::string& name = model.frames[fi].name;
+    if (name.size() > robotName.size() &&
+        name.compare(0, robotName.size(), robotName) == 0 &&
+        name[robotName.size()] == '/') {
+      frameIndices.push_back(fi);
     }
-
-    void Device::removeJoints(const std::vector<std::string>& jointNames,
-        Configuration_t referenceConfig)
-    {
-      Parent_t::removeJoints(jointNames, referenceConfig);
-
-      for (auto& pair : grippers.map)
-        pair.second = pinocchio::Gripper::create(pair.second->name(), self_);
-      // TODO update handles and jointAndShapes
-    }
-
-    std::ostream& Device::print (std::ostream& os) const
-    {
-      Parent_t::print (os);
-      // print handles
-      os << "Handles:" << std::endl;
-      handles.print (os);
-      // print grippers
-      os << "Grippers:" << std::endl;
-      grippers.print (os);
-      return os;
-    }
-
-    template<class Archive>
-    void Device::serialize(Archive & ar, const unsigned int version)
-    {
-      using namespace boost::serialization;
-      (void) version;
-      auto* har = hpp::serialization::cast(&ar);
-
-      ar & make_nvp("base", base_object<pinocchio::HumanoidRobot>(*this));
-
-      // TODO we should throw if a pinocchio::Device instance with name name_
-      // and not of type manipulation::Device is found.
-      bool written = (!har ||
-          har->template getChildClass<pinocchio::Device, Device>(name_, false) != this);
-      ar & BOOST_SERIALIZATION_NVP(written);
-      if (written) {
-        ar & BOOST_SERIALIZATION_NVP(self_);
-        // TODO (easy) add serialization of core::Container ?
-        //ar & BOOST_SERIALIZATION_NVP(handles);
-        //ar & BOOST_SERIALIZATION_NVP(grippers);
-        //ar & BOOST_SERIALIZATION_NVP(jointAndShapes);
-      }
-    }
-
-    HPP_SERIALIZATION_IMPLEMENT(Device);
-  } // namespace pinocchio
-} // namespace hpp
+  }
+  return frameIndices;
+}
+
+void Device::removeJoints(const std::vector<std::string>& jointNames,
+                          Configuration_t referenceConfig) {
+  Parent_t::removeJoints(jointNames, referenceConfig);
+
+  for (auto& pair : grippers.map)
+    pair.second = pinocchio::Gripper::create(pair.second->name(), self_);
+  // TODO update handles and jointAndShapes
+}
+
+std::ostream& Device::print(std::ostream& os) const {
+  Parent_t::print(os);
+  // print handles
+  os << "Handles:" << std::endl;
+  handles.print(os);
+  // print grippers
+  os << "Grippers:" << std::endl;
+  grippers.print(os);
+  return os;
+}
+
+template <class Archive>
+void Device::serialize(Archive& ar, const unsigned int version) {
+  using namespace boost::serialization;
+  (void)version;
+  auto* har = hpp::serialization::cast(&ar);
+
+  ar& make_nvp("base", base_object<pinocchio::HumanoidRobot>(*this));
+
+  // TODO we should throw if a pinocchio::Device instance with name name_
+  // and not of type manipulation::Device is found.
+  bool written =
+      (!har || har->template getChildClass<pinocchio::Device, Device>(
+                   name_, false) != this);
+  ar& BOOST_SERIALIZATION_NVP(written);
+  if (written) {
+    ar& BOOST_SERIALIZATION_NVP(self_);
+    // TODO (easy) add serialization of core::Container ?
+    // ar & BOOST_SERIALIZATION_NVP(handles);
+    // ar & BOOST_SERIALIZATION_NVP(grippers);
+    // ar & BOOST_SERIALIZATION_NVP(jointAndShapes);
+  }
+}
+
+HPP_SERIALIZATION_IMPLEMENT(Device);
+}  // namespace manipulation
+}  // namespace hpp
 
 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::manipulation::Device)
diff --git a/src/graph-node-optimizer.cc b/src/graph-node-optimizer.cc
index fa3443f1138592d9fc5d41f2f3e2c359b982c6d3..16e023b60b66cf4a4ec5162b5474dfcdf96ad020 100644
--- a/src/graph-node-optimizer.cc
+++ b/src/graph-node-optimizer.cc
@@ -26,43 +26,41 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/graph-node-optimizer.hh>
-
 #include <hpp/core/steering-method/straight.hh>
+#include <hpp/manipulation/graph-node-optimizer.hh>
 
 namespace hpp {
-  namespace manipulation {
-    GraphNodeOptimizerPtr_t GraphNodeOptimizer::create
-      (const core::ProblemConstPtr_t& problem)
-    {
-      GraphNodeOptimizer* ptr = new GraphNodeOptimizer (problem);
-      return GraphNodeOptimizerPtr_t (ptr);
-    }
+namespace manipulation {
+GraphNodeOptimizerPtr_t GraphNodeOptimizer::create(
+    const core::ProblemConstPtr_t& problem) {
+  GraphNodeOptimizer* ptr = new GraphNodeOptimizer(problem);
+  return GraphNodeOptimizerPtr_t(ptr);
+}
 
-    PathVectorPtr_t GraphNodeOptimizer::optimize (const PathVectorPtr_t& path)
-    {
-      core::ProblemPtr_t p = const_cast <core::ProblemPtr_t> (this->problem ());
-      core::SteeringMethodPtr_t sm = p.steeringMethod ();
+PathVectorPtr_t GraphNodeOptimizer::optimize(const PathVectorPtr_t& path) {
+  core::ProblemPtr_t p = const_cast<core::ProblemPtr_t>(this->problem());
+  core::SteeringMethodPtr_t sm = p.steeringMethod();
 
-      /// Start by flattening the path
-      PathVectorPtr_t flat = PathVector::create
-        (path->outputSize(), path->outputDerivativeSize()),
-      path->flatten (flat);
+  /// Start by flattening the path
+  PathVectorPtr_t flat = PathVector::create(path->outputSize(),
+                                            path->outputDerivativeSize()),
+                  path->flatten(flat);
 
-      PathVectorPtr_t opted = PathVector::create
-        (path->outputSize(), path->outputDerivativeSize()),
-        toConcat;
+  PathVectorPtr_t opted = PathVector::create(path->outputSize(),
+                                             path->outputDerivativeSize()),
+                  toConcat;
 
-      ConstraintSetPtr_t c;
-      for (std::size_t i_s = 0; i_s < flat->numberPaths ();) {
-        PathPtr_t p = flat->pathAtRank (i_s);
-        PathPtr_t newp = (*sm) (p->initial (), p->end ());
-        if (!newp)
-          throw std::runtime_error ("It should not be a problem to recompute "
-              "a path...");
-        opted->appendPath (newp);
-      }
-      return opted;
-    }
-  } // namespace manipulation
-} // namespace hpp
+  ConstraintSetPtr_t c;
+  for (std::size_t i_s = 0; i_s < flat->numberPaths();) {
+    PathPtr_t p = flat->pathAtRank(i_s);
+    PathPtr_t newp = (*sm)(p->initial(), p->end());
+    if (!newp)
+      throw std::runtime_error(
+          "It should not be a problem to recompute "
+          "a path...");
+    opted->appendPath(newp);
+  }
+  return opted;
+}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
index e62179cba8dd04c4f92098dde2c1978c26515973..045615bc9cc99373aaed5bfbc1af671d5b6a1039 100644
--- a/src/graph-optimizer.cc
+++ b/src/graph-optimizer.cc
@@ -26,76 +26,75 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/graph-optimizer.hh>
-
-#include <hpp/core/path.hh>
+#include <hpp/core/config-projector.hh>
 #include <hpp/core/path-vector.hh>
+#include <hpp/core/path.hh>
 #include <hpp/core/problem.hh>
-#include <hpp/core/config-projector.hh>
-
 #include <hpp/manipulation/constraint-set.hh>
-#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph-optimizer.hh>
 #include <hpp/manipulation/graph-path-validation.hh>
+#include <hpp/manipulation/graph/edge.hh>
 
 namespace hpp {
-  namespace manipulation {
-    PathVectorPtr_t GraphOptimizer::optimize (const PathVectorPtr_t& path)
-    {
-      PathVectorPtr_t opted = PathVector::create
-        (path->outputSize(), path->outputDerivativeSize()),
-        expanded = PathVector::create
-          (path->outputSize(), path->outputDerivativeSize()),
-        toConcat;
-      GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
-              this->problem()->pathValidation ());
+namespace manipulation {
+PathVectorPtr_t GraphOptimizer::optimize(const PathVectorPtr_t& path) {
+  PathVectorPtr_t opted = PathVector::create(path->outputSize(),
+                                             path->outputDerivativeSize()),
+                  expanded = PathVector::create(path->outputSize(),
+                                                path->outputDerivativeSize()),
+                  toConcat;
+  GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST(
+      GraphPathValidation, this->problem()->pathValidation());
 
-      path->flatten (expanded);
-      ConstraintSetPtr_t c;
-      for (std::size_t i_s = 0; i_s < expanded->numberPaths ();) {
-        PathVectorPtr_t toOpt = PathVector::create (
-            path->outputSize(), path->outputDerivativeSize()); 
-        PathPtr_t current = expanded->pathAtRank (i_s);
-        toOpt->appendPath (current);
-        graph::EdgePtr_t edge;
-        c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
-        if (c) edge = c->edge ();
-        bool isShort = edge && edge->isShort();
-        std::size_t i_e = i_s + 1;
-        for (; i_e < expanded->numberPaths (); ++i_e) {
-          current = expanded->pathAtRank (i_e);
-          c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
-          if (!c && edge) {
-            hppDout(info, "No manipulation::ConstraintSet");
-            break;
-          }
-          if (c && edge->state() != c->edge ()->state()) break;
-          if (isShort != c->edge()->isShort()) // We do not optimize edges marked as short
-            break;
-          toOpt->appendPath (current);
-        }
-        hppDout(info, "Edge name: " << edge->name());
-        if (isShort)
-          toConcat = toOpt;
-        else {
-          core::ProblemPtr_t p = core::Problem::create (problem()->robot());
-          p->distance(problem()->distance());
-          // It should be ok to use the path validation of each edge because it
-          // corresponds to the global path validation minus the collision pairs
-          // disabled using the edge constraint.
-          // p.pathValidation(gpv->innerValidation());
-          p->pathProjector(problem()->pathProjector());
-          p->steeringMethod(edge->steeringMethod()->copy());
-          p->constraints(p->steeringMethod()->constraints());
-          p->constraints()->configProjector()->rightHandSideFromConfig(toOpt->initial());
-          p->pathValidation(edge->pathValidation());
-          pathOptimizer_ = factory_ (p);
-          toConcat = pathOptimizer_->optimize (toOpt);
-        }
-        i_s = i_e;
-        opted->concatenate (toConcat);
+  path->flatten(expanded);
+  ConstraintSetPtr_t c;
+  for (std::size_t i_s = 0; i_s < expanded->numberPaths();) {
+    PathVectorPtr_t toOpt =
+        PathVector::create(path->outputSize(), path->outputDerivativeSize());
+    PathPtr_t current = expanded->pathAtRank(i_s);
+    toOpt->appendPath(current);
+    graph::EdgePtr_t edge;
+    c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, current->constraints());
+    if (c) edge = c->edge();
+    bool isShort = edge && edge->isShort();
+    std::size_t i_e = i_s + 1;
+    for (; i_e < expanded->numberPaths(); ++i_e) {
+      current = expanded->pathAtRank(i_e);
+      c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, current->constraints());
+      if (!c && edge) {
+        hppDout(info, "No manipulation::ConstraintSet");
+        break;
       }
-      pathOptimizer_.reset ();
-      return opted;
+      if (c && edge->state() != c->edge()->state()) break;
+      if (isShort !=
+          c->edge()->isShort())  // We do not optimize edges marked as short
+        break;
+      toOpt->appendPath(current);
+    }
+    hppDout(info, "Edge name: " << edge->name());
+    if (isShort)
+      toConcat = toOpt;
+    else {
+      core::ProblemPtr_t p = core::Problem::create(problem()->robot());
+      p->distance(problem()->distance());
+      // It should be ok to use the path validation of each edge because it
+      // corresponds to the global path validation minus the collision pairs
+      // disabled using the edge constraint.
+      // p.pathValidation(gpv->innerValidation());
+      p->pathProjector(problem()->pathProjector());
+      p->steeringMethod(edge->steeringMethod()->copy());
+      p->constraints(p->steeringMethod()->constraints());
+      p->constraints()->configProjector()->rightHandSideFromConfig(
+          toOpt->initial());
+      p->pathValidation(edge->pathValidation());
+      pathOptimizer_ = factory_(p);
+      toConcat = pathOptimizer_->optimize(toOpt);
     }
-  } // namespace manipulation
-} // namespace hpp
+    i_s = i_e;
+    opted->concatenate(toConcat);
+  }
+  pathOptimizer_.reset();
+  return opted;
+}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index e2e514f2169d16f3622f969eb6cfae0bf3414f4d..240e7d947a2ad5a792540a4d8a13407d3af78984 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -28,182 +28,184 @@
 
 #include "hpp/manipulation/graph-path-validation.hh"
 
-#include <hpp/pinocchio/configuration.hh>
-
-#include <hpp/core/path.hh>
 #include <hpp/core/path-vector.hh>
-
-#include <hpp/manipulation/graph/state.hh>
+#include <hpp/core/path.hh>
+#include <hpp/manipulation/constraint-set.hh>
 #include <hpp/manipulation/graph/edge.hh>
 #include <hpp/manipulation/graph/graph.hh>
-#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph/state.hh>
+#include <hpp/pinocchio/configuration.hh>
 
 #ifdef HPP_DEBUG
-# include <hpp/manipulation/graph/state.hh>
+#include <hpp/manipulation/graph/state.hh>
 #endif
 
 namespace hpp {
-  namespace manipulation {
-    GraphPathValidationPtr_t GraphPathValidation::create (const PathValidationPtr_t& pathValidation)
-    {
-      GraphPathValidation* p = new GraphPathValidation (pathValidation);
-      return GraphPathValidationPtr_t (p);
-    }
+namespace manipulation {
+GraphPathValidationPtr_t GraphPathValidation::create(
+    const PathValidationPtr_t& pathValidation) {
+  GraphPathValidation* p = new GraphPathValidation(pathValidation);
+  return GraphPathValidationPtr_t(p);
+}
 
-    GraphPathValidation::GraphPathValidation (const PathValidationPtr_t& pathValidation) :
-      pathValidation_ (pathValidation), constraintGraph_ ()
-    {}
+GraphPathValidation::GraphPathValidation(
+    const PathValidationPtr_t& pathValidation)
+    : pathValidation_(pathValidation), constraintGraph_() {}
 
-    bool GraphPathValidation::validate (const PathPtr_t& path, bool reverse,
-					PathPtr_t& validPart,
-					PathValidationReportPtr_t& report)
-    {
-      assert (path);
-      bool success = impl_validate (path, reverse, validPart, report);
-      assert (constraintGraph_);
-      assert (constraintGraph_->getState (validPart->initial ()));
-      assert (constraintGraph_->getState (validPart->end     ()));
-      return success;
-    }
+bool GraphPathValidation::validate(const PathPtr_t& path, bool reverse,
+                                   PathPtr_t& validPart,
+                                   PathValidationReportPtr_t& report) {
+  assert(path);
+  bool success = impl_validate(path, reverse, validPart, report);
+  assert(constraintGraph_);
+  assert(constraintGraph_->getState(validPart->initial()));
+  assert(constraintGraph_->getState(validPart->end()));
+  return success;
+}
 
-    bool GraphPathValidation::impl_validate (const PathVectorPtr_t& path,
-        bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report)
-    {
-      PathPtr_t validSubPart;
-      if (reverse) {
-        // TODO: This has never been tested.
-        assert (!reverse && "This has never been tested with reverse path");
-        for (long int i = path->numberPaths () - 1; i >= 0; i--) {
-          // We should stop at the first non valid subpath.
-          if (!impl_validate (path->pathAtRank (i), true, validSubPart, report)) {
-            PathVectorPtr_t p = PathVector::create
-	      (path->outputSize(), path->outputDerivativeSize());
-            for (long int v = path->numberPaths () - 1; v > i; v--)
-              p->appendPath (path->pathAtRank(i)->copy());
-            // TODO: Make sure this subpart is generated by the steering method.
-            p->appendPath (validSubPart);
-            validPart = p;
-            return false;
-          }
-        }
-      } else {
-        for (size_t i = 0; i != path->numberPaths (); i++) {
-          // We should stop at the first non valid subpath.
-          if (!impl_validate (path->pathAtRank (i), false, validSubPart, report)) {
-            PathVectorPtr_t p = PathVector::create
-	      (path->outputSize(), path->outputDerivativeSize());
-            for (size_t v = 0; v < i; v++)
-              p->appendPath (path->pathAtRank(v)->copy());
-            // TODO: Make sure this subpart is generated by the steering method.
-            p->appendPath (validSubPart);
-            validPart = p;
-            return false;
-          }
-        }
+bool GraphPathValidation::impl_validate(const PathVectorPtr_t& path,
+                                        bool reverse, PathPtr_t& validPart,
+                                        PathValidationReportPtr_t& report) {
+  PathPtr_t validSubPart;
+  if (reverse) {
+    // TODO: This has never been tested.
+    assert(!reverse && "This has never been tested with reverse path");
+    for (long int i = path->numberPaths() - 1; i >= 0; i--) {
+      // We should stop at the first non valid subpath.
+      if (!impl_validate(path->pathAtRank(i), true, validSubPart, report)) {
+        PathVectorPtr_t p = PathVector::create(path->outputSize(),
+                                               path->outputDerivativeSize());
+        for (long int v = path->numberPaths() - 1; v > i; v--)
+          p->appendPath(path->pathAtRank(i)->copy());
+        // TODO: Make sure this subpart is generated by the steering method.
+        p->appendPath(validSubPart);
+        validPart = p;
+        return false;
+      }
+    }
+  } else {
+    for (size_t i = 0; i != path->numberPaths(); i++) {
+      // We should stop at the first non valid subpath.
+      if (!impl_validate(path->pathAtRank(i), false, validSubPart, report)) {
+        PathVectorPtr_t p = PathVector::create(path->outputSize(),
+                                               path->outputDerivativeSize());
+        for (size_t v = 0; v < i; v++)
+          p->appendPath(path->pathAtRank(v)->copy());
+        // TODO: Make sure this subpart is generated by the steering method.
+        p->appendPath(validSubPart);
+        validPart = p;
+        return false;
       }
-      // Here, every subpath is valid.
-      validPart = path;
-      return true;
     }
+  }
+  // Here, every subpath is valid.
+  validPart = path;
+  return true;
+}
 
-    bool GraphPathValidation::impl_validate (const PathPtr_t& path,
-        bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report)
-    {
+bool GraphPathValidation::impl_validate(const PathPtr_t& path, bool reverse,
+                                        PathPtr_t& validPart,
+                                        PathValidationReportPtr_t& report) {
 #ifndef NDEBUG
-      bool success;
-      Configuration_t q0 = path->eval(path->timeRange ().second, success);
-      assert (success);
-      assert (!path->constraints () || path->constraints ()->isSatisfied (q0));
+  bool success;
+  Configuration_t q0 = path->eval(path->timeRange().second, success);
+  assert(success);
+  assert(!path->constraints() || path->constraints()->isSatisfied(q0));
 #endif
-      using pinocchio::displayConfig;
-      PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path);
-      if (pathVector)
-        return impl_validate (pathVector, reverse, validPart, report);
+  using pinocchio::displayConfig;
+  PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path);
+  if (pathVector) return impl_validate(pathVector, reverse, validPart, report);
 
-      PathPtr_t pathNoCollision;
-      ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
-      hppDout(info, (c?"Using edge path validation":"Using default path validation"));
-      PathValidationPtr_t validation (c
-          ? c->edge()->pathValidation()
-          : pathValidation_);
+  PathPtr_t pathNoCollision;
+  ConstraintSetPtr_t c =
+      HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
+  hppDout(info,
+          (c ? "Using edge path validation" : "Using default path validation"));
+  PathValidationPtr_t validation(c ? c->edge()->pathValidation()
+                                   : pathValidation_);
 
-      if (validation->validate (path, reverse, pathNoCollision, report)) {
-        validPart = path;
-        return true;
-      }
-      const Path& newPath (*pathNoCollision);
-      const Path& oldPath (*path);
-      const core::interval_t& newTR = newPath.timeRange (),
-                              oldTR = oldPath.timeRange ();
-      Configuration_t q (newPath.outputSize());
-      if (!newPath (q, newTR.first))
-        throw std::logic_error ("Initial configuration of the valid part cannot be projected.");
-      const graph::StatePtr_t& origState = constraintGraph_->getState (q);
-      if (!newPath (q, newTR.second))
-        throw std::logic_error ("End configuration of the valid part cannot be projected.");
-      // This may throw in the following case:
-      // - state constraints: object_placement + other_function
-      // - path constraints: other_function, object_lock
-      // This is semantically correct but for a path going from q0 to q1,
-      // we ensure that
-      // - object_placement (q0) = eps_place0
-      // - other_function (q0) = eps_other0
-      // - eps_place0 + eps_other0 < eps
-      // - other_function (q(s)) < eps
-      // - object_placement (q(s)) = object_placement (q0) # thanks to the object_lock
-      // So we only have:
-      // - other_function (q(s)) + object_placement (q(s)) < eps + eps_place0
-      // And not:
-      // - other_function (q(s)) + object_placement (q(s)) < eps
-      // In this case, there is no good way to recover. Just return failure.
-      graph::StatePtr_t destState;
-      try {
-        destState = constraintGraph_->getState (q);
-      } catch (const std::logic_error& e) {
-        ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
-        hppDout (error, "Edge " << c->edge()->name()
-            << " generated an error: " << e.what());
-        hppDout (error, "Likely, the constraints for paths are relaxed. If "
+  if (validation->validate(path, reverse, pathNoCollision, report)) {
+    validPart = path;
+    return true;
+  }
+  const Path& newPath(*pathNoCollision);
+  const Path& oldPath(*path);
+  const core::interval_t &newTR = newPath.timeRange(),
+                         oldTR = oldPath.timeRange();
+  Configuration_t q(newPath.outputSize());
+  if (!newPath(q, newTR.first))
+    throw std::logic_error(
+        "Initial configuration of the valid part cannot be projected.");
+  const graph::StatePtr_t& origState = constraintGraph_->getState(q);
+  if (!newPath(q, newTR.second))
+    throw std::logic_error(
+        "End configuration of the valid part cannot be projected.");
+  // This may throw in the following case:
+  // - state constraints: object_placement + other_function
+  // - path constraints: other_function, object_lock
+  // This is semantically correct but for a path going from q0 to q1,
+  // we ensure that
+  // - object_placement (q0) = eps_place0
+  // - other_function (q0) = eps_other0
+  // - eps_place0 + eps_other0 < eps
+  // - other_function (q(s)) < eps
+  // - object_placement (q(s)) = object_placement (q0) # thanks to the
+  // object_lock So we only have:
+  // - other_function (q(s)) + object_placement (q(s)) < eps + eps_place0
+  // And not:
+  // - other_function (q(s)) + object_placement (q(s)) < eps
+  // In this case, there is no good way to recover. Just return failure.
+  graph::StatePtr_t destState;
+  try {
+    destState = constraintGraph_->getState(q);
+  } catch (const std::logic_error& e) {
+    ConstraintSetPtr_t c =
+        HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
+    hppDout(error, "Edge " << c->edge()->name()
+                           << " generated an error: " << e.what());
+    hppDout(error,
+            "Likely, the constraints for paths are relaxed. If "
             "this problem occurs often, you may want to use the same "
-            "constraints for state and paths in " << c->edge()->state()->name());
-        validPart = path->extract (std::make_pair (oldTR.first,oldTR.first));
-        return false;
-      }
-      if (!oldPath (q, oldTR.first)) {
-        std::stringstream oss;
-        oss << "Initial configuration of the path to be validated failed to"
-          " be projected. After maximal number of iterations, q="
-            << displayConfig (q) << "; error=";
-        vector_t error;
-        oldPath.constraints ()->isSatisfied (q, error);
-        oss << displayConfig (error) << ".";
-        throw std::logic_error (oss.str ().c_str ());
-      }
-      const graph::StatePtr_t& oldOstate = constraintGraph_->getState (q);
-      if (!oldPath (q, oldTR.second)) {
-        std::stringstream oss;
-        oss << "End configuration of the path to be validated failed to"
-          " be projected. After maximal number of iterations, q="
-            << displayConfig (q) << "; error=";
-        vector_t error;
-        oldPath.constraints ()->isSatisfied (q, error);
-        oss << displayConfig (error) << ".";
-        throw std::logic_error (oss.str ().c_str ());
-      }
-      const graph::StatePtr_t& oldDstate = constraintGraph_->getState (q);
+            "constraints for state and paths in "
+                << c->edge()->state()->name());
+    validPart = path->extract(std::make_pair(oldTR.first, oldTR.first));
+    return false;
+  }
+  if (!oldPath(q, oldTR.first)) {
+    std::stringstream oss;
+    oss << "Initial configuration of the path to be validated failed to"
+           " be projected. After maximal number of iterations, q="
+        << displayConfig(q) << "; error=";
+    vector_t error;
+    oldPath.constraints()->isSatisfied(q, error);
+    oss << displayConfig(error) << ".";
+    throw std::logic_error(oss.str().c_str());
+  }
+  const graph::StatePtr_t& oldOstate = constraintGraph_->getState(q);
+  if (!oldPath(q, oldTR.second)) {
+    std::stringstream oss;
+    oss << "End configuration of the path to be validated failed to"
+           " be projected. After maximal number of iterations, q="
+        << displayConfig(q) << "; error=";
+    vector_t error;
+    oldPath.constraints()->isSatisfied(q, error);
+    oss << displayConfig(error) << ".";
+    throw std::logic_error(oss.str().c_str());
+  }
+  const graph::StatePtr_t& oldDstate = constraintGraph_->getState(q);
 
-      if (origState == oldOstate && destState == oldDstate) {
-        validPart = pathNoCollision;
-        return false;
-      }
+  if (origState == oldOstate && destState == oldDstate) {
+    validPart = pathNoCollision;
+    return false;
+  }
 
-      // Here, the full path is not valid. Moreover, it does not correspond to the same
-      // edge of the constraint graph. Two option are possible:
-      // - Use the steering method to create a new path and validate it.
-      // - Return a null path.
-      // validPart = path->extract (std::make_pair (oldTR.first,oldTR.first));
-      validPart = pathNoCollision;
-      return false;
-    }
-  } // namespace manipulation
-} // namespace hpp
+  // Here, the full path is not valid. Moreover, it does not correspond to the
+  // same edge of the constraint graph. Two option are possible:
+  // - Use the steering method to create a new path and validate it.
+  // - Return a null path.
+  // validPart = path->extract (std::make_pair (oldTR.first,oldTR.first));
+  validPart = pathNoCollision;
+  return false;
+}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/dot.cc b/src/graph/dot.cc
index fa95d301e6355ae118baf328206a9589978f8bfa..26516188ec1373bd7eeea83a2e99198cf5a8b74e 100644
--- a/src/graph/dot.cc
+++ b/src/graph/dot.cc
@@ -29,30 +29,28 @@
 #include "hpp/manipulation/graph/dot.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      namespace dot {
-        const std::string Tooltip::tooltipendl = "&#10;";
+namespace manipulation {
+namespace graph {
+namespace dot {
+const std::string Tooltip::tooltipendl = "&#10;";
 
-        std::ostream& operator<< (std::ostream& os, const DrawingAttributes& da)
-        {
-          if (da.attr.empty ()) return os;
-          os << da.openSection;
-          size_t i = da.attr.size ();
-          for (DrawingAttributes::Map::const_iterator it = da.attr.begin ();
-              it != da.attr.end (); ++it) {
-            os << it->first << "=" << it->second; 
-            i--;
-            if (i > 0) os << da.separator;
-          }
-          return os << da.closeSection;
-        }
+std::ostream& operator<<(std::ostream& os, const DrawingAttributes& da) {
+  if (da.attr.empty()) return os;
+  os << da.openSection;
+  size_t i = da.attr.size();
+  for (DrawingAttributes::Map::const_iterator it = da.attr.begin();
+       it != da.attr.end(); ++it) {
+    os << it->first << "=" << it->second;
+    i--;
+    if (i > 0) os << da.separator;
+  }
+  return os << da.closeSection;
+}
 
-        std::ostream& insertComments (std::ostream& os, const std::string& c)
-        {
-          return os << "/*" << std::endl << c << std::endl << "*/";
-        }
-      } // namespace dot
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+std::ostream& insertComments(std::ostream& os, const std::string& c) {
+  return os << "/*" << std::endl << c << std::endl << "*/";
+}
+}  // namespace dot
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 91aaa628c1e9e21aabb5fdd66c60fb3ecea8d861..40dcd328ac7e2870646476a9f1c0a634d649f555 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -28,828 +28,763 @@
 
 #include "hpp/manipulation/graph/edge.hh"
 
-#include <sstream>
-
-#include <hpp/util/pointer.hh>
-#include <hpp/util/exception-factory.hh>
-
-#include <hpp/pinocchio/configuration.hh>
-#include <hpp/core/obstacle-user.hh>
-#include <hpp/core/path-vector.hh>
-#include <hpp/core/path-validation.hh>
-
 #include <hpp/constraints/differentiable-function.hh>
 #include <hpp/constraints/locked-joint.hh>
+#include <hpp/core/obstacle-user.hh>
+#include <hpp/core/path-validation.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/pinocchio/configuration.hh>
+#include <hpp/util/exception-factory.hh>
+#include <hpp/util/pointer.hh>
+#include <sstream>
 
+#include "hpp/manipulation/constraint-set.hh"
 #include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/graph/statistics.hh"
 #include "hpp/manipulation/problem.hh"
 #include "hpp/manipulation/steering-method/graph.hh"
-#include "hpp/manipulation/graph/statistics.hh"
-#include "hpp/manipulation/constraint-set.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      Edge::Edge (const std::string& name) :
-	GraphComponent (name), isShort_ (false),
-        pathConstraints_ (),
-	targetConstraints_ (),
-        steeringMethod_ (),
-        securityMargins_ (),
-        pathValidation_ ()
-      {}
-
-      Edge::~Edge ()
-      {}
-
-      StatePtr_t Edge::stateTo () const
-      {
-        return to_.lock();
-      }
-
-      StatePtr_t Edge::stateFrom () const
-      {
-        return from_.lock();
-      }
-
-      void Edge::relativeMotion(const RelativeMotion::matrix_type & m)
-      {
-        if(!isInit_) throw std::logic_error("The graph must be initialized before changing the relative motion matrix.");
-        shared_ptr<core::ObstacleUserInterface> oui =
-          HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
-        if (oui) oui->filterCollisionPairs(m);
-        relMotion_ = m;
-      }
-
-      bool Edge::direction (const core::PathPtr_t& path) const
-      {
-        Configuration_t q0 = path->initial (),
-                        q1 = path->end ();
-        const bool src_contains_q0 = stateFrom()->contains (q0);
-        const bool dst_contains_q0 = stateTo  ()->contains (q0);
-        const bool src_contains_q1 = stateFrom()->contains (q1);
-        const bool dst_contains_q1 = stateTo  ()->contains (q1);
-        // Karnaugh table:
-        // 1 = forward, 0 = reverse, ? = I don't know, * = 0 or 1
-        // s0s1 \ d0d1 | 00 | 01 | 11 | 10
-        // 00          |  ? |  ? |  ? |  ?
-        // 01          |  ? |  ? |  0 |  0
-        // 11          |  ? |  1 |  * |  0
-        // 10          |  ? |  1 |  1 |  1
-        //
-        /// true if reverse
-        if (   (!src_contains_q0 && !src_contains_q1)
-            || (!dst_contains_q0 && !dst_contains_q1)
-            || (!src_contains_q0 && !dst_contains_q0))
-          HPP_THROW (std::runtime_error,
+namespace manipulation {
+namespace graph {
+Edge::Edge(const std::string& name)
+    : GraphComponent(name),
+      isShort_(false),
+      pathConstraints_(),
+      targetConstraints_(),
+      steeringMethod_(),
+      securityMargins_(),
+      pathValidation_() {}
+
+Edge::~Edge() {}
+
+StatePtr_t Edge::stateTo() const { return to_.lock(); }
+
+StatePtr_t Edge::stateFrom() const { return from_.lock(); }
+
+void Edge::relativeMotion(const RelativeMotion::matrix_type& m) {
+  if (!isInit_)
+    throw std::logic_error(
+        "The graph must be initialized before changing the relative motion "
+        "matrix.");
+  shared_ptr<core::ObstacleUserInterface> oui =
+      HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+  if (oui) oui->filterCollisionPairs(m);
+  relMotion_ = m;
+}
+
+bool Edge::direction(const core::PathPtr_t& path) const {
+  Configuration_t q0 = path->initial(), q1 = path->end();
+  const bool src_contains_q0 = stateFrom()->contains(q0);
+  const bool dst_contains_q0 = stateTo()->contains(q0);
+  const bool src_contains_q1 = stateFrom()->contains(q1);
+  const bool dst_contains_q1 = stateTo()->contains(q1);
+  // Karnaugh table:
+  // 1 = forward, 0 = reverse, ? = I don't know, * = 0 or 1
+  // s0s1 \ d0d1 | 00 | 01 | 11 | 10
+  // 00          |  ? |  ? |  ? |  ?
+  // 01          |  ? |  ? |  0 |  0
+  // 11          |  ? |  1 |  * |  0
+  // 10          |  ? |  1 |  1 |  1
+  //
+  /// true if reverse
+  if ((!src_contains_q0 && !src_contains_q1) ||
+      (!dst_contains_q0 && !dst_contains_q1) ||
+      (!src_contains_q0 && !dst_contains_q0))
+    HPP_THROW(std::runtime_error,
               "Edge " << name() << " does not seem to have generated path from"
-              << pinocchio::displayConfig(q0) << " to "
-              << pinocchio::displayConfig(q1)
-              );
-        return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1));
-      }
-
-      void Edge::securityMarginForPair
-      (const size_type& row, const size_type& col, const value_type& margin)
-      {
-        if ((row < 0) || (row >= securityMargins_.rows())) {
-          std::ostringstream os;
-          os << "Row index should be between 0 and "
-             << securityMargins_.rows()+1 << ", got " << row << ".";
-          throw std::runtime_error(os.str().c_str());
-        }
-        if ((col < 0) || (col >= securityMargins_.cols())) {
-          std::ostringstream os;
-          os << "Column index should be between 0 and "
-             << securityMargins_.cols()+1 << ", got " << col << ".";
-          throw std::runtime_error(os.str().c_str());
-        }
-        if (securityMargins_(row, col) != margin) {
-          securityMargins_(row, col) = margin;
-          securityMargins_(col, row) = margin;
-          invalidate ();
-        }
-      }
-
-      bool Edge::intersectionConstraint (const EdgePtr_t& other,
-          ConfigProjectorPtr_t proj) const
-      {
-        GraphPtr_t g = graph_.lock ();
-
-        g->insertNumericalConstraints (proj);
-        insertNumericalConstraints (proj);
-        state ()->insertNumericalConstraints (proj);
-
-        if (wkPtr_.lock() == other) // No intersection to be computed.
-          return false;
-
-        bool stateB_Eq_stateA = (state() == other->state());
-
-        other->insertNumericalConstraints (proj);
-        if (!stateB_Eq_stateA) other->state()->insertNumericalConstraints (proj);
-        return true;
-      }
-
-      EdgePtr_t Edge::create (const std::string& name,
-			      const GraphWkPtr_t& graph,
-			      const StateWkPtr_t& from, const StateWkPtr_t& to)
-      {
-        Edge* ptr = new Edge (name);
-        EdgePtr_t shPtr (ptr);
-        ptr->init(shPtr, graph, from, to);
-        return shPtr;
-      }
-
-      void Edge::init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
-		       const StateWkPtr_t& from, const StateWkPtr_t& to)
-      {
-        GraphComponent::init (weak);
-        parentGraph (graph);
-        wkPtr_ = weak;
-        from_ = from;
-        to_ = to;
-        state_ = to;
-        // add 1 joint for the environment
-        size_type n (graph.lock()->robot()->nbJoints() + 1);
-        securityMargins_.resize(n, n);
-        securityMargins_.setZero();
-      }
-
-      void Edge::initialize ()
-      {
-        if (!isInit_) {
-          targetConstraints_ = buildTargetConstraint ();
-          pathConstraints_ = buildPathConstraint ();
-        }
-        isInit_ = true;
-      }
-
-      std::ostream& Edge::print (std::ostream& os) const
-      {
-        os << "|   |   |-- ";
-        GraphComponent::print (os)
-          << " --> " << to_.lock ()->name ();
-        return os;
-      }
-
-      std::ostream& Edge::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
-      {
-        da.insertWithQuote ("label", name ());
-        da.insert ("shape", "onormal");
-        dot::Tooltip tp; tp.addLine ("Edge constains:");
-        populateTooltip (tp);
-        da.insertWithQuote ("tooltip", tp.toStr());
-        da.insertWithQuote ("labeltooltip", tp.toStr());
-        os << stateFrom()->id() << " -> " << stateTo()->id() << " " << da
-           << ";";
-        return os;
-      }
-
-      ConstraintSetPtr_t Edge::targetConstraint() const
-      {
-        throwIfNotInitialized ();
-        return targetConstraints_;
-      }
-
-      ConstraintSetPtr_t Edge::configConstraint() const
-      {
-        throwIfNotInitialized ();
-        return targetConstraints_;
-      }
-
-      // Merge constraints of several graph components into a config projector
-      // 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;
-        for (const auto& gc : components)
-          nc.insert(nc.end(), gc->numericalConstraints().begin(),
-                     gc->numericalConstraints ().end ());
-
-        // Remove duplicate constraints
-        auto end = nc.end();
-        for (auto it = nc.begin(); it != end; ++it)
-          end = std::remove(std::next(it), end, *it);
-        nc.erase(end, nc.end());
-
-        NumericalConstraints_t::iterator itnc1, itnc2;
-
-        // Look for complement
-        for (itnc1 = nc.begin(); itnc1 != nc.end(); ++itnc1) {
-          const auto& c1 = *itnc1;
-          itnc2 = std::next(itnc1);
-          constraints::ImplicitPtr_t combination;
-          itnc2 = std::find_if(std::next(itnc1), nc.end(),
-              [&c1, &combination, &graph](const auto& c2) -> bool {
-                assert (c1 != c2);
-                return    graph->isComplement (c1, c2, combination)
-                       || graph->isComplement (c2, c1, combination);
-              });
-          if (itnc2 != nc.end()) {
-            assert (*itnc1 != *itnc2);
-            *itnc1 = combination;
-            nc.erase (itnc2);
-          }
-        }
-
-        for (const auto& _nc : nc)
-          proj->add (_nc);
-      }
-
-      ConstraintSetPtr_t Edge::buildConfigConstraint()
-      {
-        return buildTargetConstraint();
-      }
-
-      ConstraintSetPtr_t Edge::buildTargetConstraint()
-      {
-        std::string n = "(" + name () + ")";
-        GraphPtr_t g = graph_.lock ();
-
-        ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
-
-        ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
-        std::vector <GraphComponentPtr_t> components;
-        components.push_back (g);
-        components.push_back (wkPtr_.lock ());
-        components.push_back (stateTo ());
-	if (state () != stateTo ()) {
-          components.push_back (state ());
-	}
-        // Copy constraints from
-        // - graph,
-        // - this edge,
-        // - the destination state,
-        // - the state in which the transition lies if different
-        mergeConstraintsIntoConfigProjector (proj, components, parentGraph ());
-
-        constraint->addConstraint (proj);
-        constraint->edge (wkPtr_.lock ());
-        return constraint;
-      }
-
-      ConstraintSetPtr_t Edge::pathConstraint() const
-      {
-        throwIfNotInitialized ();
-        return pathConstraints_;
-      }
-
-      ConstraintSetPtr_t Edge::buildPathConstraint()
-      {
-        std::string n = "(" + name () + ")";
-        GraphPtr_t g = graph_.lock ();
-
-        ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
-
-        ConfigProjectorPtr_t proj = ConfigProjector::create
-	  (g->robot(), "proj_" + n, .5*g->errorThreshold(), g->maxIterations());
-        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 ());
-
-        // Build steering method
-        const ProblemPtr_t& problem (g->problem());
-        steeringMethod_ = problem->manipulationSteeringMethod()->innerSteeringMethod()->copy();
-        steeringMethod_->constraints (constraint);
-        // Build path validation and relative motion matrix
-        // TODO this path validation will not contain obstacles added after
-        // its creation.
-        pathValidation_ = problem->pathValidationFactory ();
-        shared_ptr<core::ObstacleUserInterface> oui =
-          HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
-        if (oui) {
-          relMotion_ = RelativeMotion::matrix (g->robot());
-          RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint);
-          oui->filterCollisionPairs (relMotion_);
-          oui->setSecurityMargins(securityMargins_);
-        }
-        return constraint;
-      }
-
-      bool Edge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2)
-	const
-      {
-        ConstraintSetPtr_t constraints = pathConstraint ();
-        constraints->configProjector ()->rightHandSideFromConfig(q1);
-        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
-          return false;
-        }
-        return true;
-      }
-
-      bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
-			ConfigurationIn_t q2)
-	const
-      {
-        using pinocchio::displayConfig;
-	if (!steeringMethod_) {
-	  std::ostringstream oss;
-	  oss << "No steering method set in edge " << name () << ".";
-	  throw std::runtime_error (oss.str ().c_str ());
-	}
-        ConstraintSetPtr_t constraints = pathConstraint ();
-        constraints->configProjector ()->rightHandSideFromConfig(q1);
-        if (constraints->isSatisfied (q1)) {
-          if (constraints->isSatisfied (q2)) {
-            path = (*steeringMethod_) (q1, q2);
-            return (bool)path;
-          } else {
-	    hppDout(info, "q2 = " << displayConfig (q2)
-                    << " does not satisfy the constraints of edge "
-                    << name ());
-	    hppDout (info, "q1 = " << displayConfig (q1));
-	    return false;
-	  }
-        } else {
-	  value_type th(constraints->configProjector()->errorThreshold());
-	  if (!constraints->configProjector()->isSatisfied
-	      (q1, 2*th))
-	  {
-	    std::ostringstream oss;
-	    oss << "The initial configuration " << displayConfig (q1)
-		<< " does not satisfy the constraints of"
-	      " edge " << name () << "." << std::endl;
-	    oss << "The graph is probably malformed";
-	    throw std::runtime_error (oss.str ().c_str ());
-	  }
-	  // q1 may slightly violate the edge constraint eventhough the graph
-	  // is well constructed.
-	  return false;
-	}
-      }
-
-      bool Edge::applyConstraints (core::NodePtr_t nStart,
-                                   ConfigurationOut_t q) const
-      {
-        return generateTargetConfig (*(nStart->configuration ()), q);
-      }
-
-      bool Edge::applyConstraints (ConfigurationIn_t qoffset,
-				   ConfigurationOut_t q) const
-      {
-        return generateTargetConfig(qoffset, q);
-      }
-
-      bool Edge::generateTargetConfig (core::NodePtr_t nStart,
-                                       ConfigurationOut_t q) const
-      {
-        return generateTargetConfig (*(nStart->configuration ()), q);
-      }
-
-      bool Edge::generateTargetConfig (ConfigurationIn_t qStart,
-                                       ConfigurationOut_t q) const
-      {
-        ConstraintSetPtr_t c = targetConstraint();
-        ConfigProjectorPtr_t proj = c->configProjector ();
-        proj->rightHandSideFromConfig (qStart);
-        if (isShort_) q = qStart;
-        if (c->apply (q)) return true;
-	const ::hpp::statistics::SuccessStatistics& ss = proj->statistics ();
-	if (ss.nbFailure () > ss.nbSuccess ()) {
-	  hppDout (warning, c->name () << " fails often.\n" << ss);
-	} else {
-	  hppDout (warning, c->name () << " succeeds at rate "
-		   << (value_type)(ss.nbSuccess ()) /
-		   (value_type) ss.numberOfObservations ()
-		   << ".");
-	}
+                      << pinocchio::displayConfig(q0) << " to "
+                      << pinocchio::displayConfig(q1));
+  return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1));
+}
+
+void Edge::securityMarginForPair(const size_type& row, const size_type& col,
+                                 const value_type& margin) {
+  if ((row < 0) || (row >= securityMargins_.rows())) {
+    std::ostringstream os;
+    os << "Row index should be between 0 and " << securityMargins_.rows() + 1
+       << ", got " << row << ".";
+    throw std::runtime_error(os.str().c_str());
+  }
+  if ((col < 0) || (col >= securityMargins_.cols())) {
+    std::ostringstream os;
+    os << "Column index should be between 0 and " << securityMargins_.cols() + 1
+       << ", got " << col << ".";
+    throw std::runtime_error(os.str().c_str());
+  }
+  if (securityMargins_(row, col) != margin) {
+    securityMargins_(row, col) = margin;
+    securityMargins_(col, row) = margin;
+    invalidate();
+  }
+}
+
+bool Edge::intersectionConstraint(const EdgePtr_t& other,
+                                  ConfigProjectorPtr_t proj) const {
+  GraphPtr_t g = graph_.lock();
+
+  g->insertNumericalConstraints(proj);
+  insertNumericalConstraints(proj);
+  state()->insertNumericalConstraints(proj);
+
+  if (wkPtr_.lock() == other)  // No intersection to be computed.
+    return false;
+
+  bool stateB_Eq_stateA = (state() == other->state());
+
+  other->insertNumericalConstraints(proj);
+  if (!stateB_Eq_stateA) other->state()->insertNumericalConstraints(proj);
+  return true;
+}
+
+EdgePtr_t Edge::create(const std::string& name, const GraphWkPtr_t& graph,
+                       const StateWkPtr_t& from, const StateWkPtr_t& to) {
+  Edge* ptr = new Edge(name);
+  EdgePtr_t shPtr(ptr);
+  ptr->init(shPtr, graph, from, to);
+  return shPtr;
+}
+
+void Edge::init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
+                const StateWkPtr_t& from, const StateWkPtr_t& to) {
+  GraphComponent::init(weak);
+  parentGraph(graph);
+  wkPtr_ = weak;
+  from_ = from;
+  to_ = to;
+  state_ = to;
+  // add 1 joint for the environment
+  size_type n(graph.lock()->robot()->nbJoints() + 1);
+  securityMargins_.resize(n, n);
+  securityMargins_.setZero();
+}
+
+void Edge::initialize() {
+  if (!isInit_) {
+    targetConstraints_ = buildTargetConstraint();
+    pathConstraints_ = buildPathConstraint();
+  }
+  isInit_ = true;
+}
+
+std::ostream& Edge::print(std::ostream& os) const {
+  os << "|   |   |-- ";
+  GraphComponent::print(os) << " --> " << to_.lock()->name();
+  return os;
+}
+
+std::ostream& Edge::dotPrint(std::ostream& os,
+                             dot::DrawingAttributes da) const {
+  da.insertWithQuote("label", name());
+  da.insert("shape", "onormal");
+  dot::Tooltip tp;
+  tp.addLine("Edge constains:");
+  populateTooltip(tp);
+  da.insertWithQuote("tooltip", tp.toStr());
+  da.insertWithQuote("labeltooltip", tp.toStr());
+  os << stateFrom()->id() << " -> " << stateTo()->id() << " " << da << ";";
+  return os;
+}
+
+ConstraintSetPtr_t Edge::targetConstraint() const {
+  throwIfNotInitialized();
+  return targetConstraints_;
+}
+
+ConstraintSetPtr_t Edge::configConstraint() const {
+  throwIfNotInitialized();
+  return targetConstraints_;
+}
+
+// Merge constraints of several graph components into a config projector
+// 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;
+  for (const auto& gc : components)
+    nc.insert(nc.end(), gc->numericalConstraints().begin(),
+              gc->numericalConstraints().end());
+
+  // Remove duplicate constraints
+  auto end = nc.end();
+  for (auto it = nc.begin(); it != end; ++it)
+    end = std::remove(std::next(it), end, *it);
+  nc.erase(end, nc.end());
+
+  NumericalConstraints_t::iterator itnc1, itnc2;
+
+  // Look for complement
+  for (itnc1 = nc.begin(); itnc1 != nc.end(); ++itnc1) {
+    const auto& c1 = *itnc1;
+    itnc2 = std::next(itnc1);
+    constraints::ImplicitPtr_t combination;
+    itnc2 = std::find_if(std::next(itnc1), nc.end(),
+                         [&c1, &combination, &graph](const auto& c2) -> bool {
+                           assert(c1 != c2);
+                           return graph->isComplement(c1, c2, combination) ||
+                                  graph->isComplement(c2, c1, combination);
+                         });
+    if (itnc2 != nc.end()) {
+      assert(*itnc1 != *itnc2);
+      *itnc1 = combination;
+      nc.erase(itnc2);
+    }
+  }
+
+  for (const auto& _nc : nc) proj->add(_nc);
+}
+
+ConstraintSetPtr_t Edge::buildConfigConstraint() {
+  return buildTargetConstraint();
+}
+
+ConstraintSetPtr_t Edge::buildTargetConstraint() {
+  std::string n = "(" + name() + ")";
+  GraphPtr_t g = graph_.lock();
+
+  ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n);
+
+  ConfigProjectorPtr_t proj = ConfigProjector::create(
+      g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+  std::vector<GraphComponentPtr_t> components;
+  components.push_back(g);
+  components.push_back(wkPtr_.lock());
+  components.push_back(stateTo());
+  if (state() != stateTo()) {
+    components.push_back(state());
+  }
+  // Copy constraints from
+  // - graph,
+  // - this edge,
+  // - the destination state,
+  // - the state in which the transition lies if different
+  mergeConstraintsIntoConfigProjector(proj, components, parentGraph());
+
+  constraint->addConstraint(proj);
+  constraint->edge(wkPtr_.lock());
+  return constraint;
+}
+
+ConstraintSetPtr_t Edge::pathConstraint() const {
+  throwIfNotInitialized();
+  return pathConstraints_;
+}
+
+ConstraintSetPtr_t Edge::buildPathConstraint() {
+  std::string n = "(" + name() + ")";
+  GraphPtr_t g = graph_.lock();
+
+  ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n);
+
+  ConfigProjectorPtr_t proj = ConfigProjector::create(
+      g->robot(), "proj_" + n, .5 * g->errorThreshold(), g->maxIterations());
+  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());
+
+  // Build steering method
+  const ProblemPtr_t& problem(g->problem());
+  steeringMethod_ =
+      problem->manipulationSteeringMethod()->innerSteeringMethod()->copy();
+  steeringMethod_->constraints(constraint);
+  // Build path validation and relative motion matrix
+  // TODO this path validation will not contain obstacles added after
+  // its creation.
+  pathValidation_ = problem->pathValidationFactory();
+  shared_ptr<core::ObstacleUserInterface> oui =
+      HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
+  if (oui) {
+    relMotion_ = RelativeMotion::matrix(g->robot());
+    RelativeMotion::fromConstraint(relMotion_, g->robot(), constraint);
+    oui->filterCollisionPairs(relMotion_);
+    oui->setSecurityMargins(securityMargins_);
+  }
+  return constraint;
+}
+
+bool Edge::canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const {
+  ConstraintSetPtr_t constraints = pathConstraint();
+  constraints->configProjector()->rightHandSideFromConfig(q1);
+  if (!constraints->isSatisfied(q1) || !constraints->isSatisfied(q2)) {
+    return false;
+  }
+  return true;
+}
+
+bool Edge::build(core::PathPtr_t& path, ConfigurationIn_t q1,
+                 ConfigurationIn_t q2) const {
+  using pinocchio::displayConfig;
+  if (!steeringMethod_) {
+    std::ostringstream oss;
+    oss << "No steering method set in edge " << name() << ".";
+    throw std::runtime_error(oss.str().c_str());
+  }
+  ConstraintSetPtr_t constraints = pathConstraint();
+  constraints->configProjector()->rightHandSideFromConfig(q1);
+  if (constraints->isSatisfied(q1)) {
+    if (constraints->isSatisfied(q2)) {
+      path = (*steeringMethod_)(q1, q2);
+      return (bool)path;
+    } else {
+      hppDout(info, "q2 = " << displayConfig(q2)
+                            << " does not satisfy the constraints of edge "
+                            << name());
+      hppDout(info, "q1 = " << displayConfig(q1));
+      return false;
+    }
+  } else {
+    value_type th(constraints->configProjector()->errorThreshold());
+    if (!constraints->configProjector()->isSatisfied(q1, 2 * th)) {
+      std::ostringstream oss;
+      oss << "The initial configuration " << displayConfig(q1)
+          << " does not satisfy the constraints of"
+             " edge "
+          << name() << "." << std::endl;
+      oss << "The graph is probably malformed";
+      throw std::runtime_error(oss.str().c_str());
+    }
+    // q1 may slightly violate the edge constraint eventhough the graph
+    // is well constructed.
+    return false;
+  }
+}
+
+bool Edge::applyConstraints(core::NodePtr_t nStart,
+                            ConfigurationOut_t q) const {
+  return generateTargetConfig(*(nStart->configuration()), q);
+}
+
+bool Edge::applyConstraints(ConfigurationIn_t qoffset,
+                            ConfigurationOut_t q) const {
+  return generateTargetConfig(qoffset, q);
+}
+
+bool Edge::generateTargetConfig(core::NodePtr_t nStart,
+                                ConfigurationOut_t q) const {
+  return generateTargetConfig(*(nStart->configuration()), q);
+}
+
+bool Edge::generateTargetConfig(ConfigurationIn_t qStart,
+                                ConfigurationOut_t q) const {
+  ConstraintSetPtr_t c = targetConstraint();
+  ConfigProjectorPtr_t proj = c->configProjector();
+  proj->rightHandSideFromConfig(qStart);
+  if (isShort_) q = qStart;
+  if (c->apply(q)) return true;
+  const ::hpp::statistics::SuccessStatistics& ss = proj->statistics();
+  if (ss.nbFailure() > ss.nbSuccess()) {
+    hppDout(warning, c->name() << " fails often.\n" << ss);
+  } else {
+    hppDout(warning, c->name() << " succeeds at rate "
+                               << (value_type)(ss.nbSuccess()) /
+                                      (value_type)ss.numberOfObservations()
+                               << ".");
+  }
+  return false;
+}
+
+WaypointEdgePtr_t WaypointEdge::create(const std::string& name,
+                                       const GraphWkPtr_t& graph,
+                                       const StateWkPtr_t& from,
+                                       const StateWkPtr_t& to) {
+  WaypointEdge* ptr = new WaypointEdge(name);
+  WaypointEdgePtr_t shPtr(ptr);
+  ptr->init(shPtr, graph, from, to);
+  return shPtr;
+}
+
+void WaypointEdge::init(const WaypointEdgeWkPtr_t& weak,
+                        const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+                        const StateWkPtr_t& to) {
+  Edge::init(weak, graph, from, to);
+  nbWaypoints(0);
+  wkPtr_ = weak;
+}
+
+void WaypointEdge::initialize() {
+  Edge::initialize();
+  // Set error threshold of internal edge to error threshold of
+  // waypoint edge divided by number of edges.
+  assert(targetConstraint()->configProjector());
+  value_type eps(targetConstraint()->configProjector()->errorThreshold() /
+                 (value_type)edges_.size());
+  for (Edges_t::iterator it(edges_.begin()); it != edges_.end(); ++it) {
+    (*it)->initialize();
+    assert((*it)->targetConstraint());
+    assert((*it)->targetConstraint()->configProjector());
+    (*it)->targetConstraint()->configProjector()->errorThreshold(eps);
+  }
+}
+
+bool WaypointEdge::canConnect(ConfigurationIn_t q1,
+                              ConfigurationIn_t q2) const {
+  /// TODO: This is not correct
+  for (std::size_t i = 0; i < edges_.size(); ++i)
+    if (!edges_[i]->canConnect(q1, q2)) return false;
+  return true;
+}
+
+bool WaypointEdge::build(core::PathPtr_t& path, ConfigurationIn_t q1,
+                         ConfigurationIn_t q2) const {
+  core::PathPtr_t p;
+  core::PathVectorPtr_t pv =
+      core::PathVector::create(graph_.lock()->robot()->configSize(),
+                               graph_.lock()->robot()->numberDof());
+  // Many times, this will be called rigth after
+  // WaypointEdge::generateTargetConfig so config_ already satisfies the
+  // constraints.
+  size_type n = edges_.size();
+  assert(configs_.cols() == n + 1);
+  bool useCache = lastSucceeded_ && configs_.col(0).isApprox(q1) &&
+                  configs_.col(n).isApprox(q2);
+  configs_.col(0) = q1;
+  configs_.col(n) = q2;
+
+  for (size_type i = 0; i < n; ++i) {
+    if (i < (n - 1) && !useCache) configs_.col(i + 1) = q2;
+    if (i < (n - 1) && !edges_[i]->generateTargetConfig(configs_.col(i),
+                                                        configs_.col(i + 1))) {
+      hppDout(info, "Waypoint edge "
+                        << name()
+                        << ": generateTargetConfig failed at waypoint " << i
+                        << "."
+                        << "\nUse cache: " << useCache);
+      lastSucceeded_ = false;
+      return false;
+    }
+    assert(targetConstraint());
+    assert(targetConstraint()->configProjector());
+    value_type eps(targetConstraint()->configProjector()->errorThreshold());
+    if ((configs_.col(i) - configs_.col(i + 1)).squaredNorm() > eps * eps) {
+      if (!edges_[i]->build(p, configs_.col(i), configs_.col(i + 1))) {
+        hppDout(info, "Waypoint edge "
+                          << name() << ": build failed at waypoint " << i << "."
+                          << "\nUse cache: " << useCache);
+        lastSucceeded_ = false;
         return false;
       }
-
-      WaypointEdgePtr_t WaypointEdge::create (const std::string& name,
-       const GraphWkPtr_t& graph, const StateWkPtr_t& from,
-       const StateWkPtr_t& to)
-      {
-        WaypointEdge* ptr = new WaypointEdge (name);
-        WaypointEdgePtr_t shPtr (ptr);
-        ptr->init(shPtr, graph, from, to);
-        return shPtr;
-      }
-
-      void WaypointEdge::init (const WaypointEdgeWkPtr_t& weak,
-			       const GraphWkPtr_t& graph,
-			       const StateWkPtr_t& from,
-			       const StateWkPtr_t& to)
-      {
-        Edge::init (weak, graph, from, to);
-        nbWaypoints(0);
-        wkPtr_ = weak;
-      }
-
-      void WaypointEdge::initialize ()
-      {
-        Edge::initialize();
-        // Set error threshold of internal edge to error threshold of
-        // waypoint edge divided by number of edges.
-        assert(targetConstraint()->configProjector());
-        value_type eps(targetConstraint()->configProjector()
-                       ->errorThreshold()/(value_type)edges_.size());
-        for(Edges_t::iterator it(edges_.begin()); it != edges_.end(); ++it){
-          (*it)->initialize();
-          assert ((*it)->targetConstraint());
-          assert ((*it)->targetConstraint()->configProjector());
-          (*it)->targetConstraint()->configProjector()->errorThreshold(eps);
-        }
-      }
-
-      bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
-      {
-        /// TODO: This is not correct
-        for (std::size_t i = 0; i < edges_.size (); ++i)
-          if (!edges_[i]->canConnect(q1, q2)) return false;
-        return true;
-      }
-
-      bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
-          ConfigurationIn_t q2) const
-      {
-        core::PathPtr_t p;
-        core::PathVectorPtr_t pv = core::PathVector::create
-          (graph_.lock ()->robot ()->configSize (),
-           graph_.lock ()->robot ()->numberDof ());
-        // Many times, this will be called rigth after WaypointEdge::generateTargetConfig so config_
-        // already satisfies the constraints.
-        size_type n = edges_.size();
-        assert (configs_.cols() == n + 1);
-        bool useCache = lastSucceeded_
-          && configs_.col(0).isApprox (q1)
-          && configs_.col(n).isApprox (q2);
-        configs_.col(0) = q1;
-        configs_.col(n) = q2;
-
-        for (size_type i = 0; i < n; ++i) {
-          if (i < (n-1) && !useCache) configs_.col (i+1) = q2;
-          if (i < (n-1) && !edges_[i]->generateTargetConfig (configs_.col(i), configs_.col (i+1))) {
-            hppDout (info, "Waypoint edge " << name() << ": generateTargetConfig failed at waypoint " << i << "."
-                << "\nUse cache: " << useCache
-                );
-            lastSucceeded_ = false;
-            return false;
-          }
-          assert (targetConstraint());
-          assert (targetConstraint()->configProjector ());
-          value_type eps
-            (targetConstraint()->configProjector ()->errorThreshold ());
-          if ((configs_.col(i) - configs_.col (i+1)).squaredNorm () > eps*eps) {
-            if (!edges_[i]->build (p, configs_.col(i), configs_.col (i+1))) {
-              hppDout (info, "Waypoint edge " << name()
-                       << ": build failed at waypoint " << i << "."
-                       << "\nUse cache: " << useCache);
-              lastSucceeded_ = false;
-              return false;
-            }
-            pv->appendPath (p);
-          }
-        }
-
-        path = pv;
-        lastSucceeded_ = true;
-        return true;
-      }
-
-      bool WaypointEdge::applyConstraints (ConfigurationIn_t qStart,
-                                           ConfigurationOut_t q) const
-      {
-        return generateTargetConfig(qStart,q);
-      }
-
-      bool WaypointEdge::generateTargetConfig (ConfigurationIn_t qStart,
-                                               ConfigurationOut_t q) const
-      {
-        assert (configs_.cols() == size_type(edges_.size() + 1));
-        configs_.col(0) = qStart;
-        for (std::size_t i = 0; i < edges_.size (); ++i) {
-          configs_.col (i+1) = q;
-          if (!edges_[i]->generateTargetConfig (configs_.col(i), configs_.col (i+1))) {
-            q = configs_.col(i+1);
-            lastSucceeded_ = false;
-            return false;
-          }
-        }
-        q = configs_.col(edges_.size());
-        lastSucceeded_ = true;
-        return true;
-      }
-
-      void WaypointEdge::nbWaypoints (const size_type number)
-      {
-        edges_.resize (number + 1);
-        states_.resize (number + 1);
-        states_.back() = stateTo();
-        const size_type nbDof = graph_.lock ()->robot ()->configSize ();
-        configs_ = matrix_t (nbDof, number + 2);
-        invalidate();
-      }
-
-      void WaypointEdge::setWaypoint (const std::size_t index,
-				      const EdgePtr_t wEdge,
-				      const StatePtr_t wTo)
-      {
-        assert (edges_.size() == states_.size());
-        assert (index < edges_.size());
-        if (index == states_.size() - 1) {
-          assert (!wTo || wTo == stateTo());
-        } else {
-          states_[index] = wTo;
-        }
-        edges_[index] = wEdge;
-      }
-
-      const EdgePtr_t& WaypointEdge::waypoint (const std::size_t index) const
-      {
-        assert (index < edges_.size());
-        return edges_[index];
-      }
-
-      std::ostream& WaypointEdge::print (std::ostream& os) const
-      {
-        os << "|   |   |-- ";
-        GraphComponent::print (os)
-          << " (waypoint) --> " << stateTo ()->name ();
-        return os;
-      }
-
-      std::ostream& WaypointEdge::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
-      {
-        // First print the waypoint node, then the first edge.
-        da ["style"]="dashed";
-        for (std::size_t i = 0; i < states_.size () - 1; ++i)
-          states_[i]->dotPrint (os, da);
-
-        da ["style"]="solid";
-        for (std::size_t i = 0; i < edges_.size (); ++i)
-          edges_[i]->dotPrint (os, da) << std::endl;
-
-        da ["style"]="dotted";
-        da ["dir"] = "both";
-        da ["arrowtail"]="dot";
-
-        return Edge::dotPrint (os, da);
-      }
-
-      std::ostream& LevelSetEdge::print (std::ostream& os) const
-      {
-        os << "|   |   |-- ";
-        GraphComponent::print (os)
-          << " (level set) --> " << stateTo ()->name ();
-        return os;
-      }
-
-      std::ostream& LevelSetEdge::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
-      {
-        da.insert ("shape", "onormal");
-        da.insert ("style", "dashed");
-        return Edge::dotPrint (os, da);
-      }
-
-      void LevelSetEdge::populateTooltip (dot::Tooltip& tp) const
-      {
-        GraphComponent::populateTooltip (tp);
-        tp.addLine ("");
-        tp.addLine ("Foliation condition constraints:");
-        for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
-            it != condNumericalConstraints_.end (); ++it) {
-          tp.addLine ("- " + (*it)->function ().name ());
-        }
-        tp.addLine ("Foliation parametrization constraints:");
-        for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
-            it != paramNumericalConstraints_.end (); ++it) {
-          tp.addLine ("- " + (*it)->function ().name ());
-        }
-      }
-
-      bool LevelSetEdge::applyConstraints(ConfigurationIn_t qStart,
-                                          ConfigurationOut_t q) const
-      {
-        return generateTargetConfig(qStart, q);
-      }
-
-      bool LevelSetEdge::generateTargetConfig(ConfigurationIn_t qStart,
-                                              ConfigurationOut_t q) const
-      {
-        // First, get an offset from the histogram
-        statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistrib ();
-        if (distrib.size () == 0) {
-          hppDout (warning, "Edge " << name() << ": Distrib is empty");
-          return false;
-        }
-        const Configuration_t& qLeaf = *(distrib ()->configuration ());
-
-        return generateTargetConfigOnLeaf (qStart, qLeaf, q);
-      }
-
-      bool LevelSetEdge::applyConstraints(core::NodePtr_t nStart,
-                                          ConfigurationOut_t q) const
-      {
-        return generateTargetConfig(nStart, q);
-      }
-
-      bool LevelSetEdge::generateTargetConfig(core::NodePtr_t nStart,
-                                              ConfigurationOut_t q) const
-      {
-      // First, get an offset from the histogram that is not in the same connected component.
-        statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (nStart->connectedComponent ());
-        if (distrib.size () == 0) {
-          hppDout (warning, "Edge " << name() << ": Distrib is empty");
-          return false;
-        }
-        const Configuration_t& qLeaf = *(distrib ()->configuration ()),
-                               qStart = *(nStart->configuration ());
-
-        return generateTargetConfigOnLeaf (qStart, qLeaf, q);
-      }
-
-      bool LevelSetEdge::generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
-                                                    ConfigurationIn_t qLeaf,
-                                                    ConfigurationOut_t q) const
-      {
-        // First, set the offset.
-        ConstraintSetPtr_t cs = targetConstraint();
-        const ConfigProjectorPtr_t cp = cs->configProjector ();
-        assert (cp);
-
-        // Set right hand side of edge constraints with qStart
-	cp->rightHandSideFromConfig (qStart);
-        // Set right hand side of constraints parameterizing the target state
-        // foliation with qLeaf.
-	for (NumericalConstraints_t::const_iterator it =
-	       paramNumericalConstraints_.begin ();
-	     it != paramNumericalConstraints_.end (); ++it) {
-          cp->rightHandSideFromConfig (*it, qLeaf);
-        }
-
-        // Eventually, do the projection.
-        if (isShort_) q = qStart;
-        if (cs->apply (q)) return true;
-	::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
-	if (ss.nbFailure () > ss.nbSuccess ()) {
-	  hppDout (warning, cs->name () << " fails often." << std::endl << ss);
-	} else {
-	  hppDout (warning, cs->name () << " succeeds at rate "
-		   << (value_type)(ss.nbSuccess ()) /
-		   (value_type) ss.numberOfObservations ()
-		   << ".");
-	}
-        return false;
-      }
-
-      void LevelSetEdge::init (const LevelSetEdgeWkPtr_t& weak,
-			       const GraphWkPtr_t& graph,
-			       const StateWkPtr_t& from,
-			       const StateWkPtr_t& to)
-      {
-        Edge::init (weak, graph, from, to);
-        wkPtr_ = weak;
-      }
-
-      LevelSetEdgePtr_t LevelSetEdge::create
-      (const std::string& name, const GraphWkPtr_t& graph,
-       const StateWkPtr_t& from, const StateWkPtr_t& to)
-      {
-        LevelSetEdge* ptr = new LevelSetEdge (name);
-        LevelSetEdgePtr_t shPtr (ptr);
-        ptr->init(shPtr, graph, from, to);
-        return shPtr;
-      }
-
-      LeafHistogramPtr_t LevelSetEdge::histogram () const
-      {
-        return hist_;
-      }
-
-      void LevelSetEdge::buildHistogram ()
-      {
-        Foliation f;
-
-        /// Build the constraint set.
-        std::string n = "(" + name () + ")";
-        GraphPtr_t g = graph_.lock ();
-
-        // The parametrizer
-        ConstraintSetPtr_t param = ConstraintSet::create (g->robot (), "Set " + n);
-
-        ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
-
-        for (const auto& nc : paramNumericalConstraints_)
-          proj->add (nc);
-
-        param->addConstraint (proj);
-        param->edge (wkPtr_.lock ());
-
-        f.parametrizer (param);
-
-        // The codition
-        // TODO: We assumed that this part of the code can only be reached by
-        // configurations that are valid.
-        // It would be wiser to make sure configurations are valid, for instance
-        // by considering only configurations in the destination state of this
-        // edge.
-        ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
-        proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
-
-        for (const auto& nc : condNumericalConstraints_)
-          proj->add (nc);
-
-        f.condition (cond);
-        cond->addConstraint (proj);
-
-        hppDout(info, "Build histogram of LevelSetEdge " << name()
-            << "\nParametrizer:\n" << *param
-            << "\nCondition:\n" << *cond
-            );
-
-        // TODO: If hist_ is not NULL, remove the previous Histogram.
-        // It should not be of any use and it slows down node insertion in the
-        // roadmap.
-        hist_ = LeafHistogram::create (f);
-        g->insertHistogram (hist_);
-      }
-
-      void LevelSetEdge::initialize ()
-      {
-        if (!isInit_) {
-          Edge::initialize();
-          buildHistogram ();
-        }
-      }
-
-      ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint()
-      {
-        return buildTargetConstraint();
-      }
-
-      ConstraintSetPtr_t LevelSetEdge::buildTargetConstraint()
-      {
-        std::string n = "(" + name () + ")";
-        GraphPtr_t g = graph_.lock ();
-
-        ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
-
-        ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
-
-        // Copy constraints from
-        // - graph,
-        // - param numerical constraints
-        // - this edge,
-        // - the destination state,
-        // - the state in which the transition lies if different
-
-        g->insertNumericalConstraints (proj);
-        for (const auto& nc : paramNumericalConstraints_)
-          proj->add (nc);
-
-        insertNumericalConstraints (proj);
-        stateTo ()->insertNumericalConstraints (proj);
-        if (state () != stateTo ()) {
-	  state ()->insertNumericalConstraints (proj);
-	}
-        constraint->addConstraint (proj);
-
-        constraint->edge (wkPtr_.lock ());
-        return constraint;
-      }
-
-      void LevelSetEdge::insertParamConstraint
-      (const constraints::ImplicitPtr_t& nm)
-      {
-        invalidate ();
-        paramNumericalConstraints_.push_back (nm);
-      }
-
-      const NumericalConstraints_t& LevelSetEdge::paramConstraints() const
-      {
-        return paramNumericalConstraints_;
-      }
-
-      void LevelSetEdge::insertConditionConstraint
-      (const constraints::ImplicitPtr_t& nm)
-      {
-        invalidate ();
-        condNumericalConstraints_.push_back (nm);
-      }
-
-      const NumericalConstraints_t& LevelSetEdge::conditionConstraints() const
-      {
-        return condNumericalConstraints_;
-      }
-
-      LevelSetEdge::LevelSetEdge
-      (const std::string& name) :
-	Edge (name)
-      {
-      }
-
-      LevelSetEdge::~LevelSetEdge ()
-      {}
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+      pv->appendPath(p);
+    }
+  }
+
+  path = pv;
+  lastSucceeded_ = true;
+  return true;
+}
+
+bool WaypointEdge::applyConstraints(ConfigurationIn_t qStart,
+                                    ConfigurationOut_t q) const {
+  return generateTargetConfig(qStart, q);
+}
+
+bool WaypointEdge::generateTargetConfig(ConfigurationIn_t qStart,
+                                        ConfigurationOut_t q) const {
+  assert(configs_.cols() == size_type(edges_.size() + 1));
+  configs_.col(0) = qStart;
+  for (std::size_t i = 0; i < edges_.size(); ++i) {
+    configs_.col(i + 1) = q;
+    if (!edges_[i]->generateTargetConfig(configs_.col(i),
+                                         configs_.col(i + 1))) {
+      q = configs_.col(i + 1);
+      lastSucceeded_ = false;
+      return false;
+    }
+  }
+  q = configs_.col(edges_.size());
+  lastSucceeded_ = true;
+  return true;
+}
+
+void WaypointEdge::nbWaypoints(const size_type number) {
+  edges_.resize(number + 1);
+  states_.resize(number + 1);
+  states_.back() = stateTo();
+  const size_type nbDof = graph_.lock()->robot()->configSize();
+  configs_ = matrix_t(nbDof, number + 2);
+  invalidate();
+}
+
+void WaypointEdge::setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
+                               const StatePtr_t wTo) {
+  assert(edges_.size() == states_.size());
+  assert(index < edges_.size());
+  if (index == states_.size() - 1) {
+    assert(!wTo || wTo == stateTo());
+  } else {
+    states_[index] = wTo;
+  }
+  edges_[index] = wEdge;
+}
+
+const EdgePtr_t& WaypointEdge::waypoint(const std::size_t index) const {
+  assert(index < edges_.size());
+  return edges_[index];
+}
+
+std::ostream& WaypointEdge::print(std::ostream& os) const {
+  os << "|   |   |-- ";
+  GraphComponent::print(os) << " (waypoint) --> " << stateTo()->name();
+  return os;
+}
+
+std::ostream& WaypointEdge::dotPrint(std::ostream& os,
+                                     dot::DrawingAttributes da) const {
+  // First print the waypoint node, then the first edge.
+  da["style"] = "dashed";
+  for (std::size_t i = 0; i < states_.size() - 1; ++i)
+    states_[i]->dotPrint(os, da);
+
+  da["style"] = "solid";
+  for (std::size_t i = 0; i < edges_.size(); ++i)
+    edges_[i]->dotPrint(os, da) << std::endl;
+
+  da["style"] = "dotted";
+  da["dir"] = "both";
+  da["arrowtail"] = "dot";
+
+  return Edge::dotPrint(os, da);
+}
+
+std::ostream& LevelSetEdge::print(std::ostream& os) const {
+  os << "|   |   |-- ";
+  GraphComponent::print(os) << " (level set) --> " << stateTo()->name();
+  return os;
+}
+
+std::ostream& LevelSetEdge::dotPrint(std::ostream& os,
+                                     dot::DrawingAttributes da) const {
+  da.insert("shape", "onormal");
+  da.insert("style", "dashed");
+  return Edge::dotPrint(os, da);
+}
+
+void LevelSetEdge::populateTooltip(dot::Tooltip& tp) const {
+  GraphComponent::populateTooltip(tp);
+  tp.addLine("");
+  tp.addLine("Foliation condition constraints:");
+  for (NumericalConstraints_t::const_iterator it =
+           condNumericalConstraints_.begin();
+       it != condNumericalConstraints_.end(); ++it) {
+    tp.addLine("- " + (*it)->function().name());
+  }
+  tp.addLine("Foliation parametrization constraints:");
+  for (NumericalConstraints_t::const_iterator it =
+           paramNumericalConstraints_.begin();
+       it != paramNumericalConstraints_.end(); ++it) {
+    tp.addLine("- " + (*it)->function().name());
+  }
+}
+
+bool LevelSetEdge::applyConstraints(ConfigurationIn_t qStart,
+                                    ConfigurationOut_t q) const {
+  return generateTargetConfig(qStart, q);
+}
+
+bool LevelSetEdge::generateTargetConfig(ConfigurationIn_t qStart,
+                                        ConfigurationOut_t q) const {
+  // First, get an offset from the histogram
+  statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib =
+      hist_->getDistrib();
+  if (distrib.size() == 0) {
+    hppDout(warning, "Edge " << name() << ": Distrib is empty");
+    return false;
+  }
+  const Configuration_t& qLeaf = *(distrib()->configuration());
+
+  return generateTargetConfigOnLeaf(qStart, qLeaf, q);
+}
+
+bool LevelSetEdge::applyConstraints(core::NodePtr_t nStart,
+                                    ConfigurationOut_t q) const {
+  return generateTargetConfig(nStart, q);
+}
+
+bool LevelSetEdge::generateTargetConfig(core::NodePtr_t nStart,
+                                        ConfigurationOut_t q) const {
+  // First, get an offset from the histogram that is not in the same connected
+  // component.
+  statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib =
+      hist_->getDistribOutOfConnectedComponent(nStart->connectedComponent());
+  if (distrib.size() == 0) {
+    hppDout(warning, "Edge " << name() << ": Distrib is empty");
+    return false;
+  }
+  const Configuration_t &qLeaf = *(distrib()->configuration()),
+                        qStart = *(nStart->configuration());
+
+  return generateTargetConfigOnLeaf(qStart, qLeaf, q);
+}
+
+bool LevelSetEdge::generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
+                                              ConfigurationIn_t qLeaf,
+                                              ConfigurationOut_t q) const {
+  // First, set the offset.
+  ConstraintSetPtr_t cs = targetConstraint();
+  const ConfigProjectorPtr_t cp = cs->configProjector();
+  assert(cp);
+
+  // Set right hand side of edge constraints with qStart
+  cp->rightHandSideFromConfig(qStart);
+  // Set right hand side of constraints parameterizing the target state
+  // foliation with qLeaf.
+  for (NumericalConstraints_t::const_iterator it =
+           paramNumericalConstraints_.begin();
+       it != paramNumericalConstraints_.end(); ++it) {
+    cp->rightHandSideFromConfig(*it, qLeaf);
+  }
+
+  // Eventually, do the projection.
+  if (isShort_) q = qStart;
+  if (cs->apply(q)) return true;
+  ::hpp::statistics::SuccessStatistics& ss = cp->statistics();
+  if (ss.nbFailure() > ss.nbSuccess()) {
+    hppDout(warning, cs->name() << " fails often." << std::endl << ss);
+  } else {
+    hppDout(warning, cs->name() << " succeeds at rate "
+                                << (value_type)(ss.nbSuccess()) /
+                                       (value_type)ss.numberOfObservations()
+                                << ".");
+  }
+  return false;
+}
+
+void LevelSetEdge::init(const LevelSetEdgeWkPtr_t& weak,
+                        const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+                        const StateWkPtr_t& to) {
+  Edge::init(weak, graph, from, to);
+  wkPtr_ = weak;
+}
+
+LevelSetEdgePtr_t LevelSetEdge::create(const std::string& name,
+                                       const GraphWkPtr_t& graph,
+                                       const StateWkPtr_t& from,
+                                       const StateWkPtr_t& to) {
+  LevelSetEdge* ptr = new LevelSetEdge(name);
+  LevelSetEdgePtr_t shPtr(ptr);
+  ptr->init(shPtr, graph, from, to);
+  return shPtr;
+}
+
+LeafHistogramPtr_t LevelSetEdge::histogram() const { return hist_; }
+
+void LevelSetEdge::buildHistogram() {
+  Foliation f;
+
+  /// Build the constraint set.
+  std::string n = "(" + name() + ")";
+  GraphPtr_t g = graph_.lock();
+
+  // The parametrizer
+  ConstraintSetPtr_t param = ConstraintSet::create(g->robot(), "Set " + n);
+
+  ConfigProjectorPtr_t proj = ConfigProjector::create(
+      g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
+
+  for (const auto& nc : paramNumericalConstraints_) proj->add(nc);
+
+  param->addConstraint(proj);
+  param->edge(wkPtr_.lock());
+
+  f.parametrizer(param);
+
+  // The codition
+  // TODO: We assumed that this part of the code can only be reached by
+  // configurations that are valid.
+  // It would be wiser to make sure configurations are valid, for instance
+  // by considering only configurations in the destination state of this
+  // edge.
+  ConstraintSetPtr_t cond = ConstraintSet::create(g->robot(), "Set " + n);
+  proj = ConfigProjector::create(g->robot(), "projCond_" + n,
+                                 g->errorThreshold(), g->maxIterations());
+
+  for (const auto& nc : condNumericalConstraints_) proj->add(nc);
+
+  f.condition(cond);
+  cond->addConstraint(proj);
+
+  hppDout(info, "Build histogram of LevelSetEdge " << name()
+                                                   << "\nParametrizer:\n"
+                                                   << *param << "\nCondition:\n"
+                                                   << *cond);
+
+  // TODO: If hist_ is not NULL, remove the previous Histogram.
+  // It should not be of any use and it slows down node insertion in the
+  // roadmap.
+  hist_ = LeafHistogram::create(f);
+  g->insertHistogram(hist_);
+}
+
+void LevelSetEdge::initialize() {
+  if (!isInit_) {
+    Edge::initialize();
+    buildHistogram();
+  }
+}
+
+ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint() {
+  return buildTargetConstraint();
+}
+
+ConstraintSetPtr_t LevelSetEdge::buildTargetConstraint() {
+  std::string n = "(" + name() + ")";
+  GraphPtr_t g = graph_.lock();
+
+  ConstraintSetPtr_t constraint = ConstraintSet::create(g->robot(), "Set " + n);
+
+  ConfigProjectorPtr_t proj = ConfigProjector::create(
+      g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+
+  // Copy constraints from
+  // - graph,
+  // - param numerical constraints
+  // - this edge,
+  // - the destination state,
+  // - the state in which the transition lies if different
+
+  g->insertNumericalConstraints(proj);
+  for (const auto& nc : paramNumericalConstraints_) proj->add(nc);
+
+  insertNumericalConstraints(proj);
+  stateTo()->insertNumericalConstraints(proj);
+  if (state() != stateTo()) {
+    state()->insertNumericalConstraints(proj);
+  }
+  constraint->addConstraint(proj);
+
+  constraint->edge(wkPtr_.lock());
+  return constraint;
+}
+
+void LevelSetEdge::insertParamConstraint(const constraints::ImplicitPtr_t& nm) {
+  invalidate();
+  paramNumericalConstraints_.push_back(nm);
+}
+
+const NumericalConstraints_t& LevelSetEdge::paramConstraints() const {
+  return paramNumericalConstraints_;
+}
+
+void LevelSetEdge::insertConditionConstraint(
+    const constraints::ImplicitPtr_t& nm) {
+  invalidate();
+  condNumericalConstraints_.push_back(nm);
+}
+
+const NumericalConstraints_t& LevelSetEdge::conditionConstraints() const {
+  return condNumericalConstraints_;
+}
+
+LevelSetEdge::LevelSetEdge(const std::string& name) : Edge(name) {}
+
+LevelSetEdge::~LevelSetEdge() {}
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index 1bb99ca7e0ffb972e7cd702587789c5a6ee12732..7faebc7db5ca8acca8526dcd06463366b8f8d5aa 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -28,118 +28,94 @@
 
 #include "hpp/manipulation/graph/graph-component.hh"
 
+#include <hpp/constraints/differentiable-function.hh>
+#include <hpp/constraints/implicit.hh>
 #include <hpp/core/config-projector.hh>
 #include <hpp/core/constraint-set.hh>
-#include <hpp/constraints/implicit.hh>
-
-#include <hpp/constraints/differentiable-function.hh>
 
 #include "hpp/manipulation/graph/graph.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      typedef constraints::Implicit Implicit;
-      const std::string& GraphComponent::name() const
-      {
-        return name_;
-      }
-
-      std::ostream& GraphComponent::print (std::ostream& os) const
-      {
-        os << id () << " : " << name ();
-        return os;
-      }
-
-      std::ostream& GraphComponent::dotPrint (std::ostream& os, dot::DrawingAttributes) const
-      {
-        os << id ();
-        return os;
-      }
-
-      void GraphComponent::setDirty()
-      {
-        invalidate();
-      }
-
-      void GraphComponent::addNumericalConstraint (const ImplicitPtr_t& nm)
-      {
-        invalidate();
-        numericalConstraints_.push_back(nm);
-      }
-
-      void GraphComponent::addNumericalCost (const ImplicitPtr_t& cost)
-      {
-        invalidate();
-        numericalCosts_.push_back(cost);
-      }
-
-      void GraphComponent::resetNumericalConstraints ()
-      {
-        invalidate();
-	numericalConstraints_.clear();
-        numericalCosts_.clear();
-      }
-
-      bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
-      {
-        for (const auto& nc : numericalConstraints_)
-          proj->add (nc);
-        for (const auto& nc : numericalCosts_)
-          proj->add (nc, 1);
-        return !numericalConstraints_.empty ();
-      }
-
-      const NumericalConstraints_t& GraphComponent::numericalConstraints() const
-      {
-        return numericalConstraints_;
-      }
-
-      const NumericalConstraints_t& GraphComponent::numericalCosts() const
-      {
-        return numericalCosts_;
-      }
-
-      GraphPtr_t GraphComponent::parentGraph() const
-      {
-        return graph_.lock ();
-      }
-
-      void GraphComponent::parentGraph(const GraphWkPtr_t& parent)
-      {
-        graph_ = parent;
-        GraphPtr_t g = graph_.lock();
-        assert(g);
-        id_ = g->components().size();
-        g->components().push_back (wkPtr_);
-      }
-
-      void GraphComponent::init (const GraphComponentWkPtr_t& weak)
-      {
-        wkPtr_ = weak;
-      }
-
-      void GraphComponent::throwIfNotInitialized () const
-      {
-        if (!isInit_){
-          throw std::logic_error
-            ("The graph should have been initialized first.");
-        }
-      }
-
-      std::ostream& operator<< (std::ostream& os,
-          const hpp::manipulation::graph::GraphComponent& graphComp)
-      {
-        return graphComp.print (os);
-      }
-
-      void GraphComponent::populateTooltip (dot::Tooltip& tp) const
-      {
-        for (NumericalConstraints_t::const_iterator it = numericalConstraints_.begin ();
-            it != numericalConstraints_.end (); ++it) {
-          tp.addLine ("- " + (*it)->function ().name ());
-        }
-      }
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+namespace graph {
+typedef constraints::Implicit Implicit;
+const std::string& GraphComponent::name() const { return name_; }
+
+std::ostream& GraphComponent::print(std::ostream& os) const {
+  os << id() << " : " << name();
+  return os;
+}
+
+std::ostream& GraphComponent::dotPrint(std::ostream& os,
+                                       dot::DrawingAttributes) const {
+  os << id();
+  return os;
+}
+
+void GraphComponent::setDirty() { invalidate(); }
+
+void GraphComponent::addNumericalConstraint(const ImplicitPtr_t& nm) {
+  invalidate();
+  numericalConstraints_.push_back(nm);
+}
+
+void GraphComponent::addNumericalCost(const ImplicitPtr_t& cost) {
+  invalidate();
+  numericalCosts_.push_back(cost);
+}
+
+void GraphComponent::resetNumericalConstraints() {
+  invalidate();
+  numericalConstraints_.clear();
+  numericalCosts_.clear();
+}
+
+bool GraphComponent::insertNumericalConstraints(
+    ConfigProjectorPtr_t& proj) const {
+  for (const auto& nc : numericalConstraints_) proj->add(nc);
+  for (const auto& nc : numericalCosts_) proj->add(nc, 1);
+  return !numericalConstraints_.empty();
+}
+
+const NumericalConstraints_t& GraphComponent::numericalConstraints() const {
+  return numericalConstraints_;
+}
+
+const NumericalConstraints_t& GraphComponent::numericalCosts() const {
+  return numericalCosts_;
+}
+
+GraphPtr_t GraphComponent::parentGraph() const { return graph_.lock(); }
+
+void GraphComponent::parentGraph(const GraphWkPtr_t& parent) {
+  graph_ = parent;
+  GraphPtr_t g = graph_.lock();
+  assert(g);
+  id_ = g->components().size();
+  g->components().push_back(wkPtr_);
+}
+
+void GraphComponent::init(const GraphComponentWkPtr_t& weak) { wkPtr_ = weak; }
+
+void GraphComponent::throwIfNotInitialized() const {
+  if (!isInit_) {
+    throw std::logic_error("The graph should have been initialized first.");
+  }
+}
+
+std::ostream& operator<<(
+    std::ostream& os,
+    const hpp::manipulation::graph::GraphComponent& graphComp) {
+  return graphComp.print(os);
+}
+
+void GraphComponent::populateTooltip(dot::Tooltip& tp) const {
+  for (NumericalConstraints_t::const_iterator it =
+           numericalConstraints_.begin();
+       it != numericalConstraints_.end(); ++it) {
+    tp.addLine("- " + (*it)->function().name());
+  }
+}
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index bc309e75718802e27e0b034e324a2a685542215f..28e04620974775830b8e856e35c68321a7ec914b 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -28,276 +28,229 @@
 
 #include "hpp/manipulation/graph/graph.hh"
 
+#include <hpp/manipulation/constraint-set.hh>
 #include <hpp/util/assertion.hh>
 
-#include <hpp/manipulation/constraint-set.hh>
+#include "hpp/manipulation/graph/edge.hh"
 #include "hpp/manipulation/graph/state-selector.hh"
 #include "hpp/manipulation/graph/state.hh"
-#include "hpp/manipulation/graph/edge.hh"
 #include "hpp/manipulation/graph/statistics.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      typedef constraints::Implicit Implicit;
-      typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-      GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot,
-			       const ProblemPtr_t& problem)
-      {
-        Graph* ptr = new Graph (name, problem);
-        GraphPtr_t shPtr (ptr);
-        ptr->init (shPtr, robot);
-        shPtr->createStateSelector (name);
-        return shPtr;
-      }
-
-      void Graph::init (const GraphWkPtr_t& weak, DevicePtr_t robot)
-      {
-        GraphComponent::init (weak);
-        robot_ = robot;
-        wkPtr_ = weak;
-        parentGraph(wkPtr_);
-        insertHistogram(graph::HistogramPtr_t (
-              new graph::StateHistogram (wkPtr_.lock()))
-            );
-      }
-
-      void Graph::initialize()
-      {
-        hists_.clear ();
-        assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
-        for (std::size_t i = 1; i < components_.size(); ++i)
-          components_[i].lock()->initialize();
-        isInit_ = true;
-      }
-
-      void Graph::invalidate ()
-      {
-        for (std::size_t i = 1; i < components_.size(); ++i)
-        {
-          assert(components_[i].lock());
-          components_[i].lock()->invalidate();
-        }
-        isInit_ = false;
-      }
-
-      StateSelectorPtr_t Graph::createStateSelector (const std::string& name)
-      {
-        invalidate ();
-        stateSelector_ = StateSelector::create (name);
-        stateSelector_->parentGraph (wkPtr_);
-        return stateSelector_;
-      }
-
-      void Graph::stateSelector (StateSelectorPtr_t ns)
-      {
-        invalidate ();
-        stateSelector_ = ns;
-        stateSelector_->parentGraph (wkPtr_);
-      }
-
-      void Graph::maxIterations (size_type iterations)
-      {
-        invalidate ();
-        maxIterations_ = iterations;
-      }
-
-      size_type Graph::maxIterations () const
-      {
-        return maxIterations_;
-      }
-
-      void Graph::errorThreshold (const value_type& threshold)
-      {
-        invalidate ();
-        errorThreshold_ = threshold;
-      }
-
-      value_type Graph::errorThreshold () const
-      {
-        return errorThreshold_;
-      }
-
-      const DevicePtr_t& Graph::robot () const
-      {
-        return robot_;
-      }
-
-      void Graph::problem (const ProblemPtr_t& problem)
-      {
-        if (problem_ != problem) {
-          problem_ = problem;
-          invalidate();
-        }
-      }
-
-      const ProblemPtr_t& Graph::problem () const
-      {
-	return problem_;
-      }
-
-      StatePtr_t Graph::getState (ConfigurationIn_t config) const
-      {
-        if (!stateSelector_) throw std::runtime_error ("No StateSelector in Constraint Graph.");
-        return stateSelector_->getState (config);
-      }
-
-      StatePtr_t Graph::getState(RoadmapNodePtr_t coreNode) const
-      {
-        return stateSelector_->getState (coreNode);
-      }
-
-      Edges_t Graph::getEdges (const StatePtr_t& from, const StatePtr_t& to)
-	const
-      {
-        Edges_t edges;
-        for (Neighbors_t::const_iterator it = from->neighbors ().begin ();
-            it != from->neighbors ().end (); ++it) {
-          if (it->second->stateTo () == to)
-            edges.push_back (it->second);
-        }
-        return edges;
-      }
-
-      EdgePtr_t Graph::chooseEdge (RoadmapNodePtr_t from) const
-      {
-        return stateSelector_->chooseEdge (from);
-      }
-
-      void Graph::clearConstraintsAndComplement()
-      {
-        constraintsAndComplements_.clear();
-      }
-
-      void Graph::registerConstraints
-      (const ImplicitPtr_t& constraint,
-       const ImplicitPtr_t& complement,
-       const ImplicitPtr_t& both)
-      {
-        for (ConstraintsAndComplements_t::const_iterator it
-               (constraintsAndComplements_.begin ());
-             it != constraintsAndComplements_.end (); ++it) {
-          assert (it->constraint != constraint);
-        }
-        constraintsAndComplements_.push_back (ConstraintAndComplement_t
-                                              (constraint, complement, both));
-      }
-
-      bool Graph::isComplement (const ImplicitPtr_t& constraint,
+namespace manipulation {
+namespace graph {
+typedef constraints::Implicit Implicit;
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot,
+                         const ProblemPtr_t& problem) {
+  Graph* ptr = new Graph(name, problem);
+  GraphPtr_t shPtr(ptr);
+  ptr->init(shPtr, robot);
+  shPtr->createStateSelector(name);
+  return shPtr;
+}
+
+void Graph::init(const GraphWkPtr_t& weak, DevicePtr_t robot) {
+  GraphComponent::init(weak);
+  robot_ = robot;
+  wkPtr_ = weak;
+  parentGraph(wkPtr_);
+  insertHistogram(
+      graph::HistogramPtr_t(new graph::StateHistogram(wkPtr_.lock())));
+}
+
+void Graph::initialize() {
+  hists_.clear();
+  assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
+  for (std::size_t i = 1; i < components_.size(); ++i)
+    components_[i].lock()->initialize();
+  isInit_ = true;
+}
+
+void Graph::invalidate() {
+  for (std::size_t i = 1; i < components_.size(); ++i) {
+    assert(components_[i].lock());
+    components_[i].lock()->invalidate();
+  }
+  isInit_ = false;
+}
+
+StateSelectorPtr_t Graph::createStateSelector(const std::string& name) {
+  invalidate();
+  stateSelector_ = StateSelector::create(name);
+  stateSelector_->parentGraph(wkPtr_);
+  return stateSelector_;
+}
+
+void Graph::stateSelector(StateSelectorPtr_t ns) {
+  invalidate();
+  stateSelector_ = ns;
+  stateSelector_->parentGraph(wkPtr_);
+}
+
+void Graph::maxIterations(size_type iterations) {
+  invalidate();
+  maxIterations_ = iterations;
+}
+
+size_type Graph::maxIterations() const { return maxIterations_; }
+
+void Graph::errorThreshold(const value_type& threshold) {
+  invalidate();
+  errorThreshold_ = threshold;
+}
+
+value_type Graph::errorThreshold() const { return errorThreshold_; }
+
+const DevicePtr_t& Graph::robot() const { return robot_; }
+
+void Graph::problem(const ProblemPtr_t& problem) {
+  if (problem_ != problem) {
+    problem_ = problem;
+    invalidate();
+  }
+}
+
+const ProblemPtr_t& Graph::problem() const { return problem_; }
+
+StatePtr_t Graph::getState(ConfigurationIn_t config) const {
+  if (!stateSelector_)
+    throw std::runtime_error("No StateSelector in Constraint Graph.");
+  return stateSelector_->getState(config);
+}
+
+StatePtr_t Graph::getState(RoadmapNodePtr_t coreNode) const {
+  return stateSelector_->getState(coreNode);
+}
+
+Edges_t Graph::getEdges(const StatePtr_t& from, const StatePtr_t& to) const {
+  Edges_t edges;
+  for (Neighbors_t::const_iterator it = from->neighbors().begin();
+       it != from->neighbors().end(); ++it) {
+    if (it->second->stateTo() == to) edges.push_back(it->second);
+  }
+  return edges;
+}
+
+EdgePtr_t Graph::chooseEdge(RoadmapNodePtr_t from) const {
+  return stateSelector_->chooseEdge(from);
+}
+
+void Graph::clearConstraintsAndComplement() {
+  constraintsAndComplements_.clear();
+}
+
+void Graph::registerConstraints(const ImplicitPtr_t& constraint,
                                 const ImplicitPtr_t& complement,
-                                ImplicitPtr_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;
-      }
-
-      const ConstraintsAndComplements_t& Graph::constraintsAndComplements ()
-        const
-      {
-        return constraintsAndComplements_;
-      }
-
-      ConstraintSetPtr_t Graph::configConstraint (const StatePtr_t& state) const
-      {
-        return state->configConstraint ();
-      }
-
-      bool Graph::getConfigErrorForState (ConfigurationIn_t config,
-					  const StatePtr_t& state,
-					  vector_t& error) const
-      {
-	return configConstraint (state)->isSatisfied (config, error);
-      }
-
-      bool Graph::getConfigErrorForEdge (ConfigurationIn_t config,
-					 const EdgePtr_t& edge, vector_t& error) const
-      {
-	ConstraintSetPtr_t cs (pathConstraint (edge));
-	ConfigProjectorPtr_t cp (cs->configProjector ());
-	if (cp) cp->rightHandSideFromConfig (config);
-	return cs->isSatisfied (config, error);
-      }
-
-      bool Graph::getConfigErrorForEdgeTarget
-      (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-       const EdgePtr_t& edge, vector_t& error) const
-      {
-	ConstraintSetPtr_t cs (targetConstraint (edge));
-	ConfigProjectorPtr_t cp (cs->configProjector ());
-	if (cp) cp->rightHandSideFromConfig (leafConfig);
-	return cs->isSatisfied (config, error);
-      }
-
-      bool Graph::getConfigErrorForEdgeLeaf
-      (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-       const EdgePtr_t& edge, vector_t& error) const
-      {
-	ConstraintSetPtr_t cs (pathConstraint (edge));
-	ConfigProjectorPtr_t cp (cs->configProjector ());
-	if (cp) cp->rightHandSideFromConfig (leafConfig);
-	return cs->isSatisfied (config, error);
-      }
-
-      ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge) const
-      {
-        return edge->targetConstraint ();
-      }
-
-      ConstraintSetPtr_t Graph::targetConstraint (const EdgePtr_t& edge) const
-      {
-        return edge->targetConstraint ();
-      }
-
-      ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge) const
-      {
-        return edge->pathConstraint ();
-      }
-
-      GraphComponentWkPtr_t Graph::get(std::size_t id) const
-      {
-        if (id >= components_.size())
-          throw std::out_of_range ("ID out of range.");
-        return components_[id];
-      }
-
-      GraphComponents_t& Graph::components ()
-      {
-        return components_;
-      }
-
-      Graph::Graph (const std::string& name, const ProblemPtr_t& problem) :
-        GraphComponent (name), problem_ (problem)
-      {
-      }
-
-      std::ostream& Graph::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
-      {
-        da.separator = "; ";
-        da.openSection = "\n";
-        da.closeSection = ";\n";
-        dot::Tooltip tp; tp.addLine ("Graph constains:");
-        populateTooltip (tp);
-        da.insertWithQuote ("tooltip", tp.toStr());
-        os << "digraph " << id() << " {" << da;
-        stateSelector_->dotPrint (os);
-        os << "}" << std::endl;
-        return os;
-      }
-
-      std::ostream& Graph::print (std::ostream& os) const
-      {
-        return GraphComponent::print (os) << std::endl << *stateSelector_;
-      }
-    } // namespace graph
-  } // namespace manipulation
-
-} // namespace hpp
+                                const ImplicitPtr_t& both) {
+  for (ConstraintsAndComplements_t::const_iterator it(
+           constraintsAndComplements_.begin());
+       it != constraintsAndComplements_.end(); ++it) {
+    assert(it->constraint != constraint);
+  }
+  constraintsAndComplements_.push_back(
+      ConstraintAndComplement_t(constraint, complement, both));
+}
+
+bool Graph::isComplement(const ImplicitPtr_t& constraint,
+                         const ImplicitPtr_t& complement,
+                         ImplicitPtr_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;
+}
+
+const ConstraintsAndComplements_t& Graph::constraintsAndComplements() const {
+  return constraintsAndComplements_;
+}
+
+ConstraintSetPtr_t Graph::configConstraint(const StatePtr_t& state) const {
+  return state->configConstraint();
+}
+
+bool Graph::getConfigErrorForState(ConfigurationIn_t config,
+                                   const StatePtr_t& state,
+                                   vector_t& error) const {
+  return configConstraint(state)->isSatisfied(config, error);
+}
+
+bool Graph::getConfigErrorForEdge(ConfigurationIn_t config,
+                                  const EdgePtr_t& edge,
+                                  vector_t& error) const {
+  ConstraintSetPtr_t cs(pathConstraint(edge));
+  ConfigProjectorPtr_t cp(cs->configProjector());
+  if (cp) cp->rightHandSideFromConfig(config);
+  return cs->isSatisfied(config, error);
+}
+
+bool Graph::getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
+                                        ConfigurationIn_t config,
+                                        const EdgePtr_t& edge,
+                                        vector_t& error) const {
+  ConstraintSetPtr_t cs(targetConstraint(edge));
+  ConfigProjectorPtr_t cp(cs->configProjector());
+  if (cp) cp->rightHandSideFromConfig(leafConfig);
+  return cs->isSatisfied(config, error);
+}
+
+bool Graph::getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
+                                      ConfigurationIn_t config,
+                                      const EdgePtr_t& edge,
+                                      vector_t& error) const {
+  ConstraintSetPtr_t cs(pathConstraint(edge));
+  ConfigProjectorPtr_t cp(cs->configProjector());
+  if (cp) cp->rightHandSideFromConfig(leafConfig);
+  return cs->isSatisfied(config, error);
+}
+
+ConstraintSetPtr_t Graph::configConstraint(const EdgePtr_t& edge) const {
+  return edge->targetConstraint();
+}
+
+ConstraintSetPtr_t Graph::targetConstraint(const EdgePtr_t& edge) const {
+  return edge->targetConstraint();
+}
+
+ConstraintSetPtr_t Graph::pathConstraint(const EdgePtr_t& edge) const {
+  return edge->pathConstraint();
+}
+
+GraphComponentWkPtr_t Graph::get(std::size_t id) const {
+  if (id >= components_.size()) throw std::out_of_range("ID out of range.");
+  return components_[id];
+}
+
+GraphComponents_t& Graph::components() { return components_; }
+
+Graph::Graph(const std::string& name, const ProblemPtr_t& problem)
+    : GraphComponent(name), problem_(problem) {}
+
+std::ostream& Graph::dotPrint(std::ostream& os,
+                              dot::DrawingAttributes da) const {
+  da.separator = "; ";
+  da.openSection = "\n";
+  da.closeSection = ";\n";
+  dot::Tooltip tp;
+  tp.addLine("Graph constains:");
+  populateTooltip(tp);
+  da.insertWithQuote("tooltip", tp.toStr());
+  os << "digraph " << id() << " {" << da;
+  stateSelector_->dotPrint(os);
+  os << "}" << std::endl;
+  return os;
+}
+
+std::ostream& Graph::print(std::ostream& os) const {
+  return GraphComponent::print(os) << std::endl << *stateSelector_;
+}
+}  // namespace graph
+}  // namespace manipulation
+
+}  // namespace hpp
diff --git a/src/graph/guided-state-selector.cc b/src/graph/guided-state-selector.cc
index fcfea9368ec591bcb47dabfad604370ce4c45ace..9aa48e2c2b10460834fc8006074c83eb2241a00b 100644
--- a/src/graph/guided-state-selector.cc
+++ b/src/graph/guided-state-selector.cc
@@ -28,131 +28,126 @@
 
 #include "hpp/manipulation/graph/guided-state-selector.hh"
 
-#include <hpp/util/assertion.hh>
-#include <hpp/pinocchio/configuration.hh>
+#include <cstdlib>
 #include <hpp/core/steering-method.hh>
+#include <hpp/pinocchio/configuration.hh>
+#include <hpp/util/assertion.hh>
 
 #include "../astar.hh"
-#include "hpp/manipulation/roadmap.hh"
 #include "hpp/manipulation/roadmap-node.hh"
-
-#include <cstdlib>
+#include "hpp/manipulation/roadmap.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      GuidedStateSelectorPtr_t GuidedStateSelector::create(const std::string& name,
-          const core::RoadmapPtr_t& roadmap)
-      {
-        GuidedStateSelector* ptr = new GuidedStateSelector (name, roadmap);
-        GuidedStateSelectorPtr_t shPtr (ptr);
-        ptr->init (shPtr);
-        return shPtr;
-      }
+namespace manipulation {
+namespace graph {
+GuidedStateSelectorPtr_t GuidedStateSelector::create(
+    const std::string& name, const core::RoadmapPtr_t& roadmap) {
+  GuidedStateSelector* ptr = new GuidedStateSelector(name, roadmap);
+  GuidedStateSelectorPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
 
-      void GuidedStateSelector::init (const GuidedStateSelectorPtr_t& weak)
-      {
-        StateSelector::init (weak);
-        wkPtr_ = weak;
-      }
+void GuidedStateSelector::init(const GuidedStateSelectorPtr_t& weak) {
+  StateSelector::init(weak);
+  wkPtr_ = weak;
+}
 
-      void GuidedStateSelector::setStateList (const States_t& stateList)
-      {
-        stateList_ = stateList;
-      }
+void GuidedStateSelector::setStateList(const States_t& stateList) {
+  stateList_ = stateList;
+}
 
-      EdgePtr_t GuidedStateSelector::chooseEdge(RoadmapNodePtr_t from) const
-      {
-        if (stateList_.empty ()) return StateSelector::chooseEdge (from);
-        Astar::States_t list;
-        bool reverse = false;
-        if (from->connectedComponent () == roadmap_->initNode ()->connectedComponent ()) {
-          Astar alg (roadmap_->distance (), wkPtr_.lock(), static_cast <RoadmapNodePtr_t> (roadmap_->initNode ()));
-          list = alg.solution (from);
-        } else {
-          core::NodeVector_t::const_iterator itg = roadmap_->goalNodes ().begin ();
-          for (; itg != roadmap_->goalNodes ().end (); ++itg)
-            if ((*itg)->connectedComponent () == from->connectedComponent ())
-              break;
-          if (itg == roadmap_->goalNodes ().end ()) {
-            hppDout (error, "This configuration can reach neither the initial "
-                "configuration nor any of the goal configurations.");
-            return EdgePtr_t ();
-          }
-          reverse = true;
-          Astar alg (roadmap_->distance (), wkPtr_.lock(), from);
-          list = alg.solution (static_cast <RoadmapNodePtr_t> (*itg));
+EdgePtr_t GuidedStateSelector::chooseEdge(RoadmapNodePtr_t from) const {
+  if (stateList_.empty()) return StateSelector::chooseEdge(from);
+  Astar::States_t list;
+  bool reverse = false;
+  if (from->connectedComponent() ==
+      roadmap_->initNode()->connectedComponent()) {
+    Astar alg(roadmap_->distance(), wkPtr_.lock(),
+              static_cast<RoadmapNodePtr_t>(roadmap_->initNode()));
+    list = alg.solution(from);
+  } else {
+    core::NodeVector_t::const_iterator itg = roadmap_->goalNodes().begin();
+    for (; itg != roadmap_->goalNodes().end(); ++itg)
+      if ((*itg)->connectedComponent() == from->connectedComponent()) break;
+    if (itg == roadmap_->goalNodes().end()) {
+      hppDout(error,
+              "This configuration can reach neither the initial "
+              "configuration nor any of the goal configurations.");
+      return EdgePtr_t();
+    }
+    reverse = true;
+    Astar alg(roadmap_->distance(), wkPtr_.lock(), from);
+    list = alg.solution(static_cast<RoadmapNodePtr_t>(*itg));
+  }
+  list.erase(std::unique(list.begin(), list.end()), list.end());
+  // Check if the beginning of stateList is list
+  if (list.size() <= stateList_.size()) {
+    Neighbors_t nn;
+    if (reverse) {
+      States_t::const_reverse_iterator it1 = stateList_.rbegin();
+      Astar::States_t::const_reverse_iterator it2 = list.rbegin();
+      Astar::States_t::const_reverse_iterator itEnd2 = list.rend();
+      do {
+        if (*it1 != *it2) {
+          hppDout(error,
+                  "The target sequence of nodes does not end with "
+                  "the sequence of nodes to reach this configuration.");
+          return EdgePtr_t();
         }
-        list.erase (std::unique (list.begin(), list.end ()), list.end ());
-        // Check if the beginning of stateList is list
-        if (list.size() <= stateList_.size()) {
-          Neighbors_t nn;
-          if (reverse) {
-            States_t::const_reverse_iterator it1 = stateList_.rbegin ();
-            Astar::States_t::const_reverse_iterator it2 = list.rbegin();
-            Astar::States_t::const_reverse_iterator itEnd2 = list.rend();
-            do {
-              if (*it1 != *it2) {
-                hppDout (error, "The target sequence of nodes does not end with "
-                    "the sequence of nodes to reach this configuration.");
-                return EdgePtr_t ();
-              }
-              ++it1;
-            } while (++it2 != itEnd2);
-            StatePtr_t state = getState (from);
-            HPP_ASSERT (state == list.front ());
-            const Neighbors_t& n = state->neighbors();
-            /// You stay in the same state
-            for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
-              if (it->second->stateTo () == state)
-                nn.insert (it->second, it->first);
-            /// Go from state it1 to state
-            // The path will be build from state. So we must find an edge from
-            // state to it1, that will be reversely 
-            for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
-              if (it->second->stateTo () == *it1)
-                nn.insert (it->second, it->first);
-          } else {
-            States_t::const_iterator it1 = stateList_.begin ();
-            Astar::States_t::const_iterator it2 = list.begin();
-            Astar::States_t::const_iterator itEnd2 = list.end();
-            do {
-              if (*it1 != *it2) {
-                hppDout (error, "The target sequence of nodes does not start with "
-                    "the sequence of nodes to reach this configuration.");
-                return EdgePtr_t ();
-              }
-              ++it1;
-            } while (++it2 != itEnd2);
-            StatePtr_t state = getState (from);
-            HPP_ASSERT (state == list.back ());
-            const Neighbors_t& n = state->neighbors();
-            for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
-              /// You stay in the same state
-              /// or go from state to state it1 
-              if (it->second->stateTo () == state ||
-                  it->second->stateTo () == *it1)
-                nn.insert (it->second, it->first);
-          }
-          if (nn.size () > 0 && nn.totalWeight() > 0)
-            return nn ();
-          hppDout (error, "This state has no neighbors to get to an admissible states.");
+        ++it1;
+      } while (++it2 != itEnd2);
+      StatePtr_t state = getState(from);
+      HPP_ASSERT(state == list.front());
+      const Neighbors_t& n = state->neighbors();
+      /// You stay in the same state
+      for (Neighbors_t::const_iterator it = n.begin(); it != n.end(); ++it)
+        if (it->second->stateTo() == state) nn.insert(it->second, it->first);
+      /// Go from state it1 to state
+      // The path will be build from state. So we must find an edge from
+      // state to it1, that will be reversely
+      for (Neighbors_t::const_iterator it = n.begin(); it != n.end(); ++it)
+        if (it->second->stateTo() == *it1) nn.insert(it->second, it->first);
+    } else {
+      States_t::const_iterator it1 = stateList_.begin();
+      Astar::States_t::const_iterator it2 = list.begin();
+      Astar::States_t::const_iterator itEnd2 = list.end();
+      do {
+        if (*it1 != *it2) {
+          hppDout(error,
+                  "The target sequence of nodes does not start with "
+                  "the sequence of nodes to reach this configuration.");
+          return EdgePtr_t();
         }
-        return EdgePtr_t ();
-      }
+        ++it1;
+      } while (++it2 != itEnd2);
+      StatePtr_t state = getState(from);
+      HPP_ASSERT(state == list.back());
+      const Neighbors_t& n = state->neighbors();
+      for (Neighbors_t::const_iterator it = n.begin(); it != n.end(); ++it)
+        /// You stay in the same state
+        /// or go from state to state it1
+        if (it->second->stateTo() == state || it->second->stateTo() == *it1)
+          nn.insert(it->second, it->first);
+    }
+    if (nn.size() > 0 && nn.totalWeight() > 0) return nn();
+    hppDout(error,
+            "This state has no neighbors to get to an admissible states.");
+  }
+  return EdgePtr_t();
+}
 
-      std::ostream& GuidedStateSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
-      {
-        for (WeighedStates_t::const_iterator it = orderedStates_.begin();
-            orderedStates_.end() != it; ++it)
-          it->second->dotPrint (os);
-        return os;
-      }
+std::ostream& GuidedStateSelector::dotPrint(std::ostream& os,
+                                            dot::DrawingAttributes) const {
+  for (WeighedStates_t::const_iterator it = orderedStates_.begin();
+       orderedStates_.end() != it; ++it)
+    it->second->dotPrint(os);
+  return os;
+}
 
-      std::ostream& GuidedStateSelector::print (std::ostream& os) const
-      {
-        return StateSelector::print (os);
-      }
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+std::ostream& GuidedStateSelector::print(std::ostream& os) const {
+  return StateSelector::print(os);
+}
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 1a3ec3106d0a760ba13b8c1d3464acbb8d5ae62c..11723f73999c7f8ff4f6160d8789975dee5e1bc0 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -26,1075 +26,993 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/graph/helper.hh>
-
-#include <tr1/unordered_map>
-#include <tr1/unordered_set>
-
-#include <iterator>
 #include <array>
-
 #include <boost/regex.hpp>
-
-#include <pinocchio/multibody/model.hpp>
-
-#include <hpp/util/debug.hh>
-
-#include <hpp/pinocchio/gripper.hh>
-#include <pinocchio/multibody/model.hpp>
-
 #include <hpp/constraints/differentiable-function.hh>
 #include <hpp/constraints/locked-joint.hh>
-
-#include <hpp/manipulation/handle.hh>
-#include <hpp/manipulation/graph/state.hh>
 #include <hpp/manipulation/graph/edge.hh>
-#include <hpp/manipulation/graph/state-selector.hh>
 #include <hpp/manipulation/graph/guided-state-selector.hh>
+#include <hpp/manipulation/graph/helper.hh>
+#include <hpp/manipulation/graph/state-selector.hh>
+#include <hpp/manipulation/graph/state.hh>
+#include <hpp/manipulation/handle.hh>
 #include <hpp/manipulation/problem-solver.hh>
+#include <hpp/pinocchio/gripper.hh>
+#include <hpp/util/debug.hh>
+#include <iterator>
+#include <pinocchio/multibody/model.hpp>
+#include <tr1/unordered_map>
+#include <tr1/unordered_set>
 
-#define CASE_TO_STRING(var, value) ( (var & value) ? std::string(#value) : std::string() )
+#define CASE_TO_STRING(var, value) \
+  ((var & value) ? std::string(#value) : std::string())
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      namespace helper {
-        typedef constraints::Implicit Implicit;
-        typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-
-        template <bool forPath>
-          void addToComp
-          (const NumericalConstraints_t& nc, GraphComponentPtr_t comp)
-        {
-          if (nc.empty ()) return;
-          StatePtr_t n;
-          if (forPath) {
-            n = HPP_DYNAMIC_PTR_CAST (State, comp);
-            if (!n) throw std::logic_error ("Wrong type: expect a State");
-          }
-          for (const auto& c : nc)
-            if (c) {
-              if (forPath) n->addNumericalConstraintForPath (c);
-              else      comp->addNumericalConstraint (c);
-            }
-        }
-
-        template <bool param>
-          void specifyFoliation
-          (const NumericalConstraints_t& nc, LevelSetEdgePtr_t lse)
-        {
-          for (const auto& c : nc)
-            if (c) {
-              if (param) lse->insertParamConstraint (c);
-              else   lse->insertConditionConstraint (c);
-            }
-        }
-
-        void FoliatedManifold::addToState (StatePtr_t comp) const
-        {
-          addToComp<false>(nc, comp);
-          addToComp<false>(nc_path, comp);
-        }
-
-        void FoliatedManifold::addToEdge (EdgePtr_t comp) const
-        {
-          addToComp <false> (nc_fol, comp);
-        }
-
-        void FoliatedManifold::specifyFoliation (LevelSetEdgePtr_t lse) const
-        {
-          for (const auto& c : nc)
-            lse->insertConditionConstraint (c);
-          for (const auto& c : nc_fol)
-            lse->insertConditionConstraint (c);
-        }
-
-        namespace {
-          template <int gCase>
-            struct CaseTraits {
-              static const bool pregrasp = (gCase & WithPreGrasp);
-              static const bool preplace = (gCase & WithPrePlace);
-              static const bool intersec = !((gCase & NoGrasp) || (gCase & NoPlace));
-
-              static const bool valid =
-                   ( (gCase & WithPreGrasp) || (gCase & GraspOnly) || (gCase & NoGrasp) )
-                && ( (gCase & WithPrePlace) || (gCase & PlaceOnly) || (gCase & NoPlace) )
-                && !((gCase & NoGrasp) && (gCase & NoPlace));
-
-              static const std::size_t nbWaypoints = (pregrasp?1:0) + (intersec?1:0) + (preplace?1:0);
-              static const std::size_t Nstates = 2 + nbWaypoints;
-              static const std::size_t Nedges = 1 + nbWaypoints;
-              // static const std::size_t iNpregrasp = pregrasp?1 + 1:nbWaypoints;
-              // static const std::size_t iNpreplace = pregrasp?1 + 1:nbWaypoints;
-              typedef std::array <StatePtr_t, Nstates> StateArray;
-              typedef std::array <EdgePtr_t, Nedges> EdgeArray;
-
-              static inline const StatePtr_t& Npregrasp (const StateArray& n) { assert (pregrasp); return n[1]; }
-              static inline const StatePtr_t& Nintersec (const StateArray& n) { assert (intersec); return n[1 + (pregrasp?1:0)]; }
-              static inline const StatePtr_t& Npreplace (const StateArray& n) { assert (preplace); return n[1 + (pregrasp?1:0) + (intersec?1:0)]; }
-
-              static inline std::string caseToString ()
-              {
-                return CASE_TO_STRING (gCase, NoGrasp)
-                  +    CASE_TO_STRING (gCase, GraspOnly)
-                  +    CASE_TO_STRING (gCase, WithPreGrasp)
-                  + " - "
-                  +    CASE_TO_STRING (gCase, NoPlace)
-                  +    CASE_TO_STRING (gCase, PlaceOnly)
-                  +    CASE_TO_STRING (gCase, WithPrePlace);
-              }
-
-              static inline EdgePtr_t makeWE (
-                  const std::string& name,
-                  const StatePtr_t& from, const StatePtr_t& to,
-                  const size_type& w)
-              {
-                if (Nedges > 1) {
-                  WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
-                      (from->linkTo (name, to, w, WaypointEdge::create));
-                  we->nbWaypoints (nbWaypoints);
-                  return we;
-                } else return from->linkTo (name, to, w, Edge::create);
-              }
-
-              static inline StateArray makeWaypoints (
-                  const StatePtr_t& from, const StatePtr_t& to,
-                  const std::string& name)
-              {
-                StateSelectorPtr_t ns = from->parentGraph ()->stateSelector ();
-                StateArray states;
-                std::size_t r = 0;
-                states[r] = from; ++r;
-                if (pregrasp) {
-                  states[r] = ns->createState (name + "_pregrasp", true); ++r;
-                }
-                if (intersec) {
-                  states[r] = ns->createState (name + "_intersec", true); ++r;
-                }
-                if (preplace) {
-                  states[r] = ns->createState (name + "_preplace", true); ++r;
-                }
-                states[r] = to;
-                return states;
-              }
-
-              static inline EdgePtr_t makeLSEgrasp (const std::string& name,
-                  const StateArray& n, const EdgeArray& e,
-                  const size_type w, LevelSetEdgePtr_t& gls)
-              {
-                if (Nedges > 1) {
-                  const std::size_t T = (pregrasp?1:0) + (intersec?1:0);
-                  WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
-                    (n.front()->linkTo (name + "_ls", n.back(), w,
-                                        WaypointEdge::create));
-                  we->nbWaypoints (nbWaypoints);
-                  gls = linkWaypoint <LevelSetEdge> (n, T-1, T, name, "ls");
-                  for (std::size_t i = 0; i < Nedges; ++i)
-                    we->setWaypoint (i, e[i], n[i+1]);
-                  we->setWaypoint (T-1, gls, n[T]);
-                  gls->state (n.front());
-                  gls->setShort (pregrasp);
-                  return we;
-                } else {
-                  assert (gCase == (GraspOnly | NoPlace)
-                      && "Cannot implement a LevelSetEdge for grasping");
-                  gls = static_pointer_cast <LevelSetEdge>
-                    (n.front()->linkTo (name + "_ls", n.back(), w,
-                                        LevelSetEdge::create));
-                  return gls;
-                }
-              }
-
-              static inline EdgePtr_t makeLSEplace (const std::string& name,
-                  const StateArray& n, const EdgeArray& e,
-                  const size_type w, LevelSetEdgePtr_t& pls)
-              {
-                if (Nedges > 1) {
-                  const std::size_t T = (pregrasp?1:0) + (intersec?1:0);
-                  WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
-                    (n.back()->linkTo (name + "_ls", n.front(), w,
-                                       WaypointEdge::create));
-                  we->nbWaypoints (nbWaypoints);
-                  pls = linkWaypoint <LevelSetEdge> (n, T+1, T, name, "ls");
-                  // for (std::size_t i = Nedges - 1; i != 0; --i)
-                  for (std::size_t k = 0; k < Nedges; ++k) {
-                    std::size_t i = Nedges  - 1 - k;
-                    we->setWaypoint (Nedges - 1 - i, e[i], n[i]);
-                  }
-                  we->setWaypoint (Nedges - 1 - T, pls, n[T]);
-                  pls->state (n.back ());
-                  pls->setShort (preplace);
-                  return we;
-                } else {
-                  assert (gCase == (NoGrasp | PlaceOnly)
-                      && "Cannot implement a LevelSetEdge for placement");
-                  pls = static_pointer_cast <LevelSetEdge>
-                    (n.back()->linkTo (name + "_ls", n.front(), w,
-                                       LevelSetEdge::create));
-                  return pls;
-                }
-              }
-
-              template <typename EdgeType>
-              static inline shared_ptr<EdgeType> linkWaypoint (
-                  const StateArray& states,
-                  const std::size_t& iF, const std::size_t& iT,
-                  const std::string& prefix,
-                  const std::string& suffix = "")
-              {
-                std::stringstream ss;
-                ss << prefix << "_" << iF << iT;
-                if (suffix.length () > 0) ss << "_" << suffix;
-                return static_pointer_cast <EdgeType>
-                    (states[iF]->linkTo (ss.str(), states[iT], -1, EdgeType::create));
-              }
-
-              template <bool forward>
-              static inline EdgeArray linkWaypoints (
-                  const StateArray& states, const EdgePtr_t& edge,
-                  const std::string& name)
-              {
-                EdgeArray e;
-                WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(WaypointEdge, edge);
-                if (forward)
-                  for (std::size_t i = 0; i < Nedges; ++i) {
-                    e[i] = linkWaypoint <Edge> (states, i, i + 1, name);
-                    we->setWaypoint (i, e[i], states[i+1]);
-                  }
-                else
-                  // for (std::size_t i = Nedges - 1; i != 0; --i) {
-                  for (std::size_t k = 0; k < Nedges; ++k) {
-                    std::size_t i = Nedges  - 1 - k;
-                    e[i] = linkWaypoint <Edge> (states, i + 1, i, name);
-                    we->setWaypoint (Nedges - 1 - i, e[i], states[i]);
-                  }
-                return e;
-              }
-
-              static inline void setStateConstraints (const StateArray& n,
-                  const FoliatedManifold& g, const FoliatedManifold& pg,
-                  const FoliatedManifold& p, const FoliatedManifold& pp,
-                  const FoliatedManifold& m)
-              {
-                // From and to are not populated automatically
-                // to avoid duplicates.
-                if (pregrasp) {
-                  p .addToState (Npregrasp (n));
-                  pg.addToState (Npregrasp (n));
-                  m .addToState (Npregrasp (n));
-                }
-                if (intersec) {
-                  p .addToState (Nintersec (n));
-                  g .addToState (Nintersec (n));
-                  m .addToState (Nintersec (n));
-                }
-                if (preplace) {
-                  pp.addToState (Npreplace (n));
-                  g .addToState (Npreplace (n));
-                  m .addToState (Npreplace (n));
-                }
-              }
-
-              static inline void setEdgeConstraints (const EdgeArray& e,
-                  const FoliatedManifold& g, const FoliatedManifold& p,
-                  const FoliatedManifold& m)
-              {
-                // The border B
-                const std::size_t B = (pregrasp?1:0) + (intersec?1:0);
-                for (std::size_t i = 0; i < B     ; ++i) p.addToEdge (e[i]);
-                for (std::size_t i = B; i < Nedges; ++i) g.addToEdge (e[i]);
-                for (std::size_t i = 0; i < Nedges; ++i) m.addToEdge (e[i]);
-              }
-
-              template <bool forward>
-              static inline void setEdgeProp
-              (const EdgeArray& e, const StateArray& n)
-              {
-                /// Last is short
-                const std::size_t K = (forward?1:0);
-                for (std::size_t i = K; i < Nedges - 1 + K; ++i)
-                  e[i]->setShort (true);
-                // The border B
-                std::size_t B;
-                if ((gCase & NoGrasp)) // There is no grasp
-                  B = 0;
-                else // There is a grasp
-                  B = 1 + (pregrasp?1:0);
-                for (std::size_t i = 0; i < B     ; ++i) e[i]->state (n[0]);
-                for (std::size_t i = B; i < Nedges; ++i) e[i]->state (n[Nstates-1]);
-              }
-            };
-        }
-
-        template <int gCase> Edges_t createEdges (
-              const std::string& forwName,   const std::string& backName,
-              const StatePtr_t& from,         const StatePtr_t& to,
-              const size_type& wForw,        const size_type& wBack,
-              const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
-              const FoliatedManifold& place, const FoliatedManifold& preplace,
-              const bool levelSetGrasp,      const bool levelSetPlace,
-              const FoliatedManifold& submanifoldDef)
-          {
-            typedef CaseTraits<gCase> T;
-            hppDout (info, "Creating edges " << forwName << " and " << backName
-               << "\ncase is " << T::caseToString ());
-            assert (T::valid && "Not a valid case.");
-            typedef typename T::StateArray StateArray;
-            typedef typename T::EdgeArray EdgeArray;
-
-            // Create the edges
-            EdgePtr_t weForw = T::makeWE (forwName, from, to, wForw),
-                      weBack = T::makeWE (backName, to, from, wBack),
-                      weForwLs, weBackLs;
-
-            std::string name = forwName;
-            StateArray n = T::makeWaypoints (from, to, name);
-
-            EdgeArray eF = T::template linkWaypoints <true> (n, weForw, name);
-
-            // Set the states constraints
-            // Note that submanifold is not taken into account for states
-            // because the edges constraints will enforce configuration to stay
-            // in a leaf, and so in the manifold itself.
-            T::setStateConstraints (n, grasp, pregrasp, place, preplace,
-                submanifoldDef);
-
-            // Set the edges properties
-            T::template setEdgeProp <true> (eF, n);
-
-            // Set the edges constraints
-            T::setEdgeConstraints (eF, grasp, place, submanifoldDef);
-
-            LevelSetEdgePtr_t gls;
-            if (levelSetGrasp)
-              weForwLs = T::makeLSEgrasp (name, n, eF, 10*wForw, gls);
-
-            // Populate bacward edge
-            name = backName;
-            EdgeArray eB = T::template linkWaypoints <false> (n, weBack, name);
-
-            T::template setEdgeProp <false> (eB, n);
-
-            T::setEdgeConstraints (eB, grasp, place, submanifoldDef);
-
-            LevelSetEdgePtr_t pls;
-            if (levelSetPlace)
-              weBackLs = T::makeLSEplace (name, n, eB, 10*wBack, pls);
-
-            Edges_t ret { weForw, weBack };
-
-            if (levelSetPlace) {
-              if (!place.foliated ()) {
-                hppDout (warning, "You asked for a LevelSetEdge for placement, "
-                    "but did not specify the target foliation. "
-                    "It will have no effect");
-              }
-              grasp.addToEdge (pls);
-              place.specifyFoliation (pls);
-              submanifoldDef.addToEdge (pls);
-              pls->buildHistogram ();
-              place.addToEdge (weBackLs);
-              submanifoldDef.addToEdge (weBackLs);
-              ret.push_back (weBackLs);
-            }
-            if (levelSetGrasp) {
-              if (!grasp.foliated ()) {
-                hppDout (warning, "You asked for a LevelSetEdge for grasping, "
-                    "but did not specify the target foliation. "
-                    "It will have no effect");
-              }
-              place.addToEdge (gls);
-              grasp.specifyFoliation (gls);
-              submanifoldDef.addToEdge (gls);
-              gls->buildHistogram ();
-              grasp.addToEdge (weForwLs);
-              submanifoldDef.addToEdge (weForwLs);
-              ret.push_back (weForwLs);
-            }
-
-            return ret;
-          }
-
-        EdgePtr_t createLoopEdge (
-              const std::string& loopName,
-              const StatePtr_t& state,
-              const size_type& w,
-              const bool levelSet,
-              const FoliatedManifold& submanifoldDef)
-        {
-          // Create the edges
-          EdgePtr_t loop;
-          if (levelSet)
-               loop = state->linkTo (loopName, state, w, LevelSetEdge::create);
-          else loop = state->linkTo (loopName, state, w, Edge::create);
-
-          loop->state (state);
-          submanifoldDef.addToEdge (loop);
-
-          if (levelSet) {
-            if (!submanifoldDef.foliated ()) {
-              hppDout (warning, "You asked for a LevelSetEdge for looping, "
-                  "but did not specify the target foliation. "
-                  "It will have no effect");
-            }
-            LevelSetEdgePtr_t ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge, loop);
-            submanifoldDef.specifyFoliation (ls);
-            ls->buildHistogram ();
-          }
-
-          return loop;
-        }
-
-        void graspManifold (
-            const GripperPtr_t& gripper, const HandlePtr_t& handle,
-            FoliatedManifold& grasp, FoliatedManifold& pregrasp)
-        {
-          ImplicitPtr_t gc  = handle->createGrasp (gripper, "");
-          grasp.nc.push_back (gc);
-          grasp.nc_path.push_back (gc);
-          ImplicitPtr_t gcc = handle->createGraspComplement
-            (gripper, "");
-          if (gcc->function ().outputSize () > 0)
-            grasp.nc_fol.push_back (gcc);
-
-          const value_type c = handle->clearance () + gripper->clearance ();
-          ImplicitPtr_t pgc = handle->createPreGrasp (gripper, c, "");
-          pregrasp.nc.push_back (pgc);
-          pregrasp.nc_path.push_back (pgc);
-        }
-
-        void strictPlacementManifold (
-            const ImplicitPtr_t placement,
-            const ImplicitPtr_t preplacement,
-            const ImplicitPtr_t placementComplement,
-            FoliatedManifold& place, FoliatedManifold& preplace)
-        {
-          place.nc.push_back (placement);
-          place.nc_path.push_back (placement);
-          if (placementComplement && placementComplement->function().outputSize () > 0)
-            place.nc_fol.push_back (placementComplement);
-
-          preplace.nc.push_back (preplacement);
-          preplace.nc_path.push_back (preplacement);
-        }
-
-        void relaxedPlacementManifold (
-            const ImplicitPtr_t placement,
-            const ImplicitPtr_t preplacement,
-            const LockedJoints_t objectLocks,
-            FoliatedManifold& place, FoliatedManifold& preplace)
-        {
-          if (placement) {
-            place.nc.push_back (placement);
-            // The placement constraints are not required in the path, as long as
-            // they are satisfied at both ends and the object does not move. The
-            // former condition is ensured by the placement constraints on both
-            // ends and the latter is ensure by the LockedJoint constraints.
-            place.nc_path.push_back (placement);
-          }
-          std::copy (objectLocks.begin(), objectLocks.end(), std::back_inserter(place.lj_fol));
-
-          if (placement && preplacement) {
-            preplace.nc.push_back (preplacement);
-            // preplace.nc_path.push_back (preplacement);
-          }
-        }
-
-        namespace {
-          typedef std::size_t index_t;
-          typedef std::vector <index_t> IndexV_t;
-          typedef std::list <index_t> IndexL_t;
-          typedef std::pair <index_t, index_t> Grasp_t;
-          typedef std::tuple <StatePtr_t, FoliatedManifold> StateAndManifold_t;
-          //typedef std::vector <index_t, index_t> GraspV_t;
-          /// GraspV_t corresponds to a unique ID of a  permutation.
-          /// - its size is the number of grippers,
-          /// - the values correpond to the index of the handle (0..nbHandle-1), or
-          ///   nbHandle to mean no handle. 
-          typedef std::vector <index_t> GraspV_t;
-          struct Result;
-          struct CompiledRule {
-            enum Status {
-              Accept,
-              Refuse,
-              NoMatch,
-              Undefined
-            };
-            std::vector<boost::regex> handles;
-            Status status;
-            CompiledRule (const Result& res, const Rule& r);
-            Status check (const std::vector<std::string>& names, const GraspV_t& g) const
-            {
-              const std::size_t nG = g.size();
-              assert(nG == handles.size());
-              for (std::size_t i = 0; i < nG; ++i) {
-                if (handles[i].empty()) continue;
-                if (!boost::regex_match(names[g[i]], handles[i]))
-                  return NoMatch;
-              }
-              return status;
-            }
-          };
-          typedef std::vector<CompiledRule> CompiledRules_t;
-
-          struct Result {
-            ProblemSolverPtr_t ps;
-            GraphPtr_t graph;
-            typedef unsigned long stateid_type;
-            std::tr1::unordered_map<stateid_type, StateAndManifold_t> states;
-            typedef std::pair<stateid_type, stateid_type> edgeid_type;
-            struct edgeid_hash {
-              std::tr1::hash<edgeid_type::first_type> first;
-              std::tr1::hash<edgeid_type::second_type> second;
-              std::size_t operator() (const edgeid_type& eid) const {
-                return first(eid.first) + second(eid.second);
-              }
-            };
-            std::tr1::unordered_set<edgeid_type, edgeid_hash> edges;
-            std::vector< std::array<ImplicitPtr_t,3> > graspCs;
-            index_t nG, nOH;
-            GraspV_t dims;
-            const Grippers_t& gs;
-            const Objects_t& ohs;
-            std::vector<std::string> handleNames;
-            CompiledRules_t rules;
-            CompiledRule::Status defaultAcceptationPolicy;
-
-            Result (const ProblemSolverPtr_t problem, const Grippers_t& grippers, const Objects_t& objects, GraphPtr_t g) :
-              ps (problem), graph (g), nG (grippers.size ()), nOH (0), gs (grippers), ohs (objects),
-              defaultAcceptationPolicy (CompiledRule::Refuse)
-            {
-              for (const Object_t& o : objects) {
-                nOH += std::get<1>(o).size();
-                for (const HandlePtr_t& h : std::get<1>(o))
-                  handleNames.push_back(h->name());
-              }
-              handleNames.push_back("");
-              dims.resize (nG);
-              dims[0] = nOH + 1;
-              for (index_t i = 1; i < nG; ++i)
-                dims[i] = dims[i-1] * (nOH + 1);
-              graspCs.resize (nG * nOH);
-            }
-
-            void setRules (const Rules_t& r)
-            {
-              for (Rules_t::const_iterator _r = r.begin(); _r != r.end(); ++_r)
-                rules.push_back (CompiledRule(*this, *_r));
-            }
-
-            bool graspIsAllowed (const GraspV_t& idxOH) const
-            {
-              assert (idxOH.size () == nG);
-              for (std::size_t r = 0; r < rules.size(); ++r) {
-                switch (rules[r].check(handleNames,idxOH)) {
-                  case CompiledRule::Accept : return true;
-                  case CompiledRule::Refuse : return false;
-                  case CompiledRule::NoMatch: continue; // Check next rule
-                  default: throw std::invalid_argument ("Rules are ill-defined.");
-                }
-              }
-              return (defaultAcceptationPolicy == CompiledRule::Accept);
-            }
-
-            inline stateid_type stateid (const GraspV_t& iG)
-            {
-              stateid_type iGOH = iG[0];
-              stateid_type res;
-              for (index_t i = 1; i < nG; ++i) {
-                res = iGOH + dims[i] * (iG[i]);
-                if (res < iGOH) {
-                  hppDout (info, "State ID overflowed. There are too many states...");
-                }
-                iGOH = res;
-                // iGOH += dims[i] * (iG[i]);
-              }
-              return iGOH;
-            }
-
-            bool hasState (const GraspV_t& iG)
-            {
-              return states.count(stateid(iG)) > 0;
-            }
-
-            StateAndManifold_t& operator() (const GraspV_t& iG)
-            {
-              return states [stateid(iG)];
-            }
-
-            bool hasEdge (const GraspV_t& g1, const GraspV_t& g2)
-            {
-              return edges.count(edgeid_type(stateid(g1), stateid(g2))) > 0;
-            }
-
-            void addEdge (const GraspV_t& g1, const GraspV_t& g2)
-            {
-              edges.insert(edgeid_type(stateid(g1), stateid(g2)));
-            }
-
-            inline std::array<ImplicitPtr_t,3>& graspConstraint (
-                const index_t& iG, const index_t& iOH)
-            {
-              std::array<ImplicitPtr_t,3>& gcs =
-                graspCs [iG * nOH + iOH];
-              if (!gcs[0]) {
-                hppDout (info, "Create grasps constraints for ("
-                    << iG << ", " << iOH << ")");
-                const GripperPtr_t& g (gs[iG]);
-                const HandlePtr_t& h (handle (iOH));
-                const std::string& grasp = g->name() + " grasps " + h->name();
-                if (!ps->numericalConstraints.has(grasp)) {
-                  ps->createGraspConstraint (grasp, g->name(), h->name());
-                }
-                gcs[0] = ps->numericalConstraints.get(grasp);
-                gcs[1] = ps->numericalConstraints.get(grasp + "/complement");
-                const std::string& pregrasp = g->name() + " pregrasps " + h->name();
-                if (!ps->numericalConstraints.has(pregrasp)) {
-                  ps->createPreGraspConstraint (pregrasp, g->name(), h->name());
-                }
-                gcs[2] = ps->numericalConstraints.get(pregrasp);
-              }
-              return gcs;
-            }
-
-            const Object_t& object (const index_t& iOH) const {
-              index_t iH = iOH;
-              for (const Object_t& o : ohs) {
-                if (iH < std::get<1>(o).size()) return o;
-                iH -= std::get<1>(o).size();
-              }
-              throw std::out_of_range ("Handle index");
-            }
-
-            const HandlePtr_t& handle (const index_t& iOH) const {
-              index_t iH = iOH;
-              for (const Object_t& o : ohs) {
-                if (iH < std::get<1>(o).size()) return std::get<1>(o)[iH];
-                iH -= std::get<1>(o).size();
-              }
-              throw std::out_of_range ("Handle index");
-            }
-
-            /// Check if an object can be placed
-            bool objectCanBePlaced (const Object_t& o) const
-            {
-              // If the object has no joint, then it cannot be placed.
-              return (std::get<2>(std::get<0>(o)).size() > 0);
-            }
-
-            /// Check is an object is grasped by the GraspV_t
-            bool isObjectGrasped (const GraspV_t& idxOH,
-                const Object_t& o) const
-            {
-              assert (idxOH.size () == nG);
-              for (std::size_t i = 0; i < idxOH.size (); ++i)
-                if (idxOH[i] < nOH) // This grippers grasps an object
-                  if (std::get<2>(o) == std::get<2>(object(idxOH[i])))
-                    return true;
-              return false;
-            }
-
-            /// Get a state name from a set of grasps
-            std::string name (const GraspV_t& idxOH, bool abbrev = false) const {
-              assert (idxOH.size () == nG);
-              std::stringstream ss;
-              bool first = true;
-              std::string sepGOH = (abbrev?"-":" grasps "),
-                          sep    = (abbrev?":":" : ");
-              for (std::size_t i = 0; i < idxOH.size (); ++i) {
-                if (idxOH[i] < nOH) { // This grippers grasps an object
-                  if (first) first = false; else ss << sep; 
-                  if (abbrev) ss << i << sepGOH << idxOH[i];
-                  else
-                    ss << gs[i]->name() << sepGOH << handle (idxOH[i])->name ();
-                }
-              }
-              if (first) return (abbrev?"f":"free");
-              return ss.str();
-            }
-
-            /// Get an edge name from a set of grasps
-            std::pair<std::string, std::string> name
-              (const GraspV_t& gFrom, const GraspV_t& gTo, const index_t iG)
-            {
-              const std::string nf (name (gFrom, true)),
-                                nt (name (gTo, true));
-              std::stringstream ssForw, ssBack;
-              const char sep[] = " | ";
-              ssForw << gs[iG]->name() << " > " << handle (gTo[iG])->name () << sep << nf;
-              ssBack << gs[iG]->name() << " < " << handle (gTo[iG])->name () << sep << nt;
-              return std::make_pair (ssForw.str(), ssBack.str ());
-            }
-
-            std::string nameLoopEdge (const GraspV_t& gFrom)
-            {
-              const std::string nf (name (gFrom, true));
-              std::stringstream ss;
-              const char sep[] = " | ";
-              ss << "Loop" << sep << nf;
-              return ss.str();
-            }
-
-            void graspManifold (const index_t& iG, const index_t& iOH,
-                FoliatedManifold& grasp, FoliatedManifold& pregrasp)
-            {
-              std::array<ImplicitPtr_t,3>& gcs
-                = graspConstraint (iG, iOH);
-              grasp.nc.push_back (gcs[0]);
-              grasp.nc_path.push_back (gcs[0]);
-              if (gcs[1]->function ().outputSize () > 0)
-                grasp.nc_fol.push_back (gcs[1]);
-
-              pregrasp.nc.push_back (gcs[2]);
-              pregrasp.nc_path.push_back (gcs[2]);
-            }
-          };
-
-          CompiledRule::CompiledRule (const Result& res, const Rule& r) :
-            handles(res.nG), status (r.link_ ? Accept : Refuse)
-          {
-            assert(r.grippers_.size() == r.handles_.size());
-            for (std::size_t j = 0; j < r.grippers_.size(); ++j) {
-              boost::regex gripper (r.grippers_[j]);
-              for (std::size_t i = 0; i < res.nG; ++i) {
-                if (boost::regex_match(res.gs[i]->name(), gripper)) {
-                  assert(handles[i].empty() && "Two gripper regex match the different gripper names.");
-                  handles[i] = r.handles_[j];
-                }
-              }
-            }
-          }
-
-          const StateAndManifold_t& makeState (Result& r, const GraspV_t& g,
-              const int priority)
-          {
-            StateAndManifold_t& nam = r (g);
-            if (!std::get<0>(nam)) {
-              hppDout (info, "Creating state " << r.name (g));
-              std::get<0>(nam) = r.graph->stateSelector ()->createState
-                (r.name (g), false, priority);
-              // Loop over the grippers and create grasping constraints if required
-              FoliatedManifold unused;
-              std::set <index_t> idxsOH;
-              for (index_t i = 0; i < r.nG; ++i) {
-                if (g[i] < r.nOH) {
-                  idxsOH.insert (g[i]);
-                  r.graspManifold (i, g[i], std::get<1>(nam), unused);
-                }
-              }
-              index_t iOH = 0;
-              for (const Object_t& o : r.ohs) {
-                if (!r.objectCanBePlaced(o)) continue;
-                bool oIsGrasped = false;
-                // TODO: use the fact that the set is sorted.
-                // for (const HandlePtr_t& h : std::get<0>(o))
-                for (index_t i = 0; i < std::get<1>(o).size(); ++i) {
-                  if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
-                  ++iOH;
-                }
-                if (!oIsGrasped) {
-                  const auto& pc (std::get<0>(o));
-                  relaxedPlacementManifold (std::get<0>(pc),
-                      std::get<1>(pc),
-                      std::get<2>(pc),
-                      std::get<1>(nam), unused);
-                }
-              }
-              std::get<1>(nam).addToState (std::get<0>(nam));
-
-              createLoopEdge (r.nameLoopEdge (g),
-                  std::get<0>(nam), 0,
-                  false,
-                  // TODO std::get<1>(nam).foliated(),
-                  std::get<1>(nam));
-            }
-            return nam;
-          }
-
-          /// Arguments are such that
-          /// \li gTo[iG] != gFrom[iG]
-          /// \li for all i != iG, gTo[iG] == gFrom[iG]
-          void makeEdge (Result& r,
-              const GraspV_t& gFrom, const GraspV_t& gTo,
-              const index_t iG, const int priority)
-          {
-            if (r.hasEdge(gFrom, gTo)) {
-              hppDout (warning, "Prevented creation of duplicated edge\nfrom "
-                  << r.name (gFrom) << "\nto " << r.name (gTo));
-              return;
-            }
-            const StateAndManifold_t& from = makeState (r, gFrom, priority),
-                                     to   = makeState (r, gTo, priority+1);
-            const Object_t& o = r.object (gTo[iG]);
-
-            // Detect when grasping an object already grasped.
-            // or when there is no placement information for it.
-            bool noPlace = !r.objectCanBePlaced(o)
-                         || r.isObjectGrasped (gFrom, o);
-
-            FoliatedManifold grasp, pregrasp, place, preplace,
-                             submanifold;
-            r.graspManifold (iG, gTo[iG], grasp, pregrasp);
-            if (!noPlace) {
-              const auto& pc (std::get<0>(o));
-              relaxedPlacementManifold (std::get<0>(pc),
-                  std::get<1>(pc),
-                  std::get<2>(pc),
-                  place, preplace);
-            }
-            std::pair<std::string, std::string> names =
-              r.name (gFrom, gTo, iG);
-            {
-              FoliatedManifold unused;
-              std::set <index_t> idxsOH;
-              for (index_t i = 0; i < r.nG; ++i) {
-                if (gFrom[i] < r.nOH) {
-                  idxsOH.insert (gFrom[i]);
-                  r.graspManifold (i, gFrom[i], submanifold, unused);
-                }
-              }
-              index_t iOH = 0;
-              for (const Object_t& o : r.ohs) {
-                if (!r.objectCanBePlaced(o)) continue;
-                bool oIsGrasped = false;
-                const index_t iOHstart = iOH;
-                for (; iOH < iOHstart + std::get<1>(o).size(); ++iOH) {
-                  if (iOH == gTo [iG]) {
-                    oIsGrasped = true;
-                    iOH = iOHstart + std::get<1>(o).size();
-                    break;
-                  }
-                  if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
-                }
-                if (!oIsGrasped) {
-                  const auto& pc (std::get<0>(o));
-                  relaxedPlacementManifold (std::get<0>(pc),
-                      std::get<1>(pc),
-                      std::get<2>(pc),
-                      submanifold, unused);
-                }
-              }
-            }
-            if (pregrasp.empty ()) {
-              if (noPlace)
-                createEdges <GraspOnly | NoPlace> (
-                    names.first           , names.second,
-                    std::get<0>(from)     , std::get<0>(to),
-                    1                     , 1,
-                    grasp                 , pregrasp,
-                    place                 , preplace,
-                    grasp.foliated ()     , place.foliated(),
-                    submanifold);
-              else if (preplace.empty ())
-                createEdges <GraspOnly | PlaceOnly> (
-                    names.first           , names.second,
-                    std::get<0>(from)     , std::get<0>(to),
-                    1                     , 1,
-                    grasp                 , pregrasp,
-                    place                 , preplace,
-                    grasp.foliated ()     , place.foliated(),
-                    submanifold);
-              else {
-                hppDout (error, "GraspOnly | WithPrePlace not implemeted yet");
-                /*
-                   createEdges <GraspOnly | WithPrePlace> (
-                   names.first           , names.second,
-                   std::get<0>(from)     , std::get<0>(to),
-                   1                     , 1,
-                   grasp                 , pregrasp,
-                   place                 , preplace,
-                   grasp.foliated ()     , place.foliated(),
-                   submanifold); // */
-              }
-            } else {
-              if (noPlace)
-                createEdges <WithPreGrasp | NoPlace> (
-                    names.first           , names.second,
-                    std::get<0>(from)     , std::get<0>(to),
-                    1                     , 1,
-                    grasp                 , pregrasp,
-                    place                 , preplace,
-                    grasp.foliated ()     , place.foliated(),
-                    submanifold);
-              else if (preplace.empty ())
-                createEdges <WithPreGrasp | PlaceOnly> (
-                    names.first           , names.second,
-                    std::get<0>(from)     , std::get<0>(to),
-                    1                     , 1,
-                    grasp                 , pregrasp,
-                    place                 , preplace,
-                    grasp.foliated ()     , place.foliated(),
-                    submanifold);
-              else
-                createEdges <WithPreGrasp | WithPrePlace> (
-                    names.first           , names.second,
-                    std::get<0>(from)     , std::get<0>(to),
-                    1                     , 1,
-                    grasp                 , pregrasp,
-                    place                 , preplace,
-                    grasp.foliated ()     , place.foliated(),
-                    submanifold);
-            }
-            r.addEdge(gFrom, gTo);
-          }
-
-          /// idx are the available grippers
-          void recurseGrippers (Result& r,
-              const IndexV_t& idx_g, const IndexV_t& idx_oh,
-              const GraspV_t& grasps, const int depth)
-          {
-            bool curGraspIsAllowed = r.graspIsAllowed(grasps);
-            if (curGraspIsAllowed) makeState (r, grasps, depth);
-
-            if (idx_g.empty () || idx_oh.empty ()) return;
-            IndexV_t nIdx_g (idx_g.size() - 1);
-            IndexV_t nIdx_oh (idx_oh.size() - 1);
-            for (IndexV_t::const_iterator itx_g = idx_g.begin ();
-                itx_g != idx_g.end (); ++itx_g) {
-              // Copy all element except itx_g
-              std::copy (std::next (itx_g), idx_g.end (),
-                  std::copy (idx_g.begin (), itx_g, nIdx_g.begin ())
-                  );
-              for (IndexV_t::const_iterator itx_oh = idx_oh.begin ();
-                  itx_oh != idx_oh.end (); ++itx_oh) {
-                // Create the edge for the selected grasp
-                GraspV_t nGrasps = grasps;
-                nGrasps [*itx_g] = *itx_oh;
-
-                bool nextGraspIsAllowed = r.graspIsAllowed(nGrasps);
-                if (nextGraspIsAllowed) makeState (r, nGrasps, depth + 1);
-
-                if (curGraspIsAllowed && nextGraspIsAllowed)
-                  makeEdge (r, grasps, nGrasps, *itx_g, depth);
-
-                // Copy all element except itx_oh
-                std::copy (std::next (itx_oh), idx_oh.end (),
-                    std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
-                    );
-                // Do all the possible combination below this new grasp
-                recurseGrippers (r, nIdx_g, nIdx_oh, nGrasps, depth + 2);
-              }
-            }
-          }
-        }
-
-        void graphBuilder (
-            const ProblemSolverPtr_t& ps,
-            const Objects_t& objects,
-            const Grippers_t& grippers,
-            GraphPtr_t graph,
-            const Rules_t& rules)
-        {
-          if (!graph) throw std::logic_error ("The graph must be initialized");
-          StateSelectorPtr_t ns = graph->stateSelector ();
-          if (!ns) throw std::logic_error ("The graph does not have a StateSelector");
-
-          Result r (ps, grippers, objects, graph);
-          r.setRules (rules);
-
-          IndexV_t availG (r.nG), availOH (r.nOH);
-          for (index_t i = 0; i < r.nG; ++i) availG[i] = i;
-          for (index_t i = 0; i < r.nOH; ++i) availOH[i] = i;
-
-          GraspV_t iG (r.nG, r.nOH);
-
-          recurseGrippers (r, availG, availOH, iG, 0);
-
-          hppDout (info, "Created a graph with " << r.states.size() << " states "
-              "and " << r.edges.size() << " edges.");
-        }
-
-        GraphPtr_t graphBuilder (
-            const ProblemSolverPtr_t& ps,
-            const std::string& graphName,
-            const StringList_t& griNames,
-            const std::list <ObjectDef_t>& objs,
-            const StringList_t& envNames,
-	    const std::vector <Rule>& rules,
-            const value_type& prePlaceWidth)
-        {
-          if (ps->graphs.has (graphName))
-            throw std::invalid_argument ("A graph named " + graphName + " already exists.");
-
-          const Device& robot = *(ps->robot ());
-          const pinocchio::Model& model = robot.model();
-          Grippers_t grippers (griNames.size());
-          index_t i = 0;
-          for (const std::string& gn : griNames) {
-            grippers[i] = robot.grippers.get (gn);
-            ++i;
-          }
-          Objects_t objects (objs.size());
-          i = 0;
-          const value_type margin = 1e-3;
-          bool prePlace = (prePlaceWidth > 0);
-          for (const ObjectDef_t& od : objs) {
-            // Create handles
-            std::get<2>(objects[i]) = i;
-            std::get<1>(objects[i]).resize (od.handles.size());
-            Handles_t::iterator it = std::get<1>(objects[i]).begin();
-            for (const std::string hn : od.handles) {
-              *it = robot.handles.get (hn);
-              ++it;
-            }
-            // Create placement
-            const std::string placeN = "place_" + od.name;
-            const std::string preplaceN = "pre" + placeN;
-            // If user provides constraint "place_objectName",
-            // then
-            //   use this as placement and use "preplace_objectName" for
-            //   pre-placement if defined.
-            // else if contact surfaces are defined and selected
-            //   create default placement constraint using the ProblemSolver
-            //   methods createPlacementConstraint and createPrePlacementConstraint
-            auto& pc (std::get<0>(objects[i]));
-            if (ps->numericalConstraints.has(placeN)) {
-              std::get<0>(pc) = ps->numericalConstraints.get (placeN);
-              if (ps->numericalConstraints.has(preplaceN)) {
-                std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
-              }
-            } else if (!envNames.empty() && !od.shapes.empty ()) {
-              ps->createPlacementConstraint (placeN,
-                  od.shapes, envNames, margin);
-              std::get<0>(pc) = ps->numericalConstraints.get (placeN);
-              if (prePlace) {
-                ps->createPrePlacementConstraint (preplaceN,
-                    od.shapes, envNames, margin, prePlaceWidth);
-                std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
-              }
-            }
-            // Create object lock
-	    // Loop over all frames of object, detect joint and create locked
-	    // joint.
-            assert (robot.robotFrames (od.name).size () != 0);
-            for (const FrameIndex& f : robot.robotFrames (od.name)) {
-              if (model.frames[f].type != ::pinocchio::JOINT) continue;
-              const JointIndex j = model.frames[f].parent;
-              JointPtr_t oj (Joint::create (ps->robot(), j));
-              LiegroupSpacePtr_t space (oj->configurationSpace ());
-              LiegroupElement lge (robot.currentConfiguration()
-                                   .segment (oj->rankInConfiguration (),
-                                             oj->configSize ()), space);
-              LockedJointPtr_t lj = core::LockedJoint::create (oj, lge);
-              ps->numericalConstraints.add ("lock_" + oj->name (), lj);
-              std::get<2>(pc).push_back (lj);
-            }
-            ++i;
-          }
-          GraphPtr_t graph = Graph::create (graphName,
-              ps->robot(), ps->problem());
-          ps->graphs.add (graphName, graph);
-          ps->constraintGraph (graphName);
-          graph->stateSelector (
-              GuidedStateSelector::create ("stateSelector",
-              ps->roadmap ()));
-          graph->maxIterations  (ps->maxIterProjection ());
-          graph->errorThreshold (ps->errorThreshold ());
-
-          graphBuilder (ps, objects, grippers, graph, rules);
-          return graph;
+namespace manipulation {
+namespace graph {
+namespace helper {
+typedef constraints::Implicit Implicit;
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+
+template <bool forPath>
+void addToComp(const NumericalConstraints_t& nc, GraphComponentPtr_t comp) {
+  if (nc.empty()) return;
+  StatePtr_t n;
+  if (forPath) {
+    n = HPP_DYNAMIC_PTR_CAST(State, comp);
+    if (!n) throw std::logic_error("Wrong type: expect a State");
+  }
+  for (const auto& c : nc)
+    if (c) {
+      if (forPath)
+        n->addNumericalConstraintForPath(c);
+      else
+        comp->addNumericalConstraint(c);
+    }
+}
+
+template <bool param>
+void specifyFoliation(const NumericalConstraints_t& nc, LevelSetEdgePtr_t lse) {
+  for (const auto& c : nc)
+    if (c) {
+      if (param)
+        lse->insertParamConstraint(c);
+      else
+        lse->insertConditionConstraint(c);
+    }
+}
+
+void FoliatedManifold::addToState(StatePtr_t comp) const {
+  addToComp<false>(nc, comp);
+  addToComp<false>(nc_path, comp);
+}
+
+void FoliatedManifold::addToEdge(EdgePtr_t comp) const {
+  addToComp<false>(nc_fol, comp);
+}
+
+void FoliatedManifold::specifyFoliation(LevelSetEdgePtr_t lse) const {
+  for (const auto& c : nc) lse->insertConditionConstraint(c);
+  for (const auto& c : nc_fol) lse->insertConditionConstraint(c);
+}
+
+namespace {
+template <int gCase>
+struct CaseTraits {
+  static const bool pregrasp = (gCase & WithPreGrasp);
+  static const bool preplace = (gCase & WithPrePlace);
+  static const bool intersec = !((gCase & NoGrasp) || (gCase & NoPlace));
+
+  static const bool valid =
+      ((gCase & WithPreGrasp) || (gCase & GraspOnly) || (gCase & NoGrasp)) &&
+      ((gCase & WithPrePlace) || (gCase & PlaceOnly) || (gCase & NoPlace)) &&
+      !((gCase & NoGrasp) && (gCase & NoPlace));
+
+  static const std::size_t nbWaypoints =
+      (pregrasp ? 1 : 0) + (intersec ? 1 : 0) + (preplace ? 1 : 0);
+  static const std::size_t Nstates = 2 + nbWaypoints;
+  static const std::size_t Nedges = 1 + nbWaypoints;
+  // static const std::size_t iNpregrasp = pregrasp?1 + 1:nbWaypoints;
+  // static const std::size_t iNpreplace = pregrasp?1 + 1:nbWaypoints;
+  typedef std::array<StatePtr_t, Nstates> StateArray;
+  typedef std::array<EdgePtr_t, Nedges> EdgeArray;
+
+  static inline const StatePtr_t& Npregrasp(const StateArray& n) {
+    assert(pregrasp);
+    return n[1];
+  }
+  static inline const StatePtr_t& Nintersec(const StateArray& n) {
+    assert(intersec);
+    return n[1 + (pregrasp ? 1 : 0)];
+  }
+  static inline const StatePtr_t& Npreplace(const StateArray& n) {
+    assert(preplace);
+    return n[1 + (pregrasp ? 1 : 0) + (intersec ? 1 : 0)];
+  }
+
+  static inline std::string caseToString() {
+    return CASE_TO_STRING(gCase, NoGrasp) + CASE_TO_STRING(gCase, GraspOnly) +
+           CASE_TO_STRING(gCase, WithPreGrasp) + " - " +
+           CASE_TO_STRING(gCase, NoPlace) + CASE_TO_STRING(gCase, PlaceOnly) +
+           CASE_TO_STRING(gCase, WithPrePlace);
+  }
+
+  static inline EdgePtr_t makeWE(const std::string& name,
+                                 const StatePtr_t& from, const StatePtr_t& to,
+                                 const size_type& w) {
+    if (Nedges > 1) {
+      WaypointEdgePtr_t we = static_pointer_cast<WaypointEdge>(
+          from->linkTo(name, to, w, WaypointEdge::create));
+      we->nbWaypoints(nbWaypoints);
+      return we;
+    } else
+      return from->linkTo(name, to, w, Edge::create);
+  }
+
+  static inline StateArray makeWaypoints(const StatePtr_t& from,
+                                         const StatePtr_t& to,
+                                         const std::string& name) {
+    StateSelectorPtr_t ns = from->parentGraph()->stateSelector();
+    StateArray states;
+    std::size_t r = 0;
+    states[r] = from;
+    ++r;
+    if (pregrasp) {
+      states[r] = ns->createState(name + "_pregrasp", true);
+      ++r;
+    }
+    if (intersec) {
+      states[r] = ns->createState(name + "_intersec", true);
+      ++r;
+    }
+    if (preplace) {
+      states[r] = ns->createState(name + "_preplace", true);
+      ++r;
+    }
+    states[r] = to;
+    return states;
+  }
+
+  static inline EdgePtr_t makeLSEgrasp(const std::string& name,
+                                       const StateArray& n, const EdgeArray& e,
+                                       const size_type w,
+                                       LevelSetEdgePtr_t& gls) {
+    if (Nedges > 1) {
+      const std::size_t T = (pregrasp ? 1 : 0) + (intersec ? 1 : 0);
+      WaypointEdgePtr_t we = static_pointer_cast<WaypointEdge>(
+          n.front()->linkTo(name + "_ls", n.back(), w, WaypointEdge::create));
+      we->nbWaypoints(nbWaypoints);
+      gls = linkWaypoint<LevelSetEdge>(n, T - 1, T, name, "ls");
+      for (std::size_t i = 0; i < Nedges; ++i)
+        we->setWaypoint(i, e[i], n[i + 1]);
+      we->setWaypoint(T - 1, gls, n[T]);
+      gls->state(n.front());
+      gls->setShort(pregrasp);
+      return we;
+    } else {
+      assert(gCase == (GraspOnly | NoPlace) &&
+             "Cannot implement a LevelSetEdge for grasping");
+      gls = static_pointer_cast<LevelSetEdge>(
+          n.front()->linkTo(name + "_ls", n.back(), w, LevelSetEdge::create));
+      return gls;
+    }
+  }
+
+  static inline EdgePtr_t makeLSEplace(const std::string& name,
+                                       const StateArray& n, const EdgeArray& e,
+                                       const size_type w,
+                                       LevelSetEdgePtr_t& pls) {
+    if (Nedges > 1) {
+      const std::size_t T = (pregrasp ? 1 : 0) + (intersec ? 1 : 0);
+      WaypointEdgePtr_t we = static_pointer_cast<WaypointEdge>(
+          n.back()->linkTo(name + "_ls", n.front(), w, WaypointEdge::create));
+      we->nbWaypoints(nbWaypoints);
+      pls = linkWaypoint<LevelSetEdge>(n, T + 1, T, name, "ls");
+      // for (std::size_t i = Nedges - 1; i != 0; --i)
+      for (std::size_t k = 0; k < Nedges; ++k) {
+        std::size_t i = Nedges - 1 - k;
+        we->setWaypoint(Nedges - 1 - i, e[i], n[i]);
+      }
+      we->setWaypoint(Nedges - 1 - T, pls, n[T]);
+      pls->state(n.back());
+      pls->setShort(preplace);
+      return we;
+    } else {
+      assert(gCase == (NoGrasp | PlaceOnly) &&
+             "Cannot implement a LevelSetEdge for placement");
+      pls = static_pointer_cast<LevelSetEdge>(
+          n.back()->linkTo(name + "_ls", n.front(), w, LevelSetEdge::create));
+      return pls;
+    }
+  }
+
+  template <typename EdgeType>
+  static inline shared_ptr<EdgeType> linkWaypoint(
+      const StateArray& states, const std::size_t& iF, const std::size_t& iT,
+      const std::string& prefix, const std::string& suffix = "") {
+    std::stringstream ss;
+    ss << prefix << "_" << iF << iT;
+    if (suffix.length() > 0) ss << "_" << suffix;
+    return static_pointer_cast<EdgeType>(
+        states[iF]->linkTo(ss.str(), states[iT], -1, EdgeType::create));
+  }
+
+  template <bool forward>
+  static inline EdgeArray linkWaypoints(const StateArray& states,
+                                        const EdgePtr_t& edge,
+                                        const std::string& name) {
+    EdgeArray e;
+    WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(WaypointEdge, edge);
+    if (forward)
+      for (std::size_t i = 0; i < Nedges; ++i) {
+        e[i] = linkWaypoint<Edge>(states, i, i + 1, name);
+        we->setWaypoint(i, e[i], states[i + 1]);
+      }
+    else
+      // for (std::size_t i = Nedges - 1; i != 0; --i) {
+      for (std::size_t k = 0; k < Nedges; ++k) {
+        std::size_t i = Nedges - 1 - k;
+        e[i] = linkWaypoint<Edge>(states, i + 1, i, name);
+        we->setWaypoint(Nedges - 1 - i, e[i], states[i]);
+      }
+    return e;
+  }
+
+  static inline void setStateConstraints(const StateArray& n,
+                                         const FoliatedManifold& g,
+                                         const FoliatedManifold& pg,
+                                         const FoliatedManifold& p,
+                                         const FoliatedManifold& pp,
+                                         const FoliatedManifold& m) {
+    // From and to are not populated automatically
+    // to avoid duplicates.
+    if (pregrasp) {
+      p.addToState(Npregrasp(n));
+      pg.addToState(Npregrasp(n));
+      m.addToState(Npregrasp(n));
+    }
+    if (intersec) {
+      p.addToState(Nintersec(n));
+      g.addToState(Nintersec(n));
+      m.addToState(Nintersec(n));
+    }
+    if (preplace) {
+      pp.addToState(Npreplace(n));
+      g.addToState(Npreplace(n));
+      m.addToState(Npreplace(n));
+    }
+  }
+
+  static inline void setEdgeConstraints(const EdgeArray& e,
+                                        const FoliatedManifold& g,
+                                        const FoliatedManifold& p,
+                                        const FoliatedManifold& m) {
+    // The border B
+    const std::size_t B = (pregrasp ? 1 : 0) + (intersec ? 1 : 0);
+    for (std::size_t i = 0; i < B; ++i) p.addToEdge(e[i]);
+    for (std::size_t i = B; i < Nedges; ++i) g.addToEdge(e[i]);
+    for (std::size_t i = 0; i < Nedges; ++i) m.addToEdge(e[i]);
+  }
+
+  template <bool forward>
+  static inline void setEdgeProp(const EdgeArray& e, const StateArray& n) {
+    /// Last is short
+    const std::size_t K = (forward ? 1 : 0);
+    for (std::size_t i = K; i < Nedges - 1 + K; ++i) e[i]->setShort(true);
+    // The border B
+    std::size_t B;
+    if ((gCase & NoGrasp))  // There is no grasp
+      B = 0;
+    else  // There is a grasp
+      B = 1 + (pregrasp ? 1 : 0);
+    for (std::size_t i = 0; i < B; ++i) e[i]->state(n[0]);
+    for (std::size_t i = B; i < Nedges; ++i) e[i]->state(n[Nstates - 1]);
+  }
+};
+}  // namespace
+
+template <int gCase>
+Edges_t createEdges(const std::string& forwName, const std::string& backName,
+                    const StatePtr_t& from, const StatePtr_t& to,
+                    const size_type& wForw, const size_type& wBack,
+                    const FoliatedManifold& grasp,
+                    const FoliatedManifold& pregrasp,
+                    const FoliatedManifold& place,
+                    const FoliatedManifold& preplace, const bool levelSetGrasp,
+                    const bool levelSetPlace,
+                    const FoliatedManifold& submanifoldDef) {
+  typedef CaseTraits<gCase> T;
+  hppDout(info, "Creating edges " << forwName << " and " << backName
+                                  << "\ncase is " << T::caseToString());
+  assert(T::valid && "Not a valid case.");
+  typedef typename T::StateArray StateArray;
+  typedef typename T::EdgeArray EdgeArray;
+
+  // Create the edges
+  EdgePtr_t weForw = T::makeWE(forwName, from, to, wForw),
+            weBack = T::makeWE(backName, to, from, wBack), weForwLs, weBackLs;
+
+  std::string name = forwName;
+  StateArray n = T::makeWaypoints(from, to, name);
+
+  EdgeArray eF = T::template linkWaypoints<true>(n, weForw, name);
+
+  // Set the states constraints
+  // Note that submanifold is not taken into account for states
+  // because the edges constraints will enforce configuration to stay
+  // in a leaf, and so in the manifold itself.
+  T::setStateConstraints(n, grasp, pregrasp, place, preplace, submanifoldDef);
+
+  // Set the edges properties
+  T::template setEdgeProp<true>(eF, n);
+
+  // Set the edges constraints
+  T::setEdgeConstraints(eF, grasp, place, submanifoldDef);
+
+  LevelSetEdgePtr_t gls;
+  if (levelSetGrasp) weForwLs = T::makeLSEgrasp(name, n, eF, 10 * wForw, gls);
+
+  // Populate bacward edge
+  name = backName;
+  EdgeArray eB = T::template linkWaypoints<false>(n, weBack, name);
+
+  T::template setEdgeProp<false>(eB, n);
+
+  T::setEdgeConstraints(eB, grasp, place, submanifoldDef);
+
+  LevelSetEdgePtr_t pls;
+  if (levelSetPlace) weBackLs = T::makeLSEplace(name, n, eB, 10 * wBack, pls);
+
+  Edges_t ret{weForw, weBack};
+
+  if (levelSetPlace) {
+    if (!place.foliated()) {
+      hppDout(warning,
+              "You asked for a LevelSetEdge for placement, "
+              "but did not specify the target foliation. "
+              "It will have no effect");
+    }
+    grasp.addToEdge(pls);
+    place.specifyFoliation(pls);
+    submanifoldDef.addToEdge(pls);
+    pls->buildHistogram();
+    place.addToEdge(weBackLs);
+    submanifoldDef.addToEdge(weBackLs);
+    ret.push_back(weBackLs);
+  }
+  if (levelSetGrasp) {
+    if (!grasp.foliated()) {
+      hppDout(warning,
+              "You asked for a LevelSetEdge for grasping, "
+              "but did not specify the target foliation. "
+              "It will have no effect");
+    }
+    place.addToEdge(gls);
+    grasp.specifyFoliation(gls);
+    submanifoldDef.addToEdge(gls);
+    gls->buildHistogram();
+    grasp.addToEdge(weForwLs);
+    submanifoldDef.addToEdge(weForwLs);
+    ret.push_back(weForwLs);
+  }
+
+  return ret;
+}
+
+EdgePtr_t createLoopEdge(const std::string& loopName, const StatePtr_t& state,
+                         const size_type& w, const bool levelSet,
+                         const FoliatedManifold& submanifoldDef) {
+  // Create the edges
+  EdgePtr_t loop;
+  if (levelSet)
+    loop = state->linkTo(loopName, state, w, LevelSetEdge::create);
+  else
+    loop = state->linkTo(loopName, state, w, Edge::create);
+
+  loop->state(state);
+  submanifoldDef.addToEdge(loop);
+
+  if (levelSet) {
+    if (!submanifoldDef.foliated()) {
+      hppDout(warning,
+              "You asked for a LevelSetEdge for looping, "
+              "but did not specify the target foliation. "
+              "It will have no effect");
+    }
+    LevelSetEdgePtr_t ls = HPP_DYNAMIC_PTR_CAST(LevelSetEdge, loop);
+    submanifoldDef.specifyFoliation(ls);
+    ls->buildHistogram();
+  }
+
+  return loop;
+}
+
+void graspManifold(const GripperPtr_t& gripper, const HandlePtr_t& handle,
+                   FoliatedManifold& grasp, FoliatedManifold& pregrasp) {
+  ImplicitPtr_t gc = handle->createGrasp(gripper, "");
+  grasp.nc.push_back(gc);
+  grasp.nc_path.push_back(gc);
+  ImplicitPtr_t gcc = handle->createGraspComplement(gripper, "");
+  if (gcc->function().outputSize() > 0) grasp.nc_fol.push_back(gcc);
+
+  const value_type c = handle->clearance() + gripper->clearance();
+  ImplicitPtr_t pgc = handle->createPreGrasp(gripper, c, "");
+  pregrasp.nc.push_back(pgc);
+  pregrasp.nc_path.push_back(pgc);
+}
+
+void strictPlacementManifold(const ImplicitPtr_t placement,
+                             const ImplicitPtr_t preplacement,
+                             const ImplicitPtr_t placementComplement,
+                             FoliatedManifold& place,
+                             FoliatedManifold& preplace) {
+  place.nc.push_back(placement);
+  place.nc_path.push_back(placement);
+  if (placementComplement && placementComplement->function().outputSize() > 0)
+    place.nc_fol.push_back(placementComplement);
+
+  preplace.nc.push_back(preplacement);
+  preplace.nc_path.push_back(preplacement);
+}
+
+void relaxedPlacementManifold(const ImplicitPtr_t placement,
+                              const ImplicitPtr_t preplacement,
+                              const LockedJoints_t objectLocks,
+                              FoliatedManifold& place,
+                              FoliatedManifold& preplace) {
+  if (placement) {
+    place.nc.push_back(placement);
+    // The placement constraints are not required in the path, as long as
+    // they are satisfied at both ends and the object does not move. The
+    // former condition is ensured by the placement constraints on both
+    // ends and the latter is ensure by the LockedJoint constraints.
+    place.nc_path.push_back(placement);
+  }
+  std::copy(objectLocks.begin(), objectLocks.end(),
+            std::back_inserter(place.lj_fol));
+
+  if (placement && preplacement) {
+    preplace.nc.push_back(preplacement);
+    // preplace.nc_path.push_back (preplacement);
+  }
+}
+
+namespace {
+typedef std::size_t index_t;
+typedef std::vector<index_t> IndexV_t;
+typedef std::list<index_t> IndexL_t;
+typedef std::pair<index_t, index_t> Grasp_t;
+typedef std::tuple<StatePtr_t, FoliatedManifold> StateAndManifold_t;
+// typedef std::vector <index_t, index_t> GraspV_t;
+/// GraspV_t corresponds to a unique ID of a  permutation.
+/// - its size is the number of grippers,
+/// - the values correpond to the index of the handle (0..nbHandle-1), or
+///   nbHandle to mean no handle.
+typedef std::vector<index_t> GraspV_t;
+struct Result;
+struct CompiledRule {
+  enum Status { Accept, Refuse, NoMatch, Undefined };
+  std::vector<boost::regex> handles;
+  Status status;
+  CompiledRule(const Result& res, const Rule& r);
+  Status check(const std::vector<std::string>& names, const GraspV_t& g) const {
+    const std::size_t nG = g.size();
+    assert(nG == handles.size());
+    for (std::size_t i = 0; i < nG; ++i) {
+      if (handles[i].empty()) continue;
+      if (!boost::regex_match(names[g[i]], handles[i])) return NoMatch;
+    }
+    return status;
+  }
+};
+typedef std::vector<CompiledRule> CompiledRules_t;
+
+struct Result {
+  ProblemSolverPtr_t ps;
+  GraphPtr_t graph;
+  typedef unsigned long stateid_type;
+  std::tr1::unordered_map<stateid_type, StateAndManifold_t> states;
+  typedef std::pair<stateid_type, stateid_type> edgeid_type;
+  struct edgeid_hash {
+    std::tr1::hash<edgeid_type::first_type> first;
+    std::tr1::hash<edgeid_type::second_type> second;
+    std::size_t operator()(const edgeid_type& eid) const {
+      return first(eid.first) + second(eid.second);
+    }
+  };
+  std::tr1::unordered_set<edgeid_type, edgeid_hash> edges;
+  std::vector<std::array<ImplicitPtr_t, 3> > graspCs;
+  index_t nG, nOH;
+  GraspV_t dims;
+  const Grippers_t& gs;
+  const Objects_t& ohs;
+  std::vector<std::string> handleNames;
+  CompiledRules_t rules;
+  CompiledRule::Status defaultAcceptationPolicy;
+
+  Result(const ProblemSolverPtr_t problem, const Grippers_t& grippers,
+         const Objects_t& objects, GraphPtr_t g)
+      : ps(problem),
+        graph(g),
+        nG(grippers.size()),
+        nOH(0),
+        gs(grippers),
+        ohs(objects),
+        defaultAcceptationPolicy(CompiledRule::Refuse) {
+    for (const Object_t& o : objects) {
+      nOH += std::get<1>(o).size();
+      for (const HandlePtr_t& h : std::get<1>(o))
+        handleNames.push_back(h->name());
+    }
+    handleNames.push_back("");
+    dims.resize(nG);
+    dims[0] = nOH + 1;
+    for (index_t i = 1; i < nG; ++i) dims[i] = dims[i - 1] * (nOH + 1);
+    graspCs.resize(nG * nOH);
+  }
+
+  void setRules(const Rules_t& r) {
+    for (Rules_t::const_iterator _r = r.begin(); _r != r.end(); ++_r)
+      rules.push_back(CompiledRule(*this, *_r));
+  }
+
+  bool graspIsAllowed(const GraspV_t& idxOH) const {
+    assert(idxOH.size() == nG);
+    for (std::size_t r = 0; r < rules.size(); ++r) {
+      switch (rules[r].check(handleNames, idxOH)) {
+        case CompiledRule::Accept:
+          return true;
+        case CompiledRule::Refuse:
+          return false;
+        case CompiledRule::NoMatch:
+          continue;  // Check next rule
+        default:
+          throw std::invalid_argument("Rules are ill-defined.");
+      }
+    }
+    return (defaultAcceptationPolicy == CompiledRule::Accept);
+  }
+
+  inline stateid_type stateid(const GraspV_t& iG) {
+    stateid_type iGOH = iG[0];
+    stateid_type res;
+    for (index_t i = 1; i < nG; ++i) {
+      res = iGOH + dims[i] * (iG[i]);
+      if (res < iGOH) {
+        hppDout(info, "State ID overflowed. There are too many states...");
+      }
+      iGOH = res;
+      // iGOH += dims[i] * (iG[i]);
+    }
+    return iGOH;
+  }
+
+  bool hasState(const GraspV_t& iG) { return states.count(stateid(iG)) > 0; }
+
+  StateAndManifold_t& operator()(const GraspV_t& iG) {
+    return states[stateid(iG)];
+  }
+
+  bool hasEdge(const GraspV_t& g1, const GraspV_t& g2) {
+    return edges.count(edgeid_type(stateid(g1), stateid(g2))) > 0;
+  }
+
+  void addEdge(const GraspV_t& g1, const GraspV_t& g2) {
+    edges.insert(edgeid_type(stateid(g1), stateid(g2)));
+  }
+
+  inline std::array<ImplicitPtr_t, 3>& graspConstraint(const index_t& iG,
+                                                       const index_t& iOH) {
+    std::array<ImplicitPtr_t, 3>& gcs = graspCs[iG * nOH + iOH];
+    if (!gcs[0]) {
+      hppDout(info,
+              "Create grasps constraints for (" << iG << ", " << iOH << ")");
+      const GripperPtr_t& g(gs[iG]);
+      const HandlePtr_t& h(handle(iOH));
+      const std::string& grasp = g->name() + " grasps " + h->name();
+      if (!ps->numericalConstraints.has(grasp)) {
+        ps->createGraspConstraint(grasp, g->name(), h->name());
+      }
+      gcs[0] = ps->numericalConstraints.get(grasp);
+      gcs[1] = ps->numericalConstraints.get(grasp + "/complement");
+      const std::string& pregrasp = g->name() + " pregrasps " + h->name();
+      if (!ps->numericalConstraints.has(pregrasp)) {
+        ps->createPreGraspConstraint(pregrasp, g->name(), h->name());
+      }
+      gcs[2] = ps->numericalConstraints.get(pregrasp);
+    }
+    return gcs;
+  }
+
+  const Object_t& object(const index_t& iOH) const {
+    index_t iH = iOH;
+    for (const Object_t& o : ohs) {
+      if (iH < std::get<1>(o).size()) return o;
+      iH -= std::get<1>(o).size();
+    }
+    throw std::out_of_range("Handle index");
+  }
+
+  const HandlePtr_t& handle(const index_t& iOH) const {
+    index_t iH = iOH;
+    for (const Object_t& o : ohs) {
+      if (iH < std::get<1>(o).size()) return std::get<1>(o)[iH];
+      iH -= std::get<1>(o).size();
+    }
+    throw std::out_of_range("Handle index");
+  }
+
+  /// Check if an object can be placed
+  bool objectCanBePlaced(const Object_t& o) const {
+    // If the object has no joint, then it cannot be placed.
+    return (std::get<2>(std::get<0>(o)).size() > 0);
+  }
+
+  /// Check is an object is grasped by the GraspV_t
+  bool isObjectGrasped(const GraspV_t& idxOH, const Object_t& o) const {
+    assert(idxOH.size() == nG);
+    for (std::size_t i = 0; i < idxOH.size(); ++i)
+      if (idxOH[i] < nOH)  // This grippers grasps an object
+        if (std::get<2>(o) == std::get<2>(object(idxOH[i]))) return true;
+    return false;
+  }
+
+  /// Get a state name from a set of grasps
+  std::string name(const GraspV_t& idxOH, bool abbrev = false) const {
+    assert(idxOH.size() == nG);
+    std::stringstream ss;
+    bool first = true;
+    std::string sepGOH = (abbrev ? "-" : " grasps "),
+                sep = (abbrev ? ":" : " : ");
+    for (std::size_t i = 0; i < idxOH.size(); ++i) {
+      if (idxOH[i] < nOH) {  // This grippers grasps an object
+        if (first)
+          first = false;
+        else
+          ss << sep;
+        if (abbrev)
+          ss << i << sepGOH << idxOH[i];
+        else
+          ss << gs[i]->name() << sepGOH << handle(idxOH[i])->name();
+      }
+    }
+    if (first) return (abbrev ? "f" : "free");
+    return ss.str();
+  }
+
+  /// Get an edge name from a set of grasps
+  std::pair<std::string, std::string> name(const GraspV_t& gFrom,
+                                           const GraspV_t& gTo,
+                                           const index_t iG) {
+    const std::string nf(name(gFrom, true)), nt(name(gTo, true));
+    std::stringstream ssForw, ssBack;
+    const char sep[] = " | ";
+    ssForw << gs[iG]->name() << " > " << handle(gTo[iG])->name() << sep << nf;
+    ssBack << gs[iG]->name() << " < " << handle(gTo[iG])->name() << sep << nt;
+    return std::make_pair(ssForw.str(), ssBack.str());
+  }
+
+  std::string nameLoopEdge(const GraspV_t& gFrom) {
+    const std::string nf(name(gFrom, true));
+    std::stringstream ss;
+    const char sep[] = " | ";
+    ss << "Loop" << sep << nf;
+    return ss.str();
+  }
+
+  void graspManifold(const index_t& iG, const index_t& iOH,
+                     FoliatedManifold& grasp, FoliatedManifold& pregrasp) {
+    std::array<ImplicitPtr_t, 3>& gcs = graspConstraint(iG, iOH);
+    grasp.nc.push_back(gcs[0]);
+    grasp.nc_path.push_back(gcs[0]);
+    if (gcs[1]->function().outputSize() > 0) grasp.nc_fol.push_back(gcs[1]);
+
+    pregrasp.nc.push_back(gcs[2]);
+    pregrasp.nc_path.push_back(gcs[2]);
+  }
+};
+
+CompiledRule::CompiledRule(const Result& res, const Rule& r)
+    : handles(res.nG), status(r.link_ ? Accept : Refuse) {
+  assert(r.grippers_.size() == r.handles_.size());
+  for (std::size_t j = 0; j < r.grippers_.size(); ++j) {
+    boost::regex gripper(r.grippers_[j]);
+    for (std::size_t i = 0; i < res.nG; ++i) {
+      if (boost::regex_match(res.gs[i]->name(), gripper)) {
+        assert(handles[i].empty() &&
+               "Two gripper regex match the different gripper names.");
+        handles[i] = r.handles_[j];
+      }
+    }
+  }
+}
+
+const StateAndManifold_t& makeState(Result& r, const GraspV_t& g,
+                                    const int priority) {
+  StateAndManifold_t& nam = r(g);
+  if (!std::get<0>(nam)) {
+    hppDout(info, "Creating state " << r.name(g));
+    std::get<0>(nam) =
+        r.graph->stateSelector()->createState(r.name(g), false, priority);
+    // Loop over the grippers and create grasping constraints if required
+    FoliatedManifold unused;
+    std::set<index_t> idxsOH;
+    for (index_t i = 0; i < r.nG; ++i) {
+      if (g[i] < r.nOH) {
+        idxsOH.insert(g[i]);
+        r.graspManifold(i, g[i], std::get<1>(nam), unused);
+      }
+    }
+    index_t iOH = 0;
+    for (const Object_t& o : r.ohs) {
+      if (!r.objectCanBePlaced(o)) continue;
+      bool oIsGrasped = false;
+      // TODO: use the fact that the set is sorted.
+      // for (const HandlePtr_t& h : std::get<0>(o))
+      for (index_t i = 0; i < std::get<1>(o).size(); ++i) {
+        if (idxsOH.erase(iOH) == 1) oIsGrasped = true;
+        ++iOH;
+      }
+      if (!oIsGrasped) {
+        const auto& pc(std::get<0>(o));
+        relaxedPlacementManifold(std::get<0>(pc), std::get<1>(pc),
+                                 std::get<2>(pc), std::get<1>(nam), unused);
+      }
+    }
+    std::get<1>(nam).addToState(std::get<0>(nam));
+
+    createLoopEdge(r.nameLoopEdge(g), std::get<0>(nam), 0, false,
+                   // TODO std::get<1>(nam).foliated(),
+                   std::get<1>(nam));
+  }
+  return nam;
+}
+
+/// Arguments are such that
+/// \li gTo[iG] != gFrom[iG]
+/// \li for all i != iG, gTo[iG] == gFrom[iG]
+void makeEdge(Result& r, const GraspV_t& gFrom, const GraspV_t& gTo,
+              const index_t iG, const int priority) {
+  if (r.hasEdge(gFrom, gTo)) {
+    hppDout(warning, "Prevented creation of duplicated edge\nfrom "
+                         << r.name(gFrom) << "\nto " << r.name(gTo));
+    return;
+  }
+  const StateAndManifold_t &from = makeState(r, gFrom, priority),
+                           to = makeState(r, gTo, priority + 1);
+  const Object_t& o = r.object(gTo[iG]);
+
+  // Detect when grasping an object already grasped.
+  // or when there is no placement information for it.
+  bool noPlace = !r.objectCanBePlaced(o) || r.isObjectGrasped(gFrom, o);
+
+  FoliatedManifold grasp, pregrasp, place, preplace, submanifold;
+  r.graspManifold(iG, gTo[iG], grasp, pregrasp);
+  if (!noPlace) {
+    const auto& pc(std::get<0>(o));
+    relaxedPlacementManifold(std::get<0>(pc), std::get<1>(pc), std::get<2>(pc),
+                             place, preplace);
+  }
+  std::pair<std::string, std::string> names = r.name(gFrom, gTo, iG);
+  {
+    FoliatedManifold unused;
+    std::set<index_t> idxsOH;
+    for (index_t i = 0; i < r.nG; ++i) {
+      if (gFrom[i] < r.nOH) {
+        idxsOH.insert(gFrom[i]);
+        r.graspManifold(i, gFrom[i], submanifold, unused);
+      }
+    }
+    index_t iOH = 0;
+    for (const Object_t& o : r.ohs) {
+      if (!r.objectCanBePlaced(o)) continue;
+      bool oIsGrasped = false;
+      const index_t iOHstart = iOH;
+      for (; iOH < iOHstart + std::get<1>(o).size(); ++iOH) {
+        if (iOH == gTo[iG]) {
+          oIsGrasped = true;
+          iOH = iOHstart + std::get<1>(o).size();
+          break;
         }
-      } // namespace helper
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+        if (idxsOH.erase(iOH) == 1) oIsGrasped = true;
+      }
+      if (!oIsGrasped) {
+        const auto& pc(std::get<0>(o));
+        relaxedPlacementManifold(std::get<0>(pc), std::get<1>(pc),
+                                 std::get<2>(pc), submanifold, unused);
+      }
+    }
+  }
+  if (pregrasp.empty()) {
+    if (noPlace)
+      createEdges<GraspOnly | NoPlace>(
+          names.first, names.second, std::get<0>(from), std::get<0>(to), 1, 1,
+          grasp, pregrasp, place, preplace, grasp.foliated(), place.foliated(),
+          submanifold);
+    else if (preplace.empty())
+      createEdges<GraspOnly | PlaceOnly>(
+          names.first, names.second, std::get<0>(from), std::get<0>(to), 1, 1,
+          grasp, pregrasp, place, preplace, grasp.foliated(), place.foliated(),
+          submanifold);
+    else {
+      hppDout(error, "GraspOnly | WithPrePlace not implemeted yet");
+      /*
+         createEdges <GraspOnly | WithPrePlace> (
+         names.first           , names.second,
+         std::get<0>(from)     , std::get<0>(to),
+         1                     , 1,
+         grasp                 , pregrasp,
+         place                 , preplace,
+         grasp.foliated ()     , place.foliated(),
+         submanifold); // */
+    }
+  } else {
+    if (noPlace)
+      createEdges<WithPreGrasp | NoPlace>(
+          names.first, names.second, std::get<0>(from), std::get<0>(to), 1, 1,
+          grasp, pregrasp, place, preplace, grasp.foliated(), place.foliated(),
+          submanifold);
+    else if (preplace.empty())
+      createEdges<WithPreGrasp | PlaceOnly>(
+          names.first, names.second, std::get<0>(from), std::get<0>(to), 1, 1,
+          grasp, pregrasp, place, preplace, grasp.foliated(), place.foliated(),
+          submanifold);
+    else
+      createEdges<WithPreGrasp | WithPrePlace>(
+          names.first, names.second, std::get<0>(from), std::get<0>(to), 1, 1,
+          grasp, pregrasp, place, preplace, grasp.foliated(), place.foliated(),
+          submanifold);
+  }
+  r.addEdge(gFrom, gTo);
+}
+
+/// idx are the available grippers
+void recurseGrippers(Result& r, const IndexV_t& idx_g, const IndexV_t& idx_oh,
+                     const GraspV_t& grasps, const int depth) {
+  bool curGraspIsAllowed = r.graspIsAllowed(grasps);
+  if (curGraspIsAllowed) makeState(r, grasps, depth);
+
+  if (idx_g.empty() || idx_oh.empty()) return;
+  IndexV_t nIdx_g(idx_g.size() - 1);
+  IndexV_t nIdx_oh(idx_oh.size() - 1);
+  for (IndexV_t::const_iterator itx_g = idx_g.begin(); itx_g != idx_g.end();
+       ++itx_g) {
+    // Copy all element except itx_g
+    std::copy(std::next(itx_g), idx_g.end(),
+              std::copy(idx_g.begin(), itx_g, nIdx_g.begin()));
+    for (IndexV_t::const_iterator itx_oh = idx_oh.begin();
+         itx_oh != idx_oh.end(); ++itx_oh) {
+      // Create the edge for the selected grasp
+      GraspV_t nGrasps = grasps;
+      nGrasps[*itx_g] = *itx_oh;
+
+      bool nextGraspIsAllowed = r.graspIsAllowed(nGrasps);
+      if (nextGraspIsAllowed) makeState(r, nGrasps, depth + 1);
+
+      if (curGraspIsAllowed && nextGraspIsAllowed)
+        makeEdge(r, grasps, nGrasps, *itx_g, depth);
+
+      // Copy all element except itx_oh
+      std::copy(std::next(itx_oh), idx_oh.end(),
+                std::copy(idx_oh.begin(), itx_oh, nIdx_oh.begin()));
+      // Do all the possible combination below this new grasp
+      recurseGrippers(r, nIdx_g, nIdx_oh, nGrasps, depth + 2);
+    }
+  }
+}
+}  // namespace
+
+void graphBuilder(const ProblemSolverPtr_t& ps, const Objects_t& objects,
+                  const Grippers_t& grippers, GraphPtr_t graph,
+                  const Rules_t& rules) {
+  if (!graph) throw std::logic_error("The graph must be initialized");
+  StateSelectorPtr_t ns = graph->stateSelector();
+  if (!ns) throw std::logic_error("The graph does not have a StateSelector");
+
+  Result r(ps, grippers, objects, graph);
+  r.setRules(rules);
+
+  IndexV_t availG(r.nG), availOH(r.nOH);
+  for (index_t i = 0; i < r.nG; ++i) availG[i] = i;
+  for (index_t i = 0; i < r.nOH; ++i) availOH[i] = i;
+
+  GraspV_t iG(r.nG, r.nOH);
+
+  recurseGrippers(r, availG, availOH, iG, 0);
+
+  hppDout(info, "Created a graph with " << r.states.size()
+                                        << " states "
+                                           "and "
+                                        << r.edges.size() << " edges.");
+}
+
+GraphPtr_t graphBuilder(const ProblemSolverPtr_t& ps,
+                        const std::string& graphName,
+                        const StringList_t& griNames,
+                        const std::list<ObjectDef_t>& objs,
+                        const StringList_t& envNames,
+                        const std::vector<Rule>& rules,
+                        const value_type& prePlaceWidth) {
+  if (ps->graphs.has(graphName))
+    throw std::invalid_argument("A graph named " + graphName +
+                                " already exists.");
+
+  const Device& robot = *(ps->robot());
+  const pinocchio::Model& model = robot.model();
+  Grippers_t grippers(griNames.size());
+  index_t i = 0;
+  for (const std::string& gn : griNames) {
+    grippers[i] = robot.grippers.get(gn);
+    ++i;
+  }
+  Objects_t objects(objs.size());
+  i = 0;
+  const value_type margin = 1e-3;
+  bool prePlace = (prePlaceWidth > 0);
+  for (const ObjectDef_t& od : objs) {
+    // Create handles
+    std::get<2>(objects[i]) = i;
+    std::get<1>(objects[i]).resize(od.handles.size());
+    Handles_t::iterator it = std::get<1>(objects[i]).begin();
+    for (const std::string hn : od.handles) {
+      *it = robot.handles.get(hn);
+      ++it;
+    }
+    // Create placement
+    const std::string placeN = "place_" + od.name;
+    const std::string preplaceN = "pre" + placeN;
+    // If user provides constraint "place_objectName",
+    // then
+    //   use this as placement and use "preplace_objectName" for
+    //   pre-placement if defined.
+    // else if contact surfaces are defined and selected
+    //   create default placement constraint using the ProblemSolver
+    //   methods createPlacementConstraint and createPrePlacementConstraint
+    auto& pc(std::get<0>(objects[i]));
+    if (ps->numericalConstraints.has(placeN)) {
+      std::get<0>(pc) = ps->numericalConstraints.get(placeN);
+      if (ps->numericalConstraints.has(preplaceN)) {
+        std::get<1>(pc) = ps->numericalConstraints.get(preplaceN);
+      }
+    } else if (!envNames.empty() && !od.shapes.empty()) {
+      ps->createPlacementConstraint(placeN, od.shapes, envNames, margin);
+      std::get<0>(pc) = ps->numericalConstraints.get(placeN);
+      if (prePlace) {
+        ps->createPrePlacementConstraint(preplaceN, od.shapes, envNames, margin,
+                                         prePlaceWidth);
+        std::get<1>(pc) = ps->numericalConstraints.get(preplaceN);
+      }
+    }
+    // Create object lock
+    // Loop over all frames of object, detect joint and create locked
+    // joint.
+    assert(robot.robotFrames(od.name).size() != 0);
+    for (const FrameIndex& f : robot.robotFrames(od.name)) {
+      if (model.frames[f].type != ::pinocchio::JOINT) continue;
+      const JointIndex j = model.frames[f].parent;
+      JointPtr_t oj(Joint::create(ps->robot(), j));
+      LiegroupSpacePtr_t space(oj->configurationSpace());
+      LiegroupElement lge(robot.currentConfiguration().segment(
+                              oj->rankInConfiguration(), oj->configSize()),
+                          space);
+      LockedJointPtr_t lj = core::LockedJoint::create(oj, lge);
+      ps->numericalConstraints.add("lock_" + oj->name(), lj);
+      std::get<2>(pc).push_back(lj);
+    }
+    ++i;
+  }
+  GraphPtr_t graph = Graph::create(graphName, ps->robot(), ps->problem());
+  ps->graphs.add(graphName, graph);
+  ps->constraintGraph(graphName);
+  graph->stateSelector(
+      GuidedStateSelector::create("stateSelector", ps->roadmap()));
+  graph->maxIterations(ps->maxIterProjection());
+  graph->errorThreshold(ps->errorThreshold());
+
+  graphBuilder(ps, objects, grippers, graph, rules);
+  return graph;
+}
+}  // namespace helper
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/state-selector.cc b/src/graph/state-selector.cc
index 9e5ee2f73c4a4a69d45b527e50e90ff0aa27d146..e68e669fc66630a8ad6a549795d57e0551d87a9d 100644
--- a/src/graph/state-selector.cc
+++ b/src/graph/state-selector.cc
@@ -26,125 +26,107 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/core/node.hh>
-
-#include <hpp/pinocchio/configuration.hh>
-#include "hpp/manipulation/roadmap-node.hh"
 #include "hpp/manipulation/graph/state-selector.hh"
 
 #include <cstdlib>
+#include <hpp/core/node.hh>
+#include <hpp/pinocchio/configuration.hh>
 
-namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      StateSelectorPtr_t StateSelector::create(const std::string& name)
-      {
-        StateSelector* ptr = new StateSelector (name);
-        StateSelectorPtr_t shPtr (ptr);
-        ptr->init (shPtr);
-        return shPtr;
-      }
-
-      void StateSelector::init (const StateSelectorPtr_t& weak)
-      {
-        wkPtr_ = weak;
-      }
-
-      StatePtr_t StateSelector::createState (const std::string& name,
-          bool waypoint, const int w)
-      {
-        StatePtr_t newState = State::create (name);
-        newState->stateSelector(wkPtr_);
-        newState->parentGraph(graph_);
-        newState->isWaypoint (waypoint);
-        if (waypoint) waypoints_.push_back(newState);
-        else {
-          bool found = false;
-          for (WeighedStates_t::iterator it = orderedStates_.begin();
-              it != orderedStates_.end (); ++it) {
-            if (it->first < w) {
-              orderedStates_.insert (it, WeighedState_t(w,newState));
-              found = true;
-              break;
-            }
-          }
-          if (!found) 
-            orderedStates_.push_back (WeighedState_t(w,newState));
-        }
-        return newState;
-      }
-
-      States_t StateSelector::getStates () const
-      {
-        States_t ret;
-        for (WeighedStates_t::const_iterator it = orderedStates_.begin();
-            it != orderedStates_.end (); ++it)
-          ret.push_back (it->second);
-        return ret;
-      }
-
-      States_t StateSelector::getWaypointStates () const
-      {
-        return waypoints_;
-      }
-
-      StatePtr_t StateSelector::getState(ConfigurationIn_t config) const
-      {
-        for (WeighedStates_t::const_iterator it = orderedStates_.begin();
-	     orderedStates_.end() != it; ++it) {
-          if (it->second->contains(config))
-            return it->second;
-	}
-	std::stringstream oss;
-	oss << "A configuration has no node:" << pinocchio::displayConfig (config);
-	throw std::logic_error (oss.str ());
-      }
-
-      StatePtr_t StateSelector::getState(RoadmapNodePtr_t node) const
-      {
-        if (!node->cacheUpToDate())
-          node->graphState(getState(*(node->configuration())));
-        return node->graphState();
-      }
-
-      EdgePtr_t StateSelector::chooseEdge(RoadmapNodePtr_t from) const
-      {
-        StatePtr_t state = getState (from);
-        const Neighbors_t neighborPicker = state->neighbors();
-        if (neighborPicker.totalWeight () == 0) {
-          return EdgePtr_t ();
-        }
-        return neighborPicker ();
-      }
-
-      std::ostream& StateSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
-      {
-        for (WeighedStates_t::const_iterator it = orderedStates_.begin();
-            orderedStates_.end() != it; ++it)
-          it->second->dotPrint (os);
-        return os;
-      }
-
-      std::ostream& StateSelector::print (std::ostream& os) const
-      {
-        for (WeighedStates_t::const_iterator it = orderedStates_.begin();
-            orderedStates_.end() != it; ++it)
-          os << it->first << " " << *it->second;
-        return os;
-      }
-
-      GraphPtr_t StateSelector::parentGraph() const
-      {
-        return graph_.lock ();
-      }
+#include "hpp/manipulation/roadmap-node.hh"
 
-      void StateSelector::parentGraph(const GraphWkPtr_t& parent)
-      {
-        graph_ = parent;
-        GraphPtr_t g = graph_.lock();
-        assert(g);
+namespace hpp {
+namespace manipulation {
+namespace graph {
+StateSelectorPtr_t StateSelector::create(const std::string& name) {
+  StateSelector* ptr = new StateSelector(name);
+  StateSelectorPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+void StateSelector::init(const StateSelectorPtr_t& weak) { wkPtr_ = weak; }
+
+StatePtr_t StateSelector::createState(const std::string& name, bool waypoint,
+                                      const int w) {
+  StatePtr_t newState = State::create(name);
+  newState->stateSelector(wkPtr_);
+  newState->parentGraph(graph_);
+  newState->isWaypoint(waypoint);
+  if (waypoint)
+    waypoints_.push_back(newState);
+  else {
+    bool found = false;
+    for (WeighedStates_t::iterator it = orderedStates_.begin();
+         it != orderedStates_.end(); ++it) {
+      if (it->first < w) {
+        orderedStates_.insert(it, WeighedState_t(w, newState));
+        found = true;
+        break;
       }
-
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+    }
+    if (!found) orderedStates_.push_back(WeighedState_t(w, newState));
+  }
+  return newState;
+}
+
+States_t StateSelector::getStates() const {
+  States_t ret;
+  for (WeighedStates_t::const_iterator it = orderedStates_.begin();
+       it != orderedStates_.end(); ++it)
+    ret.push_back(it->second);
+  return ret;
+}
+
+States_t StateSelector::getWaypointStates() const { return waypoints_; }
+
+StatePtr_t StateSelector::getState(ConfigurationIn_t config) const {
+  for (WeighedStates_t::const_iterator it = orderedStates_.begin();
+       orderedStates_.end() != it; ++it) {
+    if (it->second->contains(config)) return it->second;
+  }
+  std::stringstream oss;
+  oss << "A configuration has no node:" << pinocchio::displayConfig(config);
+  throw std::logic_error(oss.str());
+}
+
+StatePtr_t StateSelector::getState(RoadmapNodePtr_t node) const {
+  if (!node->cacheUpToDate())
+    node->graphState(getState(*(node->configuration())));
+  return node->graphState();
+}
+
+EdgePtr_t StateSelector::chooseEdge(RoadmapNodePtr_t from) const {
+  StatePtr_t state = getState(from);
+  const Neighbors_t neighborPicker = state->neighbors();
+  if (neighborPicker.totalWeight() == 0) {
+    return EdgePtr_t();
+  }
+  return neighborPicker();
+}
+
+std::ostream& StateSelector::dotPrint(std::ostream& os,
+                                      dot::DrawingAttributes) const {
+  for (WeighedStates_t::const_iterator it = orderedStates_.begin();
+       orderedStates_.end() != it; ++it)
+    it->second->dotPrint(os);
+  return os;
+}
+
+std::ostream& StateSelector::print(std::ostream& os) const {
+  for (WeighedStates_t::const_iterator it = orderedStates_.begin();
+       orderedStates_.end() != it; ++it)
+    os << it->first << " " << *it->second;
+  return os;
+}
+
+GraphPtr_t StateSelector::parentGraph() const { return graph_.lock(); }
+
+void StateSelector::parentGraph(const GraphWkPtr_t& parent) {
+  graph_ = parent;
+  GraphPtr_t g = graph_.lock();
+  assert(g);
+}
+
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/state.cc b/src/graph/state.cc
index 8f6af9369663f3946c630c756bc03b2260dfbe46..5ca3f5290caaaa6d07748f1c427e57271adb18f4 100644
--- a/src/graph/state.cc
+++ b/src/graph/state.cc
@@ -30,147 +30,138 @@
 
 #include <hpp/constraints/differentiable-function.hh>
 
+#include "hpp/manipulation/constraint-set.hh"
 #include "hpp/manipulation/device.hh"
 #include "hpp/manipulation/graph/edge.hh"
 #include "hpp/manipulation/graph/graph.hh"
-#include "hpp/manipulation/constraint-set.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      State::State (const std::string& name) :
-	GraphComponent (name), configConstraints_ (),
-        isWaypoint_ (false)
-      {}
-
-      State::~State ()
-      {}
-
-      StatePtr_t State::create (const std::string& name)
-      {
-        State* state = new State (name);
-        StatePtr_t shPtr(state);
-        shPtr->init(shPtr);
-        return shPtr;
-      }
-
-      void State::init (const StateWkPtr_t& weak)
-      {
-        GraphComponent::init (weak);
-        wkPtr_ = weak;
-      }
-
-      EdgePtr_t State::linkTo(const std::string& name, const StatePtr_t& to,
-			     const size_type& w, EdgeFactory create)
-      {
-        EdgePtr_t newEdge = create(name, graph_, wkPtr_, to);
-        if (w >= 0) neighbors_.insert (newEdge, (Weight_t)w);
-        else hiddenNeighbors_.push_back (newEdge);
-        return newEdge;
-      }
-
-      bool State::contains (ConfigurationIn_t config) const
-      {
-        return configConstraint()->isSatisfied (config);
-      }
-
-      std::ostream& State::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
-      {
-        da.insertWithQuote ("label", name ());
-        da.insert ("style","filled");
-        dot::Tooltip tp; tp.addLine ("State contains:");
-        populateTooltip (tp);
-        da.insertWithQuote ("tooltip", tp.toStr());
-        os << id () << " " << da << ";" << std::endl;
-
-        dot::DrawingAttributes dac;
-        std::vector <double> p = neighbors_.probabilities ();
-        size_t i = 0;
-        for (Neighbors_t::const_iterator it = neighbors_.begin();
-            it != neighbors_.end(); ++it) {
-          std::ostringstream oss; oss << (p[i] * 3 + 0.5);
-          dac ["penwidth"] = oss.str ();
-          i++;
-          it->second->dotPrint (os, dac) << std::endl;
-        }
-        return os;
-      }
-
-      void State::populateTooltip (dot::Tooltip& tp) const
-      {
-        GraphComponent::populateTooltip (tp);
-        tp.addLine ("");
-        tp.addLine ("Numerical constraints for paths are:");
-        for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin ();
-            it != numericalConstraintsForPath_.end (); ++it) {
-          tp.addLine ("- " + (*it)->function ().name ());
-        }
-      }
-
-      std::ostream& State::print (std::ostream& os) const
-      {
-        os << "|   |-- ";
-        GraphComponent::print (os) << std::endl;
-        for (Neighbors_t::const_iterator it = neighbors_.begin();
-            it != neighbors_.end(); ++it)
-          os << *(it->second) << " - " << it->first << std::endl;
-        return os;
-      }
-
-      void State::initialize()
-      {
-        isInit_ = true;
-
-        std::string n = "(" + name () + ")";
-        GraphPtr_t g = graph_.lock ();
-        configConstraints_ = ConstraintSet::create (g->robot (), "Set " + n);
-
-        ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
-        g->insertNumericalConstraints (proj);
-        insertNumericalConstraints (proj);
-        configConstraints_->addConstraint (proj);
-      }
-
-      void State::updateWeight (const EdgePtr_t& e, const Weight_t& w)
-      {
-        for (Neighbors_t::const_iterator it = neighbors_.begin();
-            it != neighbors_.end(); ++it) {
-          if (it->second == e) {
-            /// Update the weights
-            neighbors_.insert (e, w);
-          }
-        }
-        hppDout (error, "Edge not found");
-      }
-
-      Weight_t State::getWeight (const EdgePtr_t& e)
-      {
-        for (Neighbors_t::const_iterator it = neighbors_.begin();
-            it != neighbors_.end(); ++it)
-          if (it->second == e) return it->first;
-        for (std::vector<EdgePtr_t>::const_iterator it = hiddenNeighbors_.begin();
-            it != hiddenNeighbors_.end(); ++it)
-          if (*it == e) return -1;
-        hppDout (error, "Edge not found");
-        return 0;
-      }
-
-      void State::addNumericalConstraint(const ImplicitPtr_t& numConstraint)
-      {
-        ComparisonTypes_t comp(numConstraint->comparisonType());
-        for(constraints::ComparisonType c : comp)
-        {
-          if (c == constraints::Equality)
-          {
-            throw std::logic_error("Failed to insert constraint \""
-	     + numConstraint->function().name()
-	     + "\" as a state constraint since it contains a comparison "
-             + "of type Equality");
-          }
-        }
-        GraphComponent::addNumericalConstraint(numConstraint);
-      }
-
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+namespace graph {
+State::State(const std::string& name)
+    : GraphComponent(name), configConstraints_(), isWaypoint_(false) {}
+
+State::~State() {}
+
+StatePtr_t State::create(const std::string& name) {
+  State* state = new State(name);
+  StatePtr_t shPtr(state);
+  shPtr->init(shPtr);
+  return shPtr;
+}
+
+void State::init(const StateWkPtr_t& weak) {
+  GraphComponent::init(weak);
+  wkPtr_ = weak;
+}
+
+EdgePtr_t State::linkTo(const std::string& name, const StatePtr_t& to,
+                        const size_type& w, EdgeFactory create) {
+  EdgePtr_t newEdge = create(name, graph_, wkPtr_, to);
+  if (w >= 0)
+    neighbors_.insert(newEdge, (Weight_t)w);
+  else
+    hiddenNeighbors_.push_back(newEdge);
+  return newEdge;
+}
+
+bool State::contains(ConfigurationIn_t config) const {
+  return configConstraint()->isSatisfied(config);
+}
+
+std::ostream& State::dotPrint(std::ostream& os,
+                              dot::DrawingAttributes da) const {
+  da.insertWithQuote("label", name());
+  da.insert("style", "filled");
+  dot::Tooltip tp;
+  tp.addLine("State contains:");
+  populateTooltip(tp);
+  da.insertWithQuote("tooltip", tp.toStr());
+  os << id() << " " << da << ";" << std::endl;
+
+  dot::DrawingAttributes dac;
+  std::vector<double> p = neighbors_.probabilities();
+  size_t i = 0;
+  for (Neighbors_t::const_iterator it = neighbors_.begin();
+       it != neighbors_.end(); ++it) {
+    std::ostringstream oss;
+    oss << (p[i] * 3 + 0.5);
+    dac["penwidth"] = oss.str();
+    i++;
+    it->second->dotPrint(os, dac) << std::endl;
+  }
+  return os;
+}
+
+void State::populateTooltip(dot::Tooltip& tp) const {
+  GraphComponent::populateTooltip(tp);
+  tp.addLine("");
+  tp.addLine("Numerical constraints for paths are:");
+  for (NumericalConstraints_t::const_iterator it =
+           numericalConstraintsForPath_.begin();
+       it != numericalConstraintsForPath_.end(); ++it) {
+    tp.addLine("- " + (*it)->function().name());
+  }
+}
+
+std::ostream& State::print(std::ostream& os) const {
+  os << "|   |-- ";
+  GraphComponent::print(os) << std::endl;
+  for (Neighbors_t::const_iterator it = neighbors_.begin();
+       it != neighbors_.end(); ++it)
+    os << *(it->second) << " - " << it->first << std::endl;
+  return os;
+}
+
+void State::initialize() {
+  isInit_ = true;
+
+  std::string n = "(" + name() + ")";
+  GraphPtr_t g = graph_.lock();
+  configConstraints_ = ConstraintSet::create(g->robot(), "Set " + n);
+
+  ConfigProjectorPtr_t proj = ConfigProjector::create(
+      g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
+  g->insertNumericalConstraints(proj);
+  insertNumericalConstraints(proj);
+  configConstraints_->addConstraint(proj);
+}
+
+void State::updateWeight(const EdgePtr_t& e, const Weight_t& w) {
+  for (Neighbors_t::const_iterator it = neighbors_.begin();
+       it != neighbors_.end(); ++it) {
+    if (it->second == e) {
+      /// Update the weights
+      neighbors_.insert(e, w);
+    }
+  }
+  hppDout(error, "Edge not found");
+}
+
+Weight_t State::getWeight(const EdgePtr_t& e) {
+  for (Neighbors_t::const_iterator it = neighbors_.begin();
+       it != neighbors_.end(); ++it)
+    if (it->second == e) return it->first;
+  for (std::vector<EdgePtr_t>::const_iterator it = hiddenNeighbors_.begin();
+       it != hiddenNeighbors_.end(); ++it)
+    if (*it == e) return -1;
+  hppDout(error, "Edge not found");
+  return 0;
+}
+
+void State::addNumericalConstraint(const ImplicitPtr_t& numConstraint) {
+  ComparisonTypes_t comp(numConstraint->comparisonType());
+  for (constraints::ComparisonType c : comp) {
+    if (c == constraints::Equality) {
+      throw std::logic_error(
+          "Failed to insert constraint \"" + numConstraint->function().name() +
+          "\" as a state constraint since it contains a comparison " +
+          "of type Equality");
+    }
+  }
+  GraphComponent::addNumericalConstraint(numConstraint);
+}
+
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 79057321380ad02458bee4bcc64fce6bd782fc86..f651e154c5561adfae069ecb1a57818b887710a0 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -31,280 +31,221 @@
 #include "hpp/manipulation/constraint-set.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      LeafBin::LeafBin(const vector_t& v, value_type* thr):
-        value_(v), nodes_(), thr_ (thr)
-      {}
-
-      void LeafBin::push_back(const RoadmapNodePtr_t& n)
-      {
-        nodes_.push_back(n);
-      }
-
-      bool LeafBin::operator<(const LeafBin& rhs) const
-      {
-        const vector_t& v = rhs.value ();
-        assert (value_.size() == v.size());
-        for (int p = 0; p < value_.size(); p++) {
-          if (std::abs (value_[p] - v[p]) >= *thr_)
-            return value_[p] < v[p];
-        }
-        return false;
-      }
-
-      bool LeafBin::operator==(const LeafBin& rhs) const
-      {
-        const vector_t& v = rhs.value ();
-        assert (value_.size() == v.size());
-        for (int p = 0; p < value_.size(); p++) {
-          if (std::abs (value_[p] - v[p]) >= *thr_)
-            return false;
-        }
-        return true;
-      }
-
-      const vector_t& LeafBin::value () const
-      {
-        return value_;
-      }
-
-      std::ostream& LeafBin::print (std::ostream& os) const
-      {
-        Parent::print (os) << " (";
-        /// Sort by connected component.
-        typedef std::list <RoadmapNodes_t> NodesList_t;
-        NodesList_t l;
-        bool found;
-        for (RoadmapNodes_t::const_iterator itn = nodes_.begin ();
-            itn != nodes_.end (); ++itn) {
-          found = false;
-          for (NodesList_t::iterator itc = l.begin ();
-              itc != l.end (); ++itc) {
-            if ((*itn)->connectedComponent () == itc->front ()->connectedComponent ()) {
-              itc->push_back (*itn);
-              found = true;
-              break;
-            }
-          }
-          if (!found) {
-            l.push_back (RoadmapNodes_t (1, *itn));
-          }
-        }
-        for (NodesList_t::iterator itc = l.begin ();
-            itc != l.end (); ++itc)
-          os << itc->front ()->connectedComponent () << " - " << itc->size () << ", ";
-        return os << ").";
-      }
-
-      std::ostream& LeafBin::printValue (std::ostream& os) const
-      {
-        os << "LeafBin (";
-        size_t s = value_.size();
-        if (s != 0) {
-          for (size_t i = 0; i < s - 1; i++)
-            os << value_[i] << "; ";
-          os << value_[s-1];
-        }
-        os << ")";
-        return os;
-      }
-
-      NodeBin::NodeBin(const StatePtr_t& n):
-        state_(n), roadmapNodes_()
-      {}
-
-      void NodeBin::push_back(const RoadmapNodePtr_t& n)
-      {
-        roadmapNodes_.push_back(n);
-      }
-
-      bool NodeBin::operator<(const NodeBin& rhs) const
-      {
-        return state()->id () < rhs.state ()->id ();
-      }
-
-      bool NodeBin::operator==(const NodeBin& rhs) const
-      {
-        return state() == rhs.state ();
-      }
-
-      const StatePtr_t& NodeBin::state () const
-      {
-        return state_;
-      }
-
-      std::ostream& NodeBin::print (std::ostream& os) const
-      {
-        Parent::print (os) << " (";
-        /// Sort by connected component.
-        typedef std::list <RoadmapNodes_t> NodesList_t;
-        NodesList_t l;
-        bool found;
-        for (RoadmapNodes_t::const_iterator itn = roadmapNodes_.begin ();
-            itn != roadmapNodes_.end (); ++itn) {
-          found = false;
-          for (NodesList_t::iterator itc = l.begin ();
-              itc != l.end (); ++itc) {
-            if ((*itn)->connectedComponent () == itc->front ()->connectedComponent ()) {
-              itc->push_back (*itn);
-              found = true;
-              break;
-            }
-          }
-          if (!found) {
-            l.push_back (RoadmapNodes_t (1, *itn));
-          }
-        }
-        for (NodesList_t::iterator itc = l.begin ();
-            itc != l.end (); ++itc)
-          os << itc->front ()->connectedComponent () << " - " << itc->size () << ", ";
-        return os << ").";
-      }
-
-      std::ostream& NodeBin::printValue (std::ostream& os) const
-      {
-        return os << "NodeBin (" << state()->name () << ")";
-      }
-
-      LeafHistogramPtr_t LeafHistogram::create (const Foliation f)
-      {
-        return LeafHistogramPtr_t (new LeafHistogram (f));
-      }
-
-      LeafHistogram::LeafHistogram (const Foliation f) :
-        f_ (f), threshold_ (0)
-      {
-        ConfigProjectorPtr_t p = f_.parametrizer ()->configProjector();
-        if (p) {
-          if (p->rightHandSide ().size () > 0)
-            threshold_ = p->errorThreshold () /
-              sqrt((double)p->rightHandSide ().size ());
-        }
-      }
-
-      void LeafHistogram::add (const RoadmapNodePtr_t& n)
-      {
-        if (!f_.contains (*n->configuration())) return;
-	iterator it = insert (LeafBin (f_.parameter (*n->configuration()),
-                              &threshold_));
-        it->push_back (n);
-        if (numberOfObservations()%10 == 0) {
-          hppDout (info, *this);
-        }
-      }
-
-      std::ostream& LeafHistogram::print (std::ostream& os) const
-      {
-        os << "Leaf Histogram of foliation " << f_.condition()->name() << std::endl;
-        return Parent::print (os);
-      }
-
-      HistogramPtr_t LeafHistogram::clone () const
-      {
-        return HistogramPtr_t (new LeafHistogram (f_));
-      }
-
-      StateHistogram::StateHistogram (const graph::GraphPtr_t& graph) :
-        graph_ (graph) {}
-
-      void StateHistogram::add (const RoadmapNodePtr_t& n)
-      {
-        iterator it = insert (NodeBin (constraintGraph()->getState (n)));
-        it->push_back (n);
-        if (numberOfObservations()%10 == 0) {
-          hppDout (info, *this);
-        }
-      }
-
-      std::ostream& StateHistogram::print (std::ostream& os) const
-      {
-        os << "Graph State Histogram contains: " << std::endl;
-        return Parent::print (os);
-      }
-
-      const graph::GraphPtr_t& StateHistogram::constraintGraph () const
-      {
-        return graph_;
-      }
-
-      HistogramPtr_t StateHistogram::clone () const
-      {
-        return HistogramPtr_t (new StateHistogram (constraintGraph()));
-      }
-
-      unsigned int LeafBin::numberOfObsOutOfConnectedComponent (const core::ConnectedComponentPtr_t& cc) const
-      {
-        unsigned int count = 0;
-        for (RoadmapNodes_t::const_iterator it = nodes_.begin ();
-            it != nodes_.end (); ++it)
-          if ((*it)->connectedComponent () != cc)
-            count++;
-        return count;
-      }
-
-      statistics::DiscreteDistribution < RoadmapNodePtr_t > LeafHistogram::getDistribOutOfConnectedComponent (
-          const core::ConnectedComponentPtr_t& cc) const
-      {
-        statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib;
-        for (const_iterator bin = begin(); bin != end (); ++bin) {
-          unsigned int w = bin->numberOfObsOutOfConnectedComponent (cc);
-          if (w == 0)
-            continue;
-          distrib.insert (bin->nodes ().front (), w);
-        }
-        return distrib;
-      }
-
-      statistics::DiscreteDistribution < RoadmapNodePtr_t > LeafHistogram::getDistrib () const
-      {
-        statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib;
-        for (const_iterator bin = begin(); bin != end (); ++bin) {
-          std::size_t w = bin->freq ();
-          if (w == 0)
-            continue;
-          distrib.insert (bin->nodes ().front (), w);
-        }
-        return distrib;
-      }
-
-      const LeafBin::RoadmapNodes_t& LeafBin::nodes () const
-      {
-        return nodes_;
-      }
-
-      bool Foliation::contains (ConfigurationIn_t q) const
-      {
-        return condition_->isSatisfied (q);
-      }
-
-      vector_t Foliation::parameter (ConfigurationIn_t q) const
-      {
-        if (!condition_->isSatisfied (q)) {
-          hppDout (error, "Configuration not in the foliation");
-        }
-        return parametrizer_->configProjector()->rightHandSideFromConfig (q);
-      }
-
-      ConstraintSetPtr_t Foliation::condition () const
-      {
-        return condition_;
-      }
-
-      void Foliation::condition (const ConstraintSetPtr_t c)
-      {
-        condition_ = c;
-      }
-
-      ConstraintSetPtr_t Foliation::parametrizer () const
-      {
-        return parametrizer_;
-      }
-
-      void Foliation::parametrizer (const ConstraintSetPtr_t p)
-      {
-        parametrizer_ = p;
-      }
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+namespace graph {
+LeafBin::LeafBin(const vector_t& v, value_type* thr)
+    : value_(v), nodes_(), thr_(thr) {}
+
+void LeafBin::push_back(const RoadmapNodePtr_t& n) { nodes_.push_back(n); }
+
+bool LeafBin::operator<(const LeafBin& rhs) const {
+  const vector_t& v = rhs.value();
+  assert(value_.size() == v.size());
+  for (int p = 0; p < value_.size(); p++) {
+    if (std::abs(value_[p] - v[p]) >= *thr_) return value_[p] < v[p];
+  }
+  return false;
+}
+
+bool LeafBin::operator==(const LeafBin& rhs) const {
+  const vector_t& v = rhs.value();
+  assert(value_.size() == v.size());
+  for (int p = 0; p < value_.size(); p++) {
+    if (std::abs(value_[p] - v[p]) >= *thr_) return false;
+  }
+  return true;
+}
+
+const vector_t& LeafBin::value() const { return value_; }
+
+std::ostream& LeafBin::print(std::ostream& os) const {
+  Parent::print(os) << " (";
+  /// Sort by connected component.
+  typedef std::list<RoadmapNodes_t> NodesList_t;
+  NodesList_t l;
+  bool found;
+  for (RoadmapNodes_t::const_iterator itn = nodes_.begin(); itn != nodes_.end();
+       ++itn) {
+    found = false;
+    for (NodesList_t::iterator itc = l.begin(); itc != l.end(); ++itc) {
+      if ((*itn)->connectedComponent() == itc->front()->connectedComponent()) {
+        itc->push_back(*itn);
+        found = true;
+        break;
+      }
+    }
+    if (!found) {
+      l.push_back(RoadmapNodes_t(1, *itn));
+    }
+  }
+  for (NodesList_t::iterator itc = l.begin(); itc != l.end(); ++itc)
+    os << itc->front()->connectedComponent() << " - " << itc->size() << ", ";
+  return os << ").";
+}
+
+std::ostream& LeafBin::printValue(std::ostream& os) const {
+  os << "LeafBin (";
+  size_t s = value_.size();
+  if (s != 0) {
+    for (size_t i = 0; i < s - 1; i++) os << value_[i] << "; ";
+    os << value_[s - 1];
+  }
+  os << ")";
+  return os;
+}
+
+NodeBin::NodeBin(const StatePtr_t& n) : state_(n), roadmapNodes_() {}
+
+void NodeBin::push_back(const RoadmapNodePtr_t& n) {
+  roadmapNodes_.push_back(n);
+}
+
+bool NodeBin::operator<(const NodeBin& rhs) const {
+  return state()->id() < rhs.state()->id();
+}
+
+bool NodeBin::operator==(const NodeBin& rhs) const {
+  return state() == rhs.state();
+}
+
+const StatePtr_t& NodeBin::state() const { return state_; }
+
+std::ostream& NodeBin::print(std::ostream& os) const {
+  Parent::print(os) << " (";
+  /// Sort by connected component.
+  typedef std::list<RoadmapNodes_t> NodesList_t;
+  NodesList_t l;
+  bool found;
+  for (RoadmapNodes_t::const_iterator itn = roadmapNodes_.begin();
+       itn != roadmapNodes_.end(); ++itn) {
+    found = false;
+    for (NodesList_t::iterator itc = l.begin(); itc != l.end(); ++itc) {
+      if ((*itn)->connectedComponent() == itc->front()->connectedComponent()) {
+        itc->push_back(*itn);
+        found = true;
+        break;
+      }
+    }
+    if (!found) {
+      l.push_back(RoadmapNodes_t(1, *itn));
+    }
+  }
+  for (NodesList_t::iterator itc = l.begin(); itc != l.end(); ++itc)
+    os << itc->front()->connectedComponent() << " - " << itc->size() << ", ";
+  return os << ").";
+}
+
+std::ostream& NodeBin::printValue(std::ostream& os) const {
+  return os << "NodeBin (" << state()->name() << ")";
+}
+
+LeafHistogramPtr_t LeafHistogram::create(const Foliation f) {
+  return LeafHistogramPtr_t(new LeafHistogram(f));
+}
+
+LeafHistogram::LeafHistogram(const Foliation f) : f_(f), threshold_(0) {
+  ConfigProjectorPtr_t p = f_.parametrizer()->configProjector();
+  if (p) {
+    if (p->rightHandSide().size() > 0)
+      threshold_ =
+          p->errorThreshold() / sqrt((double)p->rightHandSide().size());
+  }
+}
+
+void LeafHistogram::add(const RoadmapNodePtr_t& n) {
+  if (!f_.contains(*n->configuration())) return;
+  iterator it = insert(LeafBin(f_.parameter(*n->configuration()), &threshold_));
+  it->push_back(n);
+  if (numberOfObservations() % 10 == 0) {
+    hppDout(info, *this);
+  }
+}
+
+std::ostream& LeafHistogram::print(std::ostream& os) const {
+  os << "Leaf Histogram of foliation " << f_.condition()->name() << std::endl;
+  return Parent::print(os);
+}
+
+HistogramPtr_t LeafHistogram::clone() const {
+  return HistogramPtr_t(new LeafHistogram(f_));
+}
+
+StateHistogram::StateHistogram(const graph::GraphPtr_t& graph)
+    : graph_(graph) {}
+
+void StateHistogram::add(const RoadmapNodePtr_t& n) {
+  iterator it = insert(NodeBin(constraintGraph()->getState(n)));
+  it->push_back(n);
+  if (numberOfObservations() % 10 == 0) {
+    hppDout(info, *this);
+  }
+}
+
+std::ostream& StateHistogram::print(std::ostream& os) const {
+  os << "Graph State Histogram contains: " << std::endl;
+  return Parent::print(os);
+}
+
+const graph::GraphPtr_t& StateHistogram::constraintGraph() const {
+  return graph_;
+}
+
+HistogramPtr_t StateHistogram::clone() const {
+  return HistogramPtr_t(new StateHistogram(constraintGraph()));
+}
+
+unsigned int LeafBin::numberOfObsOutOfConnectedComponent(
+    const core::ConnectedComponentPtr_t& cc) const {
+  unsigned int count = 0;
+  for (RoadmapNodes_t::const_iterator it = nodes_.begin(); it != nodes_.end();
+       ++it)
+    if ((*it)->connectedComponent() != cc) count++;
+  return count;
+}
+
+statistics::DiscreteDistribution<RoadmapNodePtr_t>
+LeafHistogram::getDistribOutOfConnectedComponent(
+    const core::ConnectedComponentPtr_t& cc) const {
+  statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib;
+  for (const_iterator bin = begin(); bin != end(); ++bin) {
+    unsigned int w = bin->numberOfObsOutOfConnectedComponent(cc);
+    if (w == 0) continue;
+    distrib.insert(bin->nodes().front(), w);
+  }
+  return distrib;
+}
+
+statistics::DiscreteDistribution<RoadmapNodePtr_t> LeafHistogram::getDistrib()
+    const {
+  statistics::DiscreteDistribution<RoadmapNodePtr_t> distrib;
+  for (const_iterator bin = begin(); bin != end(); ++bin) {
+    std::size_t w = bin->freq();
+    if (w == 0) continue;
+    distrib.insert(bin->nodes().front(), w);
+  }
+  return distrib;
+}
+
+const LeafBin::RoadmapNodes_t& LeafBin::nodes() const { return nodes_; }
+
+bool Foliation::contains(ConfigurationIn_t q) const {
+  return condition_->isSatisfied(q);
+}
+
+vector_t Foliation::parameter(ConfigurationIn_t q) const {
+  if (!condition_->isSatisfied(q)) {
+    hppDout(error, "Configuration not in the foliation");
+  }
+  return parametrizer_->configProjector()->rightHandSideFromConfig(q);
+}
+
+ConstraintSetPtr_t Foliation::condition() const { return condition_; }
+
+void Foliation::condition(const ConstraintSetPtr_t c) { condition_ = c; }
+
+ConstraintSetPtr_t Foliation::parametrizer() const { return parametrizer_; }
+
+void Foliation::parametrizer(const ConstraintSetPtr_t p) { parametrizer_ = p; }
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/graph/validation.cc b/src/graph/validation.cc
index 3babd63a73b58a60969abe6b580114bc8187064a..38d5756b001a492517fccb57d8c425b9b62e63e6 100644
--- a/src/graph/validation.cc
+++ b/src/graph/validation.cc
@@ -28,188 +28,181 @@
 
 #include "hpp/manipulation/graph/validation.hh"
 
-#include <sstream>
-
-#include <hpp/util/indent.hh>
-
 #include <hpp/constraints/differentiable-function.hh>
-
+#include <hpp/core/collision-validation-report.hh>
 #include <hpp/core/collision-validation.hh>
 #include <hpp/core/configuration-shooter.hh>
 #include <hpp/core/relative-motion.hh>
-#include <hpp/core/collision-validation-report.hh>
+#include <hpp/util/indent.hh>
+#include <sstream>
 
-#include "hpp/manipulation/problem.hh"
 #include "hpp/manipulation/graph/edge.hh"
-#include "hpp/manipulation/graph/state.hh"
 #include "hpp/manipulation/graph/state-selector.hh"
+#include "hpp/manipulation/graph/state.hh"
+#include "hpp/manipulation/problem.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace graph {
-      bool stateAIncludedInStateB (const StatePtr_t& A, const StatePtr_t& B)
-      {
-        const NumericalConstraints_t& Ancs = A->numericalConstraints();
-        const NumericalConstraints_t& Bncs = B->numericalConstraints();
-        for (NumericalConstraints_t::const_iterator _nc = Bncs.begin();
-            _nc != Bncs.end(); ++_nc)
-          if (std::find (Ancs.begin(), Ancs.end(), *_nc) == Ancs.end())
-            return false;
-        return true;
-      }
-
-      std::ostream& Validation::print (std::ostream& os) const
-      {
-        for (std::size_t i = 0; i < warnings_.size(); ++i) {
-          os << "Warning " << i
-            << " (" << (warnings_[i].first ? warnings_[i].first->name() : "no component")
-            << ')' << incendl << warnings_[i].second << decendl;
-        }
-        for (std::size_t i = 0; i < errors_.size(); ++i) {
-          os << "Error " << i
-            << " (" << (errors_[i].first ? errors_[i].first->name() : "no component")
-            << ')' << incendl << errors_[i].second << decendl;
-        }
-        return os;
-      }
-
-      bool Validation::validate (const GraphComponentPtr_t& comp)
-      {
-        if      (HPP_DYNAMIC_PTR_CAST(State, comp))
-          return validateState (HPP_DYNAMIC_PTR_CAST(State, comp));
-        else if (HPP_DYNAMIC_PTR_CAST(Edge, comp))
-          return validateEdge  (HPP_DYNAMIC_PTR_CAST(Edge, comp));
-        else if (HPP_DYNAMIC_PTR_CAST(Graph, comp))
-          return validateGraph (HPP_DYNAMIC_PTR_CAST(Graph, comp));
-        else
-          return true;
+namespace manipulation {
+namespace graph {
+bool stateAIncludedInStateB(const StatePtr_t& A, const StatePtr_t& B) {
+  const NumericalConstraints_t& Ancs = A->numericalConstraints();
+  const NumericalConstraints_t& Bncs = B->numericalConstraints();
+  for (NumericalConstraints_t::const_iterator _nc = Bncs.begin();
+       _nc != Bncs.end(); ++_nc)
+    if (std::find(Ancs.begin(), Ancs.end(), *_nc) == Ancs.end()) return false;
+  return true;
+}
+
+std::ostream& Validation::print(std::ostream& os) const {
+  for (std::size_t i = 0; i < warnings_.size(); ++i) {
+    os << "Warning " << i << " ("
+       << (warnings_[i].first ? warnings_[i].first->name() : "no component")
+       << ')' << incendl << warnings_[i].second << decendl;
+  }
+  for (std::size_t i = 0; i < errors_.size(); ++i) {
+    os << "Error " << i << " ("
+       << (errors_[i].first ? errors_[i].first->name() : "no component") << ')'
+       << incendl << errors_[i].second << decendl;
+  }
+  return os;
+}
+
+bool Validation::validate(const GraphComponentPtr_t& comp) {
+  if (HPP_DYNAMIC_PTR_CAST(State, comp))
+    return validateState(HPP_DYNAMIC_PTR_CAST(State, comp));
+  else if (HPP_DYNAMIC_PTR_CAST(Edge, comp))
+    return validateEdge(HPP_DYNAMIC_PTR_CAST(Edge, comp));
+  else if (HPP_DYNAMIC_PTR_CAST(Graph, comp))
+    return validateGraph(HPP_DYNAMIC_PTR_CAST(Graph, comp));
+  else
+    return true;
+}
+
+bool Validation::validateState(const StatePtr_t& state) {
+  bool success = true;
+
+  // 1. Check that all the constraint are not parameterizable.
+  const NumericalConstraints_t& ncs = state->numericalConstraints();
+  for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
+       _nc != ncs.end(); ++_nc)
+    if ((*_nc)->parameterSize() > 0) {
+      std::ostringstream oss;
+      oss << incindent << "Constraint " << (*_nc)->function().name()
+          << " has a varying right hand side while constraints of a state must"
+             " have a constant one.";
+      addError(state, oss.str());
+    }
+
+  // 2. try to generate a configuration in this state.
+  bool projOk;
+  Configuration_t q;
+  std::size_t i, Nrand = 100;
+
+  for (i = 0; i < Nrand; i++) {
+    problem_->configurationShooter()->shoot(q);
+    projOk = state->configConstraint()->apply(q);
+    if (projOk) break;
+  }
+  if (!projOk) {
+    std::ostringstream oss;
+    oss << incindent << "Failed to apply the constraints to " << Nrand
+        << "random configurations.";
+    addError(state, oss.str());
+    return false;
+  }
+  if (4 * i > 3 * Nrand) {
+    std::ostringstream oss;
+    oss << incindent << "Success rate of constraint projection is " << i / Nrand
+        << '.';
+    addWarning(state, oss.str());
+    oss.clear();
+  }
+
+  // 3. check the collision pairs which will be disabled because of the
+  //    constraint.
+  core::CollisionValidationPtr_t colValidation(
+      core::CollisionValidation::create(problem_->robot()));
+  for (std::size_t i = 0; i < problem_->collisionObstacles().size(); ++i)
+    colValidation->addObstacle(problem_->collisionObstacles()[i]);
+  colValidation->computeAllContacts(true);
+
+  typedef core::RelativeMotion RelativeMotion;
+  RelativeMotion::matrix_type relMotion =
+      RelativeMotion::matrix(problem_->robot());
+  RelativeMotion::fromConstraint(relMotion, problem_->robot(),
+                                 state->configConstraint());
+
+  // Invert the relative motions.
+  hppDout(info, "Relative motion matrix:\n" << relMotion);
+  for (size_type r = 0; r < relMotion.rows(); ++r)
+    for (size_type c = 0; c < r; ++c) {
+      if (relMotion(r, c) != relMotion(c, r)) {
+        hppDout(error, "Relative motion matrix not symmetric.");
       }
-
-      bool Validation::validateState (const StatePtr_t& state)
-      {
-        bool success = true;
-
-        // 1. Check that all the constraint are not parameterizable.
-        const NumericalConstraints_t& ncs = state->numericalConstraints();
-        for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
-            _nc != ncs.end(); ++_nc)
-          if ((*_nc)->parameterSize() > 0) {
-            std::ostringstream oss;
-            oss << incindent << "Constraint " << (*_nc)->function().name() <<
-              " has a varying right hand side while constraints of a state must"
-              " have a constant one.";
-            addError (state, oss.str());
-          }
-
-        // 2. try to generate a configuration in this state.
-        bool projOk;
-        Configuration_t q;
-        std::size_t i, Nrand = 100;
-
-        for (i = 0; i < Nrand; i++) {
-          problem_->configurationShooter()->shoot(q);
-          projOk = state->configConstraint()->apply (q);
-          if (projOk) break;
-        }
-        if (!projOk) {
-          std::ostringstream oss;
-          oss << incindent << "Failed to apply the constraints to " << Nrand <<
-            "random configurations.";
-          addError (state, oss.str());
-          return false;
-        }
-        if (4 * i > 3 * Nrand) {
-          std::ostringstream oss;
-          oss << incindent << "Success rate of constraint projection is " <<
-            i / Nrand << '.';
-          addWarning (state, oss.str());
-          oss.clear();
-        }
-
-        // 3. check the collision pairs which will be disabled because of the
-        //    constraint.
-        core::CollisionValidationPtr_t colValidation (
-            core::CollisionValidation::create (problem_->robot()));
-        for (std::size_t i = 0; i < problem_->collisionObstacles().size(); ++i)
-          colValidation->addObstacle (problem_->collisionObstacles()[i]);
-        colValidation->computeAllContacts (true);
-
-        typedef core::RelativeMotion RelativeMotion;
-        RelativeMotion::matrix_type relMotion = RelativeMotion::matrix (problem_->robot());
-        RelativeMotion::fromConstraint (relMotion, problem_->robot(),
-            state->configConstraint());
-
-        // Invert the relative motions.
-        hppDout (info, "Relative motion matrix:\n" << relMotion);
-        for (size_type r = 0; r < relMotion.rows(); ++r)
-          for (size_type c = 0; c < r; ++c) {
-            if (relMotion(r,c) != relMotion(c,r)) {
-              hppDout (error, "Relative motion matrix not symmetric.");
-            }
-            switch (relMotion(r,c)) {
-              case RelativeMotion::Constrained:
-                relMotion(r,c) = relMotion(c,r) = RelativeMotion::Unconstrained;
-                break;
-              case RelativeMotion::Parameterized:
-              case RelativeMotion::Unconstrained:
-                relMotion(r,c) = relMotion(c,r) = RelativeMotion::Constrained;
-                break;
-              default:
-                throw std::logic_error ("Relative motion not understood");
-            }
-          }
-        hppDout (info, "Relative motion matrix:\n" << relMotion);
-
-        colValidation->filterCollisionPairs (relMotion);
-        core::ValidationReportPtr_t colReport;
-        bool colOk = colValidation->validate (q, colReport);
-
-        if (!colOk) {
-          std::ostringstream oss;
-          oss << incindent << "The following collision pairs will always "
-            "collide." << incendl << *colReport << decindent;
-          addError (state, oss.str());
-          if (HPP_DYNAMIC_PTR_CAST(core::CollisionValidationReport, colReport)) {
-            std::pair<std::string, std::string> names = HPP_DYNAMIC_PTR_CAST(core::CollisionValidationReport, colReport)->getObjectNames();
-            addCollision (state, names.first, names.second);
-          }
-          success = false;
-        }
-
-        return success;
+      switch (relMotion(r, c)) {
+        case RelativeMotion::Constrained:
+          relMotion(r, c) = relMotion(c, r) = RelativeMotion::Unconstrained;
+          break;
+        case RelativeMotion::Parameterized:
+        case RelativeMotion::Unconstrained:
+          relMotion(r, c) = relMotion(c, r) = RelativeMotion::Constrained;
+          break;
+        default:
+          throw std::logic_error("Relative motion not understood");
       }
-
-      bool Validation::validateEdge  (const EdgePtr_t &)
-      {
-        return true;
-      }
-
-      bool Validation::validateGraph (const GraphPtr_t& graph)
-      {
-        if (!graph) return false;
-        bool success = true;
-
-        for (std::size_t i = 1; i < graph->nbComponents(); ++i)
-          if (!validate(graph->get(i).lock())) success = false;
-
-        // Check that no state is included in a state which has a higher priority.
-        States_t states = graph->stateSelector()->getStates();
-        for (States_t::const_iterator _state = states.begin();
-            _state != states.end(); ++_state) {
-          for (States_t::const_iterator _stateHO = states.begin();
-              _stateHO != _state; ++_stateHO) {
-            if (stateAIncludedInStateB (*_state, *_stateHO)) {
-              std::ostringstream oss;
-              oss << "State " << (*_state)->name() << " is included in state "
-                << (*_stateHO)->name() << " but the latter has a higher priority.";
-              addError (*_state, oss.str());
-              success = false;
-            }
-          }
-        }
-
-        return success;
+    }
+  hppDout(info, "Relative motion matrix:\n" << relMotion);
+
+  colValidation->filterCollisionPairs(relMotion);
+  core::ValidationReportPtr_t colReport;
+  bool colOk = colValidation->validate(q, colReport);
+
+  if (!colOk) {
+    std::ostringstream oss;
+    oss << incindent
+        << "The following collision pairs will always "
+           "collide."
+        << incendl << *colReport << decindent;
+    addError(state, oss.str());
+    if (HPP_DYNAMIC_PTR_CAST(core::CollisionValidationReport, colReport)) {
+      std::pair<std::string, std::string> names =
+          HPP_DYNAMIC_PTR_CAST(core::CollisionValidationReport, colReport)
+              ->getObjectNames();
+      addCollision(state, names.first, names.second);
+    }
+    success = false;
+  }
+
+  return success;
+}
+
+bool Validation::validateEdge(const EdgePtr_t&) { return true; }
+
+bool Validation::validateGraph(const GraphPtr_t& graph) {
+  if (!graph) return false;
+  bool success = true;
+
+  for (std::size_t i = 1; i < graph->nbComponents(); ++i)
+    if (!validate(graph->get(i).lock())) success = false;
+
+  // Check that no state is included in a state which has a higher priority.
+  States_t states = graph->stateSelector()->getStates();
+  for (States_t::const_iterator _state = states.begin(); _state != states.end();
+       ++_state) {
+    for (States_t::const_iterator _stateHO = states.begin(); _stateHO != _state;
+         ++_stateHO) {
+      if (stateAIncludedInStateB(*_state, *_stateHO)) {
+        std::ostringstream oss;
+        oss << "State " << (*_state)->name() << " is included in state "
+            << (*_stateHO)->name() << " but the latter has a higher priority.";
+        addError(*_state, oss.str());
+        success = false;
       }
-    } // namespace graph
-  } // namespace manipulation
-} // namespace hpp
+    }
+  }
+
+  return success;
+}
+}  // namespace graph
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/handle.cc b/src/handle.cc
index 9f0bab5c30f0967cb8763662f33ad65a8157365c..8e5484c60570d051441ebf98f72b4306d99a6d0c 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -28,229 +28,210 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/handle.hh>
-
-#include <pinocchio/multibody/joint/joint-generic.hpp>
-
-#include <hpp/util/debug.hh>
-
-#include <hpp/pinocchio/joint.hh>
-#include <hpp/pinocchio/joint-collection.hh>
-#include <hpp/pinocchio/gripper.hh>
-
-#include <hpp/constraints/implicit.hh>
 #include <hpp/constraints/explicit/relative-pose.hh>
 #include <hpp/constraints/generic-transformation.hh>
-
+#include <hpp/constraints/implicit.hh>
 #include <hpp/manipulation/device.hh>
+#include <hpp/manipulation/handle.hh>
+#include <hpp/pinocchio/gripper.hh>
+#include <hpp/pinocchio/joint-collection.hh>
+#include <hpp/pinocchio/joint.hh>
+#include <hpp/util/debug.hh>
+#include <pinocchio/multibody/joint/joint-generic.hpp>
 
 namespace hpp {
-  namespace manipulation {
-    using constraints::Implicit;
-    using constraints::ImplicitPtr_t;
-    using constraints::Explicit;
-    using constraints::ExplicitPtr_t;
-    using constraints::DifferentiableFunction;
-
-    std::string Handle::className ("Handle");
-    namespace {
-      static const matrix3_t I3 = matrix3_t::Identity();
-
-      struct ZeroDiffFunc : public DifferentiableFunction {
-        ZeroDiffFunc (size_type sIn, size_type sInD,
-            std::string name=std::string("Empty function"))
-          : DifferentiableFunction (sIn, sInD, LiegroupSpace::empty (), name)
-        {
-          context ("Grasp complement");
-        }
-
-        inline void impl_compute (pinocchio::LiegroupElementRef, vectorIn_t) const {}
-        inline void impl_jacobian (matrixOut_t, vectorIn_t) const {}
-      };
-    }
-
-    bool isHandleOnFreeflyer (const Handle& handle)
-    {
-      const JointPtr_t& joint = handle.joint();
-      if (   joint
-          && !joint->parentJoint()
-          && joint->jointModel().shortname() == ::pinocchio::JointModelFreeFlyer::classname()) {
-	return true;
-      }
-      return false;
-    }
-
-    inline int maskSize (const std::vector<bool>& mask)
-    {
-      std::size_t res (0);
-      for (std::size_t i = 0; i < 6; ++i) {
-        if (mask[i]) ++res;
-      }
-      return (int)res;
-    }
-
-    inline std::vector<bool> boolOr(std::vector<bool> mask1,
-                                    std::vector<bool> mask2)
-    {
-      assert(mask1.size() == 6 && mask2.size() == 6);
-      std::vector<bool> res(mask1.size());
-      for (std::size_t i = 0; i < 6; ++i) {
-        res[i] = mask1[i] || mask2[i];
-      }
-      return res;
-    }
-
-    inline bool is6Dmask (const std::vector<bool>& mask)
-    {
-      for (std::size_t i = 0; i < 6; ++i) if (!mask[i]) return false;
-      return true;
-    }
-
-    inline std::vector<bool> complementMask (const std::vector<bool>& mask)
-    {
-      std::vector<bool> m(6);
-      for (std::size_t i = 0; i < 6; ++i) m[i] = !mask[i];
-      return m;
-    }
-
-    inline std::string maskToStr (const std::vector<bool>& mask)
-    {
-      std::stringstream ss;
-      ss << "(";
-      for (std::size_t i = 0; i < 5; ++i) ss << mask[i] << ",";
-      ss << mask[5] << ")";
-      return ss.str();
-    }
-
-    void Handle::mask (const std::vector<bool>& mask)
-    {
-      assert(mask.size() == 6);
-      std::size_t nRotFree = 3;
-      for (std::size_t i = 3; i < 6; ++i)
-        if (mask[i]) nRotFree--;
-      switch (nRotFree) {
-        case 1: // TODO we should check the axis are properly aligned.
-          break;
-        case 2: // This does not make sense.
-          throw std::logic_error("It is not allowed to constrain only one rotation");
-          break;
-      } 
-      mask_ = mask;
-      maskComp_ = complementMask(mask);
-    }
-
-    void Handle::maskComp (const std::vector<bool>& mask)
-    {
-      assert(maskComp_.size() == 6);
-      maskComp_ = mask;
-    }
-
-    ImplicitPtr_t Handle::createGrasp
-    (const GripperPtr_t& gripper, std::string n) const
-    {
-      if (n.empty()) {
-        n = gripper->name() + "_grasps_" + name() + "_" + maskToStr (mask_);
-      }
-      // If handle is on a freeflying object, create an explicit constraint
-      if (is6Dmask(mask_) && isHandleOnFreeflyer (*this)) {
-	return constraints::explicit_::RelativePose::create
-	  (n, robot (), gripper->joint (), joint (),
-	   gripper->objectPositionInJoint (), localPosition(),
-           6 * constraints::EqualToZero);
-      }
-      return Implicit::create (RelativeTransformationR3xSO3::create
-         (n, robot (), gripper->joint (), joint (),
-          gripper->objectPositionInJoint (), localPosition()),
-			       6 * constraints::EqualToZero, mask_);
-    }
-
-    ImplicitPtr_t Handle::createGraspComplement
-    (const GripperPtr_t& gripper, std::string n) const
-    {
-      if (n.empty()) {
-        n = gripper->name() + "_grasps_" + name() + "/complement_" +
-          maskToStr (maskComp_);
-      }
-      core::DevicePtr_t r = robot();
-      if (maskSize(maskComp_) == 0) {
-        return Implicit::create (
-            shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
-              r->configSize(), r->numberDof (), n)), ComparisonTypes_t());
-      } else {
-        return  Implicit::create (RelativeTransformationR3xSO3::create
-           (n, r, gripper->joint (), joint (),
-            gripper->objectPositionInJoint (), localPosition()),
-           6 * constraints::Equality, maskComp_);
-      }
-    }
-
-    ImplicitPtr_t Handle::createGraspAndComplement
-    (const GripperPtr_t& gripper, std::string n) const
-    {
-      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) &&
-          maskSize(boolOr(mask_, maskComp_)) == 6) {
-	return constraints::explicit_::RelativePose::create
-	  (n, robot (), gripper->joint (), joint (),
-	   gripper->objectPositionInJoint (), localPosition(), comp);
-      }
-      return Implicit::create (RelativeTransformationR3xSO3::create
-         (n, robot (), gripper->joint (), joint (),
-          gripper->objectPositionInJoint (), localPosition()),
-                               comp, boolOr(mask_, maskComp_));
-    }
-
-    ImplicitPtr_t Handle::createPreGrasp
-    (const GripperPtr_t& gripper, const value_type& shift, std::string n) const
-    {
-      Transform3f M = gripper->objectPositionInJoint ()
-        * Transform3f (I3, vector3_t (shift,0,0));
-      if (n.empty())
-        n = "Pregrasp_ " + maskToStr(mask_) + "_" + name ()
-          + "_" + gripper->name ();
-      ImplicitPtr_t result (Implicit::create
-         (RelativeTransformationR3xSO3::create
-          (n, robot(), gripper->joint (), joint (),
-           M, localPosition()),
-          6 * constraints::EqualToZero, mask_));
-      return result;
-    }
-
-    HandlePtr_t Handle::clone () const
-    {
-      HandlePtr_t other = Handle::create (name (), localPosition (), robot(), joint ());
-      other->mask(mask_);
-      other->mask(maskComp_);
-      other->clearance(clearance_);
-      return other;
-    }
-
-    std::ostream& Handle::print (std::ostream& os) const
-    {
-      os << "name :" << name () << std::endl;
-      os << "local position :" << localPosition () << std::endl;
-      os << "joint :" << joint ()->name () << std::endl;
-      os << "mask :" << maskToStr (mask()) << std::endl;
-      os << "mask complement:" << maskToStr (maskComp_) << std::endl;
-      return os;
-    }
-
-    std::ostream& operator<< (std::ostream& os, const Handle& handle)
-    {
-      return handle.print (os);
-    }
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+using constraints::DifferentiableFunction;
+using constraints::Explicit;
+using constraints::ExplicitPtr_t;
+using constraints::Implicit;
+using constraints::ImplicitPtr_t;
+
+std::string Handle::className("Handle");
+namespace {
+static const matrix3_t I3 = matrix3_t::Identity();
+
+struct ZeroDiffFunc : public DifferentiableFunction {
+  ZeroDiffFunc(size_type sIn, size_type sInD,
+               std::string name = std::string("Empty function"))
+      : DifferentiableFunction(sIn, sInD, LiegroupSpace::empty(), name) {
+    context("Grasp complement");
+  }
+
+  inline void impl_compute(pinocchio::LiegroupElementRef, vectorIn_t) const {}
+  inline void impl_jacobian(matrixOut_t, vectorIn_t) const {}
+};
+}  // namespace
+
+bool isHandleOnFreeflyer(const Handle& handle) {
+  const JointPtr_t& joint = handle.joint();
+  if (joint && !joint->parentJoint() &&
+      joint->jointModel().shortname() ==
+          ::pinocchio::JointModelFreeFlyer::classname()) {
+    return true;
+  }
+  return false;
+}
+
+inline int maskSize(const std::vector<bool>& mask) {
+  std::size_t res(0);
+  for (std::size_t i = 0; i < 6; ++i) {
+    if (mask[i]) ++res;
+  }
+  return (int)res;
+}
+
+inline std::vector<bool> boolOr(std::vector<bool> mask1,
+                                std::vector<bool> mask2) {
+  assert(mask1.size() == 6 && mask2.size() == 6);
+  std::vector<bool> res(mask1.size());
+  for (std::size_t i = 0; i < 6; ++i) {
+    res[i] = mask1[i] || mask2[i];
+  }
+  return res;
+}
+
+inline bool is6Dmask(const std::vector<bool>& mask) {
+  for (std::size_t i = 0; i < 6; ++i)
+    if (!mask[i]) return false;
+  return true;
+}
+
+inline std::vector<bool> complementMask(const std::vector<bool>& mask) {
+  std::vector<bool> m(6);
+  for (std::size_t i = 0; i < 6; ++i) m[i] = !mask[i];
+  return m;
+}
+
+inline std::string maskToStr(const std::vector<bool>& mask) {
+  std::stringstream ss;
+  ss << "(";
+  for (std::size_t i = 0; i < 5; ++i) ss << mask[i] << ",";
+  ss << mask[5] << ")";
+  return ss.str();
+}
+
+void Handle::mask(const std::vector<bool>& mask) {
+  assert(mask.size() == 6);
+  std::size_t nRotFree = 3;
+  for (std::size_t i = 3; i < 6; ++i)
+    if (mask[i]) nRotFree--;
+  switch (nRotFree) {
+    case 1:  // TODO we should check the axis are properly aligned.
+      break;
+    case 2:  // This does not make sense.
+      throw std::logic_error(
+          "It is not allowed to constrain only one rotation");
+      break;
+  }
+  mask_ = mask;
+  maskComp_ = complementMask(mask);
+}
+
+void Handle::maskComp(const std::vector<bool>& mask) {
+  assert(maskComp_.size() == 6);
+  maskComp_ = mask;
+}
+
+ImplicitPtr_t Handle::createGrasp(const GripperPtr_t& gripper,
+                                  std::string n) const {
+  if (n.empty()) {
+    n = gripper->name() + "_grasps_" + name() + "_" + maskToStr(mask_);
+  }
+  // If handle is on a freeflying object, create an explicit constraint
+  if (is6Dmask(mask_) && isHandleOnFreeflyer(*this)) {
+    return constraints::explicit_::RelativePose::create(
+        n, robot(), gripper->joint(), joint(), gripper->objectPositionInJoint(),
+        localPosition(), 6 * constraints::EqualToZero);
+  }
+  return Implicit::create(
+      RelativeTransformationR3xSO3::create(
+          n, robot(), gripper->joint(), joint(),
+          gripper->objectPositionInJoint(), localPosition()),
+      6 * constraints::EqualToZero, mask_);
+}
+
+ImplicitPtr_t Handle::createGraspComplement(const GripperPtr_t& gripper,
+                                            std::string n) const {
+  if (n.empty()) {
+    n = gripper->name() + "_grasps_" + name() + "/complement_" +
+        maskToStr(maskComp_);
+  }
+  core::DevicePtr_t r = robot();
+  if (maskSize(maskComp_) == 0) {
+    return Implicit::create(shared_ptr<ZeroDiffFunc>(new ZeroDiffFunc(
+                                r->configSize(), r->numberDof(), n)),
+                            ComparisonTypes_t());
+  } else {
+    return Implicit::create(
+        RelativeTransformationR3xSO3::create(n, r, gripper->joint(), joint(),
+                                             gripper->objectPositionInJoint(),
+                                             localPosition()),
+        6 * constraints::Equality, maskComp_);
+  }
+}
+
+ImplicitPtr_t Handle::createGraspAndComplement(const GripperPtr_t& gripper,
+                                               std::string n) const {
+  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) && maskSize(boolOr(mask_, maskComp_)) == 6) {
+    return constraints::explicit_::RelativePose::create(
+        n, robot(), gripper->joint(), joint(), gripper->objectPositionInJoint(),
+        localPosition(), comp);
+  }
+  return Implicit::create(
+      RelativeTransformationR3xSO3::create(
+          n, robot(), gripper->joint(), joint(),
+          gripper->objectPositionInJoint(), localPosition()),
+      comp, boolOr(mask_, maskComp_));
+}
+
+ImplicitPtr_t Handle::createPreGrasp(const GripperPtr_t& gripper,
+                                     const value_type& shift,
+                                     std::string n) const {
+  Transform3f M = gripper->objectPositionInJoint() *
+                  Transform3f(I3, vector3_t(shift, 0, 0));
+  if (n.empty())
+    n = "Pregrasp_ " + maskToStr(mask_) + "_" + name() + "_" + gripper->name();
+  ImplicitPtr_t result(Implicit::create(
+      RelativeTransformationR3xSO3::create(n, robot(), gripper->joint(),
+                                           joint(), M, localPosition()),
+      6 * constraints::EqualToZero, mask_));
+  return result;
+}
+
+HandlePtr_t Handle::clone() const {
+  HandlePtr_t other = Handle::create(name(), localPosition(), robot(), joint());
+  other->mask(mask_);
+  other->mask(maskComp_);
+  other->clearance(clearance_);
+  return other;
+}
+
+std::ostream& Handle::print(std::ostream& os) const {
+  os << "name :" << name() << std::endl;
+  os << "local position :" << localPosition() << std::endl;
+  os << "joint :" << joint()->name() << std::endl;
+  os << "mask :" << maskToStr(mask()) << std::endl;
+  os << "mask complement:" << maskToStr(maskComp_) << std::endl;
+  return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const Handle& handle) {
+  return handle.print(os);
+}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/leaf-connected-comp.cc b/src/leaf-connected-comp.cc
index d94d969a5d5da754a180c5bfa1beca5d620e1c1f..ff6ec888fddbe137f1b95ba7da9e94c304d1e6f0 100644
--- a/src/leaf-connected-comp.cc
+++ b/src/leaf-connected-comp.cc
@@ -26,221 +26,208 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
+#include <hpp/manipulation/graph/state.hh>
 #include <hpp/manipulation/leaf-connected-comp.hh>
-
 #include <hpp/manipulation/roadmap.hh>
-#include <hpp/manipulation/graph/state.hh>
 
 namespace hpp {
-  namespace manipulation {
-    void LeafConnectedComp::clean (LeafConnectedComps_t& set)
-    {
-      for (LeafConnectedComps_t::iterator it = set.begin ();
-	   it != set.end (); ++it) {
-	(*it)->explored_ = false;
-      }
-    }
-
-    LeafConnectedCompPtr_t LeafConnectedComp::create (const RoadmapPtr_t& roadmap)
-    {
-      LeafConnectedCompPtr_t shPtr =
-        LeafConnectedCompPtr_t(new LeafConnectedComp(roadmap));
-      shPtr->init(shPtr);
-      return shPtr;
-    }
-
-    void LeafConnectedComp::addNode (const RoadmapNodePtr_t& node)
-    {
-      assert(state_);
-      graph::StatePtr_t state = roadmap_.lock()->getState(node);
-
-      // Sanity check
-      if (state_ == state) // Oops
-        throw std::runtime_error ("RoadmapNode of LeafConnectedComp must be in"
-            " the same state");
-      nodes_.push_back(node);
-    }
-
-    void LeafConnectedComp::setFirstNode (const RoadmapNodePtr_t& node)
-    {
-      assert(!state_);
-      state_ = roadmap_.lock()->getState(node);
-      nodes_.push_back(node);
+namespace manipulation {
+void LeafConnectedComp::clean(LeafConnectedComps_t& set) {
+  for (LeafConnectedComps_t::iterator it = set.begin(); it != set.end(); ++it) {
+    (*it)->explored_ = false;
+  }
+}
+
+LeafConnectedCompPtr_t LeafConnectedComp::create(const RoadmapPtr_t& roadmap) {
+  LeafConnectedCompPtr_t shPtr =
+      LeafConnectedCompPtr_t(new LeafConnectedComp(roadmap));
+  shPtr->init(shPtr);
+  return shPtr;
+}
+
+void LeafConnectedComp::addNode(const RoadmapNodePtr_t& node) {
+  assert(state_);
+  graph::StatePtr_t state = roadmap_.lock()->getState(node);
+
+  // Sanity check
+  if (state_ == state)  // Oops
+    throw std::runtime_error(
+        "RoadmapNode of LeafConnectedComp must be in"
+        " the same state");
+  nodes_.push_back(node);
+}
+
+void LeafConnectedComp::setFirstNode(const RoadmapNodePtr_t& node) {
+  assert(!state_);
+  state_ = roadmap_.lock()->getState(node);
+  nodes_.push_back(node);
+}
+
+bool LeafConnectedComp::canReach(const LeafConnectedCompPtr_t& cc) {
+  // Store visited connected components for further cleaning.
+  LeafConnectedComp::LeafConnectedComps_t explored;
+  std::deque<RawPtr_t> queue;
+  queue.push_back(this);
+  explored_ = true;
+  explored.insert(this);
+  while (!queue.empty()) {
+    RawPtr_t current = queue.front();
+    queue.pop_front();
+    if (current == cc.get()) {
+      clean(explored);
+      return true;
     }
-
-    bool LeafConnectedComp::canReach (const LeafConnectedCompPtr_t& cc)
-    {
-      // Store visited connected components for further cleaning.
-      LeafConnectedComp::LeafConnectedComps_t explored;
-      std::deque <RawPtr_t> queue;
-      queue.push_back (this);
-      explored_ = true;
-      explored.insert (this);
-      while (!queue.empty ()) {
-	RawPtr_t current = queue.front ();
-	queue.pop_front ();
-	if (current == cc.get()) {
-	  clean (explored);
-	  return true;
-	}
-	for (LeafConnectedComp::LeafConnectedComps_t::iterator itChild =
-	       current->to_.begin ();
-	     itChild != current->to_.end (); ++itChild) {
-	  RawPtr_t child = *itChild;
-	  if (!child->explored_) {
-	    child->explored_ = true;
-	    explored.insert (child);
-	    queue.push_back (child);
-	  }
-	}
+    for (LeafConnectedComp::LeafConnectedComps_t::iterator itChild =
+             current->to_.begin();
+         itChild != current->to_.end(); ++itChild) {
+      RawPtr_t child = *itChild;
+      if (!child->explored_) {
+        child->explored_ = true;
+        explored.insert(child);
+        queue.push_back(child);
       }
-      clean (explored);
-      return false;
     }
-
-    bool LeafConnectedComp::canReach
-    (const LeafConnectedCompPtr_t& cc,
-     LeafConnectedComp::LeafConnectedComps_t& ccToThis)
-    {
-      bool reachable = false;
-      // Store visited connected components
-      LeafConnectedComp::LeafConnectedComps_t exploredForward;
-      std::deque <RawPtr_t> queue;
-      queue.push_back (this);
-      explored_ = true;
-      exploredForward.insert (this);
-      while (!queue.empty ()) {
-	RawPtr_t current = queue.front ();
-	queue.pop_front ();
-	if (current == cc.get()) {
-	  reachable = true;
-	  exploredForward.insert (current);
-	} else {
-	  for (LeafConnectedComp::LeafConnectedComps_t::iterator itChild =
-		 current->to_.begin ();
-	       itChild != current->to_.end (); ++itChild) {
-	    RawPtr_t child = *itChild;
-	    if (!child->explored_) {
-	      child->explored_ = true;
-	      exploredForward.insert (child);
-	      queue.push_back (child);
-	    }
-	  }
-	}
+  }
+  clean(explored);
+  return false;
+}
+
+bool LeafConnectedComp::canReach(
+    const LeafConnectedCompPtr_t& cc,
+    LeafConnectedComp::LeafConnectedComps_t& ccToThis) {
+  bool reachable = false;
+  // Store visited connected components
+  LeafConnectedComp::LeafConnectedComps_t exploredForward;
+  std::deque<RawPtr_t> queue;
+  queue.push_back(this);
+  explored_ = true;
+  exploredForward.insert(this);
+  while (!queue.empty()) {
+    RawPtr_t current = queue.front();
+    queue.pop_front();
+    if (current == cc.get()) {
+      reachable = true;
+      exploredForward.insert(current);
+    } else {
+      for (LeafConnectedComp::LeafConnectedComps_t::iterator itChild =
+               current->to_.begin();
+           itChild != current->to_.end(); ++itChild) {
+        RawPtr_t child = *itChild;
+        if (!child->explored_) {
+          child->explored_ = true;
+          exploredForward.insert(child);
+          queue.push_back(child);
+        }
       }
-      // Set visited connected components to unexplored
-      clean (exploredForward);
-      if (!reachable) return false;
-
-      // Store visited connected components
-      LeafConnectedComps_t exploredBackward;
-      queue.push_back (cc.get());
-      cc->explored_ = true;
-      exploredBackward.insert (cc.get());
-      while (!queue.empty ()) {
-	RawPtr_t current = queue.front ();
-	queue.pop_front ();
-	if (current == this) {
-	  exploredBackward.insert (current);
-	} else {
-	  for (LeafConnectedComps_t::iterator itChild =
-		 current->from_.begin ();
-	       itChild != current->from_.end (); ++itChild) {
-	    RawPtr_t child = *itChild;
-	    if (!child->explored_) {
-	      child->explored_ = true;
-	      exploredBackward.insert (child);
-	      queue.push_back (child);
-	    }
-	  }
-	}
-      }
-      // Set visited connected components to unexplored
-      clean (exploredBackward);
-      std::set_intersection (exploredForward.begin (), exploredForward.end (),
-			     exploredBackward.begin (), exploredBackward.end (),
-			     std::inserter (ccToThis, ccToThis.begin ()));
-      return true;
     }
-
-    void LeafConnectedComp::merge (const LeafConnectedCompPtr_t& other)
-    {
-      assert(other);
-      assert(weak_.lock().get() == this);
-
-      // Tell other's nodes that they now belong to this connected component
-      for (RoadmapNodes_t::iterator itNode = other->nodes_.begin ();
-	   itNode != other->nodes_.end (); ++itNode) {
-	(*itNode)->leafConnectedComponent (weak_.lock ());
-      }
-      // Add other's nodes to this list.
-      nodes_.insert (nodes_.end (), other->nodes_.begin(), other->nodes_.end());
-
-      // Tell other's reachableTo's that other has been replaced by this
-      for (LeafConnectedComps_t::iterator itcc = other->to_.begin ();
-	   itcc != other->to_.end (); ++itcc) {
-	(*itcc)->from_.erase (other.get());
-	(*itcc)->from_.insert (this);
-      }
-
-      // Tell other's reachableFrom's that other has been replaced by this
-      for (LeafConnectedComps_t::iterator itcc=other->from_.begin ();
-	   itcc != other->from_.end (); ++itcc) {
-	(*itcc)->to_.erase (other.get());
-	(*itcc)->to_.insert (this);
+  }
+  // Set visited connected components to unexplored
+  clean(exploredForward);
+  if (!reachable) return false;
+
+  // Store visited connected components
+  LeafConnectedComps_t exploredBackward;
+  queue.push_back(cc.get());
+  cc->explored_ = true;
+  exploredBackward.insert(cc.get());
+  while (!queue.empty()) {
+    RawPtr_t current = queue.front();
+    queue.pop_front();
+    if (current == this) {
+      exploredBackward.insert(current);
+    } else {
+      for (LeafConnectedComps_t::iterator itChild = current->from_.begin();
+           itChild != current->from_.end(); ++itChild) {
+        RawPtr_t child = *itChild;
+        if (!child->explored_) {
+          child->explored_ = true;
+          exploredBackward.insert(child);
+          queue.push_back(child);
+        }
       }
-
-      LeafConnectedComps_t tmp;
-      std::set_union (to_.begin (), to_.end (),
-		      other->to_.begin (), other->to_.end (),
-		      std::inserter (tmp, tmp.begin ()));
-      to_ = tmp; tmp.clear ();
-      to_.erase (other.get());
-      to_.erase (this);
-      std::set_union (from_.begin (), from_.end (),
-		      other->from_.begin (),
-		      other->from_.end (),
-		      std::inserter (tmp, tmp.begin()));
-      from_ = tmp; tmp.clear ();
-      from_.erase (other.get());
-      from_.erase (this);
-    }
-
-    WeighedLeafConnectedCompPtr_t WeighedLeafConnectedComp::create (const RoadmapPtr_t& roadmap)
-    {
-      WeighedLeafConnectedCompPtr_t shPtr = WeighedLeafConnectedCompPtr_t
-        (new WeighedLeafConnectedComp(roadmap));
-      shPtr->init(shPtr);
-      return shPtr;
-    }
-
-    void WeighedLeafConnectedComp::merge (const LeafConnectedCompPtr_t& otherCC)
-    {
-      WeighedLeafConnectedCompPtr_t other =
-        HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp, otherCC);
-      value_type r = ((value_type)nodes_.size()) / (value_type)(nodes_.size() + other->nodes_.size());
-
-      LeafConnectedComp::merge(otherCC);
-      weight_ *= other->weight_; // TODO a geometric mean would be more natural.
-      p_ = r * p_ + (1 - r) * other->p_;
-    }
-
-    void WeighedLeafConnectedComp::setFirstNode (const RoadmapNodePtr_t& node)
-    {
-      LeafConnectedComp::setFirstNode(node);
-      std::vector<value_type> p = state_->neighbors().probabilities();
-      p_.resize(p.size());
-      edges_ = state_->neighbors().values();
-      for (std::size_t i = 0; i < p.size(); ++i)
-        p_[i] = p[i];
-    }
-
-    std::size_t WeighedLeafConnectedComp::indexOf (const graph::EdgePtr_t e) const
-    {
-      std::size_t i = 0;
-      for (; i < edges_.size(); ++i)
-        if (edges_[i] == e) break;
-      return i;
     }
-  } //   namespace manipulation
-} // namespace hpp
+  }
+  // Set visited connected components to unexplored
+  clean(exploredBackward);
+  std::set_intersection(exploredForward.begin(), exploredForward.end(),
+                        exploredBackward.begin(), exploredBackward.end(),
+                        std::inserter(ccToThis, ccToThis.begin()));
+  return true;
+}
+
+void LeafConnectedComp::merge(const LeafConnectedCompPtr_t& other) {
+  assert(other);
+  assert(weak_.lock().get() == this);
+
+  // Tell other's nodes that they now belong to this connected component
+  for (RoadmapNodes_t::iterator itNode = other->nodes_.begin();
+       itNode != other->nodes_.end(); ++itNode) {
+    (*itNode)->leafConnectedComponent(weak_.lock());
+  }
+  // Add other's nodes to this list.
+  nodes_.insert(nodes_.end(), other->nodes_.begin(), other->nodes_.end());
+
+  // Tell other's reachableTo's that other has been replaced by this
+  for (LeafConnectedComps_t::iterator itcc = other->to_.begin();
+       itcc != other->to_.end(); ++itcc) {
+    (*itcc)->from_.erase(other.get());
+    (*itcc)->from_.insert(this);
+  }
+
+  // Tell other's reachableFrom's that other has been replaced by this
+  for (LeafConnectedComps_t::iterator itcc = other->from_.begin();
+       itcc != other->from_.end(); ++itcc) {
+    (*itcc)->to_.erase(other.get());
+    (*itcc)->to_.insert(this);
+  }
+
+  LeafConnectedComps_t tmp;
+  std::set_union(to_.begin(), to_.end(), other->to_.begin(), other->to_.end(),
+                 std::inserter(tmp, tmp.begin()));
+  to_ = tmp;
+  tmp.clear();
+  to_.erase(other.get());
+  to_.erase(this);
+  std::set_union(from_.begin(), from_.end(), other->from_.begin(),
+                 other->from_.end(), std::inserter(tmp, tmp.begin()));
+  from_ = tmp;
+  tmp.clear();
+  from_.erase(other.get());
+  from_.erase(this);
+}
+
+WeighedLeafConnectedCompPtr_t WeighedLeafConnectedComp::create(
+    const RoadmapPtr_t& roadmap) {
+  WeighedLeafConnectedCompPtr_t shPtr =
+      WeighedLeafConnectedCompPtr_t(new WeighedLeafConnectedComp(roadmap));
+  shPtr->init(shPtr);
+  return shPtr;
+}
+
+void WeighedLeafConnectedComp::merge(const LeafConnectedCompPtr_t& otherCC) {
+  WeighedLeafConnectedCompPtr_t other =
+      HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp, otherCC);
+  value_type r = ((value_type)nodes_.size()) /
+                 (value_type)(nodes_.size() + other->nodes_.size());
+
+  LeafConnectedComp::merge(otherCC);
+  weight_ *= other->weight_;  // TODO a geometric mean would be more natural.
+  p_ = r * p_ + (1 - r) * other->p_;
+}
+
+void WeighedLeafConnectedComp::setFirstNode(const RoadmapNodePtr_t& node) {
+  LeafConnectedComp::setFirstNode(node);
+  std::vector<value_type> p = state_->neighbors().probabilities();
+  p_.resize(p.size());
+  edges_ = state_->neighbors().values();
+  for (std::size_t i = 0; i < p.size(); ++i) p_[i] = p[i];
+}
+
+std::size_t WeighedLeafConnectedComp::indexOf(const graph::EdgePtr_t e) const {
+  std::size_t i = 0;
+  for (; i < edges_.size(); ++i)
+    if (edges_[i] == e) break;
+  return i;
+}
+}  //   namespace manipulation
+}  // namespace hpp
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index dcc59e3e5cd11678a62d9a47c31f92f1620c7f20..d91a1d2e69901a6a0c9c8f30a4f17a049368dd5a 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -28,484 +28,476 @@
 
 #include "hpp/manipulation/manipulation-planner.hh"
 
-#include <tuple>
-#include <iterator>
-
-#include <hpp/util/pointer.hh>
-#include <hpp/util/timer.hh>
-#include <hpp/util/assertion.hh>
-
-#include <hpp/pinocchio/configuration.hh>
-
-#include <hpp/core/path-validation.hh>
+#include <hpp/core/configuration-shooter.hh>
 #include <hpp/core/connected-component.hh>
+#include <hpp/core/nearest-neighbor.hh>
 #include <hpp/core/path-projector.hh>
+#include <hpp/core/path-validation.hh>
 #include <hpp/core/projection-error.hh>
-#include <hpp/core/nearest-neighbor.hh>
 #include <hpp/core/roadmap.hh>
-#include <hpp/core/configuration-shooter.hh>
+#include <hpp/pinocchio/configuration.hh>
+#include <hpp/util/assertion.hh>
+#include <hpp/util/pointer.hh>
+#include <hpp/util/timer.hh>
+#include <iterator>
+#include <tuple>
 
-#include "hpp/manipulation/graph/statistics.hh"
-#include "hpp/manipulation/device.hh"
 #include "hpp/manipulation/connected-component.hh"
-#include "hpp/manipulation/problem.hh"
-#include "hpp/manipulation/roadmap.hh"
-#include "hpp/manipulation/roadmap-node.hh"
+#include "hpp/manipulation/device.hh"
 #include "hpp/manipulation/graph-path-validation.hh"
 #include "hpp/manipulation/graph/edge.hh"
 #include "hpp/manipulation/graph/state-selector.hh"
+#include "hpp/manipulation/graph/statistics.hh"
+#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/roadmap-node.hh"
+#include "hpp/manipulation/roadmap.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace {
-      HPP_DEFINE_TIMECOUNTER(oneStep);
-      HPP_DEFINE_TIMECOUNTER(extend);
-      HPP_DEFINE_TIMECOUNTER(tryConnect);
-      HPP_DEFINE_TIMECOUNTER(nearestNeighbor);
-      HPP_DEFINE_TIMECOUNTER(delayedEdges);
-      HPP_DEFINE_TIMECOUNTER(tryConnectNewNodes);
-      HPP_DEFINE_TIMECOUNTER(tryConnectToRoadmap);
-      /// extend steps
-      HPP_DEFINE_TIMECOUNTER(chooseEdge);
-      HPP_DEFINE_TIMECOUNTER(generateTargetConfig);
-      HPP_DEFINE_TIMECOUNTER(buildPath);
-      HPP_DEFINE_TIMECOUNTER(projectPath);
-      HPP_DEFINE_TIMECOUNTER(validatePath);
-
-      graph::StatePtr_t getState (const graph::GraphPtr_t graph, const core::NodePtr_t& node)
-      {
-        RoadmapNodePtr_t mnode (dynamic_cast<RoadmapNode*>(node));
-        if (mnode != NULL) return mnode->graphState();
-        else return graph->getState (*node->configuration());
-      }
-
-      core::PathPtr_t connect (
-          const Configuration_t& q1, const Configuration_t& q2,
-          const graph::StatePtr_t& s1, const graph::StatePtr_t& s2,
-          const graph::GraphPtr_t& graph,
-          const PathProjectorPtr_t& pathProjector,
-          const PathValidationPtr_t& pathValidation)
-      {
-        assert (graph && s1 && s2);
-        graph::Edges_t possibleEdges = graph->getEdges (s1, s2);
-
-        core::PathPtr_t path, tmpPath;
-
-        graph::EdgePtr_t edge;
-        for (std::size_t i = 0; i < possibleEdges.size(); ++i) {
-          edge = possibleEdges[i];
-          if (edge->build (path, q1, q2)) break;
-        }
-        if (!path) return path;
-        if (pathProjector) {
-          if (!pathProjector->apply (path, tmpPath))
-            return core::PathPtr_t();
-          path = tmpPath;
-        }
-
-        PathValidationReportPtr_t report;
-        if (pathValidation->validate (path, false, tmpPath, report))
-          return path;
-        return core::PathPtr_t();
-      }
-    }
-
-    const std::vector<ManipulationPlanner::Reason>
-      ManipulationPlanner::reasons_ = {
-        SuccessBin::createReason("--Path could not be fully projected"),        // PATH_PROJECTION_SHORTER = 0, 
-        SuccessBin::createReason("--Path could not be fully validated"),        // PATH_VALIDATION_SHORTER = 1, 
-        SuccessBin::createReason("--Reached destination node"),                 // REACHED_DESTINATION_NODE = 2,
-        SuccessBin::createReason("Failure"),                                    // FAILURE = 3,                 
-        SuccessBin::createReason("--Projection of configuration on edge leaf"), // PROJECTION = 4,              
-        SuccessBin::createReason("--SteeringMethod"),                           // STEERING_METHOD = 5,         
-        SuccessBin::createReason("--Path validation returned length 0"),        // PATH_VALIDATION_ZERO = 6,    
-        SuccessBin::createReason("--Path could not be projected at all"),       // PATH_PROJECTION_ZERO = 7     
-      };
-
-    ManipulationPlannerPtr_t ManipulationPlanner::create
-    (const core::ProblemConstPtr_t& problem,
-     const core::RoadmapPtr_t& roadmap)
-    {
-      ManipulationPlanner* ptr;
-      core::RoadmapPtr_t r2 = roadmap;
-      ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST (const Problem, problem);
-      RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
-      if (!r)
-	throw std::invalid_argument
-	  ("The roadmap must be of type hpp::manipulation::Roadmap.");
-      if (!p)
-        throw std::invalid_argument
-	  ("The problem must be of type hpp::manipulation::Problem.");
-
-      ptr = new ManipulationPlanner (p, r);
-      ManipulationPlannerPtr_t shPtr (ptr);
-      ptr->init (shPtr);
-      return shPtr;
-    }
-
-    ManipulationPlanner::ErrorFreqs_t ManipulationPlanner::getEdgeStat
-      (const graph::EdgePtr_t& edge) const
-    {
-      const std::size_t& id = edge->id ();
-      ErrorFreqs_t ret;
-      if (indexPerEdgeStatistics_.size() <= id ||
-         indexPerEdgeStatistics_[id] < 0) {
-        for (std::size_t i = 0; i < reasons_.size(); ++i) ret.push_back (0);
-      } else {
-        const SuccessStatistics& ss =
-          perEdgeStatistics_[indexPerEdgeStatistics_[id]];
-        ret.push_back (ss.nbSuccess ());
-        for (std::size_t i = 0; i < reasons_.size(); ++i)
-          ret.push_back (ss.nbFailure (reasons_[i]));
-      }
-      return ret;
-    }
-
-    StringList_t ManipulationPlanner::errorList ()
-    {
-      StringList_t ret;
-      ret.push_back ("Success");
-      for (std::size_t i = 0; i < reasons_.size(); ++i) ret.push_back (reasons_[i].what);
-      return ret;
-    }
-
-    void ManipulationPlanner::oneStep ()
-    {
-      HPP_START_TIMECOUNTER(oneStep);
-
-      DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem()->robot ());
-      HPP_ASSERT(robot);
-      const graph::States_t& graphStates = problem_->constraintGraph ()
-        ->stateSelector ()->getStates ();
-      graph::States_t::const_iterator itState;
-      core::Nodes_t newNodes;
-      core::PathPtr_t path;
-
-      typedef std::tuple <core::NodePtr_t, ConfigurationPtr_t, core::PathPtr_t>
-	DelayedEdge_t;
-      typedef std::vector <DelayedEdge_t> DelayedEdges_t;
-      DelayedEdges_t delayedEdges;
-
-      // Pick a random node
-      ConfigurationPtr_t q_rand = shooter_->shoot();
-
-      // Extend each connected component
-      for (core::ConnectedComponents_t::const_iterator itcc =
-          roadmap ()->connectedComponents ().begin ();
-          itcc != roadmap ()->connectedComponents ().end (); ++itcc) {
-        // Find the nearest neighbor.
-        core::value_type distance;
-        for (itState = graphStates.begin (); itState != graphStates.end (); ++itState) {
-          HPP_START_TIMECOUNTER(nearestNeighbor);
-          RoadmapNodePtr_t near = roadmap_->nearestNodeInState (q_rand, HPP_STATIC_PTR_CAST(ConnectedComponent,*itcc), *itState, distance);
-          HPP_STOP_TIMECOUNTER(nearestNeighbor);
-          HPP_DISPLAY_LAST_TIMECOUNTER(nearestNeighbor);
-          if (!near) continue;
-
-          HPP_START_TIMECOUNTER(extend);
-          bool pathIsValid = extend (near, q_rand, path);
-          HPP_STOP_TIMECOUNTER(extend);
-          HPP_DISPLAY_LAST_TIMECOUNTER(extend);
-          // Insert new path to q_near in roadmap
-          if (pathIsValid) {
-            value_type t_final = path->timeRange ().second;
-            if (t_final != path->timeRange ().first) {
-	      bool success;
-              ConfigurationPtr_t q_new (new Configuration_t
-					(path->eval(t_final, success)));
-	      assert(success);
-	      assert(!path->constraints() ||
-		     path->constraints()->isSatisfied(*q_new));
-	      assert(problem_->constraintGraph ()->getState(*q_new));
-              delayedEdges.push_back (DelayedEdge_t (near, q_new, path));
-            }
-          }
+namespace manipulation {
+namespace {
+HPP_DEFINE_TIMECOUNTER(oneStep);
+HPP_DEFINE_TIMECOUNTER(extend);
+HPP_DEFINE_TIMECOUNTER(tryConnect);
+HPP_DEFINE_TIMECOUNTER(nearestNeighbor);
+HPP_DEFINE_TIMECOUNTER(delayedEdges);
+HPP_DEFINE_TIMECOUNTER(tryConnectNewNodes);
+HPP_DEFINE_TIMECOUNTER(tryConnectToRoadmap);
+/// extend steps
+HPP_DEFINE_TIMECOUNTER(chooseEdge);
+HPP_DEFINE_TIMECOUNTER(generateTargetConfig);
+HPP_DEFINE_TIMECOUNTER(buildPath);
+HPP_DEFINE_TIMECOUNTER(projectPath);
+HPP_DEFINE_TIMECOUNTER(validatePath);
+
+graph::StatePtr_t getState(const graph::GraphPtr_t graph,
+                           const core::NodePtr_t& node) {
+  RoadmapNodePtr_t mnode(dynamic_cast<RoadmapNode*>(node));
+  if (mnode != NULL)
+    return mnode->graphState();
+  else
+    return graph->getState(*node->configuration());
+}
+
+core::PathPtr_t connect(const Configuration_t& q1, const Configuration_t& q2,
+                        const graph::StatePtr_t& s1,
+                        const graph::StatePtr_t& s2,
+                        const graph::GraphPtr_t& graph,
+                        const PathProjectorPtr_t& pathProjector,
+                        const PathValidationPtr_t& pathValidation) {
+  assert(graph && s1 && s2);
+  graph::Edges_t possibleEdges = graph->getEdges(s1, s2);
+
+  core::PathPtr_t path, tmpPath;
+
+  graph::EdgePtr_t edge;
+  for (std::size_t i = 0; i < possibleEdges.size(); ++i) {
+    edge = possibleEdges[i];
+    if (edge->build(path, q1, q2)) break;
+  }
+  if (!path) return path;
+  if (pathProjector) {
+    if (!pathProjector->apply(path, tmpPath)) return core::PathPtr_t();
+    path = tmpPath;
+  }
+
+  PathValidationReportPtr_t report;
+  if (pathValidation->validate(path, false, tmpPath, report)) return path;
+  return core::PathPtr_t();
+}
+}  // namespace
+
+const std::vector<ManipulationPlanner::Reason> ManipulationPlanner::reasons_ = {
+    SuccessBin::createReason(
+        "--Path could not be fully projected"),  // PATH_PROJECTION_SHORTER = 0,
+    SuccessBin::createReason(
+        "--Path could not be fully validated"),  // PATH_VALIDATION_SHORTER = 1,
+    SuccessBin::createReason(
+        "--Reached destination node"),    // REACHED_DESTINATION_NODE = 2,
+    SuccessBin::createReason("Failure"),  // FAILURE = 3,
+    SuccessBin::createReason(
+        "--Projection of configuration on edge leaf"),  // PROJECTION = 4,
+    SuccessBin::createReason("--SteeringMethod"),       // STEERING_METHOD = 5,
+    SuccessBin::createReason(
+        "--Path validation returned length 0"),  // PATH_VALIDATION_ZERO = 6,
+    SuccessBin::createReason(
+        "--Path could not be projected at all"),  // PATH_PROJECTION_ZERO = 7
+};
+
+ManipulationPlannerPtr_t ManipulationPlanner::create(
+    const core::ProblemConstPtr_t& problem, const core::RoadmapPtr_t& roadmap) {
+  ManipulationPlanner* ptr;
+  core::RoadmapPtr_t r2 = roadmap;
+  ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST(const Problem, problem);
+  RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST(Roadmap, r2);
+  if (!r)
+    throw std::invalid_argument(
+        "The roadmap must be of type hpp::manipulation::Roadmap.");
+  if (!p)
+    throw std::invalid_argument(
+        "The problem must be of type hpp::manipulation::Problem.");
+
+  ptr = new ManipulationPlanner(p, r);
+  ManipulationPlannerPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+ManipulationPlanner::ErrorFreqs_t ManipulationPlanner::getEdgeStat(
+    const graph::EdgePtr_t& edge) const {
+  const std::size_t& id = edge->id();
+  ErrorFreqs_t ret;
+  if (indexPerEdgeStatistics_.size() <= id || indexPerEdgeStatistics_[id] < 0) {
+    for (std::size_t i = 0; i < reasons_.size(); ++i) ret.push_back(0);
+  } else {
+    const SuccessStatistics& ss =
+        perEdgeStatistics_[indexPerEdgeStatistics_[id]];
+    ret.push_back(ss.nbSuccess());
+    for (std::size_t i = 0; i < reasons_.size(); ++i)
+      ret.push_back(ss.nbFailure(reasons_[i]));
+  }
+  return ret;
+}
+
+StringList_t ManipulationPlanner::errorList() {
+  StringList_t ret;
+  ret.push_back("Success");
+  for (std::size_t i = 0; i < reasons_.size(); ++i)
+    ret.push_back(reasons_[i].what);
+  return ret;
+}
+
+void ManipulationPlanner::oneStep() {
+  HPP_START_TIMECOUNTER(oneStep);
+
+  DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem()->robot());
+  HPP_ASSERT(robot);
+  const graph::States_t& graphStates =
+      problem_->constraintGraph()->stateSelector()->getStates();
+  graph::States_t::const_iterator itState;
+  core::Nodes_t newNodes;
+  core::PathPtr_t path;
+
+  typedef std::tuple<core::NodePtr_t, ConfigurationPtr_t, core::PathPtr_t>
+      DelayedEdge_t;
+  typedef std::vector<DelayedEdge_t> DelayedEdges_t;
+  DelayedEdges_t delayedEdges;
+
+  // Pick a random node
+  ConfigurationPtr_t q_rand = shooter_->shoot();
+
+  // Extend each connected component
+  for (core::ConnectedComponents_t::const_iterator itcc =
+           roadmap()->connectedComponents().begin();
+       itcc != roadmap()->connectedComponents().end(); ++itcc) {
+    // Find the nearest neighbor.
+    core::value_type distance;
+    for (itState = graphStates.begin(); itState != graphStates.end();
+         ++itState) {
+      HPP_START_TIMECOUNTER(nearestNeighbor);
+      RoadmapNodePtr_t near = roadmap_->nearestNodeInState(
+          q_rand, HPP_STATIC_PTR_CAST(ConnectedComponent, *itcc), *itState,
+          distance);
+      HPP_STOP_TIMECOUNTER(nearestNeighbor);
+      HPP_DISPLAY_LAST_TIMECOUNTER(nearestNeighbor);
+      if (!near) continue;
+
+      HPP_START_TIMECOUNTER(extend);
+      bool pathIsValid = extend(near, q_rand, path);
+      HPP_STOP_TIMECOUNTER(extend);
+      HPP_DISPLAY_LAST_TIMECOUNTER(extend);
+      // Insert new path to q_near in roadmap
+      if (pathIsValid) {
+        value_type t_final = path->timeRange().second;
+        if (t_final != path->timeRange().first) {
+          bool success;
+          ConfigurationPtr_t q_new(
+              new Configuration_t(path->eval(t_final, success)));
+          assert(success);
+          assert(!path->constraints() ||
+                 path->constraints()->isSatisfied(*q_new));
+          assert(problem_->constraintGraph()->getState(*q_new));
+          delayedEdges.push_back(DelayedEdge_t(near, q_new, path));
         }
       }
-
-      HPP_START_TIMECOUNTER(delayedEdges);
-      // Insert delayed edges
-      for (const auto& edge : delayedEdges) {
-	const core::NodePtr_t& near = std::get<0>(edge);
-	const ConfigurationPtr_t& q_new = std::get<1>(edge);
-	const core::PathPtr_t& validPath = std::get<2>(edge);
-        core::NodePtr_t newNode = roadmap ()->addNode (q_new);
-	roadmap ()->addEdge (near, newNode, validPath);
-	roadmap ()->addEdge (newNode, near, validPath->reverse());
-        newNodes.push_back (newNode);
-      }
-      HPP_STOP_TIMECOUNTER(delayedEdges);
-
-      // Try to connect the new nodes together
-      HPP_START_TIMECOUNTER(tryConnectNewNodes);
-      const std::size_t nbConn = tryConnectNewNodes (newNodes);
-      HPP_STOP_TIMECOUNTER(tryConnectNewNodes);
-      HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectNewNodes);
-      if (nbConn == 0) {
-        HPP_START_TIMECOUNTER(tryConnectToRoadmap);
-        tryConnectToRoadmap (newNodes);
-        HPP_STOP_TIMECOUNTER(tryConnectToRoadmap);
-        HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectToRoadmap);
-      }
-      HPP_STOP_TIMECOUNTER(oneStep);
-      HPP_DISPLAY_LAST_TIMECOUNTER(oneStep);
-      HPP_DISPLAY_TIMECOUNTER(oneStep);
-      HPP_DISPLAY_TIMECOUNTER(extend);
-      HPP_DISPLAY_TIMECOUNTER(tryConnect);
-      HPP_DISPLAY_TIMECOUNTER(tryConnectNewNodes);
-      HPP_DISPLAY_TIMECOUNTER(tryConnectToRoadmap);
-      HPP_DISPLAY_TIMECOUNTER(nearestNeighbor);
-      HPP_DISPLAY_TIMECOUNTER(delayedEdges);
-      HPP_DISPLAY_TIMECOUNTER(chooseEdge);
-      HPP_DISPLAY_TIMECOUNTER(generateTargetConfig);
-      HPP_DISPLAY_TIMECOUNTER(buildPath);
-      HPP_DISPLAY_TIMECOUNTER(projectPath);
-      HPP_DISPLAY_TIMECOUNTER(validatePath);
     }
-
-    bool ManipulationPlanner::extend(
-        RoadmapNodePtr_t n_near,
-        const ConfigurationPtr_t& q_rand,
-        core::PathPtr_t& validPath)
-    {
-      graph::GraphPtr_t graph = problem_->constraintGraph ();
-      PathProjectorPtr_t pathProjector = problem_->pathProjector ();
-      pinocchio::DevicePtr_t robot (problem_->robot ());
-      value_type eps (graph->errorThreshold ());
-      // Select next node in the constraint graph.
-      const ConfigurationPtr_t q_near = n_near->configuration ();
-      HPP_START_TIMECOUNTER (chooseEdge);
-      graph::EdgePtr_t edge = graph->chooseEdge (n_near);
-      HPP_STOP_TIMECOUNTER (chooseEdge);
-      if (!edge) {
-        return false;
-      }
-      qProj_ = *q_rand;
-      HPP_START_TIMECOUNTER (generateTargetConfig);
-      SuccessStatistics& es = edgeStat (edge);
-      if (!edge->generateTargetConfig(n_near, qProj_)) {
-        HPP_STOP_TIMECOUNTER (generateTargetConfig);
-        es.addFailure (reasons_[FAILURE]);
-        es.addFailure (reasons_[PROJECTION]);
-        return false;
-      }
-      if (pinocchio::isApprox (robot, qProj_, *q_near, eps)) {
-        es.addFailure (reasons_[FAILURE]);
-	es.addFailure (reasons_[PATH_PROJECTION_ZERO]);
-	return false;
-      }
-      HPP_STOP_TIMECOUNTER (generateTargetConfig);
-      core::PathPtr_t path;
-      HPP_START_TIMECOUNTER (buildPath);
-      if (!edge->build (path, *q_near, qProj_)) {
-        HPP_STOP_TIMECOUNTER (buildPath);
-        es.addFailure (reasons_[FAILURE]);
-        es.addFailure (reasons_[STEERING_METHOD]);
+  }
+
+  HPP_START_TIMECOUNTER(delayedEdges);
+  // Insert delayed edges
+  for (const auto& edge : delayedEdges) {
+    const core::NodePtr_t& near = std::get<0>(edge);
+    const ConfigurationPtr_t& q_new = std::get<1>(edge);
+    const core::PathPtr_t& validPath = std::get<2>(edge);
+    core::NodePtr_t newNode = roadmap()->addNode(q_new);
+    roadmap()->addEdge(near, newNode, validPath);
+    roadmap()->addEdge(newNode, near, validPath->reverse());
+    newNodes.push_back(newNode);
+  }
+  HPP_STOP_TIMECOUNTER(delayedEdges);
+
+  // Try to connect the new nodes together
+  HPP_START_TIMECOUNTER(tryConnectNewNodes);
+  const std::size_t nbConn = tryConnectNewNodes(newNodes);
+  HPP_STOP_TIMECOUNTER(tryConnectNewNodes);
+  HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectNewNodes);
+  if (nbConn == 0) {
+    HPP_START_TIMECOUNTER(tryConnectToRoadmap);
+    tryConnectToRoadmap(newNodes);
+    HPP_STOP_TIMECOUNTER(tryConnectToRoadmap);
+    HPP_DISPLAY_LAST_TIMECOUNTER(tryConnectToRoadmap);
+  }
+  HPP_STOP_TIMECOUNTER(oneStep);
+  HPP_DISPLAY_LAST_TIMECOUNTER(oneStep);
+  HPP_DISPLAY_TIMECOUNTER(oneStep);
+  HPP_DISPLAY_TIMECOUNTER(extend);
+  HPP_DISPLAY_TIMECOUNTER(tryConnect);
+  HPP_DISPLAY_TIMECOUNTER(tryConnectNewNodes);
+  HPP_DISPLAY_TIMECOUNTER(tryConnectToRoadmap);
+  HPP_DISPLAY_TIMECOUNTER(nearestNeighbor);
+  HPP_DISPLAY_TIMECOUNTER(delayedEdges);
+  HPP_DISPLAY_TIMECOUNTER(chooseEdge);
+  HPP_DISPLAY_TIMECOUNTER(generateTargetConfig);
+  HPP_DISPLAY_TIMECOUNTER(buildPath);
+  HPP_DISPLAY_TIMECOUNTER(projectPath);
+  HPP_DISPLAY_TIMECOUNTER(validatePath);
+}
+
+bool ManipulationPlanner::extend(RoadmapNodePtr_t n_near,
+                                 const ConfigurationPtr_t& q_rand,
+                                 core::PathPtr_t& validPath) {
+  graph::GraphPtr_t graph = problem_->constraintGraph();
+  PathProjectorPtr_t pathProjector = problem_->pathProjector();
+  pinocchio::DevicePtr_t robot(problem_->robot());
+  value_type eps(graph->errorThreshold());
+  // Select next node in the constraint graph.
+  const ConfigurationPtr_t q_near = n_near->configuration();
+  HPP_START_TIMECOUNTER(chooseEdge);
+  graph::EdgePtr_t edge = graph->chooseEdge(n_near);
+  HPP_STOP_TIMECOUNTER(chooseEdge);
+  if (!edge) {
+    return false;
+  }
+  qProj_ = *q_rand;
+  HPP_START_TIMECOUNTER(generateTargetConfig);
+  SuccessStatistics& es = edgeStat(edge);
+  if (!edge->generateTargetConfig(n_near, qProj_)) {
+    HPP_STOP_TIMECOUNTER(generateTargetConfig);
+    es.addFailure(reasons_[FAILURE]);
+    es.addFailure(reasons_[PROJECTION]);
+    return false;
+  }
+  if (pinocchio::isApprox(robot, qProj_, *q_near, eps)) {
+    es.addFailure(reasons_[FAILURE]);
+    es.addFailure(reasons_[PATH_PROJECTION_ZERO]);
+    return false;
+  }
+  HPP_STOP_TIMECOUNTER(generateTargetConfig);
+  core::PathPtr_t path;
+  HPP_START_TIMECOUNTER(buildPath);
+  if (!edge->build(path, *q_near, qProj_)) {
+    HPP_STOP_TIMECOUNTER(buildPath);
+    es.addFailure(reasons_[FAILURE]);
+    es.addFailure(reasons_[STEERING_METHOD]);
+    return false;
+  }
+  HPP_STOP_TIMECOUNTER(buildPath);
+  core::PathPtr_t projPath;
+  bool projShorter = false;
+  if (pathProjector) {
+    HPP_START_TIMECOUNTER(projectPath);
+    projShorter = !pathProjector->apply(path, projPath);
+    if (projShorter) {
+      if (!projPath || projPath->length() == 0) {
+        hppDout(info, "");
+        HPP_STOP_TIMECOUNTER(projectPath);
+        es.addFailure(reasons_[FAILURE]);
+        es.addFailure(reasons_[PATH_PROJECTION_ZERO]);
         return false;
       }
-      HPP_STOP_TIMECOUNTER (buildPath);
-      core::PathPtr_t projPath;
-      bool projShorter = false;
-      if (pathProjector) {
-        HPP_START_TIMECOUNTER (projectPath);
-        projShorter = !pathProjector->apply (path, projPath);
-        if (projShorter) {
-          if (!projPath || projPath->length () == 0) {
-	    hppDout(info, "");
-            HPP_STOP_TIMECOUNTER (projectPath);
-	    es.addFailure (reasons_[FAILURE]);
-            es.addFailure (reasons_[PATH_PROJECTION_ZERO]);
-            return false;
-          }
-        }
-        HPP_STOP_TIMECOUNTER (projectPath);
-      } else projPath = path;
-      PathValidationPtr_t pathValidation (problem_->pathValidation ());
-      PathValidationReportPtr_t report;
-      core::PathPtr_t fullValidPath;
-      HPP_START_TIMECOUNTER (validatePath);
-      bool fullyValid = false;
+    }
+    HPP_STOP_TIMECOUNTER(projectPath);
+  } else
+    projPath = path;
+  PathValidationPtr_t pathValidation(problem_->pathValidation());
+  PathValidationReportPtr_t report;
+  core::PathPtr_t fullValidPath;
+  HPP_START_TIMECOUNTER(validatePath);
+  bool fullyValid = false;
+  try {
+    fullyValid =
+        pathValidation->validate(projPath, false, fullValidPath, report);
+  } catch (const core::projection_error& e) {
+    hppDout(error, e.what());
+    es.addFailure(reasons_[FAILURE]);
+    es.addFailure(reasons_[PATH_VALIDATION_ZERO]);
+    return false;
+  }
+  HPP_STOP_TIMECOUNTER(validatePath);
+  if (fullValidPath->length() == 0) {
+    es.addFailure(reasons_[FAILURE]);
+    es.addFailure(reasons_[PATH_VALIDATION_ZERO]);
+    validPath = fullValidPath;
+    return false;
+  } else {
+    if (extendStep_ == 1 || fullyValid) {
+      validPath = fullValidPath;
+    } else {
+      const value_type& length = fullValidPath->length();
+      const value_type& t_init = fullValidPath->timeRange().first;
       try {
-        fullyValid = pathValidation->validate
-          (projPath, false, fullValidPath, report);
+        validPath = fullValidPath->extract(
+            core::interval_t(t_init, t_init + length * extendStep_));
       } catch (const core::projection_error& e) {
-        hppDout (error, e.what ());
-        es.addFailure (reasons_[FAILURE]);
-        es.addFailure (reasons_[PATH_VALIDATION_ZERO]);
-        return false;
-      }
-      HPP_STOP_TIMECOUNTER (validatePath);
-      if (fullValidPath->length () == 0) {
-        es.addFailure (reasons_[FAILURE]);
-        es.addFailure (reasons_[PATH_VALIDATION_ZERO]);
-        validPath = fullValidPath;
+        hppDout(error, e.what());
+        es.addSuccess();
+        es.addFailure(reasons_[PATH_PROJECTION_SHORTER]);
         return false;
-      } else {
-        if (extendStep_ == 1 || fullyValid) {
-          validPath = fullValidPath;
-        } else {
-          const value_type& length = fullValidPath->length();
-          const value_type& t_init = fullValidPath->timeRange ().first;
-          try {
-            validPath = fullValidPath->extract
-              (core::interval_t(t_init, t_init + length * extendStep_));
-          } catch (const core::projection_error& e) {
-            hppDout (error, e.what());
-	    es.addSuccess ();
-            es.addFailure (reasons_[PATH_PROJECTION_SHORTER]);
-            return false;
-          }
-        }
-        hppDout (info, "Extension:" << std::endl
-            << es);
-      }
-      if (!projShorter && fullyValid) {
-	es.addSuccess ();
-	es.addFailure (reasons_ [REACHED_DESTINATION_NODE]);
-      }
-      else {
-	es.addSuccess ();
-	if (projShorter) {
-	  es.addFailure (reasons_ [PATH_PROJECTION_SHORTER]);
-	} else {
-	  es.addFailure (reasons_ [PATH_VALIDATION_SHORTER]);
-	}
       }
-      return true;
     }
-
-    ManipulationPlanner::SuccessStatistics& ManipulationPlanner::edgeStat
-      (const graph::EdgePtr_t& edge)
-    {
-      const std::size_t& id = edge->id ();
-      if (indexPerEdgeStatistics_.size () <= id) {
-        indexPerEdgeStatistics_.resize (id + 1, -1);
-      }
-      if (indexPerEdgeStatistics_[id] < 0) {
-        indexPerEdgeStatistics_[id] = (int) perEdgeStatistics_.size();
-        perEdgeStatistics_.push_back (SuccessStatistics (edge->name (), 2));
-      }
-      return perEdgeStatistics_[indexPerEdgeStatistics_[id]];
+    hppDout(info, "Extension:" << std::endl << es);
+  }
+  if (!projShorter && fullyValid) {
+    es.addSuccess();
+    es.addFailure(reasons_[REACHED_DESTINATION_NODE]);
+  } else {
+    es.addSuccess();
+    if (projShorter) {
+      es.addFailure(reasons_[PATH_PROJECTION_SHORTER]);
+    } else {
+      es.addFailure(reasons_[PATH_VALIDATION_SHORTER]);
     }
-
-    inline std::size_t ManipulationPlanner::tryConnectToRoadmap (const core::Nodes_t nodes)
-    {
-      PathProjectorPtr_t pathProjector (problem()->pathProjector ());
-      core::PathPtr_t path;
-      graph::GraphPtr_t graph = problem_->constraintGraph ();
-      graph::Edges_t possibleEdges;
-
-      bool connectSucceed = false;
-      std::size_t nbConnection = 0;
-      const std::size_t K = 7;
-      value_type distance;
-      for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
-          itn1 != nodes.end (); ++itn1) {
-        const Configuration_t& q1 (*(*itn1)->configuration ());
-        graph::StatePtr_t s1 = getState (graph, *itn1);
-        connectSucceed = false;
-        for (core::ConnectedComponents_t::const_iterator itcc =
-            roadmap ()->connectedComponents ().begin ();
-            itcc != roadmap ()->connectedComponents ().end (); ++itcc) {
-          if (*itcc == (*itn1)->connectedComponent ())
-            continue;
-          core::Nodes_t knearest = roadmap()->nearestNeighbor ()
-            ->KnearestSearch (q1, *itcc, K, distance);
-          for (core::Nodes_t::const_iterator itn2 = knearest.begin ();
-              itn2 != knearest.end (); ++itn2) {
-            bool _1to2 = (*itn1)->isOutNeighbor (*itn2);
-            bool _2to1 = (*itn1)->isInNeighbor (*itn2);
-            assert (!_1to2 || !_2to1);
-
-            const Configuration_t& q2 (*(*itn2)->configuration ());
-            graph::StatePtr_t s2 = getState (graph, *itn2);
-            assert (q1 != q2);
-
-            path = connect (q1, q2, s1, s2, graph, pathProjector,
-			    problem_->pathValidation());
-
-            if (path) {
-              nbConnection++;
-              if (!_1to2) roadmap ()->addEdge (*itn1, *itn2, path);
-              if (!_2to1) {
-                core::interval_t timeRange = path->timeRange ();
-                roadmap ()->addEdge (*itn2, *itn1, path->extract
-                    (core::interval_t (timeRange.second,
-                                       timeRange.first)));
-              }
-              connectSucceed = true;
-              break;
-            }
+  }
+  return true;
+}
+
+ManipulationPlanner::SuccessStatistics& ManipulationPlanner::edgeStat(
+    const graph::EdgePtr_t& edge) {
+  const std::size_t& id = edge->id();
+  if (indexPerEdgeStatistics_.size() <= id) {
+    indexPerEdgeStatistics_.resize(id + 1, -1);
+  }
+  if (indexPerEdgeStatistics_[id] < 0) {
+    indexPerEdgeStatistics_[id] = (int)perEdgeStatistics_.size();
+    perEdgeStatistics_.push_back(SuccessStatistics(edge->name(), 2));
+  }
+  return perEdgeStatistics_[indexPerEdgeStatistics_[id]];
+}
+
+inline std::size_t ManipulationPlanner::tryConnectToRoadmap(
+    const core::Nodes_t nodes) {
+  PathProjectorPtr_t pathProjector(problem()->pathProjector());
+  core::PathPtr_t path;
+  graph::GraphPtr_t graph = problem_->constraintGraph();
+  graph::Edges_t possibleEdges;
+
+  bool connectSucceed = false;
+  std::size_t nbConnection = 0;
+  const std::size_t K = 7;
+  value_type distance;
+  for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
+       ++itn1) {
+    const Configuration_t& q1(*(*itn1)->configuration());
+    graph::StatePtr_t s1 = getState(graph, *itn1);
+    connectSucceed = false;
+    for (core::ConnectedComponents_t::const_iterator itcc =
+             roadmap()->connectedComponents().begin();
+         itcc != roadmap()->connectedComponents().end(); ++itcc) {
+      if (*itcc == (*itn1)->connectedComponent()) continue;
+      core::Nodes_t knearest =
+          roadmap()->nearestNeighbor()->KnearestSearch(q1, *itcc, K, distance);
+      for (core::Nodes_t::const_iterator itn2 = knearest.begin();
+           itn2 != knearest.end(); ++itn2) {
+        bool _1to2 = (*itn1)->isOutNeighbor(*itn2);
+        bool _2to1 = (*itn1)->isInNeighbor(*itn2);
+        assert(!_1to2 || !_2to1);
+
+        const Configuration_t& q2(*(*itn2)->configuration());
+        graph::StatePtr_t s2 = getState(graph, *itn2);
+        assert(q1 != q2);
+
+        path = connect(q1, q2, s1, s2, graph, pathProjector,
+                       problem_->pathValidation());
+
+        if (path) {
+          nbConnection++;
+          if (!_1to2) roadmap()->addEdge(*itn1, *itn2, path);
+          if (!_2to1) {
+            core::interval_t timeRange = path->timeRange();
+            roadmap()->addEdge(*itn2, *itn1,
+                               path->extract(core::interval_t(
+                                   timeRange.second, timeRange.first)));
           }
-          if (connectSucceed) break;
+          connectSucceed = true;
+          break;
         }
       }
-      return nbConnection;
+      if (connectSucceed) break;
     }
-
-    inline std::size_t ManipulationPlanner::tryConnectNewNodes (const core::Nodes_t nodes)
-    {
-      PathProjectorPtr_t pathProjector (problem()->pathProjector ());
-      core::PathPtr_t path;
-      graph::GraphPtr_t graph = problem_->constraintGraph ();
-      std::size_t nbConnection = 0;
-      for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
-          itn1 != nodes.end (); ++itn1) {
-        const Configuration_t& q1 (*(*itn1)->configuration ());
-        graph::StatePtr_t s1 = getState (graph, *itn1);
-
-        for (core::Nodes_t::const_iterator itn2 = std::next (itn1);
-            itn2 != nodes.end (); ++itn2) {
-          if ((*itn1)->connectedComponent () == (*itn2)->connectedComponent ())
-            continue;
-          bool _1to2 = (*itn1)->isOutNeighbor (*itn2);
-          bool _2to1 = (*itn1)->isInNeighbor (*itn2);
-          assert (!_1to2 || !_2to1);
-          const Configuration_t& q2 (*(*itn2)->configuration ());
-          graph::StatePtr_t s2 = getState (graph, *itn2);
-          assert (q1 != q2);
-
-          path = connect (q1, q2, s1, s2, graph, pathProjector,
-			  problem_->pathValidation());
-          if (path) {
-            nbConnection++;
-            if (!_1to2) roadmap ()->addEdge (*itn1, *itn2, path);
-            if (!_2to1) {
-              core::interval_t timeRange = path->timeRange ();
-              roadmap ()->addEdge (*itn2, *itn1, path->extract
-                  (core::interval_t (timeRange.second,
-                                     timeRange.first)));
-            }
-          }
+  }
+  return nbConnection;
+}
+
+inline std::size_t ManipulationPlanner::tryConnectNewNodes(
+    const core::Nodes_t nodes) {
+  PathProjectorPtr_t pathProjector(problem()->pathProjector());
+  core::PathPtr_t path;
+  graph::GraphPtr_t graph = problem_->constraintGraph();
+  std::size_t nbConnection = 0;
+  for (core::Nodes_t::const_iterator itn1 = nodes.begin(); itn1 != nodes.end();
+       ++itn1) {
+    const Configuration_t& q1(*(*itn1)->configuration());
+    graph::StatePtr_t s1 = getState(graph, *itn1);
+
+    for (core::Nodes_t::const_iterator itn2 = std::next(itn1);
+         itn2 != nodes.end(); ++itn2) {
+      if ((*itn1)->connectedComponent() == (*itn2)->connectedComponent())
+        continue;
+      bool _1to2 = (*itn1)->isOutNeighbor(*itn2);
+      bool _2to1 = (*itn1)->isInNeighbor(*itn2);
+      assert(!_1to2 || !_2to1);
+      const Configuration_t& q2(*(*itn2)->configuration());
+      graph::StatePtr_t s2 = getState(graph, *itn2);
+      assert(q1 != q2);
+
+      path = connect(q1, q2, s1, s2, graph, pathProjector,
+                     problem_->pathValidation());
+      if (path) {
+        nbConnection++;
+        if (!_1to2) roadmap()->addEdge(*itn1, *itn2, path);
+        if (!_2to1) {
+          core::interval_t timeRange = path->timeRange();
+          roadmap()->addEdge(*itn2, *itn1,
+                             path->extract(core::interval_t(timeRange.second,
+                                                            timeRange.first)));
         }
       }
-      return nbConnection;
-    }
-
-    ManipulationPlanner::ManipulationPlanner (const ProblemConstPtr_t& problem,
-        const RoadmapPtr_t& roadmap) :
-      core::PathPlanner (problem, roadmap),
-      shooter_ (problem->configurationShooter()),
-      problem_ (problem), roadmap_ (roadmap),
-      extendStep_ (problem->getParameter
-		   ("ManipulationPlanner/extendStep").floatValue()),
-      qProj_ (problem->robot ()->configSize ())
-    {}
-
-    void ManipulationPlanner::init (const ManipulationPlannerWkPtr_t& weak)
-    {
-      core::PathPlanner::init (weak);
-      weakPtr_ = weak;
     }
-
-    using core::Parameter;
-    using core::ParameterDescription;
-
-    HPP_START_PARAMETER_DECLARATION(ManipulationPlanner)
-    core::Problem::declareParameter(ParameterDescription(Parameter::FLOAT,
-          "ManipulationPlanner/extendStep",
-          "Step of the RRT extension",
-          Parameter((value_type)1)));
-    HPP_END_PARAMETER_DECLARATION(ManipulationPlanner)
-  } // namespace manipulation
-} // namespace hpp
+  }
+  return nbConnection;
+}
+
+ManipulationPlanner::ManipulationPlanner(const ProblemConstPtr_t& problem,
+                                         const RoadmapPtr_t& roadmap)
+    : core::PathPlanner(problem, roadmap),
+      shooter_(problem->configurationShooter()),
+      problem_(problem),
+      roadmap_(roadmap),
+      extendStep_(
+          problem->getParameter("ManipulationPlanner/extendStep").floatValue()),
+      qProj_(problem->robot()->configSize()) {}
+
+void ManipulationPlanner::init(const ManipulationPlannerWkPtr_t& weak) {
+  core::PathPlanner::init(weak);
+  weakPtr_ = weak;
+}
+
+using core::Parameter;
+using core::ParameterDescription;
+
+HPP_START_PARAMETER_DECLARATION(ManipulationPlanner)
+core::Problem::declareParameter(ParameterDescription(
+    Parameter::FLOAT, "ManipulationPlanner/extendStep",
+    "Step of the RRT extension", Parameter((value_type)1)));
+HPP_END_PARAMETER_DECLARATION(ManipulationPlanner)
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/path-optimization/enforce-transition-semantic.cc b/src/path-optimization/enforce-transition-semantic.cc
index 1dc1777acc22163eb9363fd735963d3c503eab1e..2662b5ec3a2107c27c2e6d867f1734176173e263 100644
--- a/src/path-optimization/enforce-transition-semantic.cc
+++ b/src/path-optimization/enforce-transition-semantic.cc
@@ -26,151 +26,137 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/path-optimization/enforce-transition-semantic.hh>
-
-#include <hpp/pinocchio/util.hh>
 #include <hpp/core/path-vector.hh>
-
 #include <hpp/manipulation/constraint-set.hh>
 #include <hpp/manipulation/graph/edge.hh>
 #include <hpp/manipulation/graph/graph.hh>
 #include <hpp/manipulation/graph/state.hh>
+#include <hpp/manipulation/path-optimization/enforce-transition-semantic.hh>
+#include <hpp/pinocchio/util.hh>
 
 namespace hpp {
-  namespace manipulation {
-    namespace pathOptimization {
-      using hpp::core::Path;
-      using hpp::core::PathPtr_t;
-      using hpp::core::PathVector;
-      using hpp::core::PathVectorPtr_t;
-      using graph::Edges_t;
-      using graph::StatePtr_t;
+namespace manipulation {
+namespace pathOptimization {
+using graph::Edges_t;
+using graph::StatePtr_t;
+using hpp::core::Path;
+using hpp::core::PathPtr_t;
+using hpp::core::PathVector;
+using hpp::core::PathVectorPtr_t;
 
+Edges_t getAllEdges(const StatePtr_t& from, const StatePtr_t& to) {
+  Edges_t edges;
+  for (graph::Neighbors_t::const_iterator it = from->neighbors().begin();
+       it != from->neighbors().end(); ++it) {
+    if (it->second->stateTo() == to) edges.push_back(it->second);
+  }
+  for (Edges_t::const_iterator it = from->hiddenNeighbors().begin();
+       it != from->hiddenNeighbors().end(); ++it) {
+    if ((*it)->stateTo() == to) edges.push_back(*it);
+  }
+  return edges;
+}
 
-      Edges_t getAllEdges (const StatePtr_t& from, const StatePtr_t& to)
-      {
-        Edges_t edges;
-        for (graph::Neighbors_t::const_iterator it = from->neighbors ().begin ();
-            it != from->neighbors ().end (); ++it) {
-          if (it->second->stateTo () == to)
-            edges.push_back (it->second);
-        }
-        for (Edges_t::const_iterator it = from->hiddenNeighbors ().begin ();
-            it != from->hiddenNeighbors ().end (); ++it) {
-          if ((*it)->stateTo () == to)
-            edges.push_back (*it);
-        }
-        return edges;
-      }
-
-      PathVectorPtr_t EnforceTransitionSemantic::optimize (const PathVectorPtr_t& path)
-      {
-        PathVectorPtr_t input = PathVector::create (
-              path->outputSize(), path->outputDerivativeSize()); 
-        PathVectorPtr_t output = PathVector::create (
-              path->outputSize(), path->outputDerivativeSize()); 
-        path->flatten (input);
+PathVectorPtr_t EnforceTransitionSemantic::optimize(
+    const PathVectorPtr_t& path) {
+  PathVectorPtr_t input =
+      PathVector::create(path->outputSize(), path->outputDerivativeSize());
+  PathVectorPtr_t output =
+      PathVector::create(path->outputSize(), path->outputDerivativeSize());
+  path->flatten(input);
 
-        ConstraintSetPtr_t c;
-        for (std::size_t i = 0; i < input->numberPaths(); ++i) {
-          PathPtr_t current = input->pathAtRank (i);
-          output->appendPath(current);
-          c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
-          if (!c) {
-            hppDout(info, "No manipulation::ConstraintSet");
-            break;
-          }
-          Configuration_t q0 = current->initial();
-          Configuration_t q1 = current->end    ();
-          StatePtr_t src = c->edge()->stateFrom();
-          StatePtr_t dst = c->edge()->stateTo  ();
-          if (src == dst) continue;
+  ConstraintSetPtr_t c;
+  for (std::size_t i = 0; i < input->numberPaths(); ++i) {
+    PathPtr_t current = input->pathAtRank(i);
+    output->appendPath(current);
+    c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, current->constraints());
+    if (!c) {
+      hppDout(info, "No manipulation::ConstraintSet");
+      break;
+    }
+    Configuration_t q0 = current->initial();
+    Configuration_t q1 = current->end();
+    StatePtr_t src = c->edge()->stateFrom();
+    StatePtr_t dst = c->edge()->stateTo();
+    if (src == dst) continue;
 
-          bool q0_in_src = src->contains(q0);
-          bool q1_in_src = src->contains(q1);
-          bool q0_in_dst = dst->contains(q0);
-          bool q1_in_dst = dst->contains(q1);
+    bool q0_in_src = src->contains(q0);
+    bool q1_in_src = src->contains(q1);
+    bool q0_in_dst = dst->contains(q0);
+    bool q1_in_dst = dst->contains(q1);
 
-          if (q0_in_src && q1_in_dst) // Nominal case
-            continue;
-          hppDout (warning, "Transition " << i << ". "
-              "\nsrc=" << src->name() <<
-              "\ndst=" << dst->name() <<
-              "\nq0_in_src=" << q0_in_src <<
-              "\nq1_in_src=" << q1_in_src <<
-              "\nq0_in_dst=" << q0_in_dst <<
-              "\nq1_in_dst=" << q1_in_dst <<
-              setpyformat <<
-              "\nq0=" << one_line(q0) <<
-              "\nq1=" << one_line(q1) <<
-              unsetpyformat <<
-              "\nTrying with state.");
+    if (q0_in_src && q1_in_dst)  // Nominal case
+      continue;
+    hppDout(warning, "Transition "
+                         << i
+                         << ". "
+                            "\nsrc="
+                         << src->name() << "\ndst=" << dst->name()
+                         << "\nq0_in_src=" << q0_in_src << "\nq1_in_src="
+                         << q1_in_src << "\nq0_in_dst=" << q0_in_dst
+                         << "\nq1_in_dst=" << q1_in_dst << setpyformat
+                         << "\nq0=" << one_line(q0) << "\nq1=" << one_line(q1)
+                         << unsetpyformat << "\nTrying with state.");
 
-          StatePtr_t from, to;
-          if (q0_in_dst && q1_in_src) { // Reversed from nominal case
-            from = dst;
-            to = src;
-          } else if (q0_in_dst && q1_in_dst) {
-            from = dst;
-            to = dst;
-          } else if (q0_in_src && q1_in_src) {
-            from = src;
-            to = src;
-          } else if (q0_in_src) { // Keep same transition
-            continue;
-          } else if (q0_in_dst) { // Reverse current transition
-            from = dst;
-            to = src;
-          } else if (q1_in_src) { // Keep same transition
-            continue;
-          } else if (q1_in_dst) { // Reverse current transition
-            from = dst;
-            to = src;
-          }
-          if (from && to) {
-            // Check that a path from dst to to exists.
-            Edges_t transitions = getAllEdges(from, to);
-            if (transitions.size() >= 1)
-            {
-              if (transitions.size() > 1)
-              {
-                hppDout (info, "More than one transition...");
-              }
-              c->edge(transitions[0]);
-              continue;
-            }
-          }
-          hppDout (warning, "Enable to find a suitable transition for " << i << ". "
-              "\nsrc=" << src->name() <<
-              "\ndst=" << dst->name() <<
-              "\nq0_in_src=" << q0_in_src <<
-              "\nq1_in_src=" << q1_in_src <<
-              "\nq0_in_dst=" << q0_in_dst <<
-              "\nq1_in_dst=" << q1_in_dst <<
-              setpyformat <<
-              "\nq0=" << one_line(q0) <<
-              "\nq1=" << one_line(q1) <<
-              unsetpyformat <<
-              "\nTrying with state.");
-
-          StatePtr_t state = c->edge()->state();
-          // Check that a path from dst to to exists.
-          Edges_t transitions = getAllEdges(state, state);
-          if (transitions.size() >= 1)
-          {
-            if (transitions.size() > 1)
-            {
-              hppDout (info, "More than one transition...");
-            }
-            c->edge(transitions[0]);
-            continue;
-          } else {
-            hppDout (error, "Enable to find a suitable transition for " << *current);
-          }
+    StatePtr_t from, to;
+    if (q0_in_dst && q1_in_src) {  // Reversed from nominal case
+      from = dst;
+      to = src;
+    } else if (q0_in_dst && q1_in_dst) {
+      from = dst;
+      to = dst;
+    } else if (q0_in_src && q1_in_src) {
+      from = src;
+      to = src;
+    } else if (q0_in_src) {  // Keep same transition
+      continue;
+    } else if (q0_in_dst) {  // Reverse current transition
+      from = dst;
+      to = src;
+    } else if (q1_in_src) {  // Keep same transition
+      continue;
+    } else if (q1_in_dst) {  // Reverse current transition
+      from = dst;
+      to = src;
+    }
+    if (from && to) {
+      // Check that a path from dst to to exists.
+      Edges_t transitions = getAllEdges(from, to);
+      if (transitions.size() >= 1) {
+        if (transitions.size() > 1) {
+          hppDout(info, "More than one transition...");
         }
-        return output;
+        c->edge(transitions[0]);
+        continue;
+      }
+    }
+    hppDout(warning, "Enable to find a suitable transition for "
+                         << i
+                         << ". "
+                            "\nsrc="
+                         << src->name() << "\ndst=" << dst->name()
+                         << "\nq0_in_src=" << q0_in_src << "\nq1_in_src="
+                         << q1_in_src << "\nq0_in_dst=" << q0_in_dst
+                         << "\nq1_in_dst=" << q1_in_dst << setpyformat
+                         << "\nq0=" << one_line(q0) << "\nq1=" << one_line(q1)
+                         << unsetpyformat << "\nTrying with state.");
+
+    StatePtr_t state = c->edge()->state();
+    // Check that a path from dst to to exists.
+    Edges_t transitions = getAllEdges(state, state);
+    if (transitions.size() >= 1) {
+      if (transitions.size() > 1) {
+        hppDout(info, "More than one transition...");
       }
+      c->edge(transitions[0]);
+      continue;
+    } else {
+      hppDout(error, "Enable to find a suitable transition for " << *current);
+    }
+  }
+  return output;
+}
 
-    } // namespace pathOptimization
-  } // namespace manipulation
-} // namespace hpp
+}  // namespace pathOptimization
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/path-optimization/random-shortcut.cc b/src/path-optimization/random-shortcut.cc
index 0d40cc177e3dacc24eacfb142ab8e80b3acd5e1c..00ba25a96ea24a88dd97d4eb5e7b7064a1eb23c1 100644
--- a/src/path-optimization/random-shortcut.cc
+++ b/src/path-optimization/random-shortcut.cc
@@ -26,57 +26,51 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/path-optimization/random-shortcut.hh>
-
-#include <hpp/core/path.hh>
 #include <hpp/core/path-vector.hh>
+#include <hpp/core/path.hh>
 #include <hpp/manipulation/constraint-set.hh>
 #include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/path-optimization/random-shortcut.hh>
 
 namespace hpp {
-  namespace manipulation {
-    namespace pathOptimization {
-      using core::PathPtr_t;
-      using core::PathVector;
-      using core::PathVectorPtr_t;
+namespace manipulation {
+namespace pathOptimization {
+using core::PathPtr_t;
+using core::PathVector;
+using core::PathVectorPtr_t;
 
-      bool isShort (const PathVectorPtr_t& path, const value_type& t)
-      {
-        value_type localT;
-        std::size_t i = path->rankAtParam (t, localT);
-        PathPtr_t p = path->pathAtRank (i);
-        PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, p);
-        if (pv) return isShort (pv, localT);
-        ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
-        if (c) return c->edge()->isShort();
-        return false;
-      }
+bool isShort(const PathVectorPtr_t& path, const value_type& t) {
+  value_type localT;
+  std::size_t i = path->rankAtParam(t, localT);
+  PathPtr_t p = path->pathAtRank(i);
+  PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, p);
+  if (pv) return isShort(pv, localT);
+  ConstraintSetPtr_t c =
+      HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
+  if (c) return c->edge()->isShort();
+  return false;
+}
 
-      bool RandomShortcut::shootTimes (const PathVectorPtr_t& currentOpt,
-          const value_type& t0,
-          value_type& t1,
-          value_type& t2,
-          const value_type& t3)
-      {
-        bool ok = false;
-        for (int i = 0; i < 5; ++i)
-        {
-          value_type u2 = (t3-t0) * rand ()/RAND_MAX;
-          value_type u1 = (t3-t0) * rand ()/RAND_MAX;
-          if (isShort (currentOpt, u1) || isShort (currentOpt, u2))
-            continue;
-          if (u1 < u2) {
-            t1 = t0 + u1;
-            t2 = t0 + u2;
-          } else {
-            t1 = t0 + u2;
-            t2 = t0 + u1;
-          }
-          ok = true;
-          break;
-        }
-        return ok;
-      }
-    } // namespace pathOptimization
-  }  // namespace manipulation
-} // namespace hpp
+bool RandomShortcut::shootTimes(const PathVectorPtr_t& currentOpt,
+                                const value_type& t0, value_type& t1,
+                                value_type& t2, const value_type& t3) {
+  bool ok = false;
+  for (int i = 0; i < 5; ++i) {
+    value_type u2 = (t3 - t0) * rand() / RAND_MAX;
+    value_type u1 = (t3 - t0) * rand() / RAND_MAX;
+    if (isShort(currentOpt, u1) || isShort(currentOpt, u2)) continue;
+    if (u1 < u2) {
+      t1 = t0 + u1;
+      t2 = t0 + u2;
+    } else {
+      t1 = t0 + u2;
+      t2 = t0 + u1;
+    }
+    ok = true;
+    break;
+  }
+  return ok;
+}
+}  // namespace pathOptimization
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc
index 1ab278d7b802e856234ff5482fc1b0e8a0a79f93..a819b3d7399828925ff0cb10601bb44e18fba56a 100644
--- a/src/path-optimization/spline-gradient-based.cc
+++ b/src/path-optimization/spline-gradient-based.cc
@@ -26,255 +26,262 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/path-optimization/spline-gradient-based.hh>
-
 #include <hpp/core/path-optimization/linear-constraint.hh>
-
 #include <hpp/manipulation/constraint-set.hh>
-#include <hpp/manipulation/graph/state.hh>
 #include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/state.hh>
+#include <hpp/manipulation/path-optimization/spline-gradient-based.hh>
 
 namespace hpp {
-  namespace manipulation {
-    namespace pathOptimization {
-
-      template <int _PB, int _SO>
-      SplineGradientBased<_PB, _SO>::SplineGradientBased
-      (const ProblemConstPtr_t& problem) : Parent_t (problem)
-      {
-        this->checkOptimum_ = true;
+namespace manipulation {
+namespace pathOptimization {
+
+template <int _PB, int _SO>
+SplineGradientBased<_PB, _SO>::SplineGradientBased(
+    const ProblemConstPtr_t& problem)
+    : Parent_t(problem) {
+  this->checkOptimum_ = true;
+}
+
+// ----------- Convenience class -------------------------------------- //
+
+// ----------- Resolution steps --------------------------------------- //
+
+template <int _PB, int _SO>
+typename SplineGradientBased<_PB, _SO>::Ptr_t
+SplineGradientBased<_PB, _SO>::create(const ProblemConstPtr_t& problem) {
+  SplineGradientBased* ptr = new SplineGradientBased(problem);
+  Ptr_t shPtr(ptr);
+  return shPtr;
+}
+
+template <int _PB, int _SO>
+typename SplineGradientBased<_PB, _SO>::Ptr_t
+SplineGradientBased<_PB, _SO>::createFromCore(
+    const core::ProblemConstPtr_t& problem) {
+  if (!HPP_DYNAMIC_PTR_CAST(const Problem, problem))
+    throw std::invalid_argument("This is not a manipulation problem.");
+  return create(HPP_STATIC_PTR_CAST(const Problem, problem));
+}
+
+template <int _PB, int _SO>
+void SplineGradientBased<_PB, _SO>::initializePathValidation(
+    const Splines_t& splines) {
+  this->validations_.resize(splines.size());
+  for (std::size_t i = 0; i < splines.size(); ++i) {
+    ConstraintSetPtr_t set =
+        HPP_STATIC_PTR_CAST(ConstraintSet, splines[i]->constraints());
+    if (set && set->edge())
+      this->validations_[i] = set->edge()->pathValidation();
+    else
+      this->validations_[i] = this->problem()->pathValidation();
+  }
+}
+
+template <int _PB, int _SO>
+void SplineGradientBased<_PB, _SO>::addProblemConstraints(
+    const core::PathVectorPtr_t& init, const Splines_t& splines,
+    LinearConstraint& lc, SplineOptimizationDatas_t& sods) const {
+  assert(init->numberPaths() == splines.size() &&
+         sods.size() == splines.size());
+
+  bool zeroDerivative =
+      this->problem()
+          ->getParameter(
+              "SplineGradientBased/zeroDerivativesAtStateIntersection")
+          .boolValue();
+
+  const std::size_t last = splines.size() - 1;
+  graph::StatePtr_t stateOfStart;
+  for (std::size_t i = 0; i < last; ++i) {
+    core::PathPtr_t path = init->pathAtRank(i);
+    ConstraintSetPtr_t set =
+        HPP_STATIC_PTR_CAST(ConstraintSet, splines[i]->constraints());
+    if (!set || !set->edge())
+      std::invalid_argument(
+          "Cannot optimize a path that has not been "
+          "generated with a graph.");
+    graph::EdgePtr_t transition = set->edge();
+
+    this->addProblemConstraintOnPath(path, i, splines[i], lc, sods[i]);
+
+    // The path should always go through the start and end states of the
+    // transition.
+    assert(!HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, transition));
+    graph::StatePtr_t from = transition->stateFrom();
+    graph::StatePtr_t to = transition->stateTo();
+    graph::StatePtr_t from2 = from, to2 = to;
+
+    Configuration_t q0 = path->initial(), q1 = path->end();
+    const bool src_contains_q0 = from->contains(q0);
+    const bool dst_contains_q0 = to->contains(q0);
+    const bool src_contains_q1 = from->contains(q1);
+    const bool dst_contains_q1 = to->contains(q1);
+
+    bool use_direct = src_contains_q0 && dst_contains_q1;
+    bool use_reverse = src_contains_q1 && dst_contains_q0;
+    if (use_direct && use_reverse) {
+      if (i == 0 || stateOfStart == from)
+        use_reverse = false;
+      else if (stateOfStart == to)
+        use_direct = false;
+      else if (stateOfStart) {
+        if (stateOfStart->contains(q0))
+          use_reverse = false;
+        else
+          use_direct = false;  // assumes stateOfStart->contains(q0)
+      } else
+        use_reverse = false;  // default if we don't know what to do...
+    }
+    if (use_direct) {
+      // Nominal case
+      if (transition->state() != to) {
+        constrainEndIntoState(path, i, splines[i], transition->stateTo(), lc);
       }
-
-      // ----------- Convenience class -------------------------------------- //
-
-      // ----------- Resolution steps --------------------------------------- //
-
-      template <int _PB, int _SO>
-      typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::create
-      (const ProblemConstPtr_t& problem)
-      {
-	SplineGradientBased* ptr = new SplineGradientBased (problem);
-	Ptr_t shPtr (ptr);
-	return shPtr;
+      stateOfStart = to;
+    } else if (use_reverse) {
+      // Reversed nominal case
+      if (transition->state() != from) {
+        constrainEndIntoState(path, i, splines[i], from, lc);
       }
-
-      template <int _PB, int _SO>
-      typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::createFromCore
-      (const core::ProblemConstPtr_t& problem)
-      {
-        if (!HPP_DYNAMIC_PTR_CAST(const Problem, problem))
-          throw std::invalid_argument("This is not a manipulation problem.");
-        return create (HPP_STATIC_PTR_CAST(const Problem, problem));
-      }
-
-      template <int _PB, int _SO>
-      void SplineGradientBased<_PB, _SO>::initializePathValidation
-      (const Splines_t& splines)
-      {
-        this->validations_.resize(splines.size());
-        for (std::size_t i = 0; i < splines.size(); ++i) {
-          ConstraintSetPtr_t set = HPP_STATIC_PTR_CAST (ConstraintSet, splines[i]->constraints ());
-          if (set && set->edge())
-            this->validations_[i] = set->edge()->pathValidation();
-          else
-            this->validations_[i] = this->problem()->pathValidation();
+      stateOfStart = from;
+    } else {
+      if (src_contains_q0) {  // q1 must stay in state
+        to2 = transition->state();
+        stateOfStart = to;
+      } else if (dst_contains_q0) {  // q1 must stay in state
+        from2 = transition->state();
+        stateOfStart = from;
+      } else if (src_contains_q1) {  // q1 must stay in src
+        to2 = transition->state();
+        stateOfStart = from;
+        if (transition->state() != from) {
+          constrainEndIntoState(path, i, splines[i], from, lc);
         }
-      }
-
-      template <int _PB, int _SO>
-      void SplineGradientBased<_PB, _SO>::addProblemConstraints
-      (const core::PathVectorPtr_t& init, const Splines_t& splines, LinearConstraint& lc, SplineOptimizationDatas_t& sods) const
-      {
-        assert (init->numberPaths() == splines.size() && sods.size() == splines.size());
-
-        bool zeroDerivative = this->problem()->getParameter
-	  ("SplineGradientBased/zeroDerivativesAtStateIntersection").
-	  boolValue();
-
-        const std::size_t last = splines.size() - 1;
-        graph::StatePtr_t stateOfStart;
-        for (std::size_t i = 0; i < last; ++i) {
-          core::PathPtr_t path = init->pathAtRank(i);
-          ConstraintSetPtr_t set = HPP_STATIC_PTR_CAST (ConstraintSet, splines[i]->constraints ());
-          if (!set || !set->edge())
-            std::invalid_argument ("Cannot optimize a path that has not been "
-                "generated with a graph.");
-          graph::EdgePtr_t transition = set->edge();
-
-          this->addProblemConstraintOnPath (path, i, splines[i], lc, sods[i]);
-
-          // The path should always go through the start and end states of the
-          // transition.
-          assert(!HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, transition));
-          graph::StatePtr_t from = transition->stateFrom();
-          graph::StatePtr_t to = transition->stateTo();
-          graph::StatePtr_t from2 = from, to2 = to;
-
-          Configuration_t q0 = path->initial (),
-                          q1 = path->end ();
-          const bool src_contains_q0 = from->contains (q0);
-          const bool dst_contains_q0 = to  ->contains (q0);
-          const bool src_contains_q1 = from->contains (q1);
-          const bool dst_contains_q1 = to  ->contains (q1);
-
-          bool use_direct  = src_contains_q0 && dst_contains_q1;
-          bool use_reverse = src_contains_q1 && dst_contains_q0;
-          if (use_direct && use_reverse) {
-            if (i == 0 || stateOfStart == from)
-              use_reverse = false;
-            else if (stateOfStart == to)
-              use_direct = false;
-            else if (stateOfStart) {
-              if (stateOfStart->contains(q0))
-                use_reverse = false;
-              else
-                use_direct = false; // assumes stateOfStart->contains(q0)
-            } else
-              use_reverse = false; // default if we don't know what to do...
-          }
-          if (use_direct) {
-            // Nominal case
-            if (transition->state() != to) {
-              constrainEndIntoState (path, i, splines[i], transition->stateTo(),
-                                     lc);
-            }
-            stateOfStart = to;
-          } else if (use_reverse) {
-            // Reversed nominal case
-            if (transition->state() != from) {
-              constrainEndIntoState (path, i, splines[i], from, lc);
-            }
-            stateOfStart = from;
-          } else {
-            if (src_contains_q0) { // q1 must stay in state
-              to2 = transition->state();
-              stateOfStart = to;
-            } else if (dst_contains_q0) { // q1 must stay in state
-              from2 = transition->state();
-              stateOfStart = from;
-            } else if (src_contains_q1) { // q1 must stay in src
-              to2 = transition->state();
-              stateOfStart = from;
-              if (transition->state() != from) {
-                constrainEndIntoState (path, i, splines[i], from, lc);
-              }
-            } else if (dst_contains_q1) { // q1 must stay in dst
-              from2 = transition->state();
-              stateOfStart = to;
-              if (transition->state() != to) {
-                constrainEndIntoState (path, i, splines[i], to, lc);
-              }
-            } else {
-              // q0 and q1 are in state. We add no constraint.
-              hppDout (warning, "Add no constraint for this path.");
-              from2 = transition->state();
-              to2 = transition->state();
-              stateOfStart.reset();
-            }
-          }
-          if (zeroDerivative) {
-            if (   !(use_reverse && src_contains_q0 && src_contains_q1)
-                && !(use_direct  && dst_contains_q0 && dst_contains_q1)
-                && from2 != to2                         ) {
-              constraintDerivativesAtEndOfSpline (i, splines[i], lc);
-            }
-          }
+      } else if (dst_contains_q1) {  // q1 must stay in dst
+        from2 = transition->state();
+        stateOfStart = to;
+        if (transition->state() != to) {
+          constrainEndIntoState(path, i, splines[i], to, lc);
         }
-        this->addProblemConstraintOnPath (init->pathAtRank(last), last, splines[last], lc, sods[last]);
-      }
-
-      template <int _PB, int _SO>
-      void SplineGradientBased<_PB, _SO>::constrainEndIntoState
-      (const core::PathPtr_t& path, const size_type& idxSpline,
-       const SplinePtr_t& spline, const graph::StatePtr_t state,
-       LinearConstraint& lc) const
-      {
-        typename Spline::BasisFunctionVector_t B1;
-        spline->basisFunctionDerivative(0, 1, B1);
-        // TODO Should we add zero velocity sometimes ?
-
-        ConstraintSetPtr_t set = state->configConstraint();
-        value_type guessThreshold = this->problem()->getParameter
-	  ("SplineGradientBased/guessThreshold").floatValue();
-        Eigen::RowBlockIndices select =
-          this->computeActiveParameters (path, set->configProjector()->solver(), guessThreshold, true);
-        hppDout (info, "End of path " << idxSpline << ": do not change this dof " << select);
-        hppDout (info, "End of path " << idxSpline << ": state " << state->name());
-
-        const size_type rDof = this->robot_->numberDof(),
-                        col  = idxSpline * Spline::NbCoeffs * rDof,
-                        row = lc.J.rows(),
-                        nOutVar = select.nbIndices();
-
-        // Add nOutVar constraints
-        lc.addRows(nOutVar);
-        matrix_t I = select.rview(matrix_t::Identity(rDof, rDof));
-        for (size_type k = 0; k < Spline::NbCoeffs; ++k)
-          lc.J.block  (row, col + k * rDof, nOutVar, rDof) = B1(k) * I;
-        lc.b.segment(row, nOutVar) = select.rview(spline->parameters().transpose() * B1);
-
-        assert ((lc.J.block(row, col, nOutVar, rDof * Spline::NbCoeffs) * spline->rowParameters())
-            .isApprox(lc.b.segment(row, nOutVar)));
+      } else {
+        // q0 and q1 are in state. We add no constraint.
+        hppDout(warning, "Add no constraint for this path.");
+        from2 = transition->state();
+        to2 = transition->state();
+        stateOfStart.reset();
       }
-
-      template <int _PB, int _SO>
-      void SplineGradientBased<_PB, _SO>::constraintDerivativesAtEndOfSpline
-      (const size_type& idxSpline, const SplinePtr_t& spline,
-       LinearConstraint& lc) const
-      {
-        typename Spline::BasisFunctionVector_t B1;
-        spline->basisFunctionDerivative(1, 1, B1);
-
-        // ConstraintSetPtr_t set = state->configConstraint();
-        // value_type guessThreshold = this->problem().getParameter ("SplineGradientBased/guessThreshold").floatValue();
-        // Eigen::RowBlockIndices select =
-          // this->computeActiveParameters (path, set->configProjector()->solver(), guessThreshold);
-
-        const size_type rDof = this->robot_->numberDof(),
-                        col  = idxSpline * Spline::NbCoeffs * rDof,
-                        row = lc.J.rows(),
-                        // nOutVar = select.nbIndices();
-                        nOutVar = rDof;
-
-        // Add nOutVar constraints
-        lc.addRows(nOutVar);
-        // matrix_t I = select.rview(matrix_t::Identity(rDof, rDof));
-        matrix_t I (matrix_t::Identity(rDof, rDof));
-        for (size_type k = 0; k < Spline::NbCoeffs; ++k)
-          lc.J.block  (row, col + k * rDof, nOutVar, rDof) = B1(k) * I;
-        lc.b.segment(row, nOutVar).setZero();
-
-        if (!(lc.J.block(row, col, nOutVar, rDof * Spline::NbCoeffs) * spline->rowParameters())
-            .isApprox(lc.b.segment(row, nOutVar)))
-        {
-          hppDout (error, "The velocity should already be zero:\n"
-              << (lc.J.block(row, col, nOutVar, rDof * Spline::NbCoeffs) * spline->rowParameters()).transpose()
-              );
-        }
+    }
+    if (zeroDerivative) {
+      if (!(use_reverse && src_contains_q0 && src_contains_q1) &&
+          !(use_direct && dst_contains_q0 && dst_contains_q1) && from2 != to2) {
+        constraintDerivativesAtEndOfSpline(i, splines[i], lc);
       }
-
-      // ----------- Optimize ----------------------------------------------- //
-
-      // ----------- Convenience functions ---------------------------------- //
-
-      // ----------- Instanciate -------------------------------------------- //
-
-      // template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
-      // template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>;
-      // template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>;
-      template class SplineGradientBased<core::path::BernsteinBasis, 1>; // equivalent to StraightPath
-      // template class SplineGradientBased<core::path::BernsteinBasis, 2>;
-      template class SplineGradientBased<core::path::BernsteinBasis, 3>;
-
-      using core::Parameter;
-      using core::ParameterDescription;
-
-      HPP_START_PARAMETER_DECLARATION(SplineGradientBased)
-      core::Problem::declareParameter(ParameterDescription(Parameter::BOOL,
-            "SplineGradientBased/zeroDerivativesAtStateIntersection",
-            "Whether we should enforce a null velocity at each control point where the state before and after is different.",
-            Parameter(false)));
-      HPP_END_PARAMETER_DECLARATION(SplineGradientBased)
-    } // namespace pathOptimization
-  }  // namespace manipulation
-} // namespace hpp
+    }
+  }
+  this->addProblemConstraintOnPath(init->pathAtRank(last), last, splines[last],
+                                   lc, sods[last]);
+}
+
+template <int _PB, int _SO>
+void SplineGradientBased<_PB, _SO>::constrainEndIntoState(
+    const core::PathPtr_t& path, const size_type& idxSpline,
+    const SplinePtr_t& spline, const graph::StatePtr_t state,
+    LinearConstraint& lc) const {
+  typename Spline::BasisFunctionVector_t B1;
+  spline->basisFunctionDerivative(0, 1, B1);
+  // TODO Should we add zero velocity sometimes ?
+
+  ConstraintSetPtr_t set = state->configConstraint();
+  value_type guessThreshold =
+      this->problem()
+          ->getParameter("SplineGradientBased/guessThreshold")
+          .floatValue();
+  Eigen::RowBlockIndices select = this->computeActiveParameters(
+      path, set->configProjector()->solver(), guessThreshold, true);
+  hppDout(info,
+          "End of path " << idxSpline << ": do not change this dof " << select);
+  hppDout(info, "End of path " << idxSpline << ": state " << state->name());
+
+  const size_type rDof = this->robot_->numberDof(),
+                  col = idxSpline * Spline::NbCoeffs * rDof, row = lc.J.rows(),
+                  nOutVar = select.nbIndices();
+
+  // Add nOutVar constraints
+  lc.addRows(nOutVar);
+  matrix_t I = select.rview(matrix_t::Identity(rDof, rDof));
+  for (size_type k = 0; k < Spline::NbCoeffs; ++k)
+    lc.J.block(row, col + k * rDof, nOutVar, rDof) = B1(k) * I;
+  lc.b.segment(row, nOutVar) =
+      select.rview(spline->parameters().transpose() * B1);
+
+  assert((lc.J.block(row, col, nOutVar, rDof * Spline::NbCoeffs) *
+          spline->rowParameters())
+             .isApprox(lc.b.segment(row, nOutVar)));
+}
+
+template <int _PB, int _SO>
+void SplineGradientBased<_PB, _SO>::constraintDerivativesAtEndOfSpline(
+    const size_type& idxSpline, const SplinePtr_t& spline,
+    LinearConstraint& lc) const {
+  typename Spline::BasisFunctionVector_t B1;
+  spline->basisFunctionDerivative(1, 1, B1);
+
+  // ConstraintSetPtr_t set = state->configConstraint();
+  // value_type guessThreshold = this->problem().getParameter
+  // ("SplineGradientBased/guessThreshold").floatValue(); Eigen::RowBlockIndices
+  // select = this->computeActiveParameters (path,
+  // set->configProjector()->solver(), guessThreshold);
+
+  const size_type rDof = this->robot_->numberDof(),
+                  col = idxSpline * Spline::NbCoeffs * rDof, row = lc.J.rows(),
+                  // nOutVar = select.nbIndices();
+      nOutVar = rDof;
+
+  // Add nOutVar constraints
+  lc.addRows(nOutVar);
+  // matrix_t I = select.rview(matrix_t::Identity(rDof, rDof));
+  matrix_t I(matrix_t::Identity(rDof, rDof));
+  for (size_type k = 0; k < Spline::NbCoeffs; ++k)
+    lc.J.block(row, col + k * rDof, nOutVar, rDof) = B1(k) * I;
+  lc.b.segment(row, nOutVar).setZero();
+
+  if (!(lc.J.block(row, col, nOutVar, rDof * Spline::NbCoeffs) *
+        spline->rowParameters())
+           .isApprox(lc.b.segment(row, nOutVar))) {
+    hppDout(error,
+            "The velocity should already be zero:\n"
+                << (lc.J.block(row, col, nOutVar, rDof * Spline::NbCoeffs) *
+                    spline->rowParameters())
+                       .transpose());
+  }
+}
+
+// ----------- Optimize ----------------------------------------------- //
+
+// ----------- Convenience functions ---------------------------------- //
+
+// ----------- Instanciate -------------------------------------------- //
+
+// template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>; //
+// equivalent to StraightPath template class
+// SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>; template class
+// SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>;
+template class SplineGradientBased<core::path::BernsteinBasis,
+                                   1>;  // equivalent to StraightPath
+// template class SplineGradientBased<core::path::BernsteinBasis, 2>;
+template class SplineGradientBased<core::path::BernsteinBasis, 3>;
+
+using core::Parameter;
+using core::ParameterDescription;
+
+HPP_START_PARAMETER_DECLARATION(SplineGradientBased)
+core::Problem::declareParameter(ParameterDescription(
+    Parameter::BOOL, "SplineGradientBased/zeroDerivativesAtStateIntersection",
+    "Whether we should enforce a null velocity at each control point where the "
+    "state before and after is different.",
+    Parameter(false)));
+HPP_END_PARAMETER_DECLARATION(SplineGradientBased)
+}  // namespace pathOptimization
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/path-planner/end-effector-trajectory.cc b/src/path-planner/end-effector-trajectory.cc
index 8d989f7dd2006b2fe7116aaf80bcb256b4fc8bca..1a874c7c0350a5cf34a5729625f66bc2bfda8075 100644
--- a/src/path-planner/end-effector-trajectory.cc
+++ b/src/path-planner/end-effector-trajectory.cc
@@ -26,248 +26,246 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-# include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
-
-# include <pinocchio/multibody/data.hpp>
-
-# include <hpp/util/exception-factory.hh>
-# include <hpp/pinocchio/util.hh>
-# include <hpp/pinocchio/device-sync.hh>
-# include <hpp/pinocchio/liegroup-element.hh>
-
-# include <hpp/constraints/differentiable-function.hh>
-# include <hpp/constraints/implicit.hh>
-
-# include <hpp/core/config-projector.hh>
-# include <hpp/core/config-validations.hh>
-# include <hpp/core/configuration-shooter.hh>
-# include <hpp/core/path-validation.hh>
-# include <hpp/core/problem.hh>
-# include <hpp/core/roadmap.hh>
-# include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
+#include <hpp/constraints/differentiable-function.hh>
+#include <hpp/constraints/implicit.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/core/config-validations.hh>
+#include <hpp/core/configuration-shooter.hh>
+#include <hpp/core/path-validation.hh>
+#include <hpp/core/problem.hh>
+#include <hpp/core/roadmap.hh>
+#include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
+#include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
+#include <hpp/pinocchio/device-sync.hh>
+#include <hpp/pinocchio/liegroup-element.hh>
+#include <hpp/pinocchio/util.hh>
+#include <hpp/util/exception-factory.hh>
+#include <pinocchio/multibody/data.hpp>
 
 namespace hpp {
-  namespace manipulation {
-    namespace pathPlanner {
-      typedef manipulation::steeringMethod::EndEffectorTrajectory      SM_t;
-      typedef manipulation::steeringMethod::EndEffectorTrajectoryPtr_t SMPtr_t;
-
-      EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create
-      (const core::ProblemConstPtr_t& problem)
-      {
-        EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem));
-        ptr->init(ptr);
-        return ptr;
-      }
-
-      EndEffectorTrajectoryPtr_t EndEffectorTrajectory::createWithRoadmap (
-          const core::ProblemConstPtr_t& problem,
-	  const core::RoadmapPtr_t& roadmap)
-      {
-        EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem,
-								  roadmap));
-        ptr->init(ptr);
-        return ptr;
-      }
-
-      void EndEffectorTrajectory::tryConnectInitAndGoals ()
-      {}
-
-      void EndEffectorTrajectory::startSolve ()
-      {
-        //core::PathPlanner::startSolve();
-        //problem().checkProblem ();
-        if (!problem()->robot ()) {
-          std::string msg ("No device in problem.");
-          hppDout (error, msg);
-          throw std::runtime_error (msg);
-        }
-
-        if (!problem()->initConfig ()) {
-          std::string msg ("No init config in problem.");
-          hppDout (error, msg);
-          throw std::runtime_error (msg);
-        }
-
-        // Tag init and goal configurations in the roadmap
-        roadmap()->resetGoalNodes ();
-
-        SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
-        if (!sm)
-          throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
-
-        if (!sm->constraints() || !sm->constraints()->configProjector())
-          throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
-        core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
-
-        const constraints::ImplicitPtr_t& trajConstraint = sm->trajectoryConstraint ();
-        if (!trajConstraint)
-          throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
-        if (!sm->trajectory())
-          throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
-
-        const core::NumericalConstraints_t& ncs = constraints->numericalConstraints();
-        bool ok = false;
-        for (std::size_t i = 0; i < ncs.size(); ++i) {
-          if (ncs[i] == trajConstraint) {
-            ok = true;
-            break; // Same pointer
-          }
-          // Here, we do not check the right hand side on purpose.
-          // if (*ncs[i] == *trajConstraint) {
-          if (ncs[i]->functionPtr() == trajConstraint->functionPtr()
-              && ncs[i]->comparisonType() == trajConstraint->comparisonType()) {
-            ok = true;
-            // TODO We should only modify the path constraint.
-            // However, only the pointers to implicit constraints are copied
-            // while we would like the implicit constraints to be copied as well.
-            ncs[i]->rightHandSideFunction (sm->trajectory());
-            break; // logically identical
-          }
-        }
-        if (!ok) {
-          HPP_THROW (std::logic_error, "EndEffectorTrajectory could not find "
-              "constraint " << trajConstraint->function());
-        }
-      }
-
-      void EndEffectorTrajectory::oneStep ()
-      {
-        SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
-        if (!sm)
-          throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
-        if (!sm->trajectoryConstraint ())
-          throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
-        if (!sm->trajectory())
-          throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
-
-        if (!sm->constraints() || !sm->constraints()->configProjector())
-          throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
-        core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
-
-        core::ConfigValidationPtr_t  cfgValidation (problem()->configValidations());
-        core::  PathValidationPtr_t pathValidation (problem()->pathValidation());
-        core::    ValidationReportPtr_t cfgReport;
-        core::PathValidationReportPtr_t pathReport;
-
-        core::interval_t timeRange (sm->timeRange());
-
-        std::vector<core::Configuration_t> qs (configurations(*problem()->initConfig()));
-        if (qs.empty()) {
-          hppDout (info, "Failed to generate initial configs.");
-          return;
-        }
-
-        // Generate a valid initial configuration.
-        bool success = false;
-        bool resetRightHandSide = true;
-        std::size_t i;
-
-        vector_t times (nDiscreteSteps_+1);
-        matrix_t steps (problem()->robot()->configSize(), nDiscreteSteps_+1);
-
-        times[0] = timeRange.first;
-        for (int j = 1; j < nDiscreteSteps_; ++j)
-          times[j] = timeRange.first + j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
-        times[nDiscreteSteps_] = timeRange.second;
-
-        for (i = 0; i < qs.size(); ++i)
-        {
-          if (resetRightHandSide) {
-            constraints->rightHandSideAt (times[0]);
-            resetRightHandSide = false;
-          }
-          Configuration_t& q (qs[i]);
-          if (!constraints->apply (q)) continue;
-          if (!cfgValidation->validate (q, cfgReport)) continue;
-          resetRightHandSide = true;
-
-          steps.col(0) = q;
-          success = true;
-          for (int j = 1; j <= nDiscreteSteps_; ++j) {
-            constraints->rightHandSideAt (times[j]);
-            hppDout (info, "RHS: " << setpyformat << constraints->rightHandSide().transpose());
-            steps.col(j) = steps.col(j-1);
-            if (!constraints->apply (steps.col(j))) {
-              hppDout (info, "Failed to generate destination config.\n" << setpyformat
-                  << *constraints
-                  << "\nq=" << one_line (q));
-              success = false;
-              break;
-            }
-          }
-          if (!success) continue;
-          success = false;
-
-          if (!cfgValidation->validate (steps.col(nDiscreteSteps_), cfgReport)) {
-            hppDout (info, "Destination config is in collision.");
-            continue;
-          }
-
-          core::PathPtr_t path = sm->projectedPath(times, steps);
-          if (!path) {
-            hppDout (info, "Steering method failed.\n" << setpyformat
-                << "times: " << one_line(times) << '\n'
-                << "configs:\n" << condensed(steps.transpose()) << '\n'
-                );
-            continue;
-          }
-
-          core::PathPtr_t validPart;
-          if (!pathValidation->validate (path, false, validPart, pathReport)) {
-            hppDout (info, "Path is in collision.");
-            continue;
-          }
-
-          roadmap()->initNode (make_shared<Configuration_t>(steps.col(0)));
-          core::NodePtr_t init = roadmap()->   initNode ();
-          core::NodePtr_t goal = roadmap()->addGoalNode (
-              make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
-          roadmap()->addEdge (init, goal, path);
-          success = true;
-          if (feasibilityOnly_) break;
-        }
-      }
-
-      std::vector<core::Configuration_t> EndEffectorTrajectory::configurations(const core::Configuration_t& q_init)
-      {
-        if (!ikSolverInit_) {
-          std::vector<core::Configuration_t> configs(nRandomConfig_ + 1);
-          configs[0] = q_init;
-          for (int i = 1; i < nRandomConfig_ + 1; ++i)
-            problem()->configurationShooter()->shoot(configs[i]);
-          return configs;
-        }
-
-        // TODO Compute the target and call ikSolverInit_
-        // See https://gepgitlab.laas.fr/airbus-xtct/hpp_airbus_xtct for an
-        // example using IKFast.
-        throw std::runtime_error ("Using an IkSolverInitialization is not implemented yet");
-      }
-
-      EndEffectorTrajectory::EndEffectorTrajectory
-      (const core::ProblemConstPtr_t& problem) : core::PathPlanner (problem)
-      {}
-
-      EndEffectorTrajectory::EndEffectorTrajectory
-      (const core::ProblemConstPtr_t& problem,
-       const core::RoadmapPtr_t& roadmap)
-        : core::PathPlanner (problem, roadmap)
-      {}
-
-      void EndEffectorTrajectory::checkFeasibilityOnly (bool enable)
-      {
-        feasibilityOnly_ = enable;
-      }
-
-      void EndEffectorTrajectory::init (const EndEffectorTrajectoryWkPtr_t& weak)
-      {
-        core::PathPlanner::init (weak);
-        weak_ = weak;
-        nRandomConfig_ = 10;
-        nDiscreteSteps_ = 1;
-        feasibilityOnly_ = true;
+namespace manipulation {
+namespace pathPlanner {
+typedef manipulation::steeringMethod::EndEffectorTrajectory SM_t;
+typedef manipulation::steeringMethod::EndEffectorTrajectoryPtr_t SMPtr_t;
+
+EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create(
+    const core::ProblemConstPtr_t& problem) {
+  EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory(problem));
+  ptr->init(ptr);
+  return ptr;
+}
+
+EndEffectorTrajectoryPtr_t EndEffectorTrajectory::createWithRoadmap(
+    const core::ProblemConstPtr_t& problem, const core::RoadmapPtr_t& roadmap) {
+  EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory(problem, roadmap));
+  ptr->init(ptr);
+  return ptr;
+}
+
+void EndEffectorTrajectory::tryConnectInitAndGoals() {}
+
+void EndEffectorTrajectory::startSolve() {
+  // core::PathPlanner::startSolve();
+  // problem().checkProblem ();
+  if (!problem()->robot()) {
+    std::string msg("No device in problem.");
+    hppDout(error, msg);
+    throw std::runtime_error(msg);
+  }
+
+  if (!problem()->initConfig()) {
+    std::string msg("No init config in problem.");
+    hppDout(error, msg);
+    throw std::runtime_error(msg);
+  }
+
+  // Tag init and goal configurations in the roadmap
+  roadmap()->resetGoalNodes();
+
+  SMPtr_t sm(HPP_DYNAMIC_PTR_CAST(SM_t, problem()->steeringMethod()));
+  if (!sm)
+    throw std::invalid_argument(
+        "Steering method must be of type "
+        "hpp::manipulation::steeringMethod::EndEffectorTrajectory");
+
+  if (!sm->constraints() || !sm->constraints()->configProjector())
+    throw std::invalid_argument(
+        "Steering method constraint has no ConfigProjector.");
+  core::ConfigProjectorPtr_t constraints(sm->constraints()->configProjector());
+
+  const constraints::ImplicitPtr_t& trajConstraint = sm->trajectoryConstraint();
+  if (!trajConstraint)
+    throw std::invalid_argument(
+        "EndEffectorTrajectory has no trajectory constraint.");
+  if (!sm->trajectory())
+    throw std::invalid_argument("EndEffectorTrajectory has no trajectory.");
+
+  const core::NumericalConstraints_t& ncs = constraints->numericalConstraints();
+  bool ok = false;
+  for (std::size_t i = 0; i < ncs.size(); ++i) {
+    if (ncs[i] == trajConstraint) {
+      ok = true;
+      break;  // Same pointer
+    }
+    // Here, we do not check the right hand side on purpose.
+    // if (*ncs[i] == *trajConstraint) {
+    if (ncs[i]->functionPtr() == trajConstraint->functionPtr() &&
+        ncs[i]->comparisonType() == trajConstraint->comparisonType()) {
+      ok = true;
+      // TODO We should only modify the path constraint.
+      // However, only the pointers to implicit constraints are copied
+      // while we would like the implicit constraints to be copied as well.
+      ncs[i]->rightHandSideFunction(sm->trajectory());
+      break;  // logically identical
+    }
+  }
+  if (!ok) {
+    HPP_THROW(std::logic_error,
+              "EndEffectorTrajectory could not find "
+              "constraint "
+                  << trajConstraint->function());
+  }
+}
+
+void EndEffectorTrajectory::oneStep() {
+  SMPtr_t sm(HPP_DYNAMIC_PTR_CAST(SM_t, problem()->steeringMethod()));
+  if (!sm)
+    throw std::invalid_argument(
+        "Steering method must be of type "
+        "hpp::manipulation::steeringMethod::EndEffectorTrajectory");
+  if (!sm->trajectoryConstraint())
+    throw std::invalid_argument(
+        "EndEffectorTrajectory has no trajectory constraint.");
+  if (!sm->trajectory())
+    throw std::invalid_argument("EndEffectorTrajectory has no trajectory.");
+
+  if (!sm->constraints() || !sm->constraints()->configProjector())
+    throw std::invalid_argument(
+        "Steering method constraint has no ConfigProjector.");
+  core::ConfigProjectorPtr_t constraints(sm->constraints()->configProjector());
+
+  core::ConfigValidationPtr_t cfgValidation(problem()->configValidations());
+  core::PathValidationPtr_t pathValidation(problem()->pathValidation());
+  core::ValidationReportPtr_t cfgReport;
+  core::PathValidationReportPtr_t pathReport;
+
+  core::interval_t timeRange(sm->timeRange());
+
+  std::vector<core::Configuration_t> qs(
+      configurations(*problem()->initConfig()));
+  if (qs.empty()) {
+    hppDout(info, "Failed to generate initial configs.");
+    return;
+  }
+
+  // Generate a valid initial configuration.
+  bool success = false;
+  bool resetRightHandSide = true;
+  std::size_t i;
+
+  vector_t times(nDiscreteSteps_ + 1);
+  matrix_t steps(problem()->robot()->configSize(), nDiscreteSteps_ + 1);
+
+  times[0] = timeRange.first;
+  for (int j = 1; j < nDiscreteSteps_; ++j)
+    times[j] = timeRange.first +
+               j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
+  times[nDiscreteSteps_] = timeRange.second;
+
+  for (i = 0; i < qs.size(); ++i) {
+    if (resetRightHandSide) {
+      constraints->rightHandSideAt(times[0]);
+      resetRightHandSide = false;
+    }
+    Configuration_t& q(qs[i]);
+    if (!constraints->apply(q)) continue;
+    if (!cfgValidation->validate(q, cfgReport)) continue;
+    resetRightHandSide = true;
+
+    steps.col(0) = q;
+    success = true;
+    for (int j = 1; j <= nDiscreteSteps_; ++j) {
+      constraints->rightHandSideAt(times[j]);
+      hppDout(info, "RHS: " << setpyformat
+                            << constraints->rightHandSide().transpose());
+      steps.col(j) = steps.col(j - 1);
+      if (!constraints->apply(steps.col(j))) {
+        hppDout(info, "Failed to generate destination config.\n"
+                          << setpyformat << *constraints
+                          << "\nq=" << one_line(q));
+        success = false;
+        break;
       }
-    } // namespace pathPlanner
-  } // namespace manipulation
-} // namespace hpp
+    }
+    if (!success) continue;
+    success = false;
+
+    if (!cfgValidation->validate(steps.col(nDiscreteSteps_), cfgReport)) {
+      hppDout(info, "Destination config is in collision.");
+      continue;
+    }
+
+    core::PathPtr_t path = sm->projectedPath(times, steps);
+    if (!path) {
+      hppDout(info, "Steering method failed.\n"
+                        << setpyformat << "times: " << one_line(times) << '\n'
+                        << "configs:\n"
+                        << condensed(steps.transpose()) << '\n');
+      continue;
+    }
+
+    core::PathPtr_t validPart;
+    if (!pathValidation->validate(path, false, validPart, pathReport)) {
+      hppDout(info, "Path is in collision.");
+      continue;
+    }
+
+    roadmap()->initNode(make_shared<Configuration_t>(steps.col(0)));
+    core::NodePtr_t init = roadmap()->initNode();
+    core::NodePtr_t goal = roadmap()->addGoalNode(
+        make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
+    roadmap()->addEdge(init, goal, path);
+    success = true;
+    if (feasibilityOnly_) break;
+  }
+}
+
+std::vector<core::Configuration_t> EndEffectorTrajectory::configurations(
+    const core::Configuration_t& q_init) {
+  if (!ikSolverInit_) {
+    std::vector<core::Configuration_t> configs(nRandomConfig_ + 1);
+    configs[0] = q_init;
+    for (int i = 1; i < nRandomConfig_ + 1; ++i)
+      problem()->configurationShooter()->shoot(configs[i]);
+    return configs;
+  }
+
+  // TODO Compute the target and call ikSolverInit_
+  // See https://gepgitlab.laas.fr/airbus-xtct/hpp_airbus_xtct for an
+  // example using IKFast.
+  throw std::runtime_error(
+      "Using an IkSolverInitialization is not implemented yet");
+}
+
+EndEffectorTrajectory::EndEffectorTrajectory(
+    const core::ProblemConstPtr_t& problem)
+    : core::PathPlanner(problem) {}
+
+EndEffectorTrajectory::EndEffectorTrajectory(
+    const core::ProblemConstPtr_t& problem, const core::RoadmapPtr_t& roadmap)
+    : core::PathPlanner(problem, roadmap) {}
+
+void EndEffectorTrajectory::checkFeasibilityOnly(bool enable) {
+  feasibilityOnly_ = enable;
+}
+
+void EndEffectorTrajectory::init(const EndEffectorTrajectoryWkPtr_t& weak) {
+  core::PathPlanner::init(weak);
+  weak_ = weak;
+  nRandomConfig_ = 10;
+  nDiscreteSteps_ = 1;
+  feasibilityOnly_ = true;
+}
+}  // namespace pathPlanner
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index dc271a8c70b401bde4c3d690d9ab9a06cdb4a3a5..c650259e98a950c52ca9b2c04cef481e7c86a8df 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -28,364 +28,364 @@
 
 #include "hpp/manipulation/problem-solver.hh"
 
-#include <hpp/util/pointer.hh>
-#include <hpp/util/debug.hh>
-
-#include <hpp/pinocchio/gripper.hh>
-
 #include <hpp/constraints/convex-shape-contact.hh>
 #include <hpp/constraints/explicit/convex-shape-contact.hh>
-
-#include <hpp/core/path-optimization/random-shortcut.hh>
+#include <hpp/core/continuous-validation/dichotomy.hh>
+#include <hpp/core/continuous-validation/progressive.hh>
 #include <hpp/core/path-optimization/partial-shortcut.hh>
-#include <hpp/core/path-projector/progressive.hh>
+#include <hpp/core/path-optimization/random-shortcut.hh>
 #include <hpp/core/path-projector/dichotomy.hh>
 #include <hpp/core/path-projector/global.hh>
+#include <hpp/core/path-projector/progressive.hh>
 #include <hpp/core/path-projector/recursive-hermite.hh>
 #include <hpp/core/path-validation/discretized-collision-checking.hh>
 #include <hpp/core/path-validation/discretized-joint-bound.hh>
-#include <hpp/core/continuous-validation/dichotomy.hh>
-#include <hpp/core/continuous-validation/progressive.hh>
 #include <hpp/core/roadmap.hh>
 #include <hpp/core/steering-method/dubins.hh>
 #include <hpp/core/steering-method/hermite.hh>
 #include <hpp/core/steering-method/reeds-shepp.hh>
 #include <hpp/core/steering-method/snibud.hh>
 #include <hpp/core/steering-method/straight.hh>
+#include <hpp/pinocchio/gripper.hh>
+#include <hpp/util/debug.hh>
+#include <hpp/util/pointer.hh>
 
-#include "hpp/manipulation/package-config.hh" // HPP_MANIPULATION_HAS_WHOLEBODY_STEP
-
-#include "hpp/manipulation/problem.hh"
-#include "hpp/manipulation/device.hh"
-#include "hpp/manipulation/handle.hh"
-#include "hpp/manipulation/graph/graph.hh"
-#include "hpp/manipulation/manipulation-planner.hh"
-#include "hpp/manipulation/roadmap.hh"
 #include "hpp/manipulation/constraint-set.hh"
+#include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/graph-node-optimizer.hh"
 #include "hpp/manipulation/graph-optimizer.hh"
 #include "hpp/manipulation/graph-path-validation.hh"
-#include "hpp/manipulation/graph-node-optimizer.hh"
-#include "hpp/manipulation/path-optimization/random-shortcut.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/handle.hh"
+#include "hpp/manipulation/manipulation-planner.hh"
+#include "hpp/manipulation/package-config.hh"  // HPP_MANIPULATION_HAS_WHOLEBODY_STEP
 #include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
+#include "hpp/manipulation/path-optimization/random-shortcut.hh"
 #include "hpp/manipulation/path-planner/end-effector-trajectory.hh"
 #include "hpp/manipulation/problem-target/state.hh"
+#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/roadmap.hh"
 #include "hpp/manipulation/steering-method/cross-state-optimization.hh"
-#include "hpp/manipulation/steering-method/graph.hh"
 #include "hpp/manipulation/steering-method/end-effector-trajectory.hh"
+#include "hpp/manipulation/steering-method/graph.hh"
 
 namespace hpp {
-  namespace manipulation {
-    typedef constraints::Implicit Implicit;
-    typedef constraints::ImplicitPtr_t ImplicitPtr_t;
-    namespace {
-      struct PartialShortcutTraits :
-        core::pathOptimization::PartialShortcutTraits {
-          static bool removeLockedJoints () { return false; }
-      };
-
-#define MAKE_GRAPH_PATH_VALIDATION_BUILDER(name, function)                     \
-      PathValidationPtr_t create ## name ## GraphPathValidation (              \
-          const core::DevicePtr_t& robot, const value_type& stepSize)          \
-      {                                                                        \
-        return GraphPathValidation::create (function (robot, stepSize));       \
-      }
-      MAKE_GRAPH_PATH_VALIDATION_BUILDER(DiscretizedCollision             , core::pathValidation::createDiscretizedCollisionChecking)
-      MAKE_GRAPH_PATH_VALIDATION_BUILDER(DiscretizedJointBound            , core::pathValidation::createDiscretizedJointBound)
-      //MAKE_GRAPH_PATH_VALIDATION_BUILDER(DiscretizedCollisionAndJointBound, createDiscretizedJointBoundAndCollisionChecking)
-
-      template <typename ParentSM_t, typename ChildSM_t>
-      core::SteeringMethodPtr_t createSMWithGuess
-      (const core::ProblemConstPtr_t& problem)
-      {
-        shared_ptr<ParentSM_t> sm = ParentSM_t::create (problem);
-        sm->innerSteeringMethod (ChildSM_t::createWithGuess (problem));
-        return sm;
-      }
-
-      template <typename PathProjectorType>
-      core::PathProjectorPtr_t createPathProjector
-      (const core::ProblemConstPtr_t& problem, const value_type& step)
-      {
-        steeringMethod::GraphPtr_t gsm =
-          HPP_DYNAMIC_PTR_CAST
-	  (steeringMethod::Graph, problem->steeringMethod());
-        if (!gsm) throw std::logic_error
-		    ("The steering method should be of type"
-		     " steeringMethod::Graph");
-        return PathProjectorType::create (problem->distance(),
-            gsm->innerSteeringMethod(), step);
-      }
-    }
-
-    std::ostream& operator<< (std::ostream& os, const Device& robot)
-    {
-      return robot.print (os);
-    }
-
-    ProblemSolver::ProblemSolver () :
-      core::ProblemSolver (), robot_ (), problem_ ()
-    {
-      robots.add ("hpp::manipulation::Device", manipulation::Device::create);
-      robotType ("hpp::manipulation::Device");
-
-      pathPlanners.add ("M-RRT", ManipulationPlanner::create);
-      pathPlanners.add ("EndEffectorTrajectory", pathPlanner::EndEffectorTrajectory::createWithRoadmap);
-
-      pathValidations.add ("Graph-Discretized"                      , createDiscretizedCollisionGraphPathValidation);
-      pathValidations.add ("Graph-DiscretizedCollision"             , createDiscretizedCollisionGraphPathValidation);
-      pathValidations.add ("Graph-DiscretizedJointBound"            , createDiscretizedJointBoundGraphPathValidation);
-      //pathValidations.add ("Graph-DiscretizedCollisionAndJointBound", createDiscretizedCollisionAndJointBoundGraphPathValidation);
-      pathValidations.add ("Graph-Dichotomy"  , GraphPathValidation::create<core::continuousValidation::Dichotomy  >);
-      pathValidations.add ("Graph-Progressive", GraphPathValidation::create<core::continuousValidation::Progressive>);
-
-      // TODO Uncomment to make Graph-Discretized the default.
-      //pathValidationType ("Graph-Discretized", 0.05);
-
-      pathOptimizers.add ("RandomShortcut",
-          pathOptimization::RandomShortcut::create);
-      pathOptimizers.add ("Graph-RandomShortcut",
-          GraphOptimizer::create <core::pathOptimization::RandomShortcut>);
-      pathOptimizers.add ("PartialShortcut", core::pathOptimization::
-          PartialShortcut::createWithTraits <PartialShortcutTraits>);
-      pathOptimizers.add ("Graph-PartialShortcut",
-          GraphOptimizer::create <core::pathOptimization::PartialShortcut>);
-      pathOptimizers.add ("EnforceTransitionSemantic",
-          pathOptimization::EnforceTransitionSemantic::create);
-
-      pathProjectors.add ("Progressive",
-          createPathProjector <core::pathProjector::Progressive>);
-      pathProjectors.add ("Dichotomy",
-          createPathProjector <core::pathProjector::Dichotomy>);
-      pathProjectors.add ("Global",
-          createPathProjector <core::pathProjector::Global>);
-      pathProjectors.add ("RecursiveHermite",
-          createPathProjector <core::pathProjector::RecursiveHermite>);
-
-      steeringMethods.add ("Graph-SteeringMethodStraight",
-          steeringMethod::Graph::create <core::steeringMethod::Straight>);
-      steeringMethods.add ("Graph-Straight",
-          steeringMethod::Graph::create <core::steeringMethod::Straight>);
-      steeringMethods.add ("Graph-Hermite",
-          steeringMethod::Graph::create <core::steeringMethod::Hermite>);
-      steeringMethods.add ("Graph-ReedsShepp",
-          createSMWithGuess <steeringMethod::Graph, core::steeringMethod::ReedsShepp>);
-      steeringMethods.add ("Graph-Dubins",
-          createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Dubins>);
-      steeringMethods.add ("Graph-Snibud",
-          createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Snibud>);
-      steeringMethods.add ("CrossStateOptimization-Straight",
-          steeringMethod::CrossStateOptimization::create<core::steeringMethod::Straight>);
-      steeringMethods.add ("CrossStateOptimization-ReedsShepp",
-          createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::ReedsShepp>);
-      steeringMethods.add ("CrossStateOptimization-Dubins",
-          createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Dubins>);
-      steeringMethods.add ("CrossStateOptimization-Snibud",
-          createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Snibud>);
-      steeringMethods.add ("EndEffectorTrajectory", steeringMethod::EndEffectorTrajectory::create);
-
-      pathPlannerType ("M-RRT");
-      steeringMethodType ("Graph-SteeringMethodStraight");
-    }
-
-    ProblemSolverPtr_t ProblemSolver::create ()
-    {
-      return ProblemSolverPtr_t (new ProblemSolver ());
-    }
-
-    void ProblemSolver::resetProblem ()
-    {
-      ProblemPtr_t p (Problem::create(robot_));
-      if (problem_) {
-        p->parameters = problem_->parameters;
-      }
-      initializeProblem (p);
-    }
-
-    void ProblemSolver::initializeProblem (ProblemPtr_t problem)
-    {
-      problem_ = problem;
-      core::ProblemSolver::initializeProblem (problem_);
-      if (constraintGraph_)
-        problem_->constraintGraph (constraintGraph_);
-      value_type tolerance;
-      const std::string& type = parent_t::pathValidationType (tolerance);
-      problem_->setPathValidationFactory (pathValidations.get(type), tolerance);
-    }
-
-    void ProblemSolver::constraintGraph (const std::string& graphName)
-    {
-      if (!graphs.has (graphName))
-        throw std::invalid_argument ("ProblemSolver has no graph named " + graphName);
-      constraintGraph_ = graphs.get(graphName);
-      RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, roadmap());
-      if (r) r->constraintGraph (constraintGraph_);
-      if (problem_) problem_->constraintGraph (constraintGraph_);
-    }
-
-    graph::GraphPtr_t ProblemSolver::constraintGraph () const
-    {
-      return constraintGraph_;
-    }
-
-    void ProblemSolver::initConstraintGraph ()
-    {
-      if (!constraintGraph_)
-        throw std::runtime_error ("The graph is not defined.");
-      initSteeringMethod();
-      constraintGraph_->clearConstraintsAndComplement();
-      for (std::size_t i = 0; i < constraintsAndComplements.size(); ++i) {
-        const ConstraintAndComplement_t& c = constraintsAndComplements[i];
-        constraintGraph ()->registerConstraints (c.constraint, c.complement, c.both);
-      }
-      constraintGraph_->initialize();
-    }
-
-    void ProblemSolver::createPlacementConstraint
-    (const std::string& name, const StringList_t& surface1,
-     const StringList_t& surface2, const value_type& margin)
-    {
-      if (!robot_) throw std::runtime_error ("No robot loaded");
-      JointAndShapes_t floorSurfaces, objectSurfaces, l;
-      for (StringList_t::const_iterator it1 = surface1.begin ();
-          it1 != surface1.end(); ++it1) {
-        if (!robot_->jointAndShapes.has (*it1))
-          throw std::runtime_error ("First list of triangles not found.");
-        l = robot_->jointAndShapes.get (*it1);
-        objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end());
-      }
-
-      for (StringList_t::const_iterator it2 = surface2.begin ();
-          it2 != surface2.end(); ++it2) {
-        // Search first robot triangles
-        if (robot_->jointAndShapes.has (*it2))
-          l = robot_->jointAndShapes.get (*it2);
-        // and then environment triangles.
-        else if (jointAndShapes.has (*it2))
-          l = jointAndShapes.get (*it2);
-        else throw std::runtime_error ("Second list of triangles not found.");
-        floorSurfaces.insert(floorSurfaces.end(), l.begin(), l.end());
-      }
-
-      typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t;
-      Constraint_t::Constraints_t constraints
-        (Constraint_t::createConstraintAndComplement
-         (name, robot_, floorSurfaces, objectSurfaces, margin));
-
-      addNumericalConstraint(std::get<0>(constraints)->function().name(),
-                             std::get<0>(constraints));
-      addNumericalConstraint(std::get<1>(constraints)->function().name(),
-                             std::get<1>(constraints));
-      addNumericalConstraint(std::get<2>(constraints)->function().name(),
-                             std::get<2>(constraints));
-      // Set security margin to contact constraint
-      assert(HPP_DYNAMIC_PTR_CAST(constraints::ConvexShapeContact,
-                                  std::get<0>(constraints)->functionPtr()));
-      constraints::ConvexShapeContactPtr_t contactFunction
-        (HPP_STATIC_PTR_CAST(constraints::ConvexShapeContact,
-                             std::get<0>(constraints)->functionPtr()));
-      contactFunction->setNormalMargin(margin);
-      constraintsAndComplements.push_back (
-          ConstraintAndComplement_t (std::get<0>(constraints),
-                                     std::get<1>(constraints),
-                                     std::get<2>(constraints)));
-    }
-
-    void ProblemSolver::createPrePlacementConstraint
-    (const std::string& name, const StringList_t& surface1,
-     const StringList_t& surface2, const value_type& width,
-     const value_type& margin)
-    {
-      if (!robot_) throw std::runtime_error ("No robot loaded");
-      JointAndShapes_t floorSurfaces, objectSurfaces, l;
-      for (StringList_t::const_iterator it1 = surface1.begin ();
-          it1 != surface1.end(); ++it1) {
-        if (!robot_->jointAndShapes.has (*it1))
-          throw std::runtime_error ("First list of triangles not found.");
-        l = robot_->jointAndShapes.get (*it1);
-        objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end());
-      }
-
-      for (StringList_t::const_iterator it2 = surface2.begin ();
-          it2 != surface2.end(); ++it2) {
-        // Search first robot triangles
-        if (robot_->jointAndShapes.has (*it2))
-          l = robot_->jointAndShapes.get (*it2);
-        // and then environment triangles.
-        else if (jointAndShapes.has (*it2))
-          l = jointAndShapes.get (*it2);
-        else throw std::runtime_error ("Second list of triangles not found.");
-        floorSurfaces.insert(floorSurfaces.end(), l.begin(), l.end());
-      }
-
-      hpp::constraints::ConvexShapeContactPtr_t cvxShape
-        (hpp::constraints::ConvexShapeContact::create
-         (name, robot_, floorSurfaces, objectSurfaces));
-      cvxShape->setNormalMargin(margin + width);
-      addNumericalConstraint (name, Implicit::create
-                              (cvxShape, 5 * constraints::EqualToZero));
-    }
-
-    void ProblemSolver::createGraspConstraint
-    (const std::string& name, const std::string& gripper,
-     const std::string& handle)
-    {
-      GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t());
-      if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
-      HandlePtr_t h = robot_->handles.get (handle, HandlePtr_t());
-      if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
-      const std::string cname = name + "/complement";
-      const std::string bname = name + "/hold";
-      ImplicitPtr_t constraint (h->createGrasp (g, name));
-      ImplicitPtr_t complement (h->createGraspComplement (g, cname));
-      ImplicitPtr_t both (h->createGraspAndComplement (g, bname));
-      addNumericalConstraint ( name, constraint);
-      addNumericalConstraint (cname, complement);
-      addNumericalConstraint (bname, both);
-
-      constraintsAndComplements.push_back (
-          ConstraintAndComplement_t (constraint, complement, both));
-    }
-
-    void ProblemSolver::createPreGraspConstraint
-    (const std::string& name, const std::string& gripper,
-     const std::string& handle)
-    {
-      GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t());
-      if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
-      HandlePtr_t h = robot_->handles.get (handle, HandlePtr_t());
-      if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
-
-      value_type c = h->clearance () + g->clearance ();
-      ImplicitPtr_t constraint = h->createPreGrasp (g, c, name);
-      addNumericalConstraint (name, constraint);
-    }
-
-    void ProblemSolver::pathValidationType (const std::string& type,
-        const value_type& tolerance)
-    {
-      parent_t::pathValidationType(type, tolerance);
-      if (problem_)
-        problem_->setPathValidationFactory (
-            pathValidations.get(type),
-            tolerance);
-    }
-
-    void ProblemSolver::resetRoadmap ()
-    {
-      if (!problem ())
-        throw std::runtime_error ("The problem is not defined.");
-      if (roadmap())
-        roadmap()->clear();
-      RoadmapPtr_t r (Roadmap::create (problem ()->distance (), problem ()->robot ()));
-      if (constraintGraph_) r->constraintGraph (constraintGraph_);
-      roadmap (r);
-    }
-
-    void ProblemSolver::setTargetState (const graph::StatePtr_t state)
-    {
-      problemTarget::StatePtr_t t =  problemTarget::State::create(ProblemPtr_t());
-      t->target(state);
-      target_ = t;
-    }
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+typedef constraints::Implicit Implicit;
+typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+namespace {
+struct PartialShortcutTraits : core::pathOptimization::PartialShortcutTraits {
+  static bool removeLockedJoints() { return false; }
+};
+
+#define MAKE_GRAPH_PATH_VALIDATION_BUILDER(name, function)          \
+  PathValidationPtr_t create##name##GraphPathValidation(            \
+      const core::DevicePtr_t& robot, const value_type& stepSize) { \
+    return GraphPathValidation::create(function(robot, stepSize));  \
+  }
+MAKE_GRAPH_PATH_VALIDATION_BUILDER(
+    DiscretizedCollision,
+    core::pathValidation::createDiscretizedCollisionChecking)
+MAKE_GRAPH_PATH_VALIDATION_BUILDER(
+    DiscretizedJointBound, core::pathValidation::createDiscretizedJointBound)
+// MAKE_GRAPH_PATH_VALIDATION_BUILDER(DiscretizedCollisionAndJointBound,
+// createDiscretizedJointBoundAndCollisionChecking)
+
+template <typename ParentSM_t, typename ChildSM_t>
+core::SteeringMethodPtr_t createSMWithGuess(
+    const core::ProblemConstPtr_t& problem) {
+  shared_ptr<ParentSM_t> sm = ParentSM_t::create(problem);
+  sm->innerSteeringMethod(ChildSM_t::createWithGuess(problem));
+  return sm;
+}
+
+template <typename PathProjectorType>
+core::PathProjectorPtr_t createPathProjector(
+    const core::ProblemConstPtr_t& problem, const value_type& step) {
+  steeringMethod::GraphPtr_t gsm =
+      HPP_DYNAMIC_PTR_CAST(steeringMethod::Graph, problem->steeringMethod());
+  if (!gsm)
+    throw std::logic_error(
+        "The steering method should be of type"
+        " steeringMethod::Graph");
+  return PathProjectorType::create(problem->distance(),
+                                   gsm->innerSteeringMethod(), step);
+}
+}  // namespace
+
+std::ostream& operator<<(std::ostream& os, const Device& robot) {
+  return robot.print(os);
+}
+
+ProblemSolver::ProblemSolver() : core::ProblemSolver(), robot_(), problem_() {
+  robots.add("hpp::manipulation::Device", manipulation::Device::create);
+  robotType("hpp::manipulation::Device");
+
+  pathPlanners.add("M-RRT", ManipulationPlanner::create);
+  pathPlanners.add("EndEffectorTrajectory",
+                   pathPlanner::EndEffectorTrajectory::createWithRoadmap);
+
+  pathValidations.add("Graph-Discretized",
+                      createDiscretizedCollisionGraphPathValidation);
+  pathValidations.add("Graph-DiscretizedCollision",
+                      createDiscretizedCollisionGraphPathValidation);
+  pathValidations.add("Graph-DiscretizedJointBound",
+                      createDiscretizedJointBoundGraphPathValidation);
+  // pathValidations.add ("Graph-DiscretizedCollisionAndJointBound",
+  // createDiscretizedCollisionAndJointBoundGraphPathValidation);
+  pathValidations.add(
+      "Graph-Dichotomy",
+      GraphPathValidation::create<core::continuousValidation::Dichotomy>);
+  pathValidations.add(
+      "Graph-Progressive",
+      GraphPathValidation::create<core::continuousValidation::Progressive>);
+
+  // TODO Uncomment to make Graph-Discretized the default.
+  // pathValidationType ("Graph-Discretized", 0.05);
+
+  pathOptimizers.add("RandomShortcut",
+                     pathOptimization::RandomShortcut::create);
+  pathOptimizers.add(
+      "Graph-RandomShortcut",
+      GraphOptimizer::create<core::pathOptimization::RandomShortcut>);
+  pathOptimizers.add("PartialShortcut",
+                     core::pathOptimization::PartialShortcut::createWithTraits<
+                         PartialShortcutTraits>);
+  pathOptimizers.add(
+      "Graph-PartialShortcut",
+      GraphOptimizer::create<core::pathOptimization::PartialShortcut>);
+  pathOptimizers.add("EnforceTransitionSemantic",
+                     pathOptimization::EnforceTransitionSemantic::create);
+
+  pathProjectors.add("Progressive",
+                     createPathProjector<core::pathProjector::Progressive>);
+  pathProjectors.add("Dichotomy",
+                     createPathProjector<core::pathProjector::Dichotomy>);
+  pathProjectors.add("Global",
+                     createPathProjector<core::pathProjector::Global>);
+  pathProjectors.add(
+      "RecursiveHermite",
+      createPathProjector<core::pathProjector::RecursiveHermite>);
+
+  steeringMethods.add(
+      "Graph-SteeringMethodStraight",
+      steeringMethod::Graph::create<core::steeringMethod::Straight>);
+  steeringMethods.add(
+      "Graph-Straight",
+      steeringMethod::Graph::create<core::steeringMethod::Straight>);
+  steeringMethods.add(
+      "Graph-Hermite",
+      steeringMethod::Graph::create<core::steeringMethod::Hermite>);
+  steeringMethods.add("Graph-ReedsShepp",
+                      createSMWithGuess<steeringMethod::Graph,
+                                        core::steeringMethod::ReedsShepp>);
+  steeringMethods.add(
+      "Graph-Dubins",
+      createSMWithGuess<steeringMethod::Graph, core::steeringMethod::Dubins>);
+  steeringMethods.add(
+      "Graph-Snibud",
+      createSMWithGuess<steeringMethod::Graph, core::steeringMethod::Snibud>);
+  steeringMethods.add("CrossStateOptimization-Straight",
+                      steeringMethod::CrossStateOptimization::create<
+                          core::steeringMethod::Straight>);
+  steeringMethods.add("CrossStateOptimization-ReedsShepp",
+                      createSMWithGuess<steeringMethod::CrossStateOptimization,
+                                        core::steeringMethod::ReedsShepp>);
+  steeringMethods.add("CrossStateOptimization-Dubins",
+                      createSMWithGuess<steeringMethod::CrossStateOptimization,
+                                        core::steeringMethod::Dubins>);
+  steeringMethods.add("CrossStateOptimization-Snibud",
+                      createSMWithGuess<steeringMethod::CrossStateOptimization,
+                                        core::steeringMethod::Snibud>);
+  steeringMethods.add("EndEffectorTrajectory",
+                      steeringMethod::EndEffectorTrajectory::create);
+
+  pathPlannerType("M-RRT");
+  steeringMethodType("Graph-SteeringMethodStraight");
+}
+
+ProblemSolverPtr_t ProblemSolver::create() {
+  return ProblemSolverPtr_t(new ProblemSolver());
+}
+
+void ProblemSolver::resetProblem() {
+  ProblemPtr_t p(Problem::create(robot_));
+  if (problem_) {
+    p->parameters = problem_->parameters;
+  }
+  initializeProblem(p);
+}
+
+void ProblemSolver::initializeProblem(ProblemPtr_t problem) {
+  problem_ = problem;
+  core::ProblemSolver::initializeProblem(problem_);
+  if (constraintGraph_) problem_->constraintGraph(constraintGraph_);
+  value_type tolerance;
+  const std::string& type = parent_t::pathValidationType(tolerance);
+  problem_->setPathValidationFactory(pathValidations.get(type), tolerance);
+}
+
+void ProblemSolver::constraintGraph(const std::string& graphName) {
+  if (!graphs.has(graphName))
+    throw std::invalid_argument("ProblemSolver has no graph named " +
+                                graphName);
+  constraintGraph_ = graphs.get(graphName);
+  RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST(Roadmap, roadmap());
+  if (r) r->constraintGraph(constraintGraph_);
+  if (problem_) problem_->constraintGraph(constraintGraph_);
+}
+
+graph::GraphPtr_t ProblemSolver::constraintGraph() const {
+  return constraintGraph_;
+}
+
+void ProblemSolver::initConstraintGraph() {
+  if (!constraintGraph_) throw std::runtime_error("The graph is not defined.");
+  initSteeringMethod();
+  constraintGraph_->clearConstraintsAndComplement();
+  for (std::size_t i = 0; i < constraintsAndComplements.size(); ++i) {
+    const ConstraintAndComplement_t& c = constraintsAndComplements[i];
+    constraintGraph()->registerConstraints(c.constraint, c.complement, c.both);
+  }
+  constraintGraph_->initialize();
+}
+
+void ProblemSolver::createPlacementConstraint(const std::string& name,
+                                              const StringList_t& surface1,
+                                              const StringList_t& surface2,
+                                              const value_type& margin) {
+  if (!robot_) throw std::runtime_error("No robot loaded");
+  JointAndShapes_t floorSurfaces, objectSurfaces, l;
+  for (StringList_t::const_iterator it1 = surface1.begin();
+       it1 != surface1.end(); ++it1) {
+    if (!robot_->jointAndShapes.has(*it1))
+      throw std::runtime_error("First list of triangles not found.");
+    l = robot_->jointAndShapes.get(*it1);
+    objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end());
+  }
+
+  for (StringList_t::const_iterator it2 = surface2.begin();
+       it2 != surface2.end(); ++it2) {
+    // Search first robot triangles
+    if (robot_->jointAndShapes.has(*it2)) l = robot_->jointAndShapes.get(*it2);
+    // and then environment triangles.
+    else if (jointAndShapes.has(*it2))
+      l = jointAndShapes.get(*it2);
+    else
+      throw std::runtime_error("Second list of triangles not found.");
+    floorSurfaces.insert(floorSurfaces.end(), l.begin(), l.end());
+  }
+
+  typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t;
+  Constraint_t::Constraints_t constraints(
+      Constraint_t::createConstraintAndComplement(name, robot_, floorSurfaces,
+                                                  objectSurfaces, margin));
+
+  addNumericalConstraint(std::get<0>(constraints)->function().name(),
+                         std::get<0>(constraints));
+  addNumericalConstraint(std::get<1>(constraints)->function().name(),
+                         std::get<1>(constraints));
+  addNumericalConstraint(std::get<2>(constraints)->function().name(),
+                         std::get<2>(constraints));
+  // Set security margin to contact constraint
+  assert(HPP_DYNAMIC_PTR_CAST(constraints::ConvexShapeContact,
+                              std::get<0>(constraints)->functionPtr()));
+  constraints::ConvexShapeContactPtr_t contactFunction(
+      HPP_STATIC_PTR_CAST(constraints::ConvexShapeContact,
+                          std::get<0>(constraints)->functionPtr()));
+  contactFunction->setNormalMargin(margin);
+  constraintsAndComplements.push_back(ConstraintAndComplement_t(
+      std::get<0>(constraints), std::get<1>(constraints),
+      std::get<2>(constraints)));
+}
+
+void ProblemSolver::createPrePlacementConstraint(const std::string& name,
+                                                 const StringList_t& surface1,
+                                                 const StringList_t& surface2,
+                                                 const value_type& width,
+                                                 const value_type& margin) {
+  if (!robot_) throw std::runtime_error("No robot loaded");
+  JointAndShapes_t floorSurfaces, objectSurfaces, l;
+  for (StringList_t::const_iterator it1 = surface1.begin();
+       it1 != surface1.end(); ++it1) {
+    if (!robot_->jointAndShapes.has(*it1))
+      throw std::runtime_error("First list of triangles not found.");
+    l = robot_->jointAndShapes.get(*it1);
+    objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end());
+  }
+
+  for (StringList_t::const_iterator it2 = surface2.begin();
+       it2 != surface2.end(); ++it2) {
+    // Search first robot triangles
+    if (robot_->jointAndShapes.has(*it2)) l = robot_->jointAndShapes.get(*it2);
+    // and then environment triangles.
+    else if (jointAndShapes.has(*it2))
+      l = jointAndShapes.get(*it2);
+    else
+      throw std::runtime_error("Second list of triangles not found.");
+    floorSurfaces.insert(floorSurfaces.end(), l.begin(), l.end());
+  }
+
+  hpp::constraints::ConvexShapeContactPtr_t cvxShape(
+      hpp::constraints::ConvexShapeContact::create(name, robot_, floorSurfaces,
+                                                   objectSurfaces));
+  cvxShape->setNormalMargin(margin + width);
+  addNumericalConstraint(
+      name, Implicit::create(cvxShape, 5 * constraints::EqualToZero));
+}
+
+void ProblemSolver::createGraspConstraint(const std::string& name,
+                                          const std::string& gripper,
+                                          const std::string& handle) {
+  GripperPtr_t g = robot_->grippers.get(gripper, GripperPtr_t());
+  if (!g) throw std::runtime_error("No gripper with name " + gripper + ".");
+  HandlePtr_t h = robot_->handles.get(handle, HandlePtr_t());
+  if (!h) throw std::runtime_error("No handle with name " + handle + ".");
+  const std::string cname = name + "/complement";
+  const std::string bname = name + "/hold";
+  ImplicitPtr_t constraint(h->createGrasp(g, name));
+  ImplicitPtr_t complement(h->createGraspComplement(g, cname));
+  ImplicitPtr_t both(h->createGraspAndComplement(g, bname));
+  addNumericalConstraint(name, constraint);
+  addNumericalConstraint(cname, complement);
+  addNumericalConstraint(bname, both);
+
+  constraintsAndComplements.push_back(
+      ConstraintAndComplement_t(constraint, complement, both));
+}
+
+void ProblemSolver::createPreGraspConstraint(const std::string& name,
+                                             const std::string& gripper,
+                                             const std::string& handle) {
+  GripperPtr_t g = robot_->grippers.get(gripper, GripperPtr_t());
+  if (!g) throw std::runtime_error("No gripper with name " + gripper + ".");
+  HandlePtr_t h = robot_->handles.get(handle, HandlePtr_t());
+  if (!h) throw std::runtime_error("No handle with name " + handle + ".");
+
+  value_type c = h->clearance() + g->clearance();
+  ImplicitPtr_t constraint = h->createPreGrasp(g, c, name);
+  addNumericalConstraint(name, constraint);
+}
+
+void ProblemSolver::pathValidationType(const std::string& type,
+                                       const value_type& tolerance) {
+  parent_t::pathValidationType(type, tolerance);
+  if (problem_)
+    problem_->setPathValidationFactory(pathValidations.get(type), tolerance);
+}
+
+void ProblemSolver::resetRoadmap() {
+  if (!problem()) throw std::runtime_error("The problem is not defined.");
+  if (roadmap()) roadmap()->clear();
+  RoadmapPtr_t r(Roadmap::create(problem()->distance(), problem()->robot()));
+  if (constraintGraph_) r->constraintGraph(constraintGraph_);
+  roadmap(r);
+}
+
+void ProblemSolver::setTargetState(const graph::StatePtr_t state) {
+  problemTarget::StatePtr_t t = problemTarget::State::create(ProblemPtr_t());
+  t->target(state);
+  target_ = t;
+}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/problem-target/astar.hh b/src/problem-target/astar.hh
index 89823dce8a65de1f9d6f96088ed962bc0fb74228..f3062b8b13b13ecc173c1eb689956e333e5348f5 100644
--- a/src/problem-target/astar.hh
+++ b/src/problem-target/astar.hh
@@ -28,155 +28,140 @@
 // DAMAGE.
 
 #ifndef HPP_MANIPULATION_PROBLEM_TARGET_ASTAR_HH
-# define HPP_MANIPULATION_PROBLEM_TARGET_ASTAR_HH
-
-# include <limits>
-
-# include <hpp/util/exception-factory.hh>
-
-# include <hpp/core/distance.hh>
-# include <hpp/core/edge.hh>
-# include <hpp/core/node.hh>
-# include <hpp/core/path-vector.hh>
-
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/roadmap.hh>
-# include <hpp/manipulation/roadmap-node.hh>
+#define HPP_MANIPULATION_PROBLEM_TARGET_ASTAR_HH
+
+#include <hpp/core/distance.hh>
+#include <hpp/core/edge.hh>
+#include <hpp/core/node.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/roadmap-node.hh>
+#include <hpp/manipulation/roadmap.hh>
+#include <hpp/util/exception-factory.hh>
+#include <limits>
 
 namespace hpp {
-  namespace manipulation {
-    namespace problemTarget {
-      class HPP_MANIPULATION_LOCAL Astar
-      {
-        typedef core::PathVectorPtr_t PathVectorPtr_t;
-        typedef core::EdgePtr_t EdgePtr_t;
-        typedef std::vector < RoadmapNodePtr_t > Nodes_t;
-        typedef std::list < EdgePtr_t > Edges_t;
-        typedef std::map < core::NodePtr_t, EdgePtr_t> Parent_t;
-        Nodes_t closed_;
-        Nodes_t open_;
-        std::map <RoadmapNodePtr_t, value_type> costFromStart_;
-        std::map <RoadmapNodePtr_t, value_type> estimatedCostToGoal_;
-        Parent_t parent_;
-        core::RoadmapPtr_t roadmap_;
-        core::DistancePtr_t distance_;
-        graph::StatePtr_t state_;
-
-        public:
-        Astar (const core::RoadmapPtr_t& roadmap, const core::DistancePtr_t distance, graph::StatePtr_t state) :
-          roadmap_ (roadmap), distance_ (distance), state_ (state)
-        {
-        }
-
-        PathVectorPtr_t solution ()
-        {
-          core::NodePtr_t node = findPath ();
-          Edges_t edges;
-
-          while (node) {
-            Parent_t::const_iterator itEdge = parent_.find (node);
-            if (itEdge != parent_.end ()) {
-              EdgePtr_t edge = itEdge->second;
-              edges.push_front (edge);
-              node = edge->from ();
-            }
-            else node = RoadmapNodePtr_t (0x0);
-          }
-          PathVectorPtr_t pathVector;
-          for (Edges_t::const_iterator itEdge = edges.begin ();
-              itEdge != edges.end (); ++itEdge) {
-            const core::PathPtr_t& path ((*itEdge)->path ());
-            if (!pathVector)
-              pathVector = core::PathVector::create (path->outputSize (),
-                  path->outputDerivativeSize ());
-            pathVector->appendPath (path);
+namespace manipulation {
+namespace problemTarget {
+class HPP_MANIPULATION_LOCAL Astar {
+  typedef core::PathVectorPtr_t PathVectorPtr_t;
+  typedef core::EdgePtr_t EdgePtr_t;
+  typedef std::vector<RoadmapNodePtr_t> Nodes_t;
+  typedef std::list<EdgePtr_t> Edges_t;
+  typedef std::map<core::NodePtr_t, EdgePtr_t> Parent_t;
+  Nodes_t closed_;
+  Nodes_t open_;
+  std::map<RoadmapNodePtr_t, value_type> costFromStart_;
+  std::map<RoadmapNodePtr_t, value_type> estimatedCostToGoal_;
+  Parent_t parent_;
+  core::RoadmapPtr_t roadmap_;
+  core::DistancePtr_t distance_;
+  graph::StatePtr_t state_;
+
+ public:
+  Astar(const core::RoadmapPtr_t& roadmap, const core::DistancePtr_t distance,
+        graph::StatePtr_t state)
+      : roadmap_(roadmap), distance_(distance), state_(state) {}
+
+  PathVectorPtr_t solution() {
+    core::NodePtr_t node = findPath();
+    Edges_t edges;
+
+    while (node) {
+      Parent_t::const_iterator itEdge = parent_.find(node);
+      if (itEdge != parent_.end()) {
+        EdgePtr_t edge = itEdge->second;
+        edges.push_front(edge);
+        node = edge->from();
+      } else
+        node = RoadmapNodePtr_t(0x0);
+    }
+    PathVectorPtr_t pathVector;
+    for (Edges_t::const_iterator itEdge = edges.begin(); itEdge != edges.end();
+         ++itEdge) {
+      const core::PathPtr_t& path((*itEdge)->path());
+      if (!pathVector)
+        pathVector = core::PathVector::create(path->outputSize(),
+                                              path->outputDerivativeSize());
+      pathVector->appendPath(path);
+    }
+    return pathVector;
+  }
+
+ private:
+  struct SortFunctor {
+    std::map<RoadmapNodePtr_t, value_type>& cost_;
+    SortFunctor(std::map<RoadmapNodePtr_t, value_type>& cost) : cost_(cost) {}
+    bool operator()(const RoadmapNodePtr_t& n1, const RoadmapNodePtr_t& n2) {
+      return cost_[n1] < cost_[n2];
+    }
+  };  // struc SortFunctor
+
+  bool isGoal(const RoadmapNodePtr_t node) {
+    return node->graphState() == state_;
+  }
+
+  RoadmapNodePtr_t findPath() {
+    closed_.clear();
+    open_.clear();
+    parent_.clear();
+    estimatedCostToGoal_.clear();
+    costFromStart_.clear();
+
+    assert(dynamic_cast<RoadmapNodePtr_t>(roadmap_->initNode()));
+    open_.push_back(static_cast<RoadmapNodePtr_t>(roadmap_->initNode()));
+    while (!open_.empty()) {
+      std::sort(open_.begin(), open_.end(), SortFunctor(estimatedCostToGoal_));
+      Nodes_t::iterator itv = open_.begin();
+      RoadmapNodePtr_t current(*itv);
+      if (isGoal(current)) {
+        return current;
+      }
+      open_.erase(itv);
+      closed_.push_back(current);
+      for (Edges_t::const_iterator itEdge = current->outEdges().begin();
+           itEdge != current->outEdges().end(); ++itEdge) {
+        value_type transitionCost = edgeCost(*itEdge);
+        assert(dynamic_cast<RoadmapNodePtr_t>((*itEdge)->to()));
+        RoadmapNodePtr_t child = static_cast<RoadmapNodePtr_t>((*itEdge)->to());
+        if (std::find(closed_.begin(), closed_.end(), child) == closed_.end()) {
+          // node is not in closed set
+          value_type tmpCost = costFromStart_[current] + transitionCost;
+          bool childNotInOpenSet =
+              (std::find(open_.begin(), open_.end(), child) == open_.end());
+          if ((childNotInOpenSet) || (tmpCost < costFromStart_[child])) {
+            parent_[child] = *itEdge;
+            costFromStart_[child] = tmpCost;
+            estimatedCostToGoal_[child] =
+                costFromStart_[child] + heuristic(child);
+            if (childNotInOpenSet) open_.push_back(child);
           }
-          return pathVector;
         }
-
-        private:
-        struct SortFunctor {
-          std::map <RoadmapNodePtr_t, value_type>& cost_;
-          SortFunctor (std::map <RoadmapNodePtr_t, value_type>& cost) :
-            cost_ (cost) {}
-          bool operator () (const RoadmapNodePtr_t& n1, const RoadmapNodePtr_t& n2)
-          {
-            return cost_ [n1] < cost_ [n2];
-          }
-        }; // struc SortFunctor
-
-        bool isGoal (const RoadmapNodePtr_t node)
-        {
-          return node->graphState() == state_;
-        }
-
-        RoadmapNodePtr_t findPath ()
-        {
-          closed_.clear ();
-          open_.clear ();
-          parent_.clear ();
-          estimatedCostToGoal_.clear ();
-          costFromStart_.clear ();
-
-          assert(dynamic_cast<RoadmapNodePtr_t>(roadmap_->initNode()));
-          open_.push_back (static_cast<RoadmapNodePtr_t>(roadmap_->initNode ()));
-          while (!open_.empty ()) {
-            std::sort(open_.begin(), open_.end(), SortFunctor (estimatedCostToGoal_));
-            Nodes_t::iterator itv = open_.begin ();
-            RoadmapNodePtr_t current (*itv);
-            if (isGoal (current)) {
-              return current;
-            }
-            open_.erase (itv);
-            closed_.push_back (current);
-            for (Edges_t::const_iterator itEdge = current->outEdges ().begin ();
-                itEdge != current->outEdges ().end (); ++itEdge) {
-              value_type transitionCost = edgeCost (*itEdge);
-              assert(dynamic_cast<RoadmapNodePtr_t>((*itEdge)->to ()));
-              RoadmapNodePtr_t child = static_cast<RoadmapNodePtr_t>((*itEdge)->to ());
-              if (std::find (closed_.begin (), closed_.end (), child) ==
-                  closed_.end ()) {
-                // node is not in closed set
-                value_type tmpCost = costFromStart_ [current] + transitionCost;
-                bool childNotInOpenSet = (std::find (open_.begin (),
-                      open_.end (),
-                      child) == open_.end ());
-                if ((childNotInOpenSet) || (tmpCost < costFromStart_ [child])) {
-                  parent_ [child] = *itEdge;
-                  costFromStart_ [child] = tmpCost;
-                  estimatedCostToGoal_ [child] = costFromStart_ [child] +
-                    heuristic (child);
-                  if (childNotInOpenSet) open_.push_back (child);
-                }
-              }
-            }
-          }
-          throw std::runtime_error ("A* failed to find a solution to the goal.");
-        }
-
-        value_type heuristic (const RoadmapNodePtr_t node) const
-        {
-          const ConfigurationPtr_t config = node->configuration ();
-          value_type res = std::numeric_limits <value_type>::infinity ();
-          for (core::NodeVector_t::const_iterator itGoal = roadmap_->goalNodes ().begin ();
-              itGoal != roadmap_->goalNodes ().end (); ++itGoal) {
-            ConfigurationPtr_t goal = (*itGoal)->configuration ();
-            value_type dist = (*distance_) (*config, *goal);
-            if (dist < res) {
-              res = dist;
-            }
-          }
-          return res;
-        }
-
-        value_type edgeCost (const EdgePtr_t& edge)
-        {
-          return edge->path ()->length ();
-        }
-      }; // class Astar
-    } // namespace problemTarget
-  } //   namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_PROBLEM_TARGET_ASTAR_HH
+      }
+    }
+    throw std::runtime_error("A* failed to find a solution to the goal.");
+  }
+
+  value_type heuristic(const RoadmapNodePtr_t node) const {
+    const ConfigurationPtr_t config = node->configuration();
+    value_type res = std::numeric_limits<value_type>::infinity();
+    for (core::NodeVector_t::const_iterator itGoal =
+             roadmap_->goalNodes().begin();
+         itGoal != roadmap_->goalNodes().end(); ++itGoal) {
+      ConfigurationPtr_t goal = (*itGoal)->configuration();
+      value_type dist = (*distance_)(*config, *goal);
+      if (dist < res) {
+        res = dist;
+      }
+    }
+    return res;
+  }
+
+  value_type edgeCost(const EdgePtr_t& edge) { return edge->path()->length(); }
+};  // class Astar
+}  // namespace problemTarget
+}  //   namespace manipulation
+}  // namespace hpp
+
+#endif  // HPP_MANIPULATION_PROBLEM_TARGET_ASTAR_HH
diff --git a/src/problem-target/state.cc b/src/problem-target/state.cc
index d937474dd92b36fa648f790eb3c0c30a77acc4a9..4640b29c90e761606d65661023db91885d7ad535 100644
--- a/src/problem-target/state.cc
+++ b/src/problem-target/state.cc
@@ -26,52 +26,48 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/problem-target/state.hh>
-
-#include <stdexcept>
-
-#include <hpp/util/debug.hh>
-
-#include <hpp/core/node.hh>
 #include <hpp/core/connected-component.hh>
+#include <hpp/core/node.hh>
 #include <hpp/core/problem.hh>
 #include <hpp/core/roadmap.hh>
+#include <hpp/manipulation/problem-target/state.hh>
+#include <hpp/util/debug.hh>
+#include <stdexcept>
 
 #include "astar.hh"
 
 namespace hpp {
-  namespace manipulation {
-    namespace problemTarget {
-      StatePtr_t State::create (const core::ProblemPtr_t& problem)
-      {
-        State* tt = new State (problem);
-        StatePtr_t shPtr (tt);
-        tt->init (shPtr);
-        return shPtr;
-      }
+namespace manipulation {
+namespace problemTarget {
+StatePtr_t State::create(const core::ProblemPtr_t& problem) {
+  State* tt = new State(problem);
+  StatePtr_t shPtr(tt);
+  tt->init(shPtr);
+  return shPtr;
+}
 
-      void State::check (const core::RoadmapPtr_t&) const
-      {
-        if (!state_) {
-          HPP_THROW(std::runtime_error, "No state: task not specified.");
-        }
-      }
+void State::check(const core::RoadmapPtr_t&) const {
+  if (!state_) {
+    HPP_THROW(std::runtime_error, "No state: task not specified.");
+  }
+}
 
-      bool State::reached (const core::RoadmapPtr_t& roadmap) const
-      {
-        const core::ConnectedComponentPtr_t& _cc = roadmap->initNode()->connectedComponent();
-        const ConnectedComponentPtr_t cc = HPP_DYNAMIC_PTR_CAST(ConnectedComponent, _cc);
-        assert (cc);
-        return !cc->getRoadmapNodes(state_).empty();
-      }
+bool State::reached(const core::RoadmapPtr_t& roadmap) const {
+  const core::ConnectedComponentPtr_t& _cc =
+      roadmap->initNode()->connectedComponent();
+  const ConnectedComponentPtr_t cc =
+      HPP_DYNAMIC_PTR_CAST(ConnectedComponent, _cc);
+  assert(cc);
+  return !cc->getRoadmapNodes(state_).empty();
+}
 
-      core::PathVectorPtr_t State::computePath(const core::RoadmapPtr_t& roadmap) const
-      {
-        core::ProblemPtr_t p = problem_.lock();
-        assert (p);
-        Astar astar (roadmap, p->distance (), state_);
-        return astar.solution ();
-      }
-    } // namespace problemTarget
-  } // namespace manipulation
-} // namespace hpp
+core::PathVectorPtr_t State::computePath(
+    const core::RoadmapPtr_t& roadmap) const {
+  core::ProblemPtr_t p = problem_.lock();
+  assert(p);
+  Astar astar(roadmap, p->distance(), state_);
+  return astar.solution();
+}
+}  // namespace problemTarget
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/problem.cc b/src/problem.cc
index e22a0700403dfd7110f3b01154fbfc287b640e42..e4404db357f79bc7f776e5ccfbf9fcede58798dc 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -26,101 +26,87 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/problem.hh>
-
 #include <hpp/core/path-validation/discretized-collision-checking.hh>
-
-#include <hpp/manipulation/weighed-distance.hh>
-#include <hpp/manipulation/steering-method/graph.hh>
 #include <hpp/manipulation/graph-path-validation.hh>
 #include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/steering-method/graph.hh>
+#include <hpp/manipulation/weighed-distance.hh>
 
 namespace hpp {
-  namespace manipulation {
-    ProblemPtr_t Problem::create (DevicePtr_t robot)
-    {
-      ProblemPtr_t p (new Problem (robot));
-      p->init(p);
-      return p;
-    }
-
-    Problem::Problem (DevicePtr_t robot)
-      : Parent (robot), graph_()
-    {
-    }
-
-    void Problem::init (ProblemWkPtr_t wkPtr)
-    {
-      Parent::init (wkPtr);
-      wkPtr_ = wkPtr;
-
-      Parent::steeringMethod (steeringMethod::Graph::create (wkPtr_.lock()));
-      distance (WeighedDistance::create (HPP_DYNAMIC_PTR_CAST(Device, robot()), graph_));
-      setPathValidationFactory(core::pathValidation::createDiscretizedCollisionChecking, 0.05);
-    }
-
-    void Problem::constraintGraph (const graph::GraphPtr_t& graph)
-    {
-      graph_ = graph;
-      graph_->problem (wkPtr_.lock());
-
-      GraphPathValidationPtr_t pv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pathValidation());
-      if (pv) pv->constraintGraph (graph_);
-      WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,
-          distance ());
-      if (d) d->constraintGraph (graph);
-    }
-
-    PathValidationPtr_t Problem::pathValidation () const
-    {
-      return Parent::pathValidation();
-    }
-
-    void Problem::pathValidation (const PathValidationPtr_t& pathValidation)
-    {
-      GraphPathValidationPtr_t pv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pathValidation);
-      if (pv) pv->constraintGraph (graph_);
-      Parent::pathValidation (pathValidation);
-    }
-
-    PathValidationPtr_t Problem::pathValidationFactory () const
-    {
-      PathValidationPtr_t pv (pvFactory_ (robot(), pvTol_));
-
-      shared_ptr<core::ObstacleUserInterface> oui =
-        HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pv);
-      if (oui) {
-        const core::ObjectStdVector_t& obstacles (collisionObstacles ());
-        // Insert obstacles in path validation object
-        for (core::ObjectStdVector_t::const_iterator _obs = obstacles.begin ();
-            _obs != obstacles.end (); ++_obs)
-          oui->addObstacle (*_obs);
-      }
-      GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pv);
-      if (gpv) return gpv->innerValidation();
-      return pv;
-    }
-
-    SteeringMethodPtr_t Problem::manipulationSteeringMethod () const
-    {
-      return HPP_DYNAMIC_PTR_CAST (SteeringMethod,
-          Parent::steeringMethod());
-    }
-
-    void Problem::setPathValidationFactory (
-        const core::PathValidationBuilder_t& factory,
-        const value_type& tol)
-    {
-      pvFactory_ = factory;
-      pvTol_ = tol;
-      if (graph_) graph_->invalidate();
-    }
-
-    void Problem::checkProblem () const
-    {
-      core::Problem::checkProblem ();
-      if (!graph_)
-        throw std::runtime_error ("No graph in the problem.");
-    }
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+ProblemPtr_t Problem::create(DevicePtr_t robot) {
+  ProblemPtr_t p(new Problem(robot));
+  p->init(p);
+  return p;
+}
+
+Problem::Problem(DevicePtr_t robot) : Parent(robot), graph_() {}
+
+void Problem::init(ProblemWkPtr_t wkPtr) {
+  Parent::init(wkPtr);
+  wkPtr_ = wkPtr;
+
+  Parent::steeringMethod(steeringMethod::Graph::create(wkPtr_.lock()));
+  distance(
+      WeighedDistance::create(HPP_DYNAMIC_PTR_CAST(Device, robot()), graph_));
+  setPathValidationFactory(
+      core::pathValidation::createDiscretizedCollisionChecking, 0.05);
+}
+
+void Problem::constraintGraph(const graph::GraphPtr_t& graph) {
+  graph_ = graph;
+  graph_->problem(wkPtr_.lock());
+
+  GraphPathValidationPtr_t pv =
+      HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pathValidation());
+  if (pv) pv->constraintGraph(graph_);
+  WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST(WeighedDistance, distance());
+  if (d) d->constraintGraph(graph);
+}
+
+PathValidationPtr_t Problem::pathValidation() const {
+  return Parent::pathValidation();
+}
+
+void Problem::pathValidation(const PathValidationPtr_t& pathValidation) {
+  GraphPathValidationPtr_t pv =
+      HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pathValidation);
+  if (pv) pv->constraintGraph(graph_);
+  Parent::pathValidation(pathValidation);
+}
+
+PathValidationPtr_t Problem::pathValidationFactory() const {
+  PathValidationPtr_t pv(pvFactory_(robot(), pvTol_));
+
+  shared_ptr<core::ObstacleUserInterface> oui =
+      HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pv);
+  if (oui) {
+    const core::ObjectStdVector_t& obstacles(collisionObstacles());
+    // Insert obstacles in path validation object
+    for (core::ObjectStdVector_t::const_iterator _obs = obstacles.begin();
+         _obs != obstacles.end(); ++_obs)
+      oui->addObstacle(*_obs);
+  }
+  GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pv);
+  if (gpv) return gpv->innerValidation();
+  return pv;
+}
+
+SteeringMethodPtr_t Problem::manipulationSteeringMethod() const {
+  return HPP_DYNAMIC_PTR_CAST(SteeringMethod, Parent::steeringMethod());
+}
+
+void Problem::setPathValidationFactory(
+    const core::PathValidationBuilder_t& factory, const value_type& tol) {
+  pvFactory_ = factory;
+  pvTol_ = tol;
+  if (graph_) graph_->invalidate();
+}
+
+void Problem::checkProblem() const {
+  core::Problem::checkProblem();
+  if (!graph_) throw std::runtime_error("No graph in the problem.");
+}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/roadmap-node.cc b/src/roadmap-node.cc
index cac4a4ca5d7440df8a019c427506362e56688d7d..0d0f81c53bd949c269c9528cf2ede50e03eba6a8 100644
--- a/src/roadmap-node.cc
+++ b/src/roadmap-node.cc
@@ -31,11 +31,9 @@
 #include <hpp/manipulation/connected-component.hh>
 
 namespace hpp {
-  namespace manipulation {
-    RoadmapNode::RoadmapNode (const ConfigurationPtr_t& configuration,
-        ConnectedComponentPtr_t cc) :
-      core::Node (configuration, cc),
-      state_ ()
-    {}
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+RoadmapNode::RoadmapNode(const ConfigurationPtr_t& configuration,
+                         ConnectedComponentPtr_t cc)
+    : core::Node(configuration, cc), state_() {}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 85d4b770c50ed76e19d09a094443f0467212299d..ed24ad7e0be22c9782aece5a99ccd6cc11c0b3dc 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -26,162 +26,142 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/roadmap.hh>
-
-#include <hpp/util/pointer.hh>
-
-#include <hpp/core/edge.hh>
 #include <hpp/core/distance.hh>
-
-#include <hpp/manipulation/roadmap.hh>
-#include <hpp/manipulation/roadmap-node.hh>
-#include <hpp/manipulation/leaf-connected-comp.hh>
+#include <hpp/core/edge.hh>
 #include <hpp/manipulation/graph/state.hh>
 #include <hpp/manipulation/graph/statistics.hh>
+#include <hpp/manipulation/leaf-connected-comp.hh>
+#include <hpp/manipulation/roadmap-node.hh>
+#include <hpp/manipulation/roadmap.hh>
+#include <hpp/util/pointer.hh>
 
 namespace hpp {
-  namespace manipulation {
-    Roadmap::Roadmap (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot) :
-      core::Roadmap (distance, robot), weak_ () {}
-
-    RoadmapPtr_t Roadmap::create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot)
-    {
-      Roadmap* ptr = new Roadmap (distance, robot);
-      RoadmapPtr_t shPtr (ptr);
-      ptr->init(shPtr);
-      return shPtr;
-    }
-
-    void Roadmap::clear ()
-    {
-      Parent::clear ();
-      Histograms_t::const_iterator it;
-      for (it = histograms_.begin(); it != histograms_.end(); ++it)
-        (*it)->clear ();
-      if (graph_) {
-        const Histograms_t& hs = graph_->histograms();
-        for (it = hs.begin(); it != hs.end(); ++it)
-          (*it)->clear ();
-      }
-      leafCCs_.clear();
-    }
-
-    void Roadmap::push_node (const core::NodePtr_t& n)
-    {
-      Parent::push_node (n);
-      const RoadmapNodePtr_t& node =
-        static_cast <const RoadmapNodePtr_t> (n);
-      statInsert (node);
-      leafCCs_.insert(node->leafConnectedComponent());
+namespace manipulation {
+Roadmap::Roadmap(const core::DistancePtr_t& distance,
+                 const core::DevicePtr_t& robot)
+    : core::Roadmap(distance, robot), weak_() {}
+
+RoadmapPtr_t Roadmap::create(const core::DistancePtr_t& distance,
+                             const core::DevicePtr_t& robot) {
+  Roadmap* ptr = new Roadmap(distance, robot);
+  RoadmapPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+void Roadmap::clear() {
+  Parent::clear();
+  Histograms_t::const_iterator it;
+  for (it = histograms_.begin(); it != histograms_.end(); ++it) (*it)->clear();
+  if (graph_) {
+    const Histograms_t& hs = graph_->histograms();
+    for (it = hs.begin(); it != hs.end(); ++it) (*it)->clear();
+  }
+  leafCCs_.clear();
+}
+
+void Roadmap::push_node(const core::NodePtr_t& n) {
+  Parent::push_node(n);
+  const RoadmapNodePtr_t& node = static_cast<const RoadmapNodePtr_t>(n);
+  statInsert(node);
+  leafCCs_.insert(node->leafConnectedComponent());
+}
+
+void Roadmap::statInsert(const RoadmapNodePtr_t& n) {
+  Histograms_t::const_iterator it;
+  for (it = histograms_.begin(); it != histograms_.end(); ++it) (*it)->add(n);
+  if (graph_) {
+    const Histograms_t& hs = graph_->histograms();
+    for (it = hs.begin(); it != hs.end(); ++it) (*it)->add(n);
+  }
+}
+
+void Roadmap::insertHistogram(const graph::HistogramPtr_t hist) {
+  histograms_.push_back(hist);
+  core::Nodes_t::const_iterator _node;
+  for (_node = nodes().begin(); _node != nodes().end(); ++_node)
+    hist->add(static_cast<const RoadmapNodePtr_t>(*_node));
+}
+
+void Roadmap::constraintGraph(const graph::GraphPtr_t& graph) {
+  graph_ = graph;
+  // FIXME Add the current nodes() to the graph->histograms()
+  // The main issue is that new histograms may be added to
+  // graph->histograms() and this class will not know it.
+}
+
+RoadmapNodePtr_t Roadmap::nearestNodeInState(
+    const ConfigurationPtr_t& configuration,
+    const ConnectedComponentPtr_t& connectedComponent,
+    const graph::StatePtr_t& state, value_type& minDistance) const {
+  core::NodePtr_t result = NULL;
+  minDistance = std::numeric_limits<value_type>::infinity();
+  const RoadmapNodes_t& roadmapNodes =
+      connectedComponent->getRoadmapNodes(state);
+  // std::cout << "State: "  << state->name () << std::endl;
+  // std::cout << "roadmapNodes.size () = " << roadmapNodes.size ()
+  // 		<< std::endl;
+  for (RoadmapNodes_t::const_iterator itNode = roadmapNodes.begin();
+       itNode != roadmapNodes.end(); ++itNode) {
+    value_type d = (*distance())(*(*itNode)->configuration(), *configuration);
+    if (d < minDistance) {
+      minDistance = d;
+      result = *itNode;
     }
-
-    void Roadmap::statInsert (const RoadmapNodePtr_t& n)
-    {
-      Histograms_t::const_iterator it;
-      for (it = histograms_.begin(); it != histograms_.end(); ++it)
-        (*it)->add (n);
-      if (graph_) {
-        const Histograms_t& hs = graph_->histograms();
-        for (it = hs.begin(); it != hs.end(); ++it)
-          (*it)->add (n);
-      }
-    }
-
-    void Roadmap::insertHistogram (const graph::HistogramPtr_t hist)
-    {
-      histograms_.push_back (hist);
-      core::Nodes_t::const_iterator _node;
-      for (_node = nodes().begin(); _node != nodes().end(); ++_node)
-        hist->add (static_cast <const RoadmapNodePtr_t>(*_node));
-    }
-
-    void Roadmap::constraintGraph (const graph::GraphPtr_t& graph)
-    {
-      graph_ = graph;
-      // FIXME Add the current nodes() to the graph->histograms()
-      // The main issue is that new histograms may be added to
-      // graph->histograms() and this class will not know it.
-    }
-
-    RoadmapNodePtr_t Roadmap::nearestNodeInState (const ConfigurationPtr_t& configuration,
-        const ConnectedComponentPtr_t& connectedComponent,
-        const graph::StatePtr_t& state,
-        value_type& minDistance) const
-    {
-      core::NodePtr_t result = NULL;
-      minDistance = std::numeric_limits <value_type>::infinity ();
-      const RoadmapNodes_t& roadmapNodes = connectedComponent->getRoadmapNodes (state);
-      // std::cout << "State: "  << state->name () << std::endl;
-      // std::cout << "roadmapNodes.size () = " << roadmapNodes.size ()
-      // 		<< std::endl;
-      for (RoadmapNodes_t::const_iterator itNode = roadmapNodes.begin ();
-          itNode != roadmapNodes.end (); ++itNode) {
-        value_type d = (*distance()) (*(*itNode)->configuration (),
-            *configuration);
-        if (d < minDistance) {
-          minDistance = d;
-          result = *itNode;
-        }
-      }
-      return static_cast <RoadmapNode*> (result);
-    }
-
-    core::NodePtr_t Roadmap::createNode (const ConfigurationPtr_t& q) const
-    {
-      // call RoadmapNode constructor with new manipulation connected component
-      RoadmapNodePtr_t node = new RoadmapNode (q, ConnectedComponent::create(weak_));
-      LeafConnectedCompPtr_t sc = WeighedLeafConnectedComp::create (weak_.lock());
-      node->leafConnectedComponent (sc);
-      sc->setFirstNode(node);
-      return node;
-    }
-
-    graph::StatePtr_t Roadmap::getState(RoadmapNodePtr_t node)
-    {
-      return graph_->getState(node);
-    }
-
-    void Roadmap::connect (const LeafConnectedCompPtr_t& cc1,
-			   const LeafConnectedCompPtr_t& cc2)
-    {
-      if (cc1->canReach (cc2)) return;
-      LeafConnectedComp::LeafConnectedComps_t cc2Tocc1;
-      if (cc2->canReach (cc1, cc2Tocc1)) {
-	merge (cc1, cc2Tocc1);
-      } else {
-	cc1->to_.insert (cc2.get());
-	cc2->from_.insert (cc1.get());
-      }
-    }
-
-    void Roadmap::merge (const LeafConnectedCompPtr_t& cc1,
-			 LeafConnectedComp::LeafConnectedComps_t& ccs)
-    {
-      for (LeafConnectedComp::LeafConnectedComps_t::iterator itcc =
-             ccs.begin ();
-           itcc != ccs.end (); ++itcc) {
-	if (*itcc != cc1.get()) {
-	  cc1->merge ((*itcc)->self());
+  }
+  return static_cast<RoadmapNode*>(result);
+}
+
+core::NodePtr_t Roadmap::createNode(const ConfigurationPtr_t& q) const {
+  // call RoadmapNode constructor with new manipulation connected component
+  RoadmapNodePtr_t node = new RoadmapNode(q, ConnectedComponent::create(weak_));
+  LeafConnectedCompPtr_t sc = WeighedLeafConnectedComp::create(weak_.lock());
+  node->leafConnectedComponent(sc);
+  sc->setFirstNode(node);
+  return node;
+}
+
+graph::StatePtr_t Roadmap::getState(RoadmapNodePtr_t node) {
+  return graph_->getState(node);
+}
+
+void Roadmap::connect(const LeafConnectedCompPtr_t& cc1,
+                      const LeafConnectedCompPtr_t& cc2) {
+  if (cc1->canReach(cc2)) return;
+  LeafConnectedComp::LeafConnectedComps_t cc2Tocc1;
+  if (cc2->canReach(cc1, cc2Tocc1)) {
+    merge(cc1, cc2Tocc1);
+  } else {
+    cc1->to_.insert(cc2.get());
+    cc2->from_.insert(cc1.get());
+  }
+}
+
+void Roadmap::merge(const LeafConnectedCompPtr_t& cc1,
+                    LeafConnectedComp::LeafConnectedComps_t& ccs) {
+  for (LeafConnectedComp::LeafConnectedComps_t::iterator itcc = ccs.begin();
+       itcc != ccs.end(); ++itcc) {
+    if (*itcc != cc1.get()) {
+      cc1->merge((*itcc)->self());
 #ifndef NDEBUG
-	  std::size_t nb =
+      std::size_t nb =
 #endif
-	  leafCCs_.erase ((*itcc)->self());
-	  assert (nb == 1);
-	}
-      }
-    }
-
-    void Roadmap::impl_addEdge (const core::EdgePtr_t& edge)
-    {
-      Parent::impl_addEdge(edge);
-      const RoadmapNodePtr_t& f = static_cast <const RoadmapNodePtr_t> (edge->from());
-      const RoadmapNodePtr_t& t = static_cast <const RoadmapNodePtr_t> (edge->to());
-      if (f->graphState () == t->graphState ()) {
-        LeafConnectedCompPtr_t cc1 (f->leafConnectedComponent());
-        LeafConnectedCompPtr_t cc2 (t->leafConnectedComponent());
-
-        connect (cc1, cc2);
-      }
+          leafCCs_.erase((*itcc)->self());
+      assert(nb == 1);
     }
-  } // namespace manipulation
-} // namespace hpp
+  }
+}
+
+void Roadmap::impl_addEdge(const core::EdgePtr_t& edge) {
+  Parent::impl_addEdge(edge);
+  const RoadmapNodePtr_t& f = static_cast<const RoadmapNodePtr_t>(edge->from());
+  const RoadmapNodePtr_t& t = static_cast<const RoadmapNodePtr_t>(edge->to());
+  if (f->graphState() == t->graphState()) {
+    LeafConnectedCompPtr_t cc1(f->leafConnectedComponent());
+    LeafConnectedCompPtr_t cc2(t->leafConnectedComponent());
+
+    connect(cc1, cc2);
+  }
+}
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/serialization.cc b/src/serialization.cc
index 2f5a6b34c5802d436a6bc734a2702528460678fa..c35dfc051411cc520730f3a6f9a4d4240c8cef4b 100644
--- a/src/serialization.cc
+++ b/src/serialization.cc
@@ -27,19 +27,17 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
+#include <boost/serialization/list.hpp>
 #include <boost/serialization/serialization.hpp>
 #include <boost/serialization/set.hpp>
-#include <boost/serialization/list.hpp>
 #include <boost/serialization/weak_ptr.hpp>
-
-#include <pinocchio/serialization/eigen.hpp>
-#include <hpp/util/serialization.hh>
-
 #include <hpp/manipulation/device.hh>
 #include <hpp/manipulation/leaf-connected-comp.hh>
 #include <hpp/manipulation/roadmap-node.hh>
 #include <hpp/manipulation/roadmap.hh>
 #include <hpp/manipulation/serialization.hh>
+#include <hpp/util/serialization.hh>
+#include <pinocchio/serialization/eigen.hpp>
 
 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::manipulation::RoadmapNode)
 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::manipulation::ConnectedComponent)
@@ -50,74 +48,73 @@ namespace hpp {
 namespace manipulation {
 
 template <typename Archive>
-inline void RoadmapNode::serialize(Archive& ar, const unsigned int version)
-{
+inline void RoadmapNode::serialize(Archive& ar, const unsigned int version) {
   using namespace boost::serialization;
-  (void) version;
-  ar & make_nvp("base", base_object<core::Node>(*this));
-  ar & BOOST_SERIALIZATION_NVP(state_);
-  ar & BOOST_SERIALIZATION_NVP(leafCC_);
+  (void)version;
+  ar& make_nvp("base", base_object<core::Node>(*this));
+  ar& BOOST_SERIALIZATION_NVP(state_);
+  ar& BOOST_SERIALIZATION_NVP(leafCC_);
 }
 HPP_SERIALIZATION_IMPLEMENT(RoadmapNode);
 
 template <typename Archive>
-inline void ConnectedComponent::serialize(Archive& ar, const unsigned int version)
-{
+inline void ConnectedComponent::serialize(Archive& ar,
+                                          const unsigned int version) {
   using namespace boost::serialization;
-  (void) version;
-  ar & make_nvp("base", base_object<core::ConnectedComponent>(*this));
-  ar & BOOST_SERIALIZATION_NVP(roadmap_);
+  (void)version;
+  ar& make_nvp("base", base_object<core::ConnectedComponent>(*this));
+  ar& BOOST_SERIALIZATION_NVP(roadmap_);
   if (!Archive::is_saving::value) {
     RoadmapPtr_t roadmap = roadmap_.lock();
     for (const core::NodePtr_t& node : nodes()) {
-      const RoadmapNodePtr_t& n = static_cast <const RoadmapNodePtr_t> (node);
+      const RoadmapNodePtr_t& n = static_cast<const RoadmapNodePtr_t>(node);
       graphStateMap_[roadmap->getState(n)].push_back(n);
     }
   }
-  //ar & BOOST_SERIALIZATION_NVP(graphStateMap_);
+  // ar & BOOST_SERIALIZATION_NVP(graphStateMap_);
 }
 HPP_SERIALIZATION_IMPLEMENT(ConnectedComponent);
 
 template <typename Archive>
-inline void LeafConnectedComp::serialize(Archive& ar, const unsigned int version)
-{
-  (void) version;
-  ar & BOOST_SERIALIZATION_NVP(state_);
-  ar & BOOST_SERIALIZATION_NVP(nodes_);
+inline void LeafConnectedComp::serialize(Archive& ar,
+                                         const unsigned int version) {
+  (void)version;
+  ar& BOOST_SERIALIZATION_NVP(state_);
+  ar& BOOST_SERIALIZATION_NVP(nodes_);
 
-  //ar & BOOST_SERIALIZATION_NVP(explored_);
-  ar & BOOST_SERIALIZATION_NVP(roadmap_);
-  ar & BOOST_SERIALIZATION_NVP(to_);
-  ar & BOOST_SERIALIZATION_NVP(from_);
-  ar & BOOST_SERIALIZATION_NVP(weak_);
+  // ar & BOOST_SERIALIZATION_NVP(explored_);
+  ar& BOOST_SERIALIZATION_NVP(roadmap_);
+  ar& BOOST_SERIALIZATION_NVP(to_);
+  ar& BOOST_SERIALIZATION_NVP(from_);
+  ar& BOOST_SERIALIZATION_NVP(weak_);
 }
 HPP_SERIALIZATION_IMPLEMENT(LeafConnectedComp);
 
 template <typename Archive>
-inline void WeighedLeafConnectedComp::serialize(Archive& ar, const unsigned int version)
-{
+inline void WeighedLeafConnectedComp::serialize(Archive& ar,
+                                                const unsigned int version) {
   using namespace boost::serialization;
-  (void) version;
-  ar & make_nvp("base", base_object<LeafConnectedComp>(*this));
-  ar & BOOST_SERIALIZATION_NVP(weight_);
-  ar & BOOST_SERIALIZATION_NVP(p_);
-  ar & BOOST_SERIALIZATION_NVP(edges_);
+  (void)version;
+  ar& make_nvp("base", base_object<LeafConnectedComp>(*this));
+  ar& BOOST_SERIALIZATION_NVP(weight_);
+  ar& BOOST_SERIALIZATION_NVP(p_);
+  ar& BOOST_SERIALIZATION_NVP(edges_);
 }
 HPP_SERIALIZATION_IMPLEMENT(WeighedLeafConnectedComp);
 
 template <typename Archive>
-inline void Roadmap::serialize(Archive& ar, const unsigned int version)
-{
+inline void Roadmap::serialize(Archive& ar, const unsigned int version) {
   using namespace boost::serialization;
-  (void) version;
-  // Must deserialize the graph before the connected components (so the base class).
-  ar & BOOST_SERIALIZATION_NVP(graph_);
-  ar & BOOST_SERIALIZATION_NVP(weak_);
-  ar & make_nvp("base", base_object<core::Roadmap>(*this));
-  //ar & BOOST_SERIALIZATION_NVP(histograms_);
-  ar & BOOST_SERIALIZATION_NVP(leafCCs_);
+  (void)version;
+  // Must deserialize the graph before the connected components (so the base
+  // class).
+  ar& BOOST_SERIALIZATION_NVP(graph_);
+  ar& BOOST_SERIALIZATION_NVP(weak_);
+  ar& make_nvp("base", base_object<core::Roadmap>(*this));
+  // ar & BOOST_SERIALIZATION_NVP(histograms_);
+  ar& BOOST_SERIALIZATION_NVP(leafCCs_);
 }
 HPP_SERIALIZATION_IMPLEMENT(Roadmap);
 
-} //   namespace core
-} // namespace hpp
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 5767b9b7f6564abf408ff5cb3a151bfbef44ff68..a9ac705889ba4bad620b581f8550cf93fa88186d 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -27,672 +27,639 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/steering-method/cross-state-optimization.hh>
-
-#include <map>
-#include <queue>
-#include <vector>
-
-#include <hpp/util/exception-factory.hh>
-
-#include <pinocchio/multibody/model.hpp>
-
-#include <hpp/pinocchio/configuration.hh>
-#include <hpp/pinocchio/joint-collection.hh>
-
 #include <hpp/constraints/affine-function.hh>
+#include <hpp/constraints/explicit.hh>
 #include <hpp/constraints/locked-joint.hh>
 #include <hpp/constraints/solver/by-substitution.hh>
-#include <hpp/constraints/explicit.hh>
-
-#include <hpp/core/path-vector.hh>
 #include <hpp/core/configuration-shooter.hh>
-
+#include <hpp/core/path-vector.hh>
 #include <hpp/manipulation/graph/edge.hh>
 #include <hpp/manipulation/graph/state.hh>
+#include <hpp/manipulation/steering-method/cross-state-optimization.hh>
+#include <hpp/pinocchio/configuration.hh>
+#include <hpp/pinocchio/joint-collection.hh>
+#include <hpp/util/exception-factory.hh>
+#include <map>
+#include <pinocchio/multibody/model.hpp>
+#include <queue>
+#include <vector>
 
 namespace hpp {
-  namespace manipulation {
-    namespace steeringMethod {
-      using Eigen::RowBlockIndices;
-      using Eigen::ColBlockIndices;
-
-      using graph::StatePtr_t;
-      using graph::States_t;
-      using graph::EdgePtr_t;
-      using graph::Edges_t;
-      using graph::Neighbors_t;
-      using graph::NumericalConstraints_t;
-      using graph::LockedJoints_t;
-      using graph::segments_t;
-
-      CrossStateOptimizationPtr_t CrossStateOptimization::create (
-          const ProblemConstPtr_t& problem)
-      {
-        CrossStateOptimizationPtr_t shPtr(new CrossStateOptimization (problem));
-        shPtr->init(shPtr);
-        return shPtr;
-      }
-
-      CrossStateOptimizationPtr_t CrossStateOptimization::create (
-          const core::ProblemConstPtr_t& problem)
-      {
-        assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
-        ProblemConstPtr_t p(HPP_STATIC_PTR_CAST(const Problem, problem));
-        return create (p);
-      }
-
-      core::SteeringMethodPtr_t CrossStateOptimization::copy () const
-      {
-        CrossStateOptimization* ptr = new CrossStateOptimization (*this);
-        CrossStateOptimizationPtr_t shPtr (ptr);
-        ptr->init (shPtr);
-        return shPtr;
-      }
-
-      struct CrossStateOptimization::GraphSearchData
-      {
-        StatePtr_t s1, s2;
-
-        // Datas for findNextTransitions
-        struct state_with_depth {
-          StatePtr_t s;
-          EdgePtr_t e;
-          std::size_t l; // depth to root
-          std::size_t i; // index in parent state_with_depths_t
-          inline state_with_depth () : s(), e(), l(0), i (0) {}
-          inline state_with_depth (EdgePtr_t _e, std::size_t _l, std::size_t _i)
-            : s(_e->stateFrom()), e(_e), l(_l), i (_i) {}
-        };
-        typedef std::vector<state_with_depth> state_with_depths_t;
-        typedef std::map<StatePtr_t,state_with_depths_t> StateMap_t;
-        /// std::size_t is the index in state_with_depths_t at StateMap_t::iterator
-        struct state_with_depth_ptr_t {
-          StateMap_t::iterator state;
-          std::size_t parentIdx;
-          state_with_depth_ptr_t (const StateMap_t::iterator& it, std::size_t idx) : state (it), parentIdx (idx) {}
-        };
-        typedef std::queue<state_with_depth_ptr_t> Queue_t;
-        typedef std::set<EdgePtr_t> VisitedEdge_t;
-        std::size_t maxDepth;
-        StateMap_t parent1; // TODO, parent2;
-        Queue_t queue1;
-        VisitedEdge_t visitedEdge_;
-
-        const state_with_depth& getParent(const state_with_depth_ptr_t& _p) const
-        {
-          const state_with_depths_t& parents = _p.state->second;
-          return parents[_p.parentIdx];
-        }
-
-        state_with_depth_ptr_t addInitState()
-        {
-          StateMap_t::iterator next =
-            parent1.insert(StateMap_t::value_type(s1, state_with_depths_t(1))).first;
-          return state_with_depth_ptr_t (next, 0);
-        }
-
-        state_with_depth_ptr_t addParent(
-            const state_with_depth_ptr_t& _p,
-            const EdgePtr_t& transition)
-        {
-          const state_with_depths_t& parents = _p.state->second;
-          const state_with_depth& from = parents[_p.parentIdx];
-
-          // Insert state to if necessary
-          StateMap_t::iterator next = parent1.insert (
-              StateMap_t::value_type(
-                transition->stateTo(), state_with_depths_t ()
-                )).first;
-
-          next->second.push_back (
-              state_with_depth(transition, from.l + 1, _p.parentIdx));
-
-          return state_with_depth_ptr_t (next, next->second.size()-1);
-        }
-      };
-
-      void CrossStateOptimization::gatherGraphConstraints ()
-      {
-        typedef graph::Edge Edge;
-        typedef graph::EdgePtr_t EdgePtr_t;
-        typedef graph::GraphPtr_t GraphPtr_t;
-        typedef constraints::solver::BySubstitution Solver_t;
-
-        GraphPtr_t cg (problem_->constraintGraph ());
-        const ConstraintsAndComplements_t& cac
-          (cg->constraintsAndComplements ());
-        for (std::size_t i = 0; i < cg->nbComponents (); ++i) {
-          EdgePtr_t edge (HPP_DYNAMIC_PTR_CAST (Edge, cg->get (i).lock ()));
-          if (edge) {
-            const Solver_t& solver (edge->pathConstraint ()->
-                                    configProjector ()->solver ());
-            const NumericalConstraints_t& constraints
-              (solver.numericalConstraints ());
-            for (NumericalConstraints_t::const_iterator it
-                   (constraints.begin ()); it != constraints.end (); ++it) {
-              if ((*it)->parameterSize () > 0) {
-                const std::string& name ((*it)->function ().name  ());
-                if (index_.find (name) == index_.end ()) {
-                  // constraint is not in map, add it
-                  index_ [name] = constraints_.size ();
-                  // Check whether constraint is equivalent to a previous one
-                  for (NumericalConstraints_t::const_iterator it1
-                         (constraints_.begin ()); it1 != constraints_.end ();
-                       ++it1) {
-                    for (ConstraintsAndComplements_t::const_iterator it2
-                           (cac.begin ()); it2 != cac.end (); ++it2) {
-                      if (((**it1 == *(it2->complement)) &&
-                           (**it == *(it2->both))) ||
-                          ((**it1 == *(it2->both)) &&
-                           (**it == *(it2->complement)))) {
-                        assert (sameRightHandSide_.count (*it1) == 0);
-                        assert (sameRightHandSide_.count (*it) == 0);
-                        sameRightHandSide_ [*it1] = *it;
-                        sameRightHandSide_ [*it] = *it1;
-                      }
-                    }
-                  }
-                  constraints_.push_back (*it);
-                  hppDout (info, "Adding constraint \"" << name << "\"");
-                  hppDout (info, "Edge \"" << edge->name () << "\"");
-                  hppDout (info, "parameter size: " << (*it)->parameterSize ());
-
+namespace manipulation {
+namespace steeringMethod {
+using Eigen::ColBlockIndices;
+using Eigen::RowBlockIndices;
+
+using graph::EdgePtr_t;
+using graph::Edges_t;
+using graph::LockedJoints_t;
+using graph::Neighbors_t;
+using graph::NumericalConstraints_t;
+using graph::segments_t;
+using graph::StatePtr_t;
+using graph::States_t;
+
+CrossStateOptimizationPtr_t CrossStateOptimization::create(
+    const ProblemConstPtr_t& problem) {
+  CrossStateOptimizationPtr_t shPtr(new CrossStateOptimization(problem));
+  shPtr->init(shPtr);
+  return shPtr;
+}
+
+CrossStateOptimizationPtr_t CrossStateOptimization::create(
+    const core::ProblemConstPtr_t& problem) {
+  assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+  ProblemConstPtr_t p(HPP_STATIC_PTR_CAST(const Problem, problem));
+  return create(p);
+}
+
+core::SteeringMethodPtr_t CrossStateOptimization::copy() const {
+  CrossStateOptimization* ptr = new CrossStateOptimization(*this);
+  CrossStateOptimizationPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+struct CrossStateOptimization::GraphSearchData {
+  StatePtr_t s1, s2;
+
+  // Datas for findNextTransitions
+  struct state_with_depth {
+    StatePtr_t s;
+    EdgePtr_t e;
+    std::size_t l;  // depth to root
+    std::size_t i;  // index in parent state_with_depths_t
+    inline state_with_depth() : s(), e(), l(0), i(0) {}
+    inline state_with_depth(EdgePtr_t _e, std::size_t _l, std::size_t _i)
+        : s(_e->stateFrom()), e(_e), l(_l), i(_i) {}
+  };
+  typedef std::vector<state_with_depth> state_with_depths_t;
+  typedef std::map<StatePtr_t, state_with_depths_t> StateMap_t;
+  /// std::size_t is the index in state_with_depths_t at StateMap_t::iterator
+  struct state_with_depth_ptr_t {
+    StateMap_t::iterator state;
+    std::size_t parentIdx;
+    state_with_depth_ptr_t(const StateMap_t::iterator& it, std::size_t idx)
+        : state(it), parentIdx(idx) {}
+  };
+  typedef std::queue<state_with_depth_ptr_t> Queue_t;
+  typedef std::set<EdgePtr_t> VisitedEdge_t;
+  std::size_t maxDepth;
+  StateMap_t parent1;  // TODO, parent2;
+  Queue_t queue1;
+  VisitedEdge_t visitedEdge_;
+
+  const state_with_depth& getParent(const state_with_depth_ptr_t& _p) const {
+    const state_with_depths_t& parents = _p.state->second;
+    return parents[_p.parentIdx];
+  }
+
+  state_with_depth_ptr_t addInitState() {
+    StateMap_t::iterator next =
+        parent1.insert(StateMap_t::value_type(s1, state_with_depths_t(1)))
+            .first;
+    return state_with_depth_ptr_t(next, 0);
+  }
+
+  state_with_depth_ptr_t addParent(const state_with_depth_ptr_t& _p,
+                                   const EdgePtr_t& transition) {
+    const state_with_depths_t& parents = _p.state->second;
+    const state_with_depth& from = parents[_p.parentIdx];
+
+    // Insert state to if necessary
+    StateMap_t::iterator next =
+        parent1
+            .insert(StateMap_t::value_type(transition->stateTo(),
+                                           state_with_depths_t()))
+            .first;
+
+    next->second.push_back(
+        state_with_depth(transition, from.l + 1, _p.parentIdx));
+
+    return state_with_depth_ptr_t(next, next->second.size() - 1);
+  }
+};
+
+void CrossStateOptimization::gatherGraphConstraints() {
+  typedef graph::Edge Edge;
+  typedef graph::EdgePtr_t EdgePtr_t;
+  typedef graph::GraphPtr_t GraphPtr_t;
+  typedef constraints::solver::BySubstitution Solver_t;
+
+  GraphPtr_t cg(problem_->constraintGraph());
+  const ConstraintsAndComplements_t& cac(cg->constraintsAndComplements());
+  for (std::size_t i = 0; i < cg->nbComponents(); ++i) {
+    EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(Edge, cg->get(i).lock()));
+    if (edge) {
+      const Solver_t& solver(
+          edge->pathConstraint()->configProjector()->solver());
+      const NumericalConstraints_t& constraints(solver.numericalConstraints());
+      for (NumericalConstraints_t::const_iterator it(constraints.begin());
+           it != constraints.end(); ++it) {
+        if ((*it)->parameterSize() > 0) {
+          const std::string& name((*it)->function().name());
+          if (index_.find(name) == index_.end()) {
+            // constraint is not in map, add it
+            index_[name] = constraints_.size();
+            // Check whether constraint is equivalent to a previous one
+            for (NumericalConstraints_t::const_iterator it1(
+                     constraints_.begin());
+                 it1 != constraints_.end(); ++it1) {
+              for (ConstraintsAndComplements_t::const_iterator it2(cac.begin());
+                   it2 != cac.end(); ++it2) {
+                if (((**it1 == *(it2->complement)) && (**it == *(it2->both))) ||
+                    ((**it1 == *(it2->both)) && (**it == *(it2->complement)))) {
+                  assert(sameRightHandSide_.count(*it1) == 0);
+                  assert(sameRightHandSide_.count(*it) == 0);
+                  sameRightHandSide_[*it1] = *it;
+                  sameRightHandSide_[*it] = *it1;
                 }
               }
             }
+            constraints_.push_back(*it);
+            hppDout(info, "Adding constraint \"" << name << "\"");
+            hppDout(info, "Edge \"" << edge->name() << "\"");
+            hppDout(info, "parameter size: " << (*it)->parameterSize());
           }
         }
       }
-
-      bool CrossStateOptimization::findTransitions (GraphSearchData& d) const
-      {
-        while (! d.queue1.empty())
-        {
-          GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
-
-          const GraphSearchData::state_with_depth& parent = d.getParent(_state);
-          if (parent.l >= d.maxDepth) return true;
-          d.queue1.pop();
-
-          bool done = false;
-
-          const Neighbors_t& neighbors = _state.state->first->neighbors();
-          for (Neighbors_t::const_iterator _n = neighbors.begin();
-              _n != neighbors.end(); ++_n) {
-            EdgePtr_t transition = _n->second;
-
-            // Avoid identical consecutive transition
-            if (transition == parent.e) continue;
-
-            // If transition has already been visited, continue
-            // if (d.visitedEdge_.count (transition) == 1) continue;
-
-            // TODO
-            // If (transition->to() == d.s2) check if this list is feasible.
-            // - If a constraint with non-constant right hand side is present
-            //   in all transitions, then the rhs from d.q1 and d.q2 should be
-            //   equal
-
-            // Insert parent
-            d.queue1.push (
-                d.addParent (_state, transition)
-                );
-
-            done = done || (transition->stateTo() == d.s2);
-          }
-          if (done) break;
-        }
-        return false;
+    }
+  }
+}
+
+bool CrossStateOptimization::findTransitions(GraphSearchData& d) const {
+  while (!d.queue1.empty()) {
+    GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
+
+    const GraphSearchData::state_with_depth& parent = d.getParent(_state);
+    if (parent.l >= d.maxDepth) return true;
+    d.queue1.pop();
+
+    bool done = false;
+
+    const Neighbors_t& neighbors = _state.state->first->neighbors();
+    for (Neighbors_t::const_iterator _n = neighbors.begin();
+         _n != neighbors.end(); ++_n) {
+      EdgePtr_t transition = _n->second;
+
+      // Avoid identical consecutive transition
+      if (transition == parent.e) continue;
+
+      // If transition has already been visited, continue
+      // if (d.visitedEdge_.count (transition) == 1) continue;
+
+      // TODO
+      // If (transition->to() == d.s2) check if this list is feasible.
+      // - If a constraint with non-constant right hand side is present
+      //   in all transitions, then the rhs from d.q1 and d.q2 should be
+      //   equal
+
+      // Insert parent
+      d.queue1.push(d.addParent(_state, transition));
+
+      done = done || (transition->stateTo() == d.s2);
+    }
+    if (done) break;
+  }
+  return false;
+}
+
+Edges_t CrossStateOptimization::getTransitionList(GraphSearchData& d,
+                                                  const std::size_t& i) const {
+  assert(d.parent1.find(d.s2) != d.parent1.end());
+  const GraphSearchData::state_with_depths_t& roots = d.parent1[d.s2];
+  Edges_t transitions;
+  if (i >= roots.size()) return transitions;
+
+  const GraphSearchData::state_with_depth* current = &roots[i];
+  transitions.reserve(current->l);
+  graph::WaypointEdgePtr_t we;
+  while (current->e) {
+    assert(current->l > 0);
+    we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, current->e);
+    if (we) {
+      for (int i = (int)we->nbWaypoints(); i >= 0; --i)
+        transitions.push_back(we->waypoint(i));
+    } else {
+      transitions.push_back(current->e);
+    }
+    current = &d.parent1[current->s][current->i];
+  }
+  std::reverse(transitions.begin(), transitions.end());
+  return transitions;
+}
+
+namespace internal {
+bool saturate(const core::DevicePtr_t& robot, vectorIn_t q, vectorOut_t qSat,
+              pinocchio::ArrayXb& saturatedDof) {
+  qSat = q;
+  return hpp::pinocchio::saturate(robot, qSat, saturatedDof);
+}
+}  // namespace internal
+
+struct CrossStateOptimization::OptimizationData {
+  typedef constraints::solver::HierarchicalIterative::Saturation_t Saturation_t;
+  enum RightHandSideStatus_t {
+    // Constraint is not in solver for this waypoint
+    ABSENT,
+    // right hand side of constraint for this waypoint is equal to
+    // right hand side for previous waypoint
+    EQUAL_TO_PREVIOUS,
+    // right hand side of constraint for this waypoint is equal to
+    // right hand side for initial configuration
+    EQUAL_TO_INIT,
+    // right hand side of constraint for this waypoint is equal to
+    // right hand side for goal configuration
+    EQUAL_TO_GOAL
+  };  // enum RightHandSideStatus_t
+  const std::size_t N, nq, nv;
+  std::vector<Solver_t> solvers;
+  // Waypoints lying in each intermediate state
+  matrix_t waypoint;
+  // Initial guess of each solver stored as matrix columns
+  matrix_t qInit;
+  Configuration_t q1, q2;
+  core::DevicePtr_t robot;
+  // Matrix specifying for each constraint and each waypoint how
+  // the right hand side is initialized in the solver.
+  Eigen::Matrix<LiegroupElement, Eigen::Dynamic, Eigen::Dynamic> M_rhs;
+  Eigen::Matrix<RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic> M_status;
+  // Number of trials to generate each waypoint configuration
+  OptimizationData(const core::ProblemConstPtr_t _problem,
+                   const Configuration_t& _q1, const Configuration_t& _q2,
+                   const Edges_t& transitions)
+      : N(transitions.size() - 1),
+        nq(_problem->robot()->configSize()),
+        nv(_problem->robot()->numberDof()),
+        solvers(N, _problem->robot()->configSpace()),
+        waypoint(nq, N),
+        qInit(nq, N),
+        q1(_q1),
+        q2(_q2),
+        robot(_problem->robot()),
+        M_rhs(),
+        M_status() {
+    for (auto solver : solvers) {
+      // Set maximal number of iterations for each solver
+      solver.maxIterations(
+          _problem->getParameter("CrossStateOptimization/maxIteration")
+              .intValue());
+      // Set error threshold for each solver
+      solver.errorThreshold(
+          _problem->getParameter("CrossStateOptimization/errorThreshold")
+              .floatValue());
+    }
+    assert(transitions.size() > 0);
+  }
+};
+
+bool CrossStateOptimization::checkConstantRightHandSide(OptimizationData& d,
+                                                        size_type index) const {
+  const ImplicitPtr_t c(constraints_[index]);
+  LiegroupElement rhsInit(c->function().outputSpace());
+  c->rightHandSideFromConfig(d.q1, rhsInit);
+  LiegroupElement rhsGoal(c->function().outputSpace());
+  c->rightHandSideFromConfig(d.q2, rhsGoal);
+  // Check that right hand sides are close to each other
+  value_type eps(problem_->constraintGraph()->errorThreshold());
+  value_type eps2(eps * eps);
+  if ((rhsGoal - rhsInit).squaredNorm() > eps2) {
+    return false;
+  }
+  // Matrix of solver right hand sides
+  for (size_type j = 0; j < d.M_rhs.cols(); ++j) {
+    d.M_rhs(index, j) = rhsInit;
+  }
+  return true;
+}
+
+void displayRhsMatrix(
+    const Eigen::Matrix<vector_t, Eigen::Dynamic, Eigen::Dynamic>& m,
+    const NumericalConstraints_t& constraints) {
+  std::ostringstream oss;
+  oss.precision(5);
+  oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+  oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+  oss << "\\begin {document}" << std::endl;
+  oss << "\\begin {tabular}{";
+  for (size_type j = 0; j < m.cols() + 1; ++j) oss << "c";
+  oss << "}" << std::endl;
+  for (size_type i = 0; i < m.rows(); ++i) {
+    oss << constraints[i]->function().name() << " & ";
+    for (size_type j = 0; j < m.cols(); ++j) {
+      oss << "$\\left(\\begin{array}{c}" << std::endl;
+      for (size_type k = 0; k < m(i, j).size(); ++k) {
+        oss << m(i, j)[k] << "\\\\" << std::endl;
       }
-
-      Edges_t CrossStateOptimization::getTransitionList (
-          GraphSearchData& d, const std::size_t& i) const
-      {
-        assert (d.parent1.find (d.s2) != d.parent1.end());
-        const GraphSearchData::state_with_depths_t& roots = d.parent1[d.s2];
-        Edges_t transitions;
-        if (i >= roots.size()) return transitions;
-
-        const GraphSearchData::state_with_depth* current = &roots[i];
-        transitions.reserve (current->l);
-        graph::WaypointEdgePtr_t we;
-        while (current->e) {
-          assert (current->l > 0);
-          we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, current->e);
-          if (we) {
-            for (int i = (int)we->nbWaypoints(); i >= 0; --i)
-              transitions.push_back(we->waypoint(i));
-          } else {
-            transitions.push_back(current->e);
-          }
-          current = &d.parent1[current->s][current->i];
-        }
-        std::reverse (transitions.begin(), transitions.end());
-        return transitions;
-      }
-
-      namespace internal {
-        bool saturate (const core::DevicePtr_t& robot, vectorIn_t q,
-                       vectorOut_t qSat, pinocchio::ArrayXb& saturatedDof)
-        {
-          qSat = q;
-          return hpp::pinocchio::saturate (robot, qSat, saturatedDof);
-        }
-      } // namespace internal
-
-      struct CrossStateOptimization::OptimizationData
-      {
-        typedef constraints::solver::HierarchicalIterative::Saturation_t
-        Saturation_t;
-        enum RightHandSideStatus_t {
-          // Constraint is not in solver for this waypoint
-          ABSENT,
-          // right hand side of constraint for this waypoint is equal to
-          // right hand side for previous waypoint
-          EQUAL_TO_PREVIOUS,
-          // right hand side of constraint for this waypoint is equal to
-          // right hand side for initial configuration
-          EQUAL_TO_INIT,
-          // right hand side of constraint for this waypoint is equal to
-          // right hand side for goal configuration
-          EQUAL_TO_GOAL
-        }; // enum RightHandSideStatus_t
-        const std::size_t N, nq, nv;
-        std::vector <Solver_t> solvers;
-        // Waypoints lying in each intermediate state
-        matrix_t waypoint;
-        // Initial guess of each solver stored as matrix columns
-        matrix_t qInit;
-        Configuration_t q1, q2;
-        core::DevicePtr_t robot;
-        // Matrix specifying for each constraint and each waypoint how
-        // the right hand side is initialized in the solver.
-        Eigen::Matrix < LiegroupElement, Eigen::Dynamic, Eigen::Dynamic > M_rhs;
-        Eigen::Matrix < RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >
-        M_status;
-        // Number of trials to generate each waypoint configuration
-        OptimizationData (const core::ProblemConstPtr_t _problem,
-                          const Configuration_t& _q1,
-                          const Configuration_t& _q2,
-                          const Edges_t& transitions
-                          ) :
-          N (transitions.size () - 1), nq (_problem->robot()->configSize()),
-          nv (_problem->robot()->numberDof()),
-          solvers (N, _problem->robot()->configSpace ()),
-          waypoint (nq, N), qInit (nq, N), q1 (_q1), q2 (_q2),
-          robot (_problem->robot()),
-          M_rhs (), M_status ()
-        {
-          for (auto solver: solvers){
-            // Set maximal number of iterations for each solver
-            solver.maxIterations(_problem->getParameter
-                            ("CrossStateOptimization/maxIteration").intValue());
-            // Set error threshold for each solver
-            solver.errorThreshold(_problem->getParameter
-                        ("CrossStateOptimization/errorThreshold").floatValue());
-          }
-          assert (transitions.size () > 0);
-        }
-      };
-
-      bool CrossStateOptimization::checkConstantRightHandSide
-      (OptimizationData& d, size_type index) const
-      {
-        const ImplicitPtr_t c (constraints_ [index]);
-        LiegroupElement rhsInit(c->function().outputSpace());
-        c->rightHandSideFromConfig (d.q1, rhsInit);
-        LiegroupElement rhsGoal(c->function().outputSpace());
-        c->rightHandSideFromConfig (d.q2, rhsGoal);
-        // Check that right hand sides are close to each other
-        value_type eps (problem_->constraintGraph ()->errorThreshold ());
-        value_type eps2 (eps * eps);
-        if ((rhsGoal - rhsInit).squaredNorm () > eps2) {
-          return false;
-        }
-        // Matrix of solver right hand sides
-        for (size_type j=0; j<d.M_rhs.cols (); ++j) {
-          d.M_rhs (index, j) = rhsInit;
-        }
-        return true;
-      }
-
-      void displayRhsMatrix (const Eigen::Matrix < vector_t, Eigen::Dynamic,
-                             Eigen::Dynamic >& m,
-                             const NumericalConstraints_t& constraints)
-      {
-        std::ostringstream oss; oss.precision (5);
-        oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
-        oss << "\\usepackage[a3paper]{geometry}" << std::endl;
-        oss << "\\begin {document}" << std::endl;
-        oss << "\\begin {tabular}{";
-        for (size_type j=0; j<m.cols () + 1; ++j)
-          oss << "c";
-        oss << "}" << std::endl;
-        for (size_type i=0; i<m.rows (); ++i) {
-          oss << constraints [i]->function ().name () << " & ";
-          for (size_type j=0; j<m.cols (); ++j) {
-            oss << "$\\left(\\begin{array}{c}" << std::endl;
-            for (size_type k=0; k<m (i,j).size (); ++k) {
-              oss << m (i,j) [k] << "\\\\" << std::endl;
-            }
-            oss << "\\end{array}\\right)$" << std::endl;
-            if (j < m.cols () - 1) {
-              oss << " & " << std::endl;
-            }
-          }
-          oss << "\\\\" << std::endl;
-        }
-        oss << "\\end{tabular}" << std::endl;
-        oss << "\\end {document}" << std::endl;
-        hppDout (info, oss.str ());
+      oss << "\\end{array}\\right)$" << std::endl;
+      if (j < m.cols() - 1) {
+        oss << " & " << std::endl;
       }
-
-      void displayStatusMatrix (const Eigen::Matrix < CrossStateOptimization::OptimizationData::RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >& m,
-                                const NumericalConstraints_t& constraints,
-                                const graph::Edges_t& transitions)
-      {
-        std::ostringstream oss; oss.precision (5);
-        oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
-        oss << "\\usepackage[a3paper]{geometry}" << std::endl;
-        oss << "\\begin {document}" << std::endl;
-        oss << "\\paragraph{Edges}" << std::endl;
-        oss << "\\begin{enumerate}" << std::endl;
-        for (auto edge : transitions) {
-          oss << "\\item " << edge->name() << std::endl;
-        }
-        oss << "\\end{enumerate}" << std::endl;
-        oss << "\\begin {tabular}{";
-        for (size_type j=0; j<m.cols () + 1; ++j)
-          oss << "c";
-        oss << "}" << std::endl;
-        for (size_type i=0; i<m.rows (); ++i) {
-          oss << constraints [i]->function ().name () << " & ";
-          for (size_type j=0; j<m.cols (); ++j) {
-            oss << m (i,j);
-            if (j < m.cols () - 1) {
-              oss << " & " << std::endl;
-            }
-          }
-          oss << "\\\\" << std::endl;
-        }
-        oss << "\\end{tabular}" << std::endl;
-        oss << "\\end {document}" << std::endl;
-        hppDout (info, oss.str ());
-      }
-
-      bool CrossStateOptimization::contains
-      (const Solver_t& solver, const ImplicitPtr_t& c) const
-      {
-        if (solver.contains (c)) return true;
-        std::map <ImplicitPtr_t, ImplicitPtr_t>::const_iterator it
-          (sameRightHandSide_.find (c));
-        if ((it != sameRightHandSide_.end () &&
-             solver.contains (it->second))) {
-          return true;
-        }
-        return false;
+    }
+    oss << "\\\\" << std::endl;
+  }
+  oss << "\\end{tabular}" << std::endl;
+  oss << "\\end {document}" << std::endl;
+  hppDout(info, oss.str());
+}
+
+void displayStatusMatrix(
+    const Eigen::Matrix<
+        CrossStateOptimization::OptimizationData::RightHandSideStatus_t,
+        Eigen::Dynamic, Eigen::Dynamic>& m,
+    const NumericalConstraints_t& constraints,
+    const graph::Edges_t& transitions) {
+  std::ostringstream oss;
+  oss.precision(5);
+  oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+  oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+  oss << "\\begin {document}" << std::endl;
+  oss << "\\paragraph{Edges}" << std::endl;
+  oss << "\\begin{enumerate}" << std::endl;
+  for (auto edge : transitions) {
+    oss << "\\item " << edge->name() << std::endl;
+  }
+  oss << "\\end{enumerate}" << std::endl;
+  oss << "\\begin {tabular}{";
+  for (size_type j = 0; j < m.cols() + 1; ++j) oss << "c";
+  oss << "}" << std::endl;
+  for (size_type i = 0; i < m.rows(); ++i) {
+    oss << constraints[i]->function().name() << " & ";
+    for (size_type j = 0; j < m.cols(); ++j) {
+      oss << m(i, j);
+      if (j < m.cols() - 1) {
+        oss << " & " << std::endl;
       }
-
-      bool CrossStateOptimization::buildOptimizationProblem
-      (OptimizationData& d, const graph::Edges_t& transitions) const
-      {
-        if (d.N == 0) return true;
-        d.M_status.resize (constraints_.size (), d.N);
-        d.M_status.fill (OptimizationData::ABSENT);
-        d.M_rhs.resize (constraints_.size (), d.N);
-        d.M_rhs.fill (LiegroupElement ());
-        size_type index = 0;
-        // Loop over constraints
-        for (NumericalConstraints_t::const_iterator it (constraints_.begin ());
-             it != constraints_.end (); ++it) {
-          const ImplicitPtr_t& c (*it);
-          // Loop forward over waypoints to determine right hand sides equal
-          // to initial configuration
-          for (std::size_t j = 0; j < d.N; ++j) {
-            // Get transition solver
-            const Solver_t& trSolver
-              (transitions [j]->pathConstraint ()->configProjector ()->
-               solver ());
-            if (contains (trSolver, c)) {
-              if ((j==0) || d.M_status (index, j-1) ==
-                  OptimizationData::EQUAL_TO_INIT) {
-                d.M_status (index, j) = OptimizationData::EQUAL_TO_INIT;
-              } else {
-                d.M_status (index, j) = OptimizationData::EQUAL_TO_PREVIOUS;
-              }
-            }
-          }
-          // Loop backward over waypoints to determine right hand sides equal
-          // to final configuration
-          for (size_type j = d.N-1; j > 0; --j) {
-            // Get transition solver
-            const Solver_t& trSolver
-              (transitions [(std::size_t)j+1]->pathConstraint ()->
-               configProjector ()->solver ());
-            if (contains (trSolver, c)) {
-              if ((j==(size_type) d.N-1) || d.M_status (index, j+1) ==
-                  OptimizationData::EQUAL_TO_GOAL) {
-                // If constraint right hand side is already equal to
-                // initial config, check that right hand side is equal
-                // for init and goal configs.
-                if (d.M_status (index, j) ==
-                    OptimizationData::EQUAL_TO_INIT) {
-                  if (checkConstantRightHandSide (d, index)) {
-                    // stop for this constraint
-                    break;
-                  } else {
-                    // Right hand side of constraint should be equal along the
-                    // whole path but is different at init and at goal configs.
-                    return false;
-                  }
-                } else {
-                  d.M_status (index, j) = OptimizationData::EQUAL_TO_GOAL;
-                }
-              }
-            }
-          }
-          ++index;
-        } // for (NumericalConstraints_t::const_iterator it
-        displayStatusMatrix (d.M_status, constraints_, transitions);
-        graph::GraphPtr_t cg (problem_->constraintGraph ());
-        // Fill solvers with target constraints of transition
-        for (std::size_t j = 0; j < d.N; ++j) {
-          d.solvers [(std::size_t)j] = transitions [(std::size_t)j]->
-            targetConstraint()->configProjector ()->solver ();
-        }
-        // Initial guess
-        std::vector<size_type> ks;
-        size_type K = 0;
-        ks.resize(d.N);
-        for (std::size_t i = 0; i < d.N + 1; ++i) {
-          if (!transitions[i]->isShort()) ++K;
-          if (i < d.N) ks[i] = K;
+    }
+    oss << "\\\\" << std::endl;
+  }
+  oss << "\\end{tabular}" << std::endl;
+  oss << "\\end {document}" << std::endl;
+  hppDout(info, oss.str());
+}
+
+bool CrossStateOptimization::contains(const Solver_t& solver,
+                                      const ImplicitPtr_t& c) const {
+  if (solver.contains(c)) return true;
+  std::map<ImplicitPtr_t, ImplicitPtr_t>::const_iterator it(
+      sameRightHandSide_.find(c));
+  if ((it != sameRightHandSide_.end() && solver.contains(it->second))) {
+    return true;
+  }
+  return false;
+}
+
+bool CrossStateOptimization::buildOptimizationProblem(
+    OptimizationData& d, const graph::Edges_t& transitions) const {
+  if (d.N == 0) return true;
+  d.M_status.resize(constraints_.size(), d.N);
+  d.M_status.fill(OptimizationData::ABSENT);
+  d.M_rhs.resize(constraints_.size(), d.N);
+  d.M_rhs.fill(LiegroupElement());
+  size_type index = 0;
+  // Loop over constraints
+  for (NumericalConstraints_t::const_iterator it(constraints_.begin());
+       it != constraints_.end(); ++it) {
+    const ImplicitPtr_t& c(*it);
+    // Loop forward over waypoints to determine right hand sides equal
+    // to initial configuration
+    for (std::size_t j = 0; j < d.N; ++j) {
+      // Get transition solver
+      const Solver_t& trSolver(
+          transitions[j]->pathConstraint()->configProjector()->solver());
+      if (contains(trSolver, c)) {
+        if ((j == 0) ||
+            d.M_status(index, j - 1) == OptimizationData::EQUAL_TO_INIT) {
+          d.M_status(index, j) = OptimizationData::EQUAL_TO_INIT;
+        } else {
+          d.M_status(index, j) = OptimizationData::EQUAL_TO_PREVIOUS;
         }
-        if (K==0) {
-          ++K;
-          for (std::size_t i = d.N/2; i < d.N; ++i)
-            ks[i] = 1;
-        }
-        for (std::size_t i = 0; i < d.N; ++i) {
-          value_type u = value_type(ks[i]) / value_type(K);
-          pinocchio::interpolate (d.robot, d.q1, d.q2, u, d.qInit.col (i));
-          hppDout (info, "qInit = " << pinocchio::displayConfig
-                   (d.qInit.col (i)));
-        }
-
-        return true;
       }
-
-      bool CrossStateOptimization::solveOptimizationProblem
-      (OptimizationData& d) const
-      {
-        // Iterate on waypoint solvers, for each of them
-        //  1. initialize right hand side,
-        //  2. solve.
-        for (std::size_t j=0; j<d.solvers.size (); ++j) {
-          Solver_t& solver (d.solvers [j]);
-          for (std::size_t i=0; i<constraints_.size (); ++i) {
-            const ImplicitPtr_t& c (constraints_ [i]);
-              switch (d.M_status ((size_type)i, (size_type)j)) {
-            case OptimizationData::EQUAL_TO_PREVIOUS:
-              assert (j != 0);
-              solver.rightHandSideFromConfig (c, d.waypoint.col (j-1));
-              break;
-            case OptimizationData::EQUAL_TO_INIT:
-              solver.rightHandSideFromConfig (c, d.q1);
+    }
+    // Loop backward over waypoints to determine right hand sides equal
+    // to final configuration
+    for (size_type j = d.N - 1; j > 0; --j) {
+      // Get transition solver
+      const Solver_t& trSolver(transitions[(std::size_t)j + 1]
+                                   ->pathConstraint()
+                                   ->configProjector()
+                                   ->solver());
+      if (contains(trSolver, c)) {
+        if ((j == (size_type)d.N - 1) ||
+            d.M_status(index, j + 1) == OptimizationData::EQUAL_TO_GOAL) {
+          // If constraint right hand side is already equal to
+          // initial config, check that right hand side is equal
+          // for init and goal configs.
+          if (d.M_status(index, j) == OptimizationData::EQUAL_TO_INIT) {
+            if (checkConstantRightHandSide(d, index)) {
+              // stop for this constraint
               break;
-            case OptimizationData::EQUAL_TO_GOAL:
-              solver.rightHandSideFromConfig (c, d.q2);
-              break;
-            case OptimizationData::ABSENT:
-            default:
-              ;
+            } else {
+              // Right hand side of constraint should be equal along the
+              // whole path but is different at init and at goal configs.
+              return false;
             }
-          }
-          if (j == 0)
-            d.waypoint.col (j) = d.qInit.col (j);
-          else
-            d.waypoint.col (j) = d.waypoint.col (j-1);
-          Solver_t::Status status = solver.solve
-            (d.waypoint.col (j),
-             constraints::solver::lineSearch::Backtracking ());
-          size_type nbTry=0;
-          size_type nRandomConfigs(problem()->getParameter
-                         ("CrossStateOptimization/nRandomConfigs").intValue());
-
-          while(status != Solver_t::SUCCESS && nbTry < nRandomConfigs){
-            d.waypoint.col (j) = *(problem()->configurationShooter()->shoot());
-            status = solver.solve
-            (d.waypoint.col (j),
-             constraints::solver::lineSearch::Backtracking ());
-            ++nbTry;
-          }
-          switch (status) {
-          case Solver_t::ERROR_INCREASED:
-            hppDout (info, "error increased.");
-            return false;
-          case Solver_t::MAX_ITERATION_REACHED:
-            hppDout (info, "max iteration reached.");
-            return false;
-          case Solver_t::INFEASIBLE:
-            hppDout (info, "infeasible.");
-            return false;
-          case Solver_t::SUCCESS:
-            hppDout (info, "success.");
+          } else {
+            d.M_status(index, j) = OptimizationData::EQUAL_TO_GOAL;
           }
         }
-        return true;
       }
-
-      core::PathVectorPtr_t CrossStateOptimization::buildPath (
-          OptimizationData& d, const Edges_t& transitions) const
-      {
-        using core::PathVector;
-        using core::PathVectorPtr_t;
-
-        const core::DevicePtr_t& robot = problem()->robot();
-        PathVectorPtr_t pv = PathVector::create (
-            robot->configSize(), robot->numberDof());
-        core::PathPtr_t path;
-
-        std::size_t i = 0;
-        for (Edges_t::const_iterator _t = transitions.begin();
-            _t != transitions.end(); ++_t)
-        {
-          const EdgePtr_t& t = *_t;
-          bool first = (i == 0);
-          bool last  = (i == d.N);
-
-          bool status;
-          if (first && last)
-            status = t->build (path, d.q1, d.q2);
-          else if (first)
-            status = t->build (path, d.q1, d.waypoint.col (i));
-          else if (last)
-            status = t->build (path, d.waypoint.col (i-1), d.q2);
-          else {
-            status = t->build (path, d.waypoint.col (i-1), d.waypoint.col (i));
-          }
-
-          if (!status || !path) {
-            hppDout (warning, "Could not build path from solution ");
-            return PathVectorPtr_t();
-          }
-          pv->appendPath(path);
-
-          ++i;
-        }
-        return pv;
+    }
+    ++index;
+  }  // for (NumericalConstraints_t::const_iterator it
+  displayStatusMatrix(d.M_status, constraints_, transitions);
+  graph::GraphPtr_t cg(problem_->constraintGraph());
+  // Fill solvers with target constraints of transition
+  for (std::size_t j = 0; j < d.N; ++j) {
+    d.solvers[(std::size_t)j] = transitions[(std::size_t)j]
+                                    ->targetConstraint()
+                                    ->configProjector()
+                                    ->solver();
+  }
+  // Initial guess
+  std::vector<size_type> ks;
+  size_type K = 0;
+  ks.resize(d.N);
+  for (std::size_t i = 0; i < d.N + 1; ++i) {
+    if (!transitions[i]->isShort()) ++K;
+    if (i < d.N) ks[i] = K;
+  }
+  if (K == 0) {
+    ++K;
+    for (std::size_t i = d.N / 2; i < d.N; ++i) ks[i] = 1;
+  }
+  for (std::size_t i = 0; i < d.N; ++i) {
+    value_type u = value_type(ks[i]) / value_type(K);
+    pinocchio::interpolate(d.robot, d.q1, d.q2, u, d.qInit.col(i));
+    hppDout(info, "qInit = " << pinocchio::displayConfig(d.qInit.col(i)));
+  }
+
+  return true;
+}
+
+bool CrossStateOptimization::solveOptimizationProblem(
+    OptimizationData& d) const {
+  // Iterate on waypoint solvers, for each of them
+  //  1. initialize right hand side,
+  //  2. solve.
+  for (std::size_t j = 0; j < d.solvers.size(); ++j) {
+    Solver_t& solver(d.solvers[j]);
+    for (std::size_t i = 0; i < constraints_.size(); ++i) {
+      const ImplicitPtr_t& c(constraints_[i]);
+      switch (d.M_status((size_type)i, (size_type)j)) {
+        case OptimizationData::EQUAL_TO_PREVIOUS:
+          assert(j != 0);
+          solver.rightHandSideFromConfig(c, d.waypoint.col(j - 1));
+          break;
+        case OptimizationData::EQUAL_TO_INIT:
+          solver.rightHandSideFromConfig(c, d.q1);
+          break;
+        case OptimizationData::EQUAL_TO_GOAL:
+          solver.rightHandSideFromConfig(c, d.q2);
+          break;
+        case OptimizationData::ABSENT:
+        default:;
       }
-
-      core::PathPtr_t CrossStateOptimization::impl_compute (
-          ConfigurationIn_t q1, ConfigurationIn_t q2) const
-      {
-        const graph::GraphPtr_t& graph(problem_->constraintGraph ());
-        GraphSearchData d;
-        d.s1 = graph->getState (q1);
-        d.s2 = graph->getState (q2);
-        // d.maxDepth = 2;
-        d.maxDepth = problem_->getParameter
-	  ("CrossStateOptimization/maxDepth").intValue();
-
-        // Find
-        d.queue1.push (d.addInitState());
-        std::size_t idxSol = (d.s1 == d.s2 ? 1 : 0);
-        bool maxDepthReached = findTransitions (d);
-
-        while (!maxDepthReached) {
-          Edges_t transitions = getTransitionList (d, idxSol);
-          while (! transitions.empty()) {
+    }
+    if (j == 0)
+      d.waypoint.col(j) = d.qInit.col(j);
+    else
+      d.waypoint.col(j) = d.waypoint.col(j - 1);
+    Solver_t::Status status = solver.solve(
+        d.waypoint.col(j), constraints::solver::lineSearch::Backtracking());
+    size_type nbTry = 0;
+    size_type nRandomConfigs(
+        problem()
+            ->getParameter("CrossStateOptimization/nRandomConfigs")
+            .intValue());
+
+    while (status != Solver_t::SUCCESS && nbTry < nRandomConfigs) {
+      d.waypoint.col(j) = *(problem()->configurationShooter()->shoot());
+      status = solver.solve(d.waypoint.col(j),
+                            constraints::solver::lineSearch::Backtracking());
+      ++nbTry;
+    }
+    switch (status) {
+      case Solver_t::ERROR_INCREASED:
+        hppDout(info, "error increased.");
+        return false;
+      case Solver_t::MAX_ITERATION_REACHED:
+        hppDout(info, "max iteration reached.");
+        return false;
+      case Solver_t::INFEASIBLE:
+        hppDout(info, "infeasible.");
+        return false;
+      case Solver_t::SUCCESS:
+        hppDout(info, "success.");
+    }
+  }
+  return true;
+}
+
+core::PathVectorPtr_t CrossStateOptimization::buildPath(
+    OptimizationData& d, const Edges_t& transitions) const {
+  using core::PathVector;
+  using core::PathVectorPtr_t;
+
+  const core::DevicePtr_t& robot = problem()->robot();
+  PathVectorPtr_t pv =
+      PathVector::create(robot->configSize(), robot->numberDof());
+  core::PathPtr_t path;
+
+  std::size_t i = 0;
+  for (Edges_t::const_iterator _t = transitions.begin();
+       _t != transitions.end(); ++_t) {
+    const EdgePtr_t& t = *_t;
+    bool first = (i == 0);
+    bool last = (i == d.N);
+
+    bool status;
+    if (first && last)
+      status = t->build(path, d.q1, d.q2);
+    else if (first)
+      status = t->build(path, d.q1, d.waypoint.col(i));
+    else if (last)
+      status = t->build(path, d.waypoint.col(i - 1), d.q2);
+    else {
+      status = t->build(path, d.waypoint.col(i - 1), d.waypoint.col(i));
+    }
+
+    if (!status || !path) {
+      hppDout(warning, "Could not build path from solution ");
+      return PathVectorPtr_t();
+    }
+    pv->appendPath(path);
+
+    ++i;
+  }
+  return pv;
+}
+
+core::PathPtr_t CrossStateOptimization::impl_compute(
+    ConfigurationIn_t q1, ConfigurationIn_t q2) const {
+  const graph::GraphPtr_t& graph(problem_->constraintGraph());
+  GraphSearchData d;
+  d.s1 = graph->getState(q1);
+  d.s2 = graph->getState(q2);
+  // d.maxDepth = 2;
+  d.maxDepth =
+      problem_->getParameter("CrossStateOptimization/maxDepth").intValue();
+
+  // Find
+  d.queue1.push(d.addInitState());
+  std::size_t idxSol = (d.s1 == d.s2 ? 1 : 0);
+  bool maxDepthReached = findTransitions(d);
+
+  while (!maxDepthReached) {
+    Edges_t transitions = getTransitionList(d, idxSol);
+    while (!transitions.empty()) {
 #ifdef HPP_DEBUG
-            std::ostringstream ss;
-            ss << "Trying solution " << idxSol << ": ";
-            for (std::size_t j = 0; j < transitions.size(); ++j)
-              ss << transitions[j]->name() << ", ";
-            hppDout (info, ss.str());
-#endif // HPP_DEBUG
-
-            OptimizationData optData (problem(), q1, q2, transitions);
-            if (buildOptimizationProblem (optData, transitions)) {
-              if (solveOptimizationProblem (optData)) {
-                core::PathPtr_t path = buildPath (optData, transitions);
-                if (path) return path;
-                hppDout (info, "Failed to build path from solution: ");
-              } else {
-                hppDout (info, "Failed to solve");
-              }
-            }
-            ++idxSol;
-            transitions = getTransitionList(d, idxSol);
-          }
-          maxDepthReached = findTransitions (d);
+      std::ostringstream ss;
+      ss << "Trying solution " << idxSol << ": ";
+      for (std::size_t j = 0; j < transitions.size(); ++j)
+        ss << transitions[j]->name() << ", ";
+      hppDout(info, ss.str());
+#endif  // HPP_DEBUG
+
+      OptimizationData optData(problem(), q1, q2, transitions);
+      if (buildOptimizationProblem(optData, transitions)) {
+        if (solveOptimizationProblem(optData)) {
+          core::PathPtr_t path = buildPath(optData, transitions);
+          if (path) return path;
+          hppDout(info, "Failed to build path from solution: ");
+        } else {
+          hppDout(info, "Failed to solve");
         }
-
-        return core::PathPtr_t ();
       }
-
-      using core::Parameter;
-      using core::ParameterDescription;
-
-      HPP_START_PARAMETER_DECLARATION(CrossStateOptimization)
-      core::Problem::declareParameter(ParameterDescription(Parameter::INT,
-            "CrossStateOptimization/maxDepth",
-            "Maximum number of transitions to look for.",
-            Parameter((size_type)2)));
-      core::Problem::declareParameter(ParameterDescription(Parameter::INT,
-            "CrossStateOptimization/maxIteration",
-            "Maximum number of iterations of the Newton Raphson algorithm.",
-            Parameter((size_type)60)));
-      core::Problem::declareParameter(ParameterDescription(Parameter::FLOAT,
-            "CrossStateOptimization/errorThreshold",
-            "Error threshold of the Newton Raphson algorithm.",
-            Parameter(1e-4)));
-      core::Problem::declareParameter(ParameterDescription(Parameter::INT,
-            "CrossStateOptimization/nRandomConfigs",
-            "Number of random configurations to sample to initialize each "
-            "solver.", Parameter((size_type)0)));
-      HPP_END_PARAMETER_DECLARATION(CrossStateOptimization)
-    } // namespace steeringMethod
-  } // namespace manipulation
-} // namespace hpp
+      ++idxSol;
+      transitions = getTransitionList(d, idxSol);
+    }
+    maxDepthReached = findTransitions(d);
+  }
+
+  return core::PathPtr_t();
+}
+
+using core::Parameter;
+using core::ParameterDescription;
+
+HPP_START_PARAMETER_DECLARATION(CrossStateOptimization)
+core::Problem::declareParameter(ParameterDescription(
+    Parameter::INT, "CrossStateOptimization/maxDepth",
+    "Maximum number of transitions to look for.", Parameter((size_type)2)));
+core::Problem::declareParameter(ParameterDescription(
+    Parameter::INT, "CrossStateOptimization/maxIteration",
+    "Maximum number of iterations of the Newton Raphson algorithm.",
+    Parameter((size_type)60)));
+core::Problem::declareParameter(ParameterDescription(
+    Parameter::FLOAT, "CrossStateOptimization/errorThreshold",
+    "Error threshold of the Newton Raphson algorithm.", Parameter(1e-4)));
+core::Problem::declareParameter(ParameterDescription(
+    Parameter::INT, "CrossStateOptimization/nRandomConfigs",
+    "Number of random configurations to sample to initialize each "
+    "solver.",
+    Parameter((size_type)0)));
+HPP_END_PARAMETER_DECLARATION(CrossStateOptimization)
+}  // namespace steeringMethod
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/steering-method/end-effector-trajectory.cc b/src/steering-method/end-effector-trajectory.cc
index b87d0c0e4d8127df0ff337d341046c1c0536f294..a85bbfd34bc9db683313b79d721443feeb4d127c 100644
--- a/src/steering-method/end-effector-trajectory.cc
+++ b/src/steering-method/end-effector-trajectory.cc
@@ -26,210 +26,196 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
-
-#include <hpp/util/indent.hh>
-
-#include <hpp/pinocchio/util.hh>
-#include <hpp/pinocchio/device.hh>
-#include <hpp/pinocchio/urdf/util.hh>
-
 #include <hpp/constraints/differentiable-function.hh>
 #include <hpp/constraints/implicit.hh>
-
 #include <hpp/core/config-projector.hh>
 #include <hpp/core/interpolated-path.hh>
-#include <hpp/core/path.hh>
 #include <hpp/core/path-vector.hh>
+#include <hpp/core/path.hh>
 #include <hpp/core/problem.hh>
 #include <hpp/core/straight-path.hh>
+#include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
+#include <hpp/pinocchio/device.hh>
+#include <hpp/pinocchio/urdf/util.hh>
+#include <hpp/pinocchio/util.hh>
+#include <hpp/util/indent.hh>
 
 namespace hpp {
-  namespace manipulation {
-    namespace steeringMethod {
-      namespace {
-        template <bool SE3>
-        class FunctionFromPath : public constraints::DifferentiableFunction
-        {
-          public:
-            FunctionFromPath (const PathPtr_t& p)
-              : DifferentiableFunction (1, 1, SE3 ? LiegroupSpace::R3xSO3() : LiegroupSpace::Rn(p->outputSize())),
-              path_ (p)
-            {
-              assert (!SE3 ||
-                  (p->outputSize() == 7 && p->outputDerivativeSize() == 6));
-            }
-
-            const PathPtr_t& path () const
-            {
-              return path_;
-            }
-
-            std::ostream& print (std::ostream& os) const
-            {
-              return os
-                << (SE3 ? "FunctionFromSE3Path: " : "FunctionFromPath: ")
-                << path_->timeRange().first << ", "
-                << path_->timeRange().second
-                ;
-            }
-
-          protected:
-            void impl_compute (core::LiegroupElementRef result, vectorIn_t arg) const
-            {
-              bool success = (*path_) (result.vector(), arg[0]);
-              if (!success) {
-                hppDout (warning, "Failed to evaluate path at param " << arg[0]
-                    << incindent << iendl << *path_ << decindent);
-              }
-            }
-
-            void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const
-            {
-              path_->derivative (jacobian.col(0), arg[0], 1);
-            }
-
-          private:
-            PathPtr_t path_;
-        };
-      }
-
-      PathPtr_t EndEffectorTrajectory::makePiecewiseLinearTrajectory (
-          matrixIn_t points,
-          vectorIn_t weights)
-      {
-        if (points.cols() != 7)
-          throw std::invalid_argument("The input matrix should have 7 columns");
-        if (weights.size() != 6)
-          throw std::invalid_argument("The weights vector should have 6 elements");
-        LiegroupSpacePtr_t se3 = LiegroupSpace::SE3();
-        core::PathVectorPtr_t path = core::PathVector::create(7, 6);
-        if (points.rows() == 1)
-          path->appendPath (core::StraightPath::create (se3,
-              points.row(0),
-              points.row(0), interval_t(0,0)));
-        else
-          for (size_type i = 1; i < points.rows(); ++i) {
-            value_type d =
-              ( se3->elementConstRef(points.row(i))
-                - se3->elementConstRef(points.row(i-1))
-              ).cwiseProduct(weights).norm();
-            path->appendPath (core::StraightPath::create (se3,
-                  points.row(i-1),
-                  points.row(i),
-                  interval_t(0, d)));
-          }
-        return path;
-      }
-
-      void EndEffectorTrajectory::trajectoryConstraint (const constraints::ImplicitPtr_t& ic)
-      {
-        constraint_ = ic;
-        if (eeTraj_) trajectory (eeTraj_, timeRange_);
-      }
-
-      void EndEffectorTrajectory::trajectory (const PathPtr_t& eeTraj, bool se3Output)
-      {
-        if (se3Output)
-          trajectory (DifferentiableFunctionPtr_t
-              (new FunctionFromPath <true > (eeTraj)), eeTraj->timeRange());
-        else
-          trajectory (DifferentiableFunctionPtr_t
-              (new FunctionFromPath <false> (eeTraj)), eeTraj->timeRange());
-      }
-
-      void EndEffectorTrajectory::trajectory (const DifferentiableFunctionPtr_t& eeTraj, const interval_t& tr)
-      {
-        assert (eeTraj->inputSize() == 1);
-        assert (eeTraj->inputDerivativeSize() == 1);
-        eeTraj_ = eeTraj;
-        timeRange_ = tr;
-
-        if (!constraint_) return;
-
-        constraint_->rightHandSideFunction (eeTraj_);
-      }
-
-      PathPtr_t EndEffectorTrajectory::impl_compute (ConfigurationIn_t q1,
-          ConfigurationIn_t q2) const
-      {
-        try {
-        core::ConstraintSetPtr_t c (getUpdatedConstraints());
-
-        return core::StraightPath::create
-	  (problem()->robot(), q1, q2, timeRange_, c);
-        } catch (const std::exception& e) {
-          std::cout << timeRange_.first << ", " << timeRange_.second << '\n';
-          if (eeTraj_)
-            std::cout << (*eeTraj_) (vector_t::Constant(1,timeRange_.first )) << '\n'
-                      << (*eeTraj_) (vector_t::Constant(1,timeRange_.second)) << std::endl;
-          std::cout << *constraints() << std::endl;
-          std::cout << e.what() << std::endl;
-          throw;
-        }
-      }
-
-      PathPtr_t EndEffectorTrajectory::projectedPath (vectorIn_t times,
-          matrixIn_t configs) const
-      {
-        core::ConstraintSetPtr_t c (getUpdatedConstraints());
-
-        size_type N = configs.cols();
-        if (timeRange_.first != times[0] || timeRange_.second != times[N-1]) {
-          HPP_THROW (std::logic_error, "Time range (" << timeRange_.first <<
-              ", " << timeRange_.second << ") does not match configuration "
-              "times (" << times[0] << ", " << times[N-1]);
-        }
-
-        using core::InterpolatedPath;
-        using core::InterpolatedPathPtr_t;
-
-        InterpolatedPathPtr_t path = InterpolatedPath::create
-	  (problem()->robot(), configs.col(0), configs.col(N-1), timeRange_, c);
-
-        for (size_type i = 1; i < configs.cols()-1; ++i)
-          path->insert(times[i], configs.col(i));
-
-        return path;
-      }
-
-      core::ConstraintSetPtr_t EndEffectorTrajectory::getUpdatedConstraints () const
-      {
-        if (!eeTraj_) throw std::logic_error ("EndEffectorTrajectory not initialized.");
-
-        // Update (or check) the constraints
-        core::ConstraintSetPtr_t c (constraints());
-        if (!c || !c->configProjector()) {
-          throw std::logic_error ("EndEffectorTrajectory steering method must "
-              "have a ConfigProjector");
-        }
-        ConfigProjectorPtr_t cp (c->configProjector());
-
-        const core::NumericalConstraints_t& ncs = cp->numericalConstraints();
-        bool ok = false;
-        for (std::size_t i = 0; i < ncs.size(); ++i) {
-          if (ncs[i] == constraint_) {
-            ok = true;
-            break; // Same pointer
-          }
-          // Here, we do not check the right hand side on purpose.
-          // if (*ncs[i] == *constraint_) {
-          if (ncs[i]->functionPtr() == constraint_->functionPtr()
-              && ncs[i]->comparisonType() == constraint_->comparisonType()) {
-            ok = true;
-            // TODO We should only modify the path constraint.
-            // However, only the pointers to implicit constraints are copied
-            // while we would like the implicit constraints to be copied as well.
-            ncs[i]->rightHandSideFunction (eeTraj_);
-            break; // logically identical
-          }
-        }
-        if (!ok) {
-          HPP_THROW (std::logic_error, "EndEffectorTrajectory could not find "
-              "constraint " << constraint_->function());
-        }
-        return c;
-        }
-    } // namespace steeringMethod
-  } // namespace manipulation
-} // namespace hpp
+namespace manipulation {
+namespace steeringMethod {
+namespace {
+template <bool SE3>
+class FunctionFromPath : public constraints::DifferentiableFunction {
+ public:
+  FunctionFromPath(const PathPtr_t& p)
+      : DifferentiableFunction(
+            1, 1,
+            SE3 ? LiegroupSpace::R3xSO3() : LiegroupSpace::Rn(p->outputSize())),
+        path_(p) {
+    assert(!SE3 || (p->outputSize() == 7 && p->outputDerivativeSize() == 6));
+  }
+
+  const PathPtr_t& path() const { return path_; }
+
+  std::ostream& print(std::ostream& os) const {
+    return os << (SE3 ? "FunctionFromSE3Path: " : "FunctionFromPath: ")
+              << path_->timeRange().first << ", " << path_->timeRange().second;
+  }
+
+ protected:
+  void impl_compute(core::LiegroupElementRef result, vectorIn_t arg) const {
+    bool success = (*path_)(result.vector(), arg[0]);
+    if (!success) {
+      hppDout(warning, "Failed to evaluate path at param "
+                           << arg[0] << incindent << iendl << *path_
+                           << decindent);
+    }
+  }
+
+  void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const {
+    path_->derivative(jacobian.col(0), arg[0], 1);
+  }
+
+ private:
+  PathPtr_t path_;
+};
+}  // namespace
+
+PathPtr_t EndEffectorTrajectory::makePiecewiseLinearTrajectory(
+    matrixIn_t points, vectorIn_t weights) {
+  if (points.cols() != 7)
+    throw std::invalid_argument("The input matrix should have 7 columns");
+  if (weights.size() != 6)
+    throw std::invalid_argument("The weights vector should have 6 elements");
+  LiegroupSpacePtr_t se3 = LiegroupSpace::SE3();
+  core::PathVectorPtr_t path = core::PathVector::create(7, 6);
+  if (points.rows() == 1)
+    path->appendPath(core::StraightPath::create(
+        se3, points.row(0), points.row(0), interval_t(0, 0)));
+  else
+    for (size_type i = 1; i < points.rows(); ++i) {
+      value_type d = (se3->elementConstRef(points.row(i)) -
+                      se3->elementConstRef(points.row(i - 1)))
+                         .cwiseProduct(weights)
+                         .norm();
+      path->appendPath(core::StraightPath::create(
+          se3, points.row(i - 1), points.row(i), interval_t(0, d)));
+    }
+  return path;
+}
+
+void EndEffectorTrajectory::trajectoryConstraint(
+    const constraints::ImplicitPtr_t& ic) {
+  constraint_ = ic;
+  if (eeTraj_) trajectory(eeTraj_, timeRange_);
+}
+
+void EndEffectorTrajectory::trajectory(const PathPtr_t& eeTraj,
+                                       bool se3Output) {
+  if (se3Output)
+    trajectory(DifferentiableFunctionPtr_t(new FunctionFromPath<true>(eeTraj)),
+               eeTraj->timeRange());
+  else
+    trajectory(DifferentiableFunctionPtr_t(new FunctionFromPath<false>(eeTraj)),
+               eeTraj->timeRange());
+}
+
+void EndEffectorTrajectory::trajectory(
+    const DifferentiableFunctionPtr_t& eeTraj, const interval_t& tr) {
+  assert(eeTraj->inputSize() == 1);
+  assert(eeTraj->inputDerivativeSize() == 1);
+  eeTraj_ = eeTraj;
+  timeRange_ = tr;
+
+  if (!constraint_) return;
+
+  constraint_->rightHandSideFunction(eeTraj_);
+}
+
+PathPtr_t EndEffectorTrajectory::impl_compute(ConfigurationIn_t q1,
+                                              ConfigurationIn_t q2) const {
+  try {
+    core::ConstraintSetPtr_t c(getUpdatedConstraints());
+
+    return core::StraightPath::create(problem()->robot(), q1, q2, timeRange_,
+                                      c);
+  } catch (const std::exception& e) {
+    std::cout << timeRange_.first << ", " << timeRange_.second << '\n';
+    if (eeTraj_)
+      std::cout << (*eeTraj_)(vector_t::Constant(1, timeRange_.first)) << '\n'
+                << (*eeTraj_)(vector_t::Constant(1, timeRange_.second))
+                << std::endl;
+    std::cout << *constraints() << std::endl;
+    std::cout << e.what() << std::endl;
+    throw;
+  }
+}
+
+PathPtr_t EndEffectorTrajectory::projectedPath(vectorIn_t times,
+                                               matrixIn_t configs) const {
+  core::ConstraintSetPtr_t c(getUpdatedConstraints());
+
+  size_type N = configs.cols();
+  if (timeRange_.first != times[0] || timeRange_.second != times[N - 1]) {
+    HPP_THROW(std::logic_error,
+              "Time range (" << timeRange_.first << ", " << timeRange_.second
+                             << ") does not match configuration "
+                                "times ("
+                             << times[0] << ", " << times[N - 1]);
+  }
+
+  using core::InterpolatedPath;
+  using core::InterpolatedPathPtr_t;
+
+  InterpolatedPathPtr_t path = InterpolatedPath::create(
+      problem()->robot(), configs.col(0), configs.col(N - 1), timeRange_, c);
+
+  for (size_type i = 1; i < configs.cols() - 1; ++i)
+    path->insert(times[i], configs.col(i));
+
+  return path;
+}
+
+core::ConstraintSetPtr_t EndEffectorTrajectory::getUpdatedConstraints() const {
+  if (!eeTraj_)
+    throw std::logic_error("EndEffectorTrajectory not initialized.");
+
+  // Update (or check) the constraints
+  core::ConstraintSetPtr_t c(constraints());
+  if (!c || !c->configProjector()) {
+    throw std::logic_error(
+        "EndEffectorTrajectory steering method must "
+        "have a ConfigProjector");
+  }
+  ConfigProjectorPtr_t cp(c->configProjector());
+
+  const core::NumericalConstraints_t& ncs = cp->numericalConstraints();
+  bool ok = false;
+  for (std::size_t i = 0; i < ncs.size(); ++i) {
+    if (ncs[i] == constraint_) {
+      ok = true;
+      break;  // Same pointer
+    }
+    // Here, we do not check the right hand side on purpose.
+    // if (*ncs[i] == *constraint_) {
+    if (ncs[i]->functionPtr() == constraint_->functionPtr() &&
+        ncs[i]->comparisonType() == constraint_->comparisonType()) {
+      ok = true;
+      // TODO We should only modify the path constraint.
+      // However, only the pointers to implicit constraints are copied
+      // while we would like the implicit constraints to be copied as well.
+      ncs[i]->rightHandSideFunction(eeTraj_);
+      break;  // logically identical
+    }
+  }
+  if (!ok) {
+    HPP_THROW(std::logic_error,
+              "EndEffectorTrajectory could not find "
+              "constraint "
+                  << constraint_->function());
+  }
+  return c;
+}
+}  // namespace steeringMethod
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc
index 4c6b7340ba83dc4aec2d0a93d9d5f5f135345bc3..44263794d88bef07cc9711b646fc3fdf514f05f5 100644
--- a/src/steering-method/graph.cc
+++ b/src/steering-method/graph.cc
@@ -28,93 +28,80 @@
 
 #include "hpp/manipulation/steering-method/graph.hh"
 
-#include <hpp/util/pointer.hh>
-
-#include <hpp/core/straight-path.hh>
 #include <hpp/core/steering-method/straight.hh>
-
-#include <hpp/manipulation/problem.hh>
-#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/core/straight-path.hh>
 #include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/problem.hh>
+#include <hpp/util/pointer.hh>
 
 namespace hpp {
-  namespace manipulation {
-    SteeringMethod::SteeringMethod (const ProblemConstPtr_t& problem) :
-      core::SteeringMethod (problem), problem_ (problem),
-      steeringMethod_ (core::steeringMethod::Straight::create (problem))
-    {
-    }
+namespace manipulation {
+SteeringMethod::SteeringMethod(const ProblemConstPtr_t& problem)
+    : core::SteeringMethod(problem),
+      problem_(problem),
+      steeringMethod_(core::steeringMethod::Straight::create(problem)) {}
 
-    SteeringMethod::SteeringMethod (const SteeringMethod& other):
-      core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_
-      (other.steeringMethod_)
-    {
-    }
+SteeringMethod::SteeringMethod(const SteeringMethod& other)
+    : core::SteeringMethod(other),
+      problem_(other.problem_),
+      steeringMethod_(other.steeringMethod_) {}
 
-    namespace steeringMethod {
+namespace steeringMethod {
 
-      GraphPtr_t Graph::create
-        (const core::ProblemConstPtr_t& problem)
-        {
-          assert(HPP_DYNAMIC_PTR_CAST (const Problem, problem));
-          ProblemConstPtr_t p = HPP_STATIC_PTR_CAST(const Problem, problem);
-          Graph* ptr = new Graph (p);
-          GraphPtr_t shPtr (ptr);
-          ptr->init (shPtr);
-          return shPtr;
-        }
+GraphPtr_t Graph::create(const core::ProblemConstPtr_t& problem) {
+  assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+  ProblemConstPtr_t p = HPP_STATIC_PTR_CAST(const Problem, problem);
+  Graph* ptr = new Graph(p);
+  GraphPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
 
-      GraphPtr_t Graph::createCopy
-        (const GraphPtr_t& other)
-        {
-          Graph* ptr = new Graph (*other);
-          GraphPtr_t shPtr (ptr);
-          ptr->init (shPtr);
-          return shPtr;
-        }
+GraphPtr_t Graph::createCopy(const GraphPtr_t& other) {
+  Graph* ptr = new Graph(*other);
+  GraphPtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
 
-      Graph::Graph (const ProblemConstPtr_t& problem) :
-        SteeringMethod (problem), weak_ ()
-      {
-      }
+Graph::Graph(const ProblemConstPtr_t& problem)
+    : SteeringMethod(problem), weak_() {}
 
-      Graph::Graph (const Graph& other):
-        SteeringMethod (other)
-      {
-      }
+Graph::Graph(const Graph& other) : SteeringMethod(other) {}
 
-      PathPtr_t Graph::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
-      {
-        graph::Edges_t possibleEdges;
-        // If q1 and q2 are the same, call the problem steering method between
-        // them
-        if (q1 == q2) {
-          core::SteeringMethodPtr_t sm
-            (problem_->manipulationSteeringMethod()->innerSteeringMethod());
-          return (*sm) (q1, q2);
-        }
-        if (!problem_->constraintGraph())
-          throw std::invalid_argument ("The constraint graph should be set to use the steeringMethod::Graph");
-        const graph::Graph& graph = *(problem_->constraintGraph ());
-        try {
-          possibleEdges = graph.getEdges
-            (graph.getState (q1), graph.getState (q2));
-        } catch (const std::logic_error& e) {
-          hppDout (error, e.what ());
-          return PathPtr_t ();
-        }
-        PathPtr_t path;
-        if (possibleEdges.empty()) {
-          hppDout (info, "No edge found.");
-        }
-        while (!possibleEdges.empty()) {
-          if (possibleEdges.back ()->build (path, q1, q2)) {
-            return path;
-          }
-          possibleEdges.pop_back ();
-        }
-        return PathPtr_t ();
-      }
-    } // namespace steeringMethod
-  } // namespace manipulation
-} // namespace hpp
+PathPtr_t Graph::impl_compute(ConfigurationIn_t q1,
+                              ConfigurationIn_t q2) const {
+  graph::Edges_t possibleEdges;
+  // If q1 and q2 are the same, call the problem steering method between
+  // them
+  if (q1 == q2) {
+    core::SteeringMethodPtr_t sm(
+        problem_->manipulationSteeringMethod()->innerSteeringMethod());
+    return (*sm)(q1, q2);
+  }
+  if (!problem_->constraintGraph())
+    throw std::invalid_argument(
+        "The constraint graph should be set to use the steeringMethod::Graph");
+  const graph::Graph& graph = *(problem_->constraintGraph());
+  try {
+    possibleEdges = graph.getEdges(graph.getState(q1), graph.getState(q2));
+  } catch (const std::logic_error& e) {
+    hppDout(error, e.what());
+    return PathPtr_t();
+  }
+  PathPtr_t path;
+  if (possibleEdges.empty()) {
+    hppDout(info, "No edge found.");
+  }
+  while (!possibleEdges.empty()) {
+    if (possibleEdges.back()->build(path, q1, q2)) {
+      return path;
+    }
+    possibleEdges.pop_back();
+  }
+  return PathPtr_t();
+}
+}  // namespace steeringMethod
+}  // namespace manipulation
+}  // namespace hpp
diff --git a/src/weighed-distance.cc b/src/weighed-distance.cc
index 1590fbda0988b38a21c6d09214818c6396f38526..f3a0e980b2edc6af15f11a01e532aa18cc2eb8f1 100644
--- a/src/weighed-distance.cc
+++ b/src/weighed-distance.cc
@@ -26,109 +26,92 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/manipulation/weighed-distance.hh>
-
 #include <boost/serialization/serialization.hpp>
 #include <boost/serialization/weak_ptr.hpp>
-
-#include <hpp/util/debug.hh>
-#include <hpp/util/serialization.hh>
-#include <hpp/pinocchio/serialization.hh>
-
 #include <hpp/manipulation/device.hh>
-#include <hpp/manipulation/roadmap-node.hh>
-#include <hpp/manipulation/graph/graph.hh>
 #include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/roadmap-node.hh>
 #include <hpp/manipulation/serialization.hh>
+#include <hpp/manipulation/weighed-distance.hh>
+#include <hpp/pinocchio/serialization.hh>
+#include <hpp/util/debug.hh>
+#include <hpp/util/serialization.hh>
 
 namespace hpp {
-  namespace manipulation {
-    WeighedDistancePtr_t WeighedDistance::create
-      (const DevicePtr_t& robot, const graph::GraphPtr_t& graph)
-    {
-      WeighedDistance* ptr = new WeighedDistance (robot, graph);
-      WeighedDistancePtr_t shPtr (ptr);
-      ptr->init (shPtr);
-      return shPtr;
-    }
-
-    WeighedDistancePtr_t WeighedDistance::createCopy
-	(const WeighedDistancePtr_t& distance)
-    {
-      WeighedDistance* ptr = new WeighedDistance (*distance);
-      WeighedDistancePtr_t shPtr (ptr);
-      ptr->init (shPtr);
-      return shPtr;
-    }
-
-    core::DistancePtr_t WeighedDistance::clone () const
-    {
-      return createCopy (weak_.lock ());
-    }
-
-    WeighedDistance::WeighedDistance (const DevicePtr_t& robot,
-        const graph::GraphPtr_t graph) :
-      core::WeighedDistance (robot), graph_ (graph)
-    {
-    }
-
-    WeighedDistance::WeighedDistance (const WeighedDistance& distance) :
-      core::WeighedDistance (distance), graph_ (distance.graph_)
-    {
-    }
-
-    void WeighedDistance::init (WeighedDistanceWkPtr_t self)
-    {
-      weak_ = self;
-    }
-
-    value_type WeighedDistance::impl_distance (ConfigurationIn_t q1,
-					       ConfigurationIn_t q2) const
-    {
-      value_type d = core::WeighedDistance::impl_distance (q1, q2);
-      return d;
-
-      // graph::Edges_t pes = graph_->getEdges
-        // (graph_->getNode (q1), graph_->getNode (q2));
-      // while (!pes.empty ()) {
-        // if (pes.back ()->canConnect (q1, q2))
-          // return d;
-        // pes.pop_back ();
-      // }
-      // return d + 100;
-    }
-
-    value_type WeighedDistance::impl_distance (core::NodePtr_t n1,
-					       core::NodePtr_t n2) const
-    {
-      Configuration_t& q1 = *n1->configuration(),
-                       q2 = *n2->configuration();
-      value_type d = core::WeighedDistance::impl_distance (q1, q2);
-
-      graph::Edges_t pes = graph_->getEdges (
-          graph_->getState (static_cast <RoadmapNodePtr_t>(n1)),
-          graph_->getState (static_cast <RoadmapNodePtr_t>(n2)));
-      while (!pes.empty ()) {
-        if (pes.back ()->canConnect (q1, q2))
-          return d;
-        pes.pop_back ();
-      }
-      return d + 100;
-    }
-
-    template <typename Archive>
-    inline void WeighedDistance::serialize(Archive& ar, const unsigned int version)
-    {
-      using namespace boost::serialization;
-      (void) version;
-      ar & make_nvp("base", base_object<core::WeighedDistance>(*this));
-      ar & BOOST_SERIALIZATION_NVP(graph_);
-      ar & BOOST_SERIALIZATION_NVP(weak_);
-    }
-
-    HPP_SERIALIZATION_IMPLEMENT(WeighedDistance);
-
-  } //   namespace manipulation
-} // namespace hpp
+namespace manipulation {
+WeighedDistancePtr_t WeighedDistance::create(const DevicePtr_t& robot,
+                                             const graph::GraphPtr_t& graph) {
+  WeighedDistance* ptr = new WeighedDistance(robot, graph);
+  WeighedDistancePtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+WeighedDistancePtr_t WeighedDistance::createCopy(
+    const WeighedDistancePtr_t& distance) {
+  WeighedDistance* ptr = new WeighedDistance(*distance);
+  WeighedDistancePtr_t shPtr(ptr);
+  ptr->init(shPtr);
+  return shPtr;
+}
+
+core::DistancePtr_t WeighedDistance::clone() const {
+  return createCopy(weak_.lock());
+}
+
+WeighedDistance::WeighedDistance(const DevicePtr_t& robot,
+                                 const graph::GraphPtr_t graph)
+    : core::WeighedDistance(robot), graph_(graph) {}
+
+WeighedDistance::WeighedDistance(const WeighedDistance& distance)
+    : core::WeighedDistance(distance), graph_(distance.graph_) {}
+
+void WeighedDistance::init(WeighedDistanceWkPtr_t self) { weak_ = self; }
+
+value_type WeighedDistance::impl_distance(ConfigurationIn_t q1,
+                                          ConfigurationIn_t q2) const {
+  value_type d = core::WeighedDistance::impl_distance(q1, q2);
+  return d;
+
+  // graph::Edges_t pes = graph_->getEdges
+  // (graph_->getNode (q1), graph_->getNode (q2));
+  // while (!pes.empty ()) {
+  // if (pes.back ()->canConnect (q1, q2))
+  // return d;
+  // pes.pop_back ();
+  // }
+  // return d + 100;
+}
+
+value_type WeighedDistance::impl_distance(core::NodePtr_t n1,
+                                          core::NodePtr_t n2) const {
+  Configuration_t &q1 = *n1->configuration(), q2 = *n2->configuration();
+  value_type d = core::WeighedDistance::impl_distance(q1, q2);
+
+  graph::Edges_t pes =
+      graph_->getEdges(graph_->getState(static_cast<RoadmapNodePtr_t>(n1)),
+                       graph_->getState(static_cast<RoadmapNodePtr_t>(n2)));
+  while (!pes.empty()) {
+    if (pes.back()->canConnect(q1, q2)) return d;
+    pes.pop_back();
+  }
+  return d + 100;
+}
+
+template <typename Archive>
+inline void WeighedDistance::serialize(Archive& ar,
+                                       const unsigned int version) {
+  using namespace boost::serialization;
+  (void)version;
+  ar& make_nvp("base", base_object<core::WeighedDistance>(*this));
+  ar& BOOST_SERIALIZATION_NVP(graph_);
+  ar& BOOST_SERIALIZATION_NVP(weak_);
+}
+
+HPP_SERIALIZATION_IMPLEMENT(WeighedDistance);
+
+}  //   namespace manipulation
+}  // namespace hpp
 
 BOOST_CLASS_EXPORT(hpp::manipulation::WeighedDistance)
diff --git a/tests/test-constraintgraph.cc b/tests/test-constraintgraph.cc
index ae308c1a445847e06e66c52705d486a977d7983c..f79e09cb31fcc7592f6c474de3171d9b1cba2640 100644
--- a/tests/test-constraintgraph.cc
+++ b/tests/test-constraintgraph.cc
@@ -26,153 +26,149 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 // DAMAGE.
 
-#include <hpp/pinocchio/urdf/util.hh>
-#include <hpp/pinocchio/liegroup-element.hh>
-
-#include <hpp/core/steering-method/straight.hh>
-#include <hpp/core/path-validation-report.hh>
-
+#include <boost/test/unit_test.hpp>
 #include <hpp/constraints/generic-transformation.hh>
 #include <hpp/constraints/locked-joint.hh>
 #include <hpp/constraints/relative-com.hh>
-
+#include <hpp/core/path-validation-report.hh>
+#include <hpp/core/steering-method/straight.hh>
 #include <hpp/manipulation/constraint-set.hh>
-#include "hpp/manipulation/graph/state.hh"
-#include "hpp/manipulation/graph/state-selector.hh"
-#include "hpp/manipulation/graph/graph.hh"
-#include "hpp/manipulation/graph/edge.hh"
-#include "hpp/manipulation/device.hh"
-#include "hpp/manipulation/problem.hh"
-#include "hpp/manipulation/graph-path-validation.hh"
 #include <hpp/manipulation/steering-method/graph.hh>
+#include <hpp/pinocchio/liegroup-element.hh>
+#include <hpp/pinocchio/urdf/util.hh>
 
-#include <boost/test/unit_test.hpp>
+#include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/graph-path-validation.hh"
+#include "hpp/manipulation/graph/edge.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
+#include "hpp/manipulation/graph/state.hh"
+#include "hpp/manipulation/problem.hh"
 
-using hpp::core::steeringMethod::Straight;
 using hpp::core::SteeringMethodPtr_t;
+using hpp::core::steeringMethod::Straight;
 
-typedef std::vector <hpp::manipulation::graph::GraphComponentPtr_t>
-GraphComponents_t;
+typedef std::vector<hpp::manipulation::graph::GraphComponentPtr_t>
+    GraphComponents_t;
 
 namespace hpp_test {
-  using hpp::core::Configuration_t;
-  using hpp::manipulation::graph::GraphPtr_t;
-  using hpp::manipulation::graph::StateSelectorPtr_t;
-  using hpp::manipulation::graph::StateSelectorPtr_t;
-  using hpp::manipulation::graph::StatePtr_t;
-  using hpp::manipulation::graph::EdgePtr_t;
-  using hpp::manipulation::graph::Graph;
-  using hpp::manipulation::graph::GraphComponent;
-  using hpp::manipulation::graph::EdgePtr_t;
-  using hpp::manipulation::graph::Edges_t;
-
-  hpp::manipulation::DevicePtr_t robot;
-  Configuration_t q1, q2;
-
-  GraphComponents_t components;
-  GraphPtr_t graph_;
-  StateSelectorPtr_t ns;
-  StatePtr_t n1;
-  StatePtr_t n2;
-  EdgePtr_t e11;
-  EdgePtr_t e21;
-  EdgePtr_t e12;
-  EdgePtr_t e22;
-
-  void initialize (bool ur5)
-  {
-    components.clear();
-    robot = hpp::manipulation::Device::create ("test-robot");
-    hpp::manipulation::ProblemPtr_t problem(hpp::manipulation::Problem::create
-                                            (robot));
-    if (ur5) {
-      hpp::pinocchio::urdf::loadModel
-        (robot, 0, "ur5/", "anchor",
-         "package://example-robot-data/robots/ur_description/urdf/"
-         "ur5_joint_limited_robot.urdf",
-         "package://example-robot-data/robots/ur_description/srdf/"
-         "ur5_joint_limited_robot.srdf");
-    }
-    SteeringMethodPtr_t sm
-      (hpp::manipulation::steeringMethod::Graph::create (problem));
-    hpp::core::ProblemPtr_t pb (problem);
-    pb->steeringMethod (sm);
-
-    graph_ = Graph::create ("manipulation-graph", robot, problem);
-    components.push_back(graph_);
-    graph_->maxIterations (20);
-    graph_->errorThreshold (1e-4);
-    ns = graph_->createStateSelector("node-selector");
-    n1 = ns->createState ("node 1"); components.push_back(n1);
-    n2 = ns->createState ("node 2"); components.push_back(n2);
-    e11 = n1->linkTo ("edge 11", n1); components.push_back(e11);
-    e21 = n2->linkTo ("edge 21", n1); components.push_back(e21);
-    e12 = n1->linkTo ("edge 12", n2); components.push_back(e12);
-    e22 = n2->linkTo ("edge 22", n2); components.push_back(e22);
-    graph_->initialize ();
-
-    q1 = Configuration_t::Zero(6);
-    q2 = Configuration_t::Zero(6);
-    q1 << 1,1,1,0,2.5,-1.9;
-    q2 << 2,0,1,0,2.5,-1.9;
+using hpp::core::Configuration_t;
+using hpp::manipulation::graph::EdgePtr_t;
+using hpp::manipulation::graph::Edges_t;
+using hpp::manipulation::graph::Graph;
+using hpp::manipulation::graph::GraphComponent;
+using hpp::manipulation::graph::GraphPtr_t;
+using hpp::manipulation::graph::StatePtr_t;
+using hpp::manipulation::graph::StateSelectorPtr_t;
+
+hpp::manipulation::DevicePtr_t robot;
+Configuration_t q1, q2;
+
+GraphComponents_t components;
+GraphPtr_t graph_;
+StateSelectorPtr_t ns;
+StatePtr_t n1;
+StatePtr_t n2;
+EdgePtr_t e11;
+EdgePtr_t e21;
+EdgePtr_t e12;
+EdgePtr_t e22;
+
+void initialize(bool ur5) {
+  components.clear();
+  robot = hpp::manipulation::Device::create("test-robot");
+  hpp::manipulation::ProblemPtr_t problem(
+      hpp::manipulation::Problem::create(robot));
+  if (ur5) {
+    hpp::pinocchio::urdf::loadModel(
+        robot, 0, "ur5/", "anchor",
+        "package://example-robot-data/robots/ur_description/urdf/"
+        "ur5_joint_limited_robot.urdf",
+        "package://example-robot-data/robots/ur_description/srdf/"
+        "ur5_joint_limited_robot.srdf");
   }
-} // namespace hpp_test
+  SteeringMethodPtr_t sm(
+      hpp::manipulation::steeringMethod::Graph::create(problem));
+  hpp::core::ProblemPtr_t pb(problem);
+  pb->steeringMethod(sm);
+
+  graph_ = Graph::create("manipulation-graph", robot, problem);
+  components.push_back(graph_);
+  graph_->maxIterations(20);
+  graph_->errorThreshold(1e-4);
+  ns = graph_->createStateSelector("node-selector");
+  n1 = ns->createState("node 1");
+  components.push_back(n1);
+  n2 = ns->createState("node 2");
+  components.push_back(n2);
+  e11 = n1->linkTo("edge 11", n1);
+  components.push_back(e11);
+  e21 = n2->linkTo("edge 21", n1);
+  components.push_back(e21);
+  e12 = n1->linkTo("edge 12", n2);
+  components.push_back(e12);
+  e22 = n2->linkTo("edge 22", n2);
+  components.push_back(e22);
+  graph_->initialize();
+
+  q1 = Configuration_t::Zero(6);
+  q2 = Configuration_t::Zero(6);
+  q1 << 1, 1, 1, 0, 2.5, -1.9;
+  q2 << 2, 0, 1, 0, 2.5, -1.9;
+}
+}  // namespace hpp_test
 
-BOOST_AUTO_TEST_CASE (GraphStructure)
-{
+BOOST_AUTO_TEST_CASE(GraphStructure) {
   using namespace hpp_test;
   using hpp_test::graph_;
-  initialize (false);
+  initialize(false);
 
   // Check that GraphComponent keeps track of all object properly.
   size_t index = 0;
   for (GraphComponents_t::iterator it = components.begin();
-      it != components.end(); ++it) {
-    BOOST_CHECK_MESSAGE (*it == graph_->get (index).lock(),
-        "GraphComponent class do not track properly GraphComponents_t inherited objects");
+       it != components.end(); ++it) {
+    BOOST_CHECK_MESSAGE(*it == graph_->get(index).lock(),
+                        "GraphComponent class do not track properly "
+                        "GraphComponents_t inherited objects");
     index++;
   }
 
   // Test function Graph::getEdge
   StatePtr_t from(n1), to(n2);
-  Edges_t checkPossibleEdges,
-          possibleEdges = graph_->getEdges (from, to);
-  checkPossibleEdges.push_back (e12);
+  Edges_t checkPossibleEdges, possibleEdges = graph_->getEdges(from, to);
+  checkPossibleEdges.push_back(e12);
   for (size_t j = 0; j < possibleEdges.size(); j++)
-    BOOST_CHECK_MESSAGE (possibleEdges[j] == checkPossibleEdges[j],
-        "Possible edge j = " << j);
+    BOOST_CHECK_MESSAGE(possibleEdges[j] == checkPossibleEdges[j],
+                        "Possible edge j = " << j);
 
   Configuration_t cfg;
-  StatePtr_t node = graph_->getState (cfg);
-  BOOST_CHECK (node == n1);
+  StatePtr_t node = graph_->getState(cfg);
+  BOOST_CHECK(node == n1);
 }
 
-BOOST_AUTO_TEST_CASE (Initialization)
-{
+BOOST_AUTO_TEST_CASE(Initialization) {
   using namespace hpp_test;
-  using hpp_test::graph_;
-  using hpp::pinocchio::LiegroupElement;
-  using hpp::pinocchio::LiegroupSpace;
   using hpp::constraints::ComparisonTypes_t;
-  using hpp::constraints::ImplicitPtr_t;
-  using hpp::constraints::EqualToZero;
   using hpp::constraints::Equality;
+  using hpp::constraints::EqualToZero;
+  using hpp::constraints::ImplicitPtr_t;
   using hpp::constraints::LockedJoint;
   using hpp::manipulation::graph::Edge;
   using hpp::manipulation::graph::EdgePtr_t;
+  using hpp::pinocchio::LiegroupElement;
+  using hpp::pinocchio::LiegroupSpace;
+  using hpp_test::graph_;
 
   initialize(true);
   hpp::manipulation::DevicePtr_t robot(graph_->robot());
-  ImplicitPtr_t constraint(LockedJoint::create
-			   (robot->jointAt(1), LiegroupElement
-			    (LiegroupSpace::R1(true))));
+  ImplicitPtr_t constraint(LockedJoint::create(
+      robot->jointAt(1), LiegroupElement(LiegroupSpace::R1(true))));
   graph_->addNumericalConstraint(constraint);
   // Check that states refuse parameterizable constraints
-  constraint->comparisonType(ComparisonTypes_t(1,Equality));
+  constraint->comparisonType(ComparisonTypes_t(1, Equality));
   BOOST_CHECK_THROW(n1->addNumericalConstraint(constraint), std::logic_error);
 
-  for (std::size_t i=0; i < components.size(); ++i)
-  {
+  for (std::size_t i = 0; i < components.size(); ++i) {
     EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(Edge, components[i]));
     if (edge) {
       try {
@@ -180,8 +176,9 @@ BOOST_AUTO_TEST_CASE (Initialization)
         BOOST_CHECK(false && "should have thrown.");
       } catch (const std::logic_error& exc) {
         std::string msg(exc.what());
-        BOOST_CHECK(msg == std::string
-                    ("The graph should have been initialized first."));
+        BOOST_CHECK(
+            msg ==
+            std::string("The graph should have been initialized first."));
       }
     }
   }