robotoc
robotoc - efficient ROBOT Optimal Control solvers
Loading...
Searching...
No Matches
robotoc::ContactStatus Class Reference

Contact status of robot model. More...

#include <contact_status.hpp>

Public Member Functions

 ContactStatus (const std::vector< ContactType > &contact_types, const std::vector< std::string > &contact_frame_names, const double default_friction_coefficient=0.7)
 Constructor. More...
 
 ContactStatus ()
 Default constructor. More...
 
 ~ContactStatus ()=default
 Default destructor. More...
 
 ContactStatus (const ContactStatus &)=default
 Default copy constructor. More...
 
ContactStatusoperator= (const ContactStatus &)=default
 Default copy assign operator. More...
 
 ContactStatus (ContactStatus &&) noexcept=default
 Default move constructor. More...
 
ContactStatusoperator= (ContactStatus &&) noexcept=default
 Default move assign operator. More...
 
bool operator== (const ContactStatus &other) const
 Defines a comparison operator. More...
 
bool operator!= (const ContactStatus &other) const
 Defines a comparison operator. More...
 
ContactType contactType (const int contact_index) const
 Returns the type of the contact. More...
 
const std::vector< ContactType > & contactTypes () const
 Returns the types of the contacts. More...
 
const std::string & contactFrameName (const int contact_index) const
 Returns the name of the contact frame. More...
 
const std::vector< std::string > & contactFrameNames () const
 Returns the names of the contact frames. More...
 
bool isContactActive (const int contact_index) const
 Returns true if a contact is active and false if not. More...
 
bool isContactActive (const std::string &contact_frame_name) const
 Returns true if a contact is active and false if not. More...
 
const std::vector< bool > & isContactActive () const
 Returns the activity of the contacts. More...
 
bool hasActiveContacts () const
 Returns true if there are active contacts and false if not. More...
 
int dimf () const
 Returns the dimension of the active contact forces. More...
 
int maxNumContacts () const
 Returns the maximum number of the contacts. More...
 
void activateContact (const int contact_index)
 Activates a contact. More...
 
void activateContact (const std::string &contact_frame_name)
 Activates a contact. More...
 
void deactivateContact (const int contact_index)
 Deactivates a contact. More...
 
void deactivateContact (const std::string &contact_frame_name)
 Deactivates a contact. More...
 
void activateContacts (const std::vector< int > &contact_indices)
 Activates contacts. More...
 
void activateContacts (const std::vector< std::string > &contact_frame_names)
 Activates contacts. More...
 
void deactivateContacts (const std::vector< int > &contact_indices)
 Deactivates contacts. More...
 
void deactivateContacts (const std::vector< std::string > &contact_frame_names)
 Deactivates contacts. More...
 
void setContactPlacement (const int contact_index, const Eigen::Vector3d &contact_position)
 Sets a contact placement, that is, the position and rotation of the contact. The contact rotation is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot. More...
 
void setContactPlacement (const std::string &contact_frame_name, const Eigen::Vector3d &contact_position)
 Sets a contact placement, that is, the position and rotation of the contact. The contact rotation is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot. More...
 
void setContactPlacement (const int contact_index, const Eigen::Vector3d &contact_position, const Eigen::Matrix3d &contact_rotation)
 Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot. More...
 
void setContactPlacement (const std::string &contact_frame_name, const Eigen::Vector3d &contact_position, const Eigen::Matrix3d &contact_rotation)
 Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot. More...
 
void setContactPlacement (const int contact_index, const SE3 &contact_placement)
 Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot. More...
 
void setContactPlacement (const std::string &contact_frame_name, const SE3 &contact_placement)
 Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot. More...
 
void setContactPlacements (const std::vector< Eigen::Vector3d > &contact_positions)
 Sets contact placements. The rotation of each contact is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground. More...
 
void setContactPlacements (const std::unordered_map< std::string, Eigen::Vector3d > &contact_positions)
 Sets contact placements. The rotation of each contact is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground. More...
 
void setContactPlacements (const std::vector< Eigen::Vector3d > &contact_positions, const std::vector< Eigen::Matrix3d > &contact_rotations)
 Sets contact placements. More...
 
void setContactPlacements (const std::unordered_map< std::string, Eigen::Vector3d > &contact_positions, const std::unordered_map< std::string, Eigen::Matrix3d > &contact_rotations)
 Sets contact placements. More...
 
void setContactPlacements (const aligned_vector< SE3 > &contact_placements)
 Sets contact placements. More...
 
