#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 |
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 59 of file device-torque-ctrl.hh.
|
protected |
Definition at line 137 of file device-torque-ctrl.hh.
|
protected |
Definition at line 156 of file device-torque-ctrl.hh.
|
protected |
number of joints
Definition at line 155 of file device-torque-ctrl.hh.
Definition at line 157 of file device-torque-ctrl.hh.
DeviceTorqueCtrl | ( | std::string | RobotName | ) |
Definition at line 32 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 120 of file device-torque-ctrl.cpp.
|
protected |
Definition at line 252 of file device-torque-ctrl.cpp.
|
protected |
|
protected |
|
protected |
|
protected |
|
inlinevirtual |
Definition at line 65 of file device-torque-ctrl.hh.
|
virtual |
Definition at line 122 of file device-torque-ctrl.cpp.
|
protectedvirtual |
Definition at line 326 of file device-torque-ctrl.cpp.
|
inlineprotected |
Definition at line 81 of file device-torque-ctrl.hh.
|
virtual |
Definition at line 243 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 204 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 189 of file device-torque-ctrl.cpp.
|
virtual |
Definition at line 232 of file device-torque-ctrl.cpp.
|
protected |
Intermediate variables to avoid allocation during control.
Definition at line 115 of file device-torque-ctrl.hh.
|
protected |
Accelerations measured by accelerometers.
Definition at line 94 of file device-torque-ctrl.hh.
|
protected |
Definition at line 118 of file device-torque-ctrl.hh.
|
static |
Definition at line 61 of file device-torque-ctrl.hh.
|
protected |
motor currents
Definition at line 104 of file device-torque-ctrl.hh.
|
protected |
Definition at line 107 of file device-torque-ctrl.hh.
|
static |
Definition at line 63 of file device-torque-ctrl.hh.
|
protected |
input force sensor values
Definition at line 91 of file device-torque-ctrl.hh.
|
protected |
Definition at line 116 of file device-torque-ctrl.hh.
|
protected |
Rotation velocity measured by gyrometers.
Definition at line 96 of file device-torque-ctrl.hh.
|
protected |
joints' velocities
Definition at line 120 of file device-torque-ctrl.hh.
|
protected |
joints' accelerations computed by the integrator
Definition at line 102 of file device-torque-ctrl.hh.
|
protected |
base 6d pose + joints' angles
Definition at line 119 of file device-torque-ctrl.hh.
|
protected |
joints' velocities computed by the integrator
Definition at line 100 of file device-torque-ctrl.hh.
|
protected |
Definition at line 131 of file device-torque-ctrl.hh.
|
protected |
Definition at line 130 of file device-torque-ctrl.hh.
|
protected |
Definition at line 129 of file device-torque-ctrl.hh.
|
protected |
svd of the constraint matrix
Definition at line 146 of file device-torque-ctrl.hh.
|
protected |
Definition at line 134 of file device-torque-ctrl.hh.
|
protected |
Cholesky decomposition of _ZMZ.
Definition at line 150 of file device-torque-ctrl.hh.
|
protected |
Definition at line 135 of file device-torque-ctrl.hh.
|
protected |
constrained accelerations
Definition at line 151 of file device-torque-ctrl.hh.
|
protected |
Definition at line 134 of file device-torque-ctrl.hh.
|
protected |
Definition at line 88 of file device-torque-ctrl.hh.
|
protected |
Definition at line 125 of file device-torque-ctrl.hh.
|
protected |
Definition at line 141 of file device-torque-ctrl.hh.
|
protected |
numerical damping to regularize constraint resolution
Definition at line 145 of file device-torque-ctrl.hh.
|
protected |
cholesky decomposition of the K matrix
Definition at line 139 of file device-torque-ctrl.hh.
|
protected |
Definition at line 140 of file device-torque-ctrl.hh.
|
protected |
Definition at line 138 of file device-torque-ctrl.hh.
|
protected |
Definition at line 153 of file device-torque-ctrl.hh.
|
protected |
Definition at line 132 of file device-torque-ctrl.hh.
|
protected |
constraint Jacobian
Definition at line 143 of file device-torque-ctrl.hh.
|
protected |
Definition at line 134 of file device-torque-ctrl.hh.
|
protected |
Definition at line 135 of file device-torque-ctrl.hh.
|
protected |
robot geometric/inertial data
Definition at line 128 of file device-torque-ctrl.hh.
|
protected |
Definition at line 162 of file device-torque-ctrl.hh.
|
protected |
solution of Jc*dv=-dJc*v
Definition at line 152 of file device-torque-ctrl.hh.
|
protected |
Definition at line 134 of file device-torque-ctrl.hh.
|
protected |
Definition at line 135 of file device-torque-ctrl.hh.
|
protected |
Definition at line 147 of file device-torque-ctrl.hh.
|
protected |
base of constraint null space
Definition at line 148 of file device-torque-ctrl.hh.
|
protected |
projected mass matrix: Z_c^T*M*Z_c
Definition at line 149 of file device-torque-ctrl.hh.
|
protected |
Definition at line 159 of file device-torque-ctrl.hh.
|
protected |
Definition at line 160 of file device-torque-ctrl.hh.
|
protected |
proportional and derivative position-control gains
Definition at line 106 of file device-torque-ctrl.hh.
|
protected |
Definition at line 158 of file device-torque-ctrl.hh.
|
protected |
base 6d pose + joints' angles measured by encoders
Definition at line 98 of file device-torque-ctrl.hh.
|
protected |
Definition at line 123 of file device-torque-ctrl.hh.
|
protected |
Current integration step.
Definition at line 87 of file device-torque-ctrl.hh.
|
static |
Definition at line 62 of file device-torque-ctrl.hh.
|
protected |
joints' accelerations
Definition at line 122 of file device-torque-ctrl.hh.