Commit ffc99683 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #28 from nmansard/topic/Jcom

Modified the subtree center of mass
parents a5474bb4 ce5a422e
......@@ -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.
// BOOST_CHECK( pinocchio->neutralConfiguration().isApprox( m2p::q(model->neutralConfiguration()) ) );