void setContactPlacements (const aligned_unordered_map< std::string, SE3 > &contact_placements)
 Sets contact placements. More...
 
const SE3contactPlacement (const int contact_index) const
 Gets the contact placement. More...
 
const SE3contactPlacement (const std::string &contact_frame_name) const
 Gets the contact placement. More...
 
const Eigen::Vector3d & contactPosition (const int contact_index) const
 Gets the contact position. More...
 
const Eigen::Vector3d & contactPosition (const std::string &contact_frame_name) const
 Gets the contact position. More...
 
const Eigen::Matrix3d & contactRotation (const int contact_index) const
 Gets the contact rotation. More...
 
const Eigen::Matrix3d & contactRotation (const std::string &contact_frame_name) const
 Gets the contact rotation. More...
 
const aligned_vector< SE3 > & contactPlacements () const
 Gets the contact placements. More...
 
const std::vector< Eigen::Vector3d > & contactPositions () const
 Gets the contact positions. More...
 
const std::vector< Eigen::Matrix3d > & contactRotations () const
 Gets the contact rotations. More...
 
void setFrictionCoefficient (const int contact_index, const double friction_coefficient)
 Gets the friction coefficint. More...
 
void setFrictionCoefficient (const std::string &contact_frame_name, const double friction_coefficient)
 Gets the friction coefficint. More...
 
void setFrictionCoefficients (const std::vector< double > &friction_coefficients)
 Sets the friction coefficints. More...
 
void setFrictionCoefficients (const std::unordered_map< std::string, double > &friction_coefficients)
 Sets the friction coefficints. More...
 
double frictionCoefficient (const int contact_index) const
 Gets the friction coefficint. Default value is 0.7. More...
 
double frictionCoefficient (const std::string &contact_frame_name) const
 Gets the friction coefficint. Default value is 0.7. More...
 
const std::vector< double > & frictionCoefficients () const
 Gets the friction coefficints. Default value is 0.7. More...
 
int findContactIndex (const std::string &contact_frame_name) const
 Finds the contact index correspoinding to the input contact frame name. More...
 
void setRandom ()
 Fills contact status randomly. More...
 
void disp (std::ostream &os) const
 Displays the contact status onto a ostream. More...
 

Friends

std::ostream & operator<< (std::ostream &os, const ContactStatus &contact_status)
 

Detailed Description

Contact status of robot model.

Constructor & Destructor Documentation

◆ ContactStatus() [1/4]

robotoc::ContactStatus::ContactStatus ( const std::vector< ContactType > &  contact_types,
const std::vector< std::string > &  contact_frame_names,
const double  default_friction_coefficient = 0.7 
)
inline

Constructor.

Parameters
[in]contact_typesTypes of contacts.
[in]contact_frame_namesNames of contact frames.
[in]default_friction_coefficientDefault friction coefficitn. Must be positive. Default is 0.7.

◆ ContactStatus() [2/4]

robotoc::ContactStatus::ContactStatus ( )
inline

Default constructor.

◆ ~ContactStatus()

robotoc::ContactStatus::~ContactStatus ( )
default

Default destructor.

◆ ContactStatus() [3/4]

robotoc::ContactStatus::ContactStatus ( const ContactStatus )
default

Default copy constructor.

◆ ContactStatus() [4/4]

robotoc::ContactStatus::ContactStatus ( ContactStatus &&  )
defaultnoexcept

Default move constructor.

Member Function Documentation

◆ activateContact() [1/2]

void robotoc::ContactStatus::activateContact ( const int  contact_index)
inline

Activates a contact.

Parameters
[in]contact_indexIndex of the contact that is activated.

◆ activateContact() [2/2]

void robotoc::ContactStatus::activateContact ( const std::string &  contact_frame_name)
inline

Activates a contact.

Parameters
[in]contact_frame_nameName of the contact frame that is activated.

◆ activateContacts() [1/2]

void robotoc::ContactStatus::activateContacts ( const std::vector< int > &  contact_indices)
inline

Activates contacts.

Parameters
[in]contact_indicesIndices of the contacts that are activated.

◆ activateContacts() [2/2]

void robotoc::ContactStatus::activateContacts ( const std::vector< std::string > &  contact_frame_names)
inline

Activates contacts.

Parameters
[in]contact_frame_namesFrame names of the contacts that are activated.

◆ contactFrameName()

const std::string & robotoc::ContactStatus::contactFrameName ( const int  contact_index) const
inline

Returns the name of the contact frame.

Parameters
[in]contact_indexIndex of a contact of interedted.
Returns
Name of the contact frame.

◆ contactFrameNames()

const std::vector< std::string > & robotoc::ContactStatus::contactFrameNames ( ) const
inline

Returns the names of the contact frames.

Returns
Name of the contact frames.

◆ contactPlacement() [1/2]

const SE3 & robotoc::ContactStatus::contactPlacement ( const int  contact_index) const
inline

Gets the contact placement.

Parameters
[in]contact_indexIndex of the contact .
Returns
const reference to the contact placement.

◆ contactPlacement() [2/2]

const SE3 & robotoc::ContactStatus::contactPlacement ( const std::string &  contact_frame_name) const
inline

Gets the contact placement.

Parameters
[in]contact_frame_nameName of the contact frame.
Returns
const reference to the contact placement.

◆ contactPlacements()

const aligned_vector< SE3 > & robotoc::ContactStatus::contactPlacements ( ) const
inline

Gets the contact placements.

Returns
const reference to the contact placements.

◆ contactPosition() [1/2]

const Eigen::Vector3d & robotoc::ContactStatus::contactPosition ( const int  contact_index) const
inline

Gets the contact position.

Parameters
[in]contact_indexIndex of the contact .
Returns
const reference to the contact position.

◆ contactPosition() [2/2]

const Eigen::Vector3d & robotoc::ContactStatus::contactPosition ( const std::string &  contact_frame_name) const
inline

Gets the contact position.

Parameters
[in]contact_frame_nameName of the contact frame.
Returns
const reference to the contact position.

◆ contactPositions()

const std::vector< Eigen::Vector3d > & robotoc::ContactStatus::contactPositions ( ) const
inline

Gets the contact positions.

Returns
const reference to the contact positions.

◆ contactRotation() [1/2]

const Eigen::Matrix3d & robotoc::ContactStatus::contactRotation ( const int  contact_index) const
inline

Gets the contact rotation.

Parameters
[in]contact_indexIndex of the contact .
Returns
const reference to the contact rotation.

◆ contactRotation() [2/2]

const Eigen::Matrix3d & robotoc::ContactStatus::contactRotation ( const std::string &  contact_frame_name) const
inline

Gets the contact rotation.

Parameters
[in]contact_frame_nameName of the contact frame.
Returns
const reference to the contact rotation.

◆ contactRotations()

const std::vector< Eigen::Matrix3d > & robotoc::ContactStatus::contactRotations ( ) const
inline

Gets the contact rotations.

Returns
const reference to the contact rotations.

◆ contactType()

ContactType robotoc::ContactStatus::contactType ( const int  contact_index) const
inline

Returns the type of the contact.

Parameters
[in]contact_indexIndex of a contact of interedted.
Returns
Contact type.

◆ contactTypes()

const std::vector< ContactType > & robotoc::ContactStatus::contactTypes ( ) const
inline

Returns the types of the contacts.

Returns
Contact types.

◆ deactivateContact() [1/2]

void robotoc::ContactStatus::deactivateContact ( const int  contact_index)
inline

Deactivates a contact.

Parameters
[in]contact_indexIndex of the contact that is deactivated.

◆ deactivateContact() [2/2]

void robotoc::ContactStatus::deactivateContact ( const std::string &  contact_frame_name)
inline

Deactivates a contact.

Parameters
[in]contact_frame_nameName of the contact frame that is deactivated.

◆ deactivateContacts() [1/2]

void robotoc::ContactStatus::deactivateContacts ( const std::vector< int > &  contact_indices)
inline

Deactivates contacts.

Parameters
[in]contact_indicesIndices of the contacts that are deactivated.

◆ deactivateContacts() [2/2]

void robotoc::ContactStatus::deactivateContacts ( const std::vector< std::string > &  contact_frame_names)
inline

Deactivates contacts.

Parameters
[in]contact_frame_namesFrame names of the contacts that are deactivated.

◆ dimf()

int robotoc::ContactStatus::dimf ( ) const
inline

Returns the dimension of the active contact forces.

Returns
Dimension of the active contacts forces.

◆ disp()

void robotoc::ContactStatus::disp ( std::ostream &  os) const

Displays the contact status onto a ostream.

◆ findContactIndex()

int robotoc::ContactStatus::findContactIndex ( const std::string &  contact_frame_name) const
inline

Finds the contact index correspoinding to the input contact frame name.

Parameters
[in]contact_frame_nameName of the contact frame.
Returns
Contact index.

◆ frictionCoefficient() [1/2]

double robotoc::ContactStatus::frictionCoefficient ( const int  contact_index) const
inline

Gets the friction coefficint. Default value is 0.7.

Parameters
[in]contact_indexIndex of the contact.
Returns
Friction coefficient of the contact.

◆ frictionCoefficient() [2/2]

double robotoc::ContactStatus::frictionCoefficient ( const std::string &  contact_frame_name) const
inline

Gets the friction coefficint. Default value is 0.7.

Parameters
[in]contact_frame_nameName of the contact frame.
Returns
Friction coefficient of the contact.

◆ frictionCoefficients()

const std::vector< double > & robotoc::ContactStatus::frictionCoefficients ( ) const
inline

Gets the friction coefficints. Default value is 0.7.

Returns
Friction coefficients of the contacts.

◆ hasActiveContacts()

bool robotoc::ContactStatus::hasActiveContacts ( ) const
inline

Returns true if there are active contacts and false if not.

Returns
true if there are active contacts and false if not.

◆ isContactActive() [1/3]

const std::vector< bool > & robotoc::ContactStatus::isContactActive ( ) const
inline

Returns the activity of the contacts.

Returns
Const reference to the activity of the contacts.

◆ isContactActive() [2/3]

bool robotoc::ContactStatus::isContactActive ( const int  contact_index) const
inline

Returns true if a contact is active and false if not.

Parameters
[in]contact_indexIndex of a contact of interedted.
Returns
true if a contact is active and false if not.

◆ isContactActive() [3/3]

bool robotoc::ContactStatus::isContactActive ( const std::string &  contact_frame_name) const
inline

Returns true if a contact is active and false if not.

Parameters
[in]contact_frame_nameName of the contact frame that is activated.
Returns
true if a contact is active and false if not.

◆ maxNumContacts()

int robotoc::ContactStatus::maxNumContacts ( ) const
inline

Returns the maximum number of the contacts.

Returns
The maximum number of the contacts.

◆ operator!=()

bool robotoc::ContactStatus::operator!= ( const ContactStatus other) const
inline

Defines a comparison operator.

◆ operator=() [1/2]

ContactStatus & robotoc::ContactStatus::operator= ( const ContactStatus )
default

Default copy assign operator.

◆ operator=() [2/2]

ContactStatus & robotoc::ContactStatus::operator= ( ContactStatus &&  )
defaultnoexcept

Default move assign operator.

◆ operator==()

bool robotoc::ContactStatus::operator== ( const ContactStatus other) const
inline

Defines a comparison operator.

◆ setContactPlacement() [1/6]

void robotoc::ContactStatus::setContactPlacement ( const int  contact_index,
const Eigen::Vector3d &  contact_position 
)
inline

Sets a contact placement, that is, the position and rotation of the contact. The contact rotation is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot.

Parameters
[in]contact_indexIndex of the contact.
[in]contact_positionContact position.

◆ setContactPlacement() [2/6]

void robotoc::ContactStatus::setContactPlacement ( const int  contact_index,
const Eigen::Vector3d &  contact_position,
const Eigen::Matrix3d &  contact_rotation 
)
inline

Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot.

Parameters
[in]contact_indexIndex of the contact.
[in]contact_positionContact position.
[in]contact_rotationContact rotation.

◆ setContactPlacement() [3/6]

void robotoc::ContactStatus::setContactPlacement ( const int  contact_index,
const SE3 contact_placement 
)
inline

Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot.

Parameters
[in]contact_indexIndex of the contact.
[in]contact_placementContact placement.

◆ setContactPlacement() [4/6]

void robotoc::ContactStatus::setContactPlacement ( const std::string &  contact_frame_name,
const Eigen::Vector3d &  contact_position 
)
inline

Sets a contact placement, that is, the position and rotation of the contact. The contact rotation is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot.

Parameters
[in]contact_frame_nameName of the contact frame.
[in]contact_positionContact position.

◆ setContactPlacement() [5/6]

void robotoc::ContactStatus::setContactPlacement ( const std::string &  contact_frame_name,
const Eigen::Vector3d &  contact_position,
const Eigen::Matrix3d &  contact_rotation 
)
inline

Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot.

Parameters
[in]contact_frame_nameName of the contact frame.
[in]contact_positionContact position.
[in]contact_rotationContact rotation.

