sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
AdmittanceController Class Reference

#include <sot/torque_control/admittance-controller.hh>

Inheritance diagram for AdmittanceController:
Collaboration diagram for AdmittanceController:

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW AdmittanceController (const std::string &name)
 
 DECLARE_SIGNAL_IN (controlledJoints, dynamicgraph::Vector)
 6d estimated force filtered More...
 
 DECLARE_SIGNAL_IN (damping, dynamicgraph::Vector)
 mask with 1 for controlled joints, 0 otherwise More...
 
 DECLARE_SIGNAL_IN (encoders, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (fLeftFoot, dynamicgraph::Vector)
 6d estimated force More...
 
 DECLARE_SIGNAL_IN (fLeftFootFiltered, dynamicgraph::Vector)
 6d estimated force filtered More...
 
 DECLARE_SIGNAL_IN (fLeftFootRef, dynamicgraph::Vector)
 6d reference force More...
 
 DECLARE_SIGNAL_IN (force_integral_deadzone, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (force_integral_saturation, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (fRightFoot, dynamicgraph::Vector)
 6d reference force More...
 
 DECLARE_SIGNAL_IN (fRightFootFiltered, dynamicgraph::Vector)
 6d estimated force More...
 
 DECLARE_SIGNAL_IN (fRightFootRef, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (jointsVelocities, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (ki_force, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (ki_vel, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kp_force, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kp_vel, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (dqDes, dynamicgraph::Vector)
 control More...
 
 DECLARE_SIGNAL_OUT (u, dynamicgraph::Vector)
 damping factors used for the 4 end-effectors More...
 
 DECLARE_SIGNAL_OUT (vDesLeftFoot, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (vDesRightFoot, dynamicgraph::Vector)
 dqDes = J^+ * Kf * (fRef-f) More...
 
virtual void display (std::ostream &os) const
 
void init (const double &dt, const std::string &robotRef)
 
void sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
 

Protected Types

typedef pinocchio::Data::Matrix6x Matrix6x
 previous value of encoders More...
 

Protected Attributes

pinocchio::Data * m_data
 
tsid::math::Vector m_dq_des_urdf
 
tsid::math::Vector m_dq_fd
 integral of the velocity error More...
 
tsid::math::Vector m_dqErrIntegral
 
double m_dt
 if true it uses the Jacobian transpose rather than the pseudoinverse More...
 
tsid::math::Vector6 m_f_LF
 desired 6d wrench right foot More...
 
tsid::math::Vector6 m_f_RF
 
bool m_firstIter
 control (i.e. motor currents) More...
 
int m_frame_id_lf
 frame id of right foot More...
 
int m_frame_id_rf
 robot geometric/inertial data More...
 
bool m_initSucceeded
 
Matrix6x m_J_LF
 
Eigen::ColPivHouseholderQR< Matrix6xm_J_LF_QR
 
Matrix6x m_J_RF
 
Eigen::ColPivHouseholderQR< Matrix6xm_J_RF_QR
 
int m_nj
 control loop time period More...
 
tsid::math::Vector m_q_urdf
 desired 6d wrench left foot More...
 
tsid::math::Vector m_qPrev
 joint velocities computed with finite differences More...
 
tsid::robots::RobotWrapper * m_robot
 frame id of left foot More...
 
RobotUtilShrPtr m_robot_util
 
Eigen::VectorXd m_u
 
bool m_useJacobianTranspose
 true if the entity has been successfully initialized More...
 
tsid::math::Vector6 m_v_LF_int
 
tsid::math::Vector6 m_v_RF_int
 
tsid::math::Vector m_v_urdf
 

Detailed Description

Definition at line 46 of file admittance-controller.hh.

Member Typedef Documentation

◆ Matrix6x

typedef pinocchio::Data::Matrix6x Matrix6x
protected

previous value of encoders

Definition at line 124 of file admittance-controller.hh.

Constructor & Destructor Documentation

◆ AdmittanceController()

AdmittanceController ( const std::string &  name)

Definition at line 54 of file admittance-controller.cpp.

Member Function Documentation

◆ DECLARE_SIGNAL_IN() [1/16]

DECLARE_SIGNAL_IN ( controlledJoints  ,
dynamicgraph::Vector   
)

6d estimated force filtered

◆ DECLARE_SIGNAL_IN() [2/16]

DECLARE_SIGNAL_IN ( damping  ,
dynamicgraph::Vector   
)

mask with 1 for controlled joints, 0 otherwise

◆ DECLARE_SIGNAL_IN() [3/16]

DECLARE_SIGNAL_IN ( encoders  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [4/16]

DECLARE_SIGNAL_IN ( fLeftFoot  ,
dynamicgraph::Vector   
)

6d estimated force

◆ DECLARE_SIGNAL_IN() [5/16]

DECLARE_SIGNAL_IN ( fLeftFootFiltered  ,
dynamicgraph::Vector   
)

6d estimated force filtered

◆ DECLARE_SIGNAL_IN() [6/16]

DECLARE_SIGNAL_IN ( fLeftFootRef  ,
dynamicgraph::Vector   
)

6d reference force

◆ DECLARE_SIGNAL_IN() [7/16]

DECLARE_SIGNAL_IN ( force_integral_deadzone  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [8/16]

DECLARE_SIGNAL_IN ( force_integral_saturation  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [9/16]

DECLARE_SIGNAL_IN ( fRightFoot  ,
dynamicgraph::Vector   
)

6d reference force

◆ DECLARE_SIGNAL_IN() [10/16]

DECLARE_SIGNAL_IN ( fRightFootFiltered  ,
dynamicgraph::Vector   
)

6d estimated force

◆ DECLARE_SIGNAL_IN() [11/16]

DECLARE_SIGNAL_IN ( fRightFootRef  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [12/16]

DECLARE_SIGNAL_IN ( jointsVelocities  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [13/16]

DECLARE_SIGNAL_IN ( ki_force  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [14/16]

DECLARE_SIGNAL_IN ( ki_vel  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [15/16]

DECLARE_SIGNAL_IN ( kp_force  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [16/16]

DECLARE_SIGNAL_IN ( kp_vel  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [1/4]

DECLARE_SIGNAL_OUT ( dqDes  ,
dynamicgraph::Vector   
)

control

◆ DECLARE_SIGNAL_OUT() [2/4]

DECLARE_SIGNAL_OUT ( ,
dynamicgraph::Vector   
)

damping factors used for the 4 end-effectors

◆ DECLARE_SIGNAL_OUT() [3/4]

DECLARE_SIGNAL_OUT ( vDesLeftFoot  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [4/4]

DECLARE_SIGNAL_OUT ( vDesRightFoot  ,
dynamicgraph::Vector   
)

dqDes = J^+ * Kf * (fRef-f)

◆ display()

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

Definition at line 366 of file admittance-controller.cpp.

◆ init()

void init ( const double &  dt,
const std::string &  robotRef 
)

Definition at line 110 of file admittance-controller.cpp.

◆ sendMsg()

void sendMsg ( const std::string &  msg,
MsgType  t = MSG_TYPE_INFO,
const char *  = "",
int  = 0 
)
inline

Definition at line 93 of file admittance-controller.hh.

Member Data Documentation

◆ m_data

pinocchio::Data* m_data
protected

Definition at line 111 of file admittance-controller.hh.

◆ m_dq_des_urdf

tsid::math::Vector m_dq_des_urdf
protected

Definition at line 118 of file admittance-controller.hh.

◆ m_dq_fd

tsid::math::Vector m_dq_fd
protected

integral of the velocity error

Definition at line 121 of file admittance-controller.hh.

◆ m_dqErrIntegral

tsid::math::Vector m_dqErrIntegral
protected

Definition at line 119 of file admittance-controller.hh.

◆ m_dt

double m_dt
protected

if true it uses the Jacobian transpose rather than the pseudoinverse

Definition at line 102 of file admittance-controller.hh.

◆ m_f_LF

tsid::math::Vector6 m_f_LF
protected

desired 6d wrench right foot

Definition at line 114 of file admittance-controller.hh.

◆ m_f_RF

tsid::math::Vector6 m_f_RF
protected

Definition at line 113 of file admittance-controller.hh.

◆ m_firstIter

bool m_firstIter
protected

control (i.e. motor currents)

Definition at line 99 of file admittance-controller.hh.

◆ m_frame_id_lf

int m_frame_id_lf
protected

frame id of right foot

Definition at line 107 of file admittance-controller.hh.

◆ m_frame_id_rf

int m_frame_id_rf
protected

robot geometric/inertial data

Definition at line 106 of file admittance-controller.hh.

◆ m_initSucceeded

bool m_initSucceeded
protected

Definition at line 100 of file admittance-controller.hh.

◆ m_J_LF

Matrix6x m_J_LF
protected

Definition at line 126 of file admittance-controller.hh.

◆ m_J_LF_QR

Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR
protected

Definition at line 128 of file admittance-controller.hh.

◆ m_J_RF

Matrix6x m_J_RF
protected

Definition at line 125 of file admittance-controller.hh.

◆ m_J_RF_QR

Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR
protected

Definition at line 127 of file admittance-controller.hh.

◆ m_nj

int m_nj
protected

control loop time period

Definition at line 103 of file admittance-controller.hh.

◆ m_q_urdf

tsid::math::Vector m_q_urdf
protected

desired 6d wrench left foot

Definition at line 115 of file admittance-controller.hh.

◆ m_qPrev

tsid::math::Vector m_qPrev
protected

joint velocities computed with finite differences

Definition at line 122 of file admittance-controller.hh.

◆ m_robot

tsid::robots::RobotWrapper* m_robot
protected

frame id of left foot

tsid

Definition at line 110 of file admittance-controller.hh.

◆ m_robot_util

RobotUtilShrPtr m_robot_util
protected

Definition at line 132 of file admittance-controller.hh.

◆ m_u

Eigen::VectorXd m_u
protected

Definition at line 98 of file admittance-controller.hh.

◆ m_useJacobianTranspose

bool m_useJacobianTranspose
protected

true if the entity has been successfully initialized

Definition at line 101 of file admittance-controller.hh.

◆ m_v_LF_int

tsid::math::Vector6 m_v_LF_int
protected

Definition at line 130 of file admittance-controller.hh.

◆ m_v_RF_int

tsid::math::Vector6 m_v_RF_int
protected

Definition at line 129 of file admittance-controller.hh.

◆ m_v_urdf

tsid::math::Vector m_v_urdf
protected

Definition at line 116 of file admittance-controller.hh.


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