[Algo] Remove overloading of getFrameJacobians

It now only returns the Jacobian of the FRAME expressed in the local
coordinate frame of the FRAME itself.

bindings/python/algorithm/expose-frames.cpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -25,26 +25,25 @@ namespace se3
     
     static Data::Matrix6x frame_jacobian_proxy(const Model & model,
                                                Data & data,
-                                               const Eigen::VectorXd & q,
-                                               const Model::FrameIndex frame_id,
-                                               bool local,
-                                               bool update_geometry
-                                               )
+                                               const Model::FrameIndex frame_id)
     {
       Data::Matrix6x J(6,model.nv); J.setZero();
-      
-      if (update_geometry)
-      {
-        computeJacobians(model,data,q);
-        framesForwardKinematics(model,data);
-      }
-      
-      if(local) getFrameJacobian<true> (model, data, frame_id, J);
-      else getFrameJacobian<false> (model, data, frame_id, J);
+      getFrameJacobian(model, data, frame_id, J);
       
       return J;
     }
     
+    static Data::Matrix6x frame_jacobian_proxy(const Model & model,
+                                               Data & data,
+                                               const Model::FrameIndex frame_id,
+                                               const Eigen::VectorXd & q)
+    {
+      computeJacobians(model,data,q);
+      framesForwardKinematics(model,data);
+  
+      return frame_jacobian_proxy(model, data, frame_id);
+    }
+    
     void exposeFramesAlgo()
     {
       bp::def("framesKinematics",
@@ -61,16 +60,25 @@ namespace se3
               "And computes the placements of all the operational frames"
               "and put the results in data.");
       
-      bp::def("frameJacobian",frame_jacobian_proxy,
+      bp::def("frameJacobian",
+              (Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, const Eigen::VectorXd &))&frame_jacobian_proxy,
               bp::args("Model","Data",
-                       "Configuration q (size Model::nq)",
                        "Operational frame ID (int)",
-                       "frame (true = local, false = world)",
-                       "update_geometry (true = recompute the kinematics)"),
-              "Call computeJacobians if update_geometry is true. If not, user should call computeJacobians first."
-              "Then call getJacobian and return the resulted jacobian matrix. Attention: if update_geometry is true, the "
-              "function computes all the jacobians of the model. It is therefore outrageously costly wrt a dedicated "
-              "call. Use only with update_geometry for prototyping.");
+                       "Configuration q (size Model::nq)"),
+              "Compute the Jacobian of the frame given by its ID."
+              "The columns of the Jacobian are expressed in the frame coordinates.\n"
+              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
+              "where v is the time derivative of the configuration q.");
+      
+      bp::def("frameJacobian",
+              (Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex))&frame_jacobian_proxy,
+              bp::args("Model","Data",
+                       "Operational frame ID (int)"),
+              "Compute the Jacobian of the frame given by its ID."
+              "The columns of the Jacobian are expressed in the frame coordinates.\n"
+              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
+              "where v is the time derivative of the configuration q.\n"
+              "Be aware that computeJacobians and framesKinematics must have been called first.");
       
     }
   } // namespace python

bindings/python/scripts/robot_wrapper.py

@@ -148,7 +148,6 @@ class RobotWrapper(object):
         else:
             se3.updateGeometryPlacements(self.model, self.data, self.collision_model, self.collision_data, q)
 
-
     # --- ACCESS TO NAMES ----
     # Return the index of the joint whose name is given in argument.
     def index(self, name):

src/algorithm/frames.hpp

