contact-sequence.hpp

Go to the documentation of this file.

#ifndef __multicontact_api_scenario_contact_sequence_hpp__
#define __multicontact_api_scenario_contact_sequence_hpp__

#include <ndcurves/curve_abc.h>
#include <ndcurves/fwd.h>
#include <ndcurves/piecewise_curve.h>
#include <ndcurves/polynomial.h>
#include <ndcurves/se3_curve.h>

#include <boost/serialization/vector.hpp>
#include <vector>

#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"

namespace multicontact_api {
namespace scenario {

template <class _ContactPhase>
struct ContactSequenceTpl
 : public serialization::Serializable<ContactSequenceTpl<_ContactPhase> > {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 typedef _ContactPhase ContactPhase;
 typedef typename ContactPhase::Scalar Scalar;
 typedef typename ContactPhase::t_strings t_strings;
 typedef typename ContactPhase::SE3 SE3;
 typedef std::vector<ContactPhase> ContactPhaseVector;

 typedef ndcurves::pointX_t pointX_t;
 typedef ndcurves::transform_t transform_t;
 typedef ndcurves::curve_abc_t curve_t;
 typedef ndcurves::curve_SE3_t curve_SE3_t;
 typedef std::shared_ptr<curve_t> curve_ptr;
 typedef std::shared_ptr<curve_SE3_t> curve_SE3_ptr;
 typedef ndcurves::piecewise_t piecewise_t;
 typedef ndcurves::piecewise_SE3_t piecewise_SE3_t;
 typedef ndcurves::SE3Curve_t SE3Curve_t;
 typedef ndcurves::polynomial_t polynomial_t;

 ContactSequenceTpl(const size_t size = 0) : m_contact_phases(size) {}

 ContactSequenceTpl(const ContactSequenceTpl& other)
 : m_contact_phases(other.m_contact_phases) {}

 size_t size() const { return m_contact_phases.size(); }

 bool operator==(const ContactSequenceTpl& other) const {
 return m_contact_phases == other.m_contact_phases;
 }

 bool operator!=(const ContactSequenceTpl& other) const {
 return !(*this == other);
 }

 void resize(const size_t size) { m_contact_phases.resize(size); }

 /* Accessors to the contact Phases */

 size_t append(const ContactPhase& contactPhase) {
 m_contact_phases.push_back(contactPhase);
 return m_contact_phases.size() - 1;
 }

 const ContactPhaseVector contactPhases() const { return m_contact_phases; }

 ContactPhase& contactPhase(const size_t id) {
 if (id >= m_contact_phases.size())
 // throw std::invalid_argument("Contact Sequence size is
 // "+m_contact_phases.size()+" given Id is "+id);
 throw std::invalid_argument("Given Id is greater than the vector size");
 return m_contact_phases.at(id);
 }

 void removePhase(const size_t id) {
 if (id >= m_contact_phases.size())
 throw std::invalid_argument("Given Id is greater than the vector size");
 m_contact_phases.erase(m_contact_phases.begin() + id);
 }

 /* End Accessors to the contact Phases */

 /* Helpers */
 void breakContact(const std::string& eeName,
 const double phaseDuration = -1) {
 ContactPhase& lastPhase = m_contact_phases.back();
 if (!lastPhase.isEffectorInContact(eeName))
 throw std::invalid_argument(
 "In breakContact : effector is not currently in contact : " + eeName);
 if (phaseDuration > 0) {
 if (lastPhase.timeInitial() < 0) {
 throw std::invalid_argument(
 "In breakContact : duration is specified but current phase "
 "interval in not initialised.");
 } else {
 lastPhase.duration(phaseDuration);
 }
 }
 ContactPhase phase;
 // set initial values from last values of previous phase :
 phase.timeInitial(lastPhase.timeFinal());
 phase.m_c_init = lastPhase.m_c_final;
 phase.m_dc_init = lastPhase.m_dc_final;
 phase.m_ddc_init = lastPhase.m_ddc_final;
 phase.m_L_init = lastPhase.m_L_final;
 phase.m_dL_init = lastPhase.m_dL_final;
 phase.m_q_init = lastPhase.m_q_final;

 // copy contact patchs :
 for (std::string name : lastPhase.effectorsInContact()) {
 if (name != eeName) {
 phase.addContact(name, lastPhase.contactPatch(name));
 }
 }
 // add new phase at the end of the sequence
 append(phase);
 return;
 }

 void createContact(const std::string& eeName, const ContactPatch& patch,
 const double phaseDuration = -1) {
 ContactPhase& lastPhase = m_contact_phases.back();
 if (lastPhase.isEffectorInContact(eeName))
 throw std::invalid_argument(
 "In createContact : effector is already in contact : " + eeName);
 if (phaseDuration > 0) {
 if (lastPhase.timeInitial() < 0) {
 throw std::invalid_argument(
 "In createContact : duration is specified but current phase "
 "interval in not initialised.");
 } else {
 lastPhase.duration(phaseDuration);
 }
 }
 ContactPhase phase;
 // set initial values from last values of previous phase :
 phase.timeInitial(lastPhase.timeFinal());
 phase.m_c_init = lastPhase.m_c_final;
 phase.m_dc_init = lastPhase.m_dc_final;
 phase.m_ddc_init = lastPhase.m_ddc_final;
 phase.m_L_init = lastPhase.m_L_final;
 phase.m_dL_init = lastPhase.m_dL_final;
 phase.m_q_init = lastPhase.m_q_final;

 // copy contact patchs :
 for (std::string name : lastPhase.effectorsInContact()) {
 phase.addContact(name, lastPhase.contactPatch(name));
 }
 // add new contact to new phase :
 phase.addContact(eeName, patch);
 // add new phase at the end of the sequence
 append(phase);
 return;
 }

 void moveEffectorToPlacement(const std::string& eeName,
 const ContactPatch::SE3& placement,
 const double durationBreak = -1,
 const double durationCreate = -1) {
 if (!m_contact_phases.back().isEffectorInContact(eeName))
 throw std::invalid_argument(
 "In moveEffectorToPlacement : effector is not currently in contact "
 ": " +
 eeName);
 ContactPatch target(m_contact_phases.back().contactPatch(eeName));
 target.placement() = placement;
 breakContact(eeName, durationBreak);
 // copy all "init" value to "final" for the current last phase :
 ContactPhase& lastPhase = m_contact_phases.back();
 lastPhase.m_c_final = lastPhase.m_c_init;
 lastPhase.m_dc_final = lastPhase.m_dc_init;
 lastPhase.m_ddc_final = lastPhase.m_ddc_init;
 lastPhase.m_L_final = lastPhase.m_L_init;
 lastPhase.m_dL_final = lastPhase.m_dL_init;
 lastPhase.m_q_final = lastPhase.m_q_init;
 createContact(eeName, target, durationCreate);
 return;
 }

 void moveEffectorOf(const std::string& eeName,
 const ContactPatch::SE3& transform,
 const double durationBreak = -1,
 const double durationCreate = -1) {
 if (!m_contact_phases.back().isEffectorInContact(eeName))
 throw std::invalid_argument(
 "In moveEffectorToPlacement : effector is not currently in contact "
 ": " +
 eeName);
 ContactPatch::SE3 previous(
 m_contact_phases.back().contactPatch(eeName).placement());
 ContactPatch::SE3 target = transform.act(previous);
 return moveEffectorToPlacement(eeName, target, durationBreak,
 durationCreate);
 }

 bool haveTimings() const {
 double current_t = m_contact_phases.front().timeInitial();
 if (current_t < 0.) {
 std::cout << "Initial time is negative." << std::endl;
 return false;
 }
 for (size_t i = 0; i < size(); ++i) {
 const ContactPhase& phase = m_contact_phases.at(i);
 if (phase.timeInitial() != current_t) {
 std::cout << "For phase " << i
 << " initial time is not equal to previous final time."
 << std::endl;
 return false;
 }
 if (phase.timeInitial() > phase.timeFinal()) {
 std::cout << "For phase " << i << " final time is before initial time."
 << std::endl;
 return false;
 }
 current_t = phase.timeFinal();
 }
 return true;
 }

 bool haveConsistentContacts() const {
 size_t variations;
 if (m_contact_phases.front().numContacts() == 0) {
 // FIXME : may want to test this in a separate method in the future
 std::cout << "Phase without any contact at id : 0" << std::endl;
 return false;
 }
 for (size_t i = 1; i < m_contact_phases.size(); ++i) {
 variations = m_contact_phases.at(i - 1)
 .getContactsBroken(m_contact_phases.at(i))
 .size() +
 m_contact_phases.at(i - 1)
 .getContactsCreated(m_contact_phases.at(i))
 .size();
 if (variations > 1) {
 std::cout << "Several contact changes between adjacents phases at id : "
 << i << std::endl;
 return false;
 }
 if (m_contact_phases.at(i - 1)
 .getContactsRepositioned(m_contact_phases.at(i))
 .size() > 0) {
 std::cout
 << "Contact repositionning without intermediate phase at id : " << i
 << std::endl;
 return false;
 }
 if (m_contact_phases.at(i).numContacts() == 0) {
 // FIXME : may want to test this in a separate method in the future
 std::cout << "Phase without any contact at id : " << i << std::endl;
 return false;
 }
 if (variations == 0) {
 std::cout << "No contact change between adjacents phases at id : " << i
 << std::endl;
 return false;
 }
 }
 return true;
 }

 bool haveCOMvalues() const {
 if (m_contact_phases.front().m_c_init.isZero()) {
 std::cout << "Initial CoM position not defined." << std::endl;
 return false;
 }
 for (size_t i = 1; i < m_contact_phases.size(); ++i) {
 if (m_contact_phases.at(i).m_c_init.isZero()) {
 std::cout << "Intermediate CoM position not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (m_contact_phases.at(i).m_c_init !=
 m_contact_phases.at(i - 1).m_c_final) {
 std::cout << "Init CoM position do not match final CoM of previous "
 "phase for id : "
 << i << std::endl;
 return false;
 }
 if (!m_contact_phases.at(i).m_dc_init.isZero()) {
 if (m_contact_phases.at(i).m_dc_init !=
 m_contact_phases.at(i - 1).m_dc_final) {
 std::cout << "Init CoM velocity do not match final velocity of "
 "previous phase for id : "
 << i << std::endl;
 return false;
 }
 }
 if (!m_contact_phases.at(i).m_ddc_init.isZero()) {
 if (m_contact_phases.at(i).m_ddc_init !=
 m_contact_phases.at(i - 1).m_ddc_final) {
 std::cout << "Init CoM acceleration do not match final acceleration "
 "of previous phase for id : "
 << i << std::endl;
 return false;
 }
 }
 }
 if (m_contact_phases.back().m_c_final.isZero()) {
 std::cout << "Final CoM position not defined." << std::endl;
 return false;
 }
 return true;
 }

 bool haveAMvalues() const {
 for (size_t i = 1; i < m_contact_phases.size(); ++i) {
 if (m_contact_phases.at(i).m_L_init !=
 m_contact_phases.at(i - 1).m_L_final) {
 std::cout << "Init AM value do not match final value of previous phase "
 "for id : "
 << i << std::endl;
 return false;
 }
 if (m_contact_phases.at(i).m_dL_init !=
 m_contact_phases.at(i - 1).m_dL_final) {
 std::cout << "Init AM derivative do not match final AM derivative of "
 "previous phase for id : "
 << i << std::endl;
 return false;
 }
 }
 return true;
 }

 bool haveCentroidalValues() const {
 return haveAMvalues() && haveCOMvalues();
 }

 bool haveConfigurationsValues() const {
 if (m_contact_phases.front().m_q_init.isZero()) {
 std::cout << "Initial configuration not defined." << std::endl;
 return false;
 }
 for (size_t i = 1; i < m_contact_phases.size(); ++i) {
 if (m_contact_phases.at(i).m_q_init.isZero()) {
 std::cout << "Intermediate configuration not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (m_contact_phases.at(i).m_q_init !=
 m_contact_phases.at(i - 1).m_q_final) {
 std::cout << "Init configuration do not match final configuration of "
 "previous phase for id : "
 << i << std::endl;
 return false;
 }
 }
 if (m_contact_phases.back().m_q_final.isZero()) {
 std::cout << "Final configuration not defined." << std::endl;
 return false;
 }
 return true;
 }

 bool haveCOMtrajectories() const {
 if (!(haveTimings() && haveCOMvalues())) return false;
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (!phase.m_c) {
 std::cout << "CoM position trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (!phase.m_dc) {
 std::cout << "CoM velocity trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (!phase.m_ddc) {
 std::cout << "CoM acceleration trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_c->dim() != 3) {
 std::cout << "CoM trajectory is not of dimension 3 for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dc->dim() != 3) {
 std::cout
 << "CoM velocity trajectory is not of dimension 3 for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_ddc->dim() != 3) {
 std::cout
 << "CoM acceleration trajectory is not of dimension 3 for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_c->min() != phase.timeInitial()) {
 std::cout << "CoM trajectory do not start at t_init for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dc->min() != phase.timeInitial()) {
 std::cout
 << "CoM velocity trajectory do not start at t_init for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_ddc->min() != phase.timeInitial()) {
 std::cout
 << "CoM acceleration trajectory do not start at t_init for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_c->max() != phase.timeFinal()) {
 std::cout << "CoM trajectory do not end at t_final for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dc->max() != phase.timeFinal()) {
 std::cout
 << "CoM velocity trajectory do not end at t_final for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_ddc->max() != phase.timeFinal()) {
 std::cout
 << "CoM acceleration trajectory do not end at t_final for phase : "
 << i << std::endl;
 return false;
 }
 if (!(*phase.m_c)(phase.m_c->min()).isApprox(phase.m_c_init)) {
 std::cout << "CoM trajectory do not start at c_init for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dc_init.isZero()) {
 if (!(*phase.m_dc)(phase.m_dc->min()).isZero()) {
 std::cout
 << "CoM velocity trajectory do not start at dc_init for phase : "
 << i << std::endl;
 return false;
 }
 }
 // FIXME : isApprox do not work when values close to 0
 else if (!(*phase.m_dc)(phase.m_dc->min())
 .isApprox(phase.m_dc_init, 1e-6)) {
 std::cout
 << "CoM velocity trajectory do not start at dc_init for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_ddc_init.isZero()) {
 if (!(*phase.m_ddc)(phase.m_ddc->min()).isZero()) {
 std::cout << "CoM acceleration trajectory do not start at ddc_init "
 "for phase : "
 << i << std::endl;
 return false;
 }
 } else if (!(*phase.m_ddc)(phase.m_ddc->min())
 .isApprox(phase.m_ddc_init, 1e-6)) {
 std::cout << "CoM acceleration trajectory do not start at ddc_init for "
 "phase : "
 << i << std::endl;
 return false;
 }
 if (!(*phase.m_c)(phase.m_c->max()).isApprox(phase.m_c_final)) {
 std::cout << "CoM trajectory do not end at c_final for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dc_final.isZero()) {
 if (!(*phase.m_dc)(phase.m_dc->max()).isZero()) {
 std::cout
 << "CoM velocity trajectory do not end at dc_final for phase : "
 << i << std::endl;
 return false;
 }
 } else if (!(*phase.m_dc)(phase.m_dc->max())
 .isApprox(phase.m_dc_final, 1e-6)) {
 std::cout
 << "CoM velocity trajectory do not end at dc_final for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_ddc_final.isZero()) {
 if (!(*phase.m_ddc)(phase.m_ddc->max()).isZero()) {
 std::cout << "CoM acceleration trajectory do not end at ddc_final "
 "for phase : "
 << i << std::endl;
 return false;
 }
 } else if (!(*phase.m_ddc)(phase.m_ddc->max())
 .isApprox(phase.m_ddc_final, 1e-6)) {
 std::cout << "CoM acceleration trajectory do not end at ddc_final for "
 "phase : "
 << i << std::endl;
 return false;
 }
 ++i;
 }
 return true;
 }

