pinocchio 4.1.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#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
33namespace 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::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.