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 <pinocchio/fwd.hpp> |
28 |
|
|
|
29 |
|
|
// include pinocchio first |
30 |
|
|
|
31 |
|
|
#include <dynamic-graph/signal-helper.h> |
32 |
|
|
|
33 |
|
|
#include <map> |
34 |
|
|
#include <pinocchio/multibody/model.hpp> |
35 |
|
|
#include <pinocchio/parsers/urdf.hpp> |
36 |
|
|
#include <sot/core/matrix-geometry.hh> |
37 |
|
|
#include <sot/core/robot-utils.hh> |
38 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
39 |
|
|
#include <tsid/robots/robot-wrapper.hpp> |
40 |
|
|
|
41 |
|
|
#include "boost/assign.hpp" |
42 |
|
|
|
43 |
|
|
namespace dynamicgraph { |
44 |
|
|
namespace sot { |
45 |
|
|
namespace torque_control { |
46 |
|
|
|
47 |
|
|
/* --------------------------------------------------------------------- */ |
48 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
49 |
|
|
/* --------------------------------------------------------------------- */ |
50 |
|
|
|
51 |
|
|
class SOTCURRENTCONTROLLER_EXPORT CurrentController |
52 |
|
|
: public ::dynamicgraph::Entity { |
53 |
|
|
typedef CurrentController EntityClassName; |
54 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
55 |
|
|
|
56 |
|
|
public: |
57 |
|
|
/* --- CONSTRUCTOR ---- */ |
58 |
|
|
CurrentController(const std::string& name); |
59 |
|
|
|
60 |
|
|
/// Initialize |
61 |
|
|
/// @param dt: control interval |
62 |
|
|
/// @param currentOffsetIters: number of iterations while control is disabled |
63 |
|
|
/// to calibrate current sensors. The recommended way is to use the signal |
64 |
|
|
/// max_current. |
65 |
|
|
void init(const double& dt, const std::string& robotRef, |
66 |
|
|
const unsigned int& currentOffsetIters); |
67 |
|
|
|
68 |
|
|
/* --- SIGNALS --- */ |
69 |
|
|
DECLARE_SIGNAL_IN(i_des, dynamicgraph::Vector); /// desired motor currents |
70 |
|
|
DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector); /// motor currents |
71 |
|
|
DECLARE_SIGNAL_IN(i_sens_gains, |
72 |
|
|
dynamicgraph::Vector); /// gains of current sensors |
73 |
|
|
DECLARE_SIGNAL_IN( |
74 |
|
|
kp_current, dynamicgraph::Vector); /// proportional current feedback gain |
75 |
|
|
DECLARE_SIGNAL_IN( |
76 |
|
|
ki_current, dynamicgraph::Vector); /// proportional current feedback gain |
77 |
|
|
|
78 |
|
|
DECLARE_SIGNAL_IN( |
79 |
|
|
i_max, dynamicgraph::Vector); /// max current allowed before stopping the |
80 |
|
|
/// controller (in Ampers) |
81 |
|
|
DECLARE_SIGNAL_IN( |
82 |
|
|
u_max, |
83 |
|
|
dynamicgraph::Vector); /// max desired current allowed before stopping |
84 |
|
|
/// the controller (in Ampers) |
85 |
|
|
DECLARE_SIGNAL_IN(u_saturation, |
86 |
|
|
dynamicgraph::Vector); /// values at which to saturate the |
87 |
|
|
/// control (in bits) |
88 |
|
|
|
89 |
|
|
DECLARE_SIGNAL_IN( |
90 |
|
|
in_out_gain, |
91 |
|
|
dynamicgraph::Vector); /// gain from input to output control values |
92 |
|
|
DECLARE_SIGNAL_IN( |
93 |
|
|
dq, dynamicgraph::Vector); /// Joint velocities; used to compensate for |
94 |
|
|
/// BEMF effect on low level current loop |
95 |
|
|
DECLARE_SIGNAL_IN( |
96 |
|
|
bemf_factor, |
97 |
|
|
dynamicgraph::Vector); /// Link between velocity and current; to |
98 |
|
|
/// compensate for BEMF effect on low level |
99 |
|
|
/// current loop (in A/rad.s-1) |
100 |
|
|
DECLARE_SIGNAL_IN( |
101 |
|
|
percentage_bemf_compensation, |
102 |
|
|
dynamicgraph::Vector); /// percentage in [0;1] of the motor back-EMF that |
103 |
|
|
/// we should compensate 0 is none, 1 is all of it |
104 |
|
|
DECLARE_SIGNAL_IN( |
105 |
|
|
dead_zone_offsets, |
106 |
|
|
dynamicgraph::Vector); /// current control dead zone offsets |
107 |
|
|
DECLARE_SIGNAL_IN( |
108 |
|
|
percentage_dead_zone_compensation, |
109 |
|
|
dynamicgraph::Vector); /// percentage in [0;1] of the motor driver dead |
110 |
|
|
/// zone that we should compensate 0 is none, 1 is |
111 |
|
|
/// all of it |
112 |
|
|
DECLARE_SIGNAL_IN( |
113 |
|
|
i_max_dead_zone_compensation, |
114 |
|
|
dynamicgraph::Vector); /// value of current tracking error at which |
115 |
|
|
/// deadzone is completely compensated |
116 |
|
|
DECLARE_SIGNAL_IN(i_sensor_offsets_low_level, |
117 |
|
|
dynamicgraph::Vector); /// offset of the current sensors |
118 |
|
|
/// seen by the low level |
119 |
|
|
DECLARE_SIGNAL_IN( |
120 |
|
|
i_sensor_offsets_real_in, |
121 |
|
|
dynamicgraph::Vector); /// real offset of the current sensors |
122 |
|
|
|
123 |
|
|
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); |
124 |
|
|
DECLARE_SIGNAL_OUT( |
125 |
|
|
u_safe, |
126 |
|
|
dynamicgraph::Vector); /// same as u when everything is fine, 0 otherwise |
127 |
|
|
DECLARE_SIGNAL_OUT(i_real, |
128 |
|
|
dynamicgraph::Vector); /// current measurements after gain |
129 |
|
|
/// and offset compensation |
130 |
|
|
DECLARE_SIGNAL_OUT( |
131 |
|
|
i_low_level, |
132 |
|
|
dynamicgraph::Vector); /// current measurements as seen by low-level ctrl |
133 |
|
|
DECLARE_SIGNAL_OUT( |
134 |
|
|
i_sensor_offsets_real_out, |
135 |
|
|
dynamicgraph::Vector); /// real offset of the current sensors |
136 |
|
|
DECLARE_SIGNAL_OUT(dead_zone_compensation, |
137 |
|
|
dynamicgraph::Vector); /// dead-zone compensation current |
138 |
|
|
/// applied by the controller |
139 |
|
|
DECLARE_SIGNAL_OUT(i_errors, |
140 |
|
|
dynamicgraph::Vector); /// current tracking error |
141 |
|
|
DECLARE_SIGNAL_OUT( |
142 |
|
|
i_errors_ll_wo_bemf, |
143 |
|
|
dynamicgraph::Vector); /// current tracking error without BEMF effect |
144 |
|
|
|
145 |
|
|
/* --- COMMANDS --- */ |
146 |
|
|
|
147 |
|
|
void reset_integral(); |
148 |
|
|
|
149 |
|
|
/* --- ENTITY INHERITANCE --- */ |
150 |
|
|
virtual void display(std::ostream& os) const; |
151 |
|
|
|
152 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
153 |
|
|
const char* = "", int = 0) { |
154 |
|
|
logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n'; |
155 |
|
|
} |
156 |
|
|
|
157 |
|
|
protected: |
158 |
|
|
RobotUtilShrPtr m_robot_util; |
159 |
|
|
bool |
160 |
|
|
m_initSucceeded; /// true if the entity has been successfully initialized |
161 |
|
|
bool m_emergency_stop_triggered; |
162 |
|
|
double m_dt; /// control loop time period |
163 |
|
|
bool m_is_first_iter; /// true at the first iteration, false otherwise |
164 |
|
|
int m_iter; |
165 |
|
|
double m_sleep_time; /// time to sleep at every iteration (to slow down |
166 |
|
|
/// simulation) |
167 |
|
|
|
168 |
|
|
unsigned int m_currentOffsetIters; |
169 |
|
|
dynamicgraph::Vector m_i_offsets_real; |
170 |
|
|
dynamicgraph::Vector m_i_err_integr; |
171 |
|
|
|
172 |
|
|
dynamicgraph::Vector m_dz_coeff; |
173 |
|
|
|
174 |
|
|
dynamicgraph::Vector m_avg_i_err_pos; |
175 |
|
|
dynamicgraph::Vector m_avg_i_err_neg; |
176 |
|
|
|
177 |
|
|
}; // class CurrentController |
178 |
|
|
|
179 |
|
|
} // namespace torque_control |
180 |
|
|
} // namespace sot |
181 |
|
|
} // namespace dynamicgraph |
182 |
|
|
|
183 |
|
|
#endif // #ifndef __sot_torque_control_current_controller_H__ |