5 #ifndef __pinocchio_algorithm_center_of_mass_hpp__
6 #define __pinocchio_algorithm_center_of_mass_hpp__
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
21 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
35 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
50 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
76 template<
typename,
int>
77 class JointCollectionTpl,
78 typename ConfigVectorType>
79 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 &
centerOfMass(
82 const Eigen::MatrixBase<ConfigVectorType> & q,
83 const bool computeSubtreeComs =
true);
109 template<
typename,
int>
110 class JointCollectionTpl,
111 typename ConfigVectorType,
112 typename TangentVectorType>
113 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 &
centerOfMass(
116 const Eigen::MatrixBase<ConfigVectorType> & q,
117 const Eigen::MatrixBase<TangentVectorType> & v,
118 const bool computeSubtreeComs =
true);
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(
154 const Eigen::MatrixBase<ConfigVectorType> & q,
155 const Eigen::MatrixBase<TangentVectorType1> & v,
156 const Eigen::MatrixBase<TangentVectorType2> & a,
157 const bool computeSubtreeComs =
true);
175 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
176 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 &
centerOfMass(
180 const bool computeSubtreeComs =
true);
196 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
197 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 &
centerOfMass(
200 const bool computeSubtreeComs =
true)
228 template<
typename,
int>
229 class JointCollectionTpl,
230 typename ConfigVectorType>
234 const Eigen::MatrixBase<ConfigVectorType> & q,
235 const bool computeSubtreeComs =
true);
257 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
261 const bool computeSubtreeComs =
true);
283 template<
typename,
int>
284 class JointCollectionTpl,
285 typename ConfigVectorType,
286 typename Matrix3xLike>
290 const Eigen::MatrixBase<ConfigVectorType> & q,
291 const JointIndex & rootSubtreeId,
292 const Eigen::MatrixBase<Matrix3xLike> & res);
311 template<
typename,
int>
312 class JointCollectionTpl,
313 typename Matrix3xLike>
317 const JointIndex & rootSubtreeId,
318 const Eigen::MatrixBase<Matrix3xLike> & res);
338 template<
typename,
int>
339 class JointCollectionTpl,
340 typename Matrix3xLike>
344 const JointIndex & rootSubtreeId,
345 const Eigen::MatrixBase<Matrix3xLike> & res);
365 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
366 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 &
getComFromCrba(
385 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
395 #include "pinocchio/algorithm/center-of-mass.hxx"
397 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
398 #include "pinocchio/algorithm/center-of-mass.txx"
KinematicLevel
List of Kinematics Level supported by Pinocchio.
Main pinocchio namespace.
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...
Scalar computeTotalMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Compute the total mass of the model and return it.
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...
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...
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....
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...
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...
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...
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)