pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
center-of-mass.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
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 
21  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
22  inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
23 
35  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
36  inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
37  DataTpl<Scalar,Options,JointCollectionTpl> & data);
38 
47  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
48  inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
49  DataTpl<Scalar,Options,JointCollectionTpl> & data);
50 
65  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
66  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
67  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
68  DataTpl<Scalar,Options,JointCollectionTpl> & data,
69  const Eigen::MatrixBase<ConfigVectorType> & q,
70  const bool computeSubtreeComs = true);
71 
89  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
90  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
91  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
92  DataTpl<Scalar,Options,JointCollectionTpl> & data,
93  const Eigen::MatrixBase<ConfigVectorType> & q,
94  const Eigen::MatrixBase<TangentVectorType> & v,
95  const bool computeSubtreeComs = true);
96 
116  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
117  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
118  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
119  DataTpl<Scalar,Options,JointCollectionTpl> & data,
120  const Eigen::MatrixBase<ConfigVectorType> & q,
121  const Eigen::MatrixBase<TangentVectorType1> & v,
122  const Eigen::MatrixBase<TangentVectorType2> & a,
123  const bool computeSubtreeComs = true);
124 
137  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
138  inline void centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
139  DataTpl<Scalar,Options,JointCollectionTpl> & data,
140  const int LEVEL,
141  const bool computeSubtreeComs = true);
142 
154  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
155  inline void centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
156  DataTpl<Scalar,Options,JointCollectionTpl> & data,
157  const bool computeSubtreeComs = true)
158  { centerOfMass(model,data,2,computeSubtreeComs); }
159 
175  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
176  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
177  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
178  DataTpl<Scalar,Options,JointCollectionTpl> & data,
179  const Eigen::MatrixBase<ConfigVectorType> & q,
180  const bool computeSubtreeComs = true);
181 
197  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
198  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
199  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
200  DataTpl<Scalar,Options,JointCollectionTpl> & data,
201  const bool computeSubtreeComs = true);
202 
217  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix3xLike>
218  inline void
219  jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
220  DataTpl<Scalar,Options,JointCollectionTpl> & data,
221  const Eigen::MatrixBase<ConfigVectorType> & q,
222  const JointIndex & rootSubtreeId,
223  const Eigen::MatrixBase<Matrix3xLike> & res);
224 
237  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
238  inline void
239  jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
240  DataTpl<Scalar,Options,JointCollectionTpl> & data,
241  const JointIndex & rootSubtreeId,
242  const Eigen::MatrixBase<Matrix3xLike> & res);
243 
256  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
257  inline void
258  getJacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
259  const DataTpl<Scalar,Options,JointCollectionTpl> & data,
260  const JointIndex & rootSubtreeId,
261  const Eigen::MatrixBase<Matrix3xLike> & res);
262 
263 
264  /* If the CRBA has been run, then both COM and Jcom are easily available from
265  * the joint space mass matrix (data.M).
266  * Use the following function to infer them directly. In that case,
267  * the COM subtrees (also easily available from CRBA data) are not
268  * explicitely set. Use data.Ycrb[i].lever() to get them. */
280  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
281  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
282  getComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
283  DataTpl<Scalar,Options,JointCollectionTpl> & data);
284 
298  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
299  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
300  getJacobianComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
301  DataTpl<Scalar,Options,JointCollectionTpl> & data);
302 
303 } // namespace pinocchio
304 
305 /* --- Details -------------------------------------------------------------------- */
306 /* --- Details -------------------------------------------------------------------- */
307 /* --- Details -------------------------------------------------------------------- */
308 #include "pinocchio/algorithm/center-of-mass.hxx"
309 
310 #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
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=true)
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:73
pinocchio::getJacobianSubtreeCenterOfMass
void getJacobianSubtreeCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
Retrieves the Jacobian of the center of mass of the given subtree according to the current value stor...
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...
pinocchio::jacobianSubtreeCenterOfMass
void jacobianSubtreeCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
Computes the Jacobian of the center of mass of the given subtree according to a particular joint conf...