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


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:25