Merge pull request #132 from jcarpent/devel

Add forward dynamics with contacts algorithm 

CMakeLists.txt

@@ -144,6 +144,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/cholesky.hxx
   algorithm/kinematics.hpp
   algorithm/kinematics.hxx
+  algorithm/dynamics.hpp
   algorithm/center-of-mass.hpp
   algorithm/center-of-mass.hxx
   algorithm/non-linear-effects.hpp

cmake @ 9f9a9000

-Subproject commit 7cdd2146508a3431adb8e5a0d4233704080299cd
+Subproject commit 9f9a90007704d679c631431ab36189f7ba61f323

src/algorithm/cholesky.hpp

@@ -40,6 +40,8 @@ namespace se3
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
     ///
+    /// \return A reference to the upper triangular matrix \f$U\f$.
+    ///
     inline const Eigen::MatrixXd &
     decompose(const Model & model,
               Data & data);
@@ -54,7 +56,7 @@ namespace se3
     ///
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
-    /// \param[inout] y The input vector to inverse which also contains the result \f$x\f$ of the inversion.
+    /// \param[inout] y The input matrix to inverse which also contains the result \f$x\f$ of the inversion.
     ///
     template<typename Mat>
     Mat & solve(const Model & model, const Data & data,
@@ -65,7 +67,7 @@ namespace se3
     ///
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
-    /// \param[inout] v The input vector to multiply with data.M and storing the result.
+    /// \param[inout] v The input matrix to multiply with data.M and storing the result.
     /// \param[in] usingCholesky If true, use the Cholesky decomposition stored in data. Exploit the sparsity of the kinematic tree.
     ///
     /// \return A reference to the result of \f$ Mv \f$.
@@ -80,7 +82,7 @@ namespace se3
     ///
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
-    /// \param[inout] v The input vector to multiply with data.U and also storing the result.
+    /// \param[inout] v The input matrix to multiply with data.U and also storing the result.
     ///
     /// \return A reference to the result of \f$ Uv \f$ stored in v.
     ///
@@ -94,7 +96,7 @@ namespace se3
     ///
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
-    /// \param[inout] v The input vector to multiply with data.U.tranpose() and also storing the result.
+    /// \param[inout] v The input matrix to multiply with data.U.tranpose() and also storing the result.
     ///
     /// \return A reference to the result of \f$ U^{\top}v \f$ stored in v.
     ///
@@ -108,7 +110,7 @@ namespace se3
     ///
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
-    /// \param[inout] v The input vector to multiply with data.U^{-1} and also storing the result.
+    /// \param[inout] v The input matrix to multiply with data.U^{-1} and also storing the result.
     ///
     /// \return A reference to the result of \f$ U^{-1}v \f$ stored in v.
     ///
@@ -124,7 +126,7 @@ namespace se3
     ///
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
-    /// \param[inout] v The input vector to multiply with data.U^{-\top} and also storing the result.
+    /// \param[inout] v The input matrix to multiply with data.U^{-\top} and also storing the result.
     ///
     /// \return A reference to the result of \f$ U^{-\top}v \f$ stored in v.
     ///
@@ -140,7 +142,7 @@ namespace se3
     ///
     /// \param[in] model The model structure of the rigid body system.
     /// \param[in] data The data structure of the rigid body system.
-    /// \param[inout] v The input vector to multiply with data.M^{-1} and also storing the result.
+    /// \param[inout] v The input matrix to multiply with data.M^{-1} and also storing the result.
     ///
     /// \return A reference to the result of \f$ M^{-1}v \f$ stored in v.
     ///

src/algorithm/cholesky.hxx

@@ -71,13 +71,13 @@ namespace se3
              const Data & data,
              Eigen::MatrixBase<Mat> & v)
     {
-      assert(v.size() == model.nv);
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+      assert(v.rows() == model.nv);
+      
       const Eigen::MatrixXd & U = data.U;
       const std::vector<int> & nvt = data.nvSubtree_fromRow;
       
       for(int k=0;k < model.nv-1;++k) // You can stop one step before nv
-        v[k] += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1);
+        v.row(k) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1);
       
       return v.derived();
     }
