Commit b24f7d32 authored by jcarpent's avatar jcarpent
Browse files

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

parent 35f44626
#
# 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
......
#
# 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
)
......
......@@ -39,6 +39,7 @@ namespace se3
void exposeCAT();
void exposeJacobian();
void exposeGeometryAlgo();
void exposeRegressor();
void exposeAlgorithms();
......
//
// 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
......
//
// 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
//
// 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__
//
// 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__
......@@ -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.
///
......
......@@ -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)
......
......@@ -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)
//
// 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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment