Merge pull request #402 from jcarpent/topic/jacobian-variation

Add algo to compute [inefficiently] the time variation of the joint jacobian

benchmark/timings.cpp

@@ -131,6 +131,13 @@ int main(int argc, const char ** argv)
       computeJacobians(model,data,qs[_smooth]);
     }
   std::cout << "Jacobian = \t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    computeJacobiansTimeVariation(model,data,qs[_smooth],qdots[_smooth]);
+  }
+  std::cout << "Jacobian Time Variation = \t"; timer.toc(std::cout,NBT);
 
   timer.tic();
   SMOOTH(NBT)

bindings/python/algorithm/expose-jacobian.cpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -41,6 +41,21 @@ namespace se3
       
       return J;
     }
+    
+    static Data::Matrix6x
+    get_jacobian_time_variation_proxy(const Model & model,
+                                  Data & data,
+                                  Model::JointIndex jointId,
+                                  bool local)
+    {
+      Data::Matrix6x dJ(6,model.nv); dJ.setZero();
+      
+      
+      if(local) getJacobianTimeVariation<true> (model,data,jointId,dJ);
+      else getJacobianTimeVariation<false> (model,data,jointId,dJ);
+      
+      return dJ;
+    }
   
     void exposeJacobian()
     {
@@ -59,6 +74,22 @@ namespace se3
                        "update_kinematics (true = update the value of the total jacobian)"),
               "Computes the jacobian of a given given joint according to the given input configuration."
               "If local is set to true, it returns the jacobian associated to the joint frame. Otherwise, it returns the jacobian of the frame coinciding with the world frame.");
+      
+      bp::def("computeJacobiansTimeVariation",computeJacobiansTimeVariation,
+              bp::args("Model","Data",
+                       "Joint configuration q (size Model::nq)",
+                       "Joint velocity v (size Model::nv)"),
+              "Calling computeJacobiansTimeVariation",
+              bp::return_value_policy<bp::return_by_value>());
+      
+      bp::def("getJacobianTimeVariation",get_jacobian_time_variation_proxy,
+              bp::args("Model, the model of the kinematic tree",
+                       "Data, the data associated to the model where the results are stored",
+                       "Joint ID, the index of the joint.",
+                       "frame (true = local, false = world)"),
+              "Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint."
+              "You have to run computeJacobiansTimeVariation first."
+              "If local is set to true, it returns the jacobian time variation associated to the joint frame. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");
     }
     
   } // namespace python

bindings/python/multibody/data.hpp

@@ -83,6 +83,7 @@ namespace se3
         .ADD_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
         .ADD_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,J,"Jacobian of joint placement")
+        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,dJ,"Time variation of the Jacobian of joint placement (data.J).")
         .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
         
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag,

cmake @ ab14f7aa

-Subproject commit 7b0b47cae2b082521ad674c8ee575f594f483cd7
+Subproject commit ab14f7aacfeed9add231b873412fdf9078b82eb5

src/algorithm/jacobian.hpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -26,7 +26,7 @@ namespace se3
   /// \brief Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.
   ///        The result is accessible through data.J.
   ///
-  /// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this Hacobian, it is then possible to easily extract the Jacobian of a specific joint frame.
+  /// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame.
   ///
   /// \param[in] model The model structure of the rigid body system.
   /// \param[in] data The data structure of the rigid body system.
@@ -71,6 +71,41 @@ namespace se3
            const Eigen::VectorXd & q,
            const Model::JointIndex jointId);
 
+  
+  ///
+  /// \brief Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depends both on q and v
+  ///        The result is accessible through data.dJ.
+  ///
+  ///
+  /// \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).
+  ///
+  /// \return The full model Jacobian (matrix 6 x model.nv).
+  ///
+  inline const Data::Matrix6x &
+  computeJacobiansTimeVariation(const Model & model,
+                                Data & data,
+                                const Eigen::VectorXd & q,
+                                const Eigen::VectorXd & v);
+  
+  ///
+  /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint.
+  /// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it.
+  ///
+  /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] jointId The id of the joint.
+  /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).
+  ///
+  template<bool localFrame>
+  void getJacobian(const Model & model,
+                   const Data & data,
+                   const Model::JointIndex jointId,
+                   Data::Matrix6x & dJ);
+  
 } // namespace se3 
 
 /* --- Details -------------------------------------------------------------------- */

src/algorithm/jacobian.hxx

@@ -140,6 +140,97 @@ namespace se3
     return data.J;
   }
   
+  struct JacobiansTimeVariationForwardStep : public fusion::JointVisitor<JacobiansTimeVariationForwardStep>
+  {
+    typedef boost::fusion::vector <const se3::Model &,
+    se3::Data &,
+    const Eigen::VectorXd &,
+    const Eigen::VectorXd &
+    > ArgsType;
+    
+    JOINT_VISITOR_INIT(JacobiansTimeVariationForwardStep);
+    
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                     const se3::Model & model,
+                     se3::Data & data,
+                     const Eigen::VectorXd & q,
+                     const Eigen::VectorXd & v)
+    {
+      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+      const Model::JointIndex & parent = model.parents[i];
+      
+      SE3 & oMi = data.oMi[i];
+      Motion & vJ = data.v[i];
+      
+      jmodel.calc(jdata.derived(),q,v);
+      
+      vJ = jdata.v();
+      
+      data.liMi[i] = model.jointPlacements[i]*jdata.M();
+      if(parent>0)
+      {
+        oMi = data.oMi[parent]*data.liMi[i];
+        vJ += data.liMi[i].actInv(data.v[parent]);
+      }
+      else
+      {
+        oMi = data.liMi[i];
+      }
+      
+      jmodel.jointCols(data.J) = oMi.act(jdata.S());
+      
+      // Spatial velocity of joint i expressed in the global frame o
+      const Motion ov(oMi.act(vJ));
+      
+      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
+      ColsBlock dJcols = jmodel.jointCols(data.dJ);
+      ColsBlock Jcols = jmodel.jointCols(data.J);
+      
+      for(int k = 0; k < jmodel.nv(); ++k)
+        dJcols.col(k) = ov.cross(Motion(Jcols.col(k))).toVector();
+    }
+    
+  };
+  
+  inline const Data::Matrix6x &
+  computeJacobiansTimeVariation(const Model & model,
+                                Data & data,
+                                const Eigen::VectorXd & q,
+                                const Eigen::VectorXd & v)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    
+    for(Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i)
+    {
+      JacobiansTimeVariationForwardStep::run(model.joints[i],data.joints[i],
+                                             JacobiansTimeVariationForwardStep::ArgsType(model,data,q,v));
+    }
+    
+    return data.dJ;
+  }
+  
+  template<bool localFrame>
+  void getJacobianTimeVariation(const Model & model,
+                                const Data & data,
+                                const Model::JointIndex jointId,
+                                Data::Matrix6x & dJ)
+  {
+    assert( dJ.rows() == data.dJ.rows() );
+    assert( dJ.cols() == data.dJ.cols() );
+    assert(model.check(data) && "data is not consistent with model.");
+    
+    const SE3 & oMjoint = data.oMi[jointId];
+    int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
+    for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
+    {
+      if(! localFrame )   dJ.col(j) = data.dJ.col(j);
+      else                dJ.col(j) = oMjoint.actInv(Motion(data.dJ.col(j))).toVector();
+    }
+  }
+  
+  
 } // namespace se3
 
 /// @endcond

src/multibody/liegroup/operation-base.hpp

@@ -28,8 +28,9 @@ namespace se3
 #ifdef __clang__
 
 #define SE3_LIE_GROUP_TYPEDEF_ARG(prefix)              \
-   typedef prefix traits<LieGroupDerived>::Scalar Scalar;    \
-   enum {                  \
+  typedef int Index;                                   \
+  typedef prefix traits<LieGroupDerived>::Scalar Scalar;    \
+  enum {                  \
     NQ = traits<LieGroupDerived>::NQ,              \
     NV = traits<LieGroupDerived>::NV               \
   };                        \

src/multibody/liegroup/special-euclidean.hpp

@@ -162,7 +162,7 @@ namespace se3
     template <class Config_t>
     void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
-      R2crossSO2_t::random(qout);
+      R2crossSO2_t().random(qout);
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
@@ -274,7 +274,7 @@ namespace se3
     template <class Config_t>
     void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
-      R3crossSO3_t::random(qout);
+      R3crossSO3_t().random(qout);
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>

src/multibody/model.hpp

@@ -456,6 +456,9 @@ namespace se3
     /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call se3::getJacobian
     Matrix6x J;
     
+    /// \brief Derivative of the Jacobian with respect to the time.
+    Matrix6x dJ;
+    
     /// \brief Vector of joint placements wrt to algorithm end effector.
     container::aligned_vector<SE3> iMf;
 

src/multibody/model.hxx

@@ -252,6 +252,7 @@ namespace se3
     ,parents_fromRow((std::size_t)model.nv)
     ,nvSubtree_fromRow((std::size_t)model.nv)
     ,J(6,model.nv)
+    ,dJ(6,model.nv)
     ,iMf((std::size_t)model.njoints)
     ,com((std::size_t)model.njoints)
     ,vcom((std::size_t)model.njoints)

src/spatial/motion.hpp

@@ -154,9 +154,9 @@ namespace se3
     ActionMatrix_t toActionMatrix_impl () const
     {
       ActionMatrix_t X;
-      X.block <3,3> (ANGULAR, ANGULAR) = X.block <3,3> (LINEAR, LINEAR) = skew (angular_impl());
-      X.block <3,3> (LINEAR, ANGULAR) = skew (linear_impl());
-      X.block <3,3> (ANGULAR, LINEAR).setZero ();
+      X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) = skew (angular_impl());
+      X.template block <3,3> (LINEAR, ANGULAR) = skew (linear_impl());
+      X.template block <3,3> (ANGULAR, LINEAR).setZero ();
 
       return X;
     }

unittest/jacobian.cpp

 //
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015-2017 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -17,16 +17,24 @@
 
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/spatial/act-on-set.hpp"
 #include "pinocchio/parsers/sample-models.hpp"
 #include "pinocchio/tools/timer.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
 
 #include <iostream>
 
 #include <boost/test/unit_test.hpp>
 #include <boost/utility/binary.hpp>
 
+template<typename Derived>
+inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
+{
+  return ((x - x).array() == (x - x).array()).all();
+}
+
 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
 
 BOOST_AUTO_TEST_CASE ( test_jacobian )
@@ -67,6 +75,54 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
 
 }
 
+BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
+{
+  using namespace Eigen;
+  using namespace se3;
+  
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  se3::Data data(model);
+  se3::Data data_ref(model);
+  
+  VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
+  VectorXd v = VectorXd::Random(model.nv);
+  VectorXd a = VectorXd::Random(model.nv);
+  
+  computeJacobiansTimeVariation(model,data,q,v);
+  
+  BOOST_CHECK(isFinite(data.dJ));
+  
+  forwardKinematics(model,data_ref,q,v,a);
+  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
+  
+  Data::Matrix6x J(6,model.nv); J.fill(0.);
+  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
+  
+  // Regarding to the world origin
+  getJacobian<false>(model,data,idx,J);
+  getJacobianTimeVariation<false>(model,data,idx,dJ);
+  
+  Motion v_idx(J*v);
+  BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx])));
+  
+  Motion a_idx(J*a + dJ*v);
+  const Motion & a_ref = data_ref.oMi[idx].act(data_ref.a[idx]);
+  BOOST_CHECK(a_idx.isApprox(a_ref));
+  
+  
+  // Regarding to the local frame
+  getJacobian<true>(model,data,idx,J);
+  getJacobianTimeVariation<true>(model,data,idx,dJ);
+  
+  v_idx = (Motion::Vector6)(J*v);
+  BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));
+  
+  a_idx = (Motion::Vector6)(J*a + dJ*v);
+  BOOST_CHECK(a_idx.isApprox(data_ref.a[idx]));
+}
+
+
 
 BOOST_AUTO_TEST_CASE ( test_timings )
 {