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>
22 inline Scalar
computeTotalMass(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
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);
47 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
49 DataTpl<Scalar,Options,JointCollectionTpl> & data);
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);
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);
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);
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,
142 const bool computeSubtreeComs =
true);
144 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
146 inline void centerOfMass(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
147 DataTpl<Scalar,Options,JointCollectionTpl> & data,
149 const bool computeSubtreeComs =
true)
151 centerOfMass(model,data,static_cast<KinematicLevel>(kinematic_level),computeSubtreeComs);
165 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
166 const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
169 const bool computeSubtreeComs =
true)
187 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
191 const Eigen::MatrixBase<ConfigVectorType> & q,
192 const bool computeSubtreeComs =
true);
209 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
213 const bool computeSubtreeComs =
true);
229 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename Matrix3xLike>
233 const Eigen::MatrixBase<ConfigVectorType> & q,
234 const JointIndex & rootSubtreeId,
235 const Eigen::MatrixBase<Matrix3xLike> & res);
249 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename Matrix3xLike>
253 const JointIndex & rootSubtreeId,
254 const Eigen::MatrixBase<Matrix3xLike> & res);
268 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename Matrix3xLike>
272 const JointIndex & rootSubtreeId,
273 const Eigen::MatrixBase<Matrix3xLike> & res);
292 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
293 inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
310 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
320 #include "pinocchio/algorithm/center-of-mass.hxx" 322 #endif // ifndef __pinocchio_algorithm_center_of_mass_hpp__ 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 >::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...
KinematicLevel
List of Kinematics Level supported by Pinocchio.
Refers to the quantities related to the 2nd-order kinematics (joint accelerations, center of mass acceleration, etc.).
Scalar computeTotalMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Compute the total mass of the model and return it.
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 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...
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
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...
Main pinocchio namespace.
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...
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...