Merge pull request #508 from proyan/devel

Frame Jacobian Time Variation

bindings/python/algorithm/expose-frames.cpp

@@ -49,6 +49,36 @@ namespace se3
   
       return get_frame_jacobian_proxy(model, data, frame_id, rf);
     }
+
+
+    static Data::Matrix6x
+    get_frame_jacobian_time_variation_proxy(const Model & model,
+                                            Data & data,
+                                            Model::FrameIndex jointId,
+                                            ReferenceFrame rf)
+    {
+      Data::Matrix6x dJ(6,model.nv); dJ.setZero();
+      
+      if(rf == LOCAL) getFrameJacobianTimeVariation<LOCAL>(model,data,jointId,dJ);
+      else getFrameJacobianTimeVariation<WORLD>(model,data,jointId,dJ);
+      
+      return dJ;
+    }
+
+    static Data::Matrix6x frame_jacobian_time_variation_proxy(const Model & model,
+                                                              Data & data,
+                                                              const Eigen::VectorXd & q,
+                                                              const Eigen::VectorXd & v,
+                                                              const Model::FrameIndex frame_id,
+                                                              ReferenceFrame rf
+                                                              )
+    {
+      computeJointJacobiansTimeVariation(model,data,q,v);
+      framesForwardKinematics(model,data);
+  
+      return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
+    }        
+
     
     void exposeFramesAlgo()
     {
@@ -87,7 +117,28 @@ namespace se3
               "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 computeJointJacobians and framesKinematics must have been called first.");
-      
+
+      bp::def("frameJacobianTimeVariation",
+              (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &,const Eigen::VectorXd &, const Model::FrameIndex, ReferenceFrame))&frame_jacobian_time_variation_proxy,
+              bp::args("Model","Data",
+                       "Configuration q (size Model::nq)",
+                       "Joint velocity v (size Model::nv)",                       
+                       "Operational frame ID (int)",
+                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
+              "Computes the Jacobian Time Variation of the frame given by its ID either in the local or the world frames."
+              "The columns of the Jacobian time variation 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("getFrameJacobianTimeVariation",get_frame_jacobian_time_variation_proxy,
+              bp::args("Model, the model of the kinematic tree",
+                       "Data, the data associated to the model where the results are stored",
+                       "Frame ID, the index of the frame.",
+                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
+              "Returns the Jacobian time variation of a specific frame (specified by Frame ID) expressed either in the world or the local frame."
+              "You have to call computeJointJacobiansTimeVariation and framesKinematics first."
+              "If rf is set to LOCAL, it returns the jacobian time variation associated to the frame index. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");
+
     }
   } // namespace python
 } // namespace se3

bindings/python/algorithm/expose-jacobian.cpp

@@ -78,7 +78,7 @@ namespace se3
               "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("computeJointJacobians",(const Data::Matrix6x &(*)(const Model &, Data &))&computeJointJacobians,
               bp::args("Model","Data"),
               "Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
@@ -94,7 +94,7 @@ namespace se3
                        "update_kinematics (true = update the value of the total jacobian)"),
               "Computes the jacobian of a given given joint according to the given input configuration."
               "If rf is set to LOCAL, it returns the jacobian associated to the joint frame. Otherwise, it returns the jacobian of the frame coinciding with the world frame.");
-      
+
       bp::def("getJointJacobian",get_jacobian_proxy,
               bp::args("Model, the model of the kinematic tree",
                        "Data, the data associated to the model where the results are stored",
@@ -116,7 +116,7 @@ namespace se3
                        "Data, the data associated to the model where the results are stored",
                        "Joint ID, the index of the joint.",
                        "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
-              "Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint."
+              "Computes the Jacobian time variation of a specific joint expressed either in the world frame or in the local frame of the joint."
               "You have to call computeJointJacobiansTimeVariation first."
               "If rf is set to LOCAL, it returns the jacobian time variation associated to the joint frame. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");
     }

src/algorithm/frames.hpp

