algo/jacobian: add support of LOCAL_WORLD_ALIGNED option

src/algorithm/jacobian.hxx

@@ -314,22 +314,59 @@ namespace pinocchio
     
     Matrix6xLike & dJ_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ);
     
-    const typename Data::SE3 & oMjoint = data.oMi[jointId];
+    
+
+    typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
+    typedef const MotionRef<ConstColXprIn> MotionIn;
+
+    typedef typename Matrix6xLike::ColXpr ColXprOut;
+    typedef MotionRef<ColXprOut> MotionOut;
+    
     int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
-    for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
+    switch(rf)
     {
-      typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
-      const MotionRef<ConstColXprIn> v_in(data.dJ.col(j));
-      
-      typedef typename Matrix6xLike::ColXpr ColXprOut;
-      MotionRef<ColXprOut> v_out(dJ_.col(j));
-      
-      if(rf == WORLD)   v_out = v_in;
-      else              v_out = oMjoint.actInv(v_in).toVector();
+      case WORLD:
+      {
+        for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
+        {
+          MotionIn v_in(data.dJ.col(j));
+          MotionOut v_out(dJ_.col(j));
+
+          v_out = v_in;
+        }
+        break;
+      }
+      case LOCAL_WORLD_ALIGNED:
+      {
+        const typename Data::SE3 & oMjoint = data.oMi[jointId];
+        for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
+        {
+          MotionIn v_in(data.dJ.col(j));
+          MotionOut v_out(dJ_.col(j));
+          
+          v_out = v_in;
+          v_out.linear() -= oMjoint.translation().cross(v_in.angular());
+        }
+        break;
+      }
+      case LOCAL:
+      {
+        const typename Data::SE3 & oMjoint = data.oMi[jointId];
+        for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
+        {
+          MotionIn v_in(data.dJ.col(j));
+          MotionOut v_out(dJ_.col(j));
+          
+          v_out = oMjoint.actInv(v_in).toVector();
+        }
+        break;
+      }
+      default:
+        assert(false && "must never happened");
+        break;
     }
   }
   
-  
 } // namespace pinocchio
 
 /// @endcond

unittest/joint-jacobian.cpp

 //
-// Copyright (c) 2015-2018 CNRS
+// Copyright (c) 2015-2019 CNRS INRIA
 //
 
 #include "pinocchio/multibody/model.hpp"
@@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
   Data::Matrix6x J(6,model.nv); J.fill(0.);
   Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
   
-  // Regarding to the world origin
+  // Regarding to the WORLD origin
   getJointJacobian(model,data,idx,WORLD,J);
   getJointJacobianTimeVariation(model,data,idx,WORLD,dJ);
   
@@ -105,8 +105,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
   const Motion & a_ref = data_ref.oMi[idx].act(data_ref.a[idx]);
   BOOST_CHECK(a_idx.isApprox(a_ref));
   
-  
-  // Regarding to the local frame
+  // Regarding to the LOCAL frame
   getJointJacobian(model,data,idx,LOCAL,J);
   getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ);
   
@@ -116,7 +115,20 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
   a_idx = (Motion::Vector6)(J*a + dJ*v);
   BOOST_CHECK(a_idx.isApprox(data_ref.a[idx]));
   
-  // compare to finite differencies
+  // Regarding to the LOCAL_WORLD_ALIGNED frame
+  getJointJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J);
+  getJointJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ);
+  
+  Data::SE3 worldMlocal = data.oMi[idx];
+  worldMlocal.translation().setZero();
+  
+  v_idx = (Motion::Vector6)(J*v);
+  BOOST_CHECK(v_idx.isApprox(worldMlocal.act(data_ref.v[idx])));
+  
+  a_idx = (Motion::Vector6)(J*a + dJ*v);
+  BOOST_CHECK(a_idx.isApprox(worldMlocal.act(data_ref.a[idx])));
+  
+  // compare to finite differencies : WORLD
   {
     Data data_ref(model), data_ref_plus(model);
     
@@ -141,10 +153,36 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
     
     BOOST_CHECK(dJ.isApprox(dJ_ref,sqrt(alpha)));
   }
+  
+  // compare to finite differencies : LOCAL
+  {
+    Data data_ref(model), data_ref_plus(model);
+    
+    const double alpha = 1e-8;
+    Eigen::VectorXd q_plus(model.nq);
+    q_plus = integrate(model,q,alpha*v);
+    
+    Data::Matrix6x J_ref(6,model.nv); J_ref.fill(0.);
+    computeJointJacobians(model,data_ref,q);
+    getJointJacobian(model,data_ref,idx,LOCAL,J_ref);
+    
+    Data::Matrix6x J_ref_plus(6,model.nv); J_ref_plus.fill(0.);
+    computeJointJacobians(model,data_ref_plus,q_plus);
+    getJointJacobian(model,data_ref_plus,idx,LOCAL,J_ref_plus);
+    
+    const Data::SE3 M_plus = data_ref.oMi[idx].inverse()*data_ref_plus.oMi[idx];
+    
+    Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.fill(0.);
+    dJ_ref = (M_plus.toActionMatrix()*J_ref_plus - J_ref)/alpha;
+    
+    computeJointJacobiansTimeVariation(model,data,q,v);
+    Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
+    getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ);
+    
+    BOOST_CHECK(dJ.isApprox(dJ_ref,sqrt(alpha)));
+  }
 }
 
-
-
 BOOST_AUTO_TEST_CASE ( test_timings )
 {
   using namespace Eigen;
@@ -229,4 +267,3 @@ BOOST_AUTO_TEST_CASE ( test_timings )
 }
 
 BOOST_AUTO_TEST_SUITE_END ()
-