sot-torque-control  1.6.1
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
device-torque-ctrl.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef _DeviceTorqueCtrl_H_
7 #define _DeviceTorqueCtrl_H_
8 
9 #include <Eigen/Cholesky>
10 
11 #include <boost/random/mersenne_twister.hpp>
12 #include <boost/random/normal_distribution.hpp>
13 #include <boost/random/variate_generator.hpp>
14 
15 #include <tsid/robots/robot-wrapper.hpp>
16 #include <tsid/tasks/task-se3-equality.hpp>
17 
18 #include <dynamic-graph/entity.h>
19 #include <dynamic-graph/signal.h>
20 #include <dynamic-graph/signal-ptr.h>
21 #include <dynamic-graph/linear-algebra.h>
22 #include <sot/core/device.hh>
23 #include <sot/core/abstract-sot-external-interface.hh>
24 
25 /* HELPER */
26 #include <dynamic-graph/signal-helper.h>
27 #include <sot/core/robot-utils.hh>
28 
29 namespace dgsot = dynamicgraph::sot;
30 
31 namespace dynamicgraph {
32 namespace sot {
33 namespace torque_control {
34 
56 class DeviceTorqueCtrl : public dgsot::Device {
57  public:
58  static const std::string CLASS_NAME;
59  static const double TIMESTEP_DEFAULT;
60  static const double FORCE_SENSOR_NOISE_STD_DEV;
61 
62  virtual const std::string& getClassName() const { return CLASS_NAME; }
63 
64  DeviceTorqueCtrl(std::string RobotName);
65  virtual ~DeviceTorqueCtrl();
66 
67  virtual void setStateSize(const unsigned int& size);
68  virtual void setState(const dynamicgraph::Vector& st);
69  virtual void setVelocity(const dynamicgraph::Vector& vel);
70  virtual void setControlInputType(const std::string& cit);
71 
72  virtual void init(const double& dt, const std::string& urdfFile);
73 
74  protected:
75  virtual void integrate(const double& dt);
77 
78  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
79  logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n';
80  }
81 
83  double timestep_;
85 
87  dynamicgraph::SignalPtr<dynamicgraph::Vector, int>* forcesSIN_[4];
88 
90  dynamicgraph::Signal<dynamicgraph::Vector, int> accelerometerSOUT_;
92  dynamicgraph::Signal<dynamicgraph::Vector, int> gyrometerSOUT_;
94  dynamicgraph::Signal<dynamicgraph::Vector, int> robotStateSOUT_;
96  dynamicgraph::Signal<dynamicgraph::Vector, int> jointsVelocitiesSOUT_;
98  dynamicgraph::Signal<dynamicgraph::Vector, int> jointsAccelerationsSOUT_;
100  dynamicgraph::Signal<dynamicgraph::Vector, int> currentSOUT_;
102  dynamicgraph::Signal<dynamicgraph::Vector, int> p_gainsSOUT_;
103  dynamicgraph::Signal<dynamicgraph::Vector, int> d_gainsSOUT_;
104 
105  DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
106  DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
107  DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
108  DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);
109 
111  dynamicgraph::Vector accelerometer_;
112  dynamicgraph::Vector gyrometer_;
113 
114  dynamicgraph::Vector base6d_encoders_;
115  dynamicgraph::Vector jointsVelocities_;
116  dynamicgraph::Vector jointsAccelerations_;
117 
118  dynamicgraph::Vector wrenches_[4];
119  dynamicgraph::Vector temp6_;
120 
122 
124  tsid::robots::RobotWrapper* m_robot;
125  pinocchio::Data* m_data;
126  tsid::tasks::TaskSE3Equality* m_contactRF;
127  tsid::tasks::TaskSE3Equality* m_contactLF;
128  unsigned int m_nk; // number of contact forces
129 
130  tsid::math::Vector m_q, m_v, m_dv, m_f;
131  tsid::math::Vector m_q_sot, m_v_sot, m_dv_sot;
132 
133  typedef Eigen::LDLT<Eigen::MatrixXd> Cholesky;
135  Eigen::MatrixXd m_K;
136  Eigen::VectorXd m_k;
137  Eigen::MatrixXd m_Jc;
138 
140  Eigen::JacobiSVD<Matrix> m_Jc_svd;
141  Vector m_dJcv;
142  Matrix m_Z;
143  Matrix m_ZMZ;
144  Eigen::LDLT<Matrix> m_ZMZ_chol;
145  Vector m_dv_c;
146  Vector m_dvBar;
147  Vector m_tau_np6;
148  int m_nj;
149 
150  typedef boost::mt11213b ENG; // uniform random number generator
151  typedef boost::normal_distribution<double> DIST; // Normal Distribution
152  typedef boost::variate_generator<ENG, DIST> GEN; // Variate generator
156 
157  RobotUtilShrPtr m_robot_util;
158 };
159 
160 } // end namespace torque_control
161 } // end namespace sot
162 } // end namespace dynamicgraph
163 #endif /* DevicePosCtrl*/
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::forcesSIN_
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * forcesSIN_[4]
input force sensor values
Definition: device-torque-ctrl.hh:87
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_nk
unsigned int m_nk
Definition: device-torque-ctrl.hh:128
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::normalRandomNumberGenerator_
GEN normalRandomNumberGenerator_
Definition: device-torque-ctrl.hh:155
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_contactLF
tsid::tasks::TaskSE3Equality * m_contactLF
Definition: device-torque-ctrl.hh:127
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::init
virtual void init(const double &dt, const std::string &urdfFile)
Definition: device-torque-ctrl.cpp:102
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_Jc_svd
Eigen::JacobiSVD< Matrix > m_Jc_svd
numerical damping to regularize constraint resolution
Definition: device-torque-ctrl.hh:140
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_f
tsid::math::Vector m_f
Definition: device-torque-ctrl.hh:130
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::setStateSize
virtual void setStateSize(const unsigned int &size)
Definition: device-torque-ctrl.cpp:161
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::jointsVelocitiesSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsVelocitiesSOUT_
joints' velocities computed by the integrator
Definition: device-torque-ctrl.hh:96
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::jointsVelocities_
dynamicgraph::Vector jointsVelocities_
base 6d pose + joints' angles
Definition: device-torque-ctrl.hh:115
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::ENG
boost::mt11213b ENG
number of joints
Definition: device-torque-ctrl.hh:150
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_ZMZ_chol
Eigen::LDLT< Matrix > m_ZMZ_chol
projected mass matrix: Z_c^T*M*Z_c
Definition: device-torque-ctrl.hh:144
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::computeForwardDynamics
void computeForwardDynamics()
Definition: device-torque-ctrl.cpp:222
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_nj
int m_nj
Definition: device-torque-ctrl.hh:148
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_q
tsid::math::Vector m_q
Definition: device-torque-ctrl.hh:130
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::getClassName
virtual const std::string & getClassName() const
Definition: device-torque-ctrl.hh:62
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::setControlInputType
virtual void setControlInputType(const std::string &cit)
Definition: device-torque-ctrl.cpp:213
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_Jc
Eigen::MatrixXd m_Jc
Definition: device-torque-ctrl.hh:137
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::DeviceTorqueCtrl
DeviceTorqueCtrl(std::string RobotName)
Definition: device-torque-ctrl.cpp:33
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::setState
virtual void setState(const dynamicgraph::Vector &st)
Definition: device-torque-ctrl.cpp:176
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::jointsAccelerations_
dynamicgraph::Vector jointsAccelerations_
joints' velocities
Definition: device-torque-ctrl.hh:116
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_ZMZ
Matrix m_ZMZ
base of constraint null space
Definition: device-torque-ctrl.hh:143
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::DIST
boost::normal_distribution< double > DIST
Definition: device-torque-ctrl.hh:151
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_dv
tsid::math::Vector m_dv
Definition: device-torque-ctrl.hh:130
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::randomNumberGenerator_
ENG randomNumberGenerator_
Definition: device-torque-ctrl.hh:153
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::wrenches_
dynamicgraph::Vector wrenches_[4]
joints' accelerations
Definition: device-torque-ctrl.hh:118
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::DECLARE_SIGNAL_IN
DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector)
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_K_chol
Cholesky m_K_chol
Definition: device-torque-ctrl.hh:134
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::TIMESTEP_DEFAULT
static const double TIMESTEP_DEFAULT
Definition: device-torque-ctrl.hh:59
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_q_sot
tsid::math::Vector m_q_sot
Definition: device-torque-ctrl.hh:131
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::accelerometer_
dynamicgraph::Vector accelerometer_
Intermediate variables to avoid allocation during control.
Definition: device-torque-ctrl.hh:111
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_robot
tsid::robots::RobotWrapper * m_robot
robot geometric/inertial data
Definition: device-torque-ctrl.hh:124
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::accelerometerSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > accelerometerSOUT_
Accelerations measured by accelerometers.
Definition: device-torque-ctrl.hh:90
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_v
tsid::math::Vector m_v
Definition: device-torque-ctrl.hh:130
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::d_gainsSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > d_gainsSOUT_
Definition: device-torque-ctrl.hh:103
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_tau_np6
Vector m_tau_np6
solution of Jc*dv=-dJc*v
Definition: device-torque-ctrl.hh:147
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::jointsAccelerationsSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsAccelerationsSOUT_
joints' accelerations computed by the integrator
Definition: device-torque-ctrl.hh:98
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_K
Eigen::MatrixXd m_K
cholesky decomposition of the K matrix
Definition: device-torque-ctrl.hh:135
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: device-torque-ctrl.hh:78
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_Z
Matrix m_Z
Definition: device-torque-ctrl.hh:142
dynamicgraph::sot::torque_control::DeviceTorqueCtrl
Definition: device-torque-ctrl.hh:56
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::setVelocity
virtual void setVelocity(const dynamicgraph::Vector &vel)
Definition: device-torque-ctrl.cpp:202
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: device-torque-ctrl.hh:157
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_dvBar
Vector m_dvBar
constrained accelerations
Definition: device-torque-ctrl.hh:146
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::gyrometer_
dynamicgraph::Vector gyrometer_
Definition: device-torque-ctrl.hh:112
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_data
pinocchio::Data * m_data
Definition: device-torque-ctrl.hh:125
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::~DeviceTorqueCtrl
virtual ~DeviceTorqueCtrl()
Definition: device-torque-ctrl.cpp:100
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::CLASS_NAME
static const std::string CLASS_NAME
Definition: device-torque-ctrl.hh:58
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::timestep_
double timestep_
Current integration step.
Definition: device-torque-ctrl.hh:83
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::p_gainsSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > p_gainsSOUT_
proportional and derivative position-control gains
Definition: device-torque-ctrl.hh:102
dynamicgraph::sot
Definition: treeview.dox:23
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::integrate
virtual void integrate(const double &dt)
Definition: device-torque-ctrl.cpp:290
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::base6d_encoders_
dynamicgraph::Vector base6d_encoders_
Definition: device-torque-ctrl.hh:114
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_numericalDamping
double m_numericalDamping
constraint Jacobian
Definition: device-torque-ctrl.hh:139
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::currentSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > currentSOUT_
motor currents
Definition: device-torque-ctrl.hh:100
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::GEN
boost::variate_generator< ENG, DIST > GEN
Definition: device-torque-ctrl.hh:152
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::normalDistribution_
DIST normalDistribution_
Definition: device-torque-ctrl.hh:154
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::FORCE_SENSOR_NOISE_STD_DEV
static const double FORCE_SENSOR_NOISE_STD_DEV
Definition: device-torque-ctrl.hh:60
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_dv_sot
tsid::math::Vector m_dv_sot
Definition: device-torque-ctrl.hh:131
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_k
Eigen::VectorXd m_k
Definition: device-torque-ctrl.hh:136
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::robotStateSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > robotStateSOUT_
base 6d pose + joints' angles measured by encoders
Definition: device-torque-ctrl.hh:94
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_isTorqueControlled
bool m_isTorqueControlled
Definition: device-torque-ctrl.hh:121
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::temp6_
dynamicgraph::Vector temp6_
Definition: device-torque-ctrl.hh:119
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_initSucceeded
bool m_initSucceeded
Definition: device-torque-ctrl.hh:84
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_contactRF
tsid::tasks::TaskSE3Equality * m_contactRF
Definition: device-torque-ctrl.hh:126
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::Cholesky
Eigen::LDLT< Eigen::MatrixXd > Cholesky
Definition: device-torque-ctrl.hh:133
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_dJcv
Vector m_dJcv
svd of the constraint matrix
Definition: device-torque-ctrl.hh:141
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_dv_c
Vector m_dv_c
Cholesky decomposition of _ZMZ.
Definition: device-torque-ctrl.hh:145
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::m_v_sot
tsid::math::Vector m_v_sot
Definition: device-torque-ctrl.hh:131
dynamicgraph::sot::torque_control::DeviceTorqueCtrl::gyrometerSOUT_
dynamicgraph::Signal< dynamicgraph::Vector, int > gyrometerSOUT_
Rotation velocity measured by gyrometers.
Definition: device-torque-ctrl.hh:92