@@ -88,13 +88,12 @@ namespace se3
               const Data & data,
               Eigen::MatrixBase<Mat> & v)
     {
-      assert(v.size() == model.nv);
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+      assert(v.rows() == model.nv);
       
       const Eigen::MatrixXd & U = data.U;
       const std::vector<int> & nvt = data.nvSubtree_fromRow;
-      for( int i=model.nv-2;i>=0;--i ) // You can start from nv-2 (no child in nv-1)
-        v.segment(i+1,nvt[(Model::Index)i]-1) += U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i];
+      for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1)
+        v.middleRows(k+1,nvt[(Model::Index)k]-1) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k);
       
       return v.derived();
     }
@@ -109,14 +108,13 @@ namespace se3
     {
       /* We search y s.t. v = U y. 
        * For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */
-      assert(v.size() == model.nv);
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+      assert(v.rows() == model.nv);
       
       const Eigen::MatrixXd & U = data.U;
       const std::vector<int> & nvt = data.nvSubtree_fromRow;
       
       for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1)
-        v[k] -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1);
+        v.row(k) -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1);
       return v.derived();
     }
 
@@ -127,13 +125,12 @@ namespace se3
     {
       /* We search y s.t. v = U' y. 
        * For any k, v_k = y_k + sum_{m \in parent{k}} U(m,k) v(k). */
-      assert(v.size() == model.nv);
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+      assert(v.rows() == model.nv);
       
       const Eigen::MatrixXd & U = data.U;
       const std::vector<int> & nvt = data.nvSubtree_fromRow;
-      for( int i=0;i<model.nv-1;++i ) // You can stop one step before nv.
-        v.segment(i+1,nvt[(Model::Index)i]-1) -= U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i];
+      for( int k=0;k<model.nv-1;++k ) // You can stop one step before nv.
+        v.middleRows(k+1,nvt[(Model::Index)k]-1) -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k);
 
       return v.derived();
     }
@@ -145,8 +142,7 @@ namespace se3
              const Data & data,
              const Eigen::MatrixBase<Mat> & v)
       {
-        assert(v.size() == model.nv);
-        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+        assert(v.rows() == model.nv);
         
         const Eigen::MatrixXd & M = data.M;
         const std::vector<int> & nvt = data.nvSubtree_fromRow;
@@ -154,8 +150,8 @@ namespace se3
         
         for(int k=model.nv-1;k>=0;--k)
         {
-          res[k] = M.row(k).segment(k,nvt[(Model::Index)k]) * v.segment(k,nvt[(Model::Index)k]);
-          res.segment(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v[k];
+          res.row(k) = M.row(k).segment(k,nvt[(Model::Index)k]) * v.middleRows(k,nvt[(Model::Index)k]);
+          res.middleRows(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k);
         }
         
         return res;
@@ -167,7 +163,7 @@ namespace se3
                   Eigen::MatrixBase<Mat> & v)
       {
         Utv(model,data,v);
-        for( int k=0;k<model.nv;++k ) v[k] *= data.D[k];
+        for( int k=0;k<model.nv;++k ) v.row(k) *= data.D[k];
         return Uv(model,data,v);
       }
     } // internal
@@ -188,7 +184,7 @@ namespace se3
                 Eigen::MatrixBase<Mat> & v)
     {
       Uiv(model,data,v);
-      for(int k=0;k<model.nv;++k) v[k] /= data.D[k];
+      for(int k=0;k<model.nv;++k) v.row(k) /= data.D[k];
       return Utiv(model,data,v);
     }
 

