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

#include <sot/torque_control/device-torque-ctrl.hh>

Inheritance diagram for DeviceTorqueCtrl:
Collaboration diagram for DeviceTorqueCtrl:

Public Member Functions

 DeviceTorqueCtrl (std::string RobotName)
 
virtual ~DeviceTorqueCtrl ()
 
virtual const std::string & getClassName () const
 
virtual void init (const double &dt, const std::string &urdfFile)
 
virtual void setControlInputType (const std::string &cit)
 
virtual void setState (const dynamicgraph::Vector &st)
 
virtual void setStateSize (const unsigned int &size)
 
virtual void setVelocity (const dynamicgraph::Vector &vel)
 

Static Public Attributes

static const std::string CLASS_NAME
 
static const double FORCE_SENSOR_NOISE_STD_DEV = 1e-10
 
static const double TIMESTEP_DEFAULT = 0.001
 

Protected Types

typedef Eigen::LDLT< Eigen::MatrixXd > Cholesky
 
typedef boost::normal_distribution< double > DIST
 
typedef boost::mt11213b ENG
 number of joints More...
 
typedef boost::variate_generator< ENG, DISTGEN
 

Protected Member Functions

void computeForwardDynamics ()
 
 DECLARE_SIGNAL_IN (kp_constraints, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kd_constraints, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (rotor_inertias, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (gear_ratios, dynamicgraph::Vector)
 
virtual void integrate (const double &dt)
 
void sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
 

Protected Attributes

dynamicgraph::Vector accelerometer_
 Intermediate variables to avoid allocation during control. More...
 
dynamicgraph::Signal< dynamicgraph::Vector, int > accelerometerSOUT_
 Accelerations measured by accelerometers. More...
 
dynamicgraph::Vector base6d_encoders_
 
dynamicgraph::Signal< dynamicgraph::Vector, int > currentSOUT_
 motor currents More...
 
dynamicgraph::Signal< dynamicgraph::Vector, int > d_gainsSOUT_
 
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * forcesSIN_ [4]
 input force sensor values More...
 
dynamicgraph::Vector gyrometer_
 
dynamicgraph::Signal< dynamicgraph::Vector, int > gyrometerSOUT_
 Rotation velocity measured by gyrometers. More...
 
dynamicgraph::Vector jointsAccelerations_
 joints' velocities More...
 
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsAccelerationsSOUT_
 joints' accelerations computed by the integrator More...
 
dynamicgraph::Vector jointsVelocities_
 base 6d pose + joints' angles More...
 
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsVelocitiesSOUT_
 joints' velocities computed by the integrator More...
 
tsid::tasks::TaskSE3Equality * m_contactLF
 
tsid::tasks::TaskSE3Equality * m_contactRF
 
pinocchio::Data * m_data
 
Vector m_dJcv
 svd of the constraint matrix More...
 
tsid::math::Vector m_dv
 
Vector m_dv_c
 Cholesky decomposition of _ZMZ. More...
 
tsid::math::Vector m_dv_sot
 
Vector m_dvBar
 constrained accelerations More...
 
tsid::math::Vector m_f
 
bool m_initSucceeded
 
bool m_isTorqueControlled
 
Eigen::MatrixXd m_Jc
 
Eigen::JacobiSVD< Matrix > m_Jc_svd
 
Eigen::MatrixXd m_K
 cholesky decomposition of the K matrix More...
 
Eigen::VectorXd m_k
 
Cholesky m_K_chol
 
int m_nj
 
unsigned int m_nk
 
double m_numericalDamping
 constraint Jacobian More...
 
tsid::math::Vector m_q
 
tsid::math::Vector m_q_sot
 
tsid::robots::RobotWrapper * m_robot
 robot geometric/inertial data More...
 
RobotUtilShrPtr m_robot_util
 
Vector m_tau_np6
 solution of Jc*dv=-dJc*v More...
 
tsid::math::Vector m_v
 
tsid::math::Vector m_v_sot
 
Matrix m_Z
 
Matrix m_ZMZ
 base of constraint null space More...
 
Eigen::LDLT< Matrix > m_ZMZ_chol
 projected mass matrix: Z_c^T*M*Z_c More...
 
DIST normalDistribution_
 
GEN normalRandomNumberGenerator_
 
dynamicgraph::Signal< dynamicgraph::Vector, int > p_gainsSOUT_
 proportional and derivative position-control gains More...
 
ENG randomNumberGenerator_
 
dynamicgraph::Signal< dynamicgraph::Vector, int > robotStateSOUT_
 base 6d pose + joints' angles measured by encoders More...
 
dynamicgraph::Vector temp6_
 
double timestep_
 Current integration step. More...
 
dynamicgraph::Vector wrenches_ [4]
 joints' accelerations More...
 

Detailed Description

Version of device for testing the code without the real robot. This version of the device assumes that the robot is torque controlled (i.e. the control input are the desired joint torques). These desired joint torques are used to compute the joint accelerations and the contact forces. The joint accelerations are then integrated to get the measured joint angles.

The feet are supposed to be fixed to the ground. The accelerometer and the gyrometer output a constant value.

A white Gaussian noise is added to the force/torque sensor measurements.

TODO: The original Device class should be a clean abstraction for the concept of device, but currently it is not. It defines a lot of input/output signals that are specific of HRP-2 (e.g. zmpSIN, attitudeSIN, forcesSOUT) and some design choices (e.g. secondOrderIntegration). It would be nice to clean the original Device from all this stuff and move it in the specific subclasses.

Definition at line 59 of file device-torque-ctrl.hh.

Member Typedef Documentation

◆ Cholesky

typedef Eigen::LDLT<Eigen::MatrixXd> Cholesky
protected

Definition at line 137 of file device-torque-ctrl.hh.

◆ DIST

typedef boost::normal_distribution<double> DIST
protected

Definition at line 156 of file device-torque-ctrl.hh.

◆ ENG

typedef boost::mt11213b ENG
protected

number of joints

Definition at line 155 of file device-torque-ctrl.hh.

◆ GEN

typedef boost::variate_generator<ENG, DIST> GEN
protected

Definition at line 157 of file device-torque-ctrl.hh.

Constructor & Destructor Documentation

◆ DeviceTorqueCtrl()

DeviceTorqueCtrl ( std::string  RobotName)

Definition at line 32 of file device-torque-ctrl.cpp.

◆ ~DeviceTorqueCtrl()

~DeviceTorqueCtrl ( )
virtual

Definition at line 120 of file device-torque-ctrl.cpp.

Member Function Documentation

◆ computeForwardDynamics()

void computeForwardDynamics ( )
protected

Definition at line 252 of file device-torque-ctrl.cpp.

◆ DECLARE_SIGNAL_IN() [1/4]

DECLARE_SIGNAL_IN ( kp_constraints  ,
dynamicgraph::Vector   
)
protected

◆ DECLARE_SIGNAL_IN() [2/4]

DECLARE_SIGNAL_IN ( kd_constraints  ,
dynamicgraph::Vector   
)
protected

◆ DECLARE_SIGNAL_IN() [3/4]

DECLARE_SIGNAL_IN ( rotor_inertias  ,
dynamicgraph::Vector   
)
protected

◆ DECLARE_SIGNAL_IN() [4/4]

DECLARE_SIGNAL_IN ( gear_ratios  ,
dynamicgraph::Vector   
)
protected

◆ getClassName()

virtual const std::string& getClassName ( ) const
inlinevirtual

Definition at line 65 of file device-torque-ctrl.hh.

◆ init()

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

Definition at line 122 of file device-torque-ctrl.cpp.

◆ integrate()

void integrate ( const double &  dt)
protectedvirtual

Definition at line 326 of file device-torque-ctrl.cpp.

◆ sendMsg()

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

Definition at line 81 of file device-torque-ctrl.hh.

◆ setControlInputType()

void setControlInputType ( const std::string &  cit)
virtual

Definition at line 243 of file device-torque-ctrl.cpp.

◆ setState()

void setState ( const dynamicgraph::Vector &  st)
virtual

Definition at line 204 of file device-torque-ctrl.cpp.

◆ setStateSize()

void setStateSize ( const unsigned int &  size)
virtual

Definition at line 189 of file device-torque-ctrl.cpp.

◆ setVelocity()

void setVelocity ( const dynamicgraph::Vector &  vel)
virtual

Definition at line 232 of file device-torque-ctrl.cpp.

Member Data Documentation

◆ accelerometer_

dynamicgraph::Vector accelerometer_
protected

Intermediate variables to avoid allocation during control.

Definition at line 115 of file device-torque-ctrl.hh.

◆ accelerometerSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> accelerometerSOUT_
protected

Accelerations measured by accelerometers.

Definition at line 94 of file device-torque-ctrl.hh.

◆ base6d_encoders_

dynamicgraph::Vector base6d_encoders_
protected

Definition at line 118 of file device-torque-ctrl.hh.

◆ CLASS_NAME

const std::string CLASS_NAME
static

Definition at line 61 of file device-torque-ctrl.hh.

◆ currentSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> currentSOUT_
protected

motor currents

Definition at line 104 of file device-torque-ctrl.hh.

◆ d_gainsSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> d_gainsSOUT_
protected

Definition at line 107 of file device-torque-ctrl.hh.

◆ FORCE_SENSOR_NOISE_STD_DEV

const double FORCE_SENSOR_NOISE_STD_DEV = 1e-10
static

Definition at line 63 of file device-torque-ctrl.hh.

◆ forcesSIN_

dynamicgraph::SignalPtr<dynamicgraph::Vector, int>* forcesSIN_[4]
protected

input force sensor values

Definition at line 91 of file device-torque-ctrl.hh.

◆ gyrometer_

dynamicgraph::Vector gyrometer_
protected

Definition at line 116 of file device-torque-ctrl.hh.

◆ gyrometerSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> gyrometerSOUT_
protected

Rotation velocity measured by gyrometers.

Definition at line 96 of file device-torque-ctrl.hh.

◆ jointsAccelerations_

dynamicgraph::Vector jointsAccelerations_
protected

joints' velocities

Definition at line 120 of file device-torque-ctrl.hh.

◆ jointsAccelerationsSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> jointsAccelerationsSOUT_
protected

joints' accelerations computed by the integrator

Definition at line 102 of file device-torque-ctrl.hh.

◆ jointsVelocities_

dynamicgraph::Vector jointsVelocities_
protected

base 6d pose + joints' angles

Definition at line 119 of file device-torque-ctrl.hh.

◆ jointsVelocitiesSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> jointsVelocitiesSOUT_
protected

joints' velocities computed by the integrator

Definition at line 100 of file device-torque-ctrl.hh.

◆ m_contactLF

tsid::tasks::TaskSE3Equality* m_contactLF
protected

Definition at line 131 of file device-torque-ctrl.hh.

◆ m_contactRF

tsid::tasks::TaskSE3Equality* m_contactRF
protected

Definition at line 130 of file device-torque-ctrl.hh.

◆ m_data

pinocchio::Data* m_data
protected

Definition at line 129 of file device-torque-ctrl.hh.

◆ m_dJcv

Vector m_dJcv
protected

svd of the constraint matrix

Definition at line 146 of file device-torque-ctrl.hh.

◆ m_dv

tsid::math::Vector m_dv
protected

Definition at line 134 of file device-torque-ctrl.hh.

◆ m_dv_c

Vector m_dv_c
protected

Cholesky decomposition of _ZMZ.

Definition at line 150 of file device-torque-ctrl.hh.

◆ m_dv_sot

tsid::math::Vector m_dv_sot
protected

Definition at line 135 of file device-torque-ctrl.hh.

◆ m_dvBar

Vector m_dvBar
protected

constrained accelerations

Definition at line 151 of file device-torque-ctrl.hh.

◆ m_f

tsid::math::Vector m_f
protected

Definition at line 134 of file device-torque-ctrl.hh.

◆ m_initSucceeded

bool m_initSucceeded
protected

Definition at line 88 of file device-torque-ctrl.hh.

◆ m_isTorqueControlled

bool m_isTorqueControlled
protected

Definition at line 125 of file device-torque-ctrl.hh.

◆ m_Jc

Eigen::MatrixXd m_Jc
protected

Definition at line 141 of file device-torque-ctrl.hh.

◆ m_Jc_svd

Eigen::JacobiSVD<Matrix> m_Jc_svd
protected

numerical damping to regularize constraint resolution

Definition at line 145 of file device-torque-ctrl.hh.

◆ m_K

Eigen::MatrixXd m_K
protected

cholesky decomposition of the K matrix

Definition at line 139 of file device-torque-ctrl.hh.

◆ m_k

Eigen::VectorXd m_k
protected

Definition at line 140 of file device-torque-ctrl.hh.

◆ m_K_chol

Cholesky m_K_chol
protected

Definition at line 138 of file device-torque-ctrl.hh.

◆ m_nj

int m_nj
protected

Definition at line 153 of file device-torque-ctrl.hh.

◆ m_nk

unsigned int m_nk
protected

Definition at line 132 of file device-torque-ctrl.hh.

◆ m_numericalDamping

double m_numericalDamping
protected

constraint Jacobian

Definition at line 143 of file device-torque-ctrl.hh.

◆ m_q

tsid::math::Vector m_q
protected

Definition at line 134 of file device-torque-ctrl.hh.

◆ m_q_sot

tsid::math::Vector m_q_sot
protected

Definition at line 135 of file device-torque-ctrl.hh.

◆ m_robot

tsid::robots::RobotWrapper* m_robot
protected

robot geometric/inertial data

Definition at line 128 of file device-torque-ctrl.hh.

◆ m_robot_util

RobotUtilShrPtr m_robot_util
protected

Definition at line 162 of file device-torque-ctrl.hh.

◆ m_tau_np6

Vector m_tau_np6
protected

solution of Jc*dv=-dJc*v

Definition at line 152 of file device-torque-ctrl.hh.

◆ m_v

tsid::math::Vector m_v
protected

Definition at line 134 of file device-torque-ctrl.hh.

◆ m_v_sot

tsid::math::Vector m_v_sot
protected

Definition at line 135 of file device-torque-ctrl.hh.

◆ m_Z

Matrix m_Z
protected

Definition at line 147 of file device-torque-ctrl.hh.

◆ m_ZMZ

Matrix m_ZMZ
protected

base of constraint null space

Definition at line 148 of file device-torque-ctrl.hh.

◆ m_ZMZ_chol

Eigen::LDLT<Matrix> m_ZMZ_chol
protected

projected mass matrix: Z_c^T*M*Z_c

Definition at line 149 of file device-torque-ctrl.hh.

◆ normalDistribution_

DIST normalDistribution_
protected

Definition at line 159 of file device-torque-ctrl.hh.

◆ normalRandomNumberGenerator_

GEN normalRandomNumberGenerator_
protected

Definition at line 160 of file device-torque-ctrl.hh.

◆ p_gainsSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> p_gainsSOUT_
protected

proportional and derivative position-control gains

Definition at line 106 of file device-torque-ctrl.hh.

◆ randomNumberGenerator_

ENG randomNumberGenerator_
protected

Definition at line 158 of file device-torque-ctrl.hh.

◆ robotStateSOUT_

dynamicgraph::Signal<dynamicgraph::Vector, int> robotStateSOUT_
protected

base 6d pose + joints' angles measured by encoders

Definition at line 98 of file device-torque-ctrl.hh.

◆ temp6_

dynamicgraph::Vector temp6_
protected

Definition at line 123 of file device-torque-ctrl.hh.

◆ timestep_

double timestep_
protected

Current integration step.

Definition at line 87 of file device-torque-ctrl.hh.

◆ TIMESTEP_DEFAULT

const double TIMESTEP_DEFAULT = 0.001
static

Definition at line 62 of file device-torque-ctrl.hh.

◆ wrenches_

dynamicgraph::Vector wrenches_[4]
protected

joints' accelerations

Definition at line 122 of file device-torque-ctrl.hh.


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