Merge pull request #1332 from jcarpent/topic/kinematic-regressors

Add computations of kinematic regressors

README.md

@@ -49,6 +49,7 @@ conda install pinocchio -c conda-forge
    - forward/inverse dynamics and their analytical derivatives,
    - centroidal dynamics and its analytical derivatives,
    - support of multiple precision arithmetic via Boost.Multiprecision or any similar framework,
+   - computations of kinematic and dynamic regressors for system identification and more,
    - and much more with the support of modern Automatic Differentiation frameworks like [CppAD](https://github.com/coin-or/CppAD) or [CasADi](https://web.casadi.org/).
 
 **Pinocchio** is flexible:
@@ -58,7 +59,7 @@ conda install pinocchio -c conda-forge
 
 **Pinocchio** is extensible.  
 **Pinocchio** is multi-thread friendly.  
-**Pinocchio** is reliable and extensively tested (unit-tests, simulations and real robotics applications).  
+**Pinocchio** is reliable and extensively tested (unit-tests, simulations and real world robotics applications).  
 **Pinocchio** is supported and tested on Windows, Mac OS X, Unix and Linux ([see build status here](http://robotpkg.openrobots.org/rbulk/robotpkg/math/pinocchio/index.html)).
 
 ## Performances

bindings/python/algorithm/algorithms.hpp

@@ -27,6 +27,7 @@ namespace pinocchio
     void exposeCAT();
     void exposeJacobian();
     void exposeGeometryAlgo();
+    void exposeKinematicRegressor();
     void exposeRegressor();
     void exposeCholesky();
     void exposeModelAlgo();

bindings/python/algorithm/expose-algorithms.cpp

@@ -24,6 +24,7 @@ namespace pinocchio
       exposeCAT();
       exposeJacobian();
       exposeGeometryAlgo();
+      exposeKinematicRegressor();
       exposeRegressor();
       exposeCholesky();
       exposeModelAlgo();

bindings/python/algorithm/expose-kinematic-regressor.cpp

+//
+// Copyright (c) 2020 INRIA
+//
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/algorithm/regressor.hpp"
+
+namespace pinocchio
+{
+  namespace python
+  {
+
+    void exposeKinematicRegressor()
+    {
+      using namespace Eigen;
+
+      bp::def("computeJointKinematicRegressor",
+              (Data::Matrix6x (*)(const Model &, const Data &, const JointIndex, const ReferenceFrame, const SE3 &))&computeJointKinematicRegressor<double,0,JointCollectionDefaultTpl>,
+              bp::args("model","data","joint_id","reference_frame","placement"),
+              "Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame.\n\n"
+              "Parameters:\n"
+              "\tmodel: model of the kinematic tree\n"
+              "\tdata: data related to the model\n"
+              "\tjoint_id: index of the joint\n"
+              "\treference_frame: reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD)\n"
+              "\tplacement: relative placement to the joint frame\n");
+      
+      bp::def("computeJointKinematicRegressor",
+              (Data::Matrix6x (*)(const Model &, const Data &, const JointIndex, const ReferenceFrame))&computeJointKinematicRegressor<double,0,JointCollectionDefaultTpl>,
+              bp::args("model","data","joint_id","reference_frame"),
+              "Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the joint given as input.\n\n"
+              "Parameters:\n"
+              "\tmodel: model of the kinematic tree\n"
+              "\tdata: data related to the model\n"
+              "\tjoint_id: index of the joint\n"
+              "\treference_frame: reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD)\n");
+
+      bp::def("computeFrameKinematicRegressor",
+              (Data::Matrix6x (*)(const Model &, Data &, const FrameIndex, const ReferenceFrame))&computeFrameKinematicRegressor<double,0,JointCollectionDefaultTpl>,
+              bp::args("model","data","frame_id","reference_frame"),
+              "Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the frame given as input.\n\n"
+              "Parameters:\n"
+              "\tmodel: model of the kinematic tree\n"
+              "\tdata: data related to the model\n"
+              "\tframe_id: index of the frame\n"
+              "\treference_frame: reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD)\n");
+    }
+    
+  } // namespace python
+} // namespace pinocchio

src/algorithm/regressor.hpp

 //
-// Copyright (c) 2018 CNRS
+// Copyright (c) 2018-2020 CNRS INRIA
 //
 
-#ifndef __pinocchio_regressor_hpp__
-#define __pinocchio_regressor_hpp__
+#ifndef __pinocchio_algorithm_regressor_hpp__
+#define __pinocchio_algorithm_regressor_hpp__
 
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/data.hpp"
 
 namespace pinocchio
 {
+  
+  ///
+  /// \copydoc computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> &,const DataTpl<Scalar,Options,JointCollectionTpl> &, const JointIndex, const ReferenceFrame, const SE3Tpl<Scalar,Options> &)
+  ///
+  /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0.
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
+  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                      const JointIndex joint_id,
+                                      const ReferenceFrame rf,
+                                      const SE3Tpl<Scalar,Options> & placement,
+                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
+
+
+  ///
+  /// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree
+  ///        to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame.
+  ///
+  /// \remarks It assumes that the \ref forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] joint_id Index of the joint.
+  /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
+  /// \param[in] placement Relative placement to the joint frame.
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
+  computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                 const JointIndex joint_id,
+                                 const ReferenceFrame rf,
+                                 const SE3Tpl<Scalar,Options> & placement)
+  {
+    typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
+    ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
+    
+    computeJointKinematicRegressor(model,data,joint_id,rf,placement,res);
     
+    return res;
+  }
+
+  ///
+  /// \copydoc computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> &,const DataTpl<Scalar,Options,JointCollectionTpl> &, const JointIndex, const ReferenceFrame)
+  ///
+  /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0.
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
+  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                      const JointIndex joint_id,
+                                      const ReferenceFrame rf,
+                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
+
+  ///
+  /// \brief Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree
+  ///        to the placement variation of the joint given as input.
+  ///
+  /// \remarks It assumes that the \ref forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] joint_id Index of the joint.
+  /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
+  computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                 const JointIndex joint_id,
+                                 const ReferenceFrame rf)
+  {
+    typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
+    ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
+    
+    computeJointKinematicRegressor(model,data,joint_id,rf,res);
+    
+    return res;
+  }
+  
+  ///
+  /// \copydoc computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const FrameIndex, const ReferenceFrame)
+  ///
+  /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0.
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
+  void computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                      DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                      const FrameIndex frame_id,
+                                      const ReferenceFrame rf,
+                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor);
+  
+  ///
+  /// \brief Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree
+  ///        to the placement variation of the frame given as input.
+  ///
+  /// \remarks It assumes that the \ref framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] frame_id Index of the frame.
+  /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
+  computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                 DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                 const FrameIndex frame_id,
+                                 const ReferenceFrame rf)
+  {
+    typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
+    ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
+    
+    computeFrameKinematicRegressor(model,data,frame_id,rf,res);
+    
+    return res;
+  }
+
   ///
   /// \brief Computes the static regressor that links the center of mass positions of all the links
   ///        to the center of mass of the complete model according to the current configuration of the robot.
