Merge pull request #229 from orikuma/load-limiter

Added emergency nortification for temperature and torque to ThermoLimiter

python/hrpsys_config.py

@@ -387,6 +387,8 @@ def connectComps(self):
 
  # connection for te
  if self.te:
+ connectPorts(self.rh.port("q"), self.te.port("qCurrentIn"))
+ connectPorts(self.sh.port("qOut"), self.te.port("qRefIn"))
  if self.tf:
  connectPorts(self.tf.port("tauOut"), self.te.port("tauIn"))
  else:
@@ -395,14 +397,8 @@ def connectComps(self):
 
  # connection for tl
  if self.tl:
- if self.tf:
- connectPorts(self.tf.port("tauOut"), self.tl.port("tauIn"))
- else:
- connectPorts(self.rh.port("tau"), self.tl.port("tauIn"))
  if self.te:
  connectPorts(self.te.port("tempOut"), self.tl.port("tempIn"))
- connectPorts(self.rh.port("q"), self.tl.port("qCurrentIn"))
- # qRef is connected as joint angle controller
 
  # connection for tc
  if self.tc:

rtc/ThermoEstimator/ThermoEstimator.cpp

@@ -18,8 +18,6 @@
 #include <hrpModel/ModelLoaderUtil.h>
 #include <hrpUtil/MatrixSolvers.h>
 
-#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
-
 // Module specification
 // <rtc-template block="module_spec">
 static const char* thermoestimator_spec[] =
@@ -45,6 +43,8 @@ ThermoEstimator::ThermoEstimator(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
  // <rtc-template block="initializer">
  m_tauInIn("tauIn", m_tauIn),
+ m_qRefInIn("qRefIn", m_qRefIn),
+ m_qCurrentInIn("qCurrentIn", m_qCurrentIn),
  m_servoStateInIn("servoStateIn", m_servoStateIn),
  m_tempOutOut("tempOut", m_tempOut),
  m_servoStateOutOut("servoStateOut", m_servoStateOut), 
@@ -70,6 +70,8 @@ RTC::ReturnCode_t ThermoEstimator::onInitialize()
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("tauIn", m_tauInIn);
+ addInPort("qRefIn", m_qRefInIn);
+ addInPort("qCurrentIn", m_qCurrentInIn);
  addInPort("servoStateIn", m_servoStateInIn);
 
  // Set OutPort buffer
@@ -123,14 +125,38 @@ RTC::ReturnCode_t ThermoEstimator::onInitialize()
  coil::vstring motorHeatParamsFromConf = coil::split(prop["motor_heat_params"], ",");
  if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) {
  std::cerr << "[WARN]: size of motorHeatParams is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl;
+ // motorHeatParam has default values itself
  } else {
- std::cerr << "motorHeatParams is " << std::endl;
  for (int i = 0; i < m_robot->numJoints(); i++) {
  m_motorHeatParams[i].temperature = m_ambientTemp;
- std::cerr << motorHeatParamsFromConf[2 * i].c_str() << " " << motorHeatParamsFromConf[2 * i + 1].c_str() << std::endl;
  coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str());
  coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str());
  }
+ if (m_debugLevel > 0) {
+ std::cerr << "motorHeatParams is " << std::endl;
+ for (int i = 0; i < m_robot->numJoints(); i++) {
+ std::cerr << m_motorHeatParams[i].currentCoeffs << " " << m_motorHeatParams[i].thermoCoeffs << std::endl;
+ }
+ }
+ }
+
+ // set constant for joint error to torque conversion
+ coil::vstring error2tauFromConf = coil::split(prop["error_to_tau_constant"], ",");
+ if (error2tauFromConf.size() != m_robot->numJoints()) {
+ std::cerr << "[WARN]: size of error2tau is " << error2tauFromConf.size() << ", not equal to " << m_robot->numJoints() << std::endl;
+ m_error2tau.resize(0); // invalid 
+ } else {
+ m_error2tau.resize(m_robot->numJoints());
+ for (int i = 0; i < m_robot->numJoints(); i++) {
+ coil::stringTo(m_error2tau[i], error2tauFromConf[i].c_str());
+ }
+ if (m_debugLevel > 0) {
+ std::cerr << "motorHeatParams:" << std::endl;
+ for (int i = 0; i < m_robot->numJoints(); i++) {
+ std::cerr << " " << m_error2tau[i];
+ }
+ std::cerr << std::endl; 
+ }
  }
 
  return RTC::RTC_OK;
