sot-torque-control  1.6.2
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*/
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
dynamicgraph::Vector jointsVelocities_
base 6d pose + joints' angles
virtual void setControlInputType(const std::string &cit)
dynamicgraph::Signal< dynamicgraph::Vector, int > robotStateSOUT_
base 6d pose + joints' angles measured by encoders
dynamicgraph::Vector wrenches_[4]
joints' accelerations
DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector)
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsAccelerationsSOUT_
joints' accelerations computed by the integrator
virtual void setVelocity(const dynamicgraph::Vector &vel)
Eigen::LDLT< Matrix > m_ZMZ_chol
projected mass matrix: Z_c^T*M*Z_c
virtual void setStateSize(const unsigned int &size)
dynamicgraph::Signal< dynamicgraph::Vector, int > currentSOUT_
motor currents
tsid::robots::RobotWrapper * m_robot
robot geometric/inertial data
dynamicgraph::Signal< dynamicgraph::Vector, int > gyrometerSOUT_
Rotation velocity measured by gyrometers.
dynamicgraph::Signal< dynamicgraph::Vector, int > p_gainsSOUT_
proportional and derivative position-control gains
virtual const std::string & getClassName() const
virtual void setState(const dynamicgraph::Vector &st)
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * forcesSIN_[4]
input force sensor values
dynamicgraph::Vector accelerometer_
Intermediate variables to avoid allocation during control.
virtual void init(const double &dt, const std::string &urdfFile)
dynamicgraph::Signal< dynamicgraph::Vector, int > d_gainsSOUT_
DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector)
Vector m_dv_c
Cholesky decomposition of _ZMZ.
dynamicgraph::Signal< dynamicgraph::Vector, int > accelerometerSOUT_
Accelerations measured by accelerometers.
DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector)
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsVelocitiesSOUT_
joints' velocities computed by the integrator
Eigen::MatrixXd m_K
cholesky decomposition of the K matrix
DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector)
dynamicgraph::Vector jointsAccelerations_
joints' velocities
Eigen::JacobiSVD< Matrix > m_Jc_svd
numerical damping to regularize constraint resolution
to read text file
Definition: treeview.dox:22