@@ -74,6 +74,7 @@ namespace se3
   /**
    * @brief      Returns the jacobian of the frame expresssed the LOCAL frame coordinate system.
    *             You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
+   \deprecated This function is now deprecated. Please call se3::getFrameJacobian<ReferenceFrame> for same functionality
    *
    * @param[in]  model       The kinematic model
    * @param[in]  data        Data associated to model
@@ -82,11 +83,30 @@ namespace se3
    *
    * @warning    The function se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
    */
-   
+  PINOCCHIO_DEPRECATED   
   inline void getFrameJacobian(const Model & model,
                                const Data & data,
                                const Model::FrameIndex frame_id,
                                Data::Matrix6x & J);
+
+  ///
+  /// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL).
+  /// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it.
+  ///
+  /// \tparam rf Reference frame in which the Jacobian is expressed.
+  ///
+  /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] frameId The index of the frame.
+  /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).
+  ///
+  template<ReferenceFrame rf>
+  void getFrameJacobianTimeVariation(const Model & model,
+                                     const Data & data,
+                                     const Model::FrameIndex frameId,
+                                     Data::Matrix6x & dJ);
+  
  
 } // namespace se3
 

src/algorithm/frames.hxx

@@ -93,6 +93,38 @@ namespace se3
     getFrameJacobian<LOCAL>(model,data,frame_id,J);
   }
 
+  template<ReferenceFrame rf>
+  void getFrameJacobianTimeVariation(const Model & model,
+                                     const Data & data,
+                                     const Model::FrameIndex frameId,
+                                     Data::Matrix6x & dJ)
+  {
+    assert( dJ.rows() == data.dJ.rows() );
+    assert( dJ.cols() == data.dJ.cols() );    
+    assert(model.check(data) && "data is not consistent with model.");
+    
+    const Frame & frame = model.frames[frameId];
+    const Model::JointIndex & joint_id = frame.parent;
+    if (rf == WORLD)
+    {
+      getJointJacobianTimeVariation<WORLD>(model,data,joint_id,dJ);
+      return;
+    }
+    
+    if (rf == LOCAL)
+    {
+      const SE3 & oMframe = data.oMf[frameId];
+      const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
+      
+      for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
+      {
+        dJ.col(j) = oMframe.actInv(Motion(data.dJ.col(j))).toVector();
+      }
+      return;
+    }    
+  }
+  
+
 } // namespace se3
 
 #endif // ifndef __se3_frames_hxx__

src/algorithm/jacobian.hpp

@@ -248,8 +248,6 @@ namespace se3
   /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL) of the joint.
   /// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it.
   ///
-  /// \deprecated This function is now deprecated. Please refer now to se3::getJointJacobianTimeVariation for similar function with updated name.
-  ///
   /// \tparam rf Reference frame in which the Jacobian is expressed.
   ///
   /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
@@ -267,6 +265,7 @@ namespace se3
   ///
   /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL) of the joint.
   /// \note This jacobian is extracted from data.dJ. You have to run se3::computeJointJacobiansTimeVariation before calling it.
+  /// \deprecated This function is now deprecated. Please refer now to se3::getJointJacobianTimeVariation for similar function with updated name.  
   ///
   /// \tparam rf Reference frame in which the Jacobian is expressed.
   ///

unittest/frames.cpp

@@ -23,12 +23,20 @@
 #include "pinocchio/spatial/act-on-set.hpp"
 #include "pinocchio/parsers/sample-models.hpp"
 #include "pinocchio/utils/timer.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
 
 #include <iostream>
 
 #include <boost/test/unit_test.hpp>
 #include <boost/utility/binary.hpp>
 
+template<typename Derived>
+inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
+{
+  return ((x - x).array() == (x - x).array()).all();
+}
+
+
 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
 
 BOOST_AUTO_TEST_CASE ( test_kinematics )
@@ -96,5 +104,111 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
   BOOST_CHECK(Jff.isApprox(Jjj));
 }
 
+BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
+{
+  using namespace Eigen;
+  using namespace se3;
+  
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
+  const SE3 & framePlacement = SE3::Random();
+  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));  
+  se3::Data data(model);
+  se3::Data data_ref(model);
+  
+  VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
+  VectorXd v = VectorXd::Random(model.nv);
+  VectorXd a = VectorXd::Random(model.nv);
+  
+  computeJointJacobiansTimeVariation(model,data,q,v);
+  forwardKinematics(model,data_ref,q,v,a);
+  framesForwardKinematics(model,data_ref);
+  framesForwardKinematics(model,data);
+
+  BOOST_CHECK(isFinite(data.dJ));
+
+  Model::Index idx = model.getFrameId(frame_name);
+  const Frame & frame = model.frames[idx];
+  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
+    
+  
+  Data::Matrix6x J(6,model.nv); J.fill(0.);
+  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
+  
+  // Regarding to the world origin
+  getFrameJacobian<WORLD>(model,data,idx,J);
+  getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ);
+  
+  Motion v_idx(J*v);
+  const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]);
+  const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local);
+  BOOST_CHECK(v_idx.isApprox(v_ref));
+  
+  Motion a_idx(J*a + dJ*v);
+  const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
+  const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local);
+  BOOST_CHECK(a_idx.isApprox(a_ref));
+  
+  J.fill(0.);  dJ.fill(0.);
+  // Regarding to the local frame
+  getFrameJacobian<LOCAL>(model,data,idx,J);
+  getFrameJacobianTimeVariation<LOCAL>(model,data,idx,dJ);
+  
+  v_idx = (Motion::Vector6)(J*v);
+  BOOST_CHECK(v_idx.isApprox(v_ref_local));
+              
+  a_idx = (Motion::Vector6)(J*a + dJ*v);
+  BOOST_CHECK(a_idx.isApprox(a_ref_local));
+  
+  // compare to finite differencies
+  {
+    Data data_ref(model), data_ref_plus(model);
+    
+    const double alpha = 1e-8;
+    Eigen::VectorXd q_plus(model.nq);
+    q_plus = integrate(model,q,alpha*v);
+
+    //data_ref
+    Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
+    J_ref_world.fill(0.);     J_ref_local.fill(0.);
+    computeJointJacobians(model,data_ref,q);
+    framesForwardKinematics(model,data_ref);
+    const SE3 & oMf_q = data_ref.oMf[idx];
+    getFrameJacobian<WORLD>(model,data_ref,idx,J_ref_world);
+    getFrameJacobian<LOCAL>(model,data_ref,idx,J_ref_local);
+    
+    //data_ref_plus
+    Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv);
+    J_ref_plus_world.fill(0.);    J_ref_plus_local.fill(0.);
+    computeJointJacobians(model,data_ref_plus,q_plus);
+    framesForwardKinematics(model,data_ref_plus);
+    const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
+    getFrameJacobian<WORLD>(model,data_ref_plus,idx,J_ref_plus_world);
+    getFrameJacobian<LOCAL>(model,data_ref_plus,idx,J_ref_plus_local);
+
+    //Move J_ref_plus_local to reference frame
+    J_ref_plus_local = (oMf_q.inverse()*oMf_qplus).toActionMatrix()*(J_ref_plus_local);
+    
+    Data::Matrix6x dJ_ref_world(6,model.nv), dJ_ref_local(6,model.nv);
+    dJ_ref_world.fill(0.);        dJ_ref_local.fill(0.);
+    dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha;
+    dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha;
+
+    //data
+    computeJointJacobiansTimeVariation(model,data,q,v);
+    forwardKinematics(model,data,q,v);
+    framesForwardKinematics(model,data);
+    Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
+    dJ_world.fill(0.);    dJ_local.fill(0.);
+    getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ_world);
+    getFrameJacobianTimeVariation<LOCAL>(model,data,idx,dJ_local);
+
+    BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha)));
+    BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));   
+  }
+}
+             
 BOOST_AUTO_TEST_SUITE_END ()