test/joint-configuration: increase test of dIntegrate

unittest/joint-configurations.cpp

@@ -99,7 +99,7 @@ BOOST_AUTO_TEST_CASE ( diff_integration_test )
   Model model; buildModel(model);
   
   std::vector<Eigen::VectorXd> qs(2);
-  std::vector<Eigen::VectorXd> v(2);
+  std::vector<Eigen::VectorXd> vs(2);
   std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
   std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
   
@@ -108,9 +108,9 @@ BOOST_AUTO_TEST_CASE ( diff_integration_test )
   qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
   qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint
   
-  v[0] = Eigen::VectorXd::Zero(model.nv);
-  v[1] = Eigen::VectorXd::Ones(model.nv);
-  dIntegrate(model,qs[0],v[0],results[0],ARG0);
+  vs[0] = Eigen::VectorXd::Zero(model.nv);
+  vs[1] = Eigen::VectorXd::Ones(model.nv);
+  dIntegrate(model,qs[0],vs[0],results[0],ARG0);
   
   Eigen::VectorXd q_fd(model.nq), v_fd(model.nv); v_fd.setZero();
   const double eps = 1e-8;
@@ -123,6 +123,35 @@ BOOST_AUTO_TEST_CASE ( diff_integration_test )
   }
   BOOST_CHECK(results[0].isIdentity(sqrt(eps)));
   BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
+  
+  dIntegrate(model,qs[0],vs[0],results[1],ARG1);
+  BOOST_CHECK(results[1].isApprox(results[0]));
+  
+  dIntegrate(model,qs[0],vs[1],results[0],ARG0);
+  Eigen::VectorXd q_fd_intermediate(model.nq);
+  Eigen::VectorXd q0_plus_v = integrate(model,qs[0],vs[1]);
+  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
+  {
+    v_fd[k] = eps;
+    q_fd_intermediate = integrate(model,qs[0],v_fd);
+    q_fd = integrate(model,q_fd_intermediate,vs[1]);
+    results_fd[0].col(k) = difference(model,q0_plus_v,q_fd)/eps;
+    v_fd[k] = 0.;
+  }
+  
+  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
+  
+  dIntegrate(model,qs[0],vs[1],results[1],ARG1);
+  v_fd = vs[1];
+  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
+  {
+    v_fd[k] += eps;
+    q_fd = integrate(model,qs[0],v_fd);
+    results_fd[1].col(k) = difference(model,q0_plus_v,q_fd)/eps;
+    v_fd[k] -= eps;
+  }
+  
+  BOOST_CHECK(results[1].isApprox(results_fd[1],sqrt(eps)));
 }
 
 BOOST_AUTO_TEST_CASE ( integrate_difference_test )