pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
11namespace 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::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::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::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::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::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::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::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:94