[unittest/frames] add unittest for framejacobiantimederivative

unittest/frames.cpp

@@ -23,12 +23,20 @@
 #include "pinocchio/spatial/act-on-set.hpp"
 #include "pinocchio/parsers/sample-models.hpp"
 #include "pinocchio/utils/timer.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
 
 #include <iostream>
 
 #include <boost/test/unit_test.hpp>
 #include <boost/utility/binary.hpp>
 
+template<typename Derived>
+inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
+{
+  return ((x - x).array() == (x - x).array()).all();
+}
+
+
 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
 
 BOOST_AUTO_TEST_CASE ( test_kinematics )
@@ -96,5 +104,111 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
   BOOST_CHECK(Jff.isApprox(Jjj));
 }
 
+BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
+{
+  using namespace Eigen;
+  using namespace se3;
+  
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
+  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 = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
+  VectorXd v = VectorXd::Random(model.nv);
+  VectorXd a = VectorXd::Random(model.nv);
+  
+  computeJointJacobiansTimeVariation(model,data,q,v);
+  forwardKinematics(model,data_ref,q,v,a);
+  framesForwardKinematics(model,data_ref);
+  framesForwardKinematics(model,data);
+
+  BOOST_CHECK(isFinite(data.dJ));
+
+  Model::Index idx = model.getFrameId(frame_name);
+  const Frame & frame = model.frames[idx];
+  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
+    
+  
+  Data::Matrix6x J(6,model.nv); J.fill(0.);
+  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
+  
+  // Regarding to the world origin
+  getFrameJacobian<WORLD>(model,data,idx,J);
+  getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ);
+  
+  Motion v_idx(J*v);
+  const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]);
+  const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local);
+  BOOST_CHECK(v_idx.isApprox(v_ref));
+  
+  Motion a_idx(J*a + dJ*v);
+  const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
+  const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local);
+  BOOST_CHECK(a_idx.isApprox(a_ref));
+  
+  J.fill(0.);  dJ.fill(0.);
+  // Regarding to the local frame
+  getFrameJacobian<LOCAL>(model,data,idx,J);
+  getFrameJacobianTimeVariation<LOCAL>(model,data,idx,dJ);
+  
+  v_idx = (Motion::Vector6)(J*v);
+  BOOST_CHECK(v_idx.isApprox(v_ref_local));
+              
+  a_idx = (Motion::Vector6)(J*a + dJ*v);
+  BOOST_CHECK(a_idx.isApprox(a_ref_local));
+  
+  // compare to finite differencies
+  {
+    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_ref
+    Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
+    J_ref_world.fill(0.);     J_ref_local.fill(0.);
+    computeJointJacobians(model,data_ref,q);
+    framesForwardKinematics(model,data_ref);
+    const SE3 & oMf_q = data_ref.oMf[idx];
+    getFrameJacobian<WORLD>(model,data_ref,idx,J_ref_world);
+    getFrameJacobian<LOCAL>(model,data_ref,idx,J_ref_local);
+    
+    //data_ref_plus
+    Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv);
+    J_ref_plus_world.fill(0.);    J_ref_plus_local.fill(0.);
+    computeJointJacobians(model,data_ref_plus,q_plus);
+    framesForwardKinematics(model,data_ref_plus);
+    const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
+    getFrameJacobian<WORLD>(model,data_ref_plus,idx,J_ref_plus_world);
+    getFrameJacobian<LOCAL>(model,data_ref_plus,idx,J_ref_plus_local);
+
+    //Move J_ref_plus_local to reference frame
+    J_ref_plus_local = (oMf_q.inverse()*oMf_qplus).toActionMatrix()*(J_ref_plus_local);
+    
+    Data::Matrix6x dJ_ref_world(6,model.nv), dJ_ref_local(6,model.nv);
+    dJ_ref_world.fill(0.);        dJ_ref_local.fill(0.);
+    dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha;
+    dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha;
+
+    //data
+    computeJointJacobiansTimeVariation(model,data,q,v);
+    forwardKinematics(model,data,q,v);
+    framesForwardKinematics(model,data);
+    Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
+    dJ_world.fill(0.);    dJ_local.fill(0.);
+    getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ_world);
+    getFrameJacobianTimeVariation<LOCAL>(model,data,idx,dJ_local);
+
+    BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha)));
+    BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));   
+  }
+}
+             
 BOOST_AUTO_TEST_SUITE_END ()