motor_model.cpp
Go to the documentation of this file.
00001 #include <ethercat_hardware/motor_model.h>
00002 
00003 //static double max(double a, double b) {return (a>b)?a:b;}
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   // TODO : is there a beter way to copy data between two std::vectors?
00110   for (unsigned i=0; i<size; ++i) {
00111     msg.samples.push_back(trace_buffer_.at((trace_index_+1+i)%size));
00112   }
00113 
00114   // Cancel any delayed publishing from occuring
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   // Should perform locking.. publishing of diagnostics is done from separate thread
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   // TODO change names
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   // Estimate what voltage board should be outputting.
00205   double board_voltage = s.supply_voltage * s.programmed_pwm - bi.board_resistance * s.measured_current;
00206   
00207   // Compute motor voltage using motor model 
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   // Compute limits for motor voltage error.
00213   const double resistance_error = 2.00;    // assume motor resistance can be off by 200%
00214   const double backemf_constant_error = 0.15; // assume backemf const can be off by 15%
00215   double motor_voltage_error_limit = 4.0 + fabs(resistance_voltage*resistance_error) + fabs(backemf_voltage * backemf_constant_error);
00216   // Put max limit on back emf voltage
00217   motor_voltage_error_limit = min(motor_voltage_error_limit, 10.0);
00218 
00219   // Estimate resistance
00220   double est_motor_resistance = 0.0;
00221   double est_motor_resistance_accuracy = 0.0;
00222   // Don't even try calculation if the is not enough motor current
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     // not all resistance samples are created equal.  
00227     // When motor is not moving, result resistance calculation will be better,
00228     // because error in backemf constant value won't have any effect.
00229     est_motor_resistance_accuracy = 1.0 / (1.0 + fabs(backemf_voltage / resistance_voltage));
00230   }
00231 
00232   // Don't update filters if MCB is not enabled (halted)
00233   if (s.enabled) {
00234     diagnostics_mutex_.lock();
00235     
00236     // Compare measured voltage to motor voltage.  Identify errors with broken inductor leads, etc
00237     measured_voltage_error_.sample(s.measured_motor_voltage - board_voltage);
00238     abs_measured_voltage_error_.sample( fabs(measured_voltage_error_.filter()) );
00239     
00240     // Compare motor and board voltage. Identify errors with motor or encoder
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     // Compare measured/programmed only when board output voltage is not (or was recently) maxed out.
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     // Publish trace if voltage or current error hits a new max above 40% of limit
00257     // However, delay publishing in case error grows even larger in next few cycles
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     // Keep track of some values, so that the cause of motor voltage error can be determined laterx
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     // Update filtered resistance estimate with resistance calculated this cycle
00277     motor_resistance_.sample(est_motor_resistance, 0.005 * est_motor_resistance_accuracy);
00278 
00279     diagnostics_mutex_.unlock();
00280   }
00281 
00282   { // Add motor trace sample to trace buffer
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   // Add values calculated by model to new sample in trace
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   // Error limits should realy be parameters, not hardcoded.
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; // 1.0 = 100% motor_voltage_error_limit  
00326 
00327   int level = GOOD;
00328   std::string reason;
00329 
00330   // Check back-EMF consistency
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       // Problem with board
00340       reason += " Board may be damaged.";
00341     }
00342     else if (is_motor_voltage_error) 
00343     {
00344       //Something is wrong with the encoder, the motor, or the motor board
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       //Try to diagnose further
00350       if ((abs_measured_current_.filter() < current_epsilon) && (abs_current_error_.filter() > current_epsilon))
00351       {
00352         //measured_current_ ~= 0 -> motor open-circuit likely
00353         reason += " Current near zero - check for unconnected motor leads.";
00354       }
00355       else if (abs_board_voltage_.filter() < epsilon)
00356       {
00357         //motor_voltage_ ~= 0 -> motor short-circuit likely
00358         reason += " Voltage near zero - check for short circuit.";
00359       }
00360       else if (abs_velocity_.filter() < epsilon)
00361       {
00362         // motor_velocity == 0 -> encoder failure likely
00363         reason += " Velocity near zero - check for encoder error.";
00364       }
00365       else if (abs_position_delta_.filter() < encoder_tick_delta)
00366       {
00367         // encoder changing by only 0 or 1 ticks --> check for disconnected wire.
00368         reason += " Encoder delta below 1 - check encoder wiring.";
00369       }
00370     }
00371   }
00372   else if (abs_current_error_.filter() > current_error_limit_)
00373   {
00374     //complain and shut down
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   // F = A*V + (1-A)*F 
00409   // F = A*V + F - A*F 
00410   // F = F + A*(V-F)
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 }


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:45