#include <multicontact-api/scenario/contact-sequence.hpp>
Public Types | |
typedef ContactPhase::Scalar | Scalar |
typedef ContactPhase::t_strings | t_strings |
typedef 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 |
Public Member Functions | |
ContactSequenceTpl (const size_t size=0) | |
ContactSequenceTpl (const ContactSequenceTpl &other) | |
Copy contructor. | |
size_t | size () const |
bool | operator== (const ContactSequenceTpl &other) const |
bool | operator!= (const ContactSequenceTpl &other) const |
void | resize (const size_t size) |
size_t | append (const ContactPhase &contactPhase) |
append Add the given Phase at the end of the sequence | |
const ContactPhaseVector | contactPhases () const |
contactPhases return a Const copy of the contact phase vector in this sequence. Prefer accessing the contact phases through the contactPhase(id) as this one create a copy | |
ContactPhase & | contactPhase (const size_t id) |
contactPhase return a reference to the contactPhase stored at the given Id | |
void | removePhase (const size_t id) |
removePhase remove the given contactPhase from the sequence | |
void | breakContact (const std::string &eeName, const double phaseDuration=-1) |
breakContact Add a new contactPhase at the end of the current ContactSequence, The new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact removed. It copy all the 'final' values of the last phase as 'initial' values of the new phase. It also set the duration of the previous last phase. | |
void | createContact (const std::string &eeName, const ContactPatch &patch, const double phaseDuration=-1) |
createContact Add a new contactPhase at the end of the current ContactSequence, The new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact added. | |
void | moveEffectorToPlacement (const std::string &eeName, const ContactPatch::SE3 &placement, const double durationBreak=-1, const double durationCreate=-1) |
moveEffectorToPlacement Add two new phases at the end of the current ContactSequence, | |
void | moveEffectorOf (const std::string &eeName, const ContactPatch::SE3 &transform, const double durationBreak=-1, const double durationCreate=-1) |
moveEffectorOf similar to moveEffectorToPlacement exept that the new placement is defined from the previous placement and a given transform applied. | |
bool | haveTimings () const |
haveTimings Check if all the time intervals are defined and consistent (ie. the time always increase and the final time of one phase is equal to the initial one of the newt phase) | |
bool | haveConsistentContacts () const |
haveConsistentContacts check that there is always one contact change between adjacent phases in the sequence and that there isn't any phase without any contact. | |
bool | haveCOMvalues () const |
haveCOMvalues Check that the initial and final CoM position values are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones. | |
bool | haveAMvalues () const |
haveAMvalues Check that the initial and final AM values are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones. | |
bool | haveCentroidalValues () const |
haveCentroidalValues Check that the initial and final CoM position and AM values are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones. | |
bool | haveConfigurationsValues () const |
haveConfigurationsValues Check that the initial and final configuration are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones. | |
bool | haveCOMtrajectories () const |
haveCOMtrajectories check that a c, dc and ddc trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase | |
bool | haveAMtrajectories () const |
haveAMtrajectories check that a L and dL trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase | |
bool | haveCentroidalTrajectories () const |
haveCentroidalTrajectories check that all centroidal trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase | |
bool | haveEffectorsTrajectories (const Scalar prec=Eigen::NumTraits< Scalar >::dummy_precision(), const bool use_rotation=true) const |
haveEffectorsTrajectories check that for each phase preceeding a contact creation, an SE3 trajectory is defined for the effector that will be in contact. Also check that this trajectory is defined on the time-interval of the phase. Also check that the trajectory correctly end at the placement defined for the contact in the next phase. If this effector was in contact in the previous phase, it check that the trajectory start at the previous contact placement. | |
bool | haveJointsTrajectories () const |
haveJointsTrajectories Check that a q trajectory is defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase | |
bool | haveJointsDerivativesTrajectories () const |
haveJointsDerivativesTrajectories Check that a dq and ddq trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase | |
bool | haveTorquesTrajectories () const |
haveJointsTrajectories Check that a joint torque trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase | |
bool | haveContactForcesTrajectories () const |
haveJointsTrajectories Check that a contact force trajectory exist for each active contact Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase | |
bool | haveRootTrajectories () const |
haveRootTrajectories check that a root trajectory exist for each contact phases. Also check that it start and end at the correct time interval | |
bool | haveFriction () const |
haveFriction check that all the contact patch used in the sequence have a friction coefficient initialized | |
bool | haveContactModelDefined () const |
haveContactModelDefined check that all the contact patch have a contact_model defined | |
bool | haveZMPtrajectories () |
haveZMPtrajectories check that all the contact phases have a zmp trajectory | |
t_strings | getAllEffectorsInContact () const |
getAllEffectorsInContact return a vector of names of all the effectors used to create contacts during the sequence | |
piecewise_t | concatenateCtrajectories () const |
concatenateCtrajectories Return a piecewise curve which is the concatenation of the m_c curves for each contact phases in the sequence. | |
piecewise_t | concatenateDCtrajectories () const |
concatenateDCtrajectories Return a piecewise curve which is the concatenation of the m_dc curves for each contact phases in the sequence. | |
piecewise_t | concatenateDDCtrajectories () const |
concatenateDDCtrajectories Return a piecewise curve which is the concatenation of the m_ddc curves for each contact phases in the sequence. | |
piecewise_t | concatenateLtrajectories () const |
concatenateLtrajectories Return a piecewise curve which is the concatenation of the m_L curves for each contact phases in the sequence. | |
piecewise_t | concatenateDLtrajectories () const |
concatenateDLtrajectories Return a piecewise curve which is the concatenation of the m_dL curves for each contact phases in the sequence. | |
piecewise_t | concatenateZMPtrajectories () const |
concatenateZMPtrajectories Return a piecewise curve which is the concatenation of the m_zmp curves for each contact phases in the sequence. | |
piecewise_t | concatenateWrenchTrajectories () const |
concatenateWrenchTrajectories Return a piecewise curve which is the concatenation of the m_wrench curves for each contact phases in the sequence. | |
piecewise_t | concatenateQtrajectories () const |
concatenateQtrajectories Return a piecewise curve which is the concatenation of the m_q curves for each contact phases in the sequence. | |
piecewise_t | concatenateDQtrajectories () const |
concatenateDQtrajectories Return a piecewise curve which is the concatenation of the m_dq curves for each contact phases in the sequence. | |
piecewise_t | concatenateDDQtrajectories () const |
concatenateDDQtrajectories Return a piecewise curve which is the concatenation of the m_ddq curves for each contact phases in the sequence. | |
piecewise_t | concatenateTauTrajectories () const |
concatenateTauTrajectories Return a piecewise curve which is the concatenation of the m_tau curves for each contact phases in the sequence. | |
piecewise_SE3_t | concatenateRootTrajectories () const |
concatenateRootTrajectories Return a piecewise curve which is the concatenation of the m_root curves for each contact phases in the sequence. | |
piecewise_SE3_t | concatenateEffectorTrajectories (const std::string &eeName) const |
concatenateEffectorTrajectories Return a piecewise curve which is the concatenation of the effectors trajectories curves for the given effector for each contact phases in the sequence. During the phases where no effector trajectories are defined, the trajectory is constant with the value of the last phase where it was defined. | |
piecewise_t | concatenateContactForceTrajectories (const std::string &eeName) const |
concatenateContactForceTrajectories Return a piecewise curve which is the concatenation of the contact forces for the given effector for each contact phases in the sequence. During the phases where no contact forces are defined, the trajectory is constant with the value of 0. | |
piecewise_t | concatenateNormalForceTrajectories (const std::string &eeName) const |
concatenateNormalForceTrajectories Return a piecewise curve which is the concatenation of the contact normal forces for the given effector for each contact phases in the sequence. During the phases where no contact normal forces are defined, the trajectory is constant with the value of 0. | |
ContactPhase & | phaseAtTime (const double time) |
phaseAtTime return a phase of the sequence such that : phase.timeInitial <= t < phase.timeFinal if t equal to the last phase timeFinal, this index is returned | |
int | phaseIdAtTime (const double time) const |
phaseIdAtTime return the index of a phase in the sequence such that : phase.timeInitial <= t < phase.timeFinal if t equal to the last phase timeFinal, this index is returned | |
![]() | |
void | loadFromText (const std::string &filename) |
Loads a Derived object from a text file. | |
void | saveAsText (const std::string &filename) const |
Saved a Derived object as a text file. | |
void | loadFromXML (const std::string &filename, const std::string &tag_name) |
Loads a Derived object from an XML file. | |
void | saveAsXML (const std::string &filename, const std::string &tag_name) const |
Saved a Derived object as an XML file. | |
void | loadFromBinary (const std::string &filename) |
Loads a Derived object from an binary file. | |
void | saveAsBinary (const std::string &filename) const |
Saved a Derived object as an binary file. | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _ContactPhase | ContactPhase |
ContactPhaseVector | m_contact_phases |
Friends | |
class | boost::serialization::access |
typedef std::vector<ContactPhase> multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::ContactPhaseVector |
typedef std::shared_ptr<curve_t> multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::curve_ptr |
typedef std::shared_ptr<curve_SE3_t> multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::curve_SE3_ptr |
typedef ndcurves::curve_SE3_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::curve_SE3_t |
typedef ndcurves::curve_abc_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::curve_t |
typedef ndcurves::piecewise_SE3_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::piecewise_SE3_t |
typedef ndcurves::piecewise_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::piecewise_t |
typedef ndcurves::pointX_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::pointX_t |
typedef ndcurves::polynomial_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::polynomial_t |
typedef ContactPhase::Scalar multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::Scalar |
typedef ContactPhase::SE3 multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::SE3 |
typedef ndcurves::SE3Curve_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::SE3Curve_t |
typedef ContactPhase::t_strings multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::t_strings |
typedef ndcurves::transform_t multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::transform_t |
|
inline |
|
inline |
Copy contructor.
|
inline |
append Add the given Phase at the end of the sequence
contactPhase | the phase to end |
|
inline |
breakContact Add a new contactPhase at the end of the current ContactSequence, The new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact removed. It copy all the 'final' values of the last phase as 'initial' values of the new phase. It also set the duration of the previous last phase.
eeName | the name of the effector to remove from contact |
phaseDuration | if provided, the duration of the previous last phase of the sequence is set to this value (it is thus the duration BEFORE breaking the contact) |
invalid_argument | if the phaseDuration is provided but the last phase do not have a time-range defined |
invalid_argument | if eeName is not in contact in the last phase of the sequence |
|
inline |
concatenateContactForceTrajectories Return a piecewise curve which is the concatenation of the contact forces for the given effector for each contact phases in the sequence. During the phases where no contact forces are defined, the trajectory is constant with the value of 0.
eeName | the effector name |
|
inline |
concatenateCtrajectories Return a piecewise curve which is the concatenation of the m_c curves for each contact phases in the sequence.
|
inline |
concatenateDCtrajectories Return a piecewise curve which is the concatenation of the m_dc curves for each contact phases in the sequence.
|
inline |
concatenateDDCtrajectories Return a piecewise curve which is the concatenation of the m_ddc curves for each contact phases in the sequence.
|
inline |
concatenateDDQtrajectories Return a piecewise curve which is the concatenation of the m_ddq curves for each contact phases in the sequence.
|
inline |
concatenateDLtrajectories Return a piecewise curve which is the concatenation of the m_dL curves for each contact phases in the sequence.
|
inline |
concatenateDQtrajectories Return a piecewise curve which is the concatenation of the m_dq curves for each contact phases in the sequence.
|
inline |
concatenateEffectorTrajectories Return a piecewise curve which is the concatenation of the effectors trajectories curves for the given effector for each contact phases in the sequence. During the phases where no effector trajectories are defined, the trajectory is constant with the value of the last phase where it was defined.
eeName | the effector name |
|
inline |
concatenateLtrajectories Return a piecewise curve which is the concatenation of the m_L curves for each contact phases in the sequence.
|
inline |
concatenateNormalForceTrajectories Return a piecewise curve which is the concatenation of the contact normal forces for the given effector for each contact phases in the sequence. During the phases where no contact normal forces are defined, the trajectory is constant with the value of 0.
eeName | the effector name |
|
inline |
concatenateQtrajectories Return a piecewise curve which is the concatenation of the m_q curves for each contact phases in the sequence.
|
inline |
concatenateRootTrajectories Return a piecewise curve which is the concatenation of the m_root curves for each contact phases in the sequence.
|
inline |
concatenateTauTrajectories Return a piecewise curve which is the concatenation of the m_tau curves for each contact phases in the sequence.
|
inline |
concatenateWrenchTrajectories Return a piecewise curve which is the concatenation of the m_wrench curves for each contact phases in the sequence.
|
inline |
concatenateZMPtrajectories Return a piecewise curve which is the concatenation of the m_zmp curves for each contact phases in the sequence.
|
inline |
contactPhase return a reference to the contactPhase stored at the given Id
id | the desired Id in the contact sequence |
|
inline |
contactPhases return a Const copy of the contact phase vector in this sequence. Prefer accessing the contact phases through the contactPhase(id) as this one create a copy
|
inline |
createContact Add a new contactPhase at the end of the current ContactSequence, The new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact added.
eeName | the name of the effector used to create contact |
patch | the ContactPatch of the new contact |
phaseDuration | if provided, the duration of the previous last phase of the sequence is set to this value (it is thus the duration BEFORE creating the contact) |
invalid_argument | if the phaseDuration is provided but the last phase do not have a time-range defined |
invalid_argument | if eeName is already in contact in the last phase of the sequence |
|
inline |
getAllEffectorsInContact return a vector of names of all the effectors used to create contacts during the sequence
|
inline |
haveAMtrajectories check that a L and dL trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase
|
inline |
haveAMvalues Check that the initial and final AM values are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones.
|
inline |
haveCentroidalTrajectories check that all centroidal trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase
|
inline |
haveCentroidalValues Check that the initial and final CoM position and AM values are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones.
|
inline |
haveCOMtrajectories check that a c, dc and ddc trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase
|
inline |
haveCOMvalues Check that the initial and final CoM position values are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones.
|
inline |
haveConfigurationsValues Check that the initial and final configuration are defined for all phases Also check that the initial values of one phase correspond to the final values of the previous ones.
|
inline |
haveConsistentContacts check that there is always one contact change between adjacent phases in the sequence and that there isn't any phase without any contact.
|
inline |
haveJointsTrajectories Check that a contact force trajectory exist for each active contact Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase
|
inline |
haveContactModelDefined check that all the contact patch have a contact_model defined
|
inline |
haveEffectorsTrajectories check that for each phase preceeding a contact creation, an SE3 trajectory is defined for the effector that will be in contact. Also check that this trajectory is defined on the time-interval of the phase. Also check that the trajectory correctly end at the placement defined for the contact in the next phase. If this effector was in contact in the previous phase, it check that the trajectory start at the previous contact placement.
|
inline |
haveFriction check that all the contact patch used in the sequence have a friction coefficient initialized
|
inline |
haveJointsDerivativesTrajectories Check that a dq and ddq trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase
|
inline |
haveJointsTrajectories Check that a q trajectory is defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase
|
inline |
haveRootTrajectories check that a root trajectory exist for each contact phases. Also check that it start and end at the correct time interval
|
inline |
haveTimings Check if all the time intervals are defined and consistent (ie. the time always increase and the final time of one phase is equal to the initial one of the newt phase)
|
inline |
haveJointsTrajectories Check that a joint torque trajectories are defined for each phases Also check that the time interval of this trajectories matches the one of the phase and that the trajectories start and end and the correct values defined in each phase
|
inline |
haveZMPtrajectories check that all the contact phases have a zmp trajectory
|
inline |
moveEffectorOf similar to moveEffectorToPlacement exept that the new placement is defined from the previous placement and a given transform applied.
eeName | the name of the effector used to create contact |
transform | the transform applied to the placement of the contact in the last phase of the sequence |
durationBreak | the duration of the previous last phase of the sequence (it is thus the duration BEFORE breaking the contact) |
durationCreate | the duration of the first new ContactPhase (it is thus the duration BEFORE creating the contact) |
invalid_argument | if the phaseDuration is provided but the last phase do not have a time-range defined |
invalid_argument | if eeName is not in contact in the last phase of the sequence |
|
inline |
moveEffectorToPlacement Add two new phases at the end of the current ContactSequence,
eeName | the name of the effector used to create contact |
placement | the new placement for the contact of eeName |
durationBreak | the duration of the previous last phase of the sequence (it is thus the duration BEFORE breaking the contact) |
durationCreate | the duration of the first new ContactPhase (it is thus the duration BEFORE creating the contact) |
invalid_argument | if the phaseDuration is provided but the last phase do not have a time-range defined |
invalid_argument | if eeName is not in contact in the last phase of the sequence |
|
inline |
|
inline |
|
inline |
phaseAtTime return a phase of the sequence such that : phase.timeInitial <= t < phase.timeFinal if t equal to the last phase timeFinal, this index is returned
time |
invalid_argument | error if no phase are found for this time |
|
inline |
phaseIdAtTime return the index of a phase in the sequence such that : phase.timeInitial <= t < phase.timeFinal if t equal to the last phase timeFinal, this index is returned
time |
|
inline |
removePhase remove the given contactPhase from the sequence
id | the Id of the phase to remove |
|
inline |
|
inline |
|
friend |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _ContactPhase multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::ContactPhase |
ContactPhaseVector multicontact_api::scenario::ContactSequenceTpl< _ContactPhase >::m_contact_phases |