@@ -75,7 +193,9 @@ namespace pinocchio
   ///
   template<typename MotionVelocity, typename MotionAcceleration, typename OutputType>
   inline void
-  bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a, const Eigen::MatrixBase<OutputType> & regressor);
+  bodyRegressor(const MotionDense<MotionVelocity> & v,
+                const MotionDense<MotionAcceleration> & a,
+                const Eigen::MatrixBase<OutputType> & regressor);
 
   ///
   /// \brief Computes the regressor for the dynamic parameters of a single rigid body.
@@ -90,7 +210,8 @@ namespace pinocchio
   ///
   template<typename MotionVelocity, typename MotionAcceleration>
   inline Eigen::Matrix<typename MotionVelocity::Scalar,6,10,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options>
-  bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a);
+  bodyRegressor(const MotionDense<MotionVelocity> & v,
+                const MotionDense<MotionAcceleration> & a);
 
   ///
   /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given joint,
@@ -128,7 +249,7 @@ namespace pinocchio
   /// \param[in] data The data structure of the rigid body system.
   /// \param[in] frameId The id of the frame.
   ///
-  /// \return The regressor of the body.
+  /// \return The dynamic regressor of the body.
   ///
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
   inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType &
@@ -172,4 +293,4 @@ namespace pinocchio
 /* --- Details -------------------------------------------------------------------- */
 #include "pinocchio/algorithm/regressor.hxx"
 