src/algorithm/dynamics.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio 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.
+//
+// Pinocchio 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
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_dynamics_hpp__
+#define __se3_dynamics_hpp__
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/simulation/compute-all-terms.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+
+#include <Eigen/Cholesky>
+namespace se3
+{
+  
+  ///
+  /// \brief Compute the forward dynamics with contact constraints.
+  /// \note It computes the following problem: <BR>
+  ///       <CENTER> \f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\
+  ///           \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$ </CENTER> <BR>
+  ///       where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints),
+  ///       \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] q The joint configuration (vector dim model.nq).
+  /// \param[in] v The joint velocity (vector dim model.nv).
+  /// \param[in] tau The joint torque vector (dim model.nv).
+  /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
+  /// \param[in] gamma The drift of the constraints (dim nb_constraints).
+  /// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data.
+  ///
+  /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda vector.
+  ///
+  inline const Eigen::VectorXd & forwardDynamics(const Model & model,
+                                                 Data & data,
+                                                 const Eigen::VectorXd & q,
+                                                 const Eigen::VectorXd & v,
+                                                 const Eigen::VectorXd & tau,
+                                                 const Eigen::MatrixXd & J,
+                                                 const Eigen::VectorXd & gamma,
+                                                 const bool updateKinematics = true
+                                                 )
+  {
+    assert(q.size() == model.nq);
+    assert(v.size() == model.nv);
+    assert(tau.size() == model.nv);
+    assert(J.cols() == model.nv);
+    assert(J.rows() == gamma.size());
+    
+    Eigen::VectorXd & a = data.ddq;
+    Eigen::VectorXd & lambda = data.lambda;
+    
+    if (updateKinematics)
+      computeAllTerms(model, data, q, v);
+    
+    // Compute the UDUt decomposition of data.M
+    cholesky::decompose(model, data);
+    
+    // Compute the dynamic drift (control - nle)
+    data.torque_residual = tau - data.nle;
+    cholesky::solve(model, data, data.torque_residual);
+    
+    data.sDUiJt = J.transpose();
+    // Compute U^-1 * J.T
+    cholesky::Uiv(model, data, data.sDUiJt);
+    for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
+    
+    data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
+    data.llt_JMinvJt.compute(data.JMinvJt);
+    
+    // Compute the Lagrange Multipliers
+    lambda = -gamma -J*data.torque_residual;
+    data.llt_JMinvJt.solveInPlace (lambda);
+    
+    // Compute the joint acceleration
+    a = J.transpose() * lambda;
+    cholesky::solve (model, data, a);
+    a += data.torque_residual;
+    
+    return a;
+  }
+} // namespace se3
+
+
+#endif // ifndef __se3_dynamics_hpp__

src/multibody/model.hpp

@@ -29,6 +29,7 @@
 #include "pinocchio/multibody/joint/joint-variant.hpp"
 #include "pinocchio/tools/string-generator.hpp"
 #include <iostream>
+#include <Eigen/Cholesky>
 
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia)
@@ -472,6 +473,23 @@ namespace se3
     /// \brief Potential energy of the model.
     double potential_energy;
     
+    // Temporary variables used in forward dynamics
+    
+    /// \brief Inverse of the operational-space inertia matrix
+    Eigen::MatrixXd JMinvJt;
+    
+    /// \brief Cholesky decompostion of \JMinvJt.
+    Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt;
+    
+    /// \brief Lagrange Multipliers corresponding to contact forces.
+    Eigen::VectorXd lambda;
+    
+    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
+    Eigen::MatrixXd sDUiJt;
+    
+    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
+    Eigen::VectorXd torque_residual;
+    
     ///
     /// \brief Default constructor of se3::Data from a se3::Model.
     ///

src/multibody/model.hxx

@@ -279,6 +279,11 @@ namespace se3
     ,velocityLimit(ref.nv)
     ,lowerPositionLimit(ref.nq)
     ,upperPositionLimit(ref.nq)
+    ,JMinvJt()
+    ,llt_JMinvJt()
+    ,lambda()
+    ,sDUiJt(ref.nv,ref.nv)
+    ,torque_residual(ref.nv)
   {
     /* Create data strcture associated to the joints */
     for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) 

src/python/algorithms.hpp

@@ -28,6 +28,7 @@
 #include "pinocchio/algorithm/non-linear-effects.hpp"
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/dynamics.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
 #include "pinocchio/algorithm/operational-frames.hpp"
@@ -94,6 +95,19 @@ namespace se3
         aba(*model,*data,q,v,tau);
         return data->ddq;
       }
