[Algo] Add algo to compute the static regressor of the center of mass

CMakeLists.txt

 #
-# Copyright (c) 2015-2017 CNRS
+# Copyright (c) 2015-2018 CNRS
 # Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 #
 # This file is part of Pinocchio
@@ -253,6 +253,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/check.hpp
   algorithm/check.hxx
   algorithm/default-check.hpp
+  algorithm/regressor.hpp
+  algorithm/regressor.hxx
   )
 
 SET(${PROJECT_NAME}_CONTAINER_HEADERS

bindings/python/CMakeLists.txt

 # 
-# Copyright (c) 2015-2017 CNRS
+# Copyright (c) 2015-2018 CNRS
 # Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 # 
 # This file is part of Pinocchio
@@ -94,6 +94,7 @@ SET(${PROJECT_NAME}_PYTHON_SOURCES
   algorithm/expose-frames.cpp
   algorithm/expose-cat.cpp
   algorithm/expose-geometry.cpp
+  algorithm/expose-regressor.cpp
   parsers/expose-parsers.cpp
   )
 

bindings/python/algorithm/algorithms.hpp

@@ -39,6 +39,7 @@ namespace se3
     void exposeCAT();
     void exposeJacobian();
     void exposeGeometryAlgo();
+    void exposeRegressor();
 
     void exposeAlgorithms();
     

bindings/python/algorithm/expose-algorithms.cpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -36,6 +36,7 @@ namespace se3
       exposeCAT();
       exposeJacobian();
       exposeGeometryAlgo();
+      exposeRegressor();
     }
     
   } // namespace python

bindings/python/algorithm/expose-regressor.cpp

+//
+// Copyright (c) 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/regressor.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    
+    void exposeRegressor()
+    {
+      using namespace Eigen;
+
+      bp::def("computeStaticRegressor",
+              (const Data::Matrix3x & (*)(const Model &, Data &, const VectorXd &))&regressor::computeStaticRegressor,
+              bp::args("Model","Data",
+                       "Configuration q (size Model::nq)"),
+              "Compute the static regressor that links the inertia parameters of the system to its center of mass position\n"
+              ",put the result in Data and return it.",
+              bp::return_value_policy<bp::return_by_value>());
+      
+    }
+    
+  } // namespace python
+} // namespace se3

src/algorithm/regressor.hpp

+//
+// Copyright (c) 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_regressor_hpp__
+#define __se3_regressor_hpp__
+
+#include "pinocchio/multibody/model.hpp"
+
+namespace se3
+{
+  
+  namespace regressor
+  {
+    
+    
+    ///
+    /// \brief Computes the static regressor that links the center of mass positions of all the links
+    ///        to the center of mass of the complete model according to the current configuration of the robot.
+    ///
+    /// \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).
+    ///
+    /// \return The static regressor of the system.
+    ///
+    inline Data::Matrix3x &
+    computeStaticRegressor(const Model & model,
+                           Data & data,
+                           const Eigen::VectorXd & q);
+  }
+  
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+#include "pinocchio/algorithm/regressor.hxx"
+
+#endif // ifndef __se3_regressor_hpp__

src/algorithm/regressor.hxx

+//
+// Copyright (c) 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_regressor_hxx__
+#define __se3_regressor_hxx__
+
+#include "pinocchio/algorithm/check.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+
+namespace se3
+{
+  
+  namespace regressor
+  {
+    
+    inline Data::Matrix3x &
+    computeStaticRegressor(const Model & model,
+                           Data & data,
+                           const Eigen::VectorXd & q)
+    {
+#ifndef NDEBUG
+      assert(model.check(data) && "data is not consistent with model.");
+#endif
+      assert(q.size() == model.nq);
+      
+      typedef Data::Matrix3x Matrix3x;
+      typedef SizeDepType<4>::ColsReturn<Matrix3x>::Type ColsBlock;
+      
+      forwardKinematics(model,data,q);
+      
+      // Computes the total mass of the system
+      double mass = 0.;
+      for(int i = 1; i < model.njoints; ++i)
+        mass += model.inertias[(size_t)i].mass();
+      
+      const double mass_inv = 1./mass;
+      for(int i = 1; i < model.njoints; ++i)
+      {
+        const SE3 & oMi = data.oMi[(size_t)i];
+        ColsBlock sr_cols = data.staticRegressor.middleCols<4>((i-1)*4);
+        sr_cols.col(0) = oMi.translation();
+        sr_cols.rightCols<3>() = oMi.rotation();
+        sr_cols *= mass_inv;
+      }
+      
+      return data.staticRegressor;
+    }
+  }
+  
+} // namespace se3
+
+#endif // ifndef __se3_regressor_hxx__

src/multibody/model.hpp

@@ -505,6 +505,9 @@ namespace se3
     /// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics.
     Eigen::VectorXd impulse_c;
     
+    // data related to regressor
+    Matrix3x staticRegressor;
+    
     ///
     /// \brief Default constructor of se3::Data from a se3::Model.
     ///

src/multibody/model.hxx

@@ -268,6 +268,7 @@ namespace se3
     ,torque_residual(model.nv)
     ,dq_after(model.nv)
     ,impulse_c()
+    ,staticRegressor(3,4*(model.njoints-1))
   {
     /* Create data strcture associated to the joints */
     for(Model::Index i=0;i<(Model::JointIndex)(model.njoints);++i) 

unittest/CMakeLists.txt

@@ -135,3 +135,4 @@ ADD_UNIT_TEST(algo-check eigen3)
 ADD_UNIT_TEST(joint-composite eigen3)
 
 ADD_UNIT_TEST(liegroups eigen3)
+ADD_UNIT_TEST(regressor eigen3)

unittest/regressor.cpp

+//
+// Copyright (c) 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/spatial/fwd.hpp"
+#include "pinocchio/algorithm/regressor.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include <iostream>
+
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
+
+BOOST_AUTO_TEST_CASE(test_static_regressor)
+{
+  using namespace Eigen;
+  using namespace se3;
+  
+  se3::Model model; buildModels::humanoidSimple(model);
+  
+  se3::Data data(model);
+  se3::Data data_ref(model);
+  
+  model.lowerPositionLimit.head<7>().fill(-1.);
+  model.upperPositionLimit.head<7>().fill(1.);
+  
+  VectorXd q = randomConfiguration(model);
+  regressor::computeStaticRegressor(model,data,q);
+  
+  VectorXd phi(4*(model.njoints-1));
+  for(int k = 1; k < model.njoints; ++k)
+  {
+    const Inertia & Y = model.inertias[(size_t)k];
+    phi.segment<4>(4*(k-1)) << Y.mass(), Y.mass() * Y.lever();
+  }
+  
+  Vector3d com = centerOfMass(model,data_ref,q);
+  Vector3d static_com_ref;
+  static_com_ref <<  com;
+  
+  Vector3d static_com = data.staticRegressor * phi;
+  
+  BOOST_CHECK(static_com.isApprox(static_com_ref)); 
+}
+
+BOOST_AUTO_TEST_SUITE_END()