Merge pull request #9 from pFernbach/topic/fix_concatenate_effector

fix concatenate effector

include/multicontact-api/scenario/contact-sequence.hpp

@@ -1043,7 +1043,7 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
    */
   piecewise_SE3_t concatenateEffectorTrajectories(const std::string& eeName) const {
     piecewise_SE3_t res = piecewise_SE3_t();
-    transform_t last_placement;
+    transform_t last_placement, first_placement;
     // first find the first and last phase with a trajectory for this effector
     size_t first_phase = m_contact_phases.size();
     size_t last_phase = 0;
@@ -1052,9 +1052,20 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         last_phase = i;
         if (first_phase > i) {
           first_phase = i;
+          curve_SE3_ptr curve = m_contact_phases.at(i).effectorTrajectories().at(eeName);
+          first_placement = curve->operator()(curve->min());
         }
       }
     }
+    if(first_phase == m_contact_phases.size())
+      throw std::invalid_argument("The contact sequence doesn't have any phase with an effector trajectory"
+                                  " for the given effector name");
+    if(first_phase > 0){
+      // add a first constant phase at the initial placement
+      curve_SE3_ptr ptr_init(new SE3Curve_t(first_placement, first_placement, m_contact_phases.at(0).timeInitial(),
+                                       m_contact_phases.at(first_phase).timeInitial()));
+      res.add_curve_ptr(ptr_init);
+    }
     // loop over this phases to concatenate the trajectories
     for (size_t i = first_phase; i <= last_phase; ++i) {
       if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
@@ -1067,6 +1078,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         res.add_curve_ptr(ptr);
       }
     }
+    if(last_phase < m_contact_phases.size() - 1){
+      // add a last constant phase until the end of the contact sequence
+      curve_SE3_ptr ptr_final(new SE3Curve_t(last_placement, last_placement, m_contact_phases.at(last_phase).timeFinal(),
+                                       m_contact_phases.back().timeFinal()));
+      res.add_curve_ptr(ptr_final);
+    }
     return res;
   }
 

unittest/scenario.cpp

@@ -1859,6 +1859,29 @@ BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_effector_traj) {
   BOOST_CHECK(traj(8.).isApprox(traj_2->operator()(8.)));
   BOOST_CHECK(traj(2.5).isApprox(traj_0->operator()(2.)));
   BOOST_CHECK(traj(3.8).isApprox(traj_0->operator()(2.)));
+  BOOST_CHECK_THROW(cs1.concatenateEffectorTrajectories("test"), std::invalid_argument);
+
+  ContactPhase cp3 = ContactPhase(8, 12.);
+  cs1.append(cp3);
+  traj = cs1.concatenateEffectorTrajectories("right_leg");
+  BOOST_CHECK(traj.min() == 0.);
+  BOOST_CHECK(traj.max() == 12.);
+
+  ContactPhase cpm1 = ContactPhase(-2., 0.);
+  ContactSequence cs2 = ContactSequence(0);
+
+  cs2.append(cpm1);
+  cs2.append(cp0);
+  cs2.append(cp1);
+  cs2.append(cp2);
+  traj = cs2.concatenateEffectorTrajectories("right_leg");
+  BOOST_CHECK(traj.min() == -2.);
+  BOOST_CHECK(traj.max() == 8.);
+
+  cs2.append(cp3);
+  traj = cs2.concatenateEffectorTrajectories("right_leg");
+  BOOST_CHECK(traj.min() == -2.);
+  BOOST_CHECK(traj.max() == 12.);
 }
 
 BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_force_traj) {