hpp-pinocchio  4.12.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
center-of-mass-computation.hh
Go to the documentation of this file.
1 // Copyright (c) 2016, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-pinocchio.
5 // hpp-pinocchio is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-pinocchio is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH
18 # define HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH
19 
20 # include <list>
21 
22 # include <pinocchio/multibody/data.hpp> // ::pinocchio::Data
23 
24 # include <hpp/util/serialization-fwd.hh>
25 
26 # include <hpp/pinocchio/fwd.hh>
27 # include <hpp/pinocchio/device.hh>
28 
29 namespace hpp {
30  namespace pinocchio {
44  {
45  public:
46  typedef std::vector <std::size_t> JointRootIndexes_t;
48  // This fixes an alignment issue of ::pinocchio::Data::hg
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 
52  public:
57  static CenterOfMassComputationPtr_t create (const DevicePtr_t& device);
58 
63  void add (const JointPtr_t& rootOfSubtree);
64 
67  void compute (const Computation_t& flag = COMPUTE_ALL)
68  {
69  compute (robot_->d(), flag);
70  }
71 
74  void compute (DeviceData& data, const Computation_t& flag);
75 
77  const vector3_t& com () const { return com (robot_->d()); }
79  const value_type& mass () const { return mass (robot_->d()); }
81  const ComJacobian_t& jacobian () const { return jacobian(robot_->d()); }
82 
86  static const vector3_t& com (const DeviceData& d) { return d.data_->com [0]; }
90  static const value_type& mass (const DeviceData& d) { return d.data_->mass[0]; }
94  static const ComJacobian_t& jacobian (const DeviceData& d) { return d.data_->Jcom ; }
95 
97  const JointRootIndexes_t & roots () const { return roots_; }
98 
100 
101  protected:
102  CenterOfMassComputation (const DevicePtr_t& device);
103 
104  private:
105  DevicePtr_t robot_;
106  // Root of the subtrees
107  JointRootIndexes_t roots_;
108 
110  HPP_SERIALIZABLE();
111  }; // class CenterOfMassComputation
112  } // namespace pinocchio
113 } // namespace hpp
114 #endif // HPP_PINOCCHIO_CENTER_OF_MASS_COMPUTATION_HH
static const ComJacobian_t & jacobian(const DeviceData &d)
Get Jacobian of center of mass of the sub-tree.
Definition: center-of-mass-computation.hh:94
std::vector< std::size_t > JointRootIndexes_t
Definition: center-of-mass-computation.hh:46
static CenterOfMassComputationPtr_t create(const DevicePtr_t &device)
Utility functions.
Definition: body.hh:30
void add(const JointPtr_t &rootOfSubtree)
static const vector3_t & com(const DeviceData &d)
Get center of mass of the subtree.
Definition: center-of-mass-computation.hh:86
Definition: device-data.hh:41
static const value_type & mass(const DeviceData &d)
Get mass of the sub-tree.
Definition: center-of-mass-computation.hh:90
const ComJacobian_t & jacobian() const
Get Jacobian of center of mass of the sub-tree.
Definition: center-of-mass-computation.hh:81
const value_type & mass() const
Get mass of the sub-tree.
Definition: center-of-mass-computation.hh:79
void compute(const Computation_t &flag=COMPUTE_ALL)
Definition: center-of-mass-computation.hh:67
shared_ptr< CenterOfMassComputation > CenterOfMassComputationPtr_t
Definition: fwd.hh:110
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:106
Eigen::Matrix< value_type, 3, Eigen::Dynamic > ComJacobian_t
Definition: fwd.hh:89
DataPtr_t data_
Definition: device-data.hh:53
Computation_t
Definition: device-data.hh:30
Definition: center-of-mass-computation.hh:43
const JointRootIndexes_t & roots() const
Get const reference to the vector of sub-tree roots.
Definition: center-of-mass-computation.hh:97
shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:111
double value_type
Definition: fwd.hh:40
Definition: device-data.hh:36
Definition: collision-object.hh:31
const vector3_t & com() const
Get center of mass of the subtree.
Definition: center-of-mass-computation.hh:77
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition: fwd.hh:86