Commit 0c386559 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add thread safe API to CenterOfMassComputation.

parent 9e8d498c
......@@ -61,14 +61,36 @@ namespace hpp {
void add (const JointPtr_t& rootOfSubtree);
/// Compute the center of mass and Jacobian of the sub-trees.
void compute (const Computation_t& flag = COMPUTE_ALL);
/// \param flag select which values must be computed.
void compute (const Computation_t& flag = COMPUTE_ALL)
{
compute (robot_->d(), flag);
}
/// \copydoc compute(const Computation_t&)
/// \param data where to write results.
void compute (DeviceData& data, const Computation_t& flag);
/// Get center of mass of the subtree.
const vector3_t& com () const { return data.com [0]; }
const vector3_t& com () const { return com (robot_->d()); }
/// Get mass of the sub-tree.
const value_type& mass () const { return data.mass[0]; }
const value_type& mass () const { return mass (robot_->d()); }
/// Get Jacobian of center of mass of the sub-tree.
const ComJacobian_t& jacobian () const { return data.Jcom ; }
const ComJacobian_t& jacobian () const { return jacobian(robot_->d()); }
/// \copydoc com
/// \param d the data where results were written
/// (passed to compute(const DeviceData&, const Computation_t&)).
static const vector3_t& com (const DeviceData& d) { return d.data_->com [0]; }
/// \copydoc mass
/// \param d the data where results were written
/// (passed to compute(const DeviceData&, const Computation_t&)).
static const value_type& mass (const DeviceData& d) { return d.data_->mass[0]; }
/// \copydoc jacobian
/// \param d the data where results were written
/// (passed to compute(const DeviceData&, const Computation_t&)).
static const ComJacobian_t& jacobian (const DeviceData& d) { return d.data_->Jcom ; }
/// Get const reference to the vector of sub-tree roots.
const JointRootIndexes_t & roots () const { return roots_; }
......@@ -81,13 +103,6 @@ namespace hpp {
DevicePtr_t robot_;
// Root of the subtrees
JointRootIndexes_t roots_;
// Specific pinocchio Data to store the computation results
Data data;
// value_type mass_;
// vector3_t com_;
// ComJacobian_t jacobianCom_;
}; // class CenterOfMassComputation
} // namespace pinocchio
} // namespace hpp
......
......@@ -54,6 +54,7 @@ namespace hpp {
friend class Frame;
friend class DeviceSync;
friend class CollisionObject;
friend class CenterOfMassComputation;
public:
/// Collision pairs between bodies
typedef std::pair <JointPtr_t, JointPtr_t> CollisionPair_t;
......
......@@ -36,7 +36,7 @@ namespace hpp {
return CenterOfMassComputationPtr_t (new CenterOfMassComputation (d));
}
void CenterOfMassComputation::compute (const Computation_t& flag)
void CenterOfMassComputation::compute (DeviceData& d, const Computation_t& flag)
{
const Model& model = robot_->model();
......@@ -45,8 +45,7 @@ namespace hpp {
assert(computeCOM && "This does nothing");
assert (!(computeJac && !computeCOM)); // JACOBIAN => COM
// update kinematics
::pinocchio::copy(model,robot_->data(),data,0);
Data& data = *d.data_;
data.mass[0] = 0;
if(computeCOM) data.com[0].setZero();
......@@ -146,12 +145,12 @@ namespace hpp {
}
CenterOfMassComputation::CenterOfMassComputation (const DevicePtr_t& d) :
robot_(d), roots_ (), //mass_ (0), jacobianCom_ (3, d->numberDof ())
data(d->model())
robot_(d), roots_ ()
{ assert (d->modelPtr()); }
void CenterOfMassComputation::add (const JointPtr_t& j)
{
const Data& data = robot_->data();
JointIndex jid = j->index();
BOOST_FOREACH( const JointIndex rootId, roots_ )
{
......
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