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>
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>
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>
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>
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>
140 DataTpl<Scalar,Options,JointCollectionTpl> &
data,
142 const bool computeSubtreeComs =
true);
144 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
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>
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,
235 const Eigen::MatrixBase<Matrix3xLike> &
res);
249 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename Matrix3xLike>
254 const Eigen::MatrixBase<Matrix3xLike> & res);
268 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename Matrix3xLike>
273 const Eigen::MatrixBase<Matrix3xLike> & res);
292 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
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__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
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...
Refers to the quantities related to the 2nd-order kinematics (joint accelerations, center of mass acceleration, etc.).
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...
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...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
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...
KinematicLevel
List of Kinematics Level supported by Pinocchio.
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.
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
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...
JointCollectionTpl & model
Eigen::Matrix< Scalar, 3, 1, Options > Vector3