pinocchio  2.7.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  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  const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
139  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
140  DataTpl<Scalar,Options,JointCollectionTpl> & data,
141  KinematicLevel kinematic_level,
142  const bool computeSubtreeComs = true);
143 
144  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
145  PINOCCHIO_DEPRECATED
146  inline void centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
147  DataTpl<Scalar,Options,JointCollectionTpl> & data,
148  int kinematic_level,
149  const bool computeSubtreeComs = true)
150  {
151  centerOfMass(model,data,static_cast<KinematicLevel>(kinematic_level),computeSubtreeComs);
152  }
153 
165  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
166  const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
169  const bool computeSubtreeComs = true)
170  { return centerOfMass(model,data,ACCELERATION,computeSubtreeComs); }
171 
187  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
189  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
190  DataTpl<Scalar,Options,JointCollectionTpl> & data,
191  const Eigen::MatrixBase<ConfigVectorType> & q,
192  const bool computeSubtreeComs = true);
193 
209  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
211  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
212  DataTpl<Scalar,Options,JointCollectionTpl> & data,
213  const bool computeSubtreeComs = true);
214 
229  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix3xLike>
230  inline void
231  jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
232  DataTpl<Scalar,Options,JointCollectionTpl> & data,
233  const Eigen::MatrixBase<ConfigVectorType> & q,
234  const JointIndex & rootSubtreeId,
235  const Eigen::MatrixBase<Matrix3xLike> & res);
236 
249  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
250  inline void
251  jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
252  DataTpl<Scalar,Options,JointCollectionTpl> & data,
253  const JointIndex & rootSubtreeId,
254  const Eigen::MatrixBase<Matrix3xLike> & res);
255 
268  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
269  inline void
270  getJacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
271  const DataTpl<Scalar,Options,JointCollectionTpl> & data,
272  const JointIndex & rootSubtreeId,
273  const Eigen::MatrixBase<Matrix3xLike> & res);
274 
275 
276  /* If the CRBA has been run, then both COM and Jcom are easily available from
277  * the joint space mass matrix (data.M).
278  * Use the following function to infer them directly. In that case,
279  * the COM subtrees (also easily available from CRBA data) are not
280  * explicitely set. Use data.Ycrb[i].lever() to get them. */
292  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
293  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
294  getComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
295  DataTpl<Scalar,Options,JointCollectionTpl> & data);
296 
310  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
312  getJacobianComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
313  DataTpl<Scalar,Options,JointCollectionTpl> & data);
314 
315 } // namespace pinocchio
316 
317 /* --- Details -------------------------------------------------------------------- */
318 /* --- Details -------------------------------------------------------------------- */
319 /* --- Details -------------------------------------------------------------------- */
320 #include "pinocchio/algorithm/center-of-mass.hxx"
321 
322 #endif // ifndef __pinocchio_algorithm_center_of_mass_hpp__
pinocchio::KinematicLevel
KinematicLevel
List of Kinematics Level supported by Pinocchio.
Definition: fwd.hpp:54
pinocchio::DataTpl
Definition: data.hpp:29
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::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::computeTotalMass
Scalar computeTotalMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Compute the total mass of the model and return it.
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:75
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::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::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...
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl >
pinocchio::ACCELERATION
@ ACCELERATION
Refers to the quantities related to the 2nd-order kinematics (joint accelerations,...
Definition: fwd.hpp:58
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11