ThermoLimiter.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "ThermoLimiter.h"
11 #include <rtm/CorbaNaming.h>
12 #include <hrpModel/ModelLoaderUtil.h>
13 #include <hrpUtil/MatrixSolvers.h>
14 #include <cmath>
15 
16 #define DQ_MAX 1.0
17 
19 
20 // Module specification
21 // <rtc-template block="module_spec">
22 static const char* thermolimiter_spec[] =
23  {
24  "implementation_id", "ThermoLimiter",
25  "type_name", "ThermoLimiter",
26  "description", "thermo limiter",
27  "version", HRPSYS_PACKAGE_VERSION,
28  "vendor", "AIST",
29  "category", "example",
30  "activity_type", "DataFlowComponent",
31  "max_instance", "10",
32  "language", "C++",
33  "lang_type", "compile",
34  // Configuration variables
35  "conf.default.debugLevel", "0",
36  ""
37  };
38 // </rtc-template>
39 
41  : RTC::DataFlowComponentBase(manager),
42  // <rtc-template block="initializer">
43  m_tempInIn("tempIn", m_tempIn),
44  m_tauMaxOutOut("tauMax", m_tauMaxOut),
45  m_beepCommandOutOut("beepCommand", m_beepCommandOut),
46  m_ThermoLimiterServicePort("ThermoLimiterService"),
47  m_debugLevel(0)
48 {
50 }
51 
53 {
54 }
55 
56 RTC::ReturnCode_t ThermoLimiter::onInitialize()
57 {
58  std::cerr << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl;
59  // <rtc-template block="bind_config">
60  // Bind variables and configuration variable
61  bindParameter("debugLevel", m_debugLevel, "0");
62 
63  // </rtc-template>
64 
65  // Registration: InPort/OutPort/Service
66  // <rtc-template block="registration">
67  // Set InPort buffers
68  addInPort("tempIn", m_tempInIn);
69 
70  // Set OutPort buffer
71  addOutPort("tauMax", m_tauMaxOutOut);
72  addOutPort("beepCommand", m_beepCommandOutOut);
73 
74  // Set service provider to Ports
75  m_ThermoLimiterServicePort.registerProvider("service0", "ThermoLimiterService", m_ThermoLimiterService);
76 
77  // Set service consumers to Ports
78 
79  // Set CORBA Service Ports
81 
82  // </rtc-template>
83 
85  coil::stringTo(m_dt, prop["dt"].c_str());
86 
88 
89  RTC::Manager& rtcManager = RTC::Manager::instance();
90  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
91  int comPos = nameServer.find(",");
92  if (comPos < 0){
93  comPos = nameServer.length();
94  }
95  nameServer = nameServer.substr(0, comPos);
96  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
97  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
98  CosNaming::NamingContext::_duplicate(naming.getRootContext())
99  )){
100  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
101  << std::endl;
102  }
103  // set limit of motor temperature
104  coil::vstring motorTemperatureLimitFromConf = coil::split(prop["motor_temperature_limit"], ",");
105  m_motorTemperatureLimit.resize(m_robot->numJoints());
106  if (motorTemperatureLimitFromConf.size() != m_robot->numJoints()) {
107  std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motor_temperature_limit is " << motorTemperatureLimitFromConf.size() << ", not equal to " << m_robot->numJoints() << std::endl;
108  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
109  m_motorTemperatureLimit[i] = 80.0;
110  }
111  } else {
112  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
113  coil::stringTo(m_motorTemperatureLimit[i], motorTemperatureLimitFromConf[i].c_str());
114  }
115  }
116  if (m_debugLevel > 0) {
117  std::cerr << "motor_temperature_limit: ";
118  for(int i = 0; i < m_motorTemperatureLimit.size(); i++) {
119  std::cerr << m_motorTemperatureLimit[i] << " ";
120  }
121  std::cerr << std::endl;
122  }
123 
124  // set temperature of environment
125  double ambientTemp = 25.0;
126  if (prop["ambient_tmp"] != "") {
127  coil::stringTo(ambientTemp, prop["ambient_tmp"].c_str());
128  }
129  std::cerr << "[" << m_profile.instance_name << "] : ambient temperature: " << ambientTemp << std::endl;
130 
131  // set limit of motor heat parameters
132  coil::vstring motorHeatParamsFromConf = coil::split(prop["motor_heat_params"], ",");
133  m_motorHeatParams.resize(m_robot->numJoints());
134  if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) {
135  std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motor_heat_param is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl;
136  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
137  m_motorHeatParams[i].defaultParams();
138  m_motorHeatParams[i].temperature = ambientTemp;
139  }
140 
141  } else {
142  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
143  m_motorHeatParams[i].temperature = ambientTemp;
144  coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str());
145  coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str());
146  }
147  }
148 
149  if (m_debugLevel > 0) {
150  std::cerr << "motor_heat_param: ";
151  for(std::vector<MotorHeatParam>::iterator it = m_motorHeatParams.begin(); it != m_motorHeatParams.end(); ++it){
152  std::cerr << (*it).temperature << "," << (*it).currentCoeffs << "," << (*it).thermoCoeffs << ", ";
153  }
154  std::cerr << std::endl;
155  std::cerr << "default torque limit from model:" << std::endl;
156  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
157  std::cerr << m_robot->joint(i)->name << ":" << m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst << std::endl;
158  }
159  }
160 
161  // set alarm ratio threshold
162  m_alarmRatio = 0.5;
163  if (prop["alarm_ratio"] != "") {
164  coil::stringTo(m_alarmRatio, prop["alarm_ratio"].c_str());
165  }
166  if (m_debugLevel > 0) {
167  std::cerr << "alarmRatio: " << m_alarmRatio << std::endl;
168  }
169 
170  // allocate memory for outPorts
171  m_tauMaxOut.data.length(m_robot->numJoints());
172  m_debug_print_freq = static_cast<int>(0.1/m_dt); // once per 0.1 [s]
173  m_beepCommandOut.data.length(bc.get_num_beep_info());
174 
175  return RTC::RTC_OK;
176 }
177 
178 /*
179 RTC::ReturnCode_t ThermoLimiter::onFinalize()
180 {
181  return RTC::RTC_OK;
182 }
183 */
184 
185 /*
186 RTC::ReturnCode_t ThermoLimiter::onStartup(RTC::UniqueId ec_id)
187 {
188  return RTC::RTC_OK;
189 }
190 */
191 
192 /*
193 RTC::ReturnCode_t ThermoLimiter::onShutdown(RTC::UniqueId ec_id)
194 {
195  return RTC::RTC_OK;
196 }
197 */
198 
200 {
201  std::cerr << "[" << m_profile.instance_name << "] : onActivated(" << ec_id << ")" << std::endl;
202  return RTC::RTC_OK;
203 }
204 
206 {
207  std::cerr << "[" << m_profile.instance_name << "] : onDeactivated(" << ec_id << ")" << std::endl;
208  return RTC::RTC_OK;
209 }
210 
211 RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id)
212 {
213  // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << "), data = " << m_data.data << std::endl;
214  m_loop++;
215 
216  if (isDebug()) {
217  std::cerr << "[" << m_profile.instance_name << "]" << std::endl;
218  }
219 
221  RTC::Time tm;
222  tm.sec = coiltm.sec();
223  tm.nsec = coiltm.usec()*1000;
224  hrp::dvector tauMax;
225  tauMax.resize(m_robot->numJoints());
226 
227  double thermoLimitRatio = 0.0;
228  std::string thermoLimitPrefix = "ThermoLimit";
229 
230  // update port
231  if (m_tempInIn.isNew()) {
232  m_tempInIn.read();
233  }
234 
235  Guard guard(m_mutex);
236  if (isDebug()) {
237  std::cerr << "temperature: ";
238  for (unsigned int i = 0; i < m_tempIn.data.length(); i++) {
239  std::cerr << " " << m_tempIn.data[i];
240  }
241  std::cerr << std::endl;
242  }
243 
244  // calculate tauMax from temperature
245  if (m_tempIn.data.length() == m_robot->numJoints()) {
247  } else {
248  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
249  tauMax[i] = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst; // default torque limit from model
250  }
251  }
252 
253  if (isDebug()) {
254  std::cerr << "tauMax: ";
255  for (int i = 0; i < tauMax.size(); i++) {
256  std::cerr << tauMax[i] << " ";
257  }
258  std::cerr << std::endl;
259  }
260 
261  // emergency notification
262  if (m_tempIn.data.length() == m_robot->numJoints()) {
263  thermoLimitRatio = calcEmergencyRatio(m_tempIn, m_motorTemperatureLimit, m_alarmRatio, thermoLimitPrefix);
264  }
265 
266  // call beep (3136/0.8=3920)
267  callBeep(thermoLimitRatio, m_alarmRatio);
268 
269  // output restricted tauMax
270  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
271  m_tauMaxOut.data[i] = tauMax[i];
272  }
273  m_tauMaxOut.tm = tm;
275  m_beepCommandOut.tm = tm;
277  return RTC::RTC_OK;
278 }
279 
280 /*
281 RTC::ReturnCode_t ThermoLimiter::onAborting(RTC::UniqueId ec_id)
282 {
283  return RTC::RTC_OK;
284 }
285 */
286 
287 /*
288 RTC::ReturnCode_t ThermoLimiter::onError(RTC::UniqueId ec_id)
289 {
290  return RTC::RTC_OK;
291 }
292 */
293 
294 /*
295 RTC::ReturnCode_t ThermoLimiter::onReset(RTC::UniqueId ec_id)
296 {
297  return RTC::RTC_OK;
298 }
299 */
300 
301 /*
302 RTC::ReturnCode_t ThermoLimiter::onStateUpdate(RTC::UniqueId ec_id)
303 {
304  return RTC::RTC_OK;
305 }
306 */
307 
308 /*
309 RTC::ReturnCode_t ThermoLimiter::onRateChanged(RTC::UniqueId ec_id)
310 {
311  return RTC::RTC_OK;
312 }
313 */
314 
316 {
317  unsigned int numJoints = m_robot->numJoints();
318  double temp, tempLimit;
319  hrp::dvector squareTauMax(numJoints);
320 
321  if (m_tempIn.data.length() == m_robot->numJoints()) {
322 
323  for (unsigned int i = 0; i < numJoints; i++) {
324  temp = m_tempIn.data[i];
325  tempLimit = m_motorTemperatureLimit[i];
326 
327  // limit temperature
328  double term = 120;
329  squareTauMax[i] = (((tempLimit - temp) / term) + m_motorHeatParams[i].thermoCoeffs * (temp - m_motorHeatParams[i].temperature)) / m_motorHeatParams[i].currentCoeffs;
330 
331  // determine tauMax
332  if (squareTauMax[i] < 0) {
333  if (isDebug()) {
334  std::cerr << "[WARN] tauMax ** 2 = " << squareTauMax[i] << " < 0 in Joint " << i << std::endl;
335  }
336  tauMax[i] = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst; // default tauMax from model file
337  } else {
338  tauMax[i] = std::sqrt(squareTauMax[i]); // tauMax is absolute value
339  }
340  }
341  }
342  return;
343 }
344 
345 double ThermoLimiter::calcEmergencyRatio(RTC::TimedDoubleSeq &current, hrp::dvector &max, double alarmRatio, std::string &prefix)
346 {
347  double maxEmergencyRatio = 0.0;
348  if (current.data.length() == max.size()) { // estimate same dimension
349  for (unsigned int i = 0; i < current.data.length(); i++) {
350  double tmpEmergencyRatio = std::abs(current.data[i] / max[i]);
351  if (tmpEmergencyRatio > alarmRatio && m_loop % m_debug_print_freq == 0) {
352  std::cerr << prefix << "[" << m_robot->joint(i)->name << "]" << " is over " << alarmRatio << " of the limit (" << current.data[i] << "/" << max[i] << ")" << std::endl;
353  }
354  if (maxEmergencyRatio < tmpEmergencyRatio) {
355  maxEmergencyRatio = tmpEmergencyRatio;
356  }
357  }
358  }
359  return maxEmergencyRatio;
360 }
361 
362 void ThermoLimiter::callBeep(double ratio, double alarmRatio)
363 {
364  const int maxFreq = 3136; // G
365  const int minFreq = 2794; // F
366 
367  if (ratio > 1.0) { // emergency (current load is over max load)
368  const int emergency_beep_cycle = 200;
369  int current_emergency_beep_cycle = m_loop % emergency_beep_cycle;
370  if (current_emergency_beep_cycle <= (emergency_beep_cycle / 2)) {
371  bc.startBeep(4000, 60);
372  } else {
373  bc.startBeep(2000, 60);
374  }
375  } else if (ratio > alarmRatio) { // normal warning
376  int freq = minFreq + (maxFreq - minFreq) * ((ratio - alarmRatio) / (1.0 - alarmRatio));
377  bc.startBeep(freq, 500);
378  } else {
379  bc.stopBeep();
380  }
382  return;
383 }
384 
385 bool ThermoLimiter::isDebug(int cycle)
386 {
387  return ((m_debugLevel==1 && m_loop%cycle==0) || m_debugLevel > 1);
388 }
389 
390 bool ThermoLimiter::setParameter(const OpenHRP::ThermoLimiterService::tlParam& i_tlp)
391 {
392  Guard guard(m_mutex);
393  std::cerr << "[" << m_profile.instance_name << "] setThermoLimiterParam" << std::endl;
394  m_debug_print_freq = i_tlp.debug_print_freq;
395  m_alarmRatio = i_tlp.alarmRatio;
396  std::cerr << "[" << m_profile.instance_name << "] m_debug_print_freq = " << m_debug_print_freq << std::endl;
397  std::cerr << "[" << m_profile.instance_name << "] m_alarmRatio = " << m_alarmRatio << std::endl;
398  return true;
399 }
400 
401 bool ThermoLimiter::getParameter(OpenHRP::ThermoLimiterService::tlParam& i_tlp)
402 {
403  i_tlp.debug_print_freq = m_debug_print_freq;
404  i_tlp.alarmRatio = m_alarmRatio;
405  return true;
406 }
407 
408 extern "C"
409 {
410 
412  {
414  manager->registerFactory(profile,
415  RTC::Create<ThermoLimiter>,
416  RTC::Delete<ThermoLimiter>);
417  }
418 
419 };
420 
421 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
bool isDebug(int cycle=200)
OutPort< TimedDoubleSeq > m_tauMaxOutOut
#define max(a, b)
coil::Guard< coil::Mutex > Guard
bool getParameter(OpenHRP::ThermoLimiterService::tlParam &i_tlp)
virtual RTC::ReturnCode_t onInitialize()
coil::Mutex m_mutex
long int sec() const
unsigned int m_debug_print_freq
bool stringTo(To &val, const char *str)
TimedDoubleSeq m_tempIn
null component
virtual ~ThermoLimiter()
Destructor.
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
ThermoLimiter(RTC::Manager *manager)
Constructor.
TimedDoubleSeq m_tauMaxOut
bool isWritable() const
Definition: beep.h:36
CORBA::ORB_ptr getORB()
png_uint_32 i
Eigen::VectorXd dvector
coil::Properties & getProperties()
static Manager & instance()
bool addOutPort(const char *name, OutPortBase &outport)
void setDataPort(RTC::TimedLongSeq &out_data)
Definition: beep.h:43
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
boost::shared_ptr< Body > BodyPtr
std::vector< std::string > vstring
int gettimeofday(struct timeval *tv, struct timezone *tz)
coil::Properties & getConfig()
unsigned int m_debugLevel
OutPort< TimedLongSeq > m_beepCommandOutOut
void startBeep(const int _freq, const int _length=50)
Definition: beep.h:22
hrp::dvector m_motorTemperatureLimit
ExecutionContextHandle_t UniqueId
std::vector< MotorHeatParam > m_motorHeatParams
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
JHUFF_TBL long freq[]
static const char * thermolimiter_spec[]
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
TimedLongSeq m_beepCommandOut
void thermolimiter(ThermoLimiter *i_thermolimiter)
prop
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
naming
void stopBeep()
Definition: beep.h:29
virtual bool isNew()
void callBeep(double ratio, double alarmRatio)
bool addPort(PortBase &port)
virtual bool write(DataType &value)
long long m_loop
BeepClient bc
void ThermoLimiterInit(RTC::Manager *manager)
InPort< TimedDoubleSeq > m_tempInIn
int get_num_beep_info() const
Definition: beep.h:21
long int usec() const
ThermoLimiterService_impl m_ThermoLimiterService
hrp::BodyPtr m_robot
bool addInPort(const char *name, InPortBase &inport)
bool setParameter(const OpenHRP::ThermoLimiterService::tlParam &i_tlp)
RTC::CorbaPort m_ThermoLimiterServicePort
double m_alarmRatio
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void calcMaxTorqueFromTemperature(hrp::dvector &tauMax)
double calcEmergencyRatio(RTC::TimedDoubleSeq &current, hrp::dvector &max, double alarmRatio, std::string &prefix)
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51