pinocchio  2.1.3
center-of-mass.hpp
1 //
2 // Copyright (c) 2015-2016,2018 CNRS
3 //
4 
5 #ifndef __pinocchio_center_of_mass_hpp__
6 #define __pinocchio_center_of_mass_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 namespace pinocchio
12 {
13 
19  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
20  inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
21 
31  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
32  inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
33  DataTpl<Scalar,Options,JointCollectionTpl> & data);
34 
41  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
42  inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
43  DataTpl<Scalar,Options,JointCollectionTpl> & data);
44 
59  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
60  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
61  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
62  DataTpl<Scalar,Options,JointCollectionTpl> & data,
63  const Eigen::MatrixBase<ConfigVectorType> & q,
64  const bool computeSubtreeComs = true);
65 
83  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
84  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
85  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
86  DataTpl<Scalar,Options,JointCollectionTpl> & data,
87  const Eigen::MatrixBase<ConfigVectorType> & q,
88  const Eigen::MatrixBase<TangentVectorType> & v,
89  const bool computeSubtreeComs = true);
90 
110  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
111  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
112  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
113  DataTpl<Scalar,Options,JointCollectionTpl> & data,
114  const Eigen::MatrixBase<ConfigVectorType> & q,
115  const Eigen::MatrixBase<TangentVectorType1> & v,
116  const Eigen::MatrixBase<TangentVectorType2> & a,
117  const bool computeSubtreeComs = true);
118 
131  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
132  inline void centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
133  DataTpl<Scalar,Options,JointCollectionTpl> & data,
134  const int LEVEL,
135  const bool computeSubtreeComs = true);
136 
148  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
149  inline void centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
150  DataTpl<Scalar,Options,JointCollectionTpl> & data,
151  const bool computeSubtreeComs = true)
152  { centerOfMass(model,data,2,computeSubtreeComs); }
153 
172  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
173  PINOCCHIO_DEPRECATED
174  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
175  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
176  DataTpl<Scalar,Options,JointCollectionTpl> & data,
177  const Eigen::MatrixBase<ConfigVectorType> & q,
178  const bool computeSubtreeComs,
179  const bool updateKinematics);
180 
196  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
197  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
198  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
199  DataTpl<Scalar,Options,JointCollectionTpl> & data,
200  const Eigen::MatrixBase<ConfigVectorType> & q,
201  const bool computeSubtreeComs = true);
202 
218  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
219  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
220  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
221  DataTpl<Scalar,Options,JointCollectionTpl> & data,
222  const bool computeSubtreeComs = true);
223 
224  /* If the CRBA has been run, then both COM and Jcom are easily available from
225  * the mass matrix. Use the following methods to access them. In that case,
226  * the COM subtrees (also easily available from CRBA data) are not
227  * explicitely set. Use data.Ycrb[i].lever() to get them. */
238  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
239  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
240  getComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
241  DataTpl<Scalar,Options,JointCollectionTpl> & data);
242 
256  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
257  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
258  getJacobianComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
259  DataTpl<Scalar,Options,JointCollectionTpl> & data);
260 
261 } // namespace pinocchio
262 
263 /* --- Details -------------------------------------------------------------------- */
264 /* --- Details -------------------------------------------------------------------- */
265 /* --- Details -------------------------------------------------------------------- */
266 #include "pinocchio/algorithm/center-of-mass.hxx"
267 
268 #endif // ifndef __pinocchio_center_of_mass_hpp__
pinocchio::getJacobianComFromCrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & getJacobianComFromCrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Extracts both the jacobian of the center of mass (CoM), the total mass of the system and the CoM posi...
pinocchio::jacobianCenterOfMass
PINOCCHIO_DEPRECATED const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & jacobianCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs, const bool updateKinematics)
Computes both the jacobian and the the center of mass position of a given model according to a partic...
pinocchio::computeTotalMass
Scalar computeTotalMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Compute the total mass of the model and return it.
pinocchio::getComFromCrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & getComFromCrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix...
pinocchio::computeSubtreeMasses
void computeSubtreeMasses(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the mass of each kinematic subtree and store it in data.mass. The element mass[0] corresponds...
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:68
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::centerOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & centerOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
Computes the center of mass position of a given model according to a particular joint configuration...