Unverified Commit 100258ad authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1315 from proyan/devel

[algorithm/centroidal] computeCentroidalMomentum calculates vcom as well
parents 17c479e2 71217b6e
Pipeline #11659 passed with stage
in 152 minutes and 6 seconds
......@@ -23,7 +23,7 @@ namespace pinocchio
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
///
/// \returns The centroidal momenta (stored in data.hg).
/// \returns The centroidal momenta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
......@@ -45,7 +45,7 @@ namespace pinocchio
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] v The joint velocity vector (dim model.nv).
///
/// \returns The centroidal momenta (stored in data.hg).
/// \returns The centroidal momenta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
typename ConfigVectorType, typename TangentVectorType>
......@@ -86,7 +86,8 @@ namespace pinocchio
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
///
/// \returns The centroidal momenta time derivative (stored in data.dhg). The centroidal momemta is stored in data.hg.
/// \returns The centroidal momenta time derivative (stored in data.dhg), centroidal momemta (stored in data.hg),
/// center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
......@@ -110,7 +111,8 @@ namespace pinocchio
/// \param[in] v The joint velocity vector (dim model.nv).
/// \param[in] a The joint acceleration vector (dim model.nv).
///
/// \returns The centroidal momenta time derivative (stored in data.dhg). The centroidal momemta is stored in data.hg.
/// \returns The centroidal momenta time derivative (stored in data.dhg), centroidal momemta (stored in data.hg),
/// center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
......
......@@ -51,6 +51,8 @@ namespace pinocchio
data.hg = data.h[0];
data.hg.angular() += data.hg.linear().cross(data.com[0]);
data.vcom[0].noalias() = data.hg.linear()/data.mass[0];
return data.hg;
}
......@@ -93,6 +95,7 @@ namespace pinocchio
}
data.com[0] /= data.mass[0];
data.hg = data.h[0];
data.hg.angular() += data.hg.linear().cross(data.com[0]);
......@@ -100,6 +103,8 @@ namespace pinocchio
data.dhg = data.f[0];
data.dhg.angular() += data.dhg.linear().cross(data.com[0]);
data.vcom[0].noalias() = data.hg.linear()/data.mass[0];
return data.dhg;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment