Commit ce5a422e authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

Modified the subtree center of mass, and added unittest to validate it against...

Modified the subtree center of mass, and added unittest to validate it against the classical Jcom and by finite differencing.

It seems to work fine, except for a minor problem with the finite-diff, probably coming from Pinocchio itself, that will be investigated later.
parent a5474bb4
......@@ -22,43 +22,27 @@
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/device.hh>
namespace se3 {
struct SubtreeModel
{
JointIndex root;
std::vector<JointIndex> joints; // Does not include root itself
};
}
namespace hpp {
namespace pinocchio {
class CenterOfMassComputation
{
public:
typedef std::vector <se3::JointIndex> JointRootIndexes_t;
public:
static CenterOfMassComputationPtr_t create (const DevicePtr_t& device);
void add (const JointPtr_t& joint);
void add (const JointPtr_t& rootOfSubtree);
void compute (const Device::Computation_t& flag
= Device::ALL);
const vector3_t& com () const
{
return com_;
}
const value_type& mass () const
{
return mass_;
}
void computeMass ();
const ComJacobian_t& jacobian () const
{
return jacobianCom_;
}
const vector3_t& com () const { return data.com [0]; }
const value_type& mass () const { return data.mass[0]; }
const ComJacobian_t& jacobian () const { return data.Jcom ; }
const JointRootIndexes_t & roots () const { return roots_; }
void computeMass () HPP_PINOCCHIO_DEPRECATED {}
~CenterOfMassComputation ();
......@@ -66,14 +50,16 @@ namespace hpp {
CenterOfMassComputation (const DevicePtr_t& device);
private:
typedef std::vector <se3::SubtreeModel> JointIndexes_t;
DevicePtr_t robot_;
// Root of the subtrees
JointIndexes_t joints_;
JointRootIndexes_t roots_;
// Specific pinocchio Data to store the computation results
se3::Data data;
// value_type mass_;
// vector3_t com_;
// ComJacobian_t jacobianCom_;
value_type mass_;
vector3_t com_;
ComJacobian_t jacobianCom_;
}; // class CenterOfMassComputation
} // namespace pinocchio
} // namespace hpp
......
......@@ -17,8 +17,10 @@
#include "hpp/pinocchio/center-of-mass-computation.hh"
#include <algorithm>
#include <queue>
#include <boost/foreach.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/copy.hpp>
#include "hpp/pinocchio/joint.hh"
#include "hpp/pinocchio/device.hh"
......@@ -35,70 +37,118 @@ namespace hpp {
void CenterOfMassComputation::compute (const Device::Computation_t& flag)
{
assert (mass_ > 0);
const se3::Model& model = *robot_->model();
se3::Data& data = *robot_->data();
bool computeCOM = (flag & Device::COM);
bool computeJac = (flag & Device::JACOBIAN);
assert(computeCOM && "This does nothing");
assert (!(computeJac && !computeCOM)); // JACOBIAN => COM
if (computeJac) {
com_.setZero();
jacobianCom_.setZero ();
for (std::size_t i = 0; i < joints_.size(); ++i) {
se3::subtreeJacobianCenterOfMass(model, data, joints_[i]);
se3::JointIndex j = joints_[i].root;
com_ += data.mass[j] * data.com[j];
jacobianCom_ += data.mass[j] * data.Jcom;
// update kinematics
se3::copy<0>(model,*robot_->data(),data);
data.mass[0] = 0;
if(computeCOM) data.com[0].setZero();
if(computeJac) data.Jcom.setZero();
// Propagate COM initialization on all joints.
// Could be done only on subtree, with minor gain (possibly loss because of
// more difficult branch prediction). I dont recommend to implement it.
for(se3::Model::JointIndex jid=1;jid<se3::JointIndex(model.nbody);++jid)
{
const double & mass = model.inertias[jid].mass ();
data.mass[jid] = mass;
data.com [jid] = mass*data.oMi[jid].act (model.inertias[jid].lever());
}
com_ /= mass_;
jacobianCom_ /= mass_;
} else if (computeCOM) {
com_.setZero();
se3::centerOfMass(model, data,
robot_->currentConfiguration(), true, false);
for (std::size_t i = 0; i < joints_.size(); ++i) {
se3::JointIndex j = joints_[i].root;
com_ += data.mass[j] * data.oMi[j].act(data.com[j]);
// Nullify non-subtree com and mass.
int root = 0;
for( se3::JointIndex jid=1; int(jid)<model.njoint; ++jid )
{
const se3::JointIndex& rootId = roots_[root];
if(jid == rootId)
{
jid = data.lastChild[rootId];
root ++;
}
else
{
data.mass[jid] = 0;
data.com [jid] .setZero();
}
}
com_ /= mass_;
}
// Assume root is sorted from smallest id.
// Nasty cast below, from (u-long) size_t to int.
for( int root=int(roots_.size()-1); root>=0; --root )
{
se3::JointIndex rootId = roots_[root];
// Backward loop on descendents of joint rootId.
for( se3::JointIndex jid = data.lastChild[rootId];jid>=rootId;--jid )
{
if(computeJac)
se3::JacobianCenterOfMassBackwardStep
::run(model.joints[jid],data.joints[jid],
se3::JacobianCenterOfMassBackwardStep::ArgsType(model,data,false));
else
{
assert(computeCOM);
const se3::JointIndex & parent = model.parents[jid];
data.com [parent] += data.com [jid];
data.mass[parent] += data.mass[jid];
}
//std::cout << data.oMi [jid] << std::endl;
//std::cout << data.mass[jid] << std::endl;
//std::cout << data.com [jid].transpose() << std::endl;
} // end for jid
// Backward loop on ancestors of joint rootId
se3::JointIndex jid = model.parents[rootId]; // loop variable
rootId = (root>0) ? roots_[root-1] : 0; // root of previous subtree in roots_
while (jid>rootId) // stop when meeting the next subtree
{
const se3::JointIndex & parent = model.parents[jid];
if(computeJac)
se3::JacobianCenterOfMassBackwardStep
::run(model.joints[jid],data.joints[jid],
se3::JacobianCenterOfMassBackwardStep::ArgsType(model,data,false));
else
{
assert(computeCOM);
data.com [parent] += data.com [jid];
data.mass[parent] += data.mass[jid];
}
jid = parent;
} // end while
} // end for root in roots_
if(computeCOM) data.com[0] /= data.mass[0];
if(computeJac) data.Jcom /= data.mass[0];
}
CenterOfMassComputation::CenterOfMassComputation (const DevicePtr_t& d) :
robot_(d), joints_ (), mass_ (0), jacobianCom_ (3, d->numberDof ())
{}
robot_(d), roots_ (), //mass_ (0), jacobianCom_ (3, d->numberDof ())
data(*d->model())
{ assert (d->model()); }
void CenterOfMassComputation::add (const JointPtr_t& j)
{
se3::JointIndex jid = j->index();
const se3::Model& m = *robot_->model();
for (std::size_t i = 0; i < joints_.size(); ++i) {
se3::JointIndex sbId = joints_[i].root;
// Check that jid is not in the subtree
for (se3::JointIndex id = jid; id != 0; id = m.parents[id])
if (id == sbId) {
const se3::Model& model = *robot_->model();
BOOST_FOREACH( const se3::JointIndex rootId, roots_ )
{
assert (int(rootId)<model.njoint);
// Assert that the new root is not in already-recorded subtrees.
if( (jid<rootId) || (data.lastChild[rootId]<int(jid)) )
// We are doing something stupid. Should we throw an error
// or just return silently ?
throw std::invalid_argument("This joint is already in a subtree");
// return;
}
}
joints_.push_back(se3::SubtreeModel());
joints_.back().root = jid;
se3::subtreeIndexes(m, joints_.back());
}
}
void CenterOfMassComputation::computeMass ()
{
const se3::Model& model = *robot_->model();
se3::Data& data = *robot_->data();
se3::centerOfMass(model, data,
robot_->currentConfiguration(), true, false);
mass_ = 0;
for (std::size_t i = 0; i < joints_.size(); ++i)
mass_ += data.mass[i];
roots_.push_back(jid);
}
CenterOfMassComputation::~CenterOfMassComputation ()
......
......@@ -22,106 +22,171 @@ namespace se3
{
// Compute the vector of joint index of the subtree.
inline void
subtreeIndexes(const Model & model, SubtreeModel & stModel)
{
JointIndex root = stModel.root;
const size_t shift = root;
std::vector<bool> shouldInclude (model.nbody-shift, true);
for( Model::JointIndex i= (Model::JointIndex) (model.nbody-1);i>root;--i )
{
if (!shouldInclude[i - shift]) continue;
Model::JointIndex j = i;
while (j > root) j = model.parents[j];
if (j == root) continue;
j = i;
while (j > root) {
shouldInclude[j - shift] = false;
j = model.parents[j];
}
}
stModel.joints.clear();
for(int i=(int)shouldInclude.size()-1; i>=0;--i)
if (shouldInclude[i]) stModel.joints.push_back(i+shift);
}
// inline void
// subtreeIndexes(const Model & model, SubtreeModel & stModel)
// {
// JointIndex root = stModel.root;
// const size_t shift = root;
// std::vector<bool> shouldInclude (model.nbody-shift, true);
// for( Model::JointIndex i= (Model::JointIndex) (model.nbody-1);i>root;--i )
// {
// if (!shouldInclude[i - shift]) continue;
// Model::JointIndex j = i;
// while (j > root) j = model.parents[j];
// if (j == root) continue;
// j = i;
// while (j > root) {
// shouldInclude[j - shift] = false;
// j = model.parents[j];
// }
// }
// stModel.joints.clear();
// for(int i=(int)shouldInclude.size()-1; i>=0;--i)
// if (shouldInclude[i]) stModel.joints.push_back(i+shift);
// }
// struct SubtreeJacobianCenterOfMassBackwardStep
// : public fusion::JointVisitor<SubtreeJacobianCenterOfMassBackwardStep>
// {
// typedef boost::fusion::vector<const se3::Model &,
// se3::Data &,
// JointIndex
// > ArgsType;
// JOINT_VISITOR_INIT(SubtreeJacobianCenterOfMassBackwardStep);
// 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 JointIndex& r )
// {
// const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
// const Model::JointIndex & parent = model.parents[i];
// if (i != r) {
// data.com[parent] += data.com[i];
// data.mass[parent] += data.mass[i];
// }
// typedef 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==1 )
// data.Jcom.col(jmodel.idx_v())
// = data.mass[i] * Jcols.template topLeftCorner<3,1>()
// - data.com[i].cross(Jcols.template bottomLeftCorner<3,1>());
// else
// jmodel.jointCols(data.Jcom)
// = data.mass[i] * Jcols.template topRows<3>()
// - skew(data.com[i]) * Jcols.template bottomRows<3>();
// data.com[i] /= data.mass[i];
// }
// } ;
struct SubtreeJacobianCenterOfMassBackwardStep
: public fusion::JointVisitor<SubtreeJacobianCenterOfMassBackwardStep>
inline void
subtreeCenterOfMass(const Model & model, Data & data,
const std::vector <JointIndex>& roots)
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
JointIndex
> ArgsType;
JOINT_VISITOR_INIT(SubtreeJacobianCenterOfMassBackwardStep);
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 JointIndex& r )
data.com[0].setZero ();
data.mass[0] = 0;
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
{
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::JointIndex & parent = model.parents[i];
typedef 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==1 )
data.Jcom.col(jmodel.idx_v())
= data.mass[r] * Jcols.template topLeftCorner<3,1>()
- data.com[r].cross(Jcols.template bottomLeftCorner<3,1>());
else
jmodel.jointCols(data.Jcom)
= data.mass[r] * Jcols.template topRows<3>()
- skew(data.com[r]) * Jcols.template bottomRows<3>();
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
data.mass[i] = mass;
data.com[i] = mass*data.oMi[i].act(lever);
}
};
// Assume root is sorted from largest id.
// Nasty cast below, from (u-long) size_t to int.
for( int root=int(roots.size()-1); root>=0; --root )
{
se3::JointIndex rootId = roots[root];
// Backward loop on descendents
for( se3::JointIndex jid = data.lastChild[rootId];jid>=rootId;--jid )
{
const Model::JointIndex & parent = model.parents[jid];
data.com [parent] += data.com [jid];
data.mass[parent] += data.mass[jid];
} // end for jid
// Backward loop on ancestors
se3::JointIndex jid = model.parents[rootId];
rootId = (root>0) ? roots[root-1] : 0;
while (jid>rootId) // stopping when meeting the next subtree
{
const Model::JointIndex & parent = model.parents[jid];
data.com [parent] += data.com [jid];
data.mass[parent] += data.mass[jid];
jid = parent;
} // end while
} // end for i
data.com[0] /= data.mass[0];
data.Jcom /= data.mass[0];
}
inline const Data::Matrix3x &
inline void
subtreeJacobianCenterOfMass(const Model & model, Data & data,
const SubtreeModel & stModel)
const std::vector <JointIndex>& roots)
{
Model::JointIndex r = stModel.root;
assert (r == stModel.joints.back());
data.Jcom.setZero();
for(std::size_t j = 0; j < stModel.joints.size(); ++j)
data.com[0].setZero ();
data.mass[0] = 0;
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
{
Model::JointIndex i = stModel.joints[j];
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
data.mass[i] = mass;
data.com[i] = mass*data.oMi[i].act(lever);
}
// Backward step
for(std::size_t j = 0; j < stModel.joints.size(); ++j)
{
Model::JointIndex i = stModel.joints[j];
JacobianCenterOfMassBackwardStep
::run(model.joints[i],data.joints[i],
JacobianCenterOfMassBackwardStep::ArgsType(model,data, true));
}
for (Model::JointIndex i = model.parents[r]; i != 0; i = model.parents[i])
{
SubtreeJacobianCenterOfMassBackwardStep
::run(model.joints[i],data.joints[i],
SubtreeJacobianCenterOfMassBackwardStep::ArgsType(model,data, r));
}
data.Jcom /= data.mass[r];
return data.Jcom;
// Assume root is sorted from largest id.
// Nasty cast below, from (u-long) size_t to int.
for( int root=int(roots.size()-1); root>=0; --root )
{
se3::JointIndex rootId = roots[root];
// Backward loop on descendents
for( se3::JointIndex jid = data.lastChild[rootId];jid>=rootId;--jid )
{
std::cout << "Bwd1 " << jid << std::endl;
se3::JacobianCenterOfMassBackwardStep
::run(model.joints[jid],data.joints[jid],
JacobianCenterOfMassBackwardStep::ArgsType(model,data,false));
std::cout << data.oMi [jid] << std::endl;
std::cout << data.mass[jid] << std::endl;
std::cout << data.com [jid].transpose() << std::endl;
} // end for jid
// Backward loop on ancestors
se3::JointIndex jid = model.parents[rootId];
rootId = (root>0) ? roots[root-1] : 0;
while (jid>rootId) // stopping when meeting the next subtree
{
std::cout << "Bwd2 " << jid << std::endl;
se3::JacobianCenterOfMassBackwardStep
::run(model.joints[jid],data.joints[jid],
JacobianCenterOfMassBackwardStep::ArgsType(model,data,false));
jid = model.parents[jid];
} // end while
} // end for i
data.com[0] /= data.mass[0];
data.Jcom /= data.mass[0];
}
} // namespace se3
......@@ -22,6 +22,9 @@
#include <hpp/model/urdf/util.hh>
#include <hpp/model/center-of-mass-computation.hh>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/center-of-mass-computation.hh>
......@@ -49,7 +52,7 @@ BOOST_AUTO_TEST_CASE (CenterOfMassComputation)
comP->add(rp->rootJoint());
comM->computeMass();
comP->computeMass();
//comP->computeMass();
Eigen::VectorXd q = Eigen::VectorXd::Random( rm->configSize() );
q[3] += 1.0;
......@@ -66,6 +69,8 @@ BOOST_AUTO_TEST_CASE (CenterOfMassComputation)
comM->compute(model::Device::COM);
comP->compute(pinoc::Device::COM);
BOOST_CHECK(comM->com().isApprox(comP->com()));
std::cout << comM->com() << " -- "
<< comP->com().transpose() << std::endl << std::endl;
comM->compute(model::Device::ALL);
comP->compute(pinoc::Device::ALL);
......@@ -74,203 +79,111 @@ BOOST_AUTO_TEST_CASE (CenterOfMassComputation)
}
/* -------------------------------------------------------------------------- */
BOOST_AUTO_TEST_CASE (easyGetter)
{
hpp::model::DevicePtr_t model = hppModel();
hpp::pinocchio::DevicePtr_t pinocchio = hppPinocchio();
model->setDimensionExtraConfigSpace(2);
pinocchio->setDimensionExtraConfigSpace(2);
/* --- Check NQ NV */
if(verbose)
// Modify the model so that all masses except thos of the subtrees become 0.
static void nullifyMasses(se3::Model & model, const se3::Data & data,
const std::vector <se3::JointIndex> & roots )
{
int root = 0;
for( se3::JointIndex jid=1; int(jid)<model.njoint; ++jid )
{
std::cout << "nq = " << model->configSize() << " vs " << pinocchio->configSize() << std::endl;
std::cout << "nq = " << model->numberDof() << " vs " << pinocchio->numberDof() << std::endl;
const se3::JointIndex& rootId = roots[root];
if(jid == rootId)
{
jid = data.lastChild[rootId];
root ++;
}
else
{
if(verbose) std::cout<<"Nullified mass " << jid << std::endl;
model.inertias[jid].mass() = 0;
}
}
BOOST_CHECK ( model->configSize()==pinocchio->configSize() );
BOOST_CHECK ( model->numberDof() ==pinocchio->numberDof() );
/* --- Check configuration */
{
BOOST_CHECK ( model ->currentConfiguration().size()==model ->configSize() );
BOOST_CHECK ( pinocchio->currentConfiguration().size()==pinocchio->configSize() );
Eigen::VectorXd q = Eigen::VectorXd::Random( model->configSize() );
const Eigen::VectorXd qcopy = q;
model->currentConfiguration(q);
BOOST_CHECK( model->currentConfiguration() == qcopy );
q = qcopy;
pinocchio->currentConfiguration(q);
BOOST_CHECK( pinocchio->currentConfiguration() == qcopy );
std::cout << pinocchio->neutralConfiguration().transpose() << std::endl;
std::cout << model->neutralConfiguration().transpose() << std::endl;
// This does not work.