Commit c810fc0e authored by Matthieu Vigne's avatar Matthieu Vigne
Browse files

Minor fixups after review.

parent 2ac1fd28
......@@ -231,16 +231,16 @@ namespace pinocchio
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] parentJointIdIn Index of the parent joint supporting the subtree.
/// \param[in] rootSubtreeId Index of the parent joint supporting the subtree.
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 3 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix3xLike>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut);
const JointIndex & rootSubtreeId,
const Eigen::MatrixBase<Matrix3xLike> & JacobianOut);
///
/// \brief Computes the jacobian of the center of mass of the given subtree according to the current value stored in data.
......@@ -251,15 +251,15 @@ namespace pinocchio
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] parentJointIdIn Index of the parent joint supporting the subtree.
/// \param[in] rootSubtreeId Index of the parent joint supporting the subtree.
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 3 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut);
const JointIndex & rootSubtreeId,
const Eigen::MatrixBase<Matrix3xLike> & JacobianOut);
/* If the CRBA has been run, then both COM and Jcom are easily available from
* the mass matrix. Use the following methods to access them. In that case,
......
......@@ -339,48 +339,51 @@ namespace pinocchio
return data.Jcom;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix3xLike>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut)
const JointIndex & rootSubtreeId,
const Eigen::MatrixBase<Matrix3xLike> & JacobianOut)
{
forwardKinematics(model, data, q);
jacobianSubtreeCenterOfMass(model, data, parentJointIdIn, JacobianOut);
jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, JacobianOut);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
inline void
jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const JointIndex & parentJointIdIn,
typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & JacobianOut)
const JointIndex & rootSubtreeId,
const Eigen::MatrixBase<Matrix3xLike> & JacobianOut)
{
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x Matrix6x;
assert(model.check(data) && "data is not consistent with model.");
assert((parentJointIdIn < model.njoints) && "Invalid joint id.");
assert((rootSubtreeId < model.njoints) && "Invalid joint id.");
Eigen::MatrixBase<Matrix3xLike> & Jacobian = const_cast<Eigen::MatrixBase<Matrix3xLike>& >(JacobianOut);
if (parentJointIdIn == 0)
if (rootSubtreeId == 0)
{
// Joint 0 is universe: this means that the user is asking for the entier model CoM jacobian.
JacobianOut = jacobianCenterOfMass(model, data);
Jacobian = jacobianCenterOfMass(model, data);
}
else
{
// Iterate over all bodies of subtree.
for (JointIndex id : model.subtrees[parentJointIdIn])
for (int i = 0; i < model.subtrees[rootSubtreeId].size(); i++)
{
JointIndex id = model.subtrees[rootSubtreeId][i];
// Get joint jacobian in world.
Matrix6x jointJacobian = Matrix6x::Zero(6, model.nv);
getJointJacobian(model, data, id, ReferenceFrame::LOCAL, jointJacobian);
// Add jacobian of selected body com.
JacobianOut += model.inertias[id].mass() * data.oMi[id].rotation() * (
Jacobian += model.inertias[id].mass() * data.oMi[id].rotation() * (
jointJacobian.template topRows<3>() - skew(model.inertias[id].lever()) * jointJacobian.template bottomRows<3>());
}
JacobianOut /= data.mass[parentJointIdIn];
Jacobian /= data.mass[rootSubtreeId];
}
}
......
Markdown is supported
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