Merge pull request #1311 from jcarpent/topic/coriolis

Add getCoriolisMatrix

bindings/python/algorithm/expose-rnea.cpp

@@ -82,6 +82,15 @@ namespace pinocchio
               "\tq: the joint configuration vector (size model.nq)\n"
               "\tv: the joint velocity vector (size model.nv)\n",
               bp::return_value_policy<bp::return_by_value>());
+
+      bp::def("getCoriolisMatrix",
+              &getCoriolisMatrix<double,0,JointCollectionDefaultTpl>,
+              bp::args("model","Data"),
+              "Retrives the Coriolis Matrix C(q,v) of the Lagrangian dynamics after calling one of the derivative algorithms, store the result in data.C and return it.\n\n"
+              "Parameters:\n"
+              "\tmodel: model of the kinematic tree\n"
+              "\tdata: data related to the model\n",
+              bp::return_value_policy<bp::return_by_value>());
       
     }
     

cmake @ 95917116

-Subproject commit e4f507fb7e688c5d2dcf8f2d5c1af388596ba91c
+Subproject commit 95917116c0f1b0b0690a22e12d3871b3bb7cae27

src/algorithm/aba-derivatives.hxx

@@ -55,7 +55,7 @@ namespace pinocchio
       ov = data.oMi[i].act(data.v[i]);
       data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
       data.Yaba[i] = model.inertias[i].matrix();
-      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
+      data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
       
       data.oh[i] = data.oYcrb[i] * ov;
       data.of[i] = ov.cross(data.oh[i]);

src/algorithm/rnea-derivatives.hxx

 //
-// Copyright (c) 2017-2019 CNRS INRIA
+// Copyright (c) 2017-2020 CNRS INRIA
 //
 
 #ifndef __pinocchio_rnea_derivatives_hxx__
@@ -46,7 +46,7 @@ namespace pinocchio
       else
         data.oMi[i] = data.liMi[i];
       
-      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
+      data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
       data.of[i] = data.oYcrb[i] * minus_gravity;
       
       typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
@@ -242,7 +242,7 @@ namespace pinocchio
         data.a[i] += data.liMi[i].actInv(data.a[parent]);
       }
       
-      data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
+      data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
       ov = data.oMi[i].act(data.v[i]);
       oa = data.oMi[i].act(data.a[i]);
       oa_gf = oa - model.gravity; // add gravity contribution

src/algorithm/rnea.hpp

 //
-// Copyright (c) 2015-2019 CNRS INRIA
+// Copyright (c) 2015-2020 CNRS INRIA
 //
 
-#ifndef __pinocchio_rnea_hpp__
-#define __pinocchio_rnea_hpp__
+#ifndef __pinocchio_algorithm_rnea_hpp__
+#define __pinocchio_algorithm_rnea_hpp__
 
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/data.hpp"
@@ -154,9 +154,28 @@ namespace pinocchio
                         const Eigen::MatrixBase<ConfigVectorType> & q,
                         const Eigen::MatrixBase<TangentVectorType> & v);
 
+  ///
+  /// \brief Retrives the Coriolis Matrix \f$ C(q,\dot{q}) \f$ of the Lagrangian dynamics:
+  /// <CENTER> \f$ \begin{eqnarray} M \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau  \end{eqnarray} \f$ </CENTER> <BR>
+  /// after a call to the dynamics derivatives.
+  ///
+  /// \note In the previous equation, \f$ c(q, \dot{q}) = C(q, \dot{q})\dot{q} \f$.
+  ///
+  /// \tparam JointCollection Collection of Joint types.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  ///
+  /// \return The Coriolis matrix stored in data.C.
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
+  getCoriolisMatrix(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                    DataTpl<Scalar,Options,JointCollectionTpl> & data);
+
 } // namespace pinocchio 
 
 /* --- Details -------------------------------------------------------------------- */
 #include "pinocchio/algorithm/rnea.hxx"
 
-#endif // ifndef __pinocchio_rnea_hpp__
+#endif // ifndef __pinocchio_algorithm_rnea_hpp__

src/algorithm/rnea.hxx

 //
-// Copyright (c) 2015-2019 CNRS INRIA
+// Copyright (c) 2015-2020 CNRS INRIA
 //
 
