Add tlimit and angle initiailie for PDcontroller

rtc/PDcontroller/CMakeLists.txt

@@ -1,10 +1,11 @@
 set(comp_sources PDcontroller.cpp)
 add_library(PDcontroller SHARED ${comp_sources})
-target_link_libraries(PDcontroller ${OPENRTM_LIBRARIES})
+set(libs hrpModel-3.1 ${OPENRTM_LIBRARIES})
+target_link_libraries(PDcontroller ${libs})
 set_target_properties(PDcontroller PROPERTIES PREFIX "")
 
 add_executable(PDcontrollerComp PDcontrollerComp.cpp ${comp_sources})
-target_link_libraries(PDcontrollerComp ${OPENRTM_LIBRARIES})
+target_link_libraries(PDcontrollerComp ${libs})
 
 set(target PDcontroller PDcontrollerComp)
 

rtc/PDcontroller/PDcontroller.cpp

@@ -25,6 +25,8 @@ static const char* PDcontroller_spec[] =
     "language",          "C++",
     "lang_type",         "compile",
     // Configuration variables
+    "conf.default.pdgains_sim_file_name", "",
+    "conf.default.debugLevel", "0",
     ""
   };
 // </rtc-template>
@@ -37,7 +39,9 @@ PDcontroller::PDcontroller(RTC::Manager* manager)
     m_torqueOut("torque", m_torque),
     dt(0.005),
     // </rtc-template>
-    dummy(0)
+    dummy(0),
+    gain_fname(""),
+    dof(0), loop(0)
 {
 }
 
@@ -52,10 +56,28 @@ RTC::ReturnCode_t PDcontroller::onInitialize()
 
   RTC::Properties& prop = getProperties();
   coil::stringTo(dt, prop["dt"].c_str());
-  coil::stringTo(gain_fname, prop["pdgains_sim.file_name"].c_str());
+
+  m_robot = hrp::BodyPtr(new hrp::Body());
+
+  RTC::Manager& rtcManager = RTC::Manager::instance();
+  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+  int comPos = nameServer.find(",");
+  if (comPos < 0){
+      comPos = nameServer.length();
+  }
+  nameServer = nameServer.substr(0, comPos);
+  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
+                               CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                               )){
+      std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" 
+                << std::endl;
+  }
 
   // <rtc-template block="bind_config">
   // Bind variables and configuration variable
+  bindParameter("pdgains_sim_file_name", gain_fname, "");
+  bindParameter("debugLevel", m_debugLevel, "0");
 
   // Set InPort buffers
   addInPort("angle", m_angleIn);
@@ -95,11 +117,58 @@ RTC::ReturnCode_t PDcontroller::onShutdown(RTC::UniqueId ec_id)
 RTC::ReturnCode_t PDcontroller::onActivated(RTC::UniqueId ec_id)
 {
   std::cout << m_profile.instance_name << ": on Activated " << std::endl;
+  return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t PDcontroller::onDeactivated(RTC::UniqueId ec_id)
+{
+  std::cout << m_profile.instance_name << ": on Deactivated " << std::endl;
+  return RTC::RTC_OK;
+}
+
 
+RTC::ReturnCode_t PDcontroller::onExecute(RTC::UniqueId ec_id)
+{
+  loop++;
   if(m_angleIn.isNew()){
     m_angleIn.read();
+    if (dof == 0) {
+        dof = m_angle.data.length();
+        readGainFile();
+    }
+  }
+  if(m_angleRefIn.isNew()){
+    m_angleRefIn.read();
+  }
+
+  for(int i=0; i<dof; i++){
+    double q = m_angle.data[i];
+    double q_ref = m_angleRef.data[i];
+    double dq = (q - qold[i]) / dt;
+    double dq_ref = (q_ref - qold_ref[i]) / dt;
+    qold[i] = q;
+    qold_ref[i] = q_ref;
+    m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i];
+    double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;
+    m_torque.data[i] = std::max(std::min(m_torque.data[i], tlimit), -tlimit);
+    if (loop % 100 == 0 && m_debugLevel == 1) {
+        std::cerr << "[" << m_profile.instance_name << "] joint = "
+                  << i << ", tq = " << m_torque.data[i] << ", q,qref = (" << q << ", " << q_ref << "), dq,dqref = (" << dq << ", " << dq_ref << "), pd = (" << Pgain[i] << ", " << Dgain[i] << "), tlimit = " << tlimit << std::endl;
+    }
+  }
+
+  m_torqueOut.write();
+
+  return RTC::RTC_OK;
+}
+
+void PDcontroller::readGainFile()
+{
+    if (gain_fname == "") {
+        RTC::Properties& prop = getProperties();
+        coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str());
+    }
     // initialize length of vectors
-    dof = m_angle.data.length();
     qold.resize(dof);
     qold_ref.resize(dof);
     m_torque.data.length(dof);
@@ -130,47 +199,8 @@ RTC::ReturnCode_t PDcontroller::onActivated(RTC::UniqueId ec_id)
     for(int i=0; i < dof; ++i){
       m_angleRef.data[i] = qold_ref[i] = qold[i] = m_angle.data[i];
     }
-  }
-  if(m_angleRefIn.isNew()){
-    m_angleRefIn.read();
-  }
-
-  return RTC::RTC_OK;
-}
-
-RTC::ReturnCode_t PDcontroller::onDeactivated(RTC::UniqueId ec_id)
-{
-  std::cout << m_profile.instance_name << ": on Deactivated " << std::endl;
-  return RTC::RTC_OK;
-}
-
-
-RTC::ReturnCode_t PDcontroller::onExecute(RTC::UniqueId ec_id)
-{
-  if(m_angleIn.isNew()){
-    m_angleIn.read();
-  }
-  if(m_angleRefIn.isNew()){
-    m_angleRefIn.read();
-  }
-
-  for(int i=0; i<dof; i++){
-    double q = m_angle.data[i];
-    double q_ref = m_angleRef.data[i];
-    double dq = (q - qold[i]) / dt;
-    double dq_ref = (q_ref - qold_ref[i]) / dt;
-    qold[i] = q;
-    qold_ref[i] = q_ref;
-    m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i];
-    // std::cerr << i << " " << m_torque.data[i] << " (" << q << " " << q_ref << ") (" << dq << " " << dq_ref << ") " << Pgain[i] << " " << Dgain[i] << std::endl;
-  }
-
-  m_torqueOut.write();
-
-  return RTC::RTC_OK;
 }
 
-
 /*
   RTC::ReturnCode_t PDcontroller::onAborting(RTC::UniqueId ec_id)
   {

rtc/PDcontroller/PDcontroller.h

@@ -13,12 +13,15 @@
 #include <rtm/Manager.h>
 #include <rtm/DataFlowComponentBase.h>
 #include <rtm/CorbaPort.h>
+#include <rtm/CorbaNaming.h>
 #include <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 
 #include <hrpUtil/EigenTypes.h>
-
+#include <hrpModel/ModelLoaderUtil.h>
+#include <hrpModel/Body.h>
+#include <hrpModel/Link.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
@@ -87,7 +90,6 @@ class PDcontroller
   // no corresponding operation exists in OpenRTm-aist-0.2.0
   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
 
-
  protected:
   // Configuration variable declaration
   // <rtc-template block="config_declare">
@@ -126,12 +128,15 @@ class PDcontroller
   // </rtc-template>
 
  private:
+  void readGainFile();
+  hrp::BodyPtr m_robot;
   int dummy;
   double dt;
   std::ifstream gain;
   std::string gain_fname;
   hrp::dvector qold, qold_ref, Pgain, Dgain;
-  size_t dof;
+  size_t dof, loop;
+  unsigned int m_debugLevel;
 };
 
 extern "C"

sample/SampleRobot/SampleRobot.conf.in

@@ -5,7 +5,7 @@ abc_leg_offset: 0,0.09,0
 end_effectors: lleg,LLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, rleg,RLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, larm,LARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708, rarm,RARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708,
 
 # PDcontroller Setting
-pdgains_sim.file_name: @CMAKE_INSTALL_PREFIX@/share/hrpsys/samples/SampleRobot/SampleRobot.PDgain.dat
+pdgains_sim_file_name: @CMAKE_INSTALL_PREFIX@/share/hrpsys/samples/SampleRobot/SampleRobot.PDgain.dat
 
 # CollisionDetector Setting
 collision_pair: RARM_WRIST_P:WAIST LARM_WRIST_P:WAIST RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R RARM_WRIST_R:RLEG_HIP_R LARM_WRIST_R:LLEG_HIP_R LLEG_ANKLE_R:RLEG_ANKLE_R