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 #ifndef SAMPLEPD_H 00020 #define SAMPLEPD_H 00021 #define DOF (29) 00022 00023 #include <rtm/idl/BasicDataType.hh> 00024 #include <rtm/Manager.h> 00025 #include <rtm/DataFlowComponentBase.h> 00026 #include <rtm/CorbaPort.h> 00027 #include <rtm/DataInPort.h> 00028 #include <rtm/DataOutPort.h> 00029 #include <rtm/idl/BasicDataTypeSkel.h> 00030 00031 #include <vector> 00032 00033 // Service implementation headers 00034 // <rtc-template block="service_impl_h"> 00035 00036 // </rtc-template> 00037 00038 // Service Consumer stub headers 00039 // <rtc-template block="consumer_stub_h"> 00040 00041 // </rtc-template> 00042 00043 using namespace RTC; 00044 00045 class SamplePD 00046 : public RTC::DataFlowComponentBase 00047 { 00048 public: 00049 SamplePD(RTC::Manager* manager); 00050 ~SamplePD(); 00051 00052 // The initialize action (on CREATED->ALIVE transition) 00053 // formaer rtc_init_entry() 00054 virtual RTC::ReturnCode_t onInitialize(); 00055 00056 // The finalize action (on ALIVE->END transition) 00057 // formaer rtc_exiting_entry() 00058 //virtual RTC::ReturnCode_t onFinalize(); 00059 00060 // The startup action when ExecutionContext startup 00061 // former rtc_starting_entry() 00062 //virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); 00063 00064 // The shutdown action when ExecutionContext stop 00065 // former rtc_stopping_entry() 00066 //virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); 00067 00068 // The activated action (Active state entry action) 00069 // former rtc_active_entry() 00070 virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); 00071 00072 // The deactivated action (Active state exit action) 00073 // former rtc_active_exit() 00074 virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); 00075 00076 // The execution action that is invoked periodically 00077 // former rtc_active_do() 00078 virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); 00079 00080 // The aborting action when main logic error occurred. 00081 // former rtc_aborting_entry() 00082 //virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); 00083 00084 // The error action in ERROR state 00085 // former rtc_error_do() 00086 //virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); 00087 00088 // The reset action that is invoked resetting 00089 // This is same but different the former rtc_init_entry() 00090 //virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); 00091 00092 // The state update action that is invoked after onExecute() action 00093 // no corresponding operation exists in OpenRTm-aist-0.2.0 00094 //virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); 00095 00096 // The action that is invoked when execution context's rate is changed 00097 // no corresponding operation exists in OpenRTm-aist-0.2.0 00098 //virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); 00099 00100 00101 protected: 00102 // Configuration variable declaration 00103 // <rtc-template block="config_declare"> 00104 00105 // </rtc-template> 00106 00107 // DataInPort declaration 00108 // <rtc-template block="inport_declare"> 00109 TimedDoubleSeq m_angle; 00110 InPort<TimedDoubleSeq> m_angleIn; 00111 00112 // </rtc-template> 00113 00114 // DataOutPort declaration 00115 // <rtc-template block="outport_declare"> 00116 TimedDoubleSeq m_torque; 00117 OutPort<TimedDoubleSeq> m_torqueOut; 00118 00119 // </rtc-template> 00120 00121 // CORBA Port declaration 00122 // <rtc-template block="corbaport_declare"> 00123 00124 // </rtc-template> 00125 00126 // Service declaration 00127 // <rtc-template block="service_declare"> 00128 00129 // </rtc-template> 00130 00131 // Consumer declaration 00132 // <rtc-template block="consumer_declare"> 00133 00134 // </rtc-template> 00135 00136 private: 00137 int dummy; 00138 std::ifstream angle, vel, gain; 00139 double *Pgain; 00140 double *Dgain; 00141 std::vector<double> qold; 00142 double q_ref[DOF], dq_ref[DOF]; 00143 void openFiles(); 00144 void closeFiles(); 00145 }; 00146 00147 00148 extern "C" 00149 { 00150 DLL_EXPORT void SamplePDInit(RTC::Manager* manager); 00151 }; 00152 00153 #endif // SAMPLEPD_H