[All] Split CRBA and centroidal algos

CMakeLists.txt

@@ -235,6 +235,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/aba.hxx
   algorithm/aba-derivatives.hpp
   algorithm/aba-derivatives.hxx
+  algorithm/centroidal.hpp
+  algorithm/centroidal.hxx
   algorithm/rnea.hpp
   algorithm/rnea.hxx
   algorithm/rnea-derivatives.hpp

benchmark/timings.cpp

@@ -21,6 +21,7 @@
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/data.hpp"
 #include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
 #include "pinocchio/algorithm/aba.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/algorithm/cholesky.hpp"

bindings/python/CMakeLists.txt

@@ -87,6 +87,7 @@ SET(${PROJECT_NAME}_PYTHON_SOURCES
   algorithm/expose-kinematics.cpp
   algorithm/expose-dynamics.cpp
   algorithm/expose-crba.cpp
+  algorithm/expose-centroidal.cpp
   algorithm/expose-rnea.cpp
   algorithm/expose-rnea-derivatives.cpp
   algorithm/expose-aba.cpp

bindings/python/algorithm/algorithms.hpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2016,2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -30,6 +30,7 @@ namespace se3
     void exposeJointsAlgo();
     void exposeABA();
     void exposeCRBA();
+    void exposeCentroidal();
     void exposeRNEA();
     void exposeCOM();
     void exposeFramesAlgo();

bindings/python/algorithm/expose-algorithms.cpp

@@ -27,6 +27,7 @@ namespace se3
       exposeJointsAlgo();
       exposeABA();
       exposeCRBA();
+      exposeCentroidal();
       exposeRNEA();
       exposeCOM();
       exposeFramesAlgo();

bindings/python/algorithm/expose-centroidal.cpp

+//
+// Copyright (c) 2015-2016,2018 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    
+    void exposeCentroidal()
+    {
+      
+      bp::def("ccrba",ccrba,
+              bp::args("Model","Data",
+                       "Joint configuration q (size Model::nq)",
+                       "Joint velocity v (size Model::nv)"),
+              "Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite Rigid Body Inertia, puts the result in Data and returns the centroidal mapping.",
+              bp::return_value_policy<bp::return_by_value>());
+      
+      bp::def("dccrba",dccrba,
+              bp::args("Model","Data",
+                       "Joint configuration q (size Model::nq)",
+                       "Joint velocity v (size Model::nv)"),
+              "Computes the time derivative of the centroidal momentum matrix Ag in terms of q and v. It computes also the same information than ccrtba for the same price.",
+              bp::return_value_policy<bp::return_by_value>());
+      
+    }
+    
+  } // namespace python
+} // namespace se3

bindings/python/algorithm/expose-crba.cpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2016,2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -39,21 +39,6 @@ namespace se3
               bp::args("Model","Data",
                        "Joint configuration q (size Model::nq)"),
               "Computes CRBA, put the result in Data and return it.");
-      
-      bp::def("ccrba",ccrba,
-              bp::args("Model","Data",
-                       "Joint configuration q (size Model::nq)",
-                       "Joint velocity v (size Model::nv)"),
-              "Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite Rigid Body Inertia, puts the result in Data and returns the centroidal mapping.",
-              bp::return_value_policy<bp::return_by_value>());
-      
-      bp::def("dccrba",dccrba,
-              bp::args("Model","Data",
-                       "Joint configuration q (size Model::nq)",
-                       "Joint velocity v (size Model::nv)"),
-              "Computes the time derivative of the centroidal momentum matrix Ag in terms of q and v. It computes also the same information than ccrtba for the same price.",
-              bp::return_value_policy<bp::return_by_value>());
-      
     }
     
   } // namespace python

src/algorithm/centroidal.hpp