-#ifndef __pinocchio_rnea_hxx__
-#define __pinocchio_rnea_hxx__
+#ifndef __pinocchio_algorithm_rnea_hxx__
+#define __pinocchio_algorithm_rnea_hxx__
 
 /// @cond DEV
 
@@ -473,8 +473,8 @@ namespace pinocchio
     {
       typedef typename Model::JointIndex JointIndex;
       
-      const JointIndex & i = jmodel.id();
-      const JointIndex & parent = model.parents[i];
+      const JointIndex i = jmodel.id();
+      const JointIndex parent = model.parents[i];
       typename Data::RowMatrix6 & M6tmpR = data.M6tmpR;
 
       typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
@@ -543,9 +543,84 @@ namespace pinocchio
     
     return data.C;
   }
+
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  struct GetCoriolisMatrixBackwardStep
+  : public fusion::JointUnaryVisitorBase< GetCoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> >
+  {
+    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
+    
+    typedef boost::fusion::vector<const Model &,
+                                  Data &
+                                  > ArgsType;
+
+    template<typename JointModel>
+    static void algo(const JointModelBase<JointModel> & jmodel,
+                     const Model & model,
+                     Data & data)
+    {
+      typedef typename Model::JointIndex JointIndex;
+      typedef CoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> EquivalentPass;
+      
+      const JointIndex i = jmodel.id();
+      const JointIndex parent = model.parents[i];
+      typename Data::RowMatrix6 & M6tmpR = data.M6tmpR;
+
+      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
+      ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
+      ColsBlock J_cols = jmodel.jointCols(data.J);
+      typename Data::Matrix6x & dFdv = data.Fcrb[0];
+      ColsBlock dFdv_cols = jmodel.jointCols(dFdv);
+      
+      motionSet::inertiaAction(data.oYcrb[i],dJ_cols,dFdv_cols);
+      dFdv_cols.noalias() += data.vxI[i] * J_cols;
+
+      data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
+      = J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
+      
+      EquivalentPass::lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
+      for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j])
+        data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j);
+
+      M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.vxI[i];
+      for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j])
+        data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() += M6tmpR.topRows(jmodel.nv()) * data.J.col(j);
+      
+      if(parent>0)
+      {
+        data.vxI[parent] += data.vxI[i];
+      }
+      
+    }
+  };
+
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
+  getCoriolisMatrix(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                    DataTpl<Scalar,Options,JointCollectionTpl> & data)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    
+    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+    typedef typename Model::JointIndex JointIndex;
+    
+    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
+    {
+      Inertia::vxi(data.ov[i],data.oinertias[i],data.vxI[i]);
+    }
+    
+    typedef GetCoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
+    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
+    {
+      Pass2::run(model.joints[i],typename Pass2::ArgsType(model,data));
+    }
+    
+    return data.C;
+  }
   
 } // namespace pinocchio
 
 /// @endcond
 
-#endif // ifndef __pinocchio_rnea_hxx__
+#endif // ifndef __pinocchio_algorithm_rnea_hxx__

src/multibody/data.hpp

@@ -183,10 +183,13 @@ namespace pinocchio
     /// \brief Left variation of the inertia matrix
     PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
     
-    /// \brief Inertia quantities expressed in the world frame
+    /// \brief Rigid Body Inertia supported by the joint expressed in the world frame
+    PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
+    
+    /// \brief Composite Rigid Body Inertia expressed in the world frame
     PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
     
-    /// \brief Time variation of the inertia quantities expressed in the world frame
+    /// \brief Time variation of Composite Rigid Body Inertia expressed in the world frame
     PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
     
     /// \brief Temporary for derivative algorithms

src/multibody/data.hxx

@@ -53,6 +53,7 @@ namespace pinocchio
   , IS(MatrixXs::Zero(6,model.nv))
   , vxI((std::size_t)model.njoints,Inertia::Matrix6::Zero())
   , Ivx((std::size_t)model.njoints,Inertia::Matrix6::Zero())
+  , oinertias((std::size_t)model.njoints,Inertia::Zero())
   , oYcrb((std::size_t)model.njoints,Inertia::Zero())
   , doYcrb((std::size_t)model.njoints,Inertia::Matrix6::Zero())
   , ddq(VectorXs::Zero(model.nv))
