Unverified Commit 026241bc authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #9 from pFernbach/topic/fix_concatenate_effector

fix concatenate effector
parents f7f0dda2 0e4a58ea
......@@ -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;
}
......
......@@ -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) {
......
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