Merge pull request #1311 from jcarpent/topic/coriolis

jcarpent · web-flow · commit d78d951dd60c · 2020-10-10T08:59:06.000+02:00
Add getCoriolisMatrix
diff --git a/bindings/python/algorithm/expose-rnea.cpp b/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>());
       
     }
     
diff --git a/cmake b/cmake
@@ -1 +1 @@
-Subproject commit e4f507fb7e688c5d2dcf8f2d5c1af388596ba91c
+Subproject commit 95917116c0f1b0b0690a22e12d3871b3bb7cae27
diff --git a/src/algorithm/aba-derivatives.hxx b/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]);
diff --git a/src/algorithm/rnea-derivatives.hxx b/src/algorithm/rnea-derivatives.hxx
@@ -1,5 +1,5 @@
 //
-// 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
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
@@ -1,9 +1,9 @@
 //
-// 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__
diff --git a/src/algorithm/rnea.hxx b/src/algorithm/rnea.hxx
@@ -1,9 +1,9 @@
 //
-// 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__
diff --git a/src/multibody/data.hpp b/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
diff --git a/src/multibody/data.hxx b/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
diff --git a/src/multibody/joint/joint-spherical.hpp b/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());
diff --git a/src/serialization/data.hpp b/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);
diff --git a/unittest/rnea-derivatives.cpp b/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()
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp

Original file line number	Diff line number	Diff line change
`@@ -419,7 +419,7 @@ namespace pinocchio`
`419`	`419`	`{`
`420`	`420`	`typedef typename ConfigVectorLike::Scalar Scalar;`
`421`	`421`	`EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);`
`422`		`- typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;`
	`422`	`+ typedef typename Eigen::Quaternion<Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;`
`423`	`423`	`typedef Eigen::Map<const Quaternion> ConstQuaternionMap;`
`424`	`424`
`425`	`425`	`ConstQuaternionMap quat(q_joint.derived().data());`