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