sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
current-controller.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#include <dynamic-graph/factory.h>
7
8#include <sot/core/debug.hh>
11#include <tsid/utils/statistics.hpp>
12#include <tsid/utils/stop-watch.hpp>
13
14namespace dynamicgraph {
15namespace sot {
16namespace torque_control {
17namespace dynamicgraph = ::dynamicgraph;
18using namespace dynamicgraph;
19using namespace dynamicgraph::command;
20using namespace std;
21using namespace dg::sot::torque_control;
22
23#define SAFETY_SIGNALS m_i_maxSIN << m_u_maxSIN << m_u_saturationSIN
24#define INPUT_SIGNALS \
25 m_i_desSIN << m_percentage_dead_zone_compensationSIN << SAFETY_SIGNALS \
26 << m_i_max_dead_zone_compensationSIN << m_dqSIN \
27 << m_bemf_factorSIN << m_in_out_gainSIN << m_i_measuredSIN \
28 << m_dead_zone_offsetsSIN << m_i_sens_gainsSIN \
29 << m_i_sensor_offsets_low_levelSIN \
30 << m_i_sensor_offsets_real_inSIN << m_kp_currentSIN \
31 << m_ki_currentSIN << m_percentage_bemf_compensationSIN
32#define OUTPUT_SIGNALS \
33 m_uSOUT << m_u_safeSOUT << m_i_realSOUT << m_i_low_levelSOUT \
34 << m_dead_zone_compensationSOUT << m_i_errorsSOUT \
35 << m_i_errors_ll_wo_bemfSOUT << m_i_sensor_offsets_real_outSOUT
36
39typedef CurrentController EntityClassName;
40
41/* --- DG FACTORY ---------------------------------------------------- */
43
44/* ------------------------------------------------------------------- */
45/* --- CONSTRUCTION -------------------------------------------------- */
46/* ------------------------------------------------------------------- */
47// to do rename 'pwm' to 'current'
49 : Entity(name),
50 CONSTRUCT_SIGNAL_IN(i_des, dynamicgraph::Vector),
51 CONSTRUCT_SIGNAL_IN(i_measured, dynamicgraph::Vector),
52 CONSTRUCT_SIGNAL_IN(i_sens_gains, dynamicgraph::Vector),
53 CONSTRUCT_SIGNAL_IN(kp_current, dynamicgraph::Vector),
54 CONSTRUCT_SIGNAL_IN(ki_current, dynamicgraph::Vector),
55 CONSTRUCT_SIGNAL_IN(i_max, dynamicgraph::Vector),
56 CONSTRUCT_SIGNAL_IN(u_max, dynamicgraph::Vector),
57 CONSTRUCT_SIGNAL_IN(u_saturation, dynamicgraph::Vector),
58 CONSTRUCT_SIGNAL_IN(in_out_gain, dynamicgraph::Vector),
59 CONSTRUCT_SIGNAL_IN(dq, dynamicgraph::Vector),
60 CONSTRUCT_SIGNAL_IN(bemf_factor, dynamicgraph::Vector),
61 CONSTRUCT_SIGNAL_IN(percentage_bemf_compensation, dynamicgraph::Vector),
62 CONSTRUCT_SIGNAL_IN(dead_zone_offsets, dynamicgraph::Vector),
63 CONSTRUCT_SIGNAL_IN(percentage_dead_zone_compensation,
64 dynamicgraph::Vector),
65 CONSTRUCT_SIGNAL_IN(i_max_dead_zone_compensation, dynamicgraph::Vector),
66 CONSTRUCT_SIGNAL_IN(i_sensor_offsets_low_level, dynamicgraph::Vector),
67 CONSTRUCT_SIGNAL_IN(i_sensor_offsets_real_in, dynamicgraph::Vector),
68 CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, m_i_desSIN),
69 CONSTRUCT_SIGNAL_OUT(u_safe, dynamicgraph::Vector,
70 INPUT_SIGNALS << m_uSOUT << m_i_realSOUT
71 << m_i_low_levelSOUT
72 << m_i_sensor_offsets_real_outSOUT),
73 CONSTRUCT_SIGNAL_OUT(i_real, dynamicgraph::Vector,
74 m_i_measuredSIN << m_i_sens_gainsSIN
75 << m_i_sensor_offsets_real_outSOUT),
76 CONSTRUCT_SIGNAL_OUT(i_low_level, dynamicgraph::Vector,
77 m_i_measuredSIN << m_i_sens_gainsSIN
78 << m_i_sensor_offsets_low_levelSIN),
79 CONSTRUCT_SIGNAL_OUT(i_sensor_offsets_real_out, dynamicgraph::Vector,
80 m_i_measuredSIN << m_i_sensor_offsets_real_inSIN),
81 CONSTRUCT_SIGNAL_OUT(dead_zone_compensation, dynamicgraph::Vector,
82 m_u_safeSOUT << m_dead_zone_offsetsSIN),
83 CONSTRUCT_SIGNAL_OUT(i_errors, dynamicgraph::Vector,
84 m_i_realSOUT << m_uSOUT),
85 CONSTRUCT_SIGNAL_OUT(i_errors_ll_wo_bemf, dynamicgraph::Vector,
86 m_i_realSOUT << m_uSOUT
87 << m_percentage_bemf_compensationSIN
88 << m_dqSIN << m_bemf_factorSIN),
89 m_robot_util(RefVoidRobotUtil()),
90 m_initSucceeded(false),
91 m_emergency_stop_triggered(false),
92 m_is_first_iter(true),
93 m_iter(0) {
94 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
95
96 /* Commands. */
97 addCommand("init", makeCommandVoid3(
99 docCommandVoid3(
100 "Initialize the entity.",
101 "Time period in seconds (double)",
102 "Robot reference (string)",
103 "Number of iterations while control is disabled "
104 "to calibrate current sensors (int)")));
105
106 addCommand("reset_integral",
107 makeCommandVoid0(*this, &CurrentController::reset_integral,
108 docCommandVoid0("Reset the integral error.")));
109}
110
111void CurrentController::init(const double& dt, const std::string& robotRef,
112 const unsigned int& currentOffsetIters) {
113 if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
114 m_dt = dt;
115 m_initSucceeded = true;
116 m_currentOffsetIters = currentOffsetIters;
117
118 std::string localName(robotRef);
119 if (!isNameInRobotUtil(localName)) {
120 m_robot_util = createRobotUtil(localName);
121 } else {
122 m_robot_util = getRobotUtil(localName);
123 }
124
125 m_i_offsets_real.setZero(m_robot_util->m_nbJoints);
126 m_i_err_integr.setZero(m_robot_util->m_nbJoints);
127 m_dz_coeff.setZero(m_robot_util->m_nbJoints);
128 m_avg_i_err_pos.setZero(m_robot_util->m_nbJoints);
129 m_avg_i_err_neg.setZero(m_robot_util->m_nbJoints);
130}
131
132/* ------------------------------------------------------------------- */
133/* --- SIGNALS ------------------------------------------------------- */
134/* ------------------------------------------------------------------- */
135
136DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
137 if (!m_initSucceeded) {
138 SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
139 return s;
140 }
141
142 if (m_is_first_iter) m_is_first_iter = false;
143
144 if (s.size() != (Eigen::VectorXd::Index)m_robot_util->m_nbJoints)
145 s.resize(m_robot_util->m_nbJoints);
146
147 const dynamicgraph::Vector& i_des = m_i_desSIN(iter);
148 const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
149 const dynamicgraph::Vector& i_ll = m_i_low_levelSOUT(iter);
150 const dynamicgraph::Vector& cur_sens_gains = m_i_sens_gainsSIN(iter);
151 const dynamicgraph::Vector& i_offset_real =
152 m_i_sensor_offsets_real_outSOUT(iter);
153 const dynamicgraph::Vector& dq = m_dqSIN(iter);
154 // const dynamicgraph::Vector& in_out_gain =
155 // m_in_out_gainSIN(iter);
156 const dynamicgraph::Vector& dead_zone_offsets = m_dead_zone_offsetsSIN(iter);
157 const dynamicgraph::Vector& dead_zone_comp_perc =
158 m_percentage_dead_zone_compensationSIN(iter);
159 const dynamicgraph::Vector& bemf_factor = m_bemf_factorSIN(iter);
160 const dynamicgraph::Vector& bemf_comp_perc =
161 m_percentage_bemf_compensationSIN(iter);
162 const dynamicgraph::Vector& i_max_dz_comp =
163 m_i_max_dead_zone_compensationSIN(iter);
164 const dynamicgraph::Vector& kp = m_kp_currentSIN(iter);
165 const dynamicgraph::Vector& ki = m_ki_currentSIN(iter);
166
167 m_i_err_integr += ki.cwiseProduct(i_des - i_real);
168
169 s = i_des + m_i_err_integr; // feedforward + integral feedback
170 s += kp.cwiseProduct(i_des - i_real); // proportional feedback
171 s +=
172 cur_sens_gains.cwiseProduct(i_offset_real); // sensor offset compensation
173 s += bemf_comp_perc.cwiseProduct(
174 bemf_factor.cwiseProduct(dq)); // back-EMF compensation
175
176 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
177 double err = s(i) - i_ll(i);
178 if (err > i_max_dz_comp(i))
179 m_dz_coeff(i) = 1.0;
180 else if (err < -i_max_dz_comp(i))
181 m_dz_coeff(i) = -1.0;
182 else
183 m_dz_coeff(i) = err / i_max_dz_comp(i);
184 }
185
186 // compensate dead zone
187 s += m_dz_coeff.cwiseProduct(
188 dead_zone_comp_perc.cwiseProduct(dead_zone_offsets));
189 // s = s.cwiseProduct(in_out_gain);
190
191 // when estimating current offset set ctrl to zero
192 if (m_emergency_stop_triggered ||
193 m_iter < static_cast<int>(m_currentOffsetIters))
194 s.setZero();
195
196 return s;
197}
198
199DEFINE_SIGNAL_OUT_FUNCTION(u_safe, dynamicgraph::Vector) {
200 if (!m_initSucceeded) {
201 SEND_WARNING_STREAM_MSG(
202 "Cannot compute signal u_safe before initialization!");
203 return s;
204 }
205
206 const dynamicgraph::Vector& u = m_uSOUT(iter);
207 const dynamicgraph::Vector& u_max = m_u_maxSIN(iter);
208 const dynamicgraph::Vector& u_saturation = m_u_saturationSIN(iter);
209 const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
210 const dynamicgraph::Vector& in_out_gain = m_in_out_gainSIN(iter);
211
212 if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
213 s.resize(m_robot_util->m_nbJoints);
214
215 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) {
216 double i_max = m_i_maxSIN(iter)(i);
217 if ((fabs(i_real(i)) > i_max)) {
218 m_emergency_stop_triggered = true;
219 SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) +
220 " measured current is too large: " + toString(i_real(i)) +
221 "A > " + toString(i_max) + "A",
222 MSG_TYPE_ERROR);
223 }
224
225 if (fabs(u(i)) > u_max(i)) {
226 m_emergency_stop_triggered = true;
227 SEND_MSG("Joint " + m_robot_util->get_name_from_id(i) +
228 " control is too large: " + toString(u(i)) + "A > " +
229 toString(u_max(i)) + "A",
230 MSG_TYPE_ERROR);
231 }
232
233 s(i) = u(i) * in_out_gain(i);
234
235 // saturate control signal
236 if (s(i) > u_saturation(i))
237 s(i) = u_saturation(i);
238 else if (s(i) < -u_saturation(i))
239 s(i) = -u_saturation(i);
240 }
241
242 // when estimating current offset set ctrl to zero
243 if (m_emergency_stop_triggered ||
244 m_iter < static_cast<int>(m_currentOffsetIters))
245 s.setZero();
246
247 return s;
248}
249
250DEFINE_SIGNAL_OUT_FUNCTION(i_real, dynamicgraph::Vector) {
251 if (!m_initSucceeded) {
252 SEND_WARNING_STREAM_MSG(
253 "Cannot compute signal i_real before initialization!");
254 return s;
255 }
256 s = m_i_measuredSIN(iter);
257
258 // compensate for current sensor offsets
259 const dynamicgraph::Vector& offset = m_i_sensor_offsets_real_outSOUT(iter);
260 s -= offset;
261
262 // compensate for current sensor gains
263 const dynamicgraph::Vector& K = m_i_sens_gainsSIN(iter);
264 s = s.cwiseProduct(K);
265
266 return s;
267}
268
269DEFINE_SIGNAL_OUT_FUNCTION(i_low_level, dynamicgraph::Vector) {
270 if (!m_initSucceeded) {
271 SEND_WARNING_STREAM_MSG(
272 "Cannot compute signal currents_low_level before initialization!");
273 return s;
274 }
275 s = m_i_measuredSIN(iter);
276 // Compensate for current sensor offsets
277 const dynamicgraph::Vector& i_offsets_low_level =
278 m_i_sensor_offsets_low_levelSIN(iter);
279 s -= i_offsets_low_level;
280 // Compensate for the current sensor gains
281 const dynamicgraph::Vector& K = m_i_sens_gainsSIN(iter);
282 s = s.cwiseProduct(K);
283 return s;
284}
285
286DEFINE_SIGNAL_OUT_FUNCTION(i_sensor_offsets_real_out, dynamicgraph::Vector) {
287 if (!m_initSucceeded) {
288 SEND_WARNING_STREAM_MSG(
289 "Cannot compute signal i_sensor_offsets_real_out before "
290 "initialization!");
291 return s;
292 }
293 const dynamicgraph::Vector& currents = m_i_measuredSIN(iter);
294
295 // Compute current sensor offsets
296 if (m_currentOffsetIters > 0) {
297 if (m_iter < static_cast<int>(m_currentOffsetIters))
298 m_i_offsets_real += (currents - m_i_offsets_real) / (m_iter + 1);
299 else if (m_iter == static_cast<int>(m_currentOffsetIters)) {
300 SEND_MSG("Current sensor offsets computed in " + toString(m_iter) +
301 " iterations: " + toString(m_i_offsets_real),
302 MSG_TYPE_INFO);
303 for (int i = 0; i < s.size(); i++)
304 if (fabs(m_i_offsets_real(i)) > 0.6) {
305 SEND_MSG(
306 "Current offset for joint " + m_robot_util->get_name_from_id(i) +
307 " is too large, suggesting that the sensor may be broken: " +
308 toString(m_i_offsets_real(i)),
309 MSG_TYPE_WARNING);
310 m_i_offsets_real(i) = 0.0;
311 }
312 }
313 }
314 m_iter++;
315
316 if (m_i_sensor_offsets_real_inSIN.isPlugged())
317 s = m_i_sensor_offsets_real_inSIN(iter);
318 else
319 s = m_i_offsets_real;
320
321 return s;
322}
323
324DEFINE_SIGNAL_OUT_FUNCTION(dead_zone_compensation, dynamicgraph::Vector) {
325 if (!m_initSucceeded) {
326 SEND_WARNING_STREAM_MSG(
327 "Cannot compute signal dead_zone_compensation before initialization!");
328 return s;
329 }
330 const dynamicgraph::Vector& dead_zone_offsets = m_dead_zone_offsetsSIN(iter);
331 m_u_safeSOUT(iter);
332 s = m_dz_coeff.cwiseProduct(dead_zone_offsets);
333 return s;
334}
335
336DEFINE_SIGNAL_OUT_FUNCTION(i_errors, dynamicgraph::Vector) {
337 if (!m_initSucceeded) {
338 SEND_WARNING_STREAM_MSG(
339 "Cannot compute signal i_errors before initialization!");
340 return s;
341 }
342 const dynamicgraph::Vector& u = m_uSOUT(iter);
343 const dynamicgraph::Vector& currents = m_i_realSOUT(iter);
344 s = u - currents;
345 return s;
346}
347
348DEFINE_SIGNAL_OUT_FUNCTION(i_errors_ll_wo_bemf, dynamicgraph::Vector) {
349 if (!m_initSucceeded) {
350 SEND_WARNING_STREAM_MSG(
351 "Cannot compute signal i_errors_ll_wo_bemf before initialization!");
352 return s;
353 }
354 const dynamicgraph::Vector& u = m_uSOUT(iter);
355 const dynamicgraph::Vector& currents = m_i_realSOUT(iter);
356 const dynamicgraph::Vector& bemfFactor = m_bemf_factorSIN(iter);
357 const dynamicgraph::Vector& bemf_comp_perc =
358 m_percentage_bemf_compensationSIN(iter);
359 const dynamicgraph::Vector& dq = m_dqSIN(iter);
360
361 s = u - currents -
362 (dynamicgraph::Vector::Ones(m_robot_util->m_nbJoints) - bemf_comp_perc)
363 .cwiseProduct(bemfFactor.cwiseProduct(dq));
364
365 for (int i = 0; i < s.size(); i++)
366 if (s(i) > 0.0) {
367 m_avg_i_err_pos(i) += (s(i) - m_avg_i_err_pos(i)) * 1e-3;
368 } else {
369 m_avg_i_err_neg(i) += (s(i) - m_avg_i_err_neg(i)) * 1e-3;
370 }
371 return s;
372}
373
374/* --- COMMANDS ---------------------------------------------------------- */
375
377
378/* ------------------------------------------------------------------- */
379/* --- ENTITY -------------------------------------------------------- */
380/* ------------------------------------------------------------------- */
381
382void CurrentController::display(std::ostream& os) const {
383 os << "CurrentController " << getName();
384 try {
385 getProfiler().report_all(3, os);
386 } catch (ExceptionSignal e) {
387 }
388}
389
390} // namespace torque_control
391} // namespace sot
392} // namespace dynamicgraph
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
void init(const double &dt, const std::string &robotRef, const unsigned int &currentOffsetIters)
void reset_integral()
current tracking error without BEMF effect
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22