[Algo] Add additional signature for computeJacobians

bindings/python/algorithm/expose-jacobian.cpp

 //
-// Copyright (c) 2015-2017 CNRS
+// Copyright (c) 2015-2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -73,10 +73,17 @@ namespace se3
   
     void exposeJacobian()
     {
-      bp::def("computeJacobians",computeJacobians,
+      bp::def("computeJacobians",(const Data::Matrix6x &(*)(const Model &, Data &, const Eigen::VectorXd &))&computeJacobians,
               bp::args("Model","Data",
                        "Joint configuration q (size Model::nq)"),
-              "Calling computeJacobians",
+              "Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
+              "The result is accessible through data.J. This function computes also the forwardKinematics of the model.",
+              bp::return_value_policy<bp::return_by_value>());
+      
+      bp::def("computeJacobians",(const Data::Matrix6x &(*)(const Model &, Data &))&computeJacobians,
+              bp::args("Model","Data"),
+              "Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
+              "The result is accessible through data.J. This function assumes that forwardKinematics has been called before",
               bp::return_value_policy<bp::return_by_value>());
       
       bp::def("jacobian",jacobian_proxy,

src/algorithm/jacobian.hpp

@@ -41,6 +41,22 @@ namespace se3
                    Data & data,
                    const Eigen::VectorXd & q);
   
+  ///
+  /// \brief Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.
+  ///        The result is accessible through data.J. This function assumes that se3::forwardKinematics has been called before.
+  ///
+  /// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame. \sa se3::getJacobian for doing this specific extraction.
+  ///
+  /// \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).
+  ///
+  /// \return The full model Jacobian (matrix 6 x model.nv).
+  ///
+  inline const Data::Matrix6x &
+  computeJacobians(const Model & model,
+                   Data & data);
+  
   ///
   /// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or in the local frame (rf = LOCAL) of the joint.
   /// \note This jacobian is extracted from data.J. You have to run se3::computeJacobians before calling it.

src/algorithm/jacobian.hxx

 //
-// Copyright (c) 2015-2017 CNRS
+// Copyright (c) 2015-2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -55,7 +55,6 @@ namespace se3
   
   };
   
-  
   inline const Data::Matrix6x &
   computeJacobians(const Model & model, Data & data,
                    const Eigen::VectorXd & q)
@@ -71,6 +70,38 @@ namespace se3
     return data.J;
   }
   
+  struct JacobiansForwardStep2 : public fusion::JointVisitor<JacobiansForwardStep2>
+  {
+    typedef boost::fusion::vector<se3::Data &> ArgsType;
+    
+    JOINT_VISITOR_INIT(JacobiansForwardStep2);
+    
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                     se3::Data & data)
+    {
+      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+
+      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
+    }
+    
+  };
+  
+  inline const Data::Matrix6x &
+  computeJacobians(const Model & model, Data & data)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    
+    for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i )
+    {
+      JacobiansForwardStep2::run(model.joints[i],data.joints[i],
+                                 JacobiansForwardStep2::ArgsType(data));
+    }
+    
+    return data.J;
+  }
+  
   /* Return the jacobian of the output frame attached to joint <jointId> in the
    world frame or in the local frame depending on the template argument. The
    function computeJacobians should have been called first. */

unittest/jacobian.cpp

 //
-// Copyright (c) 2015-2017 CNRS
+// Copyright (c) 2015-2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -71,6 +71,13 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
 
   jacobian(model,data,q,idx,XJrh);
   BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
+  
+  /* Test computeJacobians with pre-computation of the forward kinematics */
+  Data data_fk(model);
+  forwardKinematics(model, data_fk, q);
+  computeJacobians(model, data_fk);
+  
+  BOOST_CHECK(data_fk.J.isApprox(data.J));
 
 }