@@ -174,9 +200,9 @@ RTC::ReturnCode_t ThermoEstimator::onDeactivated(RTC::UniqueId ec_id)
 RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id)
 {
  // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
- static long long loop = 0;
- loop ++;
-
+ int numJoints = m_robot->numJoints();
+ m_loop++;
+ 
  coil::TimeValue coiltm(coil::gettimeofday());
  RTC::Time tm;
  tm.sec = coiltm.sec();
@@ -185,60 +211,93 @@ RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id)
  if (m_tauInIn.isNew()) {
  m_tauInIn.read();
  }
-
- if ( m_tauIn.data.length() == m_robot->numJoints() ) {
- int numJoints = m_robot->numJoints();
- if ( DEBUGP ) {
+ if (m_qRefInIn.isNew()) {
+ m_qRefInIn.read();
+ }
+ if (m_qCurrentInIn.isNew()) {
+ m_qCurrentInIn.read();
+ }
+ if (m_servoStateInIn.isNew()) {
+ m_servoStateInIn.read();
+ }
+
+ // calculate joint torque
+ hrp::dvector jointTorque;
+ if (m_tauIn.data.length() == numJoints) { // use raw torque
+ jointTorque.resize(numJoints);
+ for (int i = 0; i < numJoints; i++) {
+ jointTorque[i] = m_tauIn.data[i];
+ }
+ if (isDebug()) {
  std::cerr << "raw torque: ";
  for (int i = 0; i < numJoints; i++) {
  std::cerr << " " << m_tauIn.data[i] ;
  }
  std::cerr << std::endl;
  }
- if ( DEBUGP ) {
- std::cerr << "estimation values: " << std::endl;
+ } else if (m_qRefIn.data.length() == numJoints
+ && m_qCurrentIn.data.length() == numJoints) { // estimate torque from joint error
+ jointTorque.resize(numJoints);
+ hrp::dvector jointError(numJoints);
+ for (int i = 0; i < numJoints; i++) {
+ jointError[i] = m_qRefIn.data[i] - m_qCurrentIn.data[i];
  }
+ estimateJointTorqueFromJointError(jointError, jointTorque);
+ if (isDebug()) {
+ std::cerr << "qRef: ";
+ for (int i = 0; i < numJoints; i++) {
+ std::cerr << " " << m_qRefIn.data[i] ;
+ }
+ std::cerr << std::endl;
+ std::cerr << "qCurrent: ";
+ for (int i = 0; i < numJoints; i++) {
+ std::cerr << " " << m_qCurrentIn.data[i] ;
+ }
+ std::cerr << std::endl;
+ }
+ } else { // invalid 
+ jointTorque.resize(0);
+ }
+
+ // calculate temperature from joint torque
+ if (jointTorque.size() == m_robot->numJoints()) {
  for (int i = 0; i < numJoints; i++) {
- MotorHeatParam param = m_motorHeatParams[i];
- double tau, currentHeat, radiation;
  // Thermo estimation
- // from Design of High Torque and High Speed Leg Module for High Power Humanoid (Junichi Urata et al.)
- // Tnew = T + (((Re*K^2/C) * tau^2) - ((1/RC) * (T - Ta))) * dt
- tau = m_tauIn.data[i];
- currentHeat = param.currentCoeffs * std::pow(tau, 2);
- radiation = -param.thermoCoeffs * (param.temperature - m_ambientTemp);
- m_motorHeatParams[i].temperature = param.temperature + (currentHeat + radiation) * m_dt;
- if ( DEBUGP ) {
- std::cerr << currentHeat << " " << radiation << ", ";
- }
+ calculateJointTemperature(jointTorque[i], m_motorHeatParams[i]);
  // output
  m_tempOut.data[i] = m_motorHeatParams[i].temperature;
  }
- if ( DEBUGP ) {
+ if (isDebug()) {
  std::cerr << std::endl << "temperature : ";
  for (int i = 0; i < numJoints; i++) {
  std::cerr << " " << m_motorHeatParams[i].temperature;
  }
  std::cerr << std::endl;
  }
+ m_tempOutOut.write();
+ }
 
- // overwrite temperature in servoState
- if ( m_servoStateInIn.isNew() && (m_servoStateIn.data.length() == m_robot->numJoints()) ) {
- m_servoStateInIn.read();
- for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++){
- size_t len = m_servoStateIn.data[i].length();
- m_servoStateOut.data[i].length(len + 1); // expand extra_data for temperature
- for (unsigned int j = 0; j < len; j++){
- m_servoStateOut.data[i][j] = m_servoStateIn.data[i][j];
- }
- // servoStateOut is int, but extra data will be casted to float in HrpsysSeqStateROSBridge
- float tmp_temperature = static_cast<float>(m_motorHeatParams[i].temperature);
- std::memcpy(&(m_servoStateOut.data[i][len]), &tmp_temperature, sizeof(float));
+ // overwrite temperature in servoState if temperature is calculated correctly
+ if (jointTorque.size() == m_robot->numJoints()
+ && m_servoStateIn.data.length() == m_robot->numJoints()) {
+ for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++) {
+ size_t len = m_servoStateIn.data[i].length();
+ m_servoStateOut.data[i].length(len + 1); // expand extra_data for temperature
+ for (unsigned int j = 0; j < len; j++) {
+ m_servoStateOut.data[i][j] = m_servoStateIn.data[i][j];
  }
- m_servoStateOutOut.write();
+ // servoStateOut is int, but extra data will be casted to float in HrpsysSeqStateROSBridge
+ float tmp_temperature = static_cast<float>(m_motorHeatParams[i].temperature);
+ std::memcpy(&(m_servoStateOut.data[i][len]), &tmp_temperature, sizeof(float));
+ }
+ } else { // pass servoStateIn to servoStateOut
+ m_servoStateOut.data.length(m_servoStateIn.data.length());
+ for (int i = 0; i < m_servoStateIn.data.length(); i++) {
+ m_servoStateOut.data[i] = m_servoStateIn.data[i];
  }
- m_tempOutOut.write();
  }
+ m_servoStateOutOut.write();
+
  return RTC::RTC_OK;
 }
 
@@ -277,7 +336,49 @@ RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id)
  }
 */
 
+void ThermoEstimator::estimateJointTorqueFromJointError(hrp::dvector &error, hrp::dvector &tau)
+{
+ if (error.size() == m_robot->numJoints()
+ && m_error2tau.size() == m_robot->numJoints()) {
+ tau.resize(m_robot->numJoints());
+ for (int i = 0; i < m_robot->numJoints(); i++) {
+ tau[i] = m_error2tau[i] * error[i];
+ }
+ if (isDebug()) {
+ std::cerr << "estimated torque: ";
+ for (int i = 0; i < m_robot->numJoints(); i++) {
+ std::cerr << " " << tau[i] ;
+ }
+ std::cerr << std::endl;
+ }
+ } else {
+ tau.resize(0); // don't calculate tau when invalid input
+ if (isDebug()) {
+ std::cerr << "Invalid size of values:" << std::endl;
+ std::cerr << "num joints: " << m_robot->numJoints() << std::endl;
+ std::cerr << "joint error: " << error.size() << std::endl;
+ std::cerr << "error2tau: " << m_error2tau.size() << std::endl;
+ }
+ }
+
+ return;
+}
+
+void ThermoEstimator::calculateJointTemperature(double tau, MotorHeatParam& param)
+{
+ // from Design of High Torque and High Speed Leg Module for High Power Humanoid (Junichi Urata et al.)
+ // Tnew = T + (((Re*K^2/C) * tau^2) - ((1/RC) * (T - Ta))) * dt
+ double currentHeat, radiation;
+ currentHeat = param.currentCoeffs * std::pow(tau, 2);
+ radiation = -param.thermoCoeffs * (param.temperature - m_ambientTemp);
+ param.temperature = param.temperature + (currentHeat + radiation) * m_dt;
+ return;
+}
 
+bool ThermoEstimator::isDebug(int cycle)
+{
+ return ((m_debugLevel==1 && m_loop%cycle==0) || m_debugLevel > 1);
+}
 
 extern "C"
 {

rtc/ThermoEstimator/ThermoEstimator.h

@@ -109,13 +109,18 @@ class ThermoEstimator
 
  // </rtc-template>
  TimedDoubleSeq m_tauIn;
- TimedDoubleSeq m_tempOut;
+ TimedDoubleSeq m_qRefIn;
+ TimedDoubleSeq m_qCurrentIn;
  OpenHRP::TimedLongSeqSeq m_servoStateIn;
+
+ TimedDoubleSeq m_tempOut;
  OpenHRP::TimedLongSeqSeq m_servoStateOut;
 
  // DataInPort declaration
  // <rtc-template block="inport_declare">
  InPort<TimedDoubleSeq> m_tauInIn;
+ InPort<TimedDoubleSeq> m_qRefInIn;
+ InPort<TimedDoubleSeq> m_qCurrentInIn;
  InPort<OpenHRP::TimedLongSeqSeq> m_servoStateInIn;
 
  // </rtc-template>
@@ -147,11 +152,15 @@ class ThermoEstimator
  private:
 
  double m_dt;
+ long long m_loop;
  unsigned int m_debugLevel;
  hrp::BodyPtr m_robot; // for numJoints
  double m_ambientTemp; // Ta
  std::vector<MotorHeatParam> m_motorHeatParams;
-
+ hrp::dvector m_error2tau;
+ void estimateJointTorqueFromJointError(hrp::dvector &error, hrp::dvector &tau);
+ void calculateJointTemperature(double tau, MotorHeatParam& param);
+ bool isDebug(int cycle = 200);
 };