Commit 9a02f72b authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add CenterOfMassComputation with unimplement part.

parent 6fbb1b8e
......@@ -77,6 +77,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/pinocchio/configuration.hh
include/hpp/pinocchio/collision-object.hh
include/hpp/pinocchio/extra-config-space.hh
include/hpp/pinocchio/center-of-mass-computation.hh
)
IF(HPP_MODEL_FOUND)
......
// Copyright (c) 2016, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH
# define HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH
# include <list>
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/device.hh>
namespace hpp {
namespace pinocchio {
class CenterOfMassComputation
{
public:
static CenterOfMassComputationPtr_t create (const DevicePtr_t& device);
void add (const JointPtr_t& joint);
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_;
}
~CenterOfMassComputation ();
protected:
CenterOfMassComputation (const DevicePtr_t& device);
private:
// Keep a tree structure in order to compute a partial COM
struct JointSubtree_t;
typedef std::vector <JointSubtree_t> JointSubtrees_t;
// JointTreeElement_t s that have no parents
JointSubtrees_t jointSubtrees_;
value_type mass_;
vector3_t com_;
ComJacobian_t jacobianCom_;
}; // class CenterOfMassComputation
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH
//
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
// Author: NMansard, Joseph Mirabel from Florent Lamiraux
//
//
// This file is part of hpp-model
......@@ -57,6 +57,7 @@ namespace hpp {
HPP_PREDEF_CLASS (Joint);
HPP_PREDEF_CLASS (JointConfiguration);
HPP_PREDEF_CLASS (Gripper);
HPP_PREDEF_CLASS (CenterOfMassComputation);
enum Request_t {COLLISION, DISTANCE};
typedef double value_type;
......@@ -90,6 +91,7 @@ namespace hpp {
typedef boost::shared_ptr <const Device> DeviceConstPtr_t;
typedef std::vector <se3::DistanceResult> DistanceResults_t;
typedef boost::shared_ptr <HumanoidRobot> HumanoidRobotPtr_t;
typedef boost::shared_ptr <CenterOfMassComputation> CenterOfMassComputationPtr_t;
typedef boost::shared_ptr<Joint> JointPtr_t;
typedef boost::shared_ptr<const Joint> JointConstPtr_t;
typedef boost::shared_ptr <Gripper> GripperPtr_t;
......
......@@ -25,6 +25,7 @@ SET(LIBRARY_SOURCES
body.cc
device-object-vector.cc
gripper.cc
center-of-mass-computation.cc
)
IF(HPP_MODEL_FOUND)
......
// Copyright (c) 2016, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.
#include "hpp/pinocchio/center-of-mass-computation.hh"
#include <algorithm>
#include <queue>
#include "hpp/pinocchio/joint.hh"
#include "hpp/pinocchio/device.hh"
namespace hpp {
namespace pinocchio {
struct CenterOfMassComputation::JointSubtree_t {
JointPtr_t j_;
value_type mass_;
JointSubtree_t (const JointPtr_t& joint) : j_ (joint) {}
};
CenterOfMassComputationPtr_t CenterOfMassComputation::create (
const DevicePtr_t& d)
{
return CenterOfMassComputationPtr_t (new CenterOfMassComputation (d));
}
void CenterOfMassComputation::computeMass ()
{
mass_ = 0;
for (JointSubtrees_t::iterator _subtree = jointSubtrees_.begin ();
_subtree != jointSubtrees_.end (); ++_subtree) {
// TODO: Compute the mass of the subtree
// Its root joint is (JointPtr_t): _subtree->j_
//
// _subtree->mass_ = ???
mass_ += _subtree->mass_;
}
assert (mass_ > 0);
}
void CenterOfMassComputation::compute (const Device::Computation_t& flag)
{
assert (mass_ > 0);
if (flag & Device::COM) {
com_.setZero();
for (JointSubtrees_t::iterator _subtree = jointSubtrees_.begin ();
_subtree != jointSubtrees_.end (); ++_subtree) {
vector3_t com;
assert(false && "Unimplemented");
// TODO: Compute com of subtree
// Its root joint is (JointPtr_t): _subtree->j_
//
// com = ???
com_ += _subtree->mass_ * com;
}
com_ /= mass_;
}
if (flag & Device::JACOBIAN) {
jacobianCom_.setZero ();
for (JointSubtrees_t::iterator _subtree = jointSubtrees_.begin ();
_subtree != jointSubtrees_.end (); ++_subtree) {
ComJacobian_t Jcom;
assert(false && "Unimplemented");
// TODO: Compute jacobian of subtree
// Its root joint is (JointPtr_t): _subtree->j_
//
// Jcom = ???
jacobianCom_ += _subtree->mass_ * Jcom;
}
jacobianCom_ /= mass_;
}
}
CenterOfMassComputation::CenterOfMassComputation (const DevicePtr_t& d) :
jointSubtrees_ (), mass_ (-1), jacobianCom_ (3, d->numberDof ())
{}
void CenterOfMassComputation::add (const JointPtr_t& j)
{
se3::JointIndex jid = j->index();
const se3::Model& m = *j->robot()->model();
for (JointSubtrees_t::iterator _subtree = jointSubtrees_.begin ();
_subtree != jointSubtrees_.end (); ++_subtree) {
se3::JointIndex sbId = _subtree->j_->index();
// Check that jid is not in the subtree
for (se3::JointIndex id = jid; id != 0; id = m.parents[id])
if (id == sbId) {
// 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;
}
}
jointSubtrees_.push_back(JointSubtree_t(j));
}
CenterOfMassComputation::~CenterOfMassComputation ()
{}
} // namespace pinocchio
} // namespace hpp
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