00001 #include <ethercat_hardware/motor_model.h>
00002
00003
00004 static double min(double a, double b) {return (a<b)?a:b;}
00005
00006 MotorModel::MotorModel(unsigned trace_size) :
00007 trace_size_(trace_size),
00008 trace_index_(0),
00009 published_traces_(0),
00010 backemf_constant_(0.0),
00011 motor_voltage_error_(0.2),
00012 abs_motor_voltage_error_(0.02),
00013 measured_voltage_error_(0.2),
00014 abs_measured_voltage_error_(0.02),
00015 current_error_(0.2),
00016 abs_current_error_(0.02),
00017 abs_velocity_(0.02),
00018 abs_measured_current_(0.02),
00019 abs_board_voltage_(0.02),
00020 abs_position_delta_(0.02)
00021 {
00022 assert(trace_size_ > 0);
00023 trace_buffer_.reserve(trace_size_);
00024 reset();
00025 }
00026
00027 void MotorModel::reset()
00028 {
00029 diagnostics_mutex_.lock();
00030 {
00031 motor_voltage_error_.reset();
00032 abs_motor_voltage_error_.reset();
00033 measured_voltage_error_.reset();
00034 abs_measured_voltage_error_.reset();
00035 current_error_.reset();
00036 abs_current_error_.reset();
00037 abs_velocity_.reset();
00038 abs_measured_current_.reset();
00039 abs_board_voltage_.reset();
00040 abs_position_delta_.reset();
00041 diagnostics_level_ = 0;
00042 diagnostics_reason_ = "OK";
00043 }
00044 diagnostics_mutex_.unlock();
00045 previous_pwm_saturated_ = false;
00046 publish_delay_ = -1;
00047 publish_level_ = -1;
00048 publish_reason_ = "OK";
00049 }
00050
00053 bool MotorModel::initialize(const ethercat_hardware::ActuatorInfo &actuator_info,
00054 const ethercat_hardware::BoardInfo &board_info)
00055 {
00056 std::string topic("motor_trace");
00057 if (!actuator_info.name.empty())
00058 topic = topic + "/" + actuator_info.name;
00059 publisher_ = new realtime_tools::RealtimePublisher<ethercat_hardware::MotorTrace>(ros::NodeHandle(), topic, 1, true);
00060 if (publisher_ == NULL)
00061 return false;
00062
00063 actuator_info_ = actuator_info;
00064 board_info_ = board_info;
00065
00066 if (actuator_info_.speed_constant > 0.0) {
00067 backemf_constant_ = 1.0 / (actuator_info_.speed_constant * 2.0 * M_PI * 1.0/60.0);
00068 } else {
00069 ROS_ERROR("Invalid speed constant of %f for %s", actuator_info_.speed_constant, actuator_info_.name.c_str());
00070 return false;
00071 }
00072
00073 current_error_limit_ = board_info_.hw_max_current * 0.30;
00074
00075 {
00076 ethercat_hardware::MotorTrace &msg(publisher_->msg_);
00077 msg.actuator_info = actuator_info;
00078 msg.board_info = board_info;
00079 msg.samples.reserve(trace_size_);
00080 }
00081
00082 return true;
00083 }
00084
00087 void MotorModel::checkPublish()
00088 {
00089 if (publish_delay_ < 0)
00090 return;
00091 --publish_delay_;
00092 if (publish_delay_ >= 0)
00093 return;
00094
00095 ++published_traces_;
00096
00097 assert(publisher_ != NULL);
00098 if ((publisher_==NULL) || (!publisher_->trylock()))
00099 return;
00100
00101 ethercat_hardware::MotorTrace &msg(publisher_->msg_);
00102
00103 msg.header.stamp = ros::Time::now();
00104 msg.reason = publish_reason_;
00105 unsigned size=trace_buffer_.size();
00106 msg.samples.clear();
00107 msg.samples.reserve(size);
00108
00109
00110 for (unsigned i=0; i<size; ++i) {
00111 msg.samples.push_back(trace_buffer_.at((trace_index_+1+i)%size));
00112 }
00113
00114
00115 publish_delay_ = -1;
00116 publish_level_ = -1;
00117
00118 publisher_->unlockAndPublish();
00119 }
00120
00125 void MotorModel::flagPublish(const std::string &reason, int level, int delay)
00126 {
00127 if (delay < 0)
00128 delay = 0;
00129 else if (delay > 900) {
00130 delay = 900;
00131 }
00132 if (level > publish_level_)
00133 {
00134 publish_reason_ = reason;
00135 publish_delay_ = delay;
00136 publish_level_ = level;
00137 }
00138 }
00139
00140
00143 void MotorModel::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
00144 {
00145
00146 double motor_voltage_error;
00147 double motor_voltage_error_max;
00148 double abs_motor_voltage_error;
00149 double abs_motor_voltage_error_max;
00150 double current_error;
00151 double current_error_max;
00152 double abs_current_error;
00153 double abs_current_error_max;
00154 double est_motor_resistance;
00155 std::string reason;
00156 int level;
00157
00158 diagnostics_mutex_.lock();
00159 {
00160 motor_voltage_error = motor_voltage_error_.filter();
00161 motor_voltage_error_max = motor_voltage_error_.filter_max();
00162 abs_motor_voltage_error = abs_motor_voltage_error_.filter();
00163 abs_motor_voltage_error_max = abs_motor_voltage_error_.filter_max();
00164 current_error = current_error_.filter();
00165 current_error_max = current_error_.filter_max();
00166 abs_current_error = abs_current_error_.filter();
00167 abs_current_error_max = abs_current_error_.filter_max();
00168 est_motor_resistance = motor_resistance_.filter();
00169 reason = diagnostics_reason_;
00170 level = diagnostics_level_;
00171 }
00172 diagnostics_mutex_.unlock();
00173
00174 if (level > 0)
00175 d.mergeSummary(level, reason);
00176
00177 d.addf("Motor Voltage Error %", "%f", 100.0 * motor_voltage_error);
00178 d.addf("Max Motor Voltage Error %", "%f", 100.0 * motor_voltage_error_max);
00179 d.addf("Abs Filtered Voltage Error %", "%f", 100.0 * abs_motor_voltage_error);
00180 d.addf("Max Abs Filtered Voltage Error %", "%f", 100.0 * abs_motor_voltage_error_max);
00181
00182
00183 d.addf("Current Error", "%f", current_error);
00184 d.addf("Max Current Error", "%f", current_error_max);
00185 d.addf("Abs Filtered Current Error", "%f", abs_current_error);
00186 d.addf("Max Abs Filtered Current Error", "%f", abs_current_error_max);
00187
00188 d.addf("Motor Resistance Estimate", "%f", est_motor_resistance);
00189
00190 d.addf("# Published traces", "%d", published_traces_);
00191 }
00192
00193
00198 void MotorModel::sample(const ethercat_hardware::MotorTraceSample &s)
00199 {
00200
00201 const ethercat_hardware::ActuatorInfo &ai(actuator_info_);
00202 const ethercat_hardware::BoardInfo &bi(board_info_);
00203
00204
00205 double board_voltage = s.supply_voltage * s.programmed_pwm - bi.board_resistance * s.measured_current;
00206
00207
00208 double resistance_voltage = s.measured_current * ai.motor_resistance;
00209 double backemf_voltage = s.velocity * ai.encoder_reduction * backemf_constant_;
00210 double motor_voltage = resistance_voltage + backemf_voltage;
00211
00212
00213 const double resistance_error = 2.00;
00214 const double backemf_constant_error = 0.15;
00215 double motor_voltage_error_limit = 4.0 + fabs(resistance_voltage*resistance_error) + fabs(backemf_voltage * backemf_constant_error);
00216
00217 motor_voltage_error_limit = min(motor_voltage_error_limit, 10.0);
00218
00219
00220 double est_motor_resistance = 0.0;
00221 double est_motor_resistance_accuracy = 0.0;
00222
00223 if (fabs(s.measured_current) > (0.02 * bi.hw_max_current + 0.005))
00224 {
00225 est_motor_resistance = (board_voltage - backemf_voltage) / s.measured_current;
00226
00227
00228
00229 est_motor_resistance_accuracy = 1.0 / (1.0 + fabs(backemf_voltage / resistance_voltage));
00230 }
00231
00232
00233 if (s.enabled) {
00234 diagnostics_mutex_.lock();
00235
00236
00237 measured_voltage_error_.sample(s.measured_motor_voltage - board_voltage);
00238 abs_measured_voltage_error_.sample( fabs(measured_voltage_error_.filter()) );
00239
00240
00241 motor_voltage_error_.sample((motor_voltage - board_voltage) / motor_voltage_error_limit);
00242 bool new_max_voltage_error = abs_motor_voltage_error_.sample( fabs(motor_voltage_error_.filter()));
00243
00244
00245 bool pwm_saturated = ((s.programmed_pwm > bi.max_pwm_ratio*0.95) || (s.programmed_pwm < -bi.max_pwm_ratio*0.95));
00246 double current_error = s.measured_current-s.executed_current;
00247 bool new_max_current_error = false;
00248 if ((!pwm_saturated && !previous_pwm_saturated_) ||
00249 (fabs(current_error + current_error_.filter()) < current_error_.filter()) )
00250 {
00251 current_error_.sample(current_error);
00252 new_max_current_error = abs_current_error_.sample(fabs(current_error_.filter()));
00253 }
00254 previous_pwm_saturated_ = pwm_saturated;
00255
00256
00257
00258 if (new_max_voltage_error && (abs_motor_voltage_error_.filter_max() > 0.5))
00259 {
00260 flagPublish("New max voltage error", 1, 500);
00261 }
00262 else if( new_max_current_error && (abs_current_error_.filter_max() > (current_error_limit_ * 0.5)))
00263 {
00264 flagPublish("New max current error", 1, 500);
00265 }
00266
00267
00268 abs_velocity_.sample(fabs(s.velocity));
00269 abs_board_voltage_.sample(fabs(board_voltage));
00270 abs_measured_current_.sample(fabs(s.measured_current));
00271 if (!trace_buffer_.empty()) {
00272 double position_delta = trace_buffer_.at(trace_index_).encoder_position - s.encoder_position;
00273 abs_position_delta_.sample(fabs(position_delta));
00274 }
00275
00276
00277 motor_resistance_.sample(est_motor_resistance, 0.005 * est_motor_resistance_accuracy);
00278
00279 diagnostics_mutex_.unlock();
00280 }
00281
00282 {
00283 assert(trace_buffer_.size() <= trace_size_);
00284 if (trace_buffer_.size() >= trace_size_) {
00285 trace_index_ = (trace_index_+1)%trace_buffer_.size();
00286 trace_buffer_.at(trace_index_) = s;
00287 } else {
00288 trace_index_ = trace_buffer_.size();
00289 trace_buffer_.push_back(s);
00290 }
00291 }
00292
00293
00294 {
00295 ethercat_hardware::MotorTraceSample &s2(trace_buffer_.at(trace_index_));
00296 s2.motor_voltage_error_limit = motor_voltage_error_limit;
00297 s2.filtered_motor_voltage_error = motor_voltage_error_.filter();
00298 s2.filtered_abs_motor_voltage_error = abs_motor_voltage_error_.filter();
00299 s2.filtered_measured_voltage_error = measured_voltage_error_.filter();
00300 s2.filtered_abs_measured_voltage_error = abs_measured_voltage_error_.filter();
00301 s2.filtered_current_error = current_error_.filter();
00302 s2.filtered_abs_current_error = abs_current_error_.filter();
00303 }
00304 }
00305
00306
00313 bool MotorModel::verify()
00314 {
00315 const int ERROR = 2;
00316 const int WARN = 1;
00317 const int GOOD = 0;
00318
00319 bool rv=true;
00320
00321
00322 double measured_voltage_error_limit = board_info_.poor_measured_motor_voltage ? 10.0 : 4.0;
00323
00324 bool is_measured_voltage_error = abs_measured_voltage_error_.filter() > measured_voltage_error_limit;
00325 bool is_motor_voltage_error = abs_motor_voltage_error_.filter() > 1.0;
00326
00327 int level = GOOD;
00328 std::string reason;
00329
00330
00331 if (is_motor_voltage_error || is_measured_voltage_error)
00332 {
00333 rv = false;
00334 level = ERROR;
00335 reason = "Problem with the MCB, motor, encoder, or actuator model.";
00336
00337 if( is_measured_voltage_error )
00338 {
00339
00340 reason += " Board may be damaged.";
00341 }
00342 else if (is_motor_voltage_error)
00343 {
00344
00345 const double epsilon = 0.001;
00346 const double current_epsilon = 0.010;
00347 double encoder_tick_delta = 2 * M_PI / actuator_info_.pulses_per_revolution;
00348
00349
00350 if ((abs_measured_current_.filter() < current_epsilon) && (abs_current_error_.filter() > current_epsilon))
00351 {
00352
00353 reason += " Current near zero - check for unconnected motor leads.";
00354 }
00355 else if (abs_board_voltage_.filter() < epsilon)
00356 {
00357
00358 reason += " Voltage near zero - check for short circuit.";
00359 }
00360 else if (abs_velocity_.filter() < epsilon)
00361 {
00362
00363 reason += " Velocity near zero - check for encoder error.";
00364 }
00365 else if (abs_position_delta_.filter() < encoder_tick_delta)
00366 {
00367
00368 reason += " Encoder delta below 1 - check encoder wiring.";
00369 }
00370 }
00371 }
00372 else if (abs_current_error_.filter() > current_error_limit_)
00373 {
00374
00375 rv = false;
00376 level = ERROR;
00377 reason = "Current loop error too large (MCB failing to hit desired current)";
00378 }
00379 else if (abs_motor_voltage_error_.filter() > 0.7)
00380 {
00381 level = WARN;
00382 reason = "Potential problem with the MCB, motor, encoder, or actuator model.";
00383 }
00384 else if (abs_current_error_.filter() > (current_error_limit_ * 0.7))
00385 {
00386 level = WARN;
00387 reason = "Potential current loop error (MCB failing to hit desired current)";
00388 }
00389
00390 if (level > diagnostics_level_)
00391 {
00392 if (level == ERROR)
00393 flagPublish(reason, level, 100);
00394 diagnostics_mutex_.lock();
00395 diagnostics_level_ = level;
00396 diagnostics_reason_ = reason;
00397 diagnostics_mutex_.unlock();
00398 }
00399
00400 return rv;
00401 }
00402
00406 void MotorModel::SimpleFilter::sample(double value, double filter_coefficient)
00407 {
00408
00409
00410
00411 filtered_value_ += filter_coefficient * (value-filtered_value_);
00412 }
00413
00418 bool MotorModel::Filter::sample(double value)
00419 {
00420 bool new_max = false;
00421 SimpleFilter::sample(value, filter_coefficient_);
00422 if (fabs(filtered_value_) > max_filtered_value_) {
00423 new_max = true;
00424 max_filtered_value_ = fabs(filtered_value_);
00425 }
00426 return new_max;
00427 }
00428
00429 MotorModel::SimpleFilter::SimpleFilter()
00430 {
00431 reset();
00432 }
00433
00434 MotorModel::Filter::Filter(double filter_coefficient) :
00435 SimpleFilter(),
00436 filter_coefficient_(filter_coefficient)
00437 {
00438 reset();
00439 }
00440
00441 void MotorModel::SimpleFilter::reset()
00442 {
00443 filtered_value_ = 0.0;
00444 }
00445
00446 void MotorModel::Filter::reset()
00447 {
00448 SimpleFilter::reset();
00449 max_filtered_value_ = 0.0;
00450 }