+      
+      static Eigen::MatrixXd fd_llt_proxy(const ModelHandler & model,
+                                          DataHandler & data,
+                                          const VectorXd_fx & q,
+                                          const VectorXd_fx & v,
+                                          const VectorXd_fx & tau,
+                                          const eigenpy::MatrixXd_fx & J,
+                                          const VectorXd_fx & gamma,
+                                          const bool update_kinematics = true)
+      {
+        forwardDynamics(*model,*data,q,v,tau,J,gamma,update_kinematics);
+        return data->ddq;
+      }
 
       static SE3::Vector3
       com_0_proxy(const ModelHandler& model,
@@ -327,6 +341,16 @@ namespace se3
                          "Joint torque tau (size Model::nv)"),
                 "Compute ABA, put the result in Data::ddq and return it.");
         
+        bp::def("forwardDynamics",fd_llt_proxy,
+                bp::args("Model","Data",
+                         "Joint configuration q (size Model::nq)",
+                         "Joint velocity v (size Model::nv)",
+                         "Joint torque tau (size Model::nv)",
+                         "Contact Jacobian J (size nb_constraint * Model::nv)",
+                         "Contact drift gamma (size nb_constraint)",
+                         "Update kinematics (if true, it updates the dynamic variable according to the current state)"),
+                "Solve the forward dynamics problem with contacts, put the result in Data::ddq and return it.");
+        
         bp::def("centerOfMass",com_0_proxy,
                 bp::args("Model","Data",
                          "Configuration q (size Model::nq)",

src/python/data.hpp

@@ -112,6 +112,8 @@ namespace se3
         
         .ADD_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
         .ADD_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
+        
+        .ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
 	  ;
       }
 
@@ -159,6 +161,8 @@ namespace se3
       IMPL_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
       IMPL_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
       
+      IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
+      
       /* --- Expose --------------------------------------------------------- */
       static void expose()
       {

src/spatial/se3.hpp

 //
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015-2016 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -109,9 +109,9 @@ namespace se3
         return derived().__equal__(other);
       }
 
-      bool isApprox (const Derived_t & other) const
+      bool isApprox (const Derived_t & other, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
       {
-        return derived().isApprox_impl(other);
+        return derived().isApprox_impl(other, prec);
       }
 
       friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X)
@@ -164,12 +164,15 @@ namespace se3
     SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p) 
     : rot(R), trans(p)
     {
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(v3,3)
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M3,3,3)
     }
 
     template<typename M4>
     SE3Tpl(const Eigen::MatrixBase<M4> & m) 
     : rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR))
     {
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M4,4,4);
     }
 
     SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
@@ -216,8 +219,7 @@ namespace se3
 
       return *this;
     }
-
-  public:
+    
     Matrix4 toHomogeneousMatrix_impl() const
     {
       Matrix4 M;
@@ -244,7 +246,6 @@ namespace se3
       return M;
     }
 
-
     void disp_impl(std::ostream & os) const
     {
       os << "  R =\n" << rot << std::endl
@@ -265,8 +266,8 @@ namespace se3
       return d.se3ActionInverse(*this);
     }
 
-    Vector3 act_impl   (const Vector3& p) const { return (rot*p+trans).eval(); }
-    Vector3 actInv_impl(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }
+    Vector3 act_impl   (const Vector3& p) const { return rot*p+trans; }
+    Vector3 actInv_impl(const Vector3& p) const { return rot.transpose()*(p-trans); }
 
     SE3Tpl act_impl    (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
     SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}
@@ -279,15 +280,11 @@ namespace se3
       return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation());
     }
 
-    bool isApprox_impl( const SE3Tpl & m2 ) const
+    bool isApprox_impl (const SE3Tpl & m2, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
     {
-      Matrix4 diff( toHomogeneousMatrix_impl() - 
-                              m2.toHomogeneousMatrix_impl());
-      return (diff.isMuchSmallerThan(toHomogeneousMatrix_impl(), 1e-14)
-              && diff.isMuchSmallerThan(m2.toHomogeneousMatrix_impl(), 1e-14) );
+      return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec);
     }
 
-  public:
     const Angular_t & rotation_impl() const { return rot; }
     Angular_t & rotation_impl() { return rot; }
     void rotation_impl(const Angular_t & R) { rot = R; }

