[Doc] Add documentation in center of mass algorithm

src/algorithm/center-of-mass.hpp

@@ -25,31 +25,85 @@
  
 namespace se3
 {
+  
+  ///
+  /// \brief Computes the center of mass position of a given model according to a particular joint configuration.
+  ///        The result is accessible throw data.com[0] for the full body com and data.com[i] for the subtree supported by joint i (expressed in the joint i frame).
+  ///
+  /// \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] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
+  ///
+  /// \return The center of mass position of the rigid body system expressed in the world frame (vector 3).
+  ///
   const Eigen::Vector3d &
-  centerOfMass (const Model & model, Data& data,
-                const Eigen::VectorXd & q,
-                const bool & computeSubtreeComs = true);
+  centerOfMass(const Model & model, Data& data,
+               const Eigen::VectorXd & q,
+               const bool & computeSubtreeComs = true);
   
+  ///
+  /// \brief Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
+  ///        The result is accessible throw data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation.
+  ///        And data.com[i], data.vcom[i] and data.acom[i] for the subtree supported by joint i (expressed in the joint i frame).
+  ///
+  /// \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] v The joint velocity vector (dim model.nv).
+  /// \param[in] a The joint acceleration vector (dim model.nv).
+  /// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
+  ///
   void centerOfMassAcceleration(const Model & model, Data & data,
                                 const Eigen::VectorXd & q,
                                 const Eigen::VectorXd & v,
                                 const Eigen::VectorXd & a,
                                 const bool & computeSubtreeComs = true);
-  /* The Jcom algorithm also compute the center of mass, using a less efficient
-   * approach.  The com can be accessed afterward using data.com[0]. */
-  const  Eigen::Matrix<double,3,Eigen::Dynamic> &
-  jacobianCenterOfMass(const Model & model, Data& data,
-		       const Eigen::VectorXd & q,
-		       const bool & computeSubtreeComs = true);
+  
+  ///
+  /// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
+  ///        The results are accessible throw data.Jcom and data.com[0] and are both expressed in the world frame.
+  ///        And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the joint i frame).
+  ///
+  /// \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] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
+  ///
+  /// \return The jacobian of center of mass position of the rigid body system expressed in the world frame (matrix 3 x model.nv).
+  ///
+  const Eigen::Matrix<double,3,Eigen::Dynamic> &
+  jacobianCenterOfMass(const Model & model, Data & data,
+                       const Eigen::VectorXd & q,
+                       const bool & computeSubtreeComs = true);
 
   /* 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,
    * the COM subtrees (also easily available from CRBA data) are not
    * explicitely set. Use data.Ycrb[i].lever() to get them. */
+  ///
+  /// \brief Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix).
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  ///
+  /// \return The center of mass position of the rigid body system expressed in the world frame (vector 3).
+  ///
   const Eigen::Vector3d & 
-  getComFromCrba        (const Model & model, Data& data);
-  const  Eigen::Matrix<double,3,Eigen::Dynamic> &
-  getJacobianComFromCrba(const Model & model, Data& data);
+  getComFromCrba(const Model & model, Data & data);
+  
+  ///
+  /// \brief Extracts both the jacobian of the center of mass (CoM) and the CoM position from the joint space inertia matrix (also called the mass matrix).
+  ///        The results are accessible throw data.Jcom and data.com[0] and are both expressed in the world frame.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  ///
+  /// \return The jacobian of the CoM expressed in the world frame (matrix 3 x model.nv).
+  ///
+  const Eigen::Matrix<double,3,Eigen::Dynamic> &
+  getJacobianComFromCrba(const Model & model, Data & data);
+  
 } // namespace se3 
 
 /* --- DETAILS -------------------------------------------------------------------- */
@@ -307,8 +361,6 @@ namespace se3
     return data.Jcom;
   }
 
-
-
 } // namespace se3
 
 #endif // ifndef __se3_center_of_mass_hpp__