$darkmode
pinocchio 4.0.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 #pragma once
5 
6 // IWYU pragma: begin_keep
7 #include <Eigen/Core>
8 
9 #include <cassert>
10 #include <cstddef>
11 #include <vector>
12 
13 #include <boost/fusion/container/vector.hpp>
14 
15 #include "pinocchio/macros.hpp"
16 #include "pinocchio/eigen-common.hpp"
17 
18 #include "pinocchio/src/utils/check.hxx"
19 
20 #include "pinocchio/math.hpp"
21 
22 #include "pinocchio/spatial.hpp"
23 
24 #include "pinocchio/multibody.hpp"
25 #include "pinocchio/multibody/joint.hpp"
26 #include "pinocchio/multibody/visitor.hpp"
27 
28 #include "pinocchio/algorithm/check.hpp"
29 #include "pinocchio/algorithm/kinematics.hpp"
30 
31 // IWYU pragma: end_keep
32 
33 namespace pinocchio
34 {
35 
43  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
44  inline Scalar computeTotalMass(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
45 
57  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
58  Scalar computeTotalMass(
59  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
60  DataTpl<Scalar, Options, JointCollectionTpl> & data);
61 
72  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
73  void computeSubtreeMasses(
74  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
75  DataTpl<Scalar, Options, JointCollectionTpl> & data);
76 
95  template<
96  typename Scalar,
97  int Options,
98  template<typename, int> class JointCollectionTpl,
99  typename ConfigVectorType>
100  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
101  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
102  DataTpl<Scalar, Options, JointCollectionTpl> & data,
103  const Eigen::MatrixBase<ConfigVectorType> & q,
104  const bool computeSubtreeComs = true);
105 
127  template<
128  typename Scalar,
129  int Options,
130  template<typename, int> class JointCollectionTpl,
131  typename ConfigVectorType,
132  typename TangentVectorType>
133  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
134  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
135  DataTpl<Scalar, Options, JointCollectionTpl> & data,
136  const Eigen::MatrixBase<ConfigVectorType> & q,
137  const Eigen::MatrixBase<TangentVectorType> & v,
138  const bool computeSubtreeComs = true);
139 
163  template<
164  typename Scalar,
165  int Options,
166  template<typename, int> class JointCollectionTpl,
167  typename ConfigVectorType,
168  typename TangentVectorType1,
169  typename TangentVectorType2>
170  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
171  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
172  DataTpl<Scalar, Options, JointCollectionTpl> & data,
173  const Eigen::MatrixBase<ConfigVectorType> & q,
174  const Eigen::MatrixBase<TangentVectorType1> & v,
175  const Eigen::MatrixBase<TangentVectorType2> & a,
176  const bool computeSubtreeComs = true);
177 
194  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
195  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
196  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
197  DataTpl<Scalar, Options, JointCollectionTpl> & data,
198  KinematicLevel kinematic_level,
199  const bool computeSubtreeComs = true);
200 
215  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
216  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
217  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
218  DataTpl<Scalar, Options, JointCollectionTpl> & data,
219  const bool computeSubtreeComs = true)
220  {
221  return centerOfMass(model, data, ACCELERATION, computeSubtreeComs);
222  }
223 
244  template<
245  typename Scalar,
246  int Options,
247  template<typename, int> class JointCollectionTpl,
248  typename ConfigVectorType>
249  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
250  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
251  DataTpl<Scalar, Options, JointCollectionTpl> & data,
252  const Eigen::MatrixBase<ConfigVectorType> & q,
253  const bool computeSubtreeComs = true);
254 
275  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
276  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
277  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
278  DataTpl<Scalar, Options, JointCollectionTpl> & data,
279  const bool computeSubtreeComs = true);
280 
298  template<
299  typename Scalar,
300  int Options,
301  template<typename, int> class JointCollectionTpl,
302  typename ConfigVectorType,
303  typename Matrix3xLike>
304  void jacobianSubtreeCenterOfMass(
305  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
306  DataTpl<Scalar, Options, JointCollectionTpl> & data,
307  const Eigen::MatrixBase<ConfigVectorType> & q,
308  const JointIndex & rootSubtreeId,
309  const Eigen::MatrixBase<Matrix3xLike> & res);
310 
325  template<
326  typename Scalar,
327  int Options,
328  template<typename, int> class JointCollectionTpl,
329  typename Matrix3xLike>
330  void jacobianSubtreeCenterOfMass(
331  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
332  DataTpl<Scalar, Options, JointCollectionTpl> & data,
333  const JointIndex & rootSubtreeId,
334  const Eigen::MatrixBase<Matrix3xLike> & res);
335 
351  template<
352  typename Scalar,
353  int Options,
354  template<typename, int> class JointCollectionTpl,
355  typename Matrix3xLike>
356  void getJacobianSubtreeCenterOfMass(
357  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
358  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
359  const JointIndex & rootSubtreeId,
360  const Eigen::MatrixBase<Matrix3xLike> & res);
361 
362  /* If the CRBA has been run, then both COM and Jcom are easily available from
363  * the joint space mass matrix (data.M).
364  * Use the following function to infer them directly. In that case,
365  * the COM subtrees (also easily available from CRBA data) are not
366  * explicitely set. Use data.Ycrb[i].lever() to get them. */
380  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
381  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & getComFromCrba(
382  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
383  DataTpl<Scalar, Options, JointCollectionTpl> & data);
384 
400  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
401  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & getJacobianComFromCrba(
402  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
403  DataTpl<Scalar, Options, JointCollectionTpl> & data);
404 
405 } // namespace pinocchio
406 
407 // IWYU pragma: begin_exports
408 #include "pinocchio/src/algorithm/center-of-mass.hxx"
409 // IWYU pragma: end_exports
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...