SampleLF.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  * General Robotix Inc.
10  */
19 #include "SampleLF.h"
20 
21 #include <iostream>
22 #include <math.h>
23 
24 #define DOF (29)
25 #define TIMESTEP 0.002
26 
27 #define GAIN_FILE "etc/PDgain.dat"
28 
29 
30 namespace {
31  const bool CONTROLLER_BRIDGE_DEBUG = false;
32 }
33 
34 
35 // Module specification
36 // <rtc-template block="module_spec">
37 static const char* samplepd_spec[] =
38  {
39  "implementation_id", "SampleLF",
40  "type_name", "SampleLF",
41  "description", "Sample LF component",
42  "version", "0.1",
43  "vendor", "AIST",
44  "category", "Generic",
45  "activity_type", "DataFlowComponent",
46  "max_instance", "10",
47  "language", "C++",
48  "lang_type", "compile",
49  // Configuration variables
50 
51  ""
52  };
53 // </rtc-template>
54 
56  : RTC::DataFlowComponentBase(manager),
57  // <rtc-template block="initializer">
58  m_angleIn("angle", m_angle),
59  m_torqueInL("r_torque_out",m_torqueL),
60  m_torqueInR("l_torque_out",m_torqueR),
61  m_torqueOut("torque", m_torque),
62 
63  // </rtc-template>
64  dummy(0),
65  qold(DOF),
66  qref_old(DOF)
67 {
68  if( CONTROLLER_BRIDGE_DEBUG )
69  {
70  std::cout << "SampleLF::SampleLF" << std::endl;
71  }
72  // Registration: InPort/OutPort/Service
73  // <rtc-template block="registration">
74 
75  // Set service provider to Ports
76 
77  // Set service consumers to Ports
78 
79  // Set CORBA Service Ports
80 
81  // </rtc-template>
82 }
83 
85 {
86  closeFiles();
87  delete [] Pgain;
88  delete [] Dgain;
89 }
90 
91 
92 RTC::ReturnCode_t SampleLF::onInitialize()
93 {
94  // <rtc-template block="bind_config">
95  // Bind variables and configuration variable
96  if( CONTROLLER_BRIDGE_DEBUG )
97  {
98  std::cout << "onInitialize" << std::endl;
99  }
100 
101  // Set InPort buffers
102  addInPort("angle", m_angleIn);
103  addInPort("r_torque_out", m_torqueInL);
104  addInPort("l_torque_out", m_torqueInR);
105 
106  // Set OutPort buffer
107  addOutPort("torque", m_torqueOut);
108  // </rtc-template>
109 
110  Pgain = new double[DOF];
111  Dgain = new double[DOF];
112 
113  gain.open(GAIN_FILE);
114  if (gain.is_open()){
115  for (int i=0; i<DOF; i++){
116  gain >> Pgain[i];
117  gain >> Dgain[i];
118  }
119  gain.close();
120  }else{
121  std::cerr << GAIN_FILE << " not opened" << std::endl;
122  }
123  m_torque.data.length(DOF);
124  m_torqueL.data.length(1);
125  m_torqueR.data.length(1);
126  m_angle.data.length(DOF);
127 
128  return RTC::RTC_OK;
129 }
130 
131 
132 /*
133 RTC::ReturnCode_t SampleLF::onFinalize()
134 {
135  return RTC::RTC_OK;
136 }
137 */
138 
139 /*
140 RTC::ReturnCode_t SampleLF::onStartup(RTC::UniqueId ec_id)
141 {
142  return RTC::RTC_OK;
143 }
144 */
145 
146 /*
147 RTC::ReturnCode_t SampleLF::onShutdown(RTC::UniqueId ec_id)
148 {
149  return RTC::RTC_OK;
150 }
151 */
152 
153 RTC::ReturnCode_t SampleLF::onActivated(RTC::UniqueId ec_id)
154 {
155 
156  std::cout << "on Activated" << std::endl;
157  check = true;
158  file = 1;
159 
160  openFiles();
161 
162  if(m_angleIn.isNew()){
163  m_angleIn.read();
164  }
165 
166  for(int i=0; i < DOF; ++i){
167  qold[i] = m_angle.data[i];
168  qref_old[i] = 0.0;
169  }
170 
171  return RTC::RTC_OK;
172 }
173 
174 
175 RTC::ReturnCode_t SampleLF::onDeactivated(RTC::UniqueId ec_id)
176 {
177  std::cout << "on Deactivated" << std::endl;
178  closeFiles();
179  return RTC::RTC_OK;
180 }
181 
182 
183 
184 RTC::ReturnCode_t SampleLF::onExecute(RTC::UniqueId ec_id)
185 {
186  if( CONTROLLER_BRIDGE_DEBUG )
187  {
188  std::cout << "SampleLF::onExecute" << std::endl;
189  }
190 
191  // この関数の振る舞いはController_impl::controlの派生先仮想関数に対応する //
192  if(m_angleIn.isNew()){
193  m_angleIn.read();
194  }
195 
196  if(m_torqueInL.isNew()){
197  m_torqueInL.read();
198  }
199 
200  if(m_torqueInR.isNew()){
201  m_torqueInR.read();
202  }
203 
204  double q_ref, dq_ref;
205  double threshold = 30.0;
206 
207 
208  // *.datの読み込み //
209  // 行頭の時間データのスキップと行の存在チェックを兼ねた処理 //
210  // 行が存在しなければ次の行を読み込む //
211  if(file==1)
212  {
213  if( !(angle1 >> dq_ref && vel1 >> dq_ref) )// skip time
214  {
215  file=2;
216  }
217  }
218 
219  if(file==2)
220  {
221  if( !(angle2 >> dq_ref && vel2 >> dq_ref) )// skip time
222  {
223  file=3;
224  }
225 
226  if( ( fabs( m_torqueR.data[0] ) < threshold || fabs( m_torqueL.data[0] ) < threshold ) && check )
227  {
228  file=3;
229  }
230 
231  check = false;
232  }
233 
234  // *.dat一行の読み込み//
235  for (int i=0; i<DOF; i++)
236  {
237  switch(file)
238  {
239  case 1:
240  angle1 >> q_ref;
241  vel1 >> dq_ref;
242  break;
243  case 2:
244  angle2 >> q_ref;
245  vel2 >> dq_ref;
246  break;
247  case 3:
248  q_ref = qref_old[i];
249  dq_ref=0.0;
250  break;
251  }//switch(file)
252  double q = m_angle.data[i];
253  double dq = (q - qold[i]) / TIMESTEP;
254 
255  m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i];
256 
257  qold[i] = q;
258  if(file !=3)
259  qref_old[i] = q_ref;
260  }//for (int i=0; i<DOF; i++)
261 
262  m_torqueOut.write();
263 
264  return RTC::RTC_OK;
265 }
266 
267 
268 /*
269  RTC::ReturnCode_t SampleLF::onAborting(RTC::UniqueId ec_id)
270  {
271  return RTC::RTC_OK;
272  }
273 */
274 
275 /*
276  RTC::ReturnCode_t SampleLF::onError(RTC::UniqueId ec_id)
277  {
278  return RTC::RTC_OK;
279  }
280 */
281 
282 /*
283  RTC::ReturnCode_t SampleLF::onReset(RTC::UniqueId ec_id)
284  {
285  return RTC::RTC_OK;
286  }
287 */
288 
289 /*
290  RTC::ReturnCode_t SampleLF::onStateUpdate(RTC::UniqueId ec_id)
291  {
292  return RTC::RTC_OK;
293  }
294 */
295 
296 /*
297  RTC::ReturnCode_t SampleLF::onRateChanged(RTC::UniqueId ec_id)
298  {
299  return RTC::RTC_OK;
300  }
301 */
303 {
304  angle1.open("etc/LFangle1.dat");
305  if (!angle1.is_open()){
306  std::cerr << "etc/LFangle1.dat not opened" << std::endl;
307  }
308 
309  angle2.open("etc/LFangle2.dat");
310  if (!angle2.is_open()){
311  std::cerr << "etc/LFangle2.dat not opened" << std::endl;
312  }
313 
314  vel1.open("etc/LFvel1.dat");
315  if (!vel1.is_open()){
316  std::cerr << "etc/LFvel1.dat not opened" << std::endl;
317  }
318 
319  vel2.open("etc/LFvel2.dat");
320  if (!vel2.is_open()){
321  std::cerr << "etc/LFvel2.dat not opened" << std::endl;
322  }
323 }
324 
326 {
327  if( angle1.is_open() ){
328  angle1.close();
329  angle1.clear();
330  }
331  if( angle2.is_open() ){
332  angle2.close();
333  angle2.clear();
334  }
335  if( vel1.is_open() ){
336  vel1.close();
337  vel1.clear();
338  }
339  if( vel2.is_open() ){
340  vel2.close();
341  vel2.clear();
342  }
343 }
344 
345 
346 
347 extern "C"
348 {
349 
351  {
353  manager->registerFactory(profile,
354  RTC::Create<SampleLF>,
355  RTC::Delete<SampleLF>);
356  }
357 
358 };
359 
png_infop png_charpp int png_charpp profile
Definition: png.h:2382
SampleLF(RTC::Manager *manager)
Definition: SampleLF.cpp:55
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
Definition: SampleLF.cpp:175
std::ifstream vel1
Definition: SampleLF.h:144
std::ifstream angle1
Definition: SampleLF.h:144
std::vector< double > qold
Definition: SampleLF.h:150
TimedDoubleSeq m_angle
Definition: SampleLF.h:108
#define TIMESTEP
Definition: SampleLF.cpp:25
static const char * samplepd_spec[]
Definition: SampleLF.cpp:37
#define DOF
Definition: SampleLF.cpp:24
png_uint_32 i
Definition: png.h:2735
#define GAIN_FILE
Definition: SampleLF.cpp:27
void openFiles()
Definition: SampleLF.cpp:302
DLL_EXPORT void SampleLFInit(RTC::Manager *manager)
Definition: SampleLF.cpp:350
bool addOutPort(const char *name, OutPortBase &outport)
InPort< TimedDoubleSeq > m_angleIn
Definition: SampleLF.h:109
void closeFiles()
Definition: SampleLF.cpp:325
TimedDoubleSeq m_torque
Definition: SampleLF.h:121
ExecutionContextHandle_t UniqueId
InPort< TimedDoubleSeq > m_torqueInL
Definition: SampleLF.h:112
OutPort< TimedDoubleSeq > m_torqueOut
Definition: SampleLF.h:122
std::ifstream vel2
Definition: SampleLF.h:144
InPort< TimedDoubleSeq > m_torqueInR
Definition: SampleLF.h:115
TimedDoubleSeq m_torqueL
Definition: SampleLF.h:111
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
Definition: SampleLF.cpp:153
~SampleLF()
Definition: SampleLF.cpp:84
NewSample LF component.
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Definition: SampleLF.cpp:184
virtual bool isNew()
virtual bool write(DataType &value)
std::vector< double > qref_old
Definition: SampleLF.h:150
std::ifstream angle2
Definition: SampleLF.h:144
std::ifstream gain
Definition: SampleLF.h:144
bool addInPort(const char *name, InPortBase &inport)
TimedDoubleSeq m_torqueR
Definition: SampleLF.h:114
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
double * Pgain
Definition: SampleLF.h:148
double * Dgain
Definition: SampleLF.h:149
#define DLL_EXPORT
int file
Definition: SampleLF.h:146
bool check
Definition: SampleLF.h:145
virtual RTC::ReturnCode_t onInitialize()
Definition: SampleLF.cpp:92


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05