 bool haveAMtrajectories() const {
 if (!(haveTimings() && haveAMvalues())) return false;
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (!phase.m_L) {
 std::cout << "AM position trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (!phase.m_dL) {
 std::cout << "AM velocity trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_L->dim() != 3) {
 std::cout << "AM trajectory is not of dimension 3 for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dL->dim() != 3) {
 std::cout
 << "AM derivative trajectory is not of dimension 3 for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_L->min() != phase.timeInitial()) {
 std::cout << "AM trajectory do not start at t_init for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dL->min() != phase.timeInitial()) {
 std::cout
 << "AM derivative trajectory do not start at t_init for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_L->max() != phase.timeFinal()) {
 std::cout << "AM trajectory do not end at t_final for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dL->max() != phase.timeFinal()) {
 std::cout
 << "AM derivative trajectory do not end at t_final for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_L_init.isZero()) {
 if (!(*phase.m_L)(phase.m_L->min()).isZero()) {
 std::cout << "AM trajectory do not start at L_init for phase : " << i
 << std::endl;
 return false;
 }
 } else if (!(*phase.m_L)(phase.m_L->min()).isApprox(phase.m_L_init)) {
 std::cout << "AM trajectory do not start at L_init for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dL_init.isZero()) {
 if (!(*phase.m_dL)(phase.m_dL->min()).isZero()) {
 std::cout
 << "AM derivative trajectory do not start at dL_init for phase : "
 << i << std::endl;
 return false;
 }
 } else if (!(*phase.m_dL)(phase.m_dL->min())
 .isApprox(phase.m_dL_init, 1e-6)) {
 std::cout
 << "AM derivative trajectory do not start at dL_init for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_L_final.isZero()) {
 if (!(*phase.m_L)(phase.m_L->max()).isZero()) {
 std::cout << "AM trajectory do not end at L_final for phase : " << i
 << std::endl;
 return false;
 }
 } else if (!(*phase.m_L)(phase.m_L->max())
 .isApprox(phase.m_L_final, 1e-6)) {
 std::cout << "AM trajectory do not end at L_final for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_dL_final.isZero()) {
 if (!(*phase.m_dL)(phase.m_dL->max()).isZero()) {
 std::cout
 << "AM derivative trajectory do not end at dL_final for phase : "
 << i << std::endl;
 return false;
 }
 } else if (!(*phase.m_dL)(phase.m_dL->max()).isApprox(phase.m_dL_final)) {
 std::cout
 << "AM derivative trajectory do not end at dL_final for phase : "
 << i << std::endl;
 return false;
 }
 ++i;
 }
 return true;
 }

