00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "ethercat_hardware/motor_heating_model.h"
00036
00037 #include <boost/crc.hpp>
00038 #include <boost/static_assert.hpp>
00039 #include <boost/filesystem.hpp>
00040 #include <boost/bind.hpp>
00041 #include <boost/foreach.hpp>
00042 #include <boost/timer.hpp>
00043
00044
00045 #include <tinyxml.h>
00046
00047 #include <stdio.h>
00048 #include <errno.h>
00049 #include <exception>
00050
00051 namespace ethercat_hardware
00052 {
00053
00054
00055 static const int DEBUG_LEVEL = 0;
00056
00057
00058 bool MotorHeatingModelParametersEepromConfig::verifyCRC(void) const
00059 {
00060 BOOST_STATIC_ASSERT(sizeof(ethercat_hardware::MotorHeatingModelParametersEepromConfig) == 256);
00061 BOOST_STATIC_ASSERT( offsetof(ethercat_hardware::MotorHeatingModelParametersEepromConfig, crc32_) == (256-4));
00062 boost::crc_32_type crc32;
00063 crc32.process_bytes(this, offsetof(MotorHeatingModelParametersEepromConfig, crc32_));
00064 return (this->crc32_ == crc32.checksum());
00065 }
00066
00067 void MotorHeatingModelParametersEepromConfig::generateCRC(void)
00068 {
00069 boost::crc_32_type crc32;
00070 crc32.process_bytes(this, offsetof(MotorHeatingModelParametersEepromConfig, crc32_));
00071 this->crc32_ = crc32.checksum();
00072 }
00073
00074
00075
00076 MotorHeatingModelCommon::MotorHeatingModelCommon(ros::NodeHandle nh)
00077 {
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090 if (!nh.getParam("load_save_files", load_save_files_))
00091 {
00092 load_save_files_ = true;
00093 }
00094 if (!nh.getParam("update_save_files", update_save_files_))
00095 {
00096 update_save_files_ = true;
00097 }
00098 if (!nh.getParam("do_not_halt", disable_halt_))
00099 {
00100
00101
00102 disable_halt_ = true;
00103 }
00104 if (!nh.getParam("save_directory", save_directory_))
00105 {
00106 save_directory_ = "/var/lib/motor_heating_model";
00107 }
00108 if (!nh.getParam("enable_model", enable_model_))
00109 {
00110 enable_model_ = true;
00111 }
00112 if (!nh.getParam("publish_temperature", publish_temperature_))
00113 {
00114 publish_temperature_ = false;
00115 }
00116 }
00117
00118
00119 MotorHeatingModelCommon::MotorHeatingModelCommon() :
00120 update_save_files_ ( true ),
00121 save_directory_ ("/var/lib/motor_heating_model" ),
00122 load_save_files_ ( true ),
00123 disable_halt_ ( false ),
00124 enable_model_( true ),
00125 publish_temperature_( false )
00126 {
00127
00128 }
00129
00130
00131 void MotorHeatingModelCommon::attach(boost::shared_ptr<MotorHeatingModel> model)
00132 {
00133 {
00134 boost::lock_guard<boost::mutex> lock(mutex_);
00135 models_.push_back(model);
00136 }
00137 }
00138
00139
00140 bool MotorHeatingModelCommon::initialize()
00141 {
00142 if (update_save_files_)
00143 {
00144
00145 save_thread_ = boost::thread(boost::bind(&MotorHeatingModelCommon::saveThreadFunc, this));
00146 }
00147 return true;
00148 }
00149
00150
00158 void MotorHeatingModelCommon::saveThreadFunc()
00159 {
00160
00161
00162
00163 while (true)
00164 {
00165 sleep(10);
00166 {
00167 boost::lock_guard<boost::mutex> lock(mutex_);
00168 BOOST_FOREACH( boost::shared_ptr<MotorHeatingModel> model, models_ )
00169 {
00170 model->saveTemperatureState();
00171 }
00172 }
00173 }
00174
00175 {
00176 boost::lock_guard<boost::mutex> lock(mutex_);
00177 models_.clear();
00178 }
00179 }
00180
00181
00184 bool MotorHeatingModelCommon::createSaveDirectory()
00185 {
00186
00187 if (!boost::filesystem::exists(save_directory_))
00188 {
00189 ROS_WARN("Motor heating motor save directory '%s' does not exist, creating it", save_directory_.c_str());
00190 try {
00191 boost::filesystem::create_directory(save_directory_);
00192 }
00193 catch (const std::exception &e)
00194 {
00195 ROS_ERROR("Error creating save directory '%s' for motor model : %s",
00196 save_directory_.c_str(), e.what());
00197 return false;
00198 }
00199 }
00200
00201 return true;
00202 }
00203
00204
00205
00214 MotorHeatingModel::MotorHeatingModel(const MotorHeatingModelParameters &motor_params,
00215 const std::string &actuator_name,
00216 const std::string &hwid,
00217 const std::string &save_directory
00218 ) :
00219 overheat_(false),
00220 heating_energy_sum_(0.0),
00221 ambient_temperature_sum_(0.0),
00222 duration_since_last_sample_(0.0),
00223 publisher_(NULL),
00224 motor_params_(motor_params),
00225 actuator_name_(actuator_name),
00226 save_filename_(save_directory + "/" + actuator_name_ + ".save"),
00227 hwid_(hwid)
00228 {
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239 static const double default_ambient_temperature = 60.0;
00240 winding_temperature_ = default_ambient_temperature;
00241 housing_temperature_ = default_ambient_temperature;
00242 ambient_temperature_ = default_ambient_temperature;
00243
00244
00245 winding_to_housing_thermal_conductance_ = 1.0 / motor_params_.winding_to_housing_thermal_resistance_;
00246 housing_to_ambient_thermal_conductance_ = 1.0 / motor_params_.housing_to_ambient_thermal_resistance_;
00247 winding_thermal_mass_inverse_ =
00248 motor_params_.winding_to_housing_thermal_resistance_ / motor_params_.winding_thermal_time_constant_;
00249 housing_thermal_mass_inverse_ =
00250 motor_params_.housing_to_ambient_thermal_resistance_ / motor_params_.housing_thermal_time_constant_;
00251 }
00252
00253
00254 bool MotorHeatingModel::startTemperaturePublisher()
00255 {
00256 std::string topic("motor_temperature");
00257 if (!actuator_name_.empty())
00258 {
00259 topic = topic + "/" + actuator_name_;
00260 publisher_ = new realtime_tools::RealtimePublisher<ethercat_hardware::MotorTemperature>(ros::NodeHandle(), topic, 1, true);
00261 if (publisher_ == NULL)
00262 {
00263 ROS_ERROR("Could not allocate realtime publisher");
00264 return false;
00265 }
00266 }
00267 return true;
00268 }
00269
00270
00271
00272 double MotorHeatingModel::calculateMotorHeatPower(const ethercat_hardware::MotorTraceSample &s,
00273 const ethercat_hardware::ActuatorInfo &ai)
00274 {
00275
00276
00277
00278
00279
00280
00281
00282
00283 double output_voltage = s.programmed_pwm * s.supply_voltage;
00284
00285 double backemf_constant = 1.0 / (ai.speed_constant * 2.0 * M_PI * 1.0/60.0);
00286 double backemf_voltage = s.velocity * ai.encoder_reduction * backemf_constant;
00287
00288 double resistance_voltage = output_voltage - backemf_voltage;
00289
00290
00291 double heating_power = resistance_voltage * s.measured_current;
00292
00293 if (DEBUG_LEVEL)
00294 {
00295 if ((heating_power < -0.5) || (heating_power > 15.0))
00296 {
00297 ROS_DEBUG("heating power = %f, output_voltage=%f, backemf_voltage=%f, resistance_voltage=%f, current=%f",
00298 heating_power, output_voltage, backemf_voltage, resistance_voltage, s.measured_current);
00299 }
00300 }
00301
00302
00303 heating_power = std::max(heating_power, 0.0);
00304
00305 return heating_power;
00306 }
00307
00308
00309 bool MotorHeatingModel::update(double heating_power, double ambient_temperature, double duration)
00310 {
00311
00312
00313
00314 double heating_energy = heating_power * duration;
00315 double winding_energy_loss =
00316 (winding_temperature_ - housing_temperature_) * winding_to_housing_thermal_conductance_ * duration;
00317 double housing_energy_loss =
00318 (housing_temperature_ - ambient_temperature) * housing_to_ambient_thermal_conductance_ * duration;
00319
00320 winding_temperature_ += (heating_energy - winding_energy_loss) * winding_thermal_mass_inverse_;
00321 housing_temperature_ += (winding_energy_loss - housing_energy_loss) * housing_thermal_mass_inverse_;
00322
00323 {
00324 boost::lock_guard<boost::mutex> lock(mutex_);
00325 heating_energy_sum_ += heating_energy;
00326 ambient_temperature_sum_ += (ambient_temperature * duration);
00327 duration_since_last_sample_ += duration;
00328 if (winding_temperature_ > motor_params_.max_winding_temperature_)
00329 overheat_ = true;
00330 }
00331
00332 return !overheat_;
00333 }
00334
00335
00336
00337 double MotorHeatingModel::updateFromDowntimeWithInterval(double downtime,
00338 double saved_ambient_temperature,
00339 double interval,
00340 unsigned cycles)
00341 {
00342 static const double heating_power = 0.0;
00343 for (unsigned i=0; i<cycles; ++i)
00344 {
00345 if (downtime > interval)
00346 {
00347 update(heating_power, saved_ambient_temperature, interval);
00348 downtime -= interval;
00349 }
00350 else
00351 {
00352
00353 update(heating_power, saved_ambient_temperature, downtime);
00354 return 0.0;
00355 }
00356 }
00357 return downtime;
00358 }
00359
00360
00361 void MotorHeatingModel::updateFromDowntime(double downtime, double saved_ambient_temperature)
00362 {
00363 ROS_DEBUG("Initial temperatures : winding = %f, housing = %f", winding_temperature_, housing_temperature_);
00364 double saved_downtime = downtime;
00365
00366 boost::timer timer;
00367
00368
00369
00370 downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 0.01, 200);
00371 downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 0.10, 200);
00372 downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 1.00, 200);
00373 downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 10.0, 2000);
00374
00375
00376 if (downtime > 0.0)
00377 {
00378 ROS_DEBUG("Downtime too long, using ambient temperature as final motor temperature");
00379 winding_temperature_ = saved_ambient_temperature;
00380 housing_temperature_ = saved_ambient_temperature;
00381 }
00382
00383 ROS_DEBUG("Took %f milliseconds to sim %f seconds", timer.elapsed()*1000., saved_downtime);
00384 ROS_DEBUG("Final temperatures : winding = %f, housing = %f", winding_temperature_, housing_temperature_);
00385 }
00386
00387
00388 void MotorHeatingModel::reset()
00389 {
00390 {
00391 boost::lock_guard<boost::mutex> lock(mutex_);
00392 overheat_ = false;
00393 }
00394 }
00395
00396
00397 void MotorHeatingModel::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
00398 {
00399
00400
00401 bool overheat;
00402 double winding_temperature;
00403 double housing_temperature;
00404 double ambient_temperature_sum;
00405 double duration_since_last_sample;
00406 double average_ambient_temperature;
00407 double average_heating_power;
00408
00409 {
00410 boost::lock_guard<boost::mutex> lock(mutex_);
00411 overheat = overheat_;
00412 winding_temperature = winding_temperature_;
00413 housing_temperature = housing_temperature_;
00414 ambient_temperature_sum = ambient_temperature_sum_;
00415 duration_since_last_sample = duration_since_last_sample_;
00416
00417 if (duration_since_last_sample > 0.0)
00418 {
00419 average_ambient_temperature = ambient_temperature_sum / duration_since_last_sample;
00420 ambient_temperature_ = average_ambient_temperature;
00421 average_heating_power = heating_energy_sum_ / duration_since_last_sample;
00422 }
00423 else
00424 {
00425
00426 average_ambient_temperature = ambient_temperature_;
00427 average_heating_power = 0.0;
00428 }
00429
00430 ambient_temperature_sum_ = 0.0;
00431 heating_energy_sum_ = 0.0;
00432 duration_since_last_sample_ = 0.0;
00433 }
00434
00435
00436
00437 const int ERROR = 2;
00438 const int WARN = 1;
00439
00440
00441 if (overheat)
00442 {
00443
00444 d.mergeSummary(ERROR, "Motor overheated");
00445 }
00446 else if (winding_temperature > (motor_params_.max_winding_temperature_ * 0.90))
00447 {
00448
00449 d.mergeSummary(WARN, "Motor hot");
00450 }
00451
00452 d.addf("Motor winding temp limit (C)", "%f", motor_params_.max_winding_temperature_);
00453 d.addf("Motor winding temp (C)", "%f", winding_temperature);
00454 d.addf("Motor housing temp (C)", "%f", housing_temperature);
00455
00456
00457 d.addf("Heating power (Watts)", "%f", average_heating_power);
00458 d.addf("Ambient temp (C)", "%f", average_ambient_temperature);
00459
00460
00461 if ((publisher_ != NULL) && (publisher_->trylock()))
00462 {
00463
00464 ethercat_hardware::MotorTemperature &msg(publisher_->msg_);
00465 msg.stamp = ros::Time::now();
00466 msg.winding_temperature = winding_temperature;
00467 msg.housing_temperature = housing_temperature;
00468 msg.ambient_temperature = average_ambient_temperature;
00469 msg.heating_power = average_heating_power;
00470
00471
00472 publisher_->unlockAndPublish();
00473 }
00474 }
00475
00476
00477
00478 static bool getStringAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, std::string &value)
00479 {
00480 const char *val_str = elt->Attribute(param_name);
00481 if (val_str == NULL)
00482 {
00483 ROS_ERROR("No '%s' attribute for actuator '%s'", param_name, filename.c_str());
00484 return false;
00485 }
00486 value = val_str;
00487 return true;
00488 }
00489
00490
00491 static bool getDoubleAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, double &value)
00492 {
00493 const char *val_str = elt->Attribute(param_name);
00494 if (val_str == NULL)
00495 {
00496 ROS_ERROR("No '%s' attribute in '%s'", param_name, filename.c_str());
00497 return false;
00498 }
00499
00500 char *endptr=NULL;
00501 value = strtod(val_str, &endptr);
00502 if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
00503 {
00504 ROS_ERROR("Couldn't convert '%s' to double for attribute '%s' in '%s'",
00505 val_str, param_name, filename.c_str());
00506 return false;
00507 }
00508
00509 return true;
00510 }
00511
00512
00513 static bool getIntegerAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, int &value)
00514 {
00515 const char *val_str = elt->Attribute(param_name);
00516 if (val_str == NULL)
00517 {
00518 ROS_ERROR("No '%s' attribute in '%s'", param_name, filename.c_str());
00519 return false;
00520 }
00521
00522 char *endptr=NULL;
00523 value = strtol(val_str, &endptr, 0);
00524 if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
00525 {
00526 ROS_ERROR("Couldn't convert '%s' to integer for attribute '%s' in '%s'",
00527 val_str, param_name, filename.c_str());
00528 return false;
00529 }
00530
00531 return true;
00532 }
00533
00534
00535 static void saturateTemperature(double &temperature, const char *name)
00536 {
00537 static const double max_realistic_temperature = 200.0;
00538 static const double min_realistic_temperature = -10.0;
00539
00540 if (temperature > max_realistic_temperature)
00541 {
00542 ROS_WARN("%s temperature of %f Celcius is unrealisic. Using %f instead",
00543 name, temperature, max_realistic_temperature);
00544 temperature = max_realistic_temperature;
00545 }
00546 if (temperature < min_realistic_temperature)
00547 {
00548 ROS_WARN("%s temperature of %f Celcius is unrealisic. Using %f instead",
00549 name, temperature, min_realistic_temperature);
00550 temperature = min_realistic_temperature;
00551 }
00552 }
00553
00554
00555 bool MotorHeatingModel::loadTemperatureState()
00556 {
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573 if (!boost::filesystem::exists(save_filename_))
00574 {
00575 ROS_WARN("Motor heating model saved file '%s' does not exist. Using defaults", save_filename_.c_str());
00576 return false;
00577 }
00578
00579 TiXmlDocument xml;
00580 if (!xml.LoadFile(save_filename_))
00581 {
00582 ROS_ERROR("Unable to parse XML in save file '%s'", save_filename_.c_str());
00583 return false;
00584 }
00585
00586
00587 TiXmlElement *motor_temp_elt = xml.RootElement();
00588 if (motor_temp_elt == NULL)
00589 {
00590 ROS_ERROR("Unable to parse XML in save file '%s'", save_filename_.c_str());
00591 return false;
00592 }
00593
00594 std::string version;
00595 std::string actuator_name;
00596 std::string hwid;
00597 double winding_temperature;
00598 double housing_temperature;
00599 double ambient_temperature;
00600 int save_time_sec, save_time_nsec;
00601 if (!getStringAttribute(motor_temp_elt, save_filename_, "version", version))
00602 {
00603 return false;
00604 }
00605 const char* expected_version = "1";
00606 if (version != expected_version)
00607 {
00608 ROS_ERROR("Unknown version '%s', expected '%s'", version.c_str(), expected_version);
00609 return false;
00610 }
00611
00612 bool success = true;
00613 success &= getStringAttribute(motor_temp_elt, save_filename_, "actuator_name", actuator_name);
00614 success &= getStringAttribute(motor_temp_elt, save_filename_, "hwid", hwid);
00615 success &= getDoubleAttribute(motor_temp_elt, save_filename_, "housing_temperature", housing_temperature);
00616 success &= getDoubleAttribute(motor_temp_elt, save_filename_, "winding_temperature", winding_temperature);
00617 success &= getDoubleAttribute(motor_temp_elt, save_filename_, "ambient_temperature", ambient_temperature);
00618 success &= getIntegerAttribute(motor_temp_elt, save_filename_, "save_time_sec", save_time_sec);
00619 success &= getIntegerAttribute(motor_temp_elt, save_filename_, "save_time_nsec", save_time_nsec);
00620 if (!success)
00621 {
00622 return false;
00623 }
00624
00625 if (actuator_name != actuator_name_)
00626 {
00627 ROS_ERROR("In save file '%s' : expected actuator name '%s', got '%s'",
00628 save_filename_.c_str(), actuator_name_.c_str(), actuator_name.c_str());
00629 return false;
00630 }
00631
00632 if (hwid != hwid_)
00633 {
00634 ROS_WARN("In save file '%s' : expected HWID '%s', got '%s'",
00635 save_filename_.c_str(), hwid_.c_str(), hwid.c_str());
00636 }
00637
00638 saturateTemperature(housing_temperature, "Housing");
00639 saturateTemperature(winding_temperature, "Winding");
00640 saturateTemperature(ambient_temperature, "Ambient");
00641
00642
00643 winding_temperature_ = winding_temperature;
00644 housing_temperature_ = housing_temperature;
00645 ambient_temperature_ = ambient_temperature;
00646
00647
00648 double downtime = (ros::Time::now() - ros::Time(save_time_sec, save_time_nsec)).toSec();
00649 if (downtime < 0.0)
00650 {
00651 ROS_WARN("In save file '%s' : save time is %f seconds in future", save_filename_.c_str(), -downtime);
00652 }
00653 else
00654 {
00655 updateFromDowntime(downtime, ambient_temperature);
00656 }
00657
00658
00659 saturateTemperature(housing_temperature_, "(2) Housing");
00660 saturateTemperature(winding_temperature_, "(2) Winding");
00661
00662 return true;
00663 }
00664
00665
00666
00667
00668
00669 bool MotorHeatingModel::saveTemperatureState()
00670 {
00671
00672
00673
00674
00675 std::string tmp_filename = save_filename_ + ".tmp";
00676
00677 double winding_temperature;
00678 double housing_temperature;
00679 double ambient_temperature;
00680 {
00681 boost::lock_guard<boost::mutex> lock(mutex_);
00682 winding_temperature = winding_temperature_;
00683 housing_temperature = housing_temperature_;
00684 ambient_temperature = ambient_temperature_;
00685 }
00686
00687 TiXmlDocument xml;
00688 TiXmlDeclaration *decl = new TiXmlDeclaration( "1.0", "", "" );
00689 TiXmlElement *elmt = new TiXmlElement("motor_heating_model");
00690 elmt->SetAttribute("version", "1");
00691 elmt->SetAttribute("actuator_name", actuator_name_);
00692 elmt->SetAttribute("hwid", hwid_);
00693 elmt->SetDoubleAttribute("winding_temperature", winding_temperature);
00694 elmt->SetDoubleAttribute("housing_temperature", housing_temperature);
00695 elmt->SetDoubleAttribute("ambient_temperature", ambient_temperature);
00696 ros::Time now = ros::Time::now();
00697 elmt->SetAttribute("save_time_sec", now.sec);
00698 elmt->SetAttribute("save_time_nsec", now.nsec);
00699
00700 xml.LinkEndChild(decl);
00701 xml.LinkEndChild( elmt );
00702
00703
00704 if (!xml.SaveFile(tmp_filename))
00705 {
00706 ROS_WARN("Could not save motor heating model file '%s'", tmp_filename.c_str());
00707 return false;
00708 }
00709
00710
00711 if (rename(tmp_filename.c_str(), save_filename_.c_str()) != 0)
00712 {
00713 int error = errno;
00714 char errbuf[100];
00715 strerror_r(error, errbuf, sizeof(errbuf));
00716 errbuf[sizeof(errbuf)-1] = '\0';
00717 ROS_WARN("Problem renaming '%s' to '%s' : (%d) '%s'",
00718 tmp_filename.c_str(), save_filename_.c_str(), error, errbuf);
00719 return false;
00720 }
00721
00722 return true;
00723 }
00724
00725
00726
00727 };