ThermoLimiter.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "ThermoLimiter.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/ModelLoaderUtil.h>
00013 #include <hrpUtil/MatrixSolvers.h>
00014 #include <cmath>
00015 
00016 #define DQ_MAX 1.0
00017 
00018 typedef coil::Guard<coil::Mutex> Guard;
00019 
00020 // Module specification
00021 // <rtc-template block="module_spec">
00022 static const char* thermolimiter_spec[] =
00023   {
00024     "implementation_id", "ThermoLimiter",
00025     "type_name",         "ThermoLimiter",
00026     "description",       "thermo limiter",
00027     "version",           HRPSYS_PACKAGE_VERSION,
00028     "vendor",            "AIST",
00029     "category",          "example",
00030     "activity_type",     "DataFlowComponent",
00031     "max_instance",      "10",
00032     "language",          "C++",
00033     "lang_type",         "compile",
00034     // Configuration variables
00035     "conf.default.debugLevel", "0",
00036     ""
00037   };
00038 // </rtc-template>
00039 
00040 ThermoLimiter::ThermoLimiter(RTC::Manager* manager)
00041   : RTC::DataFlowComponentBase(manager),
00042     // <rtc-template block="initializer">
00043     m_tempInIn("tempIn", m_tempIn),
00044     m_tauMaxOutOut("tauMax", m_tauMaxOut),
00045     m_beepCommandOutOut("beepCommand", m_beepCommandOut),
00046     m_ThermoLimiterServicePort("ThermoLimiterService"),
00047     m_debugLevel(0)
00048 {
00049   m_ThermoLimiterService.thermolimiter(this);
00050 }
00051 
00052 ThermoLimiter::~ThermoLimiter()
00053 {
00054 }
00055 
00056 RTC::ReturnCode_t ThermoLimiter::onInitialize()
00057 {
00058   std::cerr << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl;
00059   // <rtc-template block="bind_config">
00060   // Bind variables and configuration variable
00061   bindParameter("debugLevel", m_debugLevel, "0");
00062   
00063   // </rtc-template>
00064 
00065   // Registration: InPort/OutPort/Service
00066   // <rtc-template block="registration">
00067   // Set InPort buffers
00068   addInPort("tempIn", m_tempInIn);
00069 
00070   // Set OutPort buffer
00071   addOutPort("tauMax", m_tauMaxOutOut);
00072   addOutPort("beepCommand", m_beepCommandOutOut);
00073   
00074   // Set service provider to Ports
00075   m_ThermoLimiterServicePort.registerProvider("service0", "ThermoLimiterService", m_ThermoLimiterService);
00076 
00077   // Set service consumers to Ports
00078   
00079   // Set CORBA Service Ports
00080   addPort(m_ThermoLimiterServicePort);
00081 
00082   // </rtc-template>
00083 
00084   RTC::Properties& prop = getProperties();
00085   coil::stringTo(m_dt, prop["dt"].c_str());
00086 
00087   m_robot = hrp::BodyPtr(new hrp::Body());
00088 
00089   RTC::Manager& rtcManager = RTC::Manager::instance();
00090   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00091   int comPos = nameServer.find(",");
00092   if (comPos < 0){
00093     comPos = nameServer.length();
00094   }
00095   nameServer = nameServer.substr(0, comPos);
00096   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00097   if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
00098                                CosNaming::NamingContext::_duplicate(naming.getRootContext())
00099         )){
00100     std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
00101               << std::endl;
00102   }
00103   // set limit of motor temperature
00104   coil::vstring motorTemperatureLimitFromConf = coil::split(prop["motor_temperature_limit"], ",");
00105   m_motorTemperatureLimit.resize(m_robot->numJoints());
00106   if (motorTemperatureLimitFromConf.size() != m_robot->numJoints()) {
00107     std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motor_temperature_limit is " << motorTemperatureLimitFromConf.size() << ", not equal to " << m_robot->numJoints() << std::endl;
00108     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00109       m_motorTemperatureLimit[i] = 80.0;
00110     }
00111   } else {
00112     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00113       coil::stringTo(m_motorTemperatureLimit[i], motorTemperatureLimitFromConf[i].c_str());
00114     }
00115   }
00116   if (m_debugLevel > 0) {
00117     std::cerr <<  "motor_temperature_limit: ";
00118     for(int i = 0; i < m_motorTemperatureLimit.size(); i++) {
00119       std::cerr << m_motorTemperatureLimit[i] << " ";
00120     }
00121     std::cerr << std::endl;
00122   }
00123 
00124   // set temperature of environment
00125   double ambientTemp = 25.0;
00126   if (prop["ambient_tmp"] != "") {
00127     coil::stringTo(ambientTemp, prop["ambient_tmp"].c_str());
00128   }
00129   std::cerr << "[" << m_profile.instance_name << "] : ambient temperature: " << ambientTemp << std::endl;
00130 
00131   // set limit of motor heat parameters
00132   coil::vstring motorHeatParamsFromConf = coil::split(prop["motor_heat_params"], ",");
00133   m_motorHeatParams.resize(m_robot->numJoints());
00134   if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) {
00135     std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motor_heat_param is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl;
00136     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00137       m_motorHeatParams[i].defaultParams();
00138       m_motorHeatParams[i].temperature = ambientTemp;
00139     }
00140 
00141   } else {
00142     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00143       m_motorHeatParams[i].temperature = ambientTemp;
00144       coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str());
00145       coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str());
00146     }
00147   }
00148   
00149   if (m_debugLevel > 0) {
00150     std::cerr <<  "motor_heat_param: ";
00151     for(std::vector<MotorHeatParam>::iterator it = m_motorHeatParams.begin(); it != m_motorHeatParams.end(); ++it){
00152       std::cerr << (*it).temperature << "," << (*it).currentCoeffs << "," << (*it).thermoCoeffs << ", ";
00153     }
00154     std::cerr << std::endl;
00155     std::cerr << "default torque limit from model:" << std::endl;
00156     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00157       std::cerr << m_robot->joint(i)->name << ":" << m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst << std::endl;
00158     }
00159   }
00160 
00161   // set alarm ratio threshold
00162   m_alarmRatio = 0.5;
00163   if (prop["alarm_ratio"] != "") {
00164     coil::stringTo(m_alarmRatio, prop["alarm_ratio"].c_str());
00165   }
00166   if (m_debugLevel > 0) {
00167     std::cerr << "alarmRatio: " << m_alarmRatio << std::endl;
00168   }
00169 
00170   // allocate memory for outPorts
00171   m_tauMaxOut.data.length(m_robot->numJoints());
00172   m_debug_print_freq = static_cast<int>(0.1/m_dt); // once per 0.1 [s]
00173   m_beepCommandOut.data.length(bc.get_num_beep_info());
00174   
00175   return RTC::RTC_OK;
00176 }
00177 
00178 /*
00179 RTC::ReturnCode_t ThermoLimiter::onFinalize()
00180 {
00181   return RTC::RTC_OK;
00182 }
00183 */
00184 
00185 /*
00186 RTC::ReturnCode_t ThermoLimiter::onStartup(RTC::UniqueId ec_id)
00187 {
00188   return RTC::RTC_OK;
00189 }
00190 */
00191 
00192 /*
00193 RTC::ReturnCode_t ThermoLimiter::onShutdown(RTC::UniqueId ec_id)
00194 {
00195   return RTC::RTC_OK;
00196 }
00197 */
00198 
00199 RTC::ReturnCode_t ThermoLimiter::onActivated(RTC::UniqueId ec_id)
00200 {
00201   std::cerr << "[" << m_profile.instance_name << "] : onActivated(" << ec_id << ")" << std::endl;
00202   return RTC::RTC_OK;
00203 }
00204 
00205 RTC::ReturnCode_t ThermoLimiter::onDeactivated(RTC::UniqueId ec_id)
00206 {
00207   std::cerr << "[" << m_profile.instance_name << "] : onDeactivated(" << ec_id << ")" << std::endl;
00208   return RTC::RTC_OK;
00209 }
00210 
00211 RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id)
00212 {
00213   // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << "), data = " << m_data.data << std::endl;
00214   m_loop++;
00215 
00216   if (isDebug()) {
00217     std::cerr << "[" << m_profile.instance_name << "]" << std::endl;
00218   }
00219   
00220   coil::TimeValue coiltm(coil::gettimeofday());
00221   RTC::Time tm;
00222   tm.sec = coiltm.sec();
00223   tm.nsec = coiltm.usec()*1000;
00224   hrp::dvector tauMax;
00225   tauMax.resize(m_robot->numJoints());
00226 
00227   double thermoLimitRatio = 0.0;
00228   std::string thermoLimitPrefix = "ThermoLimit";
00229   
00230   // update port
00231   if (m_tempInIn.isNew()) {
00232     m_tempInIn.read();
00233   }
00234 
00235   Guard guard(m_mutex);
00236   if (isDebug()) {
00237     std::cerr << "temperature: ";
00238     for (unsigned int i = 0; i < m_tempIn.data.length(); i++) {
00239       std::cerr << " " << m_tempIn.data[i];
00240     }
00241     std::cerr << std::endl;
00242   }
00243  
00244   // calculate tauMax from temperature
00245   if (m_tempIn.data.length() == m_robot->numJoints()) {
00246     calcMaxTorqueFromTemperature(tauMax);
00247   } else {
00248     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00249       tauMax[i] = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst; // default torque limit from model
00250     }
00251   }
00252 
00253   if (isDebug()) {
00254     std::cerr << "tauMax: ";
00255     for (int i = 0; i < tauMax.size(); i++) {
00256       std::cerr << tauMax[i] << " ";
00257     }
00258     std::cerr << std::endl;
00259   }
00260 
00261   // emergency notification
00262   if (m_tempIn.data.length() == m_robot->numJoints()) {
00263     thermoLimitRatio = calcEmergencyRatio(m_tempIn, m_motorTemperatureLimit, m_alarmRatio, thermoLimitPrefix);
00264   }
00265 
00266   // call beep (3136/0.8=3920)
00267   callBeep(thermoLimitRatio, m_alarmRatio);
00268   
00269   // output restricted tauMax
00270   for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00271     m_tauMaxOut.data[i] = tauMax[i];
00272   }
00273   m_tauMaxOut.tm = tm;
00274   m_tauMaxOutOut.write();
00275   m_beepCommandOut.tm = tm;
00276   if (bc.isWritable()) m_beepCommandOutOut.write();
00277   return RTC::RTC_OK;
00278 }
00279 
00280 /*
00281 RTC::ReturnCode_t ThermoLimiter::onAborting(RTC::UniqueId ec_id)
00282 {
00283   return RTC::RTC_OK;
00284 }
00285 */
00286 
00287 /*
00288 RTC::ReturnCode_t ThermoLimiter::onError(RTC::UniqueId ec_id)
00289 {
00290   return RTC::RTC_OK;
00291 }
00292 */
00293 
00294 /*
00295 RTC::ReturnCode_t ThermoLimiter::onReset(RTC::UniqueId ec_id)
00296 {
00297   return RTC::RTC_OK;
00298 }
00299 */
00300 
00301 /*
00302 RTC::ReturnCode_t ThermoLimiter::onStateUpdate(RTC::UniqueId ec_id)
00303 {
00304   return RTC::RTC_OK;
00305 }
00306 */
00307 
00308 /*
00309 RTC::ReturnCode_t ThermoLimiter::onRateChanged(RTC::UniqueId ec_id)
00310 {
00311   return RTC::RTC_OK;
00312 }
00313 */
00314 
00315 void ThermoLimiter::calcMaxTorqueFromTemperature(hrp::dvector &tauMax)
00316 {
00317   unsigned int numJoints = m_robot->numJoints();
00318   double temp, tempLimit;
00319   hrp::dvector squareTauMax(numJoints);
00320   
00321   if (m_tempIn.data.length() ==  m_robot->numJoints()) {
00322 
00323     for (unsigned int i = 0; i < numJoints; i++) {
00324       temp = m_tempIn.data[i];
00325       tempLimit = m_motorTemperatureLimit[i];
00326 
00327       // limit temperature
00328       double term = 120;
00329       squareTauMax[i] = (((tempLimit - temp) / term) + m_motorHeatParams[i].thermoCoeffs * (temp - m_motorHeatParams[i].temperature)) / m_motorHeatParams[i].currentCoeffs;
00330 
00331       // determine tauMax
00332       if (squareTauMax[i] < 0) {
00333           if (isDebug()) {
00334               std::cerr << "[WARN] tauMax ** 2 = " << squareTauMax[i] << " < 0 in Joint " << i << std::endl;
00335           }
00336         tauMax[i] = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst; // default tauMax from model file
00337       } else {
00338         tauMax[i] = std::sqrt(squareTauMax[i]); // tauMax is absolute value
00339       }
00340     }
00341   }
00342   return;
00343 }
00344 
00345 double ThermoLimiter::calcEmergencyRatio(RTC::TimedDoubleSeq &current, hrp::dvector &max, double alarmRatio, std::string &prefix)
00346 {
00347   double maxEmergencyRatio = 0.0;
00348   if (current.data.length() == max.size()) { // estimate same dimension
00349     for (unsigned int i = 0; i < current.data.length(); i++) {
00350       double tmpEmergencyRatio = std::abs(current.data[i] / max[i]);
00351       if (tmpEmergencyRatio > alarmRatio && m_loop % m_debug_print_freq == 0) {
00352           std::cerr << prefix << "[" << m_robot->joint(i)->name << "]" << " is over " << alarmRatio << " of the limit (" << current.data[i] << "/" << max[i] << ")" << std::endl;
00353       }
00354       if (maxEmergencyRatio < tmpEmergencyRatio) {
00355         maxEmergencyRatio = tmpEmergencyRatio;
00356       }
00357     }
00358   }
00359   return maxEmergencyRatio;
00360 }
00361 
00362 void ThermoLimiter::callBeep(double ratio, double alarmRatio)
00363 {
00364   const int maxFreq = 3136; // G
00365   const int minFreq = 2794; // F
00366 
00367   if (ratio > 1.0) { // emergency (current load is over max load)
00368     const int emergency_beep_cycle = 200;
00369     int current_emergency_beep_cycle = m_loop % emergency_beep_cycle;
00370     if (current_emergency_beep_cycle <= (emergency_beep_cycle / 2)) {
00371       bc.startBeep(4000, 60);
00372     } else {
00373       bc.startBeep(2000, 60);
00374     }
00375   } else if (ratio > alarmRatio) { // normal warning
00376     int freq = minFreq + (maxFreq - minFreq) * ((ratio - alarmRatio) / (1.0 - alarmRatio));
00377     bc.startBeep(freq, 500);
00378   } else {
00379     bc.stopBeep();
00380   }
00381   bc.setDataPort(m_beepCommandOut);
00382   return;
00383 }
00384 
00385 bool ThermoLimiter::isDebug(int cycle)
00386 {
00387   return ((m_debugLevel==1 && m_loop%cycle==0) || m_debugLevel > 1);
00388 }
00389 
00390 bool ThermoLimiter::setParameter(const OpenHRP::ThermoLimiterService::tlParam& i_tlp)
00391 {
00392   Guard guard(m_mutex);
00393   std::cerr << "[" << m_profile.instance_name << "] setThermoLimiterParam" << std::endl;
00394   m_debug_print_freq = i_tlp.debug_print_freq;
00395   m_alarmRatio = i_tlp.alarmRatio;
00396   std::cerr << "[" << m_profile.instance_name << "] m_debug_print_freq = " << m_debug_print_freq << std::endl;
00397   std::cerr << "[" << m_profile.instance_name << "] m_alarmRatio = " << m_alarmRatio << std::endl;
00398   return true;
00399 }
00400 
00401 bool ThermoLimiter::getParameter(OpenHRP::ThermoLimiterService::tlParam& i_tlp)
00402 {
00403   i_tlp.debug_print_freq = m_debug_print_freq;
00404   i_tlp.alarmRatio = m_alarmRatio;
00405   return true;
00406 }
00407 
00408 extern "C"
00409 {
00410 
00411   void ThermoLimiterInit(RTC::Manager* manager)
00412   {
00413     RTC::Properties profile(thermolimiter_spec);
00414     manager->registerFactory(profile,
00415                              RTC::Create<ThermoLimiter>,
00416                              RTC::Delete<ThermoLimiter>);
00417   }
00418 
00419 };
00420 
00421 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19