[C++][unittest] Reworked computations of frame Jacobian in global frame....

[C++][unittest] Reworked computations of frame Jacobian in global frame. Reworked unittest. Change argument from Matrix6x to MatrixXd to follow getJacobian convention

src/algorithm/operational-frames.hpp

@@ -20,8 +20,10 @@
 
 #include "pinocchio/multibody/visitor.hpp"
 #include "pinocchio/multibody/model.hpp"
+#include "pinocchio/spatial/skew.hpp"
 
 #include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
 
 namespace se3
 {
@@ -107,20 +109,29 @@ inline void  framesForwardKinematic(const Model & model,
 
 template<bool localFrame>
 inline void getFrameJacobian(const Model & model, const Data& data,
-     Model::Index frame_id, Data::Matrix6x & J)
+     Model::Index frame_id, Eigen::MatrixXd & J)
 {
   assert( J.rows() == data.J.rows() );
   assert( J.cols() == data.J.cols() );
 
   const Model::Index & parent = model.operational_frames[frame_id].parent_id;
   const SE3 & oMframe = data.oMof[frame_id];
-
+  
   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) = model.operational_frames[frame_id].frame_placement.actInv(Motion(data.J.col(j))).toVector();
-      else                J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+      if(! localFrame )
+      {
+        J.col(j).topRows<3>() += c_cross *  J.col(j).bottomRows<3>();
+      }  
+      else
+      {
+        J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+      }
     }
 }
 

unittest/operational-frames.cpp

@@ -70,34 +70,44 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
 
   VectorXd q = VectorXd::Ones(model.nq);
   q.middleRows<4> (3).normalize();
+  VectorXd q_dot = VectorXd::Ones(model.nv);
   framesForwardKinematic(model, data, q);
 
 
   computeJacobians(model,data,q);
 
 
-  MatrixXd expected(6,model.nv); expected.fill(0);
-  Matrix6x Jof(6,model.nv); Jof.fill(0);
+  /// In global frame
+
+  MatrixXd Joj(6,model.nv); Joj.fill(0);
+  MatrixXd Jof(6,model.nv); Jof.fill(0);
   Model::Index idx = model.getFrameId(frame_name);
 
   getFrameJacobian<false>(model,data,idx,Jof);
-  getJacobian<false>(model, data, parent_idx, expected);
-  expected = frame_placement.inverse().toActionMatrix() * expected;
+  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.translation().cross(nu_joint.angular());
+
+
+  BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
 
-  BOOST_CHECK(Jof.isApprox(expected, 1e-12));
 
+  /// In local frame
 
-  expected.fill(0); Jof.fill(0);
-  getFrameJacobian<true>(model,data,idx,Jof);
-  getJacobian<true>(model, data, parent_idx, expected);
+  MatrixXd Jjj(6,model.nv); Jjj.fill(0);
+  MatrixXd Jff(6,model.nv); Jff.fill(0);
+  getFrameJacobian<true>(model,data,idx,Jff);
+  getJacobian<true>(model, data, parent_idx, Jjj);
 
-  expected = frame_placement.inverse().toActionMatrix() * expected;
+  nu_frame = Jff*q_dot;
+  nu_joint = Jjj*q_dot;
 
-  // std::cout << Jof << std::endl;
-  // std::cout << "----" << std::endl;
-  // std::cout << frame_placement << std::endl;
-  // std::cout << expected << std::endl;
-  BOOST_CHECK(Jof.isApprox(expected, 1e-12));
+  BOOST_CHECK(nu_frame.toVector().isApprox(frame_placement.actInv(nu_joint).toVector(), 1e-12));
 }
 
 BOOST_AUTO_TEST_SUITE_END ()