pinocchio  3.2.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
center-of-mass.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_center_of_mass_hpp__
6 #define __pinocchio_algorithm_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  Scalar computeTotalMass(
37  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
38  DataTpl<Scalar, Options, JointCollectionTpl> & data);
39 
50  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
51  void computeSubtreeMasses(
52  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
53  DataTpl<Scalar, Options, JointCollectionTpl> & data);
54 
73  template<
74  typename Scalar,
75  int Options,
76  template<typename, int>
77  class JointCollectionTpl,
78  typename ConfigVectorType>
79  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
80  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
81  DataTpl<Scalar, Options, JointCollectionTpl> & data,
82  const Eigen::MatrixBase<ConfigVectorType> & q,
83  const bool computeSubtreeComs = true);
84 
106  template<
107  typename Scalar,
108  int Options,
109  template<typename, int>
110  class JointCollectionTpl,
111  typename ConfigVectorType,
112  typename TangentVectorType>
113  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
114  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
115  DataTpl<Scalar, Options, JointCollectionTpl> & data,
116  const Eigen::MatrixBase<ConfigVectorType> & q,
117  const Eigen::MatrixBase<TangentVectorType> & v,
118  const bool computeSubtreeComs = true);
119 
143  template<
144  typename Scalar,
145  int Options,
146  template<typename, int>
147  class JointCollectionTpl,
148  typename ConfigVectorType,
149  typename TangentVectorType1,
150  typename TangentVectorType2>
151  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
152  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
153  DataTpl<Scalar, Options, JointCollectionTpl> & data,
154  const Eigen::MatrixBase<ConfigVectorType> & q,
155  const Eigen::MatrixBase<TangentVectorType1> & v,
156  const Eigen::MatrixBase<TangentVectorType2> & a,
157  const bool computeSubtreeComs = true);
158 
175  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
176  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
177  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
178  DataTpl<Scalar, Options, JointCollectionTpl> & data,
179  KinematicLevel kinematic_level,
180  const bool computeSubtreeComs = true);
181 
196  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
197  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
198  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
199  DataTpl<Scalar, Options, JointCollectionTpl> & data,
200  const bool computeSubtreeComs = true)
201  {
202  return centerOfMass(model, data, ACCELERATION, computeSubtreeComs);
203  }
204 
225  template<
226  typename Scalar,
227  int Options,
228  template<typename, int>
229  class JointCollectionTpl,
230  typename ConfigVectorType>
231  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
232  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
233  DataTpl<Scalar, Options, JointCollectionTpl> & data,
234  const Eigen::MatrixBase<ConfigVectorType> & q,
235  const bool computeSubtreeComs = true);
236 
257  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
258  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
259  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
260  DataTpl<Scalar, Options, JointCollectionTpl> & data,
261  const bool computeSubtreeComs = true);
262 
280  template<
281  typename Scalar,
282  int Options,
283  template<typename, int>
284  class JointCollectionTpl,
285  typename ConfigVectorType,
286  typename Matrix3xLike>
287  void jacobianSubtreeCenterOfMass(
288  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
289  DataTpl<Scalar, Options, JointCollectionTpl> & data,
290  const Eigen::MatrixBase<ConfigVectorType> & q,
291  const JointIndex & rootSubtreeId,
292  const Eigen::MatrixBase<Matrix3xLike> & res);
293 
308  template<
309  typename Scalar,
310  int Options,
311  template<typename, int>
312  class JointCollectionTpl,
313  typename Matrix3xLike>
314  void jacobianSubtreeCenterOfMass(
315  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
316  DataTpl<Scalar, Options, JointCollectionTpl> & data,
317  const JointIndex & rootSubtreeId,
318  const Eigen::MatrixBase<Matrix3xLike> & res);
319 
335  template<
336  typename Scalar,
337  int Options,
338  template<typename, int>
339  class JointCollectionTpl,
340  typename Matrix3xLike>
341  void getJacobianSubtreeCenterOfMass(
342  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
343  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
344  const JointIndex & rootSubtreeId,
345  const Eigen::MatrixBase<Matrix3xLike> & res);
346 
347  /* If the CRBA has been run, then both COM and Jcom are easily available from
348  * the joint space mass matrix (data.M).
349  * Use the following function to infer them directly. In that case,
350  * the COM subtrees (also easily available from CRBA data) are not
351  * explicitely set. Use data.Ycrb[i].lever() to get them. */
365  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
366  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & getComFromCrba(
367  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
368  DataTpl<Scalar, Options, JointCollectionTpl> & data);
369 
385  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
386  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & getJacobianComFromCrba(
387  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
388  DataTpl<Scalar, Options, JointCollectionTpl> & data);
389 
390 } // namespace pinocchio
391 
392 /* --- Details -------------------------------------------------------------------- */
393 /* --- Details -------------------------------------------------------------------- */
394 /* --- Details -------------------------------------------------------------------- */
395 #include "pinocchio/algorithm/center-of-mass.hxx"
396 
397 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
398  #include "pinocchio/algorithm/center-of-mass.txx"
399 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
400 
401 #endif // ifndef __pinocchio_algorithm_center_of_mass_hpp__
pinocchio::KinematicLevel
KinematicLevel
List of Kinematics Level supported by Pinocchio.
Definition: fwd.hpp:60
pinocchio::ACCELERATION
@ ACCELERATION
Definition: fwd.hpp:65
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
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::computeTotalMass
Scalar computeTotalMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Compute the total mass of the model and return it.
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...
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::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::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::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::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::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:94