tsid
1.8.0
Efficient Task Space Inverse Dynamics for Multi-body Systems based on Pinocchio
|
#include <tsid/contacts/contact-point.hpp>
Public Types | |
typedef math::ConstRefVector | ConstRefVector |
typedef math::Matrix3x | Matrix3x |
typedef math::Vector6 | Vector6 |
typedef math::Vector3 | Vector3 |
typedef math::Vector | Vector |
typedef tasks::TaskSE3Equality | TaskSE3Equality |
typedef math::ConstraintInequality | ConstraintInequality |
typedef math::ConstraintEquality | ConstraintEquality |
typedef pinocchio::SE3 | SE3 |
![]() | |
typedef math::ConstraintInequality | ConstraintInequality |
typedef math::ConstraintEquality | ConstraintEquality |
typedef math::ConstRefVector | ConstRefVector |
typedef math::Matrix | Matrix |
typedef math::Matrix3x | Matrix3x |
typedef tasks::TaskSE3Equality | TaskSE3Equality |
typedef tasks::TaskMotion | TaskMotion |
typedef pinocchio::Data | Data |
typedef robots::RobotWrapper | RobotWrapper |
Public Member Functions | |
ContactPoint (const std::string &name, RobotWrapper &robot, const std::string &frameName, ConstRefVector contactNormal, const double frictionCoefficient, const double minNormalForce, const double maxNormalForce) | |
unsigned int | n_motion () const override |
Return the number of motion constraints. More... | |
unsigned int | n_force () const override |
Return the number of force variables. More... | |
const ConstraintBase & | computeMotionTask (double t, ConstRefVector q, ConstRefVector v, Data &data) override |
const ConstraintInequality & | computeForceTask (double t, ConstRefVector q, ConstRefVector v, const Data &data) override |
const Matrix & | getForceGeneratorMatrix () override |
const ConstraintEquality & | computeForceRegularizationTask (double t, ConstRefVector q, ConstRefVector v, const Data &data) override |
const TaskSE3Equality & | getMotionTask () const override |
const ConstraintBase & | getMotionConstraint () const override |
const ConstraintInequality & | getForceConstraint () const override |
const ConstraintEquality & | getForceRegularizationTask () const override |
double | getMotionTaskWeight () const |
const Matrix3x & | getContactPoints () const override |
double | getNormalForce (ConstRefVector f) const override |
double | getMinNormalForce () const override |
double | getMaxNormalForce () const override |
const Vector & | Kp () |
const Vector & | Kd () |
void | Kp (ConstRefVector Kp) |
void | Kd (ConstRefVector Kp) |
bool | setContactNormal (ConstRefVector contactNormal) |
bool | setFrictionCoefficient (const double frictionCoefficient) |
bool | setMinNormalForce (const double minNormalForce) override |
bool | setMaxNormalForce (const double maxNormalForce) override |
bool | setMotionTaskWeight (const double w) |
void | setReference (const SE3 &ref) |
void | setForceReference (ConstRefVector &f_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) | |
virtual | ~ContactBase ()=default |
const std::string & | name () const |
void | name (const std::string &name) |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix | ConstRefMatrix |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstraintBase | ConstraintBase |
Protected Member Functions | |
void | updateForceInequalityConstraints () |
void | updateForceRegularizationTask () |
void | updateForceGeneratorMatrix () |
Protected Attributes | |
TaskSE3Equality | m_motionTask |
ConstraintInequality | m_forceInequality |
ConstraintEquality | m_forceRegTask |
Vector3 | m_contactNormal |
Vector3 | m_fRef |
Vector3 | m_weightForceRegTask |
Matrix3x | m_contactPoints |
Vector | m_Kp3 |
Vector | m_Kd3 |
double | m_mu |
double | m_fMin |
double | m_fMax |
double | m_regularizationTaskWeight |
double | m_motionTaskWeight |
Matrix | m_forceGenMat |
![]() | |
std::string | m_name |
RobotWrapper & | m_robot |
Reference on the robot model. More... | |
typedef pinocchio::SE3 tsid::contacts::ContactPoint::SE3 |
ContactPoint::ContactPoint | ( | const std::string & | name, |
RobotWrapper & | robot, | ||
const std::string & | frameName, | ||
ConstRefVector | contactNormal, | ||
const double | frictionCoefficient, | ||
const double | minNormalForce, | ||
const double | maxNormalForce | ||
) |
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
double tsid::contacts::ContactPoint::getMotionTaskWeight | ( | ) | const |
|
overridevirtual |
Implements tsid::contacts::ContactBase.
const Vector & ContactPoint::Kd | ( | ) |
void ContactPoint::Kd | ( | ConstRefVector | Kp | ) |
const Vector & ContactPoint::Kp | ( | ) |
void ContactPoint::Kp | ( | ConstRefVector | Kp | ) |
|
overridevirtual |
Return the number of force variables.
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Return the number of motion constraints.
Implements tsid::contacts::ContactBase.
bool ContactPoint::setContactNormal | ( | ConstRefVector | contactNormal | ) |
void ContactPoint::setForceReference | ( | ConstRefVector & | f_ref | ) |
bool ContactPoint::setFrictionCoefficient | ( | const double | frictionCoefficient | ) |
|
overridevirtual |
Implements tsid::contacts::ContactBase.
|
overridevirtual |
Implements tsid::contacts::ContactBase.
bool tsid::contacts::ContactPoint::setMotionTaskWeight | ( | const double | w | ) |
void ContactPoint::setReference | ( | const SE3 & | ref | ) |
void ContactPoint::setRegularizationTaskWeightVector | ( | ConstRefVector & | w | ) |
|
protected |
|
protected |
|
protected |
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 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix tsid::contacts::ContactPoint::ConstRefMatrix |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |