[Algo] Add algo to compute inefficiently the time derivative of the joint Jacobian

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,91 @@ 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));
+      jmodel.jointCols(data.dJ) = (ov.toActionMatrix() * jmodel.jointCols(data.J));
+    }
+    
+  };
+  
+  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