[frame jacobian] Express jacobian in correct frame

src/algorithm/operational-frames.hpp

@@ -119,13 +119,14 @@ inline void getFrameJacobian(const Model & model, const Data& data,
   
   int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
 
+  SE3::Vector3 lever(data.oMi[parent].rotation() * (model.operational_frames[frame_id].frame_placement.translation()));
 
   if (!localFrame) getJacobian<localFrame>(model, data, parent, J);
   for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
     {
       if(! localFrame )
       {
-        J.col(j).topRows<3>() += model.operational_frames[frame_id].frame_placement.inverse().translation().cross( J.col(j).bottomRows<3>());
+        J.col(j).topRows<3>() -= lever.cross( J.col(j).bottomRows<3>());
       }  
       else
       {

unittest/operational-frames.cpp

@@ -85,13 +85,12 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
 
   getFrameJacobian<false>(model,data,idx,Jof);
   getJacobian<false>(model, data, parent_idx, Joj);
-  // expected = frame_placement.inverse().toActionMatrix() * expected;
 
   Motion nu_frame(Jof*q_dot);
   Motion nu_joint(Joj*q_dot);
 
   Motion nu_frame_from_nu_joint(nu_joint);
-  nu_frame_from_nu_joint.linear() += frame_placement.inverse().translation().cross(nu_joint.angular());
+  nu_frame_from_nu_joint.linear() -= (data.oMi[parent_idx].rotation() *frame_placement.translation()).cross(nu_joint.angular());
 
 
   BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));