Commit 3f99ecae authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Pierre Fernbach
Browse files

[Tests] add test case for new helpers in contactPhase

parent 4e112df5
......@@ -31,6 +31,7 @@ using curves::pointX_t;
using curves::matrix3_t;
using curves::quaternion_t;
using curves::t_pointX_t;
using curves::t_point3_t;
using curves::curve_ptr_t;
using curves::curve_SE3_ptr_t;
using namespace multicontact_api::scenario;
......@@ -1031,6 +1032,244 @@ BOOST_AUTO_TEST_CASE(contact_phase)
}
BOOST_AUTO_TEST_CASE(contact_phase_helpers){
point3_t p0(0., 0., 0.);
point3_t p1(1., 2., 3.);
point3_t p2(4., 4., 4.);
point3_t p3(10., 10., 10.);
point3_t d0(1., 1., 1.);
point3_t d1(2., 2., 2.);
point3_t d2(3., 3., 3.);
point3_t d3(5., 5., 5.);
point3_t dd0(1.5, 1.5, 1.5);
point3_t dd1(2.5, 2.5, 2.5);
point3_t dd2(3.5, 3.5, 3.5);
point3_t dd3(5.5, 5.5, 5.5);
double t0 = 1.0;
double t1 = 1.5;
double t2 = 3.0;
double t3 = 10.0;
t_pointX_t points;
points.push_back(p0);
points.push_back(p1);
points.push_back(p2);
points.push_back(p3);
t_pointX_t points_derivative;
points_derivative.push_back(d0);
points_derivative.push_back(d1);
points_derivative.push_back(d2);
points_derivative.push_back(d3);
t_pointX_t points_second_derivative;
points_second_derivative.push_back(dd0);
points_second_derivative.push_back(dd1);
points_second_derivative.push_back(dd2);
points_second_derivative.push_back(dd3);
std::vector<double> time_points;
time_points.push_back(t0);
time_points.push_back(t1);
time_points.push_back(t2);
time_points.push_back(t3);
// check COM trajectory :
ContactPhase cp;
cp.setCOMtrajectoryFromPoints(points,points_derivative,points_second_derivative,time_points);
BOOST_CHECK_EQUAL(cp.m_c->dim(),3);
BOOST_CHECK_EQUAL(cp.m_dc->dim(),3);
BOOST_CHECK_EQUAL(cp.m_ddc->dim(),3);
BOOST_CHECK_EQUAL(cp.m_c->min(),t0);
BOOST_CHECK_EQUAL(cp.m_dc->min(),t0);
BOOST_CHECK_EQUAL(cp.m_ddc->min(),t0);
BOOST_CHECK_EQUAL(cp.m_c->max(),t3);
BOOST_CHECK_EQUAL(cp.m_dc->max(),t3);
BOOST_CHECK_EQUAL(cp.m_ddc->max(),t3);
BOOST_CHECK((*cp.m_c)(t0).isApprox(p0));
BOOST_CHECK((*cp.m_c)(t1).isApprox(p1));
BOOST_CHECK((*cp.m_c)(t2).isApprox(p2));
BOOST_CHECK((*cp.m_c)(t3).isApprox(p3));
BOOST_CHECK((*cp.m_dc)(t0).isApprox(d0));
BOOST_CHECK((*cp.m_dc)(t1).isApprox(d1));
BOOST_CHECK((*cp.m_dc)(t2).isApprox(d2));
BOOST_CHECK((*cp.m_dc)(t3).isApprox(d3));
BOOST_CHECK((*cp.m_ddc)(t0).isApprox(dd0));
BOOST_CHECK((*cp.m_ddc)(t1).isApprox(dd1));
BOOST_CHECK((*cp.m_ddc)(t2).isApprox(dd2));
BOOST_CHECK((*cp.m_ddc)(t3).isApprox(dd3));
BOOST_CHECK(cp.m_c_init.isApprox(p0));
BOOST_CHECK(cp.m_c_final.isApprox(p3));
BOOST_CHECK(cp.m_dc_init.isApprox(d0));
BOOST_CHECK(cp.m_dc_final.isApprox(d3));
BOOST_CHECK(cp.m_ddc_init.isApprox(dd0));
BOOST_CHECK(cp.m_ddc_final.isApprox(dd3));
// check that init/final are not modified if they ere already set :
ContactPhase cp2 = buildRandomContactPhase();
point3_t c_init,c_final,dc_init,dc_final,ddc_init,ddc_final;
c_init = cp2.m_c_init;
dc_init = cp2.m_dc_init;
ddc_init = cp2.m_ddc_init;
c_final = cp2.m_c_final;
dc_final = cp2.m_dc_final;
ddc_final = cp2.m_ddc_final;
cp2.setCOMtrajectoryFromPoints(points,points_derivative,points_second_derivative,time_points);
BOOST_CHECK(cp2.m_c_init.isApprox(c_init));
BOOST_CHECK(cp2.m_c_final.isApprox(c_final));
BOOST_CHECK(cp2.m_dc_init.isApprox(dc_init));
BOOST_CHECK(cp2.m_dc_final.isApprox(dc_final));
BOOST_CHECK(cp2.m_ddc_init.isApprox(ddc_init));
BOOST_CHECK(cp2.m_ddc_final.isApprox(ddc_final));
BOOST_CHECK(!cp2.m_c_init.isApprox(p0));
BOOST_CHECK(!cp2.m_c_final.isApprox(p3));
BOOST_CHECK(!cp2.m_dc_init.isApprox(d0));
BOOST_CHECK(!cp2.m_dc_final.isApprox(d3));
BOOST_CHECK(!cp2.m_ddc_init.isApprox(dd0));
BOOST_CHECK(!cp2.m_ddc_final.isApprox(dd3));
//check AM trajectory
cp.setAMtrajectoryFromPoints(points,points_derivative,time_points);
BOOST_CHECK_EQUAL(cp.m_L->dim(),3);
BOOST_CHECK_EQUAL(cp.m_dL->dim(),3);
BOOST_CHECK_EQUAL(cp.m_L->min(),t0);
BOOST_CHECK_EQUAL(cp.m_dL->min(),t0);
BOOST_CHECK_EQUAL(cp.m_L->max(),t3);
BOOST_CHECK_EQUAL(cp.m_dL->max(),t3);
BOOST_CHECK((*cp.m_L)(t0).isApprox(p0));
BOOST_CHECK((*cp.m_L)(t1).isApprox(p1));
BOOST_CHECK((*cp.m_L)(t2).isApprox(p2));
BOOST_CHECK((*cp.m_L)(t3).isApprox(p3));
BOOST_CHECK((*cp.m_dL)(t0).isApprox(d0));
BOOST_CHECK((*cp.m_dL)(t1).isApprox(d1));
BOOST_CHECK((*cp.m_dL)(t2).isApprox(d2));
BOOST_CHECK((*cp.m_dL)(t3).isApprox(d3));
BOOST_CHECK(cp.m_L_init.isApprox(p0));
BOOST_CHECK(cp.m_L_final.isApprox(p3));
BOOST_CHECK(cp.m_dL_init.isApprox(d0));
BOOST_CHECK(cp.m_dL_final.isApprox(d3));
// check that init/final are not modified if they ere already set :
point3_t L_init,L_final,dL_init,dL_final;
L_init = cp2.m_L_init;
dL_init = cp2.m_dL_init;
L_final = cp2.m_L_final;
dL_final = cp2.m_dL_final;
cp2.setAMtrajectoryFromPoints(points,points_derivative,time_points);
BOOST_CHECK(cp2.m_L_init.isApprox(L_init));
BOOST_CHECK(cp2.m_L_final.isApprox(L_final));
BOOST_CHECK(cp2.m_dL_init.isApprox(dL_init));
BOOST_CHECK(cp2.m_dL_final.isApprox(dL_final));
BOOST_CHECK(!cp2.m_L_init.isApprox(p0));
BOOST_CHECK(!cp2.m_L_final.isApprox(p3));
BOOST_CHECK(!cp2.m_dL_init.isApprox(d0));
BOOST_CHECK(!cp2.m_dL_final.isApprox(d3));
// check q trajectory :
pointX_t q0 = point12_t::Random();
pointX_t q1 = point12_t::Random();
pointX_t q2 = point12_t::Random();
pointX_t q3 = point12_t::Random();
pointX_t dq0 = point12_t::Random();
pointX_t dq1 = point12_t::Random();
pointX_t dq2 = point12_t::Random();
pointX_t dq3 = point12_t::Random();
pointX_t ddq0 = point12_t::Random();
pointX_t ddq1 = point12_t::Random();
pointX_t ddq2 = point12_t::Random();
pointX_t ddq3 = point12_t::Random();
t_pointX_t points_q;
points_q.push_back(q0);
points_q.push_back(q1);
points_q.push_back(q2);
points_q.push_back(q3);
t_pointX_t points_derivative_q;
points_derivative_q.push_back(dq0);
points_derivative_q.push_back(dq1);
points_derivative_q.push_back(dq2);
points_derivative_q.push_back(dq3);
t_pointX_t points_second_derivative_q;
points_second_derivative_q.push_back(ddq0);
points_second_derivative_q.push_back(ddq1);
points_second_derivative_q.push_back(ddq2);
points_second_derivative_q.push_back(ddq3);
cp.setJointsTrajectoryFromPoints(points_q,points_derivative_q,points_second_derivative_q,time_points);
BOOST_CHECK_EQUAL(cp.m_q->dim(),12);
BOOST_CHECK_EQUAL(cp.m_dq->dim(),12);
BOOST_CHECK_EQUAL(cp.m_ddq->dim(),12);
BOOST_CHECK_EQUAL(cp.m_q->min(),t0);
BOOST_CHECK_EQUAL(cp.m_dq->min(),t0);
BOOST_CHECK_EQUAL(cp.m_ddq->min(),t0);
BOOST_CHECK_EQUAL(cp.m_q->max(),t3);
BOOST_CHECK_EQUAL(cp.m_dq->max(),t3);
BOOST_CHECK_EQUAL(cp.m_ddq->max(),t3);
BOOST_CHECK((*cp.m_q)(t0).isApprox(q0));
BOOST_CHECK((*cp.m_q)(t1).isApprox(q1));
BOOST_CHECK((*cp.m_q)(t2).isApprox(q2));
BOOST_CHECK((*cp.m_q)(t3).isApprox(q3));
BOOST_CHECK((*cp.m_dq)(t0).isApprox(dq0));
BOOST_CHECK((*cp.m_dq)(t1).isApprox(dq1));
BOOST_CHECK((*cp.m_dq)(t2).isApprox(dq2));
BOOST_CHECK((*cp.m_dq)(t3).isApprox(dq3));
BOOST_CHECK((*cp.m_ddq)(t0).isApprox(ddq0));
BOOST_CHECK((*cp.m_ddq)(t1).isApprox(ddq1));
BOOST_CHECK((*cp.m_ddq)(t2).isApprox(ddq2));
BOOST_CHECK((*cp.m_ddq)(t3).isApprox(ddq3));
BOOST_CHECK(cp.m_q_init.isApprox(q0));
BOOST_CHECK(cp.m_q_final.isApprox(q3));
// check that init/final are not modified if they ere already set :
pointX_t q_init,q_final;
q_init = cp2.m_q_init;
q_final = cp2.m_q_final;
cp2.setJointsTrajectoryFromPoints(points_q,points_derivative_q,points_second_derivative_q,time_points);
BOOST_CHECK(cp2.m_q_init.isApprox(q_init));
BOOST_CHECK(cp2.m_q_final.isApprox(q_final));
BOOST_CHECK(!cp2.m_q_init.isApprox(q0));
BOOST_CHECK(!cp2.m_q_final.isApprox(q3));
// check that errors are correctly throw when required :
// not the correct size
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points_q,points_derivative,points_second_derivative,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points,points_derivative_q,points_second_derivative,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points,points_derivative,points_second_derivative_q,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points_q,points_derivative_q,points_second_derivative_q,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setAMtrajectoryFromPoints(points_q,points_derivative_q,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setAMtrajectoryFromPoints(points,points_derivative_q,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setAMtrajectoryFromPoints(points_q,points_derivative,time_points),std::invalid_argument);
// not the same number of points :
t_pointX_t points2 = points;
points2.push_back(d0);
t_pointX_t points_derivative2 = points_derivative;
points_derivative2.push_back(d0);
t_pointX_t points_second_derivative2 = points_second_derivative;
points_second_derivative2.push_back(d0);
std::vector<double> time_points2 = time_points;
time_points2.push_back(50.);
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points2,points_derivative,points_second_derivative,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points,points_derivative2,points_second_derivative,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points,points_derivative,points_second_derivative2,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setCOMtrajectoryFromPoints(points,points_derivative,points_second_derivative,time_points2),std::invalid_argument);
BOOST_CHECK_THROW(cp.setAMtrajectoryFromPoints(points2,points_derivative,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setAMtrajectoryFromPoints(points,points_derivative2,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setAMtrajectoryFromPoints(points,points_derivative,time_points2),std::invalid_argument);
t_pointX_t points_q2 = points_q;
points_q2.push_back(q0);
t_pointX_t points_derivative_q2 = points_derivative_q;
points_derivative_q2.push_back(q0);
t_pointX_t points_second_derivative_q2 = points_second_derivative_q;
points_second_derivative_q2.push_back(q0);
BOOST_CHECK_THROW(cp.setJointsTrajectoryFromPoints(points_q2,points_derivative_q,points_second_derivative_q,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setJointsTrajectoryFromPoints(points_q,points_derivative_q2,points_second_derivative_q,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setJointsTrajectoryFromPoints(points_q,points_derivative_q,points_second_derivative_q2,time_points),std::invalid_argument);
BOOST_CHECK_THROW(cp.setJointsTrajectoryFromPoints(points_q,points_derivative_q,points_second_derivative_q,time_points2),std::invalid_argument);
}
BOOST_AUTO_TEST_CASE(contact_sequence)
{
// test without specifying size and using 'append' :
......
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