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