Minor fixups after review.

src/algorithm/center-of-mass.hpp

@@ -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,

src/algorithm/center-of-mass.hxx

@@ -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];
     }
   }