unittest/CMakeLists.txt

@@ -55,6 +55,7 @@ ADD_UNIT_TEST(crba eigen3)
 ADD_UNIT_TEST(com eigen3)
 ADD_UNIT_TEST(jacobian eigen3)
 ADD_UNIT_TEST(cholesky eigen3)
+ADD_UNIT_TEST(dynamics eigen3)
 
 IF(URDFDOM_FOUND)
   ADD_UNIT_TEST(urdf "eigen3;urdfdom")
@@ -88,4 +89,4 @@ ENDIF(LUA5_1_FOUND)
 ADD_UNIT_TEST(joints eigen3)
 ADD_UNIT_TEST(compute-all-terms eigen3)
 ADD_UNIT_TEST(energy eigen3)
-ADD_UNIT_TEST(operational-frames eigen3)
\ No newline at end of file
+ADD_UNIT_TEST(operational-frames eigen3)

unittest/dynamics.cpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio 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.
+//
+// Pinocchio 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
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/dynamics.hpp"
+#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/tools/timer.hpp"
+
+#include <iostream>
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE DynamicTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+
+BOOST_AUTO_TEST_SUITE ( DynamicsTest)
+
+BOOST_AUTO_TEST_CASE ( test_FD )
+{
+  using namespace Eigen;
+  using namespace se3;
+  
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model,true);
+  se3::Data data(model);
+  
+  VectorXd q = VectorXd::Ones(model.nq);
+  q.segment <4> (3).normalize();
+  
+  se3::computeJacobians(model, data, q);
+  
+  VectorXd v = VectorXd::Ones(model.nv);
+  VectorXd tau = VectorXd::Zero(model.nv);
+  
+  const std::string RF = "rleg6_body";
+  const std::string LF = "lleg6_body";
+  
+  Data::Matrix6x J_RF (6, model.nv);
+  getJacobian <true> (model, data, model.getBodyId(RF), J_RF);
+  Data::Matrix6x J_LF (6, model.nv);
+  getJacobian <true> (model, data, model.getBodyId(LF), J_LF);
+  
+  Eigen::MatrixXd J (12, model.nv);
+  J.topRows<6> () = J_RF;
+  J.bottomRows<6> () = J_LF;
+  
+  Eigen::VectorXd gamma (VectorXd::Ones(12));
+  
+  Eigen::MatrixXd H(J.transpose());
+  
+  se3::forwardDynamics(model, data, q, v, tau, J, gamma, true);
+  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
+  
+  MatrixXd Minv (data.M.inverse());
+  MatrixXd JMinvJt (J * Minv * J.transpose());
+  
+  Eigen::MatrixXd G_ref(J.transpose());
+  cholesky::Uiv(model, data, G_ref);
+  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
+    Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
+    BOOST_CHECK(H_ref.isApprox(JMinvJt,1-12));
+  
+  VectorXd lambda_ref = -JMinvJt.inverse() * (J*Minv*(tau - data.nle) + gamma);
+  BOOST_CHECK(data.lambda.isApprox(lambda_ref, 1e-12));
+    
+  VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref);
+  
+  Eigen::VectorXd dynamics_residual_ref (data.M * a_ref + data.nle - tau - J.transpose()*lambda_ref);
+  BOOST_CHECK(dynamics_residual_ref.norm() <= 1e-12);
+  
+  Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
+  BOOST_CHECK(constraint_residual.norm() <= 1e-12);
+  
+  Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda);
+  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
+}
+
+BOOST_AUTO_TEST_CASE (timings_fd_llt)
+{
+  using namespace Eigen;
+  using namespace se3;
+  
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model,true);
+  se3::Data data(model);
+  
+#ifdef NDEBUG
+#ifdef _INTENSE_TESTING_
+  const size_t NBT = 1000*1000;
+#else
+  const size_t NBT = 100;
+#endif
+  
+#else
+  const size_t NBT = 1;
+  std::cout << "(the time score in debug mode is not relevant)  " ;
+#endif // ifndef NDEBUG
+  
+  VectorXd q = VectorXd::Ones(model.nq);
+  q.segment <4> (3).normalize();