tq.patch

Index: SequencePlayer/SequencePlayer.cpp
===================================================================
--- SequencePlayer/SequencePlayer.cpp	(リビジョン 1002)
+++ SequencePlayer/SequencePlayer.cpp	(作業コピー)
@@ -209,6 +209,9 @@
 
         double zmp[3], acc[3], pos[3], rpy[3];
         m_seq->get(m_qRef.data.get_buffer(), zmp, acc, pos, rpy, m_tqRef.data.get_buffer());
+        std::cerr << "get_data ";
+        for(int i = 0 ; i < m_qRef.data.length(); i++ ) std::cerr << m_qRef.data.get_buffer()[i] << " ";
+        std::cerr << std::endl;
         m_zmpRef.data.x = zmp[0];
         m_zmpRef.data.y = zmp[1];
         m_zmpRef.data.z = zmp[2];
@@ -325,7 +328,7 @@
 
 bool SequencePlayer::setJointAngles(const double *angles, double tm)
 {
-    if ( m_debugLevel > 0 ) {
+    if ( m_debugLevel > 10 ) {
         std::cerr << __PRETTY_FUNCTION__ << std::endl;
     }
     Guard guard(m_mutex);
@@ -333,7 +336,9 @@
     for (int i=0; i<m_robot->numJoints(); i++){
         hrp::Link *j = m_robot->joint(i);
         if (j) j->q = angles[i];
+        std::cerr << angles[i] << " ";
     }
+    std::cerr << std::endl;
     m_robot->calcForwardKinematics();
     hrp::Vector3 absZmp = m_robot->calcCM();
     absZmp[2] = 0;
Index: TorqueController/TorqueController.cpp
===================================================================
--- TorqueController/TorqueController.cpp	(リビジョン 1002)
+++ TorqueController/TorqueController.cpp	(作業コピー)
@@ -185,6 +185,8 @@
 
 RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id)
 { 
+  static int loop = 0;
+  loop++;
   // make timestamp
   coil::TimeValue coiltm(coil::gettimeofday());
   hrp::dvector dq(m_robot->numJoints());
@@ -207,6 +209,7 @@
   }
 
   if (m_qRefIn.data.length() == m_robot->numJoints() &&
+      m_tauCurrentIn.data.length() == m_robot->numJoints() &&
       m_qCurrentIn.data.length() == m_robot->numJoints()) {
 
     // update model
@@ -225,11 +228,14 @@
     m_qRefOut.tm = tm;
     m_qRefOutOut.write();
   } else {
-    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;
-    std::cerr << "qRefIn: " << m_qRefIn.data.length() << std::endl;
-    std::cerr << std::endl;
+    if ( loop % 100 == 1) {
+      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;
+      std::cerr << "    qRefIn: " << m_qRefIn.data.length() << std::endl;
+      std::cerr << "tauCurrent: " << m_tauCurrentIn.data.length() << std::endl;
+      std::cerr << std::endl;
+    }
   }
   return RTC::RTC_OK;
 }
@@ -324,6 +330,7 @@
     }
   } else {
     std::cerr << "TorqueController input is not correct" << std::endl;
+    std::cerr << "numJoints: " << m_robot->numJoints() << std::endl;
     std::cerr << "tauCurrent: " << m_tauCurrentIn.data.length() << std::endl;
     std::cerr << std::endl;
   }