sot-talos-balance  1.7.0
DistributeWrench Class Reference

#include <sot/talos_balance/distribute-wrench.hh>

Inheritance diagram for DistributeWrench:
[legend]
Collaboration diagram for DistributeWrench:
[legend]

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW DistributeWrench (const std::string &name)
 
Eigen::Vector3d computeCoP (const dynamicgraph::Vector &wrench, const pinocchio::SE3 &pose) const
 
 DECLARE_SIGNAL_IN (wrenchDes, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (q, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (rho, double)
 
 DECLARE_SIGNAL_IN (phase, int)
 
 DECLARE_SIGNAL_IN (frictionCoefficient, double)
 
 DECLARE_SIGNAL_IN (wSum, double)
 
 DECLARE_SIGNAL_IN (wNorm, double)
 
 DECLARE_SIGNAL_IN (wRatio, double)
 
 DECLARE_SIGNAL_IN (wAnkle, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_INNER (kinematics_computations, int)
 
 DECLARE_SIGNAL_INNER (qp_computations, int)
 
 DECLARE_SIGNAL_OUT (wrenchLeft, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (ankleWrenchLeft, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (surfaceWrenchLeft, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (copLeft, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (wrenchRight, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (ankleWrenchRight, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (surfaceWrenchRight, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (copRight, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (wrenchRef, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (zmpRef, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (emergencyStop, bool)
 
virtual void display (std::ostream &os) const
 
void init (const std::string &robotName)
 
void set_left_foot_sizes (const dynamicgraph::Vector &s)
 
void set_right_foot_sizes (const dynamicgraph::Vector &s)
 

Public Attributes

double m_eps
 

Protected Member Functions

void computeWrenchFaceMatrix (const double mu)
 sizes of the left foot (pos x, neg x, pos y, neg y) More...
 
void distributeWrench (const Eigen::VectorXd &wrenchDes, const double rho, const double mu)
 
void saturateWrench (const Eigen::VectorXd &wrenchDes, const int phase, const double mu)
 

Protected Attributes

Eigen::MatrixXd m_Aeq1
 
Eigen::MatrixXd m_Aeq2
 
Eigen::MatrixXd m_Aineq1
 
Eigen::MatrixXd m_Aineq2
 
pinocchio::SE3 m_ankle_M_sole
 
Eigen::VectorXd m_Beq1
 
Eigen::VectorXd m_Beq2
 
Eigen::VectorXd m_Bineq1
 
Eigen::VectorXd m_Bineq2
 
Eigen::VectorXd m_C1
 
Eigen::VectorXd m_C2
 
pinocchio::SE3 m_contactLeft
 
pinocchio::SE3 m_contactRight
 
pinocchio::Data m_data
 Pinocchio robot model. More...
 
bool m_emergency_stop_triggered
 
bool m_initSucceeded
 
pinocchio::FrameIndex m_left_foot_id
 ankle to sole transformation More...
 
Eigen::Vector4d m_left_foot_sizes
 
pinocchio::Model m_model
 true if the entity has been successfully initialized More...
 
Eigen::MatrixXd m_Q1
 
Eigen::MatrixXd m_Q2
 
Eigen::QuadProgDense m_qp1
 
Eigen::QuadProgDense m_qp2
 
pinocchio::FrameIndex m_right_foot_id
 
Eigen::Vector4d m_right_foot_sizes
 sizes of the left foot (pos x, neg x, pos y, neg y) More...
 
RobotUtilShrPtr m_robot_util
 Pinocchio robot data. More...
 
Eigen::VectorXd m_wAnkle
 
double m_wNorm
 
double m_wRatio
 
Eigen::Matrix< double, 16, 6 > m_wrenchFaceMatrix
 
Eigen::Matrix< double, 6, 1 > m_wrenchLeft
 
Eigen::Matrix< double, 6, 1 > m_wrenchRight
 
double m_wSum
 

Detailed Description

Definition at line 59 of file distribute-wrench.hh.

Constructor & Destructor Documentation

◆ DistributeWrench()

Member Function Documentation

◆ computeCoP()

Eigen::Vector3d computeCoP ( const dynamicgraph::Vector &  wrench,
const pinocchio::SE3 &  pose 
) const

◆ computeWrenchFaceMatrix()

void computeWrenchFaceMatrix ( const double  mu)
protected

sizes of the left foot (pos x, neg x, pos y, neg y)

Definition at line 192 of file distribute-wrench.cpp.

References DistributeWrench::m_right_foot_sizes, and DistributeWrench::m_wrenchFaceMatrix.

Referenced by DistributeWrench::distributeWrench(), and DistributeWrench::saturateWrench().

◆ DECLARE_SIGNAL_IN() [1/9]

DECLARE_SIGNAL_IN ( wrenchDes  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [2/9]

DECLARE_SIGNAL_IN ( ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [3/9]

DECLARE_SIGNAL_IN ( rho  ,
double   
)

◆ DECLARE_SIGNAL_IN() [4/9]

DECLARE_SIGNAL_IN ( phase  ,
int   
)

◆ DECLARE_SIGNAL_IN() [5/9]

DECLARE_SIGNAL_IN ( frictionCoefficient  ,
double   
)

◆ DECLARE_SIGNAL_IN() [6/9]

DECLARE_SIGNAL_IN ( wSum  ,
double   
)

◆ DECLARE_SIGNAL_IN() [7/9]

DECLARE_SIGNAL_IN ( wNorm  ,
double   
)

◆ DECLARE_SIGNAL_IN() [8/9]

DECLARE_SIGNAL_IN ( wRatio  ,
double   
)

◆ DECLARE_SIGNAL_IN() [9/9]

DECLARE_SIGNAL_IN ( wAnkle  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_INNER() [1/2]

DECLARE_SIGNAL_INNER ( kinematics_computations  ,
int   
)

◆ DECLARE_SIGNAL_INNER() [2/2]

DECLARE_SIGNAL_INNER ( qp_computations  ,
int   
)

◆ DECLARE_SIGNAL_OUT() [1/11]

DECLARE_SIGNAL_OUT ( wrenchLeft  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [2/11]

DECLARE_SIGNAL_OUT ( ankleWrenchLeft  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [3/11]

DECLARE_SIGNAL_OUT ( surfaceWrenchLeft  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [4/11]

DECLARE_SIGNAL_OUT ( copLeft  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [5/11]

DECLARE_SIGNAL_OUT ( wrenchRight  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [6/11]

DECLARE_SIGNAL_OUT ( ankleWrenchRight  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [7/11]

DECLARE_SIGNAL_OUT ( surfaceWrenchRight  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [8/11]

DECLARE_SIGNAL_OUT ( copRight  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [9/11]

DECLARE_SIGNAL_OUT ( wrenchRef  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [10/11]

DECLARE_SIGNAL_OUT ( zmpRef  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [11/11]

DECLARE_SIGNAL_OUT ( emergencyStop  ,
bool   
)

◆ display()

void display ( std::ostream &  os) const
virtual

Definition at line 688 of file distribute-wrench.cpp.

◆ distributeWrench()

◆ init()

◆ saturateWrench()

◆ set_left_foot_sizes()

void set_left_foot_sizes ( const dynamicgraph::Vector &  s)

◆ set_right_foot_sizes()

void set_right_foot_sizes ( const dynamicgraph::Vector &  s)

Member Data Documentation

◆ m_Aeq1

Eigen::MatrixXd m_Aeq1
protected

Definition at line 143 of file distribute-wrench.hh.

Referenced by DistributeWrench::saturateWrench().

◆ m_Aeq2

Eigen::MatrixXd m_Aeq2
protected

Definition at line 153 of file distribute-wrench.hh.

Referenced by DistributeWrench::distributeWrench().

◆ m_Aineq1

Eigen::MatrixXd m_Aineq1
protected

Definition at line 146 of file distribute-wrench.hh.

Referenced by DistributeWrench::saturateWrench().

◆ m_Aineq2

Eigen::MatrixXd m_Aineq2
protected

Definition at line 156 of file distribute-wrench.hh.

Referenced by DistributeWrench::distributeWrench().

◆ m_ankle_M_sole

pinocchio::SE3 m_ankle_M_sole
protected

◆ m_Beq1

Eigen::VectorXd m_Beq1
protected

Definition at line 144 of file distribute-wrench.hh.

Referenced by DistributeWrench::saturateWrench().

◆ m_Beq2

Eigen::VectorXd m_Beq2
protected

Definition at line 154 of file distribute-wrench.hh.

Referenced by DistributeWrench::distributeWrench().

◆ m_Bineq1

Eigen::VectorXd m_Bineq1
protected

Definition at line 147 of file distribute-wrench.hh.

Referenced by DistributeWrench::saturateWrench().

◆ m_Bineq2

Eigen::VectorXd m_Bineq2
protected

Definition at line 157 of file distribute-wrench.hh.

Referenced by DistributeWrench::distributeWrench().

◆ m_C1

Eigen::VectorXd m_C1
protected

Definition at line 141 of file distribute-wrench.hh.

Referenced by DistributeWrench::saturateWrench().

◆ m_C2

Eigen::VectorXd m_C2
protected

Definition at line 151 of file distribute-wrench.hh.

Referenced by DistributeWrench::distributeWrench().

◆ m_contactLeft

◆ m_contactRight

◆ m_data

◆ m_emergency_stop_triggered

◆ m_eps

◆ m_initSucceeded

◆ m_left_foot_id

◆ m_left_foot_sizes

Eigen::Vector4d m_left_foot_sizes
protected

◆ m_model

pinocchio::Model m_model
protected

true if the entity has been successfully initialized

Definition at line 114 of file distribute-wrench.hh.

Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and DistributeWrench::init().

◆ m_Q1

Eigen::MatrixXd m_Q1
protected

Definition at line 140 of file distribute-wrench.hh.

Referenced by DistributeWrench::saturateWrench().

◆ m_Q2

Eigen::MatrixXd m_Q2
protected

Definition at line 150 of file distribute-wrench.hh.

Referenced by DistributeWrench::distributeWrench().

◆ m_qp1

Eigen::QuadProgDense m_qp1
protected

◆ m_qp2

Eigen::QuadProgDense m_qp2
protected

◆ m_right_foot_id

◆ m_right_foot_sizes

Eigen::Vector4d m_right_foot_sizes
protected

sizes of the left foot (pos x, neg x, pos y, neg y)

Definition at line 131 of file distribute-wrench.hh.

Referenced by DistributeWrench::computeWrenchFaceMatrix(), DistributeWrench::init(), and DistributeWrench::set_right_foot_sizes().

◆ m_robot_util

RobotUtilShrPtr m_robot_util
protected

Pinocchio robot data.

Definition at line 116 of file distribute-wrench.hh.

Referenced by DistributeWrench::init().

◆ m_wAnkle

Eigen::VectorXd m_wAnkle
protected

◆ m_wNorm

◆ m_wRatio

◆ m_wrenchFaceMatrix

Eigen::Matrix<double, 16, 6> m_wrenchFaceMatrix
protected

◆ m_wrenchLeft

Eigen::Matrix<double,6,1> m_wrenchLeft
protected

◆ m_wrenchRight

Eigen::Matrix<double,6,1> m_wrenchRight
protected

◆ m_wSum


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