SamplePD.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * National Institute of Advanced Industrial Science and Technology (AIST)
00009  * General Robotix Inc. 
00010  */
00019 #include "SamplePD.h"
00020 
00021 #include <iostream>
00022 
00023 #define TIMESTEP 0.002
00024 
00025 #define ANGLE_FILE "etc/angle.dat"
00026 #define VEL_FILE   "etc/vel.dat"
00027 #define ACC_FILE   "etc/acc.dat"
00028 
00029 #define GAIN_FILE  "etc/PDgain.dat"
00030 
00031 namespace {
00032   const bool CONTROLLER_BRIDGE_DEBUG = false;
00033 }
00034 
00035 
00036 // Module specification
00037 // <rtc-template block="module_spec">
00038 static const char* samplepd_spec[] =
00039   {
00040     "implementation_id", "SamplePD",
00041     "type_name",         "SamplePD",
00042     "description",       "Sample PD component",
00043     "version",           "0.1",
00044     "vendor",            "AIST",
00045     "category",          "Generic",
00046     "activity_type",     "DataFlowComponent",
00047     "max_instance",      "10",
00048     "language",          "C++",
00049     "lang_type",         "compile",
00050     // Configuration variables
00051 
00052     ""
00053   };
00054 // </rtc-template>
00055 
00056 SamplePD::SamplePD(RTC::Manager* manager)
00057   : RTC::DataFlowComponentBase(manager),
00058     // <rtc-template block="initializer">
00059     m_angleIn("angle", m_angle),
00060     m_torqueOut("torque", m_torque),
00061     
00062     // </rtc-template>
00063     dummy(0),
00064     qold(DOF)
00065 {
00066   if( CONTROLLER_BRIDGE_DEBUG )
00067   {
00068     std::cout << "SamplePD::SamplePD" << std::endl;
00069   }
00070 
00071   // Registration: InPort/OutPort/Service
00072   // <rtc-template block="registration">
00073   
00074   // Set service provider to Ports
00075   
00076   // Set service consumers to Ports
00077   
00078   // Set CORBA Service Ports
00079   
00080   // </rtc-template>
00081 }
00082 
00083 SamplePD::~SamplePD()
00084 {
00085   closeFiles();
00086   delete [] Pgain;
00087   delete [] Dgain;
00088 }
00089 
00090 
00091 RTC::ReturnCode_t SamplePD::onInitialize()
00092 {
00093   // <rtc-template block="bind_config">
00094   // Bind variables and configuration variable
00095   if( CONTROLLER_BRIDGE_DEBUG )
00096   {
00097     std::cout << "onInitialize" << std::endl;
00098   }
00099 
00100   // Set InPort buffers
00101   addInPort("angle", m_angleIn);
00102   
00103   // Set OutPort buffer
00104   addOutPort("torque", m_torqueOut);
00105   // </rtc-template>
00106 
00107   Pgain = new double[DOF];
00108   Dgain = new double[DOF];
00109 
00110   gain.open(GAIN_FILE);
00111   if (gain.is_open()){
00112     for (int i=0; i<DOF; i++){
00113       gain >> Pgain[i];
00114       gain >> Dgain[i];
00115     }
00116     gain.close();
00117   }else{
00118     std::cerr << GAIN_FILE << " not found" << std::endl;
00119   }
00120   m_torque.data.length(DOF);
00121   m_angle.data.length(DOF);
00122 
00123   return RTC::RTC_OK;
00124 }
00125 
00126 
00127 /*
00128 RTC::ReturnCode_t SamplePD::onFinalize()
00129 {
00130   return RTC::RTC_OK;
00131 }
00132 */
00133 
00134 /*
00135 RTC::ReturnCode_t SamplePD::onStartup(RTC::UniqueId ec_id)
00136 {
00137   return RTC::RTC_OK;
00138 }
00139 */
00140 
00141 /*
00142 RTC::ReturnCode_t SamplePD::onShutdown(RTC::UniqueId ec_id)
00143 {
00144     log("SamplePD::onShutdown");
00145   return RTC::RTC_OK;
00146 }
00147 */
00148 
00149 RTC::ReturnCode_t SamplePD::onActivated(RTC::UniqueId ec_id)
00150 {
00151   std::cout << "on Activated" << std::endl;
00152   openFiles();
00153   
00154   if(m_angleIn.isNew()){
00155     m_angleIn.read();
00156   }
00157   
00158   for(int i=0; i < DOF; ++i){
00159     qold[i] = m_angle.data[i];
00160     q_ref[i] = dq_ref[i] = 0.0;
00161   }
00162   
00163   return RTC::RTC_OK;
00164 }
00165 
00166 
00167 RTC::ReturnCode_t SamplePD::onDeactivated(RTC::UniqueId ec_id)
00168 {
00169   std::cout << "on Deactivated" << std::endl;
00170   closeFiles();
00171   return RTC::RTC_OK;
00172 }
00173 
00174 
00175 
00176 RTC::ReturnCode_t SamplePD::onExecute(RTC::UniqueId ec_id)
00177 {
00178   if( CONTROLLER_BRIDGE_DEBUG )
00179   {
00180     std::cout << "onExecute" << std::endl;
00181     std::string localStr;
00182     std::cin >> localStr; 
00183   }
00184 
00185   if(m_angleIn.isNew()){
00186     m_angleIn.read();
00187   }
00188 
00189   if(!angle.eof()){
00190     angle >> q_ref[0]; vel >> dq_ref[0];// skip time
00191     for (int i=0; i<DOF; i++){
00192       angle >> q_ref[i];
00193       vel >> dq_ref[i];
00194     }
00195   }
00196   for(int i=0; i<DOF; i++){
00197     double q = m_angle.data[i];
00198     double dq = (q - qold[i]) / TIMESTEP;
00199     qold[i] = q;
00200     
00201     m_torque.data[i] = -(q - q_ref[i]) * Pgain[i] - (dq - dq_ref[i]) * Dgain[i];
00202   }
00203       
00204   m_torqueOut.write();
00205   
00206   return RTC::RTC_OK;
00207 }
00208 
00209 /*
00210 RTC::ReturnCode_t SamplePD::onAborting(RTC::UniqueId ec_id)
00211 {
00212     return RTC::RTC_OK;
00213 }
00214 */
00215 
00216 /*
00217 RTC::ReturnCode_t SamplePD::onError(RTC::UniqueId ec_id)
00218 {
00219     return RTC::RTC_OK;
00220 }
00221 */
00222 
00223 /*
00224 RTC::ReturnCode_t SamplePD::onReset(RTC::UniqueId ec_id)
00225 {
00226     return RTC::RTC_OK;
00227 }
00228 */
00229 
00230 /*
00231 RTC::ReturnCode_t SamplePD::onStateUpdate(RTC::UniqueId ec_id)
00232 {
00233     return RTC::RTC_OK;
00234 }
00235 */
00236 
00237 /*
00238 RTC::ReturnCode_t SamplePD::onRateChanged(RTC::UniqueId ec_id)
00239 {
00240     return RTC::RTC_OK;
00241 }
00242 */
00243 
00244 void SamplePD::openFiles()
00245 {
00246   angle.open(ANGLE_FILE);
00247   if(!angle.is_open()){
00248     std::cerr << ANGLE_FILE << " not opened" << std::endl;
00249   }
00250 
00251   vel.open(VEL_FILE);
00252   if (!vel.is_open()){
00253     std::cerr << VEL_FILE << " not opened" << std::endl;
00254   }  
00255 }
00256 
00257 void SamplePD::closeFiles()
00258 {
00259   if( angle.is_open() ){
00260     angle.close();
00261     angle.clear();
00262   }
00263   if( vel.is_open() ){
00264     vel.close();
00265     vel.clear();
00266   }
00267 }
00268 
00269 extern "C"
00270 {
00271 
00272   DLL_EXPORT void SamplePDInit(RTC::Manager* manager)
00273   {
00274     coil::Properties profile(samplepd_spec);
00275     manager->registerFactory(profile,
00276                              RTC::Create<SamplePD>,
00277                              RTC::Delete<SamplePD>);
00278   }
00279 
00280 };
00281 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19