PDcontroller.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; -*-
00010 #include "PDcontroller.h"
00011 #include <iostream>
00012 #include <coil/stringutil.h>
00013 
00014 // Module specification
00015 // <rtc-template block="module_spec">
00016 static const char* PDcontroller_spec[] =
00017   {
00018     "implementation_id", "PDcontroller",
00019     "type_name",         "PDcontroller",
00020     "description",       "PDcontroller component",
00021     "version",           HRPSYS_PACKAGE_VERSION,
00022     "vendor",            "AIST",
00023     "category",          "example",
00024     "activity_type",     "DataFlowComponent",
00025     "max_instance",      "10",
00026     "language",          "C++",
00027     "lang_type",         "compile",
00028     // Configuration variables
00029     "conf.default.pdgains_sim_file_name", "",
00030     "conf.default.debugLevel", "0",
00031     ""
00032   };
00033 // </rtc-template>
00034 
00035 PDcontroller::PDcontroller(RTC::Manager* manager)
00036   : RTC::DataFlowComponentBase(manager),
00037     // <rtc-template block="initializer">
00038     m_angleIn("angle", m_angle),
00039     m_angleRefIn("angleRef", m_angleRef),
00040     m_torqueOut("torque", m_torque),
00041     dt(0.005),
00042     // </rtc-template>
00043     gain_fname(""),
00044     dof(0), loop(0),
00045     dummy(0)
00046 {
00047 }
00048 
00049 PDcontroller::~PDcontroller()
00050 {
00051 }
00052 
00053 
00054 RTC::ReturnCode_t PDcontroller::onInitialize()
00055 {
00056   std::cout << m_profile.instance_name << ": onInitialize() " << std::endl;
00057 
00058   RTC::Properties& prop = getProperties();
00059   coil::stringTo(dt, prop["dt"].c_str());
00060   ref_dt = dt;
00061   coil::stringTo(ref_dt, prop["ref_dt"].c_str());
00062   nstep = ref_dt/dt;
00063   step = nstep;
00064 
00065   m_robot = hrp::BodyPtr(new hrp::Body());
00066 
00067   RTC::Manager& rtcManager = RTC::Manager::instance();
00068   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00069   int comPos = nameServer.find(",");
00070   if (comPos < 0){
00071       comPos = nameServer.length();
00072   }
00073   nameServer = nameServer.substr(0, comPos);
00074   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00075   if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
00076                                CosNaming::NamingContext::_duplicate(naming.getRootContext())
00077                                )){
00078       std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" 
00079                 << std::endl;
00080   }
00081 
00082   // <rtc-template block="bind_config">
00083   // Bind variables and configuration variable
00084   bindParameter("pdgains_sim_file_name", gain_fname, "");
00085   bindParameter("debugLevel", m_debugLevel, "0");
00086 
00087   // Set InPort buffers
00088   addInPort("angle", m_angleIn);
00089   addInPort("angleRef", m_angleRefIn);
00090   
00091   // Set OutPort buffer
00092   addOutPort("torque", m_torqueOut);
00093   
00094   // </rtc-template>
00095 
00096   return RTC::RTC_OK;
00097 }
00098 
00099 
00100 
00101 /*
00102 RTC::ReturnCode_t PDcontroller::onFinalize()
00103 {
00104   return RTC::RTC_OK;
00105 }
00106 */
00107 
00108 /*
00109 RTC::ReturnCode_t PDcontroller::onStartup(RTC::UniqueId ec_id)
00110 {
00111   return RTC::RTC_OK;
00112 }
00113 */
00114 
00115 /*
00116 RTC::ReturnCode_t PDcontroller::onShutdown(RTC::UniqueId ec_id)
00117 {
00118   return RTC::RTC_OK;
00119 }
00120 */
00121 
00122 RTC::ReturnCode_t PDcontroller::onActivated(RTC::UniqueId ec_id)
00123 {
00124   std::cout << m_profile.instance_name << ": on Activated " << std::endl;
00125   if(m_angleIn.isNew()){
00126     m_angleIn.read();
00127     if (dof == 0) {
00128         dof = m_angle.data.length();
00129         readGainFile();
00130     }
00131   }
00132   return RTC::RTC_OK;
00133 }
00134 
00135 RTC::ReturnCode_t PDcontroller::onDeactivated(RTC::UniqueId ec_id)
00136 {
00137   std::cout << m_profile.instance_name << ": on Deactivated " << std::endl;
00138   return RTC::RTC_OK;
00139 }
00140 
00141 
00142 RTC::ReturnCode_t PDcontroller::onExecute(RTC::UniqueId ec_id)
00143 {
00144   loop++;
00145   if(m_angleIn.isNew()){
00146     m_angleIn.read();
00147     if (dof == 0) {
00148         dof = m_angle.data.length();
00149         readGainFile();
00150     }
00151   }
00152   if(m_angleRefIn.isNew()){
00153     m_angleRefIn.read();
00154     step = nstep;
00155   }
00156 
00157   for(size_t i=0; i<dof; i++){
00158     double q = m_angle.data[i];
00159     double q_ref = step > 0 ? qold_ref[i] + (m_angleRef.data[i] - qold_ref[i])/step : qold_ref[i];
00160     double dq = (q - qold[i]) / dt;
00161     double dq_ref = (q_ref - qold_ref[i]) / dt;
00162     qold[i] = q;
00163     qold_ref[i] = q_ref;
00164     m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i];
00165     double tlimit;
00166     if (m_robot && m_robot->numJoints() == dof) {
00167         tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tlimit_ratio[i];
00168     } else {
00169         tlimit = (std::numeric_limits<double>::max)() * tlimit_ratio[i];
00170         if (i == 0 && loop % 500 == 0) {
00171             std::cerr << "[" << m_profile.instance_name << "] m_robot is not set properly!! Maybe ModelLoader is missing?" << std::endl;
00172         }
00173     }
00174     if (loop % 100 == 0 && m_debugLevel == 1) {
00175         std::cerr << "[" << m_profile.instance_name << "] joint = "
00176                   << 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;
00177     }
00178     m_torque.data[i] = std::max(std::min(m_torque.data[i], tlimit), -tlimit);
00179   }
00180   step--; 
00181   
00182   m_torqueOut.write();
00183   
00184   return RTC::RTC_OK;
00185 }
00186 
00187 void PDcontroller::readGainFile()
00188 {
00189     if (gain_fname == "") {
00190         RTC::Properties& prop = getProperties();
00191         coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str());
00192     }
00193     // initialize length of vectors
00194     qold.resize(dof);
00195     qold_ref.resize(dof);
00196     m_torque.data.length(dof);
00197     m_angleRef.data.length(dof);
00198     Pgain.resize(dof);
00199     Dgain.resize(dof);
00200     gain.open(gain_fname.c_str());
00201     tlimit_ratio.resize(dof);
00202     if (gain.is_open()){
00203       double tmp;
00204       for (unsigned int i=0; i<dof; i++){
00205           if (gain >> tmp) {
00206               Pgain[i] = tmp;
00207           } else {
00208               std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl;
00209           }
00210           if (gain >> tmp) {
00211               Dgain[i] = tmp;
00212           } else {
00213               std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl;
00214           }
00215       }
00216       gain.close();
00217       std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] opened" << std::endl;
00218     }else{
00219       std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] not opened" << std::endl;
00220     }
00221     // tlimit_ratio initialize
00222     {
00223         RTC::Properties& prop = getProperties();
00224         if (prop["pdcontrol_tlimit_ratio"] != "") {
00225             coil::vstring tlimit_ratio_str = coil::split(prop["pdcontrol_tlimit_ratio"], ",");
00226             if (tlimit_ratio_str.size() == dof) {
00227                 for (size_t i = 0; i < dof; i++) {
00228                     coil::stringTo(tlimit_ratio[i], tlimit_ratio_str[i].c_str());
00229                 }
00230                 std::cerr << "[" << m_profile.instance_name << "] tlimit_ratio is set to " << prop["pdcontrol_tlimit_ratio"] << std::endl;
00231             } else {
00232                 for (size_t i = 0; i < dof; i++) {
00233                     tlimit_ratio[i] = 1.0;
00234                 }
00235                 std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tlimit_ratio found, but invalid length (" << tlimit_ratio_str.size() << " != " << dof << ")." << std::endl;
00236                 std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl;
00237             }
00238         } else {
00239             for (size_t i = 0; i < dof; i++) {
00240                 tlimit_ratio[i] = 1.0;
00241             }
00242             std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tlimit_ratio found." << std::endl;
00243             std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl;
00244         }
00245     }
00246     // initialize angleRef, old_ref and old with angle
00247     for(unsigned int i=0; i < dof; ++i){
00248       m_angleRef.data[i] = qold_ref[i] = qold[i] = m_angle.data[i];
00249     }
00250     // Print loaded gain
00251     std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl;
00252     for (unsigned int i=0; i<m_robot->numJoints(); i++) {
00253         std::cerr << "[" << m_profile.instance_name << "]   " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl;
00254     }
00255 }
00256 
00257 /*
00258   RTC::ReturnCode_t PDcontroller::onAborting(RTC::UniqueId ec_id)
00259   {
00260   return RTC::RTC_OK;
00261   }
00262 */
00263 
00264 /*
00265   RTC::ReturnCode_t PDcontroller::onError(RTC::UniqueId ec_id)
00266   {
00267   return RTC::RTC_OK;
00268   }
00269 */
00270 
00271 /*
00272   RTC::ReturnCode_t PDcontroller::onReset(RTC::UniqueId ec_id)
00273   {
00274   return RTC::RTC_OK;
00275   }
00276 */
00277 
00278 /*
00279   RTC::ReturnCode_t PDcontroller::onStateUpdate(RTC::UniqueId ec_id)
00280   {
00281   return RTC::RTC_OK;
00282   }
00283 */
00284 
00285 /*
00286   RTC::ReturnCode_t PDcontroller::onRateChanged(RTC::UniqueId ec_id)
00287   {
00288   return RTC::RTC_OK;
00289   }
00290 */
00291 
00292 extern "C"
00293 {
00294 
00295   void PDcontrollerInit(RTC::Manager* manager)
00296   {
00297     RTC::Properties profile(PDcontroller_spec);
00298     manager->registerFactory(profile,
00299                              RTC::Create<PDcontroller>,
00300                              RTC::Delete<PDcontroller>);
00301   }
00302 
00303 };
00304 
00305 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55