HGcontroller.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "HGcontroller.h"
00011 
00012 // Module specification
00013 // <rtc-template block="module_spec">
00014 static const char* hgcontroller_spec[] =
00015   {
00016     "implementation_id", "HGcontroller",
00017     "type_name",         "HGcontroller",
00018     "description",       "high gain joint controller component for OpenHRP",
00019     "version",           HRPSYS_PACKAGE_VERSION,
00020     "vendor",            "AIST",
00021     "category",          "example",
00022     "activity_type",     "DataFlowComponent",
00023     "max_instance",      "10",
00024     "language",          "C++",
00025     "lang_type",         "compile",
00026     // Configuration variables
00027     ""
00028   };
00029 // </rtc-template>
00030 
00031 HGcontroller::HGcontroller(RTC::Manager* manager)
00032   : RTC::DataFlowComponentBase(manager),
00033     // <rtc-template block="initializer">
00034     m_qIn("qIn", m_q),
00035     m_qOut("qOut", m_q),
00036     m_dqOut("dqOut", m_dq),
00037     m_ddqOut("ddqOut", m_ddq),
00038     // </rtc-template>
00039     m_dt(0.005),
00040         dummy(0)
00041 {
00042 }
00043 
00044 HGcontroller::~HGcontroller()
00045 {
00046 }
00047 
00048 
00049 
00050 RTC::ReturnCode_t HGcontroller::onInitialize()
00051 {
00052   std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00053 
00054   RTC::Properties& ref = getProperties();
00055   coil::stringTo(m_dt, ref["dt"].c_str());
00056 
00057   // <rtc-template block="bind_config">
00058   // Bind variables and configuration variable
00059   
00060   // </rtc-template>
00061 
00062   // Registration: InPort/OutPort/Service
00063   // <rtc-template block="registration">
00064   // Set InPort buffers
00065   addInPort("qIn", m_qIn);
00066 
00067   // Set OutPort buffer
00068   addOutPort("qOut", m_qOut);
00069   addOutPort("dqOut", m_dqOut);
00070   addOutPort("ddqOut", m_ddqOut);
00071   
00072   // Set service provider to Ports
00073   
00074   // Set service consumers to Ports
00075   
00076   // Set CORBA Service Ports
00077   
00078   // </rtc-template>
00079 
00080   return RTC::RTC_OK;
00081 }
00082 
00083 
00084 
00085 /*
00086 RTC::ReturnCode_t HGcontroller::onFinalize()
00087 {
00088   return RTC::RTC_OK;
00089 }
00090 */
00091 
00092 /*
00093 RTC::ReturnCode_t HGcontroller::onStartup(RTC::UniqueId ec_id)
00094 {
00095   return RTC::RTC_OK;
00096 }
00097 */
00098 
00099 /*
00100 RTC::ReturnCode_t HGcontroller::onShutdown(RTC::UniqueId ec_id)
00101 {
00102   return RTC::RTC_OK;
00103 }
00104 */
00105 
00106 RTC::ReturnCode_t HGcontroller::onActivated(RTC::UniqueId ec_id)
00107 {
00108   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00109   return RTC::RTC_OK;
00110 }
00111 
00112 RTC::ReturnCode_t HGcontroller::onDeactivated(RTC::UniqueId ec_id)
00113 {
00114   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00115   return RTC::RTC_OK;
00116 }
00117 
00118 RTC::ReturnCode_t HGcontroller::onExecute(RTC::UniqueId ec_id)
00119 {
00120   //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00121 
00122   if (m_qIn.isNew()) m_qIn.read();
00123 
00124   if (m_q.data.length()){
00125       if (m_dq.data.length() == 0){
00126           // initialize m_dq, m_ddq
00127           m_dq.data.length(m_q.data.length());
00128           m_ddq.data.length(m_q.data.length());
00129           for (unsigned int i=0; i<m_q.data.length(); i++){
00130               m_dq.data[i] = m_ddq.data[i] = 0.0;
00131           }
00132       }else{
00133           // compute m_dq, m_ddq
00134           for (unsigned int i=0; i<m_q.data.length(); i++){
00135               m_dq.data[i] = (m_q.data[i] - m_qOld.data[i])/m_dt;
00136               m_ddq.data[i] = (m_dq.data[i] - m_dqOld.data[i])/m_dt;
00137           }
00138       }
00139 
00140       m_qOut.write();
00141       m_dqOut.write();
00142       m_ddqOut.write();
00143 
00144       m_qOld = m_q;
00145       m_dqOld = m_dq;
00146   }
00147 
00148   return RTC::RTC_OK;
00149 }
00150 
00151 /*
00152 RTC::ReturnCode_t HGcontroller::onAborting(RTC::UniqueId ec_id)
00153 {
00154   return RTC::RTC_OK;
00155 }
00156 */
00157 
00158 /*
00159 RTC::ReturnCode_t HGcontroller::onError(RTC::UniqueId ec_id)
00160 {
00161   return RTC::RTC_OK;
00162 }
00163 */
00164 
00165 /*
00166 RTC::ReturnCode_t HGcontroller::onReset(RTC::UniqueId ec_id)
00167 {
00168   return RTC::RTC_OK;
00169 }
00170 */
00171 
00172 /*
00173 RTC::ReturnCode_t HGcontroller::onStateUpdate(RTC::UniqueId ec_id)
00174 {
00175   return RTC::RTC_OK;
00176 }
00177 */
00178 
00179 /*
00180 RTC::ReturnCode_t HGcontroller::onRateChanged(RTC::UniqueId ec_id)
00181 {
00182   return RTC::RTC_OK;
00183 }
00184 */
00185 
00186 
00187 
00188 extern "C"
00189 {
00190 
00191   void HGcontrollerInit(RTC::Manager* manager)
00192   {
00193     RTC::Properties profile(hgcontroller_spec);
00194     manager->registerFactory(profile,
00195                              RTC::Create<HGcontroller>,
00196                              RTC::Delete<HGcontroller>);
00197   }
00198 
00199 };
00200 
00201 


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