#include <multicontact-api/scenario/contact-phase.hpp>
Public Types | |
typedef ndcurves::pointX_t | pointX_t |
typedef ndcurves::point3_t | point3_t |
typedef ndcurves::point6_t | point6_t |
typedef ndcurves::t_point3_t | t_point3_t |
typedef ndcurves::t_pointX_t | 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 ndcurves::curve_ptr_t | curve_ptr |
typedef ndcurves::curve_SE3_ptr_t | curve_SE3_ptr |
typedef ndcurves::piecewise3_t | piecewise3_t |
typedef ndcurves::piecewise_t | piecewise_t |
typedef piecewise_t::t_time_t | t_time_t |
typedef std::vector< std::string > | t_strings |
typedef ContactPatchTpl< Scalar > | ContactPatch |
typedef ContactPatch::SE3 | SE3 |
typedef std::map< std::string, ContactPatch > | ContactPatchMap |
typedef CurveMap< curve_ptr > | CurveMap_t |
typedef CurveMap< curve_SE3_ptr > | CurveSE3Map_t |
Public Member Functions | |
ContactPhaseTpl () | |
Default constructor. | |
ContactPhaseTpl (const Scalar t_init, const Scalar t_final) | |
ContactPhaseTpl Constructor with time interval. | |
template<typename S2 > | |
ContactPhaseTpl (const ContactPhaseTpl< S2 > &other) | |
Copy constructor. | |
template<typename S2 > | |
bool | operator== (const ContactPhaseTpl< S2 > &other) const |
template<typename S2 > | |
bool | operator!= (const ContactPhaseTpl< S2 > &other) const |
Scalar | timeInitial () const |
void | timeInitial (const Scalar t) |
Scalar | timeFinal () const |
void | timeFinal (const Scalar t) |
Scalar | duration () const |
void | duration (const Scalar d) |
CurveMap_t | contactForces () const |
CurveMap_t | contactNormalForces () const |
CurveSE3Map_t | effectorTrajectories () const |
curve_ptr | contactForces (const std::string &eeName) |
curve_ptr | contactNormalForces (const std::string &eeName) |
curve_SE3_ptr | effectorTrajectories (const std::string &eeName) |
bool | addContactForceTrajectory (const std::string &eeName, const curve_ptr trajectory) |
addContactForceTrajectory add a trajectory to the map of contact forces. If a trajectory already exist for this effector, it is overwritted. | |
bool | addContactNormalForceTrajectory (const std::string &eeName, const curve_ptr trajectory) |
addContactNormalForceTrajectory add a trajectory to the map of contact normal forces. If a trajectory already exist for this effector, it is overwritted. | |
bool | addEffectorTrajectory (const std::string &eeName, const curve_SE3_ptr trajectory) |
adEffectorTrajectory add a trajectory to the map of contact forces. If a trajectory already exist for this effector, it is overwritted. | |
ContactPatchMap | contactPatches () const |
ContactPatch & | contactPatch (const std::string &eeName) |
bool | addContact (const std::string &eeName, const ContactPatch &patch) |
addContact add a new contact patch to this contact phase If a contact phase already exist for this effector, it is overwritted. If an end effector trajectory exist for this contact, it is removed. | |
bool | removeContact (const std::string &eeName) |
removeContact remove the given contact This will also remove the contact_patch all the contact_forces and contact_normal_forces related to this contact | |
std::size_t | numContacts () const |
t_strings | effectorsInContact () const |
bool | isEffectorInContact (const std::string &eeName) const |
t_strings | effectorsWithTrajectory () const |
effectorsWithTrajectory return a set of all effectors for which an effector trajectory have been defined | |
bool | effectorHaveAtrajectory (const std::string &eeName) const |
effectorHaveAtrajectory check if an end effector trajectory have been defined for a given effector | |
bool | isConsistent (const bool throw_if_inconsistent=false) const |
isConsistent check if all the members of the phase are consistent together: | |
void | setCOMtrajectoryFromPoints (const t_pointX_t &points, const t_pointX_t &points_derivative, const t_pointX_t &points_second_derivative, const t_time_t &time_points) |
setCOMtrajectoryFromPoints set the c,dc and ddc curves from a list of discrete COM positions, velocity and accelerations. The trajectories are build with first order polynomials connecting each discrete points given. this method also set the initial/final values for c, dc and ddc from the first and last discrete point given. | |
void | setAMtrajectoryFromPoints (const t_pointX_t &points, const t_pointX_t &points_derivative, const t_time_t &time_points) |
setAMtrajectoryFromPoints set the L and d_L curves from a list of discrete Angular velocity values and their derivatives The trajectories are build with first order polynomials connecting each discrete points given. This method also set the initial/final values for L, and dL from the first and last discrete point given. | |
void | setJointsTrajectoryFromPoints (const t_pointX_t &points, const t_pointX_t &points_derivative, const t_pointX_t &points_second_derivative, const t_time_t &time_points) |
setJointsTrajectoryFromPoints set the q,dq and ddq curves from a list of discrete joints positions, velocity and accelerations. The trajectories are build with first order polynomials connecting each discrete points given. This method also set initial/final values for q from the first and last discrete point given. | |
t_strings | getContactsBroken (const ContactPhase &to) const |
getContactsBroken return the list of effectors in contact in '*this' but not in contact in 'to' | |
t_strings | getContactsCreated (const ContactPhase &to) const |
getContactsCreated return the list of effectors in contact in 'to' but not in contact in '*this' | |
t_strings | getContactsRepositioned (const ContactPhase &to) const |
getContactsRepositioned return the list of effectors in contact both in 'to' and '*this' but not at the same placement | |
t_strings | getContactsVariations (const ContactPhase &to) const |
getContactsVariations return the list of all the effectors whose contacts differ between *this and to | |
void | disp (std::ostream &os) const |
![]() | |
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 _Scalar | Scalar |
point3_t | m_c_init |
Initial position of the center of mass for this contact phase. | |
point3_t | m_dc_init |
Initial velocity of the center of mass for this contact phase. | |
point3_t | m_ddc_init |
Initial acceleration of the center of mass for this contact phase. | |
point3_t | m_L_init |
Initial angular momentum for this contact phase. | |
point3_t | m_dL_init |
Initial angular momentum derivative for this contact phase. | |
pointX_t | m_q_init |
Initial whole body configuration of this phase. | |
point3_t | m_c_final |
Final position of the center of mass for this contact phase. | |
point3_t | m_dc_final |
Final velocity of the center of mass for this contact phase. | |
point3_t | m_ddc_final |
Final acceleration of the center of mass for this contact phase. | |
point3_t | m_L_final |
Final angular momentum for this contact phase. | |
point3_t | m_dL_final |
Final angular momentum derivative for this contact phase. | |
pointX_t | m_q_final |
Final whole body configuration of this phase. | |
curve_ptr | m_q |
trajectory for the joint positions | |
curve_ptr | m_dq |
trajectory for the joint velocities | |
curve_ptr | m_ddq |
trajectory for the joint accelerations | |
curve_ptr | m_tau |
trajectory for the joint torques | |
curve_ptr | m_c |
trajectory for the center of mass position | |
curve_ptr | m_dc |
trajectory for the center of mass velocity | |
curve_ptr | m_ddc |
trajectory for the center of mass acceleration | |
curve_ptr | m_L |
trajectory for the angular momentum | |
curve_ptr | m_dL |
trajectory for the angular momentum derivative | |
curve_ptr | m_wrench |
trajectory for the centroidal wrench | |
curve_ptr | m_zmp |
trajectory for the zmp | |
curve_SE3_ptr | m_root |
SE3 trajectory of the root of the robot. | |
Protected Attributes | |
CurveMap_t | m_contact_forces |
map with keys : effector name containing the contact forces | |
CurveMap_t | m_contact_normal_force |
map with keys : effector name containing the contact normal force | |
CurveSE3Map_t | m_effector_trajectories |
map with keys : effector name containing the end effector trajectory | |
t_strings | m_effector_in_contact |
set of the name of all effector in contact for this phase | |
ContactPatchMap | m_contact_patches |
map effector name : contact patches. All the patches are actives | |
Scalar | m_t_init |
time at the beginning of the contact phase | |
Scalar | m_t_final |
time at the end of the contact phase | |
Friends | |
class | boost::serialization::access |
template<typename S2 > | |
std::ostream & | operator<< (std::ostream &os, const ContactPhaseTpl< S2 > &cp) |
typedef ContactPatchTpl<Scalar> multicontact_api::scenario::ContactPhaseTpl< _Scalar >::ContactPatch |
typedef std::map<std::string, ContactPatch> multicontact_api::scenario::ContactPhaseTpl< _Scalar >::ContactPatchMap |
typedef ndcurves::curve_ptr_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::curve_ptr |
typedef ndcurves::curve_SE3_ptr_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::curve_SE3_ptr |
typedef ndcurves::curve_SE3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::curve_SE3_t |
typedef ndcurves::curve_abc_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::curve_t |
typedef CurveMap<curve_ptr> multicontact_api::scenario::ContactPhaseTpl< _Scalar >::CurveMap_t |
typedef CurveMap<curve_SE3_ptr> multicontact_api::scenario::ContactPhaseTpl< _Scalar >::CurveSE3Map_t |
typedef ndcurves::piecewise3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::piecewise3_t |
typedef ndcurves::piecewise_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::piecewise_t |
typedef ndcurves::point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::point3_t |
typedef ndcurves::point6_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::point6_t |
typedef ndcurves::pointX_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::pointX_t |
typedef ContactPatch::SE3 multicontact_api::scenario::ContactPhaseTpl< _Scalar >::SE3 |
typedef ndcurves::t_point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::t_point3_t |
typedef ndcurves::t_pointX_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::t_pointX_t |
typedef std::vector<std::string> multicontact_api::scenario::ContactPhaseTpl< _Scalar >::t_strings |
typedef piecewise_t::t_time_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::t_time_t |
typedef ndcurves::transform_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::transform_t |
|
inline |
Default constructor.
|
inline |
ContactPhaseTpl Constructor with time interval.
t_init | the time at the beginning of this contact phase |
t_final | the time at the end of this contact phase |
invalid_argument | if t_final < t_init |
|
inline |
Copy constructor.
|
inline |
addContact add a new contact patch to this contact phase If a contact phase already exist for this effector, it is overwritted. If an end effector trajectory exist for this contact, it is removed.
eeName | the name of the effector in contact |
patch | the contact patch |
|
inline |
addContactForceTrajectory add a trajectory to the map of contact forces. If a trajectory already exist for this effector, it is overwritted.
eeName | the name of the effector (key of the map) |
trajectory | the trajectory to add |
invalid_argument | if eeName is not defined in contact for this phase |
|
inline |
addContactNormalForceTrajectory add a trajectory to the map of contact normal forces. If a trajectory already exist for this effector, it is overwritted.
eeName | the name of the effector (key of the map) |
trajectory | the trajectory to add |
invalid_argument | if eeName is not defined in contact for this phase |
invalid_argument | if trajectory is not of dimension 1 |
|
inline |
adEffectorTrajectory add a trajectory to the map of contact forces. If a trajectory already exist for this effector, it is overwritted.
eeName | the name of the effector (key of the map) |
trajectory | the trajectory to add |
invalid_argument | if eeName is defined in contact for this phase |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
effectorHaveAtrajectory check if an end effector trajectory have been defined for a given effector
eeName | the effector name |
|
inline |
|
inline |
effectorsWithTrajectory return a set of all effectors for which an effector trajectory have been defined
|
inline |
|
inline |
|
inline |
getContactsBroken return the list of effectors in contact in '*this' but not in contact in 'to'
to | the other ContactPhase |
|
inline |
getContactsCreated return the list of effectors in contact in 'to' but not in contact in '*this'
to | the other ContactPhase |
|
inline |
getContactsRepositioned return the list of effectors in contact both in 'to' and '*this' but not at the same placement
to | the other ContactPhase |
|
inline |
getContactsVariations return the list of all the effectors whose contacts differ between *this and to
to | the other ContactPhase |
|
inline |
isConsistent check if all the members of the phase are consistent together:
throw_if_inconsistent | if true, throw an runtime_error if not consistent |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
removeContact remove the given contact This will also remove the contact_patch all the contact_forces and contact_normal_forces related to this contact
eeName | the name of the effector to remove |
|
inline |
setAMtrajectoryFromPoints set the L and d_L curves from a list of discrete Angular velocity values and their derivatives The trajectories are build with first order polynomials connecting each discrete points given. This method also set the initial/final values for L, and dL from the first and last discrete point given.
points | list of discrete Angular Momentum values |
points_derivative | list of discrete Angular momentum derivative |
time_points | list of times corresponding to each discrete point given. |
|
inline |
setCOMtrajectoryFromPoints set the c,dc and ddc curves from a list of discrete COM positions, velocity and accelerations. The trajectories are build with first order polynomials connecting each discrete points given. this method also set the initial/final values for c, dc and ddc from the first and last discrete point given.
points | list of discrete CoM positions |
points_derivative | list of discrete CoM velocities |
points_second_derivative | list of discrete CoM accelerations |
time_points | list of times corresponding to each discrete point given. |
|
inline |
setJointsTrajectoryFromPoints set the q,dq and ddq curves from a list of discrete joints positions, velocity and accelerations. The trajectories are build with first order polynomials connecting each discrete points given. This method also set initial/final values for q from the first and last discrete point given.
points | list of discrete joints positions |
points_derivative | list of discrete joints velocities |
points_second_derivative | list of discrete joints accelerations |
time_points | list of times corresponding to each discrete point given. |
|
inline |
|
inline |
|
inline |
|
inline |
|
friend |
|
friend |
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_c |
trajectory for the center of mass position
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_c_final |
Final position of the center of mass for this contact phase.
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_c_init |
Initial position of the center of mass for this contact phase.
|
protected |
map with keys : effector name containing the contact forces
|
protected |
map with keys : effector name containing the contact normal force
|
protected |
map effector name : contact patches. All the patches are actives
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_dc |
trajectory for the center of mass velocity
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_dc_final |
Final velocity of the center of mass for this contact phase.
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_dc_init |
Initial velocity of the center of mass for this contact phase.
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_ddc |
trajectory for the center of mass acceleration
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_ddc_final |
Final acceleration of the center of mass for this contact phase.
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_ddc_init |
Initial acceleration of the center of mass for this contact phase.
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_ddq |
trajectory for the joint accelerations
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_dL |
trajectory for the angular momentum derivative
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_dL_final |
Final angular momentum derivative for this contact phase.
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_dL_init |
Initial angular momentum derivative for this contact phase.
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_dq |
trajectory for the joint velocities
|
protected |
set of the name of all effector in contact for this phase
|
protected |
map with keys : effector name containing the end effector trajectory
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_L |
trajectory for the angular momentum
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_L_final |
Final angular momentum for this contact phase.
point3_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_L_init |
Initial angular momentum for this contact phase.
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_q |
trajectory for the joint positions
pointX_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_q_final |
Final whole body configuration of this phase.
pointX_t multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_q_init |
Initial whole body configuration of this phase.
curve_SE3_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_root |
SE3 trajectory of the root of the robot.
|
protected |
time at the end of the contact phase
|
protected |
time at the beginning of the contact phase
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_tau |
trajectory for the joint torques
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_wrench |
trajectory for the centroidal wrench
curve_ptr multicontact_api::scenario::ContactPhaseTpl< _Scalar >::m_zmp |
trajectory for the zmp
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar multicontact_api::scenario::ContactPhaseTpl< _Scalar >::Scalar |