@@ -248,6 +249,7 @@ namespace pinocchio
     && data1.IS == data2.IS
     && data1.vxI == data2.vxI
     && data1.Ivx == data2.Ivx
+    && data1.oinertias == data2.oinertias
     && data1.oYcrb == data2.oYcrb
     && data1.doYcrb == data2.doYcrb
     && data1.ddq == data2.ddq

src/multibody/joint/joint-spherical.hpp

@@ -419,7 +419,7 @@ namespace pinocchio
     {
       typedef typename ConfigVectorLike::Scalar Scalar;
       EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
-      typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
+      typedef typename Eigen::Quaternion<Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
       typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
 
       ConstQuaternionMap quat(q_joint.derived().data());

src/serialization/data.hpp

@@ -57,6 +57,7 @@ namespace boost
       PINOCCHIO_MAKE_DATA_NVP(ar,data,IS);
       PINOCCHIO_MAKE_DATA_NVP(ar,data,vxI);
       PINOCCHIO_MAKE_DATA_NVP(ar,data,Ivx);
+      PINOCCHIO_MAKE_DATA_NVP(ar,data,oinertias);
       PINOCCHIO_MAKE_DATA_NVP(ar,data,oYcrb);
       PINOCCHIO_MAKE_DATA_NVP(ar,data,doYcrb);
       PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq);

unittest/rnea-derivatives.cpp

@@ -471,4 +471,39 @@ BOOST_AUTO_TEST_CASE(test_multiple_calls)
   BOOST_CHECK(data1.M.isApprox(data2.M));
 }
 
+BOOST_AUTO_TEST_CASE(test_get_coriolis)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  Model model;
+  buildModels::humanoidRandom(model);
+  
+  model.lowerPositionLimit.head<3>().fill(-1.);
+  model.upperPositionLimit.head<3>().fill( 1.);
+  
+  Data data_ref(model);
+  Data data(model);
+  
+  VectorXd q = randomConfiguration(model);
+  VectorXd v = VectorXd::Random(model.nv);
+  VectorXd tau = VectorXd::Random(model.nv);
+  
+  computeCoriolisMatrix(model,data_ref,q,v);
+  
+  computeRNEADerivatives(model,data,q,v,tau);
+  getCoriolisMatrix(model,data);
+  
+  BOOST_CHECK(data.J.isApprox(data_ref.J));
+  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
+  BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdv));
+  for(JointIndex k = 1; k < model.joints.size(); ++k)
+  {
+    BOOST_CHECK(data.vxI[k].isApprox(data_ref.vxI[k]));
+    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
+  }
+  
+  BOOST_CHECK(data.C.isApprox(data_ref.C));
+}
+
 BOOST_AUTO_TEST_SUITE_END()

unittest/rnea.cpp

@@ -243,90 +243,70 @@ BOOST_AUTO_TEST_CASE(test_compute_static_torque)
   BOOST_CHECK(static_torque_ref.isApprox(data.tau));
 }
   
