SamplePD_HG.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 "SamplePD_HG.h"
20 
21 #include <iostream>
22 
23 #define PD_DOF (17)
24 #define HG_DOF (12)
25 #define TIMESTEP 0.002
26 
27 #define WAIST_P 26
28 #define WAIST_R 27
29 #define CHEST 28
30 #define LARM_SHOULDER_P 19
31 #define LARM_SHOULDER_R 20
32 #define LARM_SHOULDER_Y 21
33 #define LARM_ELBOW 22
34 #define LARM_WRIST_Y 23
35 #define LARM_WRIST_P 24
36 #define LARM_WRIST_R 25
37 #define RARM_SHOULDER_P 6
38 #define RARM_SHOULDER_R 7
39 #define RARM_SHOULDER_Y 8
40 #define RARM_ELBOW 9
41 #define RARM_WRIST_Y 10
42 #define RARM_WRIST_P 11
43 #define RARM_WRIST_R 12
44 #define LLEG_HIP_R 13
45 #define LLEG_HIP_P 14
46 #define LLEG_HIP_Y 15
47 #define LLEG_KNEE 16
48 #define LLEG_ANKLE_P 17
49 #define LLEG_ANKLE_R 18
50 #define RLEG_HIP_R 0
51 #define RLEG_HIP_P 1
52 #define RLEG_HIP_Y 2
53 #define RLEG_KNEE 3
54 #define RLEG_ANKLE_P 4
55 #define RLEG_ANKLE_R 5
56 
57 #define ANGLE_FILE "etc/angle.dat"
58 #define VEL_FILE "etc/vel.dat"
59 #define ACC_FILE "etc/acc.dat"
60 
61 #define GAIN_FILE "etc/PDgain.dat"
62 
63 namespace {
64  const bool CONTROLLER_BRIDGE_DEBUG = false;
65 }
66 
67 
68 // Module specification
69 // <rtc-template block="module_spec">
70 static const char* SamplePD_HG_spec[] =
71  {
72  "implementation_id", "SamplePD_HG",
73  "type_name", "SamplePD_HG",
74  "description", "Sample PD component",
75  "version", "0.1",
76  "vendor", "AIST",
77  "category", "Generic",
78  "activity_type", "DataFlowComponent",
79  "max_instance", "10",
80  "language", "C++",
81  "lang_type", "compile",
82  // Configuration variables
83 
84  ""
85  };
86 // </rtc-template>
87 
89  : RTC::DataFlowComponentBase(manager),
90  // <rtc-template block="initializer">
91  m_angle_inIn("angle_in", m_angle_in),
92  m_torqueOut("torque", m_torque),
93  m_angle_outOut("angle_out", m_angle_out),
94  m_velOut("vel", m_vel),
95  m_accOut("acc", m_acc),
96 
97  // </rtc-template>
98  dummy(0),
99  qold(DOF)
100 {
101  if( CONTROLLER_BRIDGE_DEBUG )
102  {
103  std::cout << "SamplePD_HG::SamplePD_HG" << std::endl;
104  }
105 
106  // Registration: InPort/OutPort/Service
107  // <rtc-template block="registration">
108 
109  // Set service provider to Ports
110 
111  // Set service consumers to Ports
112 
113  // Set CORBA Service Ports
114 
115  // </rtc-template>
116 }
117 
119 {
120  closeFiles();
121  delete [] Pgain;
122  delete [] Dgain;
123 }
124 
125 
126 RTC::ReturnCode_t SamplePD_HG::onInitialize()
127 {
128  // <rtc-template block="bind_config">
129  // Bind variables and configuration variable
130  if( CONTROLLER_BRIDGE_DEBUG )
131  {
132  std::cout << "onInitialize" << std::endl;
133  }
134 
135  // Set InPort buffers
136  addInPort("angle_in", m_angle_inIn);
137 
138  // Set OutPort buffer
139  addOutPort("torque", m_torqueOut);
140  addOutPort("angle_out", m_angle_outOut);
141  addOutPort("vel", m_velOut);
142  addOutPort("acc", m_accOut);
143 
144  Pgain = new double[DOF];
145  Dgain = new double[DOF];
146 
147  gain.open(GAIN_FILE);
148  if (gain.is_open()){
149  for (int i=0; i<DOF; i++){
150  gain >> Pgain[i];
151  gain >> Dgain[i];
152  }
153  gain.close();
154  }else{
155  std::cerr << GAIN_FILE << " not opened" << std::endl;
156  }
157  // </rtc-template>
158 
159  m_angle_in.data.length(DOF);
160  m_angle_out.data.length(HG_DOF);
161  m_vel.data.length(HG_DOF);
162  m_acc.data.length(HG_DOF);
163  m_torque.data.length(PD_DOF);
164 
165  return RTC::RTC_OK;
166 }
167 
168 
169 
170 /*
171 RTC::ReturnCode_t SamplePD_HG::onFinalize()
172 {
173  return RTC::RTC_OK;
174 }
175 */
176 
177 /*
178 RTC::ReturnCode_t SamplePD_HG::onStartup(RTC::UniqueId ec_id)
179 {
180  return RTC::RTC_OK;
181 }
182 */
183 
184 /*
185 RTC::ReturnCode_t SamplePD_HG::onShutdown(RTC::UniqueId ec_id)
186 {
187  return RTC::RTC_OK;
188 }
189 */
190 
191 RTC::ReturnCode_t SamplePD_HG::onActivated(RTC::UniqueId ec_id)
192 {
193  std::cout << "on Activated" << std::endl;
194  openFiles();
195 
196  if(m_angle_inIn.isNew()){
197  m_angle_inIn.read();
198  }
199 
200  for(int i=0; i < DOF; ++i){
201  qold[i] = m_angle_in.data[i];
202  q_ref[i] = dq_ref[i] = ddq_ref[i] = 0.0;
203  }
204 
205  return RTC::RTC_OK;
206 }
207 
208 
210 {
211  std::cout << "on Deactivated" << std::endl;
212  closeFiles();
213  return RTC::RTC_OK;
214 }
215 
216 
217 
218 RTC::ReturnCode_t SamplePD_HG::onExecute(RTC::UniqueId ec_id)
219 {
220  if( CONTROLLER_BRIDGE_DEBUG )
221  {
222  std::cout << "onExecute" << std::endl;
223  std::string localStr;
224  std::cin >> localStr;
225  }
226 
227  if(m_angle_inIn.isNew()){
228  m_angle_inIn.read();
229  }
230 
231  if(!angle.eof()){
232  angle >> q_ref[0]; vel >> dq_ref[0]; acc >> ddq_ref[0];// skip time
233  for (int i=0; i<DOF; i++){
234  angle >> q_ref[i];
235  vel >> dq_ref[i];
236  acc >> ddq_ref[i];
237  }
238  }
239 
240  double tor_ref[DOF];
241  for(int i=0; i<DOF; i++){
242  double q = m_angle_in.data[i];
243  double dq = (q - qold[i]) / TIMESTEP;
244  qold[i] = q;
245 
246  tor_ref[i] = -(q - q_ref[i]) * Pgain[i] - (dq - dq_ref[i]) * Dgain[i];
247  }
248 
249  m_torque.data[0] = tor_ref[WAIST_P];
250  m_torque.data[1] = tor_ref[WAIST_R];
251  m_torque.data[2] = tor_ref[CHEST];
252  m_torque.data[3] = tor_ref[LARM_SHOULDER_P];
253  m_torque.data[4] = tor_ref[LARM_SHOULDER_R];
254  m_torque.data[5] = tor_ref[LARM_SHOULDER_Y];
255  m_torque.data[6] = tor_ref[LARM_ELBOW];
256  m_torque.data[7] = tor_ref[LARM_WRIST_Y];
257  m_torque.data[8] = tor_ref[LARM_WRIST_P];
258  m_torque.data[9] = tor_ref[LARM_WRIST_R];
259  m_torque.data[10] = tor_ref[RARM_SHOULDER_P];
260  m_torque.data[11] = tor_ref[RARM_SHOULDER_R];
261  m_torque.data[12] = tor_ref[RARM_SHOULDER_Y];
262  m_torque.data[13] = tor_ref[RARM_ELBOW];
263  m_torque.data[14] = tor_ref[RARM_WRIST_Y];
264  m_torque.data[15] = tor_ref[RARM_WRIST_P];
265  m_torque.data[16] = tor_ref[RARM_WRIST_R];
266 
267  m_angle_out.data[0] = q_ref[LLEG_HIP_R];
268  m_angle_out.data[1] = q_ref[LLEG_HIP_P];
269  m_angle_out.data[2] = q_ref[LLEG_HIP_Y];
270  m_angle_out.data[3] = q_ref[LLEG_KNEE];
271  m_angle_out.data[4] = q_ref[LLEG_ANKLE_P];
272  m_angle_out.data[5] = q_ref[LLEG_ANKLE_R];
273  m_angle_out.data[6] = q_ref[RLEG_HIP_R];
274  m_angle_out.data[7] = q_ref[RLEG_HIP_P];
275  m_angle_out.data[8] = q_ref[RLEG_HIP_Y];
276  m_angle_out.data[9] = q_ref[RLEG_KNEE];
277  m_angle_out.data[10] = q_ref[RLEG_ANKLE_P];
278  m_angle_out.data[11] = q_ref[RLEG_ANKLE_R];
279  m_vel.data[0] = dq_ref[LLEG_HIP_R];
280  m_vel.data[1] = dq_ref[LLEG_HIP_P];
281  m_vel.data[2] = dq_ref[LLEG_HIP_Y];
282  m_vel.data[3] = dq_ref[LLEG_KNEE];
283  m_vel.data[4] = dq_ref[LLEG_ANKLE_P];
284  m_vel.data[5] = dq_ref[LLEG_ANKLE_R];
285  m_vel.data[6] = dq_ref[RLEG_HIP_R];
286  m_vel.data[7] = dq_ref[RLEG_HIP_P];
287  m_vel.data[8] = dq_ref[RLEG_HIP_Y];
288  m_vel.data[9] = dq_ref[RLEG_KNEE];
289  m_vel.data[10] = dq_ref[RLEG_ANKLE_P];
290  m_vel.data[11] = dq_ref[RLEG_ANKLE_R];
291  m_acc.data[0] = ddq_ref[LLEG_HIP_R];
292  m_acc.data[1] = ddq_ref[LLEG_HIP_P];
293  m_acc.data[2] = ddq_ref[LLEG_HIP_Y];
294  m_acc.data[3] = ddq_ref[LLEG_KNEE];
295  m_acc.data[4] = ddq_ref[LLEG_ANKLE_P];
296  m_acc.data[5] = ddq_ref[LLEG_ANKLE_R];
297  m_acc.data[6] = ddq_ref[RLEG_HIP_R];
298  m_acc.data[7] = ddq_ref[RLEG_HIP_P];
299  m_acc.data[8] = ddq_ref[RLEG_HIP_Y];
300  m_acc.data[9] = ddq_ref[RLEG_KNEE];
301  m_acc.data[10] = ddq_ref[RLEG_ANKLE_P];
302  m_acc.data[11] = ddq_ref[RLEG_ANKLE_R];
303 
304  m_torqueOut.write();
306  m_velOut.write();
307  m_accOut.write();
308 
309  return RTC::RTC_OK;
310 }
311 
312 
313 /*
314  RTC::ReturnCode_t SamplePD_HG::onAborting(RTC::UniqueId ec_id)
315  {
316  return RTC::RTC_OK;
317  }
318 */
319 
320 /*
321  RTC::ReturnCode_t SamplePD_HG::onError(RTC::UniqueId ec_id)
322  {
323  return RTC::RTC_OK;
324  }
325 */
326 
327 /*
328  RTC::ReturnCode_t SamplePD_HG::onReset(RTC::UniqueId ec_id)
329  {
330  return RTC::RTC_OK;
331  }
332 */
333 
334 /*
335  RTC::ReturnCode_t SamplePD_HG::onStateUpdate(RTC::UniqueId ec_id)
336  {
337  return RTC::RTC_OK;
338  }
339 */
340 
341 /*
342  RTC::ReturnCode_t SamplePD_HG::onRateChanged(RTC::UniqueId ec_id)
343  {
344  return RTC::RTC_OK;
345  }
346 */
347 
349 {
350  angle.open(ANGLE_FILE);
351  if (!angle.is_open()){
352  std::cerr << ANGLE_FILE << " not opened" << std::endl;
353  }
354 
355  vel.open(VEL_FILE);
356  if (!vel.is_open()){
357  std::cerr << VEL_FILE << " not opened" << std::endl;
358  }
359 
360  acc.open(ACC_FILE);
361  if (!acc.is_open())
362  {
363  std::cerr << ACC_FILE << " not opend" << std::endl;
364  }
365 }
366 
368 {
369  if( angle.is_open() ){
370  angle.close();
371  angle.clear();
372  }
373  if( vel.is_open() ){
374  vel.close();
375  vel.clear();
376  }
377  if( acc.is_open() ){
378  acc.close();
379  acc.clear();
380  }
381 }
382 
383 
384 extern "C"
385 {
386 
388  {
390  manager->registerFactory(profile,
391  RTC::Create<SamplePD_HG>,
392  RTC::Delete<SamplePD_HG>);
393  }
394 
395 };
396 
png_infop png_charpp int png_charpp profile
Definition: png.h:2382
virtual RTC::ReturnCode_t onInitialize()
TimedDoubleSeq m_vel
Definition: SamplePD_HG.h:119
static const char * SamplePD_HG_spec[]
Definition: SamplePD_HG.cpp:70
#define RARM_WRIST_R
Definition: SamplePD_HG.cpp:43
DLL_EXPORT void SamplePD_HGInit(RTC::Manager *manager)
Sample PD component.
#define LLEG_HIP_Y
Definition: SamplePD_HG.cpp:46
double * Pgain
Definition: SamplePD_HG.h:146
#define RLEG_ANKLE_R
Definition: SamplePD_HG.cpp:55
#define LARM_ELBOW
Definition: SamplePD_HG.cpp:33
double dq_ref[DOF]
Definition: SamplePD_HG.h:149
#define WAIST_R
Definition: SamplePD_HG.cpp:28
#define RARM_WRIST_Y
Definition: SamplePD_HG.cpp:41
void openFiles()
#define LLEG_ANKLE_R
Definition: SamplePD_HG.cpp:49
png_uint_32 i
Definition: png.h:2735
#define RLEG_ANKLE_P
Definition: SamplePD_HG.cpp:54
TimedDoubleSeq m_acc
Definition: SamplePD_HG.h:121
bool addOutPort(const char *name, OutPortBase &outport)
OutPort< TimedDoubleSeq > m_angle_outOut
Definition: SamplePD_HG.h:118
#define RLEG_HIP_R
Definition: SamplePD_HG.cpp:50
#define DOF
#define RARM_SHOULDER_P
Definition: SamplePD_HG.cpp:37
#define LARM_SHOULDER_P
Definition: SamplePD_HG.cpp:30
std::vector< double > qold
Definition: SamplePD_HG.h:148
TimedDoubleSeq m_angle_out
Definition: SamplePD_HG.h:117
#define RARM_WRIST_P
Definition: SamplePD_HG.cpp:42
#define TIMESTEP
Definition: SamplePD_HG.cpp:25
ExecutionContextHandle_t UniqueId
#define LARM_SHOULDER_R
Definition: SamplePD_HG.cpp:31
#define RARM_SHOULDER_R
Definition: SamplePD_HG.cpp:38
#define LARM_WRIST_R
Definition: SamplePD_HG.cpp:36
#define RLEG_HIP_Y
Definition: SamplePD_HG.cpp:52
std::ifstream acc
Definition: SamplePD_HG.h:145
double q_ref[DOF]
Definition: SamplePD_HG.h:149
#define LARM_WRIST_Y
Definition: SamplePD_HG.cpp:34
InPort< TimedDoubleSeq > m_angle_inIn
Definition: SamplePD_HG.h:111
OutPort< TimedDoubleSeq > m_torqueOut
Definition: SamplePD_HG.h:124
#define ANGLE_FILE
Definition: SamplePD_HG.cpp:57
#define HG_DOF
Definition: SamplePD_HG.cpp:24
#define GAIN_FILE
Definition: SamplePD_HG.cpp:61
TimedDoubleSeq m_torque
Definition: SamplePD_HG.h:123
SamplePD_HG(RTC::Manager *manager)
Definition: SamplePD_HG.cpp:88
#define CHEST
Definition: SamplePD_HG.cpp:29
#define RARM_ELBOW
Definition: SamplePD_HG.cpp:40
#define WAIST_P
Definition: SamplePD_HG.cpp:27
void closeFiles()
virtual bool isNew()
OutPort< TimedDoubleSeq > m_accOut
Definition: SamplePD_HG.h:122
std::ifstream gain
Definition: SamplePD_HG.h:145
#define RLEG_KNEE
Definition: SamplePD_HG.cpp:53
#define LLEG_ANKLE_P
Definition: SamplePD_HG.cpp:48
virtual bool write(DataType &value)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
#define LLEG_HIP_R
Definition: SamplePD_HG.cpp:44
double * Dgain
Definition: SamplePD_HG.h:147
#define LLEG_HIP_P
Definition: SamplePD_HG.cpp:45
#define LLEG_KNEE
Definition: SamplePD_HG.cpp:47
#define RARM_SHOULDER_Y
Definition: SamplePD_HG.cpp:39
#define VEL_FILE
Definition: SamplePD_HG.cpp:58
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
#define RLEG_HIP_P
Definition: SamplePD_HG.cpp:51
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
bool addInPort(const char *name, InPortBase &inport)
double ddq_ref[DOF]
Definition: SamplePD_HG.h:149
OutPort< TimedDoubleSeq > m_velOut
Definition: SamplePD_HG.h:120
#define LARM_WRIST_P
Definition: SamplePD_HG.cpp:35
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
#define PD_DOF
Definition: SamplePD_HG.cpp:23
#define LARM_SHOULDER_Y
Definition: SamplePD_HG.cpp:32
TimedDoubleSeq m_angle_in
Definition: SamplePD_HG.h:110
std::ifstream vel
Definition: SamplePD_HG.h:145
#define ACC_FILE
Definition: SamplePD_HG.cpp:59
std::ifstream angle
Definition: SamplePD_HG.h:145
#define DLL_EXPORT


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