◆ setContactPlacement() [6/6]

void robotoc::ContactStatus::setContactPlacement ( const std::string &  contact_frame_name,
const SE3 contact_placement 
)
inline

Sets a contact placement, that is, the position and rotation of the contact. For the point contacts, the rotation is only used in the friction cone constraints. For the surface contacts, the rotation represents the rotational contact constraints on the contact frame of the robot.

Parameters
[in]contact_frame_nameName of the contact frame.
[in]contact_placementContact placement.

◆ setContactPlacements() [1/6]

void robotoc::ContactStatus::setContactPlacements ( const aligned_unordered_map< std::string, SE3 > &  contact_placements)
inline

Sets contact placements.

Parameters
[in]contact_placementsContact placements. Size must be ContactStatus::maxNumContacts().

◆ setContactPlacements() [2/6]

void robotoc::ContactStatus::setContactPlacements ( const aligned_vector< SE3 > &  contact_placements)
inline

Sets contact placements.

Parameters
[in]contact_placementsContact placements. Size must be ContactStatus::maxNumContacts().

◆ setContactPlacements() [3/6]

void robotoc::ContactStatus::setContactPlacements ( const std::unordered_map< std::string, Eigen::Vector3d > &  contact_positions)
inline

Sets contact placements. The rotation of each contact is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground.

Parameters
[in]contact_positionsContact positions. Size must be ContactStatus::maxNumContacts().

◆ setContactPlacements() [4/6]

void robotoc::ContactStatus::setContactPlacements ( const std::unordered_map< std::string, Eigen::Vector3d > &  contact_positions,
const std::unordered_map< std::string, Eigen::Matrix3d > &  contact_rotations 
)
inline

Sets contact placements.

Parameters
[in]contact_positionsContact positions. Size must be ContactStatus::maxNumContacts().
[in]contact_rotationsContact rotations. Size must be ContactStatus::maxNumContacts().

◆ setContactPlacements() [5/6]

void robotoc::ContactStatus::setContactPlacements ( const std::vector< Eigen::Vector3d > &  contact_positions)
inline

Sets contact placements. The rotation of each contact is set to Eigen::Matrix3d::Identity(), which represents the vertical direction to the ground.

Parameters
[in]contact_positionsContact positions. Size must be ContactStatus::maxNumContacts().

◆ setContactPlacements() [6/6]

void robotoc::ContactStatus::setContactPlacements ( const std::vector< Eigen::Vector3d > &  contact_positions,
const std::vector< Eigen::Matrix3d > &  contact_rotations 
)
inline

Sets contact placements.

Parameters
[in]contact_positionsContact positions. Size must be ContactStatus::maxNumContacts().
[in]contact_rotationsContact rotations. Size must be ContactStatus::maxNumContacts().

◆ setFrictionCoefficient() [1/2]

void robotoc::ContactStatus::setFrictionCoefficient ( const int  contact_index,
const double  friction_coefficient 
)
inline

Gets the friction coefficint.

Parameters
[in]contact_indexIndex of the contact.
[in]friction_coefficientFriction coefficient. Must be positive.

◆ setFrictionCoefficient() [2/2]

void robotoc::ContactStatus::setFrictionCoefficient ( const std::string &  contact_frame_name,
const double  friction_coefficient 
)
inline

Gets the friction coefficint.

Parameters
[in]contact_frame_nameName of the contact frame.
[in]friction_coefficientFriction coefficient. Must be positive.

◆ setFrictionCoefficients() [1/2]

void robotoc::ContactStatus::setFrictionCoefficients ( const std::unordered_map< std::string, double > &  friction_coefficients)
inline

Sets the friction coefficints.

Parameters
[in]friction_coefficientsFriction coefficients. Size must be ContactStatus::maxNumContacts() and each element must be positive.

◆ setFrictionCoefficients() [2/2]

void robotoc::ContactStatus::setFrictionCoefficients ( const std::vector< double > &  friction_coefficients)
inline

Sets the friction coefficints.

Parameters
[in]friction_coefficientsFriction coefficients. Size must be ContactStatus::maxNumContacts() and each element must be positive.

◆ setRandom()

void robotoc::ContactStatus::setRandom ( )
inline

Fills contact status randomly.

Friends And Related Function Documentation

◆ operator<<

std::ostream & operator<< ( std::ostream &  os,
const ContactStatus contact_status 
)
friend

The documentation for this class was generated from the following files: