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> |
9 |
|
|
#include <sot/torque_control/commands-helper.hh> |
10 |
|
|
#include <sot/torque_control/current-controller.hh> |
11 |
|
|
#include <tsid/utils/statistics.hpp> |
12 |
|
|
#include <tsid/utils/stop-watch.hpp> |
13 |
|
|
|
14 |
|
|
namespace dynamicgraph { |
15 |
|
|
namespace sot { |
16 |
|
|
namespace torque_control { |
17 |
|
|
namespace dynamicgraph = ::dynamicgraph; |
18 |
|
|
using namespace dynamicgraph; |
19 |
|
|
using namespace dynamicgraph::command; |
20 |
|
|
using namespace std; |
21 |
|
|
using 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 |
|
|
|
37 |
|
|
/// Define EntityClassName here rather than in the header file |
38 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
39 |
|
|
typedef CurrentController EntityClassName; |
40 |
|
|
|
41 |
|
|
/* --- DG FACTORY ---------------------------------------------------- */ |
42 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(CurrentController, "CurrentController"); |
43 |
|
|
|
44 |
|
|
/* ------------------------------------------------------------------- */ |
45 |
|
|
/* --- CONSTRUCTION -------------------------------------------------- */ |
46 |
|
|
/* ------------------------------------------------------------------- */ |
47 |
|
|
// to do rename 'pwm' to 'current' |
48 |
|
|
CurrentController::CurrentController(const std::string& name) |
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( |
98 |
|
|
*this, &CurrentController::init, |
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 |
|
|
|
111 |
|
|
void 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 |
|
|
|
136 |
|
|
DEFINE_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 |
|
|
|
199 |
|
|
DEFINE_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 |
|
|
|
250 |
|
|
DEFINE_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 |
|
|
|
269 |
|
|
DEFINE_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 |
|
|
|
286 |
|
|
DEFINE_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 |
|
|
|
324 |
|
|
DEFINE_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 |
|
|
|
336 |
|
|
DEFINE_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 |
|
|
|
348 |
|
|
DEFINE_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 |
|
|
|
376 |
|
|
void CurrentController::reset_integral() { m_i_err_integr.setZero(); } |
377 |
|
|
|
378 |
|
|
/* ------------------------------------------------------------------- */ |
379 |
|
|
/* --- ENTITY -------------------------------------------------------- */ |
380 |
|
|
/* ------------------------------------------------------------------- */ |
381 |
|
|
|
382 |
|
|
void 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 |