sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
current-controller.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_current_controller_H__
7 #define __sot_torque_control_current_controller_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(__sot_torque_control_current_controller_H__)
15 #define SOTCURRENTCONTROLLER_EXPORT __declspec(dllexport)
16 #else
17 #define SOTCURRENTCONTROLLER_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTCURRENTCONTROLLER_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <map>
28 #include "boost/assign.hpp"
29 
30 #include <pinocchio/multibody/model.hpp>
31 #include <pinocchio/parsers/urdf.hpp>
32 
33 #include <tsid/robots/robot-wrapper.hpp>
34 #include <dynamic-graph/signal-helper.h>
35 #include <sot/core/matrix-geometry.hh>
36 #include <sot/core/robot-utils.hh>
38 
39 namespace dynamicgraph {
40 namespace sot {
41 namespace torque_control {
42 
43 /* --------------------------------------------------------------------- */
44 /* --- CLASS ----------------------------------------------------------- */
45 /* --------------------------------------------------------------------- */
46 
47 class SOTCURRENTCONTROLLER_EXPORT CurrentController : public ::dynamicgraph::Entity {
49  DYNAMIC_GRAPH_ENTITY_DECL();
50 
51  public:
52  /* --- CONSTRUCTOR ---- */
53  CurrentController(const std::string& name);
54 
59  void init(const double& dt, const std::string& robotRef, const unsigned int& currentOffsetIters);
60 
61  /* --- SIGNALS --- */
62  DECLARE_SIGNAL_IN(i_des, dynamicgraph::Vector);
63  DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector);
64  DECLARE_SIGNAL_IN(i_sens_gains, dynamicgraph::Vector);
65  DECLARE_SIGNAL_IN(kp_current, dynamicgraph::Vector);
66  DECLARE_SIGNAL_IN(ki_current, dynamicgraph::Vector);
67 
68  DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector);
69  DECLARE_SIGNAL_IN(u_max,
70  dynamicgraph::Vector);
71  DECLARE_SIGNAL_IN(u_saturation, dynamicgraph::Vector);
72 
73  DECLARE_SIGNAL_IN(in_out_gain, dynamicgraph::Vector);
74  DECLARE_SIGNAL_IN(
75  dq, dynamicgraph::Vector);
76  DECLARE_SIGNAL_IN(bemf_factor, dynamicgraph::Vector);
77  DECLARE_SIGNAL_IN(percentage_bemf_compensation,
79  dynamicgraph::Vector);
80  DECLARE_SIGNAL_IN(dead_zone_offsets, dynamicgraph::Vector);
82  DECLARE_SIGNAL_IN(percentage_dead_zone_compensation,
83  dynamicgraph::Vector);
84  DECLARE_SIGNAL_IN(
86  i_max_dead_zone_compensation,
87  dynamicgraph::Vector);
88  DECLARE_SIGNAL_IN(i_sensor_offsets_low_level,
89  dynamicgraph::Vector);
90  DECLARE_SIGNAL_IN(i_sensor_offsets_real_in, dynamicgraph::Vector);
91 
92  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
93  DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector);
94  DECLARE_SIGNAL_OUT(i_real, dynamicgraph::Vector);
95  DECLARE_SIGNAL_OUT(i_low_level, dynamicgraph::Vector);
96  DECLARE_SIGNAL_OUT(i_sensor_offsets_real_out, dynamicgraph::Vector);
97  DECLARE_SIGNAL_OUT(dead_zone_compensation,
98  dynamicgraph::Vector);
99  DECLARE_SIGNAL_OUT(i_errors, dynamicgraph::Vector);
100  DECLARE_SIGNAL_OUT(i_errors_ll_wo_bemf, dynamicgraph::Vector);
101 
102  /* --- COMMANDS --- */
103 
104  void reset_integral();
105 
106  /* --- ENTITY INHERITANCE --- */
107  virtual void display(std::ostream& os) const;
108 
109  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
110  logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n';
111  }
112 
113  protected:
114  RobotUtilShrPtr m_robot_util;
117  double m_dt;
119  int m_iter;
120  double m_sleep_time;
121 
122  unsigned int m_currentOffsetIters;
123  dynamicgraph::Vector m_i_offsets_real;
124  dynamicgraph::Vector m_i_err_integr;
125 
126  dynamicgraph::Vector m_dz_coeff;
127 
128  dynamicgraph::Vector m_avg_i_err_pos;
129  dynamicgraph::Vector m_avg_i_err_neg;
130 
131 }; // class CurrentController
132 
133 } // namespace torque_control
134 } // namespace sot
135 } // namespace dynamicgraph
136 
137 #endif // #ifndef __sot_torque_control_current_controller_H__
#define SOTCURRENTCONTROLLER_EXPORT
bool m_emergency_stop_triggered
true if the entity has been successfully initialized
int m_iter
true at the first iteration, false otherwise
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
to read text file
Definition: treeview.dox:22
unsigned int m_currentOffsetIters
time to sleep at every iteration (to slow down simulation)