-#endif // ifndef __pinocchio_regressor_hpp__
+#endif // ifndef __pinocchio_algorithm_regressor_hpp__

src/algorithm/regressor.hxx

 //
-// Copyright (c) 2018-2019 CNRS INRIA
+// Copyright (c) 2018-2020 CNRS INRIA
 //
 
-#ifndef __pinocchio_regressor_hxx__
-#define __pinocchio_regressor_hxx__
+#ifndef __pinocchio_algorithm_regressor_hxx__
+#define __pinocchio_algorithm_regressor_hxx__
 
 #include "pinocchio/algorithm/check.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
@@ -12,6 +12,103 @@
 
 namespace pinocchio
 {
+
+  namespace internal
+  {
+    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
+    void computeJointKinematicRegressorGeneric(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                               const JointIndex joint_id,
+                                               const ReferenceFrame rf,
+                                               const SE3Tpl<Scalar,Options> & global_frame_placement,
+                                               const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
+    {
+      assert(model.check(data) && "data is not consistent with model.");
+      PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.rows(), 6);
+      PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.cols(), 6*(model.njoints-1));
+      
+      typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
+      typedef typename Data::SE3 SE3;
+      
+      Matrix6xReturnType & kinematic_regressor_ = kinematic_regressor.const_cast_derived();
+      kinematic_regressor_.setZero();
+      
+      const SE3Tpl<Scalar,Options> & oMi = global_frame_placement;
+      SE3 oMp; // placement of the frame following the jointPlacement transform
+      SE3 iMp; // relative placement between the joint frame and the jointPlacement
+      for(JointIndex i = joint_id; i > 0; i = model.parents[i])
+      {
+        const JointIndex parent_id = model.parents[i];
+        oMp = data.oMi[parent_id] * model.jointPlacements[i];
+        switch(rf)
+        {
+          case LOCAL:
+            iMp = oMi.actInv(oMp);
+            kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6*(i-1))) = iMp.toActionMatrix(); // TODO: we can avoid a copy
+            break;
+          case LOCAL_WORLD_ALIGNED:
+            iMp.rotation() = oMp.rotation();
+            iMp.translation() = oMp.translation() - oMi.translation();
+            kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6*(i-1))) = iMp.toActionMatrix(); // TODO: we can avoid a copy
+            break;
+          case WORLD:
+            kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6*(i-1))) = oMp.toActionMatrix(); // TODO: we can avoid a copy
+            break;
+        }
+      }
+    }
+  }
+
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
+  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                      const JointIndex joint_id,
+                                      const ReferenceFrame rf,
+                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
+  {
+    PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints);
+    internal::computeJointKinematicRegressorGeneric(model,data,joint_id,rf,data.oMi[joint_id],
+                                                    kinematic_regressor.const_cast_derived());
+  }
+
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
+  void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                      const JointIndex joint_id,
+                                      const ReferenceFrame rf,
+                                      const SE3Tpl<Scalar,Options> & placement,
+                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
+  {
+    PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints);
+    
+    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
+    typedef typename Data::SE3 SE3;
+    
+    const SE3 global_placement = data.oMi[joint_id] * placement;
+    
+    internal::computeJointKinematicRegressorGeneric(model,data,joint_id,rf,global_placement,
+                                                    kinematic_regressor.const_cast_derived());
+  }
+
+  
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType>
+  void computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                      DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                      const FrameIndex frame_id,
+                                      const ReferenceFrame rf,
+                                      const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
+  {
+    PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id > 0 && (Eigen::DenseIndex)frame_id < model.nframes);
+    
+    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+    typedef typename Model::Frame Frame;
+
+    const Frame & frame = model.frames[frame_id];
+    data.oMf[frame_id] = data.oMi[frame.parent] * frame.placement;
+    
+    internal::computeJointKinematicRegressorGeneric(model,data,frame.parent,rf,data.oMf[frame_id],
+                                                    kinematic_regressor.const_cast_derived());
+  }
   
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
   inline typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
@@ -128,7 +225,8 @@ namespace pinocchio
 
   template<typename MotionVelocity, typename MotionAcceleration>
   inline Eigen::Matrix<typename MotionVelocity::Scalar,6,10,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options>
-  bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a)
+  bodyRegressor(const MotionDense<MotionVelocity> & v,
+                const MotionDense<MotionAcceleration> & a)
   {
     typedef typename MotionVelocity::Scalar Scalar;
     enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options };
@@ -293,4 +391,4 @@ namespace pinocchio
 
 } // namespace pinocchio
 
-#endif // ifndef __pinocchio_regressor_hxx__
+#endif // ifndef __pinocchio_algorithm_regressor_hxx__

unittest/python/CMakeLists.txt

@@ -24,6 +24,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
 
   # Algo
   bindings_com
+  bindings_kinematic_regressor
   bindings_regressor
   bindings_dynamics
   bindings_kinematics

unittest/python/bindings_kinematic_regressor.py

+import unittest
+from test_case import PinocchioTestCase as TestCase
+import pinocchio as pin
+
+class TestKinematicRegressorBindings(TestCase):
+
+    def test_all(self):
+        model = pin.buildSampleModelHumanoidRandom()
+
+        joint_name = "larm6_joint"
+        joint_id = model.getJointId(joint_name)
+        frame_id = model.addBodyFrame("test_body", joint_id, pin.SE3.Identity(), -1)
+
+        data = model.createData()
+
+        model.lowerPositionLimit[:7] = -1.
+        model.upperPositionLimit[:7] = 1.
+
+        q = pin.randomConfiguration(model)
+        pin.forwardKinematics(model,data,q)
+
+        R1 = pin.computeJointKinematicRegressor(model,data,joint_id,pin.ReferenceFrame.LOCAL,pin.SE3.Identity())
+        R2 = pin.computeJointKinematicRegressor(model,data,joint_id,pin.ReferenceFrame.LOCAL)
+
+        self.assertApprox(R1,R2)
+
+        R3 = pin.computeFrameKinematicRegressor(model,data,frame_id,pin.ReferenceFrame.LOCAL)
+        self.assertApprox(R1,R3)
+
+if __name__ == '__main__':
+    unittest.main()
+

unittest/regressor.cpp

 //
-// Copyright (c) 2018 CNRS
+// Copyright (c) 2018-2020 CNRS INRIA
 //
 
 #include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/explog.hpp"
 #include "pinocchio/algorithm/regressor.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/center-of-mass.hpp"
 #include "pinocchio/parsers/sample-models.hpp"
@@ -16,6 +18,204 @@
 
 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
+BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  pinocchio::Model model; buildModels::humanoidRandom(model);
+  
+  pinocchio::Data data(model);
+  pinocchio::Data data_ref(model);
+  
+  model.lowerPositionLimit.head<7>().fill(-1.);
+  model.upperPositionLimit.head<7>().fill(1.);
+  
+//  const std::string joint_name = "larm5_joint";
+//  const JointIndex joint_id = model.getJointId(joint_name);
+  
+  const VectorXd q = randomConfiguration(model);
+  
+  forwardKinematics(model,data,q);
+  
+  const double eps = 1e-8;
+  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
+  {
+    Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    
+    Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    
+    computeJointKinematicRegressor(model, data, joint_id, LOCAL, kinematic_regressor_L);
+    computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA);
+    computeJointKinematicRegressor(model, data, joint_id, WORLD, kinematic_regressor_W);
+    
+    Model model_plus = model; Data data_plus(model_plus);
+    const SE3 & oMi = data.oMi[joint_id];
+    const SE3 Mi_LWA = SE3(oMi.rotation(),SE3::Vector3::Zero());
+    const SE3 & oMi_plus = data_plus.oMi[joint_id];
+    for(int i = 1; i < model.njoints; ++i)
+    {
+      Motion::Vector6 v = Motion::Vector6::Zero();
+      const SE3 & M_placement = model.jointPlacements[(JointIndex)i];
+      SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i];
+      for(Eigen::DenseIndex k = 0; k < 6; ++k)
+      {
+        v[k] = eps;
+        M_placement_plus = M_placement * exp6(Motion(v));
+        
+        forwardKinematics(model_plus,data_plus,q);
+        
+        const Motion diff_L = log6(oMi.actInv(oMi_plus));
+        kinematic_regressor_L_fd.middleCols<6>(6*(i-1)).col(k) = diff_L.toVector()/eps;
+        const Motion diff_LWA = Mi_LWA.act(diff_L);
+        kinematic_regressor_LWA_fd.middleCols<6>(6*(i-1)).col(k) = diff_LWA.toVector()/eps;
+        const Motion diff_W = oMi.act(diff_L);
+        kinematic_regressor_W_fd.middleCols<6>(6*(i-1)).col(k) = diff_W.toVector()/eps;
+        v[k] = 0.;
+      }
+      
+      M_placement_plus = M_placement;
+    }
+    
+    BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd,sqrt(eps)));
+    BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd,sqrt(eps)));
+    BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd,sqrt(eps)));
+  }
+}
+
+BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint_placement)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  pinocchio::Model model; buildModels::humanoidRandom(model);
+  
+  pinocchio::Data data(model);
+  pinocchio::Data data_ref(model);
+  
+  model.lowerPositionLimit.head<7>().fill(-1.);
+  model.upperPositionLimit.head<7>().fill(1.);
+  
+  const VectorXd q = randomConfiguration(model);
+  
+  forwardKinematics(model,data,q);
+  forwardKinematics(model,data_ref,q);
+  
+  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
+  {
+    Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    
+    computeJointKinematicRegressor(model, data, joint_id, LOCAL, SE3::Identity(), kinematic_regressor_L);
+    computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, SE3::Identity(), kinematic_regressor_LWA);
+    computeJointKinematicRegressor(model, data, joint_id, WORLD, SE3::Identity(), kinematic_regressor_W);
+    
+    Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    
+    computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL, kinematic_regressor_L_ref);
+    computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA_ref);
+    computeJointKinematicRegressor(model, data_ref, joint_id, WORLD, kinematic_regressor_W_ref);
+    
+    BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref));
+    BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref));
+    BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref));
+  }
+}
+
+BOOST_AUTO_TEST_CASE(test_kinematic_regressor_frame)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  pinocchio::Model model; buildModels::humanoidRandom(model);
+  
+  model.lowerPositionLimit.head<7>().fill(-1.);
+  model.upperPositionLimit.head<7>().fill(1.);
+  
+  const std::string joint_name = "larm5_joint";
+  const JointIndex joint_id = model.getJointId(joint_name);
+  model.addBodyFrame("test_body", joint_id, SE3::Random(), -1);
+  
+  pinocchio::Data data(model);
+  pinocchio::Data data_ref(model);
+  
+  const VectorXd q = randomConfiguration(model);
+  
+  forwardKinematics(model,data,q);
+  updateFramePlacements(model,data);
+  forwardKinematics(model,data_ref,q);
+  
+  const double eps = 1e-8;
+  for(FrameIndex frame_id = 1; frame_id < (FrameIndex)model.nframes; ++frame_id)
+  {
+    const Frame & frame = model.frames[frame_id];
+    
+    Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
+    Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));