Merge pull request #392 from jcarpent/topic/devel

Correct frameJacobians

src/algorithm/frames.hpp

@@ -129,20 +129,19 @@ namespace se3
     
     const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
     
-    // Lever between the joint center and the frame center expressed in the global frame
-    const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
     
-    getJacobian<local_frame>(model, data, parent, J);
+    if(!local_frame)
+      getJacobian<local_frame>(model, data, parent, J);
 
-    if (!frame.placement.isIdentity())
+    // Lever between the joint center and the frame center expressed in the global frame
+    const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
+      
+    for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
     {
-      for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
-      {
-        if(!local_frame)
-          J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
-        else
-          J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
-      }
+      if(!local_frame)
+        J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
+      else
+        J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
     }
   }
 

unittest/frames.cpp

@@ -64,46 +64,53 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
   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 = VectorXd::Ones(model.nq);
   q.middleRows<4> (3).normalize();
   VectorXd q_dot = VectorXd::Ones(model.nv);
-  framesForwardKinematics(model, data, q);
-
-
-  computeJacobians(model,data,q);
 
 
   /// In global frame
-
   Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
   Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
   Model::Index idx = model.getFrameId(frame_name);
+  const Frame & frame = model.frames[idx];
+  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
 
+  computeJacobians(model,data,q);
+  computeJacobians(model,data_ref,q);
+  framesForwardKinematics(model,data);
   getFrameJacobian<false>(model,data,idx,Jof);
-  getJacobian<false>(model, data, parent_idx, Joj);
+  getJacobian<false>(model, data_ref, parent_idx, Joj);
 
   Motion nu_frame(Jof*q_dot);
   Motion nu_joint(Joj*q_dot);
+  
+  SE3 translation(SE3::Identity());
+  translation.translation(data.oMi[parent_idx].rotation()*frame.placement.translation());
+  
+  Motion nu_frame_from_nu_joint(translation.actInv(nu_joint));
 
-  Motion nu_frame_from_nu_joint(nu_joint);
-  nu_frame_from_nu_joint.linear() -= (data.oMi[parent_idx].rotation() *framePlacement.translation()).cross(nu_joint.angular());
-
-
-  BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
-
+  BOOST_CHECK(nu_frame.isApprox(nu_frame_from_nu_joint, 1e-12));
 
   /// In local frame
 
   Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
   Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
   getFrameJacobian<true>(model,data,idx,Jff);
-  getJacobian<true>(model, data, parent_idx, Jjj);
-
-  nu_frame = Jff*q_dot;
-  nu_joint = Jjj*q_dot;
-
-  BOOST_CHECK(nu_frame.toVector().isApprox(framePlacement.actInv(nu_joint).toVector(), 1e-12));
+  getJacobian<true>(model, data_ref, parent_idx, Jjj);
+
+  nu_frame = Motion(Jff*q_dot);
+  nu_joint = Motion(Jjj*q_dot);
+  
+  const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
+  Data::Matrix6x Jjj_from_frame(jXf * Jff);
+  BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
+  
+  nu_frame_from_nu_joint = frame.placement.act(nu_frame);
+  BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
 }
 
 BOOST_AUTO_TEST_SUITE_END ()