motor_model.cpp
Go to the documentation of this file.
2 
3 //static double max(double a, double b) {return (a>b)?a:b;}
4 static double min(double a, double b) {return (a<b)?a:b;}
5 
6 MotorModel::MotorModel(unsigned trace_size) :
7  trace_size_(trace_size),
8  trace_index_(0),
9  published_traces_(0),
10  backemf_constant_(0.0),
11  motor_voltage_error_(0.2),
12  abs_motor_voltage_error_(0.02),
13  measured_voltage_error_(0.2),
14  abs_measured_voltage_error_(0.02),
15  current_error_(0.2),
16  abs_current_error_(0.02),
17  abs_velocity_(0.02),
18  abs_measured_current_(0.02),
19  abs_board_voltage_(0.02),
20  abs_position_delta_(0.02)
21 {
22  assert(trace_size_ > 0);
23  trace_buffer_.reserve(trace_size_);
24  reset();
25 }
26 
28 {
29  diagnostics_mutex_.lock();
30  {
42  diagnostics_reason_ = "OK";
43  }
44  diagnostics_mutex_.unlock();
46  publish_delay_ = -1;
47  publish_level_ = -1;
48  publish_reason_ = "OK";
49 }
50 
53 bool MotorModel::initialize(const ethercat_hardware::ActuatorInfo &actuator_info,
54  const ethercat_hardware::BoardInfo &board_info)
55 {
56  std::string topic("motor_trace");
57  if (!actuator_info.name.empty())
58  topic = topic + "/" + actuator_info.name;
60  if (publisher_ == NULL)
61  return false;
62 
63  actuator_info_ = actuator_info;
64  board_info_ = board_info;
65 
66  if (actuator_info_.speed_constant > 0.0) {
67  backemf_constant_ = 1.0 / (actuator_info_.speed_constant * 2.0 * M_PI * 1.0/60.0);
68  } else {
69  ROS_ERROR("Invalid speed constant of %f for %s", actuator_info_.speed_constant, actuator_info_.name.c_str());
70  return false;
71  }
72 
73  current_error_limit_ = board_info_.hw_max_current * 0.30;
74 
75  {
76  ethercat_hardware::MotorTrace &msg(publisher_->msg_);
77  msg.actuator_info = actuator_info;
78  msg.board_info = board_info;
79  msg.samples.reserve(trace_size_);
80  }
81 
82  return true;
83 }
84 
88 {
89  if (publish_delay_ < 0)
90  return;
92  if (publish_delay_ >= 0)
93  return;
94 
96 
97  assert(publisher_ != NULL);
98  if ((publisher_==NULL) || (!publisher_->trylock()))
99  return;
100 
101  ethercat_hardware::MotorTrace &msg(publisher_->msg_);
102 
103  msg.header.stamp = ros::Time::now();
104  msg.reason = publish_reason_;
105  unsigned size=trace_buffer_.size();
106  msg.samples.clear();
107  msg.samples.reserve(size);
108 
109  // TODO : is there a beter way to copy data between two std::vectors?
110  for (unsigned i=0; i<size; ++i) {
111  msg.samples.push_back(trace_buffer_.at((trace_index_+1+i)%size));
112  }
113 
114  // Cancel any delayed publishing from occuring
115  publish_delay_ = -1;
116  publish_level_ = -1;
117 
119 }
120 
125 void MotorModel::flagPublish(const std::string &reason, int level, int delay)
126 {
127  if (delay < 0)
128  delay = 0;
129  else if (delay > 900) {
130  delay = 900;
131  }
132  if (level > publish_level_)
133  {
134  publish_reason_ = reason;
135  publish_delay_ = delay;
136  publish_level_ = level;
137  }
138 }
139 
140 
144 {
145  // Should perform locking.. publishing of diagnostics is done from separate thread
146  double motor_voltage_error;
147  double motor_voltage_error_max;
148  double abs_motor_voltage_error;
149  double abs_motor_voltage_error_max;
150  double current_error;
151  double current_error_max;
152  double abs_current_error;
153  double abs_current_error_max;
154  double est_motor_resistance;
155  std::string reason;
156  int level;
157 
158  diagnostics_mutex_.lock();
159  {
160  motor_voltage_error = motor_voltage_error_.filter();
161  motor_voltage_error_max = motor_voltage_error_.filter_max();
162  abs_motor_voltage_error = abs_motor_voltage_error_.filter();
163  abs_motor_voltage_error_max = abs_motor_voltage_error_.filter_max();
164  current_error = current_error_.filter();
165  current_error_max = current_error_.filter_max();
166  abs_current_error = abs_current_error_.filter();
167  abs_current_error_max = abs_current_error_.filter_max();
168  est_motor_resistance = motor_resistance_.filter();
169  reason = diagnostics_reason_;
170  level = diagnostics_level_;
171  }
172  diagnostics_mutex_.unlock();
173 
174  if (level > 0)
175  d.mergeSummary(level, reason);
176 
177  d.addf("Motor Voltage Error %", "%f", 100.0 * motor_voltage_error);
178  d.addf("Max Motor Voltage Error %", "%f", 100.0 * motor_voltage_error_max);
179  d.addf("Abs Filtered Voltage Error %", "%f", 100.0 * abs_motor_voltage_error);
180  d.addf("Max Abs Filtered Voltage Error %", "%f", 100.0 * abs_motor_voltage_error_max);
181 
182  // TODO change names
183  d.addf("Current Error", "%f", current_error);
184  d.addf("Max Current Error", "%f", current_error_max);
185  d.addf("Abs Filtered Current Error", "%f", abs_current_error);
186  d.addf("Max Abs Filtered Current Error", "%f", abs_current_error_max);
187 
188  d.addf("Motor Resistance Estimate", "%f", est_motor_resistance);
189 
190  d.addf("# Published traces", "%d", published_traces_);
191 }
192 
193 
198 void MotorModel::sample(const ethercat_hardware::MotorTraceSample &s)
199 {
200 
201  const ethercat_hardware::ActuatorInfo &ai(actuator_info_);
202  const ethercat_hardware::BoardInfo &bi(board_info_);
203 
204  // Estimate what voltage board should be outputting.
205  double board_voltage = s.supply_voltage * s.programmed_pwm - bi.board_resistance * s.measured_current;
206 
207  // Compute motor voltage using motor model
208  double resistance_voltage = s.measured_current * ai.motor_resistance;
209  double backemf_voltage = s.velocity * ai.encoder_reduction * backemf_constant_;
210  double motor_voltage = resistance_voltage + backemf_voltage;
211 
212  // Compute limits for motor voltage error.
213  const double resistance_error = 2.00; // assume motor resistance can be off by 200%
214  const double backemf_constant_error = 0.15; // assume backemf const can be off by 15%
215  double motor_voltage_error_limit = 4.0 + fabs(resistance_voltage*resistance_error) + fabs(backemf_voltage * backemf_constant_error);
216  // Put max limit on back emf voltage
217  motor_voltage_error_limit = min(motor_voltage_error_limit, 10.0);
218 
219  // Estimate resistance
220  double est_motor_resistance = 0.0;
221  double est_motor_resistance_accuracy = 0.0;
222  // Don't even try calculation if the is not enough motor current
223  if (fabs(s.measured_current) > (0.02 * bi.hw_max_current + 0.005))
224  {
225  est_motor_resistance = (board_voltage - backemf_voltage) / s.measured_current;
226  // not all resistance samples are created equal.
227  // When motor is not moving, result resistance calculation will be better,
228  // because error in backemf constant value won't have any effect.
229  est_motor_resistance_accuracy = 1.0 / (1.0 + fabs(backemf_voltage / resistance_voltage));
230  }
231 
232  // Don't update filters if MCB is not enabled (halted)
233  if (s.enabled) {
234  diagnostics_mutex_.lock();
235 
236  // Compare measured voltage to motor voltage. Identify errors with broken inductor leads, etc
237  measured_voltage_error_.sample(s.measured_motor_voltage - board_voltage);
239 
240  // Compare motor and board voltage. Identify errors with motor or encoder
241  motor_voltage_error_.sample((motor_voltage - board_voltage) / motor_voltage_error_limit);
242  bool new_max_voltage_error = abs_motor_voltage_error_.sample( fabs(motor_voltage_error_.filter()));
243 
244  // Compare measured/programmed only when board output voltage is not (or was recently) maxed out.
245  bool pwm_saturated = ((s.programmed_pwm > bi.max_pwm_ratio*0.95) || (s.programmed_pwm < -bi.max_pwm_ratio*0.95));
246  double current_error = s.measured_current-s.executed_current;
247  bool new_max_current_error = false;
248  if ((!pwm_saturated && !previous_pwm_saturated_) ||
249  (fabs(current_error + current_error_.filter()) < current_error_.filter()) )
250  {
251  current_error_.sample(current_error);
252  new_max_current_error = abs_current_error_.sample(fabs(current_error_.filter()));
253  }
254  previous_pwm_saturated_ = pwm_saturated;
255 
256  // Publish trace if voltage or current error hits a new max above 40% of limit
257  // However, delay publishing in case error grows even larger in next few cycles
258  if (new_max_voltage_error && (abs_motor_voltage_error_.filter_max() > 0.5))
259  {
260  flagPublish("New max voltage error", 1, 500);
261  }
262  else if( new_max_current_error && (abs_current_error_.filter_max() > (current_error_limit_ * 0.5)))
263  {
264  flagPublish("New max current error", 1, 500);
265  }
266 
267  // Keep track of some values, so that the cause of motor voltage error can be determined laterx
268  abs_velocity_.sample(fabs(s.velocity));
269  abs_board_voltage_.sample(fabs(board_voltage));
270  abs_measured_current_.sample(fabs(s.measured_current));
271  if (!trace_buffer_.empty()) {
272  double position_delta = trace_buffer_.at(trace_index_).encoder_position - s.encoder_position;
273  abs_position_delta_.sample(fabs(position_delta));
274  }
275 
276  // Update filtered resistance estimate with resistance calculated this cycle
277  motor_resistance_.sample(est_motor_resistance, 0.005 * est_motor_resistance_accuracy);
278 
279  diagnostics_mutex_.unlock();
280  }
281 
282  { // Add motor trace sample to trace buffer
283  assert(trace_buffer_.size() <= trace_size_);
284  if (trace_buffer_.size() >= trace_size_) {
286  trace_buffer_.at(trace_index_) = s;
287  } else {
288  trace_index_ = trace_buffer_.size();
289  trace_buffer_.push_back(s);
290  }
291  }
292 
293  // Add values calculated by model to new sample in trace
294  {
295  ethercat_hardware::MotorTraceSample &s2(trace_buffer_.at(trace_index_));
296  s2.motor_voltage_error_limit = motor_voltage_error_limit;
297  s2.filtered_motor_voltage_error = motor_voltage_error_.filter();
298  s2.filtered_abs_motor_voltage_error = abs_motor_voltage_error_.filter();
299  s2.filtered_measured_voltage_error = measured_voltage_error_.filter();
300  s2.filtered_abs_measured_voltage_error = abs_measured_voltage_error_.filter();
301  s2.filtered_current_error = current_error_.filter();
302  s2.filtered_abs_current_error = abs_current_error_.filter();
303  }
304 }
305 
306 
314 {
315  const int ERROR = 2;
316  const int WARN = 1;
317  const int GOOD = 0;
318 
319  bool rv=true;
320 
321  // Error limits should realy be parameters, not hardcoded.
322  double measured_voltage_error_limit = board_info_.poor_measured_motor_voltage ? 10.0 : 4.0;
323 
324  bool is_measured_voltage_error = abs_measured_voltage_error_.filter() > measured_voltage_error_limit;
325  bool is_motor_voltage_error = abs_motor_voltage_error_.filter() > 1.0; // 1.0 = 100% motor_voltage_error_limit
326 
327  int level = GOOD;
328  std::string reason;
329 
330  // Check back-EMF consistency
331  if (is_motor_voltage_error || is_measured_voltage_error)
332  {
333  rv = false;
334  level = ERROR;
335  reason = "Problem with the MCB, motor, encoder, or actuator model.";
336 
337  if( is_measured_voltage_error )
338  {
339  // Problem with board
340  reason += " Board may be damaged.";
341  }
342  else if (is_motor_voltage_error)
343  {
344  //Something is wrong with the encoder, the motor, or the motor board
345  const double epsilon = 0.001;
346  const double current_epsilon = 0.010;
347  double encoder_tick_delta = 2 * M_PI / actuator_info_.pulses_per_revolution;
348 
349  //Try to diagnose further
350  if ((abs_measured_current_.filter() < current_epsilon) && (abs_current_error_.filter() > current_epsilon))
351  {
352  //measured_current_ ~= 0 -> motor open-circuit likely
353  reason += " Current near zero - check for unconnected motor leads.";
354  }
355  else if (abs_board_voltage_.filter() < epsilon)
356  {
357  //motor_voltage_ ~= 0 -> motor short-circuit likely
358  reason += " Voltage near zero - check for short circuit.";
359  }
360  else if (abs_velocity_.filter() < epsilon)
361  {
362  // motor_velocity == 0 -> encoder failure likely
363  reason += " Velocity near zero - check for encoder error.";
364  }
365  else if (abs_position_delta_.filter() < encoder_tick_delta)
366  {
367  // encoder changing by only 0 or 1 ticks --> check for disconnected wire.
368  reason += " Encoder delta below 1 - check encoder wiring.";
369  }
370  }
371  }
373  {
374  //complain and shut down
375  rv = false;
376  level = ERROR;
377  reason = "Current loop error too large (MCB failing to hit desired current)";
378  }
379  else if (abs_motor_voltage_error_.filter() > 0.7)
380  {
381  level = WARN;
382  reason = "Potential problem with the MCB, motor, encoder, or actuator model.";
383  }
384  else if (abs_current_error_.filter() > (current_error_limit_ * 0.7))
385  {
386  level = WARN;
387  reason = "Potential current loop error (MCB failing to hit desired current)";
388  }
389 
390  if (level > diagnostics_level_)
391  {
392  if (level == ERROR)
393  flagPublish(reason, level, 100);
394  diagnostics_mutex_.lock();
395  diagnostics_level_ = level;
396  diagnostics_reason_ = reason;
397  diagnostics_mutex_.unlock();
398  }
399 
400  return rv;
401 }
402 
406 void MotorModel::SimpleFilter::sample(double value, double filter_coefficient)
407 {
408  // F = A*V + (1-A)*F
409  // F = A*V + F - A*F
410  // F = F + A*(V-F)
411  filtered_value_ += filter_coefficient * (value-filtered_value_);
412 }
413 
418 bool MotorModel::Filter::sample(double value)
419 {
420  bool new_max = false;
421  SimpleFilter::sample(value, filter_coefficient_);
422  if (fabs(filtered_value_) > max_filtered_value_) {
423  new_max = true;
424  max_filtered_value_ = fabs(filtered_value_);
425  }
426  return new_max;
427 }
428 
430 {
431  reset();
432 }
433 
434 MotorModel::Filter::Filter(double filter_coefficient) :
435  SimpleFilter(),
436  filter_coefficient_(filter_coefficient)
437 {
438  reset();
439 }
440 
442 {
443  filtered_value_ = 0.0;
444 }
445 
447 {
449  max_filtered_value_ = 0.0;
450 }
msg
double filter_max() const
Definition: motor_model.h:69
double epsilon
boost::mutex diagnostics_mutex_
Definition: motor_model.h:77
void reset()
Definition: motor_model.cpp:27
bool verify()
Check for errors between sample data and motor model.
WARN
double current_error_limit_
Definition: motor_model.h:42
void sample(double value, double filter_coefficient)
Updates filter with newly sampled value.
unsigned trace_index_
Definition: motor_model.h:34
double max_filtered_value_
Definition: motor_model.h:72
bool sample(double value)
Updates filter with newly sampled value, also tracks max value.
Filter abs_position_delta_
Definition: motor_model.h:90
Filter abs_measured_voltage_error_
Definition: motor_model.h:81
ethercat_hardware::BoardInfo board_info_
Definition: motor_model.h:37
unsigned published_traces_
Definition: motor_model.h:35
Filter motor_voltage_error_
Definition: motor_model.h:78
unsigned trace_size_
Definition: motor_model.h:33
Filter abs_measured_current_
Definition: motor_model.h:88
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
Collects and publishes device diagnostics.
Filter abs_velocity_
Definition: motor_model.h:87
SimpleFilter motor_resistance_
Definition: motor_model.h:84
int diagnostics_level_
Definition: motor_model.h:48
void addf(const std::string &key, const char *format,...)
Filter(double filter_coefficient)
realtime_tools::RealtimePublisher< ethercat_hardware::MotorTrace > * publisher_
Definition: motor_model.h:41
std::string diagnostics_reason_
Definition: motor_model.h:49
Filter measured_voltage_error_
Definition: motor_model.h:80
void sample(const ethercat_hardware::MotorTraceSample &s)
Call for each update.
int publish_delay_
Definition: motor_model.h:43
bool initialize(const ethercat_hardware::ActuatorInfo &actuator_info, const ethercat_hardware::BoardInfo &board_info)
Initializes motor trace publisher.
Definition: motor_model.cpp:53
Filter abs_board_voltage_
Definition: motor_model.h:89
Filter abs_motor_voltage_error_
Definition: motor_model.h:79
double filter() const
Definition: motor_model.h:57
std::vector< ethercat_hardware::MotorTraceSample > trace_buffer_
Definition: motor_model.h:40
int publish_level_
Definition: motor_model.h:44
bool previous_pwm_saturated_
Definition: motor_model.h:39
void checkPublish()
Publishes motor trace if delay time is up.
Definition: motor_model.cpp:87
Filter current_error_
Definition: motor_model.h:82
void flagPublish(const std::string &reason, int level, int delay)
flags delayed publish of motor trace.
void mergeSummary(unsigned char lvl, const std::string s)
std::string publish_reason_
Definition: motor_model.h:45
static Time now()
ethercat_hardware::ActuatorInfo actuator_info_
Definition: motor_model.h:36
static double min(double a, double b)
Definition: motor_model.cpp:4
double backemf_constant_
Definition: motor_model.h:38
#define ROS_ERROR(...)
MotorModel(unsigned trace_size)
Definition: motor_model.cpp:6
Filter abs_current_error_
Definition: motor_model.h:83
ERROR


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Mar 4 2021 03:10:21