diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index a154034084f1c2fdca52e006cb00d8ed3730cc74..737d5e5dc6b2d6d37eadb77756e062a064ae8a79 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -28,6 +28,7 @@
 
 #include <hpp/pinocchio/configuration.hh>
 
+#include <hpp/constraints/affine-function.hh>
 #include <hpp/constraints/locked-joint.hh>
 #include <hpp/constraints/solver/by-substitution.hh>
 #include <hpp/constraints/explicit.hh>
@@ -316,7 +317,7 @@ namespace hpp {
               lj->rightHandSideFromConfig (q2);
               rhs = lj->rightHandSide();
             } else {
-              f = Identity::Ptr_t (new Identity (lj->configSpace(), os.str()));
+              f = constraints::Identity::create (lj->configSpace(), os.str());
               ic = _row(lj->outputConf()    , (i - 1) * nq);
               oc = _row(lj->outputConf()    ,  i      * nq);
               iv = _col(lj->outputVelocity(), (i - 1) * nv);
diff --git a/src/steering-method/cross-state-optimization/function.cc b/src/steering-method/cross-state-optimization/function.cc
index 3ba0c3d79aefbf8eafacf286723ea8ec674ef6ea..b74aa61b27cb4aa08879d58a0744fec05310afec 100644
--- a/src/steering-method/cross-state-optimization/function.cc
+++ b/src/steering-method/cross-state-optimization/function.cc
@@ -76,29 +76,6 @@ 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
-      {
-        public:
-          typedef boost::shared_ptr<Identity> Ptr_t;
-
-          Identity (const LiegroupSpacePtr_t space, const std::string& name) :
-            DifferentiableFunction (space->nq(), space->nv(), space, name) {}
-
-        protected:
-          void impl_compute (LiegroupElement& y, vectorIn_t arg) const
-          {
-            y.vector() = arg;
-          }
-
-          void impl_jacobian (matrixOut_t J, vectorIn_t) const
-          {
-            J.setIdentity();
-          }
-      }; // 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 :