ThermoEstimator.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00015 #include "ThermoEstimator.h"
00016 #include "hrpsys/idl/RobotHardwareService.hh"
00017 #include <rtm/CorbaNaming.h>
00018 #include <hrpModel/ModelLoaderUtil.h>
00019 #include <hrpUtil/MatrixSolvers.h>
00020 
00021 // Module specification
00022 // <rtc-template block="module_spec">
00023 static const char* thermoestimator_spec[] =
00024 {
00025   "implementation_id", "ThermoEstimator",
00026   "type_name",         "ThermoEstimator",
00027   "description",       "null component",
00028   "version",           HRPSYS_PACKAGE_VERSION,
00029   "vendor",            "AIST",
00030   "category",          "example",
00031   "activity_type",     "DataFlowComponent",
00032   "max_instance",      "10",
00033   "language",          "C++",
00034   "lang_type",         "compile",
00035   // Configuration variables
00036   "conf.default.debugLevel", "0",
00037   ""
00038 };
00039 
00040 // </rtc-template>
00041 
00042 ThermoEstimator::ThermoEstimator(RTC::Manager* manager)
00043   : RTC::DataFlowComponentBase(manager),
00044     // <rtc-template block="initializer">
00045     m_tauInIn("tauIn", m_tauIn),
00046     m_qRefInIn("qRefIn", m_qRefIn),
00047     m_qCurrentInIn("qCurrentIn", m_qCurrentIn),
00048     m_servoStateInIn("servoStateIn", m_servoStateIn),
00049     m_tempOutOut("tempOut", m_tempOut),
00050     m_servoStateOutOut("servoStateOut", m_servoStateOut),    
00051     // </rtc-template>
00052     m_debugLevel(0)
00053 {
00054 }
00055 
00056 ThermoEstimator::~ThermoEstimator()
00057 {
00058 }
00059 
00060 RTC::ReturnCode_t ThermoEstimator::onInitialize()
00061 {
00062   std::cerr << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl;
00063   // <rtc-template block="bind_config">
00064   // Bind variables and configuration variable
00065   bindParameter("debugLevel", m_debugLevel, "0");
00066   
00067   // </rtc-template>
00068 
00069   // Registration: InPort/OutPort/Service
00070   // <rtc-template block="registration">
00071   // Set InPort buffers
00072   addInPort("tauIn", m_tauInIn);
00073   addInPort("qRefIn", m_qRefInIn);
00074   addInPort("qCurrentIn", m_qCurrentInIn);
00075   addInPort("servoStateIn", m_servoStateInIn);
00076 
00077   // Set OutPort buffer
00078   addOutPort("tempOut", m_tempOutOut);
00079   addOutPort("servoStateOut", m_servoStateOutOut);
00080   
00081   // Set service provider to Ports
00082   
00083   // Set service consumers to Ports
00084   
00085   // Set CORBA Service Ports
00086   
00087   // </rtc-template>
00088 
00089   // set parmeters
00090   RTC::Properties& prop = getProperties();
00091   coil::stringTo(m_dt, prop["dt"].c_str());
00092 
00093   m_robot = hrp::BodyPtr(new hrp::Body());
00094 
00095   RTC::Manager& rtcManager = RTC::Manager::instance();
00096   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00097   int comPos = nameServer.find(",");
00098   if (comPos < 0){
00099     comPos = nameServer.length();
00100   }
00101   nameServer = nameServer.substr(0, comPos);
00102   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00103   if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
00104                                CosNaming::NamingContext::_duplicate(naming.getRootContext())
00105         )){
00106     std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
00107               << std::endl;
00108   }
00109 
00110   // init outport
00111   m_tempOut.data.length(m_robot->numJoints());
00112   m_servoStateIn.data.length(m_robot->numJoints());
00113   m_servoStateOut.data.length(m_robot->numJoints());
00114   
00115   // set temperature of environment
00116   if (prop["ambient_tmp"] != "") {
00117     coil::stringTo(m_ambientTemp, prop["ambient_tmp"].c_str());
00118   } else {
00119     m_ambientTemp = 25.0;
00120   }
00121   std::cerr << "[" << m_profile.instance_name << "] : ambient temperature: " << m_ambientTemp << std::endl;
00122   
00123   // set motor heat parameters
00124   m_motorHeatParams.resize(m_robot->numJoints());
00125   coil::vstring motorHeatParamsFromConf = coil::split(prop["motor_heat_params"], ",");
00126   if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) {
00127     std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motorHeatParams is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl;
00128     // motorHeatParam has default values itself
00129   } else {
00130     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00131       m_motorHeatParams[i].temperature = m_ambientTemp;
00132       coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str());
00133       coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str());
00134     }
00135     if (m_debugLevel > 0) {
00136       std::cerr <<  "motorHeatParams is " << std::endl;
00137       for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00138         std::cerr << m_motorHeatParams[i].currentCoeffs << " " << m_motorHeatParams[i].thermoCoeffs << std::endl;
00139       }
00140     }
00141   }
00142 
00143   // set constant for joint error to torque conversion
00144   coil::vstring error2tauFromConf = coil::split(prop["error_to_tau_constant"], ",");
00145   if (error2tauFromConf.size() != m_robot->numJoints()) {
00146     std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of error2tau is " << error2tauFromConf.size() << ", not equal to " << m_robot->numJoints() << std::endl;
00147     m_error2tau.resize(0); // invalid 
00148   } else {
00149     m_error2tau.resize(m_robot->numJoints());
00150     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00151       coil::stringTo(m_error2tau[i], error2tauFromConf[i].c_str());
00152     }
00153     if (m_debugLevel > 0) {
00154       std::cerr <<  "motorHeatParams:" << std::endl;
00155       for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00156         std::cerr << " " << m_error2tau[i];
00157       }
00158       std::cerr << std::endl;      
00159     }
00160   }
00161   
00162   return RTC::RTC_OK;
00163 }
00164 
00165 
00166 
00167 /*
00168   RTC::ReturnCode_t ThermoEstimator::onFinalize()
00169   {
00170   return RTC::RTC_OK;
00171   }
00172 */
00173 
00174 /*
00175   RTC::ReturnCode_t ThermoEstimator::onStartup(RTC::UniqueId ec_id)
00176   {
00177   return RTC::RTC_OK;
00178   }
00179 */
00180 
00181 /*
00182   RTC::ReturnCode_t ThermoEstimator::onShutdown(RTC::UniqueId ec_id)
00183   {
00184   return RTC::RTC_OK;
00185   }
00186 */
00187 
00188 RTC::ReturnCode_t ThermoEstimator::onActivated(RTC::UniqueId ec_id)
00189 {
00190   std::cerr << "[" << m_profile.instance_name << "] : onActivated(" << ec_id << ")" << std::endl;
00191   return RTC::RTC_OK;
00192 }
00193 
00194 RTC::ReturnCode_t ThermoEstimator::onDeactivated(RTC::UniqueId ec_id)
00195 {
00196   std::cerr << "[" << m_profile.instance_name << "] : onDeactivated(" << ec_id << ")" << std::endl;
00197   return RTC::RTC_OK;
00198 }
00199 
00200 RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id)
00201 {
00202   // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00203   unsigned int numJoints = m_robot->numJoints();
00204   m_loop++;
00205   
00206   coil::TimeValue coiltm(coil::gettimeofday());
00207   RTC::Time tm;
00208   tm.sec = coiltm.sec();
00209   tm.nsec = coiltm.usec()*1000;
00210 
00211   if (m_tauInIn.isNew()) {
00212     m_tauInIn.read();
00213   }
00214   if (m_qRefInIn.isNew()) {
00215     m_qRefInIn.read();
00216   }
00217   if (m_qCurrentInIn.isNew()) {
00218     m_qCurrentInIn.read();
00219   }
00220   if (m_servoStateInIn.isNew()) {
00221     m_servoStateInIn.read();
00222   }
00223 
00224   // calculate joint torque
00225   hrp::dvector jointTorque;
00226   if (m_tauIn.data.length() == numJoints) { // use raw torque
00227     jointTorque.resize(numJoints);
00228     for (unsigned int i = 0; i < numJoints; i++) {
00229       jointTorque[i] = m_tauIn.data[i];
00230     }
00231     if (isDebug()) {
00232       std::cerr << "raw torque: ";
00233       for (unsigned int i = 0; i < numJoints; i++) {
00234         std::cerr << " " << m_tauIn.data[i] ;
00235       }
00236       std::cerr << std::endl;
00237     }
00238   } else if (m_qRefIn.data.length() == numJoints
00239              && m_qCurrentIn.data.length() == numJoints) { // estimate torque from joint error
00240     jointTorque.resize(numJoints);
00241     hrp::dvector jointError(numJoints);
00242     for (unsigned int i = 0; i < numJoints; i++) {
00243       jointError[i] = m_qRefIn.data[i] - m_qCurrentIn.data[i];
00244     }
00245     estimateJointTorqueFromJointError(jointError, jointTorque);
00246     if (isDebug()) {
00247       std::cerr << "qRef: ";
00248       for (unsigned int i = 0; i < numJoints; i++) {
00249         std::cerr << " " << m_qRefIn.data[i] ;
00250       }
00251       std::cerr << std::endl;
00252       std::cerr << "qCurrent: ";
00253       for (unsigned int i = 0; i < numJoints; i++) {
00254         std::cerr << " " << m_qCurrentIn.data[i] ;
00255       }
00256       std::cerr << std::endl;
00257     }
00258   } else { // invalid 
00259     jointTorque.resize(0);
00260   }
00261 
00262   // calculate temperature from joint torque
00263   if (jointTorque.size() ==  m_robot->numJoints()) {
00264     for (unsigned int i = 0; i < numJoints; i++) {
00265       // Thermo estimation
00266       calculateJointTemperature(jointTorque[i], m_motorHeatParams[i]);
00267       // output
00268       m_tempOut.data[i] = m_motorHeatParams[i].temperature;
00269     }
00270     if (isDebug()) {
00271       std::cerr << std::endl << "temperature  : ";
00272       for (unsigned int i = 0; i < numJoints; i++) {
00273         std::cerr << " " << m_motorHeatParams[i].temperature;
00274       }
00275       std::cerr << std::endl;
00276     }
00277     m_tempOutOut.write();
00278   }
00279 
00280   // overwrite temperature in servoState if temperature is calculated correctly
00281   if (jointTorque.size() == m_robot->numJoints()
00282       && m_servoStateIn.data.length() ==  m_robot->numJoints()) {
00283     for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++) {
00284       size_t len = m_servoStateIn.data[i].length();
00285       m_servoStateOut.data[i].length(len + 1); // expand extra_data for temperature
00286       for (unsigned int j = 0; j < len; j++) {
00287         m_servoStateOut.data[i][j] = m_servoStateIn.data[i][j];
00288       }
00289       // servoStateOut is int, but extra data will be casted to float in HrpsysSeqStateROSBridge
00290       float tmp_temperature = static_cast<float>(m_motorHeatParams[i].temperature);
00291       std::memcpy(&(m_servoStateOut.data[i][len]), &tmp_temperature, sizeof(float));
00292     }
00293   } else { // pass servoStateIn to servoStateOut
00294     m_servoStateOut.data.length(m_servoStateIn.data.length());
00295     for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++) {
00296       m_servoStateOut.data[i] = m_servoStateIn.data[i];
00297     }
00298   }
00299   m_servoStateOutOut.write();
00300   
00301   return RTC::RTC_OK;
00302 }
00303 
00304 /*
00305   RTC::ReturnCode_t ThermoEstimator::onAborting(RTC::UniqueId ec_id)
00306   {
00307   return RTC::RTC_OK;
00308   }
00309 */
00310 
00311 /*
00312   RTC::ReturnCode_t ThermoEstimator::onError(RTC::UniqueId ec_id)
00313   {
00314   return RTC::RTC_OK;
00315   }
00316 */
00317 
00318 /*
00319   RTC::ReturnCode_t ThermoEstimator::onReset(RTC::UniqueId ec_id)
00320   {
00321   return RTC::RTC_OK;
00322   }
00323 */
00324 
00325 /*
00326   RTC::ReturnCode_t ThermoEstimator::onStateUpdate(RTC::UniqueId ec_id)
00327   {
00328   return RTC::RTC_OK;
00329   }
00330 */
00331 
00332 /*
00333   RTC::ReturnCode_t ThermoEstimator::onRateChanged(RTC::UniqueId ec_id)
00334   {
00335   return RTC::RTC_OK;
00336   }
00337 */
00338 
00339 void ThermoEstimator::estimateJointTorqueFromJointError(hrp::dvector &error, hrp::dvector &tau)
00340 {
00341   if (error.size() == m_robot->numJoints()
00342       && m_error2tau.size() == m_robot->numJoints()) {
00343     tau.resize(m_robot->numJoints());
00344     for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00345       tau[i] = m_error2tau[i] * error[i];
00346     }
00347     if (isDebug()) {
00348       std::cerr << "estimated torque: ";
00349       for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
00350         std::cerr << " " << tau[i] ;
00351       }
00352       std::cerr << std::endl;
00353     }
00354   } else {
00355     tau.resize(0); // don't calculate tau when invalid input
00356     if (isDebug()) {
00357       std::cerr << "Invalid size of values:" << std::endl;
00358       std::cerr << "num joints: " << m_robot->numJoints() << std::endl;
00359       std::cerr << "joint error: " << error.size() << std::endl;
00360       std::cerr << "error2tau: " << m_error2tau.size() << std::endl;
00361     }
00362   }
00363   
00364   return;
00365 }
00366 
00367 void ThermoEstimator::calculateJointTemperature(double tau, MotorHeatParam& param)
00368 {
00369   // from Design of High Torque and High Speed Leg Module for High Power Humanoid (Junichi Urata et al.)
00370   // Tnew = T + (((Re*K^2/C) * tau^2) - ((1/RC) * (T - Ta))) * dt
00371   double currentHeat, radiation;
00372   currentHeat = param.currentCoeffs * std::pow(tau, 2);
00373   radiation = -param.thermoCoeffs * (param.temperature - m_ambientTemp);
00374   param.temperature = param.temperature + (currentHeat + radiation) * m_dt;
00375   return;
00376 }
00377 
00378 bool ThermoEstimator::isDebug(int cycle)
00379 {
00380   return ((m_debugLevel==1 && m_loop%cycle==0) || m_debugLevel > 1);
00381 }
00382 
00383 extern "C"
00384 {
00385 
00386   void ThermoEstimatorInit(RTC::Manager* manager)
00387   {
00388     RTC::Properties profile(thermoestimator_spec);
00389     manager->registerFactory(profile,
00390                              RTC::Create<ThermoEstimator>,
00391                              RTC::Delete<ThermoEstimator>);
00392   }
00393 
00394 };
00395 
00396 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56