[algorithm/frames] add getFrameJacobianTimeVariation in the same groove as...

[algorithm/frames] add getFrameJacobianTimeVariation in the same groove as getJointJacobianTimeVariation

bindings/python/algorithm/expose-frames.cpp

@@ -49,6 +49,22 @@ 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;
+    }
+
     
     void exposeFramesAlgo()
     {
@@ -87,7 +103,16 @@ 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("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

@@ -88,6 +88,25 @@ namespace se3
                                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[frame_id];
+    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[frame_id];
+      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__