diff --git a/CMakeLists.txt b/CMakeLists.txt
index edfe0750adfd6516e1115effbb2f90ba1cbb2563..2504d9e9654119a3a35c0109a3afbd5dbc952a58 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -85,7 +85,6 @@ SET (${PROJECT_NAME}_HEADERS
   include/hpp/manipulation/symbolic-planner.hh
   include/hpp/manipulation/manipulation-planner.hh
   include/hpp/manipulation/graph-path-validation.hh
-  include/hpp/manipulation/graph-steering-method.hh
   include/hpp/manipulation/graph-optimizer.hh
   include/hpp/manipulation/graph/node.hh
   include/hpp/manipulation/graph/state.hh
@@ -110,6 +109,7 @@ SET (${PROJECT_NAME}_HEADERS
 
   include/hpp/manipulation/steering-method/cross-state-optimization.hh
   include/hpp/manipulation/steering-method/fwd.hh
+  include/hpp/manipulation/steering-method/graph.hh
   )
 
 ADD_SUBDIRECTORY(src)
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index 7a322b67fac91eea6d4d175f147a6e33ae41f0d3..f36a30b8d16c3f7bfd34667bc6b07ef39cbac903 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -1,5 +1,6 @@
 INPUT     = @CMAKE_SOURCE_DIR@/doc \
             @CMAKE_SOURCE_DIR@/include \
+            @CMAKE_SOURCE_DIR@/src/steering-method/cross-state-optimization/function.cc \
             @CMAKE_BINARY_DIR@/doc
 
 HTML_EXTRA_FILES = @CMAKE_SOURCE_DIR@/doc/ObjectManipulation_MasterThesis_JosephMirabel.pdf
diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index a2cd02a19b867888a00994a075c9d87f8f6067ab..8834f25b2c923da2d6823e6cbd862d2baf4c869c 100644
--- a/include/hpp/manipulation/device.hh
+++ b/include/hpp/manipulation/device.hh
@@ -35,23 +35,11 @@ namespace hpp {
     /// 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 core::Containers<
-        boost::mpl::vector < HandlePtr_t,
-                             GripperPtr_t,
-                             JointAndShapes_t,
-                             FrameIndices_t> >
+    class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot
     {
       public:
         typedef pinocchio::HumanoidRobot Parent_t;
 
-        typedef core::Containers<
-          boost::mpl::vector < HandlePtr_t,
-          pinocchio::GripperPtr_t,
-          JointAndShapes_t,
-          FrameIndices_t> > Containers_t;
-
         /// Constructor
         /// \param name of the new instance,
         static DevicePtr_t create (const std::string& name)
@@ -82,6 +70,11 @@ namespace hpp {
 
         /// \}
 
+        core::Container <HandlePtr_t> handles;
+        core::Container <GripperPtr_t> grippers;
+        core::Container <JointAndShapes_t> jointAndShapes;
+        core::Container <FrameIndices_t> frameIndices;
+
       protected:
         /// Constructor
         /// \param name of the new instance,
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 360ce8e9ae01d5d5e96810237cf04e6f59e9fcd1..16cb9a31c44c3f8532cfa8e03fa2367a98e1dd44 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -87,9 +87,7 @@ namespace hpp {
     HPP_PREDEF_CLASS (GraphPathValidation);
     typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
     HPP_PREDEF_CLASS (SteeringMethod);
-    HPP_PREDEF_CLASS (GraphSteeringMethod);
     typedef boost::shared_ptr < SteeringMethod > SteeringMethodPtr_t;
-    typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t;
     typedef core::PathOptimizer PathOptimizer;
     typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
     HPP_PREDEF_CLASS (GraphOptimizer);
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index 34bc9e48476fa46b12655512255de19d889ad403..59d76e409a96e0f05bc27490dc8e5d29ef4ea64b 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -55,7 +55,7 @@ namespace hpp {
           if (!pathValidation ())
             throw std::runtime_error ("No GraphPathValidation in the problem.");
           if (!steeringMethod ())
-            throw std::runtime_error ("No GraphSteeringMethod in the problem.");
+            throw std::runtime_error ("No SteeringMethod in the problem.");
         }
 
         /// Get the path validation as a GraphPathValidation
@@ -63,7 +63,7 @@ namespace hpp {
 
         void pathValidation (const PathValidationPtr_t& pathValidation);
 
-        /// Get the steering method as a GraphSteeringMethod
+        /// Get the steering method as a SteeringMethod
         SteeringMethodPtr_t steeringMethod () const;
 
         /// Build a new path validation
diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
index 14293f6128a0730d85c4a18fe4381c709a94aa81..8b555508fab9dcdbaa1025881e7a490043fd38b3 100644
--- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh
+++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
@@ -23,11 +23,36 @@
 # include <hpp/manipulation/fwd.hh>
 # include <hpp/manipulation/problem.hh>
 # include <hpp/manipulation/steering-method/fwd.hh>
-# include <hpp/manipulation/graph-steering-method.hh>
+# include <hpp/manipulation/steering-method/graph.hh>
 
 namespace hpp {
   namespace manipulation {
     namespace steeringMethod {
+      /// \addtogroup steering_method
+      /// \{
+
+      /// Optimization-based steering method.
+      ///
+      /// #### Methodology
+      ///
+      /// 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$ 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).
       class HPP_MANIPULATION_DLLAPI CrossStateOptimization :
         public SteeringMethod
       {
@@ -35,7 +60,11 @@ namespace hpp {
           static CrossStateOptimizationPtr_t create (const Problem& problem);
 
           /// \warning core::Problem will be casted to Problem
-          static CrossStateOptimizationPtr_t createFromCore
+          static CrossStateOptimizationPtr_t create
+            (const core::Problem& problem);
+
+          template <typename T>
+            static CrossStateOptimizationPtr_t create
             (const core::Problem& problem);
 
           core::SteeringMethodPtr_t copy () const;
@@ -77,6 +106,16 @@ namespace hpp {
           /// Weak pointer to itself
           CrossStateOptimizationWkPtr_t weak_;
       }; // class CrossStateOptimization
+      /// \}
+
+      template <typename T>
+        CrossStateOptimizationPtr_t CrossStateOptimization::create
+        (const core::Problem& problem)
+      {
+        CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create (problem);
+        gsm->innerSteeringMethod (T::create (problem));
+        return gsm;
+      }
     } // namespace steeringMethod
   } // namespace manipulation
 } // namespace hpp
diff --git a/include/hpp/manipulation/steering-method/fwd.hh b/include/hpp/manipulation/steering-method/fwd.hh
index 3b245f6bf4d74a71de57110f81f8078aaba51dd2..467557d9d37ee8684783ecdeab090c12ee5b19bc 100644
--- a/include/hpp/manipulation/steering-method/fwd.hh
+++ b/include/hpp/manipulation/steering-method/fwd.hh
@@ -26,6 +26,8 @@
 namespace hpp {
   namespace manipulation {
     namespace steeringMethod {
+      HPP_PREDEF_CLASS (Graph);
+      typedef boost::shared_ptr < Graph > GraphPtr_t;
       HPP_PREDEF_CLASS (CrossStateOptimization);
       typedef boost::shared_ptr < CrossStateOptimization > CrossStateOptimizationPtr_t;
     } // namespace steeringMethod
diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/steering-method/graph.hh
similarity index 51%
rename from include/hpp/manipulation/graph-steering-method.hh
rename to include/hpp/manipulation/steering-method/graph.hh
index d21d7a46184d737c5c77a5ede6e303c35754a6c7..b3145b7109500354416b1fab673554c6e0b9707b 100644
--- a/include/hpp/manipulation/graph-steering-method.hh
+++ b/include/hpp/manipulation/steering-method/graph.hh
@@ -15,8 +15,8 @@
 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
 
 
-#ifndef HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
-# define HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
+#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>
@@ -24,13 +24,12 @@
 # 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 {
-    using core::PathPtr_t;
     /// \addtogroup steering_method
     /// \{
-
     class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod
     {
       public:
@@ -62,63 +61,67 @@ namespace hpp {
         core::SteeringMethodPtr_t steeringMethod_;
     };
 
-    class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod
-    {
-      typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
-
-      public:
-        /// Create instance and return shared pointer
-        /// \warning core::Problem will be casted to Problem
-        static GraphSteeringMethodPtr_t create
-          (const core::Problem& problem);
-
-        template <typename T>
-          static GraphSteeringMethodPtr_t create
-          (const core::Problem& problem);
-
-        /// Create instance and return shared pointer
-        static GraphSteeringMethodPtr_t create (const Problem& problem);
-
-        /// Create copy and return shared pointer
-        static GraphSteeringMethodPtr_t createCopy
-          (const GraphSteeringMethodPtr_t& other);
-
-        /// Copy instance and return shared pointer
-        virtual core::SteeringMethodPtr_t copy () const
-        {
-          return createCopy (weak_.lock ());
-        }
-
-      protected:
-        /// Constructor
-        GraphSteeringMethod (const Problem& problem);
-
-        /// Copy constructor
-        GraphSteeringMethod (const GraphSteeringMethod&);
-
-        virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
-        void init (GraphSteeringMethodWkPtr_t weak)
-        {
-          SteeringMethod::init (weak);
-          weak_ = weak;
-        }
-
-      private:
-        /// Weak pointer to itself
-        GraphSteeringMethodWkPtr_t weak_;
-    };
-
-    template <typename T>
-      GraphSteeringMethodPtr_t GraphSteeringMethod::create
-      (const core::Problem& problem)
-    {
-      GraphSteeringMethodPtr_t gsm = GraphSteeringMethod::create (problem);
-      gsm->innerSteeringMethod (T::create (problem));
-      return gsm;
-    }
+    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::Problem& problem);
+
+          template <typename T>
+            static GraphPtr_t create
+            (const core::Problem& problem);
+
+          /// Create instance and return shared pointer
+          static GraphPtr_t create (const Problem& 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 Problem& 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::Problem& problem)
+      {
+        GraphPtr_t gsm = Graph::create (problem);
+        gsm->innerSteeringMethod (T::create (problem));
+        return gsm;
+      }
+    } // namespace steeringMethod
     /// \}
   } // namespace manipulation
 } // namespace hpp
 
-#endif // HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
+#endif // HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 66d6b9eed3168cbd25eb1d8faac2a71c5e1eece2..ecb8644bfc75c921001c9f865d9beb7ffdcc2381 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -34,7 +34,6 @@ SET(SOURCES
   weighed-distance.cc
   problem.cc
   graph-path-validation.cc
-  graph-steering-method.cc
   graph-optimizer.cc
 
   graph/state.cc
@@ -55,6 +54,7 @@ SET(SOURCES
   problem-target/state.cc
 
   steering-method/cross-state-optimization.cc
+  steering-method/graph.cc
   )
 
 IF(HPP_WHOLEBODY_STEP_FOUND)
diff --git a/src/device.cc b/src/device.cc
index b641f991bae504f0dd36f052f3537e3a12ebeb0a..d4f01097a99d17ae8b4814dca13fe3f719d6e73d 100644
--- a/src/device.cc
+++ b/src/device.cc
@@ -43,7 +43,7 @@ namespace hpp {
     void Device::setRobotRootPosition (const std::string& rn,
         const Transform3f& t)
     {
-      FrameIndices_t idxs = get<JointIndices_t> (rn);
+      const FrameIndices_t& idxs = frameIndices.get (rn);
       pinocchio::Model& m = model();
       pinocchio::GeomModel& gm = geomModel();
       // The root frame should be the first frame.
@@ -93,11 +93,11 @@ namespace hpp {
       }
 
       frameCacheSize_ = model().frames.size();
-      if (has<FrameIndices_t>(name)) {
-        const FrameIndices_t& old = get<FrameIndices_t>(name);
+      if (frameIndices.has(name)) {
+        const FrameIndices_t& old = frameIndices.get(name);
         newF.insert(newF.begin(), old.begin(), old.end());
       }
-      add (name, newF);
+      frameIndices.add (name, newF);
       createData();
       createGeomData();
     }
@@ -107,10 +107,10 @@ namespace hpp {
       Parent_t::print (os);
       // print handles
       os << "Handles:" << std::endl;
-      Containers_t::print <HandlePtr_t> (os);
+      handles.print (os);
       // print grippers
       os << "Grippers:" << std::endl;
-      Containers_t::print <GripperPtr_t> (os);
+      grippers.print (os);
       return os;
     }
   } // namespace manipulation
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
deleted file mode 100644
index 446388814d27f4f33ca2ac072b2dd4d8cef5c01e..0000000000000000000000000000000000000000
--- a/src/graph-steering-method.cc
+++ /dev/null
@@ -1,102 +0,0 @@
-// Copyright (c) 2014, LAAS-CNRS
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-//
-// This file is part of hpp-manipulation.
-// hpp-manipulation is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-//
-// hpp-manipulation is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-// General Lesser Public License for more details.  You should have
-// received a copy of the GNU Lesser General Public License along with
-// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
-
-#include "hpp/manipulation/graph-steering-method.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/manipulation/graph/edge.hh>
-
-namespace hpp {
-  namespace manipulation {
-    SteeringMethod::SteeringMethod (const Problem& problem) :
-      core::SteeringMethod (problem), problem_ (problem),
-      steeringMethod_ (core::SteeringMethodStraight::create (problem))
-    {
-    }
-
-    SteeringMethod::SteeringMethod (const SteeringMethod& other):
-      core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_
-      (other.steeringMethod_)
-    {
-    }
-
-    GraphSteeringMethodPtr_t GraphSteeringMethod::create
-      (const core::Problem& problem)
-    {
-      HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
-      const Problem& p = static_cast <const Problem&> (problem);
-      return create (p);
-    }
-
-    GraphSteeringMethodPtr_t GraphSteeringMethod::create
-    (const Problem& problem)
-    {
-      GraphSteeringMethod* ptr = new GraphSteeringMethod (problem);
-      GraphSteeringMethodPtr_t shPtr (ptr);
-      ptr->init (shPtr);
-      return shPtr;
-    }
-
-    GraphSteeringMethodPtr_t GraphSteeringMethod::createCopy
-    (const GraphSteeringMethodPtr_t& other)
-    {
-      GraphSteeringMethod* ptr = new GraphSteeringMethod (*other);
-      GraphSteeringMethodPtr_t shPtr (ptr);
-      ptr->init (shPtr);
-      return shPtr;
-    }
-
-    GraphSteeringMethod::GraphSteeringMethod (const Problem& problem) :
-      SteeringMethod (problem), weak_ ()
-    {
-    }
-
-    GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other):
-      SteeringMethod (other)
-    {
-    }
-
-    PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
-    {
-      graph::Edges_t possibleEdges;
-      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 manipulation
-} // namespace hpp
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 5c30efacb34c22d02160553f75547432a454bd73..537fd7e80259f17aff19bd50f258e40823be8c65 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -29,7 +29,7 @@
 
 #include "hpp/manipulation/device.hh"
 #include "hpp/manipulation/problem.hh"
-#include "hpp/manipulation/graph-steering-method.hh"
+#include "hpp/manipulation/steering-method/graph.hh"
 #include "hpp/manipulation/graph/statistics.hh"
 #include "hpp/manipulation/constraint-set.hh"
 
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index a1ff2a520a16dbb2dabc6238ee2e05eab4191a12..224c71d802ecf9ad3f0f0f231f6a2c181e56ef24 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -663,8 +663,6 @@ namespace hpp {
             inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint (
                 const index_t& iG, const index_t& iOH)
             {
-              typedef core::ProblemSolver CPS_t;
-
               boost::array<NumericalConstraintPtr_t,3>& gcs =
                 graspCs [iG * nOH + iOH];
               if (!gcs[0]) {
@@ -673,16 +671,16 @@ namespace hpp {
                 const GripperPtr_t& g (gs[iG]);
                 const HandlePtr_t& h (handle (iOH));
                 const std::string& grasp = g->name() + " grasps " + h->name();
-                if (!ps->CPS_t::has<NumericalConstraintPtr_t>(grasp)) {
+                if (!ps->numericalConstraints.has(grasp)) {
                   ps->createGraspConstraint (grasp, g->name(), h->name());
                 }
-                gcs[0] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp);
-                gcs[1] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp + "/complement");
+                gcs[0] = ps->numericalConstraints.get(grasp);
+                gcs[1] = ps->numericalConstraints.get(grasp + "/complement");
                 const std::string& pregrasp = g->name() + " pregrasps " + h->name();
-                if (!ps->CPS_t::has<NumericalConstraintPtr_t>(pregrasp)) {
+                if (!ps->numericalConstraints.has(pregrasp)) {
                   ps->createPreGraspConstraint (pregrasp, g->name(), h->name());
                 }
-                gcs[2] = ps->CPS_t::get<NumericalConstraintPtr_t>(pregrasp);
+                gcs[2] = ps->numericalConstraints.get(pregrasp);
               }
               return gcs;
             }
@@ -1050,7 +1048,7 @@ namespace hpp {
           Grippers_t grippers (griNames.size());
           index_t i = 0;
           BOOST_FOREACH (const std::string& gn, griNames) {
-            grippers[i] = robot.get <GripperPtr_t> (gn);
+            grippers[i] = robot.grippers.get (gn);
             ++i;
           }
           Objects_t objects (objs.size());
@@ -1063,7 +1061,7 @@ namespace hpp {
             objects[i].get<1> ().resize (od.handles.size());
             Handles_t::iterator it = objects[i].get<1> ().begin();
             BOOST_FOREACH (const std::string hn, od.handles) {
-              *it = robot.get <HandlePtr_t> (hn);
+              *it = robot.handles.get (hn);
               ++it;
             }
             // Create placement
@@ -1076,30 +1074,30 @@ namespace hpp {
             // else if contact surfaces are defined and selected
             //   create default placement constraint using the ProblemSolver
             //   methods createPlacementConstraint and createPrePlacementConstraint
-            if (ps->core::ProblemSolver::has<NumericalConstraintPtr_t>(placeN)) {
+            if (ps->numericalConstraints.has(placeN)) {
               objects[i].get<0> ().get<0> () =
-                ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN);
-              if (ps->core::ProblemSolver::has<NumericalConstraintPtr_t>(preplaceN)) {
+                ps->numericalConstraints.get (placeN);
+              if (ps->numericalConstraints.has(preplaceN)) {
                 objects[i].get<0> ().get<1> () =
-                  ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN);
+                  ps->numericalConstraints.get (preplaceN);
               }
             } else if (!envNames.empty() && !od.shapes.empty ()) {
               ps->createPlacementConstraint (placeN,
                   od.shapes, envNames, margin);
               objects[i].get<0> ().get<0> () =
-                ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN);
+                ps->numericalConstraints.get (placeN);
               if (prePlace) {
                 ps->createPrePlacementConstraint (preplaceN,
                     od.shapes, envNames, margin, prePlaceWidth);
                 objects[i].get<0> ().get<1> () =
-                  ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN);
+                  ps->numericalConstraints.get (preplaceN);
               }
             }
             // Create object lock
 	    // Loop over all frames of object, detect joint and create locked
 	    // joint.
-            assert (robot.has <FrameIndices_t> (od.name));
-            BOOST_FOREACH (const se3::FrameIndex& f, robot.get<FrameIndices_t> (od.name)) {
+            assert (robot.frameIndices.has (od.name));
+            BOOST_FOREACH (const se3::FrameIndex& f, robot.frameIndices.get (od.name)) {
               if (model.frames[f].type != se3::JOINT) continue;
               const JointIndex j = model.frames[f].parent;
               JointPtr_t oj (new Joint (ps->robot(), j));
@@ -1108,7 +1106,7 @@ namespace hpp {
                                    .segment (oj->rankInConfiguration (),
                                              oj->configSize ()), space);
               LockedJointPtr_t lj = core::LockedJoint::create (oj, lge);
-              ps->add <LockedJointPtr_t> ("lock_" + oj->name (), lj);
+              ps->lockedJoints.add ("lock_" + oj->name (), lj);
               objects[i].get<0> ().get<2> ().push_back (lj);
             }
             ++i;
diff --git a/src/path-optimization/keypoints.cc b/src/path-optimization/keypoints.cc
index 384750c16389c4c7d43ea9d96e117a7bbefca9f2..fd802d0ef62e54127df5aded609f6685a245fdfe 100644
--- a/src/path-optimization/keypoints.cc
+++ b/src/path-optimization/keypoints.cc
@@ -24,7 +24,7 @@
 #include <hpp/manipulation/graph/edge.hh>
 #include <hpp/manipulation/graph/graph.hh>
 #include <hpp/manipulation/constraint-set.hh>
-#include <hpp/manipulation/graph-steering-method.hh>
+#include <hpp/manipulation/steering-method/graph.hh>
 
 namespace hpp {
   namespace manipulation {
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 543de2fe8068390290c06954db7a0cfae1a09a3f..6016b9f50e4e2db209e34505850c606418b3b048 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -33,7 +33,10 @@
 #include <hpp/core/path-projector/global.hh>
 #include <hpp/core/path-projector/recursive-hermite.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/manipulation/package-config.hh" // HPP_MANIPULATION_HAS_WHOLEBODY_STEP
@@ -49,12 +52,12 @@
 #include "hpp/manipulation/graph-optimizer.hh"
 #include "hpp/manipulation/graph-path-validation.hh"
 #include "hpp/manipulation/graph-node-optimizer.hh"
-#include "hpp/manipulation/graph-steering-method.hh"
 #include "hpp/manipulation/path-optimization/config-optimization.hh"
 #include "hpp/manipulation/path-optimization/keypoints.hh"
 #include "hpp/manipulation/path-optimization/spline-gradient-based.hh"
 #include "hpp/manipulation/problem-target/state.hh"
 #include "hpp/manipulation/steering-method/cross-state-optimization.hh"
+#include "hpp/manipulation/steering-method/graph.hh"
 
 #if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
 #include <hpp/wholebody-step/small-steps.hh>
@@ -78,14 +81,23 @@ namespace hpp {
           }
         };
 
+      template <typename ParentSM_t, typename ChildSM_t>
+      core::SteeringMethodPtr_t createSMWithGuess
+      (const core::Problem& problem)
+      {
+        boost::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::Problem& problem, const value_type& step)
       {
-        GraphSteeringMethodPtr_t gsm =
-          HPP_DYNAMIC_PTR_CAST (GraphSteeringMethod, problem.steeringMethod());
+        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"
-            " GraphSteeringMethod");
+            " steeringMethod::Graph");
         return PathProjectorType::create (problem.distance(),
             gsm->innerSteeringMethod(), step);
       }
@@ -99,63 +111,70 @@ namespace hpp {
     ProblemSolver::ProblemSolver () :
       core::ProblemSolver (), robot_ (), problem_ (0x0)
     {
-      parent_t::add<core::RobotBuilder_t> ("hpp::manipulation::Device",
-                                           hpp::manipulation::Device::create);
-      parent_t::robotType ("hpp::manipulation::Device");
-      parent_t::add<core::PathPlannerBuilder_t>
-        ("M-RRT", ManipulationPlanner::create);
-      parent_t::add<core::PathPlannerBuilder_t>
-        ("SymbolicPlanner", SymbolicPlanner::create);
-      using core::PathOptimizerBuilder_t;
-      parent_t::add <PathOptimizerBuilder_t> ("Graph-RandomShortcut",
+      robots.add ("hpp::manipulation::Device", manipulation::Device::create);
+      robotType ("hpp::manipulation::Device");
+
+      pathPlanners.add ("M-RRT", ManipulationPlanner::create);
+      pathPlanners.add ("SymbolicPlanner", SymbolicPlanner::create);
+
+      pathOptimizers.add ("Graph-RandomShortcut",
           GraphOptimizer::create <core::RandomShortcut>);
-      parent_t::add <PathOptimizerBuilder_t> ("PartialShortcut", core::pathOptimization::
+      pathOptimizers.add ("PartialShortcut", core::pathOptimization::
           PartialShortcut::createWithTraits <PartialShortcutTraits>);
-      parent_t::add <PathOptimizerBuilder_t> ("Graph-PartialShortcut",
+      pathOptimizers.add ("Graph-PartialShortcut",
           GraphOptimizer::create <core::pathOptimization::PartialShortcut>);
-      parent_t::add <PathOptimizerBuilder_t> ("ConfigOptimization",
+      pathOptimizers.add ("ConfigOptimization",
           core::pathOptimization::ConfigOptimization::createWithTraits
           <pathOptimization::ConfigOptimizationTraits>);
-      parent_t::add <PathOptimizerBuilder_t> ("Graph-ConfigOptimization",
+      pathOptimizers.add ("Graph-ConfigOptimization",
           GraphOptimizer::create <
           GraphConfigOptimizationTraits
             <pathOptimization::ConfigOptimizationTraits>
             >);
 
-      parent_t::add <core::PathProjectorBuilder_t> ("Progressive",
+      pathProjectors.add ("Progressive",
           createPathProjector <core::pathProjector::Progressive>);
-      parent_t::add <core::PathProjectorBuilder_t> ("Dichotomy",
+      pathProjectors.add ("Dichotomy",
           createPathProjector <core::pathProjector::Dichotomy>);
-      parent_t::add <core::PathProjectorBuilder_t> ("Global",
+      pathProjectors.add ("Global",
           createPathProjector <core::pathProjector::Global>);
-      parent_t::add <core::PathProjectorBuilder_t> ("RecursiveHermite",
+      pathProjectors.add ("RecursiveHermite",
           createPathProjector <core::pathProjector::RecursiveHermite>);
 
-      // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
-      // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
-      // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
-      parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore);
-      // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
-      parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
-
-      using core::SteeringMethodBuilder_t;
-      parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
-          GraphSteeringMethod::create <core::SteeringMethodStraight>);
-      parent_t::add <SteeringMethodBuilder_t> ("Graph-Straight",
-          GraphSteeringMethod::create <core::steeringMethod::Straight>);
-      parent_t::add <SteeringMethodBuilder_t> ("Graph-Hermite",
-          GraphSteeringMethod::create <core::steeringMethod::Hermite>);
-      parent_t::add <SteeringMethodBuilder_t> ("CrossStateOptimization",
-          steeringMethod::CrossStateOptimization::createFromCore);
-
-      parent_t::add <PathOptimizerBuilder_t> ("KeypointsShortcut",
+      // pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
+      // pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
+      // pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
+      pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore);
+      // pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
+      pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
+
+      steeringMethods.add ("Graph-SteeringMethodStraight",
+          steeringMethod::Graph::create <core::SteeringMethodStraight>);
+      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>);
+
+      pathOptimizers.add ("KeypointsShortcut",
           pathOptimization::Keypoints::create);
 
 #if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
-      parent_t::add <PathOptimizerBuilder_t>
-        ("Walkgen", wholebodyStep::SmallSteps::create);
-      parent_t::add <PathOptimizerBuilder_t>
-        ("Graph-Walkgen", pathOptimization::SmallSteps::create);
+      pathOptimizers.add ("Walkgen", wholebodyStep::SmallSteps::create);
+      pathOptimizers.add ("Graph-Walkgen", pathOptimization::SmallSteps::create);
 #endif
 
       pathPlannerType ("M-RRT");
@@ -220,9 +239,9 @@ namespace hpp {
       JointAndShapes_t l;
       for (StringList_t::const_iterator it1 = surface1.begin ();
           it1 != surface1.end(); ++it1) {
-        if (!robot_->has <JointAndShapes_t> (*it1))
+        if (!robot_->jointAndShapes.has (*it1))
           throw std::runtime_error ("First list of triangles not found.");
-        l = robot_->get <JointAndShapes_t> (*it1);
+        l = robot_->jointAndShapes.get (*it1);
         for (JointAndShapes_t::const_iterator it = l.begin ();
             it != l.end(); ++it) {
           constraints.first->addObject (ConvexShape (it->second, it->first));
@@ -232,11 +251,11 @@ namespace hpp {
       for (StringList_t::const_iterator it2 = surface2.begin ();
           it2 != surface2.end(); ++it2) {
         // Search first robot triangles
-        if (robot_->has <JointAndShapes_t> (*it2))
-          l = robot_->get <JointAndShapes_t> (*it2);
+        if (robot_->jointAndShapes.has (*it2))
+          l = robot_->jointAndShapes.get (*it2);
         // and then environment triangles.
-        else if (core::ProblemSolver::has <JointAndShapes_t> (*it2))
-          l = core::ProblemSolver::get <JointAndShapes_t> (*it2);
+        else if (jointAndShapes.has (*it2))
+          l = jointAndShapes.get (*it2);
         else throw std::runtime_error ("Second list of triangles not found.");
         for (JointAndShapes_t::const_iterator it = l.begin ();
             it != l.end(); ++it) {
@@ -271,9 +290,9 @@ namespace hpp {
       JointAndShapes_t l;
       for (StringList_t::const_iterator it1 = surface1.begin ();
           it1 != surface1.end(); ++it1) {
-        if (!robot_->has <JointAndShapes_t> (*it1))
+        if (!robot_->jointAndShapes.has (*it1))
           throw std::runtime_error ("First list of triangles not found.");
-        l = robot_->get <JointAndShapes_t> (*it1);
+        l = robot_->jointAndShapes.get (*it1);
 
         for (JointAndShapes_t::const_iterator it = l.begin ();
             it != l.end(); ++it) {
@@ -284,11 +303,11 @@ namespace hpp {
       for (StringList_t::const_iterator it2 = surface2.begin ();
           it2 != surface2.end(); ++it2) {
         // Search first robot triangles
-        if (robot_->has <JointAndShapes_t> (*it2))
-          l = robot_->get <JointAndShapes_t> (*it2);
+        if (robot_->jointAndShapes.has (*it2))
+          l = robot_->jointAndShapes.get (*it2);
         // and then environment triangles.
-        else if (has <JointAndShapes_t> (*it2))
-          l = get <JointAndShapes_t> (*it2);
+        else if (jointAndShapes.has (*it2))
+          l = jointAndShapes.get (*it2);
         else throw std::runtime_error ("Second list of triangles not found.");
 
         for (JointAndShapes_t::const_iterator it = l.begin ();
@@ -309,9 +328,9 @@ namespace hpp {
       if (!constraintGraph ()) {
         throw std::runtime_error ("The graph is not defined.");
       }
-      GripperPtr_t g = robot_->get <GripperPtr_t> (gripper);
+      GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t());
       if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
-      HandlePtr_t h = robot_->get <HandlePtr_t> (handle);
+      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";
@@ -328,9 +347,9 @@ namespace hpp {
     (const std::string& name, const std::string& gripper,
      const std::string& handle)
     {
-      GripperPtr_t g = robot_->get <GripperPtr_t> (gripper);
+      GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t());
       if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
-      HandlePtr_t h = robot_->get <HandlePtr_t> (handle);
+      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 ();
@@ -344,7 +363,7 @@ namespace hpp {
       parent_t::pathValidationType(type, tolerance);
       assert (problem_);
       problem_->setPathValidationFactory (
-          parent_t::get<core::PathValidationBuilder_t>(type),
+          pathValidations.get(type),
           tolerance);
     }
 
diff --git a/src/problem.cc b/src/problem.cc
index 69a00b26049d662dddde3edd8560482f5503a589..883f4a9d7b12e5bf0e6f59adb4222193ee506aea 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -19,7 +19,7 @@
 #include <hpp/core/discretized-collision-checking.hh>
 
 #include <hpp/manipulation/weighed-distance.hh>
-#include <hpp/manipulation/graph-steering-method.hh>
+#include <hpp/manipulation/steering-method/graph.hh>
 #include <hpp/manipulation/graph-path-validation.hh>
 #include <hpp/manipulation/graph/graph.hh>
 
@@ -28,7 +28,7 @@ namespace hpp {
     Problem::Problem (DevicePtr_t robot)
       : Parent (robot), graph_()
     {
-      Parent::steeringMethod (GraphSteeringMethod::create (*this));
+      Parent::steeringMethod (steeringMethod::Graph::create (*this));
       distance (WeighedDistance::create (robot, graph_));
       setPathValidationFactory(core::DiscretizedCollisionChecking::create, 0.05);
 
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 8d0929af461da143f0ef7308efe0920b9b26ad5d..83bef2bb41a234f11902394442e538961b9bc2d1 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -24,6 +24,8 @@
 
 #include <hpp/util/exception-factory.hh>
 
+#include <pinocchio/multibody/model.hpp>
+
 #include <hpp/pinocchio/configuration.hh>
 
 #include <hpp/constraints/hybrid-solver.hh>
@@ -39,10 +41,18 @@
 namespace hpp {
   namespace manipulation {
     namespace steeringMethod {
-      using namespace graph;
       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 Problem& problem)
       {
@@ -51,7 +61,7 @@ namespace hpp {
         return shPtr;
       }
 
-      CrossStateOptimizationPtr_t CrossStateOptimization::createFromCore (
+      CrossStateOptimizationPtr_t CrossStateOptimization::create (
           const core::Problem& problem)
       {
         HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
@@ -194,18 +204,25 @@ namespace hpp {
         typedef std::vector<States_t> StatesPerConf_t;
         StatesPerConf_t statesPerConf_;
         struct RightHandSideSetter {
-          DifferentiableFunctionPtr_t impF, expF;
+          DifferentiableFunctionPtr_t impF;
+          size_type expFidx;
           Configuration_t* qrhs;
           vector_t rhs;
           RightHandSideSetter () : qrhs (NULL) {}
-          RightHandSideSetter (DifferentiableFunctionPtr_t _impF, DifferentiableFunctionPtr_t _expF, Configuration_t* _qrhs)
-            : impF(_impF), expF(_expF), qrhs (_qrhs) {}
-          RightHandSideSetter (DifferentiableFunctionPtr_t _impF, DifferentiableFunctionPtr_t _expF, vector_t _rhs)
-            : impF(_impF), expF(_expF), qrhs (NULL), rhs (_rhs) {}
+          // TODO delete this constructor
+          RightHandSideSetter (DifferentiableFunctionPtr_t _impF, size_type _expFidx, Configuration_t* _qrhs)
+            : impF(_impF), expFidx(_expFidx), qrhs (_qrhs) {}
+          RightHandSideSetter (DifferentiableFunctionPtr_t _impF, size_type _expFidx, vector_t _rhs)
+            : impF(_impF), expFidx(_expFidx), qrhs (NULL), rhs (_rhs) {}
           void apply(constraints::HybridSolver& s)
           {
-            if (qrhs != NULL) s.rightHandSideFromInput (impF, expF, *qrhs);
-            else              s.rightHandSide          (impF, expF, rhs);
+            if (expFidx >= 0) {
+              if (qrhs != NULL) s.explicitSolver().rightHandSideFromInput (expFidx, *qrhs);
+              else              s.explicitSolver().rightHandSide          (expFidx, rhs);
+            } else {
+              if (qrhs != NULL) s.rightHandSideFromInput (impF, DifferentiableFunctionPtr_t(), *qrhs);
+              else              s.rightHandSide          (impF, DifferentiableFunctionPtr_t(), rhs);
+            }
           }
         };
         typedef std::vector<RightHandSideSetter> RightHandSideSetters_t;
@@ -216,9 +233,10 @@ namespace hpp {
           solver (N * nq, N * nv), robot (_robot), statesPerConf_ (N)
         {
           solver.integration (boost::bind(&OptimizationData::_integrate, this, _1, _2, _3));
+          solver.saturation  (boost::bind(&OptimizationData::_saturate , this, _1, _2));
         }
 
-        void addGraphConstraints (const GraphPtr_t& graph)
+        void addGraphConstraints (const graph::GraphPtr_t& graph)
         {
           for (std::size_t i = 0; i < N; ++i) {
             _add (graph->lockedJoints(), i);
@@ -245,29 +263,39 @@ namespace hpp {
             LockedJointPtr_t lj (*_lj);
             std::ostringstream os;
             os << lj->jointName() << " | " << i << " -> " << (i+1);
-            DifferentiableFunctionPtr_t f;
+            DifferentiableFunctionPtr_t f, f_implicit;
             // i = Input, o = Output,
             // c = Config, v = Velocity
             RowBlockIndices ic, oc, ov;
             ColBlockIndices iv;
             ComparisonTypes_t cts;
-            Configuration_t* qrhs;
+            vector_t rhs;
             if (i == 0) {
-              f = lj->function();
+              f = lj->explicitFunction();
               ic = _row(lj->inputConf()     , 0);
               oc = _row(lj->outputConf()    , 0);
               iv = _col(lj->inputVelocity() , 0);
               ov = _row(lj->outputVelocity(), 0);
-              cts = ComparisonTypes_t (lj->numberDof(), constraints::Equality);
-              qrhs = &q1;
+              cts = lj->comparisonType();
+              lj->rightHandSideFromConfig (q1);
+              rhs = lj->rightHandSide();
             } else if (i == N) {
-              f = lj->function();
+              f = lj->explicitFunction();
+              // Currently, this function is mandatory because if the same
+              // joint is locked all along the path, then, one of the LockedJoint 
+              // has to be treated implicitely.
+              // TODO it would be smarter to detect this case beforehand. If the
+              // chain in broken in the middle, an explicit formulation exists
+              // (by inverting the equality in the next else section) and we
+              // miss it.
+              f_implicit = _stateFunction (lj->functionPtr(), N-1);
               ic = _row(lj->inputConf()     , 0);
               oc = _row(lj->outputConf()    , (N-1) * nq);
               iv = _col(lj->inputVelocity() , 0);
               ov = _row(lj->outputVelocity(), (N-1) * nv);
-              cts = ComparisonTypes_t (lj->numberDof(), constraints::Equality);
-              qrhs = &q2;
+              cts = lj->comparisonType();
+              lj->rightHandSideFromConfig (q2);
+              rhs = lj->rightHandSide();
             } else {
               f = Identity::Ptr_t (new Identity (lj->configSpace(), os.str()));
               ic = _row(lj->outputConf()    , (i - 1) * nq);
@@ -275,21 +303,26 @@ namespace hpp {
               iv = _col(lj->outputVelocity(), (i - 1) * nv);
               ov = _row(lj->outputVelocity(),  i      * nv);
               cts = ComparisonTypes_t (lj->numberDof(), constraints::EqualToZero);
-              qrhs = NULL;
             }
 
-            bool added = solver.explicitSolver().add (f, ic, oc, iv, ov, cts);
-            if (!added) {
-              HPP_THROW (std::invalid_argument,
-                  "Could not add locked joint " << lj->jointName() <<
-                  " of transition " << trans->name() << " at id " << i);
+            // It is important to use the index of the function since the same
+            // function may be added several times on different part.
+            size_type expFidx = solver.explicitSolver().add (f, ic, oc, iv, ov, cts);
+            if (expFidx < 0) {
+              if (f_implicit) {
+                solver.add (f_implicit, 0, cts);
+              } else {
+                HPP_THROW (std::invalid_argument,
+                    "Could not add locked joint " << lj->jointName() <<
+                    " of transition " << trans->name() << " at id " << i);
+              }
             }
 
-            // This must be done later
-            if (qrhs != NULL)
+            // Setting the right hand side must be done later
+            if (rhs.size() > 0)
               rhsSetters_.push_back (RightHandSideSetter(
-                    DifferentiableFunctionPtr_t(), f, qrhs));
-              // solver.rightHandSideFromInput (DifferentiableFunctionPtr_t(), f, *qrhs);
+                    f_implicit, expFidx, rhs));
+            f_implicit.reset();
           }
 
           // TODO handle numerical constraints
@@ -335,15 +368,15 @@ namespace hpp {
               // qrhs = NULL;
             }
 
-            bool added = false;
-            if (ef) added = solver.explicitSolver().add (ef, ic, oc, iv, ov, cts);
-            if (!added) solver.add (f, 0, cts);
+            size_type expFidx = -1;
+            if (ef) expFidx = solver.explicitSolver().add (ef, ic, oc, iv, ov, cts);
+            if (expFidx < 0) solver.add (f, 0, cts);
 
             // TODO This must be done later...
             // if (qrhs != NULL) {
             if (rhs.size() > 0) {
               // solver.rightHandSideFromInput (f, ef, rhs);
-              rhsSetters_.push_back (RightHandSideSetter(f, ef, rhs));
+              rhsSetters_.push_back (RightHandSideSetter(f, expFidx, rhs));
             }
           }
         }
@@ -393,23 +426,23 @@ namespace hpp {
           for (LockedJoints_t::const_iterator _lj = ljs.begin();
               _lj != ljs.end(); ++_lj) {
             LockedJointPtr_t lj (*_lj);
-            bool added = solver.explicitSolver().add (
-                lj->function(),
+            size_type expFidx = solver.explicitSolver().add (
+                lj->explicitFunction(),
                 _row(lj->inputConf()     , i * nq),
                 _row(lj->outputConf()    , i * nq),
                 _col(lj->inputVelocity() , i * nv),
                 _row(lj->outputVelocity(), i * nv),
                 lj->comparisonType ());
-            if (!added)
+            if (expFidx < 0)
               throw std::invalid_argument ("Could not add locked joint " + lj->jointName());
 
             // This must be done later
             rhsSetters_.push_back (RightHandSideSetter(
                   DifferentiableFunctionPtr_t(),
-                  lj->function(),
+                  expFidx,
                   lj->rightHandSide()));
             // solver.rightHandSide (DifferentiableFunctionPtr_t(),
-                // lj->function(),
+                // lj->explicitFunction(),
                 // lj->rightHandSide());
           }
         }
@@ -447,6 +480,53 @@ namespace hpp {
                 v.segment(iv,nv),
                 qout.segment(iq,nq));
         }
+        bool _saturate (vectorIn_t q, Eigen::VectorXi& sat)
+        {
+          bool ret = false;
+          const se3::Model& model = robot->model();
+
+          for (std::size_t i = 1; i < model.joints.size(); ++i) {
+            const size_type jnq = model.joints[i].nq();
+            const size_type jnv = model.joints[i].nv();
+            const size_type jiq = model.joints[i].idx_q();
+            const size_type jiv = model.joints[i].idx_v();
+            for (std::size_t k = 0; k < N; ++k) {
+              const size_type idx_q = k * nq + jiq;
+              const size_type idx_v = k * nv + jiv;
+              for (size_type j = 0; j < jnq; ++j) {
+                const size_type iq = idx_q + j;
+                const size_type iv = idx_v + std::min(j,jnv-1);
+                if        (q[iq] >= model.upperPositionLimit[jiq + j]) {
+                  sat[iv] =  1;
+                  ret = true;
+                } else if (q[iq] <= model.lowerPositionLimit[jiq + j]) {
+                  sat[iv] = -1;
+                  ret = true;
+                } else
+                  sat[iv] =  0;
+              }
+            }
+          }
+
+          const hpp::pinocchio::ExtraConfigSpace& ecs = robot->extraConfigSpace();
+          const size_type& d = ecs.dimension();
+
+          for (size_type k = 0; k < d; ++k) {
+            for (std::size_t j = 0; j < N; ++j) {
+              const size_type iq = j * nq + model.nq + k;
+              const size_type iv = j * nv + model.nv + k;
+              if        (q[iq] >= ecs.upper(k)) {
+                sat[iv] =  1;
+                ret = true;
+              } else if (q[iq] <= ecs.lower(k)) {
+                sat[iv] = -1;
+                ret = true;
+              } else
+                sat[iv] =  0;
+            }
+          }
+          return ret;
+        }
       };
 
       void CrossStateOptimization::buildOptimizationProblem (
@@ -541,7 +621,7 @@ namespace hpp {
       core::PathPtr_t CrossStateOptimization::impl_compute (
           ConfigurationIn_t q1, ConfigurationIn_t q2) const
       {
-        const Graph& graph = *problem_.constraintGraph ();
+        const graph::Graph& graph = *problem_.constraintGraph ();
         GraphSearchData d;
         d.s1 = graph.getState (q1);
         d.s2 = graph.getState (q2);
diff --git a/src/steering-method/cross-state-optimization/function.cc b/src/steering-method/cross-state-optimization/function.cc
index a536f0e1db50ac42af581291d1c08d791ba389ae..3ba0c3d79aefbf8eafacf286723ea8ec674ef6ea 100644
--- a/src/steering-method/cross-state-optimization/function.cc
+++ b/src/steering-method/cross-state-optimization/function.cc
@@ -31,6 +31,8 @@ namespace hpp {
         }
       }
 
+      /// Apply the constraint on a subspace of the input space.
+      /// i.e.: \f$ f (q_0, ... , q_n) = f_{inner} (q_k) \f$
       class HPP_MANIPULATION_LOCAL StateFunction :
         public constraints::DifferentiableFunction
       {
@@ -74,6 +76,8 @@ namespace hpp {
           const segment_t sa_, sd_;
       }; // class Function
 
+      /// \f$ q_{out} = q_{in} \f$
+      /// \todo Make this derive from constraints::AffineFunction
       class HPP_MANIPULATION_LOCAL Identity :
         public constraints::DifferentiableFunction
       {
@@ -95,6 +99,8 @@ namespace hpp {
           }
       }; // class Function
 
+      /// Compute the difference between the value of the function in two points.
+      /// i.e.: \f$ f (q_0, ... , q_n) = f_{inner} (q_{left}) - f_{inner} (q_{right}) \f$
       class HPP_MANIPULATION_LOCAL EdgeFunction :
         public constraints::DifferentiableFunction
       {
diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc
new file mode 100644
index 0000000000000000000000000000000000000000..107baac9bfc62da4668d1bcd758e5509fa2c1cd7
--- /dev/null
+++ b/src/steering-method/graph.cc
@@ -0,0 +1,107 @@
+// Copyright (c) 2014, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#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/manipulation/graph/edge.hh>
+
+namespace hpp {
+  namespace manipulation {
+    SteeringMethod::SteeringMethod (const Problem& problem) :
+      core::SteeringMethod (problem), problem_ (problem),
+      steeringMethod_ (core::SteeringMethodStraight::create (problem))
+    {
+    }
+
+    SteeringMethod::SteeringMethod (const SteeringMethod& other):
+      core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_
+      (other.steeringMethod_)
+    {
+    }
+
+    namespace steeringMethod {
+
+      GraphPtr_t Graph::create
+        (const core::Problem& problem)
+        {
+          HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
+          const Problem& p = static_cast <const Problem&> (problem);
+          return create (p);
+        }
+
+      GraphPtr_t Graph::create
+        (const Problem& problem)
+        {
+          Graph* ptr = new Graph (problem);
+          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 Problem& problem) :
+        SteeringMethod (problem), weak_ ()
+      {
+      }
+
+      Graph::Graph (const Graph& other):
+        SteeringMethod (other)
+      {
+      }
+
+      PathPtr_t Graph::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+      {
+        graph::Edges_t possibleEdges;
+        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