PA10Controller.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 "PA10Controller.h"
20 
21 #include <iostream>
22 
23 #define TIMESTEP 0.001
24 
25 #define ANGLE_FILE "etc/angle.dat"
26 #define VEL_FILE "etc/vel.dat"
27 #define GAIN_FILE "etc/PDgain.dat"
28 
29 // Module specification
30 // <rtc-template block="module_spec">
31 static const char* PA10Controller_spec[] =
32  {
33  "implementation_id", "PA10Controller",
34  "type_name", "PA10Controller",
35  "description", "PA10Controller component",
36  "version", "0.1",
37  "vendor", "AIST",
38  "category", "Generic",
39  "activity_type", "DataFlowComponent",
40  "max_instance", "10",
41  "language", "C++",
42  "lang_type", "compile",
43  // Configuration variables
44 
45  ""
46  };
47 // </rtc-template>
48 
50  : RTC::DataFlowComponentBase(manager),
51  // <rtc-template block="initializer">
52  m_angleIn("angle", m_angle),
53  m_torqueOut("torque", m_torque),
54 
55  // </rtc-template>
56  dummy(0),
57  qold(DOF)
58 {
59  // Registration: InPort/OutPort/Service
60  // <rtc-template block="registration">
61  // Set service provider to Ports
62 
63  // Set service consumers to Ports
64 
65  // Set CORBA Service Ports
66 
67  // </rtc-template>
68 }
69 
71 {
72  closeFiles();
73  delete [] Pgain;
74  delete [] Dgain;
75 }
76 
77 
78 RTC::ReturnCode_t PA10Controller::onInitialize()
79 {
80  // <rtc-template block="bind_config">
81  // Bind variables and configuration variable
82 
83  // Set InPort buffers
84  addInPort("angle", m_angleIn);
85 
86  // Set OutPort buffer
87  addOutPort("torque", m_torqueOut);
88 
89  // </rtc-template>
90 
91  Pgain = new double[DOF];
92  Dgain = new double[DOF];
93 
94  gain.open(GAIN_FILE);
95  if (gain.is_open()){
96  for (int i=0; i<DOF; i++){
97  gain >> Pgain[i];
98  gain >> Dgain[i];
99  }
100  gain.close();
101  }else{
102  std::cerr << GAIN_FILE << " not opened" << std::endl;
103  }
104  m_torque.data.length(DOF);
105  m_angle.data.length(DOF);
106 
107  return RTC::RTC_OK;
108 }
109 
110 
111 
112 /*
113 RTC::ReturnCode_t PA10Controller::onFinalize()
114 {
115  return RTC::RTC_OK;
116 }
117 */
118 
119 /*
120 RTC::ReturnCode_t PA10Controller::onStartup(RTC::UniqueId ec_id)
121 {
122  return RTC::RTC_OK;
123 }
124 */
125 
126 /*
127 RTC::ReturnCode_t PA10Controller::onShutdown(RTC::UniqueId ec_id)
128 {
129  return RTC::RTC_OK;
130 }
131 */
132 
134 {
135  std::cout << "on Activated" << std::endl;
136  openFiles();
137 
138  if(m_angleIn.isNew()){
139  m_angleIn.read();
140  }
141 
142  for(int i=0; i < DOF; ++i){
143  qold[i] = m_angle.data[i];
144  q_ref[i] = dq_ref[i] = 0.0;
145  }
146 
147  return RTC::RTC_OK;
148 }
149 
151 {
152  std::cout << "on Deactivated" << std::endl;
153  closeFiles();
154  return RTC::RTC_OK;
155 }
156 
157 
158 RTC::ReturnCode_t PA10Controller::onExecute(RTC::UniqueId ec_id)
159 {
160  if(m_angleIn.isNew()){
161  m_angleIn.read();
162  }
163 
164  if(!angle.eof()){
165  angle >> q_ref[0]; vel >> dq_ref[0];// skip time
166  for (int i=0; i<DOF; i++){
167  angle >> q_ref[i];
168  vel >> dq_ref[i];
169  }
170  }
171 
172  for(int i=0; i<DOF; i++){
173  double q = m_angle.data[i];
174  double dq = (q - qold[i]) / TIMESTEP;
175  qold[i] = q;
176 
177  m_torque.data[i] = -(q - q_ref[i]) * Pgain[i] - (dq - dq_ref[i]) * Dgain[i];
178  }
179 
180  m_torqueOut.write();
181 
182  return RTC::RTC_OK;
183 }
184 
185 
186 /*
187  RTC::ReturnCode_t PA10Controller::onAborting(RTC::UniqueId ec_id)
188  {
189  return RTC::RTC_OK;
190  }
191 */
192 
193 /*
194  RTC::ReturnCode_t PA10Controller::onError(RTC::UniqueId ec_id)
195  {
196  return RTC::RTC_OK;
197  }
198 */
199 
200 /*
201  RTC::ReturnCode_t PA10Controller::onReset(RTC::UniqueId ec_id)
202  {
203  return RTC::RTC_OK;
204  }
205 */
206 
207 /*
208  RTC::ReturnCode_t PA10Controller::onStateUpdate(RTC::UniqueId ec_id)
209  {
210  return RTC::RTC_OK;
211  }
212 */
213 
214 /*
215  RTC::ReturnCode_t PA10Controller::onRateChanged(RTC::UniqueId ec_id)
216  {
217  return RTC::RTC_OK;
218  }
219 */
220 
222 {
223  angle.open(ANGLE_FILE);
224  if(!angle.is_open()){
225  std::cerr << ANGLE_FILE << " not opened" << std::endl;
226  }
227 
228  vel.open(VEL_FILE);
229  if (!vel.is_open()){
230  std::cerr << VEL_FILE << " not opened" << std::endl;
231  }
232 }
233 
235 {
236  if(angle.is_open()){
237  angle.close();
238  angle.clear();
239  }
240 
241  if(vel.is_open()){
242  vel.close();
243  angle.clear();
244  }
245 }
246 
247 extern "C"
248 {
249 
251  {
253  manager->registerFactory(profile,
254  RTC::Create<PA10Controller>,
255  RTC::Delete<PA10Controller>);
256  }
257 
258 };
259 
260 
png_infop png_charpp int png_charpp profile
Definition: png.h:2382
InPort< TimedDoubleSeq > m_angleIn
#define ANGLE_FILE
TimedDoubleSeq m_angle
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
TimedDoubleSeq m_torque
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
#define VEL_FILE
std::ifstream gain
double q_ref[DOF]
png_uint_32 i
Definition: png.h:2735
bool addOutPort(const char *name, OutPortBase &outport)
#define DOF
std::ifstream vel
std::vector< double > qold
#define GAIN_FILE
virtual RTC::ReturnCode_t onInitialize()
std::ifstream angle
ExecutionContextHandle_t UniqueId
DLL_EXPORT void PA10ControllerInit(RTC::Manager *manager)
OutPort< TimedDoubleSeq > m_torqueOut
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Sample PD component.
virtual bool isNew()
virtual bool write(DataType &value)
static const char * PA10Controller_spec[]
PA10Controller(RTC::Manager *manager)
double dq_ref[DOF]
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
#define TIMESTEP
#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:04