Verified Commit 6ab91589 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test/joint-configuration: increase test of dIntegrate

parent cd231f9c
......@@ -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 )
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment