algo/centroidal: add computeCentroidalMappingTimeVariation

src/algorithm/centroidal.hpp

@@ -208,6 +208,29 @@ namespace pinocchio
          DataTpl<Scalar,Options,JointCollectionTpl> & data,
          const Eigen::MatrixBase<ConfigVectorType> & q,
          const Eigen::MatrixBase<TangentVectorType> & v);
+
+  ///
+  /// \brief Computes the Centroidal Momentum Matrix time derivative.
+  ///
+  /// \tparam JointCollection Collection of Joint types.
+  /// \tparam ConfigVectorType Type of the joint configuration vector.
+  /// \tparam TangentVectorType Type of the joint velocity vector. 
+  ///
+  /// \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 Centroidal Momentum Matrix time derivative dAg (accessible via data.dAg).
+  ///
+  /// \remarks As another output, this algorithm also computes the Centroidal Momentum Matrix Ag (accessible via data.Ag), the Joint Jacobian matrix (accessible via data.J) and the time derivatibe of the Joint Jacobian matrix (accessible via data.dJ).
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
+  computeCentroidalMappingTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                        DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                        const Eigen::MatrixBase<ConfigVectorType> & q,
+                                        const Eigen::MatrixBase<TangentVectorType> & v);
   
 } // namespace pinocchio
 

src/algorithm/centroidal.hxx

@@ -323,6 +323,56 @@ namespace pinocchio
     
     return data.dAg;
   }
+
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
+  computeCentroidalMappingTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                        DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                        const Eigen::MatrixBase<ConfigVectorType> & q,
+                                        const Eigen::MatrixBase<TangentVectorType> & v)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
+    PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The velocity vector is not of right size");
+    
+    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
+    typedef typename Model::JointIndex JointIndex;
+    
+    forwardKinematics(model,data,q,v);
+    data.oYcrb[0].setZero();
+    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
+    {
+      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
+      data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame
+      data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
+    }
+    
+    typedef DCcrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
+    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
+    {
+      Pass2::run(model.joints[i],data.joints[i],
+                 typename Pass2::ArgsType(model,data));
+    }
+    
+    // Express the centroidal map around the center of mass
+    data.com[0] = data.oYcrb[0].lever();
+    
+    typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x;
+    
+    const Block3x Ag_lin = data.Ag.template middleRows<3> (Force::LINEAR);
+    Block3x Ag_ang = data.Ag.template middleRows<3>  (Force::ANGULAR);
+    for(Eigen::DenseIndex i = 0; i<model.nv; ++i)
+      Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
+    
+    // Express the centroidal time derivative map around the center of mass
+    const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
+    Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
+    for(Eigen::DenseIndex i = 0; i<model.nv; ++i)
+      dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]);
+    
+    return data.dAg;
+  }
   
 } // namespace pinocchio