algo/center-of-mass: simplify signature of jacobianCenterOfMass

bindings/python/algorithm/expose-com.cpp

@@ -12,7 +12,7 @@ namespace pinocchio
   namespace python
   {
     
-    BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMass_overload,jacobianCenterOfMass,3,5)
+    BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMass_overload,jacobianCenterOfMass,3,4)
     
     static SE3::Vector3
     com_0_proxy(const Model& model,
@@ -65,11 +65,11 @@ namespace pinocchio
               "Computes the center of mass position, velocity and acceleration by storing the result in Data"
               "and returns the center of mass position of the full model expressed in the world frame.");
       
-      bp::def("jacobianCenterOfMass",&jacobianCenterOfMass<double,0,JointCollectionDefaultTpl,VectorXd>,
+      bp::def("jacobianCenterOfMass",
+              (const Data::Matrix3x & (*)(const Model &, Data &, const Eigen::MatrixBase<Eigen::VectorXd> &, bool))&jacobianCenterOfMass<double,0,JointCollectionDefaultTpl,VectorXd>,
               jacobianCenterOfMass_overload(bp::args("Model","Data",
                        "Joint configuration q (size Model::nq)",
-                       "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees",
-                       "updateKinematics If true, the algorithm updates first the geometry of the tree. Otherwise, it uses the current kinematics stored in data."),
+                       "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"),
               "Computes the jacobian of the center of mass, puts the result in Data and return it.")[
               bp::return_value_policy<bp::return_by_value>()]);
       

bindings/python/scripts/deprecated.py

 #
-# Copyright (c) 2018 CNRS
+# Copyright (c) 2018 CNRS INRIA
 #
 
 ## In this file, are reported some deprecated functions that are still maintained until the next important future releases ##

src/algorithm/center-of-mass.hpp

@@ -125,24 +125,70 @@ namespace pinocchio
   ///        The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
   ///        And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).
   ///
+  /// \deprecated This function signature is now deprecated. The argument updateKinematics was redundant with the input argument q.
+  ///
   /// \tparam JointCollection Collection of Joint types.
   /// \tparam ConfigVectorType Type of the joint configuration vector.
   ///
   /// \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.
+  /// \param[in] computeSubtreeComs If true, the algorithm also computes the center of mass of the subtrees.
   /// \param[in] updateKinematics If true, the algorithm updates first the geometry of the tree. Otherwise, it uses the current kinematics stored in data.
   ///
   /// \return The jacobian of center of mass position of the rigid body system expressed in the world frame (matrix 3 x model.nv).
   ///
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
+  PINOCCHIO_DEPRECATED
   inline const typename 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,
-                       const bool updateKinematics = true);
+                       const bool computeSubtreeComs,
+                       const bool updateKinematics);
+  
+  ///
+  /// \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 through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
+  ///        And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).
+  ///
+  /// \tparam JointCollection Collection of Joint types.
+  /// \tparam ConfigVectorType Type of the joint configuration vector.
+  ///
+  /// \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 also computes 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).
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
+  inline const typename 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);
+  
+  ///
+  /// \brief Computes both the jacobian and the the center of mass position of a given model according to the current value stored in data.
+  ///        It assumes that forwardKinematics has been called first.
+  ///        The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
+  ///        And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).
+  ///
+  /// \tparam JointCollection Collection of Joint types.
+  /// \tparam ConfigVectorType Type of the joint configuration vector.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] computeSubtreeComs If true, the algorithm also computes 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).
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
+  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                       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,

src/algorithm/center-of-mass.hxx

@@ -232,12 +232,36 @@ namespace pinocchio
   };
 
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
+  PINOCCHIO_DEPRECATED
   inline const typename 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,
                        const bool updateKinematics)
+  {
+    if(updateKinematics)
+      return jacobianCenterOfMass(model,data,q,computeSubtreeComs);
+    else
+      return jacobianCenterOfMass(model,data,computeSubtreeComs);
+  }
+  
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
+  inline const typename 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)
+  {
+    forwardKinematics(model, data, q);
+    return jacobianCenterOfMass(model, data, computeSubtreeComs);
+  }
+    
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
+  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                       const bool computeSubtreeComs)
   {
     assert(model.check(data) && "data is not consistent with model.");
     
@@ -251,10 +275,6 @@ namespace pinocchio
     
     data.com[0].setZero();
     data.mass[0] = Scalar(0);
-    
-    // Forward step
-    if(updateKinematics)
-      forwardKinematics(model, data, q);
       
     for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
     {