Debugged config parameter index miss and added isDebug

snozawa · Jun 4, 2014 · 065cd2f · 065cd2f
1 parent acf734c
commit 065cd2f
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 16 deletions.
diff --git a/rtc/TorqueController/TorqueController.cpp b/rtc/TorqueController/TorqueController.cpp
@@ -34,9 +34,6 @@ static const char* torquecontroller_spec[] =
 };
 // </rtc-template>
 
-#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
-#define DQ_MAX 1.0
-
 typedef coil::Guard<coil::Mutex> Guard;
 
 TorqueController::TorqueController(RTC::Manager* manager)
@@ -133,9 +130,9 @@ RTC::ReturnCode_t TorqueController::onInitialize()
  } else if (motorTorqueControllerParamsFromConf.size() == 3 * m_robot->numJoints()) { // use TwoDofControllerWithDamper
  std::cerr << "use TwoDofControllerWithDamper" << std::endl;
  for (int i = 0; i < m_robot->numJoints(); i++) { // use TwoDofControllerWithDamper
- coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[2 * i].c_str());
- coil::stringTo(tdcParamKd[i], motorTorqueControllerParamsFromConf[2 * i + 1].c_str());
- coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[2 * i + 2].c_str());
+ coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[3 * i].c_str());
+ coil::stringTo(tdcParamKd[i], motorTorqueControllerParamsFromConf[3 * i + 1].c_str());
+ coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[3 * i + 2].c_str());
  m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamKd[i], tdcParamT[i], m_dt));
  }
  if (m_debugLevel > 0) {
@@ -201,8 +198,8 @@ RTC::ReturnCode_t TorqueController::onDeactivated(RTC::UniqueId ec_id)
 
 RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id)
 { 
- static int loop = 0;
- loop++;
+ m_loop++;
+
  // make timestamp
  coil::TimeValue coiltm(coil::gettimeofday());
  hrp::dvector dq(m_robot->numJoints());
@@ -242,7 +239,7 @@ RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id)
  m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit);
  }
  } else {
- if (DEBUGP) {
+ if (isDebug()) {
  std::cerr << "TorqueController input is not correct" << std::endl;
  std::cerr << " numJoints: " << m_robot->numJoints() << std::endl;
  std::cerr << " qCurrent: " << m_qCurrentIn.data.length() << std::endl;
@@ -299,13 +296,11 @@ RTC::ReturnCode_t TorqueController::onRateChanged(RTC::UniqueId ec_id)
 
 void TorqueController::executeTorqueControl(hrp::dvector &dq)
 {
- static int loop = 0;
  int numJoints = m_robot->numJoints();
  hrp::dvector tauMax(numJoints);
- loop++;
  dq.resize(numJoints);
 
- if (DEBUGP) {
+ if (isDebug()) {
  std::cerr << "[" << m_profile.instance_name << "]" << std::endl;
  }
 
@@ -320,7 +315,7 @@ void TorqueController::executeTorqueControl(hrp::dvector &dq)
 
  // execute torque control
  // tauCurrent.length is assumed to be equal to numJoints (check in onExecute)
- if (DEBUGP) {
+ if (isDebug()) {
  std::cerr << "tauCurrentIn: ";
  for (int i = 0; i < numJoints; i++) {
  std::cerr << " " << m_tauCurrentIn.data[i];
@@ -337,13 +332,13 @@ void TorqueController::executeTorqueControl(hrp::dvector &dq)
  for (int i = 0; i < numJoints; i++) {
  dq[i] = m_motorTorqueControllers[i].execute(m_tauCurrentIn.data[i], tauMax[i]); // twoDofController: tau = -K(q - qRef)
  // output debug message
- if (DEBUGP && m_motorTorqueControllers[i].getMotorControllerState() != MotorTorqueController::INACTIVE) {
+ if (isDebug() && m_motorTorqueControllers[i].getMotorControllerState() != MotorTorqueController::INACTIVE) {
  m_motorTorqueControllers[i].printMotorControllerVariables();
  }
 
  }
 
- if (DEBUGP) {
+ if (isDebug()) {
  std::cerr << "dq: ";
  for (int i = 0; i < dq.size(); i++) {
  std::cerr << dq[i] << " ";
@@ -444,6 +439,11 @@ bool TorqueController::setMultipleReferenceTorques(const OpenHRP::TorqueControll
  return succeed;
 }
 
+bool TorqueController::isDebug(int cycle)
+{
+ return ((m_debugLevel == 1 && (m_loop % cycle == 0)) || m_debugLevel > 1);
+}
+
 extern "C"
 {
 

diff --git a/rtc/TorqueController/TorqueController.h b/rtc/TorqueController/TorqueController.h
@@ -157,10 +157,12 @@ class TorqueController
 
  double m_dt;
  unsigned int m_debugLevel;
+ int m_loop;
  hrp::BodyPtr m_robot;
  std::vector<MotorTorqueController> m_motorTorqueControllers;
  coil::Mutex m_mutex;
  void executeTorqueControl(hrp::dvector &dq);
+ bool isDebug(int cycle = 200);
 };
 
 

diff --git a/rtc/TorqueController/testMotorTorqueController.cpp b/rtc/TorqueController/testMotorTorqueController.cpp
@@ -6,7 +6,7 @@
 #define ABS(x) (((x) < 0) ? (-(x)) : (x))
 
 int main (int argc, char* argv[]) {
- double ke = 0.35, kd = 20.0, tc = 0.05, dt = 0.005;
+ double ke = 2.0, kd = 20.0, tc = 0.05, dt = 0.005;
  const int test_num = 2;
  MotorTorqueController *controller[test_num];
  controller[0] = new MotorTorqueController("hoge", ke, tc, dt);