center-of-mass.hpp
Go to the documentation of this file.
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::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::DataTpl
Definition: context/generic.hpp:25
pinocchio::ACCELERATION
@ ACCELERATION
Definition: multibody/fwd.hpp:65
pinocchio::Options
Options
Definition: joint-configuration.hpp:1116
pinocchio::KinematicLevel
KinematicLevel
List of Kinematics Level supported by Pinocchio.
Definition: multibody/fwd.hpp:59
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
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: multibody/data.hpp:94
pinocchio::DataTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: multibody/data.hpp:77
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::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...
data.hpp
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::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
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...
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
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
Definition: context/generic.hpp:20
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
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
Author(s):
autogenerated on Sat Jun 22 2024 02:41:43