[bindings/framejacobiantimevariation] add a function...

[bindings/framejacobiantimevariation] add a function framesJacobianTimeVariation in the groove of framesJacobian that also calls compute and fk.

bindings/python/algorithm/expose-frames.cpp

@@ -65,6 +65,20 @@ namespace se3
       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()
     {
@@ -104,6 +118,18 @@ namespace se3
               "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",

src/algorithm/frames.hxx

@@ -103,7 +103,7 @@ namespace se3
     assert( dJ.cols() == data.dJ.cols() );    
     assert(model.check(data) && "data is not consistent with model.");
     
-    const Frame & frame = model.frames[frame_id];
+    const Frame & frame = model.frames[frameId];
     const Model::JointIndex & joint_id = frame.parent;
     if (rf == WORLD)
     {
@@ -113,7 +113,7 @@ namespace se3
     
     if (rf == LOCAL)
     {
-      const SE3 & oMframe = data.oMf[frame_id];
+      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])