-  BOOST_AUTO_TEST_CASE(test_compute_coriolis)
+BOOST_AUTO_TEST_CASE(test_compute_coriolis)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  const double prec = Eigen::NumTraits<double>::dummy_precision();
+  
+  Model model;
+  buildModels::humanoidRandom(model);
+  
+  model.lowerPositionLimit.head<3>().fill(-1.);
+  model.upperPositionLimit.head<3>().fill( 1.);
+  
+  Data data_ref(model);
+  Data data(model);
+  
+  VectorXd q = randomConfiguration(model);
+  
+  VectorXd v (VectorXd::Random(model.nv));
+  computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
+  BOOST_CHECK(data.C.isZero(prec));
+  
+  
+  model.gravity.setZero();
+  rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
+  computeJointJacobiansTimeVariation(model,data_ref,q,v);
+  computeCoriolisMatrix(model,data,q,v);
+  
+  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
+  BOOST_CHECK(data.J.isApprox(data_ref.J));
+  
+  VectorXd tau = data.C * v;
+  BOOST_CHECK(tau.isApprox(data_ref.tau,prec));
+  
+  dccrba(model,data_ref,q,v);
+  crba(model,data_ref,q);
+  
+  const Data::Vector3 & com = data_ref.com[0];
+  Motion vcom(data_ref.vcom[0],Data::Vector3::Zero());
+  SE3 cM1(data.oMi[1]); cM1.translation() -= com;
+  
+  BOOST_CHECK((cM1.toDualActionMatrix()*data_ref.M.topRows<6>()).isApprox(data_ref.Ag,prec));
+  
+  Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
+  Force dh(data_ref.dAg * v);
+  BOOST_CHECK(dh.isApprox(dh_ref,prec));
+  
   {
-    using namespace Eigen;
-    using namespace pinocchio;
-    
-    const double prec = Eigen::NumTraits<double>::dummy_precision();
-    
-    Model model;
-    buildModels::humanoidRandom(model);
-    
-    model.lowerPositionLimit.head<3>().fill(-1.);
-    model.upperPositionLimit.head<3>().fill( 1.);
-    
-    Data data_ref(model);
-    Data data(model);
-    
-    VectorXd q = randomConfiguration(model);
-    
-    VectorXd v (VectorXd::Random(model.nv));
-    computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
-    BOOST_CHECK(data.C.isZero(prec));
-    
-    
-    model.gravity.setZero();
-    rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
-    computeJointJacobiansTimeVariation(model,data_ref,q,v);
-    computeCoriolisMatrix(model,data,q,v);
-
-    BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
-    BOOST_CHECK(data.J.isApprox(data_ref.J));
+    Data data_ref(model), data_ref_plus(model);
+    Eigen::MatrixXd dM(data.C + data.C.transpose());
     
-    VectorXd tau = data.C * v;
-    BOOST_CHECK(tau.isApprox(data_ref.tau,prec));
+    const double alpha = 1e-8;
+    Eigen::VectorXd q_plus(model.nq);
+    q_plus = integrate(model,q,alpha*v);
     
-    dccrba(model,data_ref,q,v);
     crba(model,data_ref,q);
+    data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
+    crba(model,data_ref_plus,q_plus);
+    data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();
     
-    const Data::Vector3 & com = data_ref.com[0];
-    Motion vcom(data_ref.vcom[0],Data::Vector3::Zero());
-    SE3 cM1(data.oMi[1]); cM1.translation() -= com;
-    
-    BOOST_CHECK((cM1.toDualActionMatrix()*data_ref.M.topRows<6>()).isApprox(data_ref.Ag,prec));
-    
-    Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
-    Force dh(data_ref.dAg * v);
-    BOOST_CHECK(dh.isApprox(dh_ref,prec));
-    
-    {
-      Data data_ref(model), data_ref_plus(model);
-      Eigen::MatrixXd dM(data.C + data.C.transpose());
-
-      const double alpha = 1e-8;
-      Eigen::VectorXd q_plus(model.nq);
-      q_plus = integrate(model,q,alpha*v);
-      
-      crba(model,data_ref,q);
-      data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
-      crba(model,data_ref_plus,q_plus);
-      data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();
-      
-      Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
-      BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
-    }
-    
-//    {
-//      //v.setZero();
-//      v.setOnes();
-//      Eigen::VectorXd v_fd(v);
-//      Eigen::MatrixXd drnea_dv(model.nv,model.nv);
-//      Data data_fd(model);
-//      
-//      const VectorXd tau0 = rnea(model,data_fd,q,v,0*v);
-//      const double eps = 1e-8;
-//      for(int i = 0; i < model.nv; ++i)
-//      {
-//        v_fd[i] += eps;
-//        rnea(model,data_fd,q,v_fd,0*v);
-//        drnea_dv.col(i) = (data_fd.tau - tau0)/eps;
-//        v_fd[i] -= eps;
-//      }
-//      BOOST_CHECK(v_fd.isApprox(v));
-//      std::cout << "drnea_dv:\n" << drnea_dv.block<6,6>(0,0) << std::endl;
-//      std::cout << "C:\n" << data.C.block<6,6>(0,0) << std::endl;
-//    }
-    
+    Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
+    BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
   }
-BOOST_AUTO_TEST_SUITE_END ()
+  
+}
+
+BOOST_AUTO_TEST_SUITE_END()