#include <contact-point.hpp>
Public Types | |
typedef math::ConstraintEquality | ConstraintEquality |
typedef math::ConstraintInequality | ConstraintInequality |
typedef math::ConstRefVector | ConstRefVector |
typedef math::Matrix3x | Matrix3x |
typedef pinocchio::SE3 | SE3 |
typedef tasks::TaskSE3Equality | TaskSE3Equality |
typedef math::Vector | Vector |
typedef math::Vector3 | Vector3 |
typedef math::Vector6 | Vector6 |
![]() | |
typedef math::ConstraintEquality | ConstraintEquality |
typedef math::ConstraintInequality | ConstraintInequality |
typedef math::ConstRefVector | ConstRefVector |
typedef pinocchio::Data | Data |
typedef math::Matrix | Matrix |
typedef math::Matrix3x | Matrix3x |
typedef robots::RobotWrapper | RobotWrapper |
typedef tasks::TaskMotion | TaskMotion |
typedef tasks::TaskSE3Equality | TaskSE3Equality |
Public Member Functions | |
const ConstraintEquality & | computeForceRegularizationTask (double t, ConstRefVector q, ConstRefVector v, const Data &data) override |
const ConstraintInequality & | computeForceTask (double t, ConstRefVector q, ConstRefVector v, const Data &data) override |
const ConstraintBase & | computeMotionTask (double t, ConstRefVector q, ConstRefVector v, Data &data) override |
ContactPoint (const std::string &name, RobotWrapper &robot, const std::string &frameName, ConstRefVector contactNormal, const double frictionCoefficient, const double minNormalForce, const double maxNormalForce) | |
const Matrix3x & | getContactPoints () const override |
const ConstraintInequality & | getForceConstraint () const override |
const Matrix & | getForceGeneratorMatrix () override |
const ConstraintEquality & | getForceRegularizationTask () const override |
double | getMaxNormalForce () const override |
double | getMinNormalForce () const override |
const ConstraintBase & | getMotionConstraint () const override |
const TaskSE3Equality & | getMotionTask () const override |
double | getMotionTaskWeight () const |
double | getNormalForce (ConstRefVector f) const override |
const Vector & | Kd () |
void | Kd (ConstRefVector Kp) |
const Vector & | Kp () |
void | Kp (ConstRefVector Kp) |
unsigned int | n_force () const override |
Return the number of force variables. More... | |
unsigned int | n_motion () const override |
Return the number of motion constraints. More... | |
bool | setContactNormal (ConstRefVector contactNormal) |
void | setForceReference (ConstRefVector &f_ref) |
bool | setFrictionCoefficient (const double frictionCoefficient) |
bool | setMaxNormalForce (const double maxNormalForce) override |
bool | setMinNormalForce (const double minNormalForce) override |
bool | setMotionTaskWeight (const double w) |
void | setReference (const SE3 &ref) |
void | setRegularizationTaskWeightVector (ConstRefVector &w) |
void | useLocalFrame (bool local_frame) |
Specifies if properties of the contact point and motion task are expressed in the local or local world oriented frame. The contact forces, contact normal and contact coefficients are interpreted in the specified frame. More... | |
![]() | |
ContactBase (const std::string &name, RobotWrapper &robot) | |
const std::string & | name () const |
void | name (const std::string &name) |
virtual | ~ContactBase ()=default |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix | ConstRefMatrix |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstraintBase | ConstraintBase |
Protected Member Functions | |
void | updateForceGeneratorMatrix () |
void | updateForceInequalityConstraints () |
void | updateForceRegularizationTask () |
Protected Attributes | |
Vector3 | m_contactNormal |
Matrix3x | m_contactPoints |
double | m_fMax |
double | m_fMin |
Matrix | m_forceGenMat |
ConstraintInequality | m_forceInequality |
ConstraintEquality | m_forceRegTask |
Vector3 | m_fRef |
Vector | m_Kd3 |
Vector | m_Kp3 |
TaskSE3Equality | m_motionTask |
double | m_motionTaskWeight |
double | m_mu |
double | m_regularizationTaskWeight |
Vector3 | m_weightForceRegTask |
![]() | |
std::string | m_name |
RobotWrapper & | m_robot |
Reference on the robot model. More... | |
Definition at line 28 of file contacts/contact-point.hpp.
Definition at line 40 of file contacts/contact-point.hpp.
Definition at line 39 of file contacts/contact-point.hpp.
typedef math::ConstRefVector tsid::contacts::ContactPoint::ConstRefVector |
Definition at line 33 of file contacts/contact-point.hpp.
Definition at line 34 of file contacts/contact-point.hpp.
Definition at line 41 of file contacts/contact-point.hpp.
Definition at line 38 of file contacts/contact-point.hpp.
Definition at line 37 of file contacts/contact-point.hpp.
Definition at line 36 of file contacts/contact-point.hpp.
Definition at line 35 of file contacts/contact-point.hpp.
ContactPoint::ContactPoint | ( | const std::string & | name, |
RobotWrapper & | robot, | ||
const std::string & | frameName, | ||
ConstRefVector | contactNormal, | ||
const double | frictionCoefficient, | ||
const double | minNormalForce, | ||
const double | maxNormalForce | ||
) |
Definition at line 29 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 211 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 202 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 195 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 95 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 227 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 209 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 231 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 217 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 216 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 223 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 219 of file src/contacts/contact-point.cpp.
double tsid::contacts::ContactPoint::getMotionTaskWeight | ( | ) | const |
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 88 of file src/contacts/contact-point.cpp.
const Vector & ContactPoint::Kd | ( | ) |
Definition at line 121 of file src/contacts/contact-point.cpp.
void ContactPoint::Kd | ( | ConstRefVector | Kp | ) |
Definition at line 134 of file src/contacts/contact-point.cpp.
const Vector & ContactPoint::Kp | ( | ) |
Definition at line 116 of file src/contacts/contact-point.cpp.
void ContactPoint::Kp | ( | ConstRefVector | Kp | ) |
Definition at line 126 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Return the number of force variables.
Implements tsid::contacts::ContactBase.
Definition at line 114 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Return the number of motion constraints.
Implements tsid::contacts::ContactBase.
Definition at line 113 of file src/contacts/contact-point.cpp.
bool ContactPoint::setContactNormal | ( | ConstRefVector | contactNormal | ) |
Definition at line 142 of file src/contacts/contact-point.cpp.
void ContactPoint::setForceReference | ( | ConstRefVector & | f_ref | ) |
Definition at line 184 of file src/contacts/contact-point.cpp.
bool ContactPoint::setFrictionCoefficient | ( | const double | frictionCoefficient | ) |
Definition at line 152 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 173 of file src/contacts/contact-point.cpp.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
Definition at line 161 of file src/contacts/contact-point.cpp.
bool tsid::contacts::ContactPoint::setMotionTaskWeight | ( | const double | w | ) |
void ContactPoint::setReference | ( | const SE3 & | ref | ) |
Definition at line 191 of file src/contacts/contact-point.cpp.
void ContactPoint::setRegularizationTaskWeightVector | ( | ConstRefVector & | w | ) |
Definition at line 99 of file src/contacts/contact-point.cpp.
|
protected |
Definition at line 111 of file src/contacts/contact-point.cpp.
|
protected |
Definition at line 61 of file src/contacts/contact-point.cpp.
|
protected |
Definition at line 104 of file src/contacts/contact-point.cpp.
void ContactPoint::useLocalFrame | ( | bool | local_frame | ) |
Specifies if properties of the contact point and motion task are expressed in the local or local world oriented frame. The contact forces, contact normal and contact coefficients are interpreted in the specified frame.
local_frame | If true, use the local frame, otherwise use the local world oriented |
Definition at line 57 of file src/contacts/contact-point.cpp.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix tsid::contacts::ContactPoint::ConstRefMatrix |
Definition at line 32 of file contacts/contact-point.hpp.
|
protected |
Definition at line 114 of file contacts/contact-point.hpp.
|
protected |
Definition at line 117 of file contacts/contact-point.hpp.
|
protected |
Definition at line 121 of file contacts/contact-point.hpp.
|
protected |
Definition at line 120 of file contacts/contact-point.hpp.
|
protected |
Definition at line 124 of file contacts/contact-point.hpp.
|
protected |
Definition at line 112 of file contacts/contact-point.hpp.
|
protected |
Definition at line 113 of file contacts/contact-point.hpp.
|
protected |
Definition at line 115 of file contacts/contact-point.hpp.
|
protected |
Definition at line 118 of file contacts/contact-point.hpp.
|
protected |
Definition at line 118 of file contacts/contact-point.hpp.
|
protected |
Definition at line 111 of file contacts/contact-point.hpp.
|
protected |
Definition at line 123 of file contacts/contact-point.hpp.
|
protected |
Definition at line 119 of file contacts/contact-point.hpp.
|
protected |
Definition at line 122 of file contacts/contact-point.hpp.
|
protected |
Definition at line 116 of file contacts/contact-point.hpp.