 bool haveCentroidalTrajectories() const {
 return haveAMtrajectories() && haveCOMtrajectories();
 }

 bool haveEffectorsTrajectories(
 const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(),
 const bool use_rotation = true) const {
 if (!haveTimings()) return false;
 for (size_t i = 0; i < m_contact_phases.size() - 1; ++i) {
 for (std::string eeName : m_contact_phases.at(i).getContactsCreated(
 m_contact_phases.at(i + 1))) {
 if (!m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
 std::cout << "No end effector trajectory for " << eeName
 << " at phase " << i << " but it is in contact at phase "
 << i + 1 << std::endl;
 return false;
 }
 const typename ContactPhase::curve_SE3_ptr traj =
 m_contact_phases.at(i).effectorTrajectories().at(eeName);
 if (traj->min() != m_contact_phases.at(i).timeInitial()) {
 std::cout << "Effector trajectory for " << eeName
 << " do not start at t_init in phase " << i << std::endl;
 return false;
 }
 if (traj->max() != m_contact_phases.at(i).timeFinal()) {
 std::cout << "Effector trajectory for " << eeName
 << " do not end at t_final in phase " << i << std::endl;
 return false;
 }
 ContactPatch::SE3 pTrajMax =
 ContactPatch::SE3((*traj)(traj->max()).matrix());
 ContactPatch::SE3 pPhaseMax =
 m_contact_phases.at(i + 1).contactPatches().at(eeName).placement();
 if ((use_rotation &&
 !(pTrajMax.toHomogeneousMatrix() - pPhaseMax.toHomogeneousMatrix())
 .isMuchSmallerThan(1.0, prec)) ||
 !(pTrajMax.translation() - pPhaseMax.translation())
 .isMuchSmallerThan(1.0, prec)) {
 std::cout << "Effector trajectory for " << eeName
 << " do not end at it's contact placement in the next "
 "phase, for phase "
 << i << std::endl;
 std::cout << "Last point : " << std::endl
 << pTrajMax << std::endl
 << "Next contact : " << std::endl
 << pPhaseMax << std::endl;
 return false;
 }
 if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) {
 ContactPatch::SE3 pTrajMin =
 ContactPatch::SE3((*traj)(traj->min()).matrix());
 ContactPatch::SE3 pPhaseMin = m_contact_phases.at(i - 1)
 .contactPatches()
 .at(eeName)
 .placement();
 if ((use_rotation && !(pTrajMin.toHomogeneousMatrix() -
 pPhaseMin.toHomogeneousMatrix())
 .isMuchSmallerThan(1.0, prec)) ||
 !(pTrajMin.translation() - pPhaseMin.translation())
 .isMuchSmallerThan(1.0, prec)) {
 std::cout << "Effector trajectory for " << eeName
 << " do not start at it's contact placement in the "
 "previous phase, for phase "
 << i << std::endl;
 std::cout << "First point : " << std::endl
 << pTrajMin << std::endl
 << "Previous contact : " << std::endl
 << pPhaseMin << std::endl;
 return false;
 }
 }
 }
 }
 return true;
 }

 bool haveJointsTrajectories() const {
 if (!(haveTimings() && haveConfigurationsValues())) return false;
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (!phase.m_q) {
 std::cout << "joint position trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_q->min() != phase.timeInitial()) {
 std::cout << "joint trajectory do not start at t_init for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_q->max() != phase.timeFinal()) {
 std::cout << "joint trajectory do not end at t_final for phase : " << i
 << std::endl;
 return false;
 }
 if (!(*phase.m_q)(phase.m_q->min()).isApprox(phase.m_q_init)) {
 std::cout << "joint trajectory do not start at q_init for phase : " << i
 << std::endl;
 return false;
 }
 if (!(*phase.m_q)(phase.m_q->max()).isApprox(phase.m_q_final)) {
 std::cout << "joint trajectory do not end at q_final for phase : " << i
 << std::endl;
 return false;
 }
 ++i;
 }
 return true;
 }

 bool haveJointsDerivativesTrajectories() const {
 if (!(haveTimings())) return false;
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (!phase.m_dq) {
 std::cout << "joint velocity trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (!phase.m_ddq) {
 std::cout << "joint acceleration trajectory not defined for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_dq->min() != phase.timeInitial()) {
 std::cout
 << "joint velocity trajectory do not start at t_init for phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_ddq->min() != phase.timeInitial()) {
 std::cout << "joint acceleration trajectory do not start at t_init for "
 "phase : "
 << i << std::endl;
 return false;
 }
 if (phase.m_dq->max() != phase.timeFinal()) {
 std::cout
 << "joint velocity trajectory do not end at t_final for phase : "
 << i << std::endl;
 return false;
 }
 if ((phase.m_ddq->max() != phase.timeFinal()) &&
 i < size() - 1) { // not required for the last phase
 std::cout << "joint acceleration trajectory do not end at t_final for "
 "phase : "
 << i << std::endl;
 return false;
 }
 ++i;
 }
 return true;
 }

 bool haveTorquesTrajectories() const {
 if (!haveTimings()) return false;
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (!phase.m_tau) {
 std::cout << "Torque trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }

 if (phase.m_tau->min() != phase.timeInitial()) {
 std::cout << "Torque trajectory do not start at t_init for phase : "
 << i << std::endl;
 return false;
 }
 if ((phase.m_tau->max() != phase.timeFinal()) && i < size() - 1) {
 std::cout << "Torque trajectory do not end at t_final for phase : " << i
 << std::endl;
 return false;
 }
 ++i;
 }
 return true;
 }

 bool haveContactForcesTrajectories() const {
 if (!haveTimings()) return false;
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 for (std::string eeName : phase.effectorsInContact()) {
 if (phase.contactForces().count(eeName) == 0) {
 std::cout << "No contact forces trajectory for effector " << eeName
 << " at phase " << i << std::endl;
 return false;
 }
 if (phase.contactNormalForces().count(eeName) == 0) {
 std::cout << "No contact normal force trajectory for effector "
 << eeName << " for phase " << i << std::endl;
 return false;
 }
 if (phase.contactForces().at(eeName)->min() != phase.timeInitial()) {
 std::cout << "Contact forces trajectory for effector " << eeName
 << " do not start at t_init for phase " << i << std::endl;
 return false;
 }
 if (phase.contactForces().at(eeName)->max() != phase.timeFinal()) {
 std::cout << "Contact forces trajectory for effector " << eeName
 << " do not end at t_final for phase " << i << std::endl;
 return false;
 }
 if (phase.contactNormalForces().at(eeName)->min() !=
 phase.timeInitial()) {
 std::cout << "Contact normal force trajectory for effector " << eeName
 << " do not start at t_init for phase " << i << std::endl;
 return false;
 }
 if (phase.contactNormalForces().at(eeName)->max() !=
 phase.timeFinal()) {
 std::cout << "Contact normal force trajectory for effector " << eeName
 << " do not end at t_final for phase " << i << std::endl;
 return false;
 }
 }
 ++i;
 }
 return true;
 }

 bool haveRootTrajectories() const {
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (!phase.m_root) {
 std::cout << "Root trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_root->min() != phase.timeInitial()) {
 std::cout << "Root trajectory do not start at t_init for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_root->max() != phase.timeFinal()) {
 std::cout << "Root trajectory do not start at t_final for phase : " << i
 << std::endl;
 return false;
 }
 ++i;
 }
 return true;
 }

 bool haveFriction() const {
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 for (const std::string& eeName : phase.effectorsInContact()) {
 if (phase.contactPatches().at(eeName).friction() <= 0.) {
 std::cout << "Friction not defined for phase " << i
 << " and effector " << eeName << std::endl;
 return false;
 }
 }
 ++i;
 }
 return true;
 }

 bool haveContactModelDefined() const {
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 for (const std::string& eeName : phase.effectorsInContact()) {
 if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type ==
 ContactType::CONTACT_UNDEFINED) {
 std::cout << "ContactModel not defined for phase " << i
 << " and effector " << eeName << std::endl;
 return false;
 }
 }
 ++i;
 }
 return true;
 }

 bool haveZMPtrajectories() {
 size_t i = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (!phase.m_zmp) {
 std::cout << "ZMP trajectory not defined for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_zmp->dim() != 3) {
 std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_zmp->min() != phase.timeInitial()) {
 std::cout << "ZMP trajectory do not start at t_init for phase : " << i
 << std::endl;
 return false;
 }
 if (phase.m_zmp->max() != phase.timeFinal()) {
 std::cout << "ZMP trajectory do not end at t_final for phase : " << i
 << std::endl;
 return false;
 }
 ++i;
 }
 return true;
 }

 t_strings getAllEffectorsInContact() const {
 // use set to guarantee uniqueness, but return a vector for easier use and
 // python bindings
 std::set<std::string> res_set;
 for (const ContactPhase& phase : m_contact_phases) {
 for (const std::string& eeName : phase.effectorsInContact()) {
 res_set.insert(eeName);
 }
 }
 return t_strings(res_set.begin(), res_set.end());
 }

 piecewise_t concatenateCtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_c);
 }
 return res;
 }

 piecewise_t concatenateDCtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_dc);
 }
 return res;
 }

 piecewise_t concatenateDDCtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_ddc);
 }
 return res;
 }

 piecewise_t concatenateLtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_L);
 }
 return res;
 }

 piecewise_t concatenateDLtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_dL);
 }
 return res;
 }

 piecewise_t concatenateZMPtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_zmp);
 }
 return res;
 }

 piecewise_t concatenateWrenchTrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_wrench);
 }
 return res;
 }

 piecewise_t concatenateQtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_q);
 }
 return res;
 }

 piecewise_t concatenateDQtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_dq);
 }
 return res;
 }

 piecewise_t concatenateDDQtrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_ddq);
 }
 return res;
 }

 piecewise_t concatenateTauTrajectories() const {
 piecewise_t res = piecewise_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_tau);
 }
 return res;
 }

 piecewise_SE3_t concatenateRootTrajectories() const {
 piecewise_SE3_t res = piecewise_SE3_t();
 for (const ContactPhase& phase : m_contact_phases) {
 res.add_curve_ptr(phase.m_root);
 }
 return res;
 }

 piecewise_SE3_t concatenateEffectorTrajectories(
 const std::string& eeName) const {
 piecewise_SE3_t res = piecewise_SE3_t();
 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;
 for (size_t i = 0; i < m_contact_phases.size(); ++i) {
 if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
 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)) {
 res.add_curve_ptr(
 m_contact_phases.at(i).effectorTrajectories().at(eeName));
 last_placement = res(res.max());
 } else {
 // when the trajectory do not exist, add a constant one with the
 // previous value
 curve_SE3_ptr ptr(new SE3Curve_t(last_placement, last_placement,
 m_contact_phases.at(i).timeInitial(),
 m_contact_phases.at(i).timeFinal()));
 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;
 }

 piecewise_t concatenateContactForceTrajectories(
 const std::string& eeName) const {
 piecewise_t res = piecewise_t();
 // first find the dimension used, in order to fill with 0 when required:
 size_t dim = 0;
 for (const ContactPhase& phase : m_contact_phases) {
 if (phase.contactForces().count(eeName) > 0) {
 dim = phase.contactForces().at(eeName)->dim();
 }
 }
 if (dim == 0) {
 // no forces trajectory for this effector
 return res;
 }
 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(dim, 1);
 // loop over all phases and either add trajectory if it exist or add zeros
 for (const ContactPhase& phase : m_contact_phases) {
 if (phase.contactForces().count(eeName) > 0) {
 res.add_curve_ptr(phase.contactForces().at(eeName));
 } else {
 curve_ptr ptr(
 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
 res.add_curve_ptr(ptr);
 }
 }
 return res;
 }

 piecewise_t concatenateNormalForceTrajectories(
 const std::string& eeName) const {
 piecewise_t res = piecewise_t();
 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(1, 1);
 // loop over all phases and either add trajectory if it exist or add zeros
 for (const ContactPhase& phase : m_contact_phases) {
 if (phase.contactNormalForces().count(eeName) > 0) {
 res.add_curve_ptr(phase.contactNormalForces().at(eeName));
 } else {
 curve_ptr ptr(
 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
 res.add_curve_ptr(ptr);
 }
 }
 return res;
 }

 ContactPhase& phaseAtTime(const double time) {
 const int id = phaseIdAtTime(time);
 if (id >= 0)
 return m_contact_phases.at(id);
 else
 throw std::invalid_argument("No phase found for the given time.");
 }

 int phaseIdAtTime(const double time) const {
 for (int i = 0; i < static_cast<int>(m_contact_phases.size()); ++i) {
 const ContactPhase& phase = m_contact_phases[i];
 if (time >= phase.timeInitial() && time < phase.timeFinal()) {
 return i;
 }
 }
 if (time == m_contact_phases.back().timeFinal())
 return static_cast<int>(m_contact_phases.size() - 1);
 return -1;
 }

 /* End Helpers */

 /*Public Attributes*/
 ContactPhaseVector m_contact_phases;
 /*Public Attributes*/

 private:
 // Serialization of the class
 friend class boost::serialization::access;

 template <class Archive>
 void save(Archive& ar, const unsigned int /*version*/) const {
 const size_t m_size = size();
 ar& boost::serialization::make_nvp("size", m_size);
 for (typename ContactPhaseVector::const_iterator it =
 m_contact_phases.begin();
 it != m_contact_phases.end(); ++it) {
 ar& boost::serialization::make_nvp("contact_phase", *it);
 }
 }

 template <class Archive>
 void load(Archive& ar, const unsigned int /*version*/) {
 size_t m_size;
 ar >> boost::serialization::make_nvp("size", m_size);
 assert(m_size > 0);
 resize(m_size);
 for (typename ContactPhaseVector::iterator it = m_contact_phases.begin();
 it != m_contact_phases.end(); ++it) {
 ar >> boost::serialization::make_nvp("contact_phase", *it);
 }
 }

 BOOST_SERIALIZATION_SPLIT_MEMBER()

}; // end class ContactSequence

} // namespace scenario
} // namespace multicontact_api

MULTICONTACT_API_DEFINE_CLASS_TEMPLATE_VERSION(
 typename Scalar, multicontact_api::scenario::ContactSequenceTpl<Scalar>)

#endif // __multicontact_api_scenario_contact_sequence_hpp__