sot-torque-control  1.5.2
DeviceTorqueCtrl Class Reference

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

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

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 (gear_ratios, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kd_constraints, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (kp_constraints, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (rotor_inertias, 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
 numerical damping to regularize constraint resolution More...
 
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 56 of file device-torque-ctrl.hh.

Member Typedef Documentation

◆ Cholesky

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

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

◆ DIST

typedef boost::normal_distribution<double> DIST
protected

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

◆ ENG

typedef boost::mt11213b ENG
protected

number of joints

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

◆ GEN

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

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

Constructor & Destructor Documentation

◆ DeviceTorqueCtrl()

◆ ~DeviceTorqueCtrl()

~DeviceTorqueCtrl ( )
virtual

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

Member Function Documentation

◆ computeForwardDynamics()

◆ DECLARE_SIGNAL_IN() [1/4]

DECLARE_SIGNAL_IN ( gear_ratios  ,
dynamicgraph::Vector   
)
protected

◆ DECLARE_SIGNAL_IN() [2/4]

DECLARE_SIGNAL_IN ( kd_constraints  ,
dynamicgraph::Vector   
)
protected

◆ DECLARE_SIGNAL_IN() [3/4]

DECLARE_SIGNAL_IN ( kp_constraints  ,
dynamicgraph::Vector   
)
protected

◆ DECLARE_SIGNAL_IN() [4/4]

DECLARE_SIGNAL_IN ( rotor_inertias  ,
dynamicgraph::Vector   
)
protected

◆ getClassName()

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

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

References DeviceTorqueCtrl::CLASS_NAME.

◆ init()

◆ integrate()

◆ sendMsg()

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

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

◆ setControlInputType()

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

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

References DeviceTorqueCtrl::m_isTorqueControlled.

◆ setState()

◆ setStateSize()

◆ setVelocity()

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

Member Data Documentation

◆ accelerometer_

dynamicgraph::Vector accelerometer_
protected

Intermediate variables to avoid allocation during control.

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

◆ accelerometerSOUT_

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

Accelerations measured by accelerometers.

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl(), and DeviceTorqueCtrl::integrate().

◆ base6d_encoders_

dynamicgraph::Vector base6d_encoders_
protected

◆ CLASS_NAME

const std::string CLASS_NAME
static

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

Referenced by DeviceTorqueCtrl::getClassName().

◆ currentSOUT_

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

motor currents

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl().

◆ d_gainsSOUT_

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

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl().

◆ FORCE_SENSOR_NOISE_STD_DEV

const double FORCE_SENSOR_NOISE_STD_DEV = 1e-10
static

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

◆ forcesSIN_

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

input force sensor values

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl(), and DeviceTorqueCtrl::integrate().

◆ gyrometer_

dynamicgraph::Vector gyrometer_
protected

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

◆ gyrometerSOUT_

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

Rotation velocity measured by gyrometers.

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl(), and DeviceTorqueCtrl::integrate().

◆ jointsAccelerations_

dynamicgraph::Vector jointsAccelerations_
protected

joints' velocities

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

Referenced by DeviceTorqueCtrl::integrate(), and DeviceTorqueCtrl::setStateSize().

◆ jointsAccelerationsSOUT_

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

joints' accelerations computed by the integrator

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl(), DeviceTorqueCtrl::integrate(), and DeviceTorqueCtrl::setStateSize().

◆ jointsVelocities_

dynamicgraph::Vector jointsVelocities_
protected

base 6d pose + joints' angles

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

Referenced by DeviceTorqueCtrl::integrate(), and DeviceTorqueCtrl::setStateSize().

◆ jointsVelocitiesSOUT_

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

joints' velocities computed by the integrator

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl(), DeviceTorqueCtrl::integrate(), and DeviceTorqueCtrl::setStateSize().

◆ m_contactLF

tsid::tasks::TaskSE3Equality* m_contactLF
protected

◆ m_contactRF

tsid::tasks::TaskSE3Equality* m_contactRF
protected

◆ m_data

pinocchio::Data* m_data
protected

◆ m_dJcv

Vector m_dJcv
protected

svd of the constraint matrix

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics(), and DeviceTorqueCtrl::init().

◆ m_dv

tsid::math::Vector m_dv
protected

◆ m_dv_c

Vector m_dv_c
protected

Cholesky decomposition of _ZMZ.

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics().

◆ m_dv_sot

tsid::math::Vector m_dv_sot
protected

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

Referenced by DeviceTorqueCtrl::init(), and DeviceTorqueCtrl::integrate().

◆ m_dvBar

Vector m_dvBar
protected

constrained accelerations

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics(), and DeviceTorqueCtrl::init().

◆ m_f

tsid::math::Vector m_f
protected

◆ m_initSucceeded

◆ m_isTorqueControlled

bool m_isTorqueControlled
protected

◆ m_Jc

Eigen::MatrixXd m_Jc
protected

◆ m_Jc_svd

Eigen::JacobiSVD<Matrix> m_Jc_svd
protected

numerical damping to regularize constraint resolution

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics().

◆ m_K

Eigen::MatrixXd m_K
protected

cholesky decomposition of the K matrix

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics(), and DeviceTorqueCtrl::init().

◆ m_k

Eigen::VectorXd m_k
protected

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

Referenced by DeviceTorqueCtrl::init().

◆ m_K_chol

Cholesky m_K_chol
protected

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

◆ m_nj

◆ m_nk

unsigned int m_nk
protected

◆ m_numericalDamping

double m_numericalDamping
protected

constraint Jacobian

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics().

◆ m_q

◆ m_q_sot

tsid::math::Vector m_q_sot
protected

◆ m_robot

tsid::robots::RobotWrapper* m_robot
protected

◆ m_robot_util

◆ m_tau_np6

Vector m_tau_np6
protected

solution of Jc*dv=-dJc*v

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics(), and DeviceTorqueCtrl::init().

◆ m_v

◆ m_v_sot

tsid::math::Vector m_v_sot
protected

◆ m_Z

Matrix m_Z
protected

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics().

◆ m_ZMZ

Matrix m_ZMZ
protected

base of constraint null space

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

Referenced by DeviceTorqueCtrl::computeForwardDynamics().

◆ m_ZMZ_chol

Eigen::LDLT<Matrix> m_ZMZ_chol
protected

projected mass matrix: Z_c^T*M*Z_c

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

◆ normalDistribution_

DIST normalDistribution_
protected

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

◆ normalRandomNumberGenerator_

GEN normalRandomNumberGenerator_
protected

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

Referenced by DeviceTorqueCtrl::integrate().

◆ p_gainsSOUT_

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

proportional and derivative position-control gains

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl().

◆ randomNumberGenerator_

ENG randomNumberGenerator_
protected

Definition at line 153 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 94 of file device-torque-ctrl.hh.

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl(), DeviceTorqueCtrl::integrate(), and DeviceTorqueCtrl::setStateSize().

◆ temp6_

dynamicgraph::Vector temp6_
protected

◆ timestep_

double timestep_
protected

Current integration step.

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

Referenced by DeviceTorqueCtrl::init().

◆ TIMESTEP_DEFAULT

const double TIMESTEP_DEFAULT = 0.001
static

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

◆ wrenches_

dynamicgraph::Vector wrenches_[4]
protected

joints' accelerations

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

Referenced by DeviceTorqueCtrl::DeviceTorqueCtrl(), and DeviceTorqueCtrl::integrate().


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