Commit de18e056 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #132 from jcarpent/devel

Add forward dynamics with contacts algorithm 
parents 9d42031d 3c047ae3
......@@ -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
......
Subproject commit 7cdd2146508a3431adb8e5a0d4233704080299cd
Subproject commit 9f9a90007704d679c631431ab36189f7ba61f323
......@@ -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.
///
......
......@@ -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);
}
......
//
// 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__
......@@ -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.
///
......
......@@ -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)
......
......@@ -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"
......@@ -95,6 +96,19 @@ namespace se3
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,
DataHandler & data,
......@@ -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)",
......
......@@ -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()
{
......
//
// 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()) {}
......@@ -217,7 +220,6 @@ 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; }
......
......@@ -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")
......
//
// 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();
se3::computeJacobians(model, data, q);
VectorXd v = VectorXd::Ones(model.nv);
VectorXd tau = VectorXd::Zero(model.nv);
const std::string