@@ -53,22 +53,18 @@ namespace se3
                                       );
 
   /**
-   * @brief      Returns the jacobian of the frame expresssed in the world frame or
-     in the local frame depending on the template argument. 
+   * @brief      Returns the jacobian of the frame expresssed in the local frame depending on the template argument.
    *             You must first call se3::framesForwardKinematics and se3::computeJacobians to update values in data structure.
    
-   * @remark Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system. Expressed in the global frame, the jacobian maps to the spatial velocity of the point coinciding with the center of the world and attached to the frame.
+   * @remark     Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system.
    *
    * @param[in]  model       The kinematic model
    * @param[in]  data        Data associated to model
-   * @param[in]  frame_id    Id of the operational frame we want to compute the jacobian
-   * @param[out] J           The Jacobian of the
+   * @param[in]  frame_id    Id of the operational Frame
+   * @param[out] J           The Jacobian of the Frame expressed in the coordinates Frame.
    *
-   * @tparam     local_frame  If true, the jacobian is expressed in the local frame. Otherwise, the jacobian is expressed in the world frame.
-   * 
-   * @warning    The function se3::computeJacobians should have been called first
+   * @warning    The function se3::computeJacobians and se3::framesForwardKinematics should have been called first.
    */
-  template<bool local_frame>
   inline void getFrameJacobian(const Model & model,
                                const Data& data,
                                const Model::FrameIndex frame_id,
@@ -112,9 +108,6 @@ namespace se3
     framesForwardKinematics(model, data);
   }
   
-  
-  
-  template<bool local_frame>
   inline void getFrameJacobian(const Model & model,
                                const Data & data,
                                const Model::FrameIndex frame_id,
@@ -124,24 +117,15 @@ namespace se3
     assert(data.J.cols() == model.nv);
     assert(model.check(data) && "data is not consistent with model.");
     
-    const Model::JointIndex & parent = model.frames[frame_id].parent;
-    const SE3 & oMframe = data.oMf[frame_id];
     const Frame & frame = model.frames[frame_id];
+    const Model::JointIndex & parent = frame.parent;
+    const SE3 & oMframe = data.oMf[frame_id];
     
     const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
     
-    if(!local_frame)
-      getJacobian<local_frame>(model, data, parent, J);
-
-    // Lever between the joint center and the frame center expressed in the global frame
-    const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
-      
     for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
     {
-      if(!local_frame)
-        J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
-      else
-        J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+      J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
     }
   }
 

unittest/frames.cpp

@@ -66,50 +66,27 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
   se3::Data data(model);
   se3::Data data_ref(model);
   
-
   VectorXd q = VectorXd::Ones(model.nq);
   q.middleRows<4> (3).normalize();
   VectorXd q_dot = VectorXd::Ones(model.nv);
 
 
-  /// In global frame
-  Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
-  Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
+  /// In local frame
   Model::Index idx = model.getFrameId(frame_name);
   const Frame & frame = model.frames[idx];
   BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
-
-  computeJacobians(model,data,q);
-  computeJacobians(model,data_ref,q);
-  framesForwardKinematics(model,data);
-  getFrameJacobian<false>(model,data,idx,Jof);
-  getJacobian<false>(model, data_ref, parent_idx, Joj);
-
-  Motion nu_frame(Jof*q_dot);
-  Motion nu_joint(Joj*q_dot);
-  
-  SE3 translation(SE3::Identity());
-  translation.translation(data.oMi[parent_idx].rotation()*frame.placement.translation());
-  
-  Motion nu_frame_from_nu_joint(translation.actInv(nu_joint));
-
-  BOOST_CHECK(nu_frame.isApprox(nu_frame_from_nu_joint, 1e-12));
-
-  /// In local frame
-
   Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
   Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
-  getFrameJacobian<true>(model,data,idx,Jff);
+  getFrameJacobian(model,data,idx,Jff);
   getJacobian<true>(model, data_ref, parent_idx, Jjj);
 
-  nu_frame = Motion(Jff*q_dot);
-  nu_joint = Motion(Jjj*q_dot);
+  Motion nu_frame = Motion(Jff*q_dot);
+  Motion nu_joint = Motion(Jjj*q_dot);
   
   const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
   Data::Matrix6x Jjj_from_frame(jXf * Jff);
   BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
   
-  nu_frame_from_nu_joint = frame.placement.act(nu_frame);
   BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
 }