ThermoEstimator.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
15 #include "ThermoEstimator.h"
16 #include "hrpsys/idl/RobotHardwareService.hh"
17 #include <rtm/CorbaNaming.h>
18 #include <hrpModel/ModelLoaderUtil.h>
19 #include <hrpUtil/MatrixSolvers.h>
20 
21 // Module specification
22 // <rtc-template block="module_spec">
23 static const char* thermoestimator_spec[] =
24 {
25  "implementation_id", "ThermoEstimator",
26  "type_name", "ThermoEstimator",
27  "description", "null component",
28  "version", HRPSYS_PACKAGE_VERSION,
29  "vendor", "AIST",
30  "category", "example",
31  "activity_type", "DataFlowComponent",
32  "max_instance", "10",
33  "language", "C++",
34  "lang_type", "compile",
35  // Configuration variables
36  "conf.default.debugLevel", "0",
37  ""
38 };
39 
40 // </rtc-template>
41 
43  : RTC::DataFlowComponentBase(manager),
44  // <rtc-template block="initializer">
45  m_tauInIn("tauIn", m_tauIn),
46  m_qRefInIn("qRefIn", m_qRefIn),
47  m_qCurrentInIn("qCurrentIn", m_qCurrentIn),
48  m_servoStateInIn("servoStateIn", m_servoStateIn),
49  m_tempOutOut("tempOut", m_tempOut),
50  m_servoStateOutOut("servoStateOut", m_servoStateOut),
51  // </rtc-template>
52  m_debugLevel(0)
53 {
54 }
55 
57 {
58 }
59 
60 RTC::ReturnCode_t ThermoEstimator::onInitialize()
61 {
62  std::cerr << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl;
63  // <rtc-template block="bind_config">
64  // Bind variables and configuration variable
65  bindParameter("debugLevel", m_debugLevel, "0");
66 
67  // </rtc-template>
68 
69  // Registration: InPort/OutPort/Service
70  // <rtc-template block="registration">
71  // Set InPort buffers
72  addInPort("tauIn", m_tauInIn);
73  addInPort("qRefIn", m_qRefInIn);
74  addInPort("qCurrentIn", m_qCurrentInIn);
75  addInPort("servoStateIn", m_servoStateInIn);
76 
77  // Set OutPort buffer
78  addOutPort("tempOut", m_tempOutOut);
79  addOutPort("servoStateOut", m_servoStateOutOut);
80 
81  // Set service provider to Ports
82 
83  // Set service consumers to Ports
84 
85  // Set CORBA Service Ports
86 
87  // </rtc-template>
88 
89  // set parmeters
91  coil::stringTo(m_dt, prop["dt"].c_str());
92 
94 
95  RTC::Manager& rtcManager = RTC::Manager::instance();
96  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
97  int comPos = nameServer.find(",");
98  if (comPos < 0){
99  comPos = nameServer.length();
100  }
101  nameServer = nameServer.substr(0, comPos);
102  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
103  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
104  CosNaming::NamingContext::_duplicate(naming.getRootContext())
105  )){
106  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
107  << std::endl;
108  }
109 
110  // init outport
111  m_tempOut.data.length(m_robot->numJoints());
112  m_servoStateIn.data.length(m_robot->numJoints());
113  m_servoStateOut.data.length(m_robot->numJoints());
114 
115  // set temperature of environment
116  if (prop["ambient_tmp"] != "") {
117  coil::stringTo(m_ambientTemp, prop["ambient_tmp"].c_str());
118  } else {
119  m_ambientTemp = 25.0;
120  }
121  std::cerr << "[" << m_profile.instance_name << "] : ambient temperature: " << m_ambientTemp << std::endl;
122 
123  // set motor heat parameters
124  m_motorHeatParams.resize(m_robot->numJoints());
125  coil::vstring motorHeatParamsFromConf = coil::split(prop["motor_heat_params"], ",");
126  if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) {
127  std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motorHeatParams is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl;
128  // motorHeatParam has default values itself
129  } else {
130  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
131  m_motorHeatParams[i].temperature = m_ambientTemp;
132  coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str());
133  coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str());
134  }
135  if (m_debugLevel > 0) {
136  std::cerr << "motorHeatParams is " << std::endl;
137  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
138  std::cerr << m_motorHeatParams[i].currentCoeffs << " " << m_motorHeatParams[i].thermoCoeffs << std::endl;
139  }
140  }
141  }
142 
143  // set constant for joint error to torque conversion
144  coil::vstring error2tauFromConf = coil::split(prop["error_to_tau_constant"], ",");
145  if (error2tauFromConf.size() != m_robot->numJoints()) {
146  std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of error2tau is " << error2tauFromConf.size() << ", not equal to " << m_robot->numJoints() << std::endl;
147  m_error2tau.resize(0); // invalid
148  } else {
149  m_error2tau.resize(m_robot->numJoints());
150  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
151  coil::stringTo(m_error2tau[i], error2tauFromConf[i].c_str());
152  }
153  if (m_debugLevel > 0) {
154  std::cerr << "motorHeatParams:" << std::endl;
155  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
156  std::cerr << " " << m_error2tau[i];
157  }
158  std::cerr << std::endl;
159  }
160  }
161 
162  return RTC::RTC_OK;
163 }
164 
165 
166 
167 /*
168  RTC::ReturnCode_t ThermoEstimator::onFinalize()
169  {
170  return RTC::RTC_OK;
171  }
172 */
173 
174 /*
175  RTC::ReturnCode_t ThermoEstimator::onStartup(RTC::UniqueId ec_id)
176  {
177  return RTC::RTC_OK;
178  }
179 */
180 
181 /*
182  RTC::ReturnCode_t ThermoEstimator::onShutdown(RTC::UniqueId ec_id)
183  {
184  return RTC::RTC_OK;
185  }
186 */
187 
189 {
190  std::cerr << "[" << m_profile.instance_name << "] : onActivated(" << ec_id << ")" << std::endl;
191  return RTC::RTC_OK;
192 }
193 
195 {
196  std::cerr << "[" << m_profile.instance_name << "] : onDeactivated(" << ec_id << ")" << std::endl;
197  return RTC::RTC_OK;
198 }
199 
201 {
202  // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
203  unsigned int numJoints = m_robot->numJoints();
204  m_loop++;
205 
207  RTC::Time tm;
208  tm.sec = coiltm.sec();
209  tm.nsec = coiltm.usec()*1000;
210 
211  if (m_tauInIn.isNew()) {
212  m_tauInIn.read();
213  }
214  if (m_qRefInIn.isNew()) {
215  m_qRefInIn.read();
216  }
217  if (m_qCurrentInIn.isNew()) {
219  }
220  if (m_servoStateInIn.isNew()) {
222  }
223 
224  // calculate joint torque
225  hrp::dvector jointTorque;
226  if (m_tauIn.data.length() == numJoints) { // use raw torque
227  jointTorque.resize(numJoints);
228  for (unsigned int i = 0; i < numJoints; i++) {
229  jointTorque[i] = m_tauIn.data[i];
230  }
231  if (isDebug()) {
232  std::cerr << "raw torque: ";
233  for (unsigned int i = 0; i < numJoints; i++) {
234  std::cerr << " " << m_tauIn.data[i] ;
235  }
236  std::cerr << std::endl;
237  }
238  } else if (m_qRefIn.data.length() == numJoints
239  && m_qCurrentIn.data.length() == numJoints) { // estimate torque from joint error
240  jointTorque.resize(numJoints);
241  hrp::dvector jointError(numJoints);
242  for (unsigned int i = 0; i < numJoints; i++) {
243  jointError[i] = m_qRefIn.data[i] - m_qCurrentIn.data[i];
244  }
245  estimateJointTorqueFromJointError(jointError, jointTorque);
246  if (isDebug()) {
247  std::cerr << "qRef: ";
248  for (unsigned int i = 0; i < numJoints; i++) {
249  std::cerr << " " << m_qRefIn.data[i] ;
250  }
251  std::cerr << std::endl;
252  std::cerr << "qCurrent: ";
253  for (unsigned int i = 0; i < numJoints; i++) {
254  std::cerr << " " << m_qCurrentIn.data[i] ;
255  }
256  std::cerr << std::endl;
257  }
258  } else { // invalid
259  jointTorque.resize(0);
260  }
261 
262  // calculate temperature from joint torque
263  if (jointTorque.size() == m_robot->numJoints()) {
264  for (unsigned int i = 0; i < numJoints; i++) {
265  // Thermo estimation
267  // output
268  m_tempOut.data[i] = m_motorHeatParams[i].temperature;
269  }
270  if (isDebug()) {
271  std::cerr << std::endl << "temperature : ";
272  for (unsigned int i = 0; i < numJoints; i++) {
273  std::cerr << " " << m_motorHeatParams[i].temperature;
274  }
275  std::cerr << std::endl;
276  }
278  }
279 
280  // overwrite temperature in servoState if temperature is calculated correctly
281  if (jointTorque.size() == m_robot->numJoints()
282  && m_servoStateIn.data.length() == m_robot->numJoints()) {
283  for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++) {
284  size_t len = m_servoStateIn.data[i].length();
285  m_servoStateOut.data[i].length(len + 1); // expand extra_data for temperature
286  for (unsigned int j = 0; j < len; j++) {
287  m_servoStateOut.data[i][j] = m_servoStateIn.data[i][j];
288  }
289  // servoStateOut is int, but extra data will be casted to float in HrpsysSeqStateROSBridge
290  float tmp_temperature = static_cast<float>(m_motorHeatParams[i].temperature);
291  std::memcpy(&(m_servoStateOut.data[i][len]), &tmp_temperature, sizeof(float));
292  }
293  } else { // pass servoStateIn to servoStateOut
294  m_servoStateOut.data.length(m_servoStateIn.data.length());
295  for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++) {
296  m_servoStateOut.data[i] = m_servoStateIn.data[i];
297  }
298  }
300 
301  return RTC::RTC_OK;
302 }
303 
304 /*
305  RTC::ReturnCode_t ThermoEstimator::onAborting(RTC::UniqueId ec_id)
306  {
307  return RTC::RTC_OK;
308  }
309 */
310 
311 /*
312  RTC::ReturnCode_t ThermoEstimator::onError(RTC::UniqueId ec_id)
313  {
314  return RTC::RTC_OK;
315  }
316 */
317 
318 /*
319  RTC::ReturnCode_t ThermoEstimator::onReset(RTC::UniqueId ec_id)
320  {
321  return RTC::RTC_OK;
322  }
323 */
324 
325 /*
326  RTC::ReturnCode_t ThermoEstimator::onStateUpdate(RTC::UniqueId ec_id)
327  {
328  return RTC::RTC_OK;
329  }
330 */
331 
332 /*
333  RTC::ReturnCode_t ThermoEstimator::onRateChanged(RTC::UniqueId ec_id)
334  {
335  return RTC::RTC_OK;
336  }
337 */
338 
340 {
341  if (error.size() == m_robot->numJoints()
342  && m_error2tau.size() == m_robot->numJoints()) {
343  tau.resize(m_robot->numJoints());
344  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
345  tau[i] = m_error2tau[i] * error[i];
346  }
347  if (isDebug()) {
348  std::cerr << "estimated torque: ";
349  for (unsigned int i = 0; i < m_robot->numJoints(); i++) {
350  std::cerr << " " << tau[i] ;
351  }
352  std::cerr << std::endl;
353  }
354  } else {
355  tau.resize(0); // don't calculate tau when invalid input
356  if (isDebug()) {
357  std::cerr << "Invalid size of values:" << std::endl;
358  std::cerr << "num joints: " << m_robot->numJoints() << std::endl;
359  std::cerr << "joint error: " << error.size() << std::endl;
360  std::cerr << "error2tau: " << m_error2tau.size() << std::endl;
361  }
362  }
363 
364  return;
365 }
366 
368 {
369  // from Design of High Torque and High Speed Leg Module for High Power Humanoid (Junichi Urata et al.)
370  // Tnew = T + (((Re*K^2/C) * tau^2) - ((1/RC) * (T - Ta))) * dt
371  double currentHeat, radiation;
372  currentHeat = param.currentCoeffs * std::pow(tau, 2);
373  radiation = -param.thermoCoeffs * (param.temperature - m_ambientTemp);
374  param.temperature = param.temperature + (currentHeat + radiation) * m_dt;
375  return;
376 }
377 
379 {
380  return ((m_debugLevel==1 && m_loop%cycle==0) || m_debugLevel > 1);
381 }
382 
383 extern "C"
384 {
385 
387  {
389  manager->registerFactory(profile,
390  RTC::Create<ThermoEstimator>,
391  RTC::Delete<ThermoEstimator>);
392  }
393 
394 };
395 
396 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
null component
virtual RTC::ReturnCode_t onInitialize()
bool stringTo(To &val, const char *str)
virtual ~ThermoEstimator()
Destructor.
void calculateJointTemperature(double tau, MotorHeatParam &param)
bool isDebug(int cycle=200)
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
CORBA::ORB_ptr getORB()
png_uint_32 i
Eigen::VectorXd dvector
coil::Properties & getProperties()
static Manager & instance()
bool addOutPort(const char *name, OutPortBase &outport)
boost::shared_ptr< Body > BodyPtr
unsigned int m_debugLevel
void estimateJointTorqueFromJointError(hrp::dvector &error, hrp::dvector &tau)
TimedDoubleSeq m_tauIn
std::vector< std::string > vstring
int gettimeofday(struct timeval *tv, struct timezone *tz)
coil::Properties & getConfig()
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateInIn
ExecutionContextHandle_t UniqueId
OpenHRP::TimedLongSeqSeq m_servoStateIn
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
std::vector< MotorHeatParam > m_motorHeatParams
InPort< TimedDoubleSeq > m_qCurrentInIn
TimedDoubleSeq m_qCurrentIn
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
double currentCoeffs
void ThermoEstimatorInit(RTC::Manager *manager)
TimedDoubleSeq m_qRefIn
TimedDoubleSeq m_tempOut
hrp::dvector m_error2tau
hrp::BodyPtr m_robot
InPort< TimedDoubleSeq > m_qRefInIn
prop
OutPort< OpenHRP::TimedLongSeqSeq > m_servoStateOutOut
naming
InPort< TimedDoubleSeq > m_tauInIn
long int sec() const
virtual bool isNew()
long int usec() const
virtual bool write(DataType &value)
OpenHRP::TimedLongSeqSeq m_servoStateOut
static const char * thermoestimator_spec[]
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
bool addInPort(const char *name, InPortBase &inport)
ThermoEstimator(RTC::Manager *manager)
Constructor.
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
OutPort< TimedDoubleSeq > m_tempOutOut


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:21