[Tests] add test case for new helpers in contactPhase

unittest/scenario.cpp

@@ -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' :