Commit f52ee642 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[algorithm] computeSubtreeMasses

parent aa58d690
......@@ -57,6 +57,11 @@ namespace pinocchio
bp::args("Model", "Data"),
"Compute the total mass of the model, put it in data.mass[0] and return it.");
bp::def("computeSubtreeMasses",
(void (*)(const Model &, Data &))&computeSubtreeMasses<double,0,JointCollectionDefaultTpl>,
bp::args("Model", "Data"),
"Compute the mass of each kinematic subtree and store it in data.mass.");
bp::def("centerOfMass",com_0_proxy,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)"),
......
......@@ -25,12 +25,23 @@ namespace pinocchio
/// \param[in] data The data structure of the rigid body system.
///
/// \warning This method does not fill the whole data.mass vector. Only data.mass[0] is updated.
/// If you need the whole data.mass vector to be computed, use computeSubtreeMasses
///
/// \return Total mass of the model.
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data);
/// \brief Compute the mass of each kinematic subtree and store it in data.mass. The element mass[0] corresponds to the total mass of the model.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
///
/// \note If you are only interested in knowing the total mass of the model, computeTotalMass will probably be slightly faster.
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data);
///
/// \brief Computes the center of mass position of a given model according to a particular joint configuration.
/// The result is accessible through data.com[0] for the full body com and data.com[i] for the subtree supported by joint i (expressed in the joint i frame).
......
......@@ -32,6 +32,24 @@ namespace pinocchio
return data.mass[0];
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
// Forward Step
for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
{
data.mass[i] = model.inertias[i].mass();
}
// Backward Step
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
{
const JointIndex & parent = model.parents[i];
data.mass[parent] += data.mass[i];
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......
......@@ -280,7 +280,7 @@ namespace pinocchio
/// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
container::aligned_vector<Vector3> acom;
/// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corrresponds to the total mass of the model.
/// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corresponds to the total mass of the model.
std::vector<Scalar> mass;
/// \brief Jacobien of center of mass.
......
......@@ -110,6 +110,29 @@ BOOST_AUTO_TEST_CASE ( test_mass )
BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12);
}
BOOST_AUTO_TEST_CASE ( test_subtree_masses )
{
using namespace Eigen;
using namespace pinocchio;
pinocchio::Model model;
pinocchio::buildModels::humanoidRandom(model);
pinocchio::Data data1(model);
computeSubtreeMasses(model,data1);
pinocchio::Data data2(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
centerOfMass(model,data2,q);
for(size_t i=0; i<(size_t)(model.njoints);++i)
{
BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12);
}
}
//BOOST_AUTO_TEST_CASE ( test_timings )
//{
// using namespace Eigen;
......
......@@ -36,6 +36,15 @@ class TestComBindings(TestCase):
pin.centerOfMass(self.model,data2,self.q)
self.assertApprox(mass,data2.mass[0])
def test_subtree_masses(self):
pin.computeSubtreeMasses(self.model,self.data)
data2 = self.model.createData()
pin.centerOfMass(self.model,data2,self.q)
for i in range(self.model.njoints):
self.assertApprox(self.data.mass[i],data2.mass[i])
def test_Jcom_update3(self):
Jcom = pin.jacobianCenterOfMass(self.model,self.data,self.q)
self.assertFalse(np.isnan(Jcom).any())
......
Markdown is supported
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