Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00019 #include "PD_HGtest.h"
00020
00021 #include <iostream>
00022
00023 #define DOF (2)
00024 #define TIMESTEP 0.002
00025
00026 #define WAIST_FILE "etc/root.dat"
00027
00028 namespace {
00029 const bool CONTROLLER_BRIDGE_DEBUG = false;
00030 }
00031
00032
00033
00034
00035 static const char* PD_HGtest_spec[] =
00036 {
00037 "implementation_id", "PD_HGtest",
00038 "type_name", "PD_HGtest",
00039 "description", "Sample PD component",
00040 "version", "0.1",
00041 "vendor", "AIST",
00042 "category", "Generic",
00043 "activity_type", "DataFlowComponent",
00044 "max_instance", "10",
00045 "language", "C++",
00046 "lang_type", "compile",
00047
00048
00049 ""
00050 };
00051
00052
00053 PD_HGtest::PD_HGtest(RTC::Manager* manager)
00054 : RTC::DataFlowComponentBase(manager),
00055
00056 m_torque0Out("torque0", m_torque0),
00057 m_torque1Out("torque1", m_torque1),
00058 m_root_transOut("root_trans", m_root_trans),
00059 m_root_velOut("root_vel", m_root_vel),
00060 m_root_accOut("root_acc", m_root_acc)
00061
00062 {
00063 if( CONTROLLER_BRIDGE_DEBUG )
00064 {
00065 std::cout << "PD_HGtest::PD_HGtest" << std::endl;
00066 }
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 }
00079
00080 PD_HGtest::~PD_HGtest()
00081 {
00082 closeFiles();
00083 }
00084
00085
00086 RTC::ReturnCode_t PD_HGtest::onInitialize()
00087 {
00088
00089
00090 if( CONTROLLER_BRIDGE_DEBUG )
00091 {
00092 std::cout << "onInitialize" << std::endl;
00093 }
00094
00095
00096
00097
00098 addOutPort("torque0", m_torque0Out);
00099 addOutPort("torque1", m_torque1Out);
00100 addOutPort("root_trans", m_root_transOut);
00101 addOutPort("root_vel", m_root_velOut);
00102 addOutPort("root_acc", m_root_accOut);
00103
00104
00105
00106 m_torque0.data.length(1);
00107 m_torque1.data.length(1);
00108 m_root_vel.data.length(6);
00109 m_root_acc.data.length(6);
00110
00111 return RTC::RTC_OK;
00112 }
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137 RTC::ReturnCode_t PD_HGtest::onActivated(RTC::UniqueId ec_id)
00138 {
00139 std::cout << "on Activated" << std::endl;
00140 openFiles();
00141
00142 return RTC::RTC_OK;
00143 }
00144
00145
00146 RTC::ReturnCode_t PD_HGtest::onDeactivated(RTC::UniqueId ec_id)
00147 {
00148 std::cout << "on Deactivated" << std::endl;
00149 closeFiles();
00150 return RTC::RTC_OK;
00151 }
00152
00153
00154
00155 RTC::ReturnCode_t PD_HGtest::onExecute(RTC::UniqueId ec_id)
00156 {
00157 if( CONTROLLER_BRIDGE_DEBUG )
00158 {
00159 std::cout << "onExecute" << std::endl;
00160 std::string localStr;
00161 std::cin >> localStr;
00162 }
00163
00164 static double root_x_p, root_x_v, root_x_a;
00165 if(!waist.eof()){
00166 waist >> root_x_p;
00167 waist >> root_x_a;
00168 waist >> root_x_v;
00169 waist >> root_x_p;
00170 }
00171
00172 m_torque0.data[0] = 0.0;
00173 m_torque1.data[0] = 0.0;
00174
00175 m_root_trans.data.position.x = root_x_p;
00176 m_root_trans.data.position.y = 0;
00177 m_root_trans.data.position.z = 1;
00178 m_root_trans.data.orientation.r = 0;
00179 m_root_trans.data.orientation.p = 0;
00180 m_root_trans.data.orientation.y = 0;
00181 for(int i=0; i<6; i++)
00182 m_root_vel.data[i] = 0.0;
00183 m_root_vel.data[0] = root_x_v;
00184 for(int i=0; i<6; i++)
00185 m_root_acc.data[i] = 0.0;
00186 m_root_acc.data[0] = root_x_a;
00187
00188 m_torque0Out.write();
00189 m_torque1Out.write();
00190 m_root_transOut.write();
00191 m_root_velOut.write();
00192 m_root_accOut.write();
00193
00194 return RTC::RTC_OK;
00195 }
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233 void PD_HGtest::openFiles()
00234 {
00235 waist.open(WAIST_FILE);
00236 if (!waist.is_open())
00237 {
00238 std::cerr << WAIST_FILE << " not opened" << std::endl;
00239 }
00240 }
00241
00242 void PD_HGtest::closeFiles()
00243 {
00244 if(waist.is_open()){
00245 waist.close();
00246 waist.clear();
00247 }
00248 }
00249
00250
00251 extern "C"
00252 {
00253
00254 DLL_EXPORT void PD_HGtestInit(RTC::Manager* manager)
00255 {
00256 coil::Properties profile(PD_HGtest_spec);
00257 manager->registerFactory(profile,
00258 RTC::Create<PD_HGtest>,
00259 RTC::Delete<PD_HGtest>);
00260 }
00261
00262 };
00263