Skip to content

GitLab

Commit 2ac1fd28 authored by Matthieu Vigne's avatar Matthieu Vigne
Browse files

Add function to get subtree CoM jacobian. 

Add jacobianSubtreeCenterOfMass and associated python bindings.
This function computes the jacobian of the center of mass of
a subtree, as a barycenter of individual body jacobian.
parent 06f2c611
Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/expose-com.cpp
View file @ 2ac1fd28
......@@ -11,7 +11,7 @@ namespace pinocchio
{
namespace python
{
BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMassUpdate_overload,jacobianCenterOfMass,3,4)
BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMassNoUpdate_overload,jacobianCenterOfMass,2,3)
......@@ -24,7 +24,7 @@ namespace pinocchio
{
return centerOfMass(model,data,q,computeSubtreeComs);
}
static SE3::Vector3
com_1_proxy(const Model& model,
Data & data,
......@@ -34,7 +34,7 @@ namespace pinocchio
{
return centerOfMass(model,data,q,v,computeSubtreeComs);
}
static SE3::Vector3
com_2_proxy(const Model & model,
Data & data,
......@@ -63,6 +63,29 @@ namespace pinocchio
centerOfMass(model,data,computeSubtreeComs);
}
static Data::Matrix3x
jacobian_subtree_com_kinematics_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
Model::JointIndex jointId)
{
Data::Matrix3x J(3,model.nv); J.setZero();
jacobianSubtreeCenterOfMass(model, data, q, jointId, J);
return J;
}
static Data::Matrix3x
jacobian_subtree_com_proxy(const Model & model,
Data & data,
Model::JointIndex jointId)
{
Data::Matrix3x J(3,model.nv); J.setZero();
jacobianSubtreeCenterOfMass(model, data, jointId, J);
return J;
}
BOOST_PYTHON_FUNCTION_OVERLOADS(com_0_overload, com_0_proxy, 3, 4)
BOOST_PYTHON_FUNCTION_OVERLOADS(com_1_overload, com_1_proxy, 4, 5)
......@@ -72,11 +95,11 @@ namespace pinocchio
BOOST_PYTHON_FUNCTION_OVERLOADS(com_level_overload, com_level_proxy, 3, 4)
BOOST_PYTHON_FUNCTION_OVERLOADS(com_default_overload, com_default_proxy, 2, 3)
void exposeCOM()
{
using namespace Eigen;
bp::def("computeTotalMass",
(double (*)(const Model &))&computeTotalMass<double,0,JointCollectionDefaultTpl>,
bp::args("Model"),
......@@ -163,7 +186,23 @@ namespace pinocchio
"computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"),
"Computes the jacobian of the center of mass, puts the result in Data and return it.")[
bp::return_value_policy<bp::return_by_value>()]);
bp::def("getSubtreeCoMJacobian",jacobian_subtree_com_kinematics_proxy,
bp::args("Model, the model of the kinematic tree",
"Data, the data associated to the model where the results are stored",
"Joint configuration q (size Model::nq)",
"Joint ID, the index of the joint."),
"Computes the jacobian of the CoM of the given subtree in the world frame, according to the given joint configuration.",
bp::return_value_policy<bp::return_by_value>());
bp::def("getSubtreeCoMJacobian",jacobian_subtree_com_proxy,
bp::args("Model, the model of the kinematic tree",
"Data, the data associated to the model where the results are stored",
"Joint ID, the index of the joint."),
"Computes the jacobian of the CoM of the given subtree in the world frame, according to the given entries in data.",
bp::return_value_policy<bp::return_by_value>());
}
} // namespace python
} // namespace pinocchio
src/algorithm/center-of-mass.hpp
View file @ 2ac1fd28
......@@ -10,7 +10,7 @@
namespace pinocchio
{
/// \brief Compute the total mass of the model and return it.
///
/// \param[in] model The model structure of the rigid body system.
......@@ -62,7 +62,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position and velocity of a given model according to a particular joint configuration and velocity.
/// The result is accessible through data.com[0], data.vcom[0] for the full body com position and velocity.
......@@ -87,7 +87,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
/// The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation.
......@@ -115,7 +115,7 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a,
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data and the requested level.
/// The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity.
......@@ -133,7 +133,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const int LEVEL,
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data.
/// The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity.
......@@ -150,7 +150,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const bool computeSubtreeComs = true)
{ centerOfMass(model,data,2,computeSubtreeComs); }
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
/// The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
......@@ -177,7 +177,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool computeSubtreeComs,
const bool updateKinematics);
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
/// The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
......@@ -199,7 +199,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool computeSubtreeComs = true);
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to the current value stored in data.
/// It assumes that forwardKinematics has been called first.
......@@ -221,6 +221,46 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const bool computeSubtreeComs = true);
///
/// \brief Computes the jacobian of the center of mass of the given subtree according to a particular joint configuration.
/// In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
///
/// \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] parentJointIdIn Index of the parent joint supporting the subtree.
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 3 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut);
///
/// \brief Computes the jacobian of the center of mass of the given subtree according to the current value stored in data.
/// It assumes that forwardKinematics has been called first.
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] parentJointIdIn Index of the parent joint supporting the subtree.
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 3 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut);
/* If the CRBA has been run, then both COM and Jcom are easily available from
* the mass matrix. Use the following methods to access them. In that case,
* the COM subtrees (also easily available from CRBA data) are not
......@@ -239,7 +279,7 @@ namespace pinocchio
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
getComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data);
///
/// \brief Extracts both the jacobian of the center of mass (CoM), the total mass of the system and the CoM position from the joint space inertia matrix (also called the mass matrix).
/// The results are accessible through data.Jcom, data.mass[0] and data.com[0] and are both expressed in the world frame.
......@@ -257,8 +297,8 @@ namespace pinocchio
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
getJacobianComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data);
} // namespace pinocchio
} // namespace pinocchio
/* --- Details -------------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------------- */
......
src/algorithm/center-of-mass.hxx
View file @ 2ac1fd28
......@@ -8,6 +8,7 @@
#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
/// @cond DEV
......@@ -60,7 +61,7 @@ namespace pinocchio
const bool computeSubtreeComs)
{
forwardKinematics(model,data,q.derived());
const int LEVEL = 0;
centerOfMass(model,data,LEVEL,computeSubtreeComs);
return data.com[0];
......@@ -75,12 +76,12 @@ namespace pinocchio
const bool computeSubtreeComs)
{
forwardKinematics(model,data,q.derived(),v.derived());
const int LEVEL = 1;
centerOfMass(model,data,LEVEL,computeSubtreeComs);
return data.com[0];
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......@@ -91,12 +92,12 @@ namespace pinocchio
const bool computeSubtreeComs)
{
forwardKinematics(model,data,q,v,a);
const int LEVEL = 2;
centerOfMass(model,data,LEVEL,computeSubtreeComs);
return data.com[0];
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
......@@ -105,16 +106,16 @@ namespace pinocchio
{
assert(model.check(data) && "data is not consistent with model.");
assert(LEVEL >= 0);
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Model::JointIndex JointIndex;
typedef typename Data::SE3 SE3;
typedef typename Data::Motion Motion;
typedef typename Data::Inertia Inertia;
const bool do_position = (LEVEL>=0);
const bool do_velocity = (LEVEL>=1);
const bool do_acceleration = (LEVEL>=2);
......@@ -137,36 +138,36 @@ namespace pinocchio
const Motion & a = data.a[i];
data.mass[i] = mass;
if(do_position)
data.com[i].noalias() = mass * lever;
if(do_velocity)
data.vcom[i].noalias() = mass * (v.angular().cross(lever) + v.linear());
if(do_acceleration)
data.acom[i].noalias() = mass * (a.angular().cross(lever) + a.linear())
+ v.angular().cross(data.vcom[i]); // take into accound the coriolis part of the acceleration
}
// Backward Step
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
{
const JointIndex & parent = model.parents[i];
const SE3 & liMi = data.liMi[i];
data.mass[parent] += data.mass[i];
if(do_position)
data.com[parent] += (liMi.rotation()*data.com[i]
+ data.mass[i] * liMi.translation());
if(do_velocity)
data.vcom[parent] += liMi.rotation()*data.vcom[i];
if(do_acceleration)
data.acom[parent] += liMi.rotation()*data.acom[i];
if(computeSubtreeComs)
{
if(do_position)
......@@ -177,7 +178,7 @@ namespace pinocchio
data.acom[i] /= data.mass[i];
}
}
if(do_position)
data.com[0] /= data.mass[0];
if(do_velocity)
......@@ -192,7 +193,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever());
}
......@@ -207,12 +208,12 @@ namespace pinocchio
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &,
const bool &
> ArgsType;
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointDataDerived> & jdata,
......@@ -228,10 +229,10 @@ namespace pinocchio
typedef typename Data::Matrix6x Matrix6x;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
ColBlock Jcols = jmodel.jointCols(data.J);
Jcols = data.oMi[i].act(jdata.S());
if(JointModel::NV == Eigen::Dynamic)
{
if(jmodel.nv() == 1)
......@@ -262,8 +263,8 @@ namespace pinocchio
- skew(data.com[i]) * Jcols.template bottomRows<3>();
}
}
if(computeSubtreeComs)
data.com[i] /= data.mass[i];
}
......@@ -284,7 +285,7 @@ namespace pinocchio
else
return jacobianCenterOfMass(model,data,computeSubtreeComs);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......@@ -295,7 +296,7 @@ namespace pinocchio
forwardKinematics(model, data, q);
return jacobianCenterOfMass(model, data, computeSubtreeComs);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......@@ -303,27 +304,27 @@ namespace pinocchio
const bool computeSubtreeComs)
{
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Model::JointIndex JointIndex;
typedef typename Data::SE3 SE3;
typedef typename Data::Inertia Inertia;
data.com[0].setZero();
data.mass[0] = Scalar(0);
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
{
const typename Inertia::Scalar & mass = model.inertias[i].mass();
const typename SE3::Vector3 & lever = model.inertias[i].lever();
data.mass[i] = mass;
data.com[i].noalias() = mass*data.oMi[i].act(lever);
}
// Backward step
typedef JacobianCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
for(JointIndex i= (JointIndex) (model.njoints-1); i>0; --i)
......@@ -331,13 +332,58 @@ namespace pinocchio
Pass2::run(model.joints[i],data.joints[i],
typename Pass2::ArgsType(model,data,computeSubtreeComs));
}
data.com[0] /= data.mass[0];
data.Jcom /= data.mass[0];
return data.Jcom;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut)
{
forwardKinematics(model, data, q);
jacobianSubtreeCenterOfMass(model, data, parentJointIdIn, JacobianOut);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut)
{
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x Matrix6x;
assert(model.check(data) && "data is not consistent with model.");
assert((parentJointIdIn < model.njoints) && "Invalid joint id.");
if (parentJointIdIn == 0)
{
// Joint 0 is universe: this means that the user is asking for the entier model CoM jacobian.
JacobianOut = jacobianCenterOfMass(model, data);
}
else
{
// Iterate over all bodies of subtree.
for (JointIndex id : model.subtrees[parentJointIdIn])
{
// Get joint jacobian in world.
Matrix6x jointJacobian = Matrix6x::Zero(6, model.nv);
getJointJacobian(model, data, id, ReferenceFrame::LOCAL, jointJacobian);
// Add jacobian of selected body com.
JacobianOut += model.inertias[id].mass() * data.oMi[id].rotation() * (
jointJacobian.template topRows<3>() - skew(model.inertias[id].lever()) * jointJacobian.template bottomRows<3>());
}
JacobianOut /= data.mass[parentJointIdIn];
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
getJacobianComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......@@ -345,24 +391,24 @@ namespace pinocchio
{
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::SE3 SE3;
const SE3 & oM1 = data.liMi[1];
// Extract the total mass of the system.
data.mass[0] = data.M(0,0);
// As the 6 first rows of M*a are a wrench, we just need to multiply by the
// relative rotation between the first joint and the world
const typename SE3::Matrix3 oR1_over_m (oM1.rotation() / data.M(0,0));
// I don't know why, but the colwise multiplication is much more faster
// than the direct Eigen multiplication
for(long k=0; k<model.nv;++k)
data.Jcom.col(k).noalias() = oR1_over_m * data.M.template topRows<3>().col(k);
return data.Jcom;
}
......
unittest/com.cpp
View file @ 2ac1fd28
......@@ -5,7 +5,9 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/utils/timer.hpp"
......@@ -31,9 +33,9 @@ BOOST_AUTO_TEST_CASE ( test_com )
q.middleRows<4> (3).normalize();
VectorXd v = VectorXd::Ones(model.nv);
VectorXd a = VectorXd::Ones(model.nv);
crba(model,data,q);
/* Test COM against CRBA*/
Vector3d com = centerOfMass(model,data,q);
......@@ -53,7 +55,7 @@ BOOST_AUTO_TEST_CASE ( test_com )
model.gravity.setZero();
centerOfMass(model,data,q,v,a);
nonLinearEffects(model, data, q, v);
pinocchio::SE3::Vector3 acom_from_nle (data.nle.head <3> ()/data.mass[0]);
BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));
......@@ -63,8 +65,8 @@ BOOST_AUTO_TEST_CASE ( test_com )
/* Test CoM velocity againt jacobianCenterOfMass */
BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
centerOfMass(model,data,q,v);
/* Test CoM velocity againt jacobianCenterOfMass */
BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
......@@ -143,14 +145,14 @@ BOOST_AUTO_TEST_CASE ( test_subtree_masses )
// pinocchio::Data data(model);
//
// long flag = BOOST_BINARY(1111);
// PinocchioTicToc timer(PinocchioTicToc::US);
// PinocchioTicToc timer(PinocchioTicToc::US);
// #ifdef NDEBUG
// #ifdef _INTENSE_TESTING_
// const size_t NBT = 1000*1000;
// #else
// const size_t NBT = 10;
// #endif
// #else
// #else
// const size_t NBT = 1;
// std::cout << "(the time score in debug mode is not relevant) " ;
// #endif
......@@ -180,7 +182,7 @@ BOOST_AUTO_TEST_CASE ( test_subtree_masses )
// if(verbose) std::cout << "Without sub-tree =\t";
// timer.toc(std::cout,NBT);
// }
//
//
// if( flag >> 2 & 1 )
// {
// timer.tic();
......@@ -193,4 +195,26 @@ BOOST_AUTO_TEST_CASE ( test_subtree_masses )
// }
//}
BOOST_AUTO_TEST_CASE ( test_subtree_com_jacobian )
{
using namespace Eigen;
using namespace pinocchio;
pinocchio::Model model;
pinocchio::buildModels::humanoidRandom(model);
pinocchio::Data data(model);
VectorXd q = pinocchio::randomConfiguration(model);
VectorXd v = VectorXd::Random(model.nv);
computeAllTerms(model, data, q, v);