#include <sot/torque_control/device-torque-ctrl.hh>
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, DIST > | GEN |
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 |
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... | |
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.
|
protected |
Definition at line 133 of file device-torque-ctrl.hh.
|
protected |
Definition at line 151 of file device-torque-ctrl.hh.
|
protected |
number of joints
Definition at line 150 of file device-torque-ctrl.hh.
Definition at line 152 of file device-torque-ctrl.hh.
DeviceTorqueCtrl | ( | std::string | RobotName | ) |
Definition at line 33 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 100 of file device-torque-ctrl.cpp.
|
protected |
Definition at line 222 of file device-torque-ctrl.cpp.
|
protected |
|
protected |
|
protected |
|
protected |
|
inlinevirtual |
Definition at line 62 of file device-torque-ctrl.hh.
|
virtual |
Definition at line 102 of file device-torque-ctrl.cpp.
|
protectedvirtual |
Definition at line 290 of file device-torque-ctrl.cpp.
|
inlineprotected |
Definition at line 78 of file device-torque-ctrl.hh.
|
virtual |
Definition at line 213 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 176 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 161 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 202 of file device-torque-ctrl.cpp.
|
protected |
Intermediate variables to avoid allocation during control.
Definition at line 111 of file device-torque-ctrl.hh.
|
protected |
Accelerations measured by accelerometers.
Definition at line 90 of file device-torque-ctrl.hh.
|
protected |
Definition at line 114 of file device-torque-ctrl.hh.
|
static |
Definition at line 58 of file device-torque-ctrl.hh.
|
protected |
motor currents
Definition at line 100 of file device-torque-ctrl.hh.
|
protected |
Definition at line 103 of file device-torque-ctrl.hh.
|
static |
Definition at line 60 of file device-torque-ctrl.hh.
|
protected |
input force sensor values
Definition at line 87 of file device-torque-ctrl.hh.
|
protected |
Definition at line 112 of file device-torque-ctrl.hh.
|
protected |
Rotation velocity measured by gyrometers.
Definition at line 92 of file device-torque-ctrl.hh.
|
protected |
joints' velocities
Definition at line 116 of file device-torque-ctrl.hh.
|
protected |
joints' accelerations computed by the integrator
Definition at line 98 of file device-torque-ctrl.hh.
|
protected |
base 6d pose + joints' angles
Definition at line 115 of file device-torque-ctrl.hh.
|
protected |
joints' velocities computed by the integrator
Definition at line 96 of file device-torque-ctrl.hh.
|
protected |
Definition at line 127 of file device-torque-ctrl.hh.
|
protected |
Definition at line 126 of file device-torque-ctrl.hh.
|
protected |
Definition at line 125 of file device-torque-ctrl.hh.
|
protected |
svd of the constraint matrix
Definition at line 141 of file device-torque-ctrl.hh.
|
protected |
Definition at line 130 of file device-torque-ctrl.hh.
|
protected |
Cholesky decomposition of _ZMZ.
Definition at line 145 of file device-torque-ctrl.hh.
|
protected |
Definition at line 131 of file device-torque-ctrl.hh.
|
protected |
constrained accelerations
Definition at line 146 of file device-torque-ctrl.hh.
|
protected |
Definition at line 130 of file device-torque-ctrl.hh.
|
protected |
Definition at line 84 of file device-torque-ctrl.hh.
|
protected |
Definition at line 121 of file device-torque-ctrl.hh.
|
protected |
Definition at line 137 of file device-torque-ctrl.hh.
|
protected |
numerical damping to regularize constraint resolution
Definition at line 140 of file device-torque-ctrl.hh.
|
protected |
cholesky decomposition of the K matrix
Definition at line 135 of file device-torque-ctrl.hh.
|
protected |
Definition at line 136 of file device-torque-ctrl.hh.
|
protected |
Definition at line 134 of file device-torque-ctrl.hh.
|
protected |
Definition at line 148 of file device-torque-ctrl.hh.
|
protected |
Definition at line 128 of file device-torque-ctrl.hh.
|
protected |
constraint Jacobian
Definition at line 139 of file device-torque-ctrl.hh.
|
protected |
Definition at line 130 of file device-torque-ctrl.hh.
|
protected |
Definition at line 131 of file device-torque-ctrl.hh.
|
protected |
robot geometric/inertial data
Definition at line 124 of file device-torque-ctrl.hh.
|
protected |
Definition at line 157 of file device-torque-ctrl.hh.
|
protected |
solution of Jc*dv=-dJc*v
Definition at line 147 of file device-torque-ctrl.hh.
|
protected |
Definition at line 130 of file device-torque-ctrl.hh.
|
protected |
Definition at line 131 of file device-torque-ctrl.hh.
|
protected |
Definition at line 142 of file device-torque-ctrl.hh.
|
protected |
base of constraint null space
Definition at line 143 of file device-torque-ctrl.hh.
|
protected |
projected mass matrix: Z_c^T*M*Z_c
Definition at line 144 of file device-torque-ctrl.hh.
|
protected |
Definition at line 154 of file device-torque-ctrl.hh.
|
protected |
Definition at line 155 of file device-torque-ctrl.hh.
|
protected |
proportional and derivative position-control gains
Definition at line 102 of file device-torque-ctrl.hh.
|
protected |
Definition at line 153 of file device-torque-ctrl.hh.
|
protected |
base 6d pose + joints' angles measured by encoders
Definition at line 94 of file device-torque-ctrl.hh.
|
protected |
Definition at line 119 of file device-torque-ctrl.hh.
|
protected |
Current integration step.
Definition at line 83 of file device-torque-ctrl.hh.
|
static |
Definition at line 59 of file device-torque-ctrl.hh.
|
protected |
joints' accelerations
Definition at line 118 of file device-torque-ctrl.hh.