[C++] correct computation of jacobian

src/algorithm/operational-frames.hpp

@@ -119,14 +119,13 @@ inline void getFrameJacobian(const Model & model, const Data& data,
   
   int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
 
-  Eigen::Matrix3d c_cross = skew(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>() += c_cross *  J.col(j).bottomRows<3>();
+        J.col(j).topRows<3>() += model.operational_frames[frame_id].frame_placement.inverse().translation().cross( J.col(j).bottomRows<3>());
       }  
       else
       {

unittest/operational-frames.cpp

@@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
   Motion nu_joint(Joj*q_dot);
 
   Motion nu_frame_from_nu_joint(nu_joint);
-  nu_frame_from_nu_joint.linear() += frame_placement.translation().cross(nu_joint.angular());
+  nu_frame_from_nu_joint.linear() += frame_placement.inverse().translation().cross(nu_joint.angular());
 
 
   BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));