+//
+// Copyright (c) 2015-2018 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_centroidal_hpp__
+#define __se3_centroidal_hpp__
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/algorithm/check.hpp"
+
+namespace se3
+{
+  
+  ///
+  /// \brief Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta
+  ///        according to the current joint configuration and velocity.
+  ///
+  ///
+  /// \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 Ag.
+  ///
+  inline const Data::Matrix6x &
+  ccrba(const Model & model,
+        Data & data,
+        const Eigen::VectorXd & q,
+        const Eigen::VectorXd & v);
+  
+  ///
+  /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.
+  ///
+  /// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$ \dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\f$.
+  ///
+  /// \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 configuration vector (dim model.nv).
+  ///
+  /// \return The Centroidal Momentum Matrix time derivative dAg
+  ///
+  inline const Data::Matrix6x &
+  dccrba(const Model & model,
+         Data & data,
+         const Eigen::VectorXd & q,
+         const Eigen::VectorXd & v);
+  
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+#include "pinocchio/algorithm/centroidal.hxx"
+
+#endif // ifndef __se3_centroidal_hpp__

src/algorithm/centroidal.hxx

+//
+// Copyright (c) 2015-2018 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_centroidal_hxx__
+#define __se3_centroidal_hxx__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/spatial/act-on-set.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/check.hpp"
+
+/// @cond DEV
+
+namespace se3
+{
+  
+  struct CcrbaForwardStep : public fusion::JointVisitor<CcrbaForwardStep>
+  {
+    typedef boost::fusion::vector< const se3::Model &,
+    se3::Data &,
+    const Eigen::VectorXd &
+    > ArgsType;
+    
+    JOINT_VISITOR_INIT(CcrbaForwardStep);
+    
+    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 Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+      const Model::Index & parent = model.parents[i];
+      
+      jmodel.calc(jdata.derived(),q);
+      
+      data.liMi[i] = model.jointPlacements[i]*jdata.M();
+      data.Ycrb[i] = model.inertias[i];
+      
+      if (parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
+      else data.oMi[i] = data.liMi[i];
+    }
+    
+  }; // struct CcrbaForwardStep
+  
+  struct CcrbaBackwardStep : public fusion::JointVisitor<CcrbaBackwardStep>
+  {
+    typedef boost::fusion::vector< const se3::Model &,
+    se3::Data &
+    > ArgsType;
+    
+    JOINT_VISITOR_INIT(CcrbaBackwardStep);
+    
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                     const se3::Model & model,
+                     se3::Data & data)
+    {
+      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
+      
+      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+      const Model::Index & parent = model.parents[i];
+      
+      data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
+      
+      jdata.U() = data.Ycrb[i] * jdata.S();
+      
+      ColsBlock jF
+      = data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
+      //        = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
+      
+      forceSet::se3Action(data.oMi[i],jdata.U(),jF);
+    }
+    
+  }; // struct CcrbaBackwardStep
+  
+  inline const Data::Matrix6x &
+  ccrba(const Model & model, Data & data,
+        const Eigen::VectorXd & q,
+        const Eigen::VectorXd & v)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    typedef Eigen::Block<Data::Matrix6x,3,-1> Block3x;
+    
+    forwardKinematics(model, data, q);
+    data.Ycrb[0].setZero();
+    for(Model::Index i=1;i<(Model::Index)(model.njoints);++i )
+      data.Ycrb[i] = model.inertias[i];
+    
+    
+    for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
+    {
+      CcrbaBackwardStep::run(model.joints[i],data.joints[i],
+                             CcrbaBackwardStep::ArgsType(model,data));
+    }
+    
+    // Express the centroidal map around the center of mass
+    data.com[0] = data.Ycrb[0].lever();
+    
+    const Block3x Ag_lin = data.Ag.middleRows<3>(Force::LINEAR);
+    Block3x Ag_ang = data.Ag.middleRows<3>(Force::ANGULAR);
+    for (long i = 0; i<model.nv; ++i)
+      Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
+    
+    data.hg.toVector().noalias() = data.Ag*v;
+    
+    data.Ig.mass() = data.Ycrb[0].mass();
+    data.Ig.lever().setZero();
+    data.Ig.inertia() = data.Ycrb[0].inertia();
+    
+    return data.Ag;
+  }
+  
+  struct DCcrbaForwardStep : public fusion::JointVisitor<DCcrbaForwardStep>
+  {
+    typedef boost::fusion::vector< const se3::Model &,
+    se3::Data &,
+    const Eigen::VectorXd &,
+    const Eigen::VectorXd &
+    > ArgsType;
+    
+    JOINT_VISITOR_INIT(DCcrbaForwardStep);
+    
+    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::Index & parent = model.parents[i];
+      
+      jmodel.calc(jdata.derived(),q,v);
+      
+      data.liMi[i] = model.jointPlacements[i]*jdata.M();
+      data.Ycrb[i] = model.inertias[i];
+      
+      data.v[i] = jdata.v();
+      
+      if (parent>0)
+      {
+        data.oMi[i] = data.oMi[parent]*data.liMi[i];
+        data.v[i] += data.liMi[i].actInv(data.v[parent]);
+      }
+      else data.oMi[i] = data.liMi[i];
+    }
+    
+  }; // struct DCcrbaForwardStep
+  
+  struct DCcrbaBackwardStep : public fusion::JointVisitor<DCcrbaBackwardStep>
+  {
+    typedef boost::fusion::vector< const se3::Model &,
+    se3::Data &
+    > ArgsType;
+    
+    JOINT_VISITOR_INIT(DCcrbaBackwardStep);
+    
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                     const se3::Model & model,
+                     se3::Data & data)
+    {
+      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
+      
+      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+      const Model::Index & parent = model.parents[i];
+      const Inertia & Y = data.oYcrb[i];
+      const Inertia::Matrix6 & doYcrb = data.doYcrb[i];
+      
+      ColsBlock J_cols = jmodel.jointCols(data.J);
+      J_cols = data.oMi[i].act(jdata.S());
+      
+      ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
+      motionSet::motionAction(data.ov[i],J_cols,dJ_cols);
+      
+      data.oYcrb[parent] += Y;
+      if(parent > 0)
+        data.doYcrb[parent] += doYcrb;
+      
+      // Calc Ag
+      ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
+      motionSet::inertiaAction(Y,J_cols,Ag_cols);
+      
+      // Calc dAg = Ivx + vxI
+      ColsBlock dAg_cols = jmodel.jointCols(data.dAg);
+      motionSet::inertiaAction(Y,dJ_cols,dAg_cols);
+      dAg_cols += doYcrb * J_cols;
+    }
+    
+  }; // struct DCcrbaBackwardStep
+  
+  inline const Data::Matrix6x &
+  dccrba(const Model & model, Data & data,
+         const Eigen::VectorXd & q,
+         const Eigen::VectorXd & v)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x;
+    
+    forwardKinematics(model,data,q,v);
+    data.oYcrb[0].setZero();
+    for(Model::Index i=1;i<(Model::Index)(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]);
+    }
+    
+    for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
+    {
+      DCcrbaBackwardStep::run(model.joints[i],data.joints[i],
+                              DCcrbaBackwardStep::ArgsType(model,data));
+    }
+    
+    // Express the centroidal map around the center of mass
+    data.com[0] = data.oYcrb[0].lever();
+    
+    const Block3x Ag_lin = data.Ag.middleRows<3> (Force::LINEAR);
+    Block3x Ag_ang = data.Ag.middleRows<3>  (Force::ANGULAR);
+    for (long i = 0; i<model.nv; ++i)
+      Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
+    
+    data.hg.toVector().noalias() = data.Ag*v;
+    data.vcom[0].noalias() = data.hg.linear()/data.oYcrb[0].mass();
+    
+    const Block3x dAg_lin = data.dAg.middleRows<3>(Force::LINEAR);
+    Block3x dAg_ang = data.dAg.middleRows<3>(Force::ANGULAR);
+    for (long i = 0; i<model.nv; ++i)
+      dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]);
+    
+    data.Ig.mass() = data.oYcrb[0].mass();
+    data.Ig.lever().setZero();
+    data.Ig.inertia() = data.oYcrb[0].inertia();
+    
+    return data.dAg;
+  }
+  
+  // --- CHECKER ---------------------------------------------------------------
+  // --- CHECKER ---------------------------------------------------------------
+  // --- CHECKER ---------------------------------------------------------------
+  
+  
+} // namespace se3
+
+/// @endcond
+
+#endif // ifndef __se3_centroidal_hxx__
+

src/algorithm/crba.hpp

@@ -66,42 +66,6 @@ namespace se3
   crbaMinimal(const Model & model,
               Data & data,
               const Eigen::VectorXd & q);
-  
-  ///
-  /// \brief Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta
-  ///        according to the current joint configuration and velocity.
-  ///
-  ///
-  /// \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 Ag. 
-  ///
-  inline const Data::Matrix6x &
-  ccrba(const Model & model,
-        Data & data,
-        const Eigen::VectorXd & q,
-        const Eigen::VectorXd & v);
-  
-  ///
-  /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.
-  ///
-  /// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$ \dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\f$.
-  ///
-  /// \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 configuration vector (dim model.nv).
-  ///
-  /// \return The Centroidal Momentum Matrix time derivative dAg
-  ///
-  inline const Data::Matrix6x &
-  dccrba(const Model & model,
-        Data & data,
-        const Eigen::VectorXd & q,
-        const Eigen::VectorXd & v);
 
   DEFINE_ALGO_CHECKER(CRBA);
 

src/algorithm/crba.hxx

@@ -216,232 +216,6 @@ namespace se3
     return data.M;
   }
   
-  struct CcrbaForwardStep : public fusion::JointVisitor<CcrbaForwardStep>
-  {
-    typedef boost::fusion::vector< const se3::Model &,
-    se3::Data &,
-    const Eigen::VectorXd &
-    > ArgsType;
-    
-    JOINT_VISITOR_INIT(CcrbaForwardStep);
-    
-    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 Model::JointIndex & i = (Model::JointIndex) jmodel.id();
-      const Model::Index & parent = model.parents[i];
-      
-      jmodel.calc(jdata.derived(),q);
-      
-      data.liMi[i] = model.jointPlacements[i]*jdata.M();
-      data.Ycrb[i] = model.inertias[i];
-      
-      if (parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
-      else data.oMi[i] = data.liMi[i];
-    }
-    
-  }; // struct CcrbaForwardStep
-  
-  struct CcrbaBackwardStep : public fusion::JointVisitor<CcrbaBackwardStep>
-  {
-    typedef boost::fusion::vector< const se3::Model &,
-                                  se3::Data &
-                                  > ArgsType;
-    
-    JOINT_VISITOR_INIT(CcrbaBackwardStep);
-    
-    template<typename JointModel>
-    static void algo(const se3::JointModelBase<JointModel> & jmodel,
-                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
-                     const se3::Model & model,
-                     se3::Data & data)
-    {
-      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
-      
-      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
-      const Model::Index & parent = model.parents[i];
-      
-      data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
-      
-      jdata.U() = data.Ycrb[i] * jdata.S();
-
-      ColsBlock jF
-        = data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
-      //        = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
-
-      forceSet::se3Action(data.oMi[i],jdata.U(),jF);
-    }
-
-  }; // struct CcrbaBackwardStep
-  
-  inline const Data::Matrix6x &
-  ccrba(const Model & model, Data & data,
-        const Eigen::VectorXd & q,
-        const Eigen::VectorXd & v)
-  {
-    assert(model.check(data) && "data is not consistent with model.");
-    typedef Eigen::Block<Data::Matrix6x,3,-1> Block3x;
-    
-    forwardKinematics(model, data, q);
-    data.Ycrb[0].setZero();
-    for(Model::Index i=1;i<(Model::Index)(model.njoints);++i )
-      data.Ycrb[i] = model.inertias[i];
-    
-    
-    for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
-    {
-      CcrbaBackwardStep::run(model.joints[i],data.joints[i],
-                             CcrbaBackwardStep::ArgsType(model,data));
-    }
-    
-    // Express the centroidal map around the center of mass
-    data.com[0] = data.Ycrb[0].lever();
-    
-    const Block3x Ag_lin = data.Ag.middleRows<3>(Force::LINEAR);
-    Block3x Ag_ang = data.Ag.middleRows<3>(Force::ANGULAR);
-    for (long i = 0; i<model.nv; ++i)
-      Ag_ang.col(i) += Ag_lin.col(i).cross(data.