pinocchio  3.3.1
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> class JointCollectionTpl,
77  typename ConfigVectorType>
78  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
79  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
80  DataTpl<Scalar, Options, JointCollectionTpl> & data,
81  const Eigen::MatrixBase<ConfigVectorType> & q,
82  const bool computeSubtreeComs = true);
83 
105  template<
106  typename Scalar,
107  int Options,
108  template<typename, int> class JointCollectionTpl,
109  typename ConfigVectorType,
110  typename TangentVectorType>
111  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
112  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
113  DataTpl<Scalar, Options, JointCollectionTpl> & data,
114  const Eigen::MatrixBase<ConfigVectorType> & q,
115  const Eigen::MatrixBase<TangentVectorType> & v,
116  const bool computeSubtreeComs = true);
117 
141  template<
142  typename Scalar,
143  int Options,
144  template<typename, int> class JointCollectionTpl,
145  typename ConfigVectorType,
146  typename TangentVectorType1,
147  typename TangentVectorType2>
148  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
149  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
150  DataTpl<Scalar, Options, JointCollectionTpl> & data,
151  const Eigen::MatrixBase<ConfigVectorType> & q,
152  const Eigen::MatrixBase<TangentVectorType1> & v,
153  const Eigen::MatrixBase<TangentVectorType2> & a,
154  const bool computeSubtreeComs = true);
155 
172  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
173  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
174  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
175  DataTpl<Scalar, Options, JointCollectionTpl> & data,
176  KinematicLevel kinematic_level,
177  const bool computeSubtreeComs = true);
178 
193  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
194  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
195  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
196  DataTpl<Scalar, Options, JointCollectionTpl> & data,
197  const bool computeSubtreeComs = true)
198  {
199  return centerOfMass(model, data, ACCELERATION, computeSubtreeComs);
200  }
201 
222  template<
223  typename Scalar,
224  int Options,
225  template<typename, int> class JointCollectionTpl,
226  typename ConfigVectorType>
227  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
228  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
229  DataTpl<Scalar, Options, JointCollectionTpl> & data,
230  const Eigen::MatrixBase<ConfigVectorType> & q,
231  const bool computeSubtreeComs = true);
232 
253  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
254  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
255  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
256  DataTpl<Scalar, Options, JointCollectionTpl> & data,
257  const bool computeSubtreeComs = true);
258 
276  template<
277  typename Scalar,
278  int Options,
279  template<typename, int> class JointCollectionTpl,
280  typename ConfigVectorType,
281  typename Matrix3xLike>
282  void jacobianSubtreeCenterOfMass(
283  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
284  DataTpl<Scalar, Options, JointCollectionTpl> & data,
285  const Eigen::MatrixBase<ConfigVectorType> & q,
286  const JointIndex & rootSubtreeId,
287  const Eigen::MatrixBase<Matrix3xLike> & res);
288 
303  template<
304  typename Scalar,
305  int Options,
306  template<typename, int> class JointCollectionTpl,
307  typename Matrix3xLike>
308  void jacobianSubtreeCenterOfMass(
309  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
310  DataTpl<Scalar, Options, JointCollectionTpl> & data,
311  const JointIndex & rootSubtreeId,
312  const Eigen::MatrixBase<Matrix3xLike> & res);
313 
329  template<
330  typename Scalar,
331  int Options,
332  template<typename, int> class JointCollectionTpl,
333  typename Matrix3xLike>
334  void getJacobianSubtreeCenterOfMass(
335  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
336  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
337  const JointIndex & rootSubtreeId,
338  const Eigen::MatrixBase<Matrix3xLike> & res);
339 
340  /* If the CRBA has been run, then both COM and Jcom are easily available from
341  * the joint space mass matrix (data.M).
342  * Use the following function to infer them directly. In that case,
343  * the COM subtrees (also easily available from CRBA data) are not
344  * explicitely set. Use data.Ycrb[i].lever() to get them. */
358  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
359  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & getComFromCrba(
360  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
361  DataTpl<Scalar, Options, JointCollectionTpl> & data);
362 
378  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
379  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & getJacobianComFromCrba(
380  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
381  DataTpl<Scalar, Options, JointCollectionTpl> & data);
382 
383 } // namespace pinocchio
384 
385 /* --- Details -------------------------------------------------------------------- */
386 /* --- Details -------------------------------------------------------------------- */
387 /* --- Details -------------------------------------------------------------------- */
388 #include "pinocchio/algorithm/center-of-mass.hxx"
389 
390 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
391  #include "pinocchio/algorithm/center-of-mass.txx"
392 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
393 
394 #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