HGcontroller.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "HGcontroller.h"
11 
12 // Module specification
13 // <rtc-template block="module_spec">
14 static const char* hgcontroller_spec[] =
15  {
16  "implementation_id", "HGcontroller",
17  "type_name", "HGcontroller",
18  "description", "high gain joint controller component for OpenHRP",
19  "version", HRPSYS_PACKAGE_VERSION,
20  "vendor", "AIST",
21  "category", "example",
22  "activity_type", "DataFlowComponent",
23  "max_instance", "10",
24  "language", "C++",
25  "lang_type", "compile",
26  // Configuration variables
27  ""
28  };
29 // </rtc-template>
30 
32  : RTC::DataFlowComponentBase(manager),
33  // <rtc-template block="initializer">
34  m_qIn("qIn", m_q),
35  m_qOut("qOut", m_q),
36  m_dqOut("dqOut", m_dq),
37  m_ddqOut("ddqOut", m_ddq),
38  // </rtc-template>
39  m_dt(0.005),
40  dummy(0)
41 {
42 }
43 
45 {
46 }
47 
48 
49 
50 RTC::ReturnCode_t HGcontroller::onInitialize()
51 {
52  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
53 
55  coil::stringTo(m_dt, ref["dt"].c_str());
56 
57  // <rtc-template block="bind_config">
58  // Bind variables and configuration variable
59 
60  // </rtc-template>
61 
62  // Registration: InPort/OutPort/Service
63  // <rtc-template block="registration">
64  // Set InPort buffers
65  addInPort("qIn", m_qIn);
66 
67  // Set OutPort buffer
68  addOutPort("qOut", m_qOut);
69  addOutPort("dqOut", m_dqOut);
70  addOutPort("ddqOut", m_ddqOut);
71 
72  // Set service provider to Ports
73 
74  // Set service consumers to Ports
75 
76  // Set CORBA Service Ports
77 
78  // </rtc-template>
79 
80  return RTC::RTC_OK;
81 }
82 
83 
84 
85 /*
86 RTC::ReturnCode_t HGcontroller::onFinalize()
87 {
88  return RTC::RTC_OK;
89 }
90 */
91 
92 /*
93 RTC::ReturnCode_t HGcontroller::onStartup(RTC::UniqueId ec_id)
94 {
95  return RTC::RTC_OK;
96 }
97 */
98 
99 /*
100 RTC::ReturnCode_t HGcontroller::onShutdown(RTC::UniqueId ec_id)
101 {
102  return RTC::RTC_OK;
103 }
104 */
105 
106 RTC::ReturnCode_t HGcontroller::onActivated(RTC::UniqueId ec_id)
107 {
108  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
109  return RTC::RTC_OK;
110 }
111 
113 {
114  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
115  return RTC::RTC_OK;
116 }
117 
118 RTC::ReturnCode_t HGcontroller::onExecute(RTC::UniqueId ec_id)
119 {
120  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
121 
122  if (m_qIn.isNew()) m_qIn.read();
123 
124  if (m_q.data.length()){
125  if (m_dq.data.length() == 0){
126  // initialize m_dq, m_ddq
127  m_dq.data.length(m_q.data.length());
128  m_ddq.data.length(m_q.data.length());
129  for (unsigned int i=0; i<m_q.data.length(); i++){
130  m_dq.data[i] = m_ddq.data[i] = 0.0;
131  }
132  }else{
133  // compute m_dq, m_ddq
134  for (unsigned int i=0; i<m_q.data.length(); i++){
135  m_dq.data[i] = (m_q.data[i] - m_qOld.data[i])/m_dt;
136  m_ddq.data[i] = (m_dq.data[i] - m_dqOld.data[i])/m_dt;
137  }
138  }
139 
140  m_qOut.write();
141  m_dqOut.write();
142  m_ddqOut.write();
143 
144  m_qOld = m_q;
145  m_dqOld = m_dq;
146  }
147 
148  return RTC::RTC_OK;
149 }
150 
151 /*
152 RTC::ReturnCode_t HGcontroller::onAborting(RTC::UniqueId ec_id)
153 {
154  return RTC::RTC_OK;
155 }
156 */
157 
158 /*
159 RTC::ReturnCode_t HGcontroller::onError(RTC::UniqueId ec_id)
160 {
161  return RTC::RTC_OK;
162 }
163 */
164 
165 /*
166 RTC::ReturnCode_t HGcontroller::onReset(RTC::UniqueId ec_id)
167 {
168  return RTC::RTC_OK;
169 }
170 */
171 
172 /*
173 RTC::ReturnCode_t HGcontroller::onStateUpdate(RTC::UniqueId ec_id)
174 {
175  return RTC::RTC_OK;
176 }
177 */
178 
179 /*
180 RTC::ReturnCode_t HGcontroller::onRateChanged(RTC::UniqueId ec_id)
181 {
182  return RTC::RTC_OK;
183 }
184 */
185 
186 
187 
188 extern "C"
189 {
190 
192  {
194  manager->registerFactory(profile,
195  RTC::Create<HGcontroller>,
196  RTC::Delete<HGcontroller>);
197  }
198 
199 };
200 
201 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
OutPort< TimedDoubleSeq > m_ddqOut
Definition: HGcontroller.h:119
TimedDoubleSeq m_dqOld
Definition: HGcontroller.h:140
bool stringTo(To &val, const char *str)
InPort< TimedDoubleSeq > m_qIn
Definition: HGcontroller.h:109
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
png_uint_32 i
coil::Properties & getProperties()
virtual ~HGcontroller()
Destructor.
OutPort< TimedDoubleSeq > m_qOut
Definition: HGcontroller.h:117
bool addOutPort(const char *name, OutPortBase &outport)
virtual RTC::ReturnCode_t onInitialize()
void HGcontrollerInit(RTC::Manager *manager)
TimedDoubleSeq m_q
Definition: HGcontroller.h:105
ExecutionContextHandle_t UniqueId
OutPort< TimedDoubleSeq > m_dqOut
Definition: HGcontroller.h:118
double m_dt
Definition: Beeper.cpp:24
TimedDoubleSeq m_ddq
Definition: HGcontroller.h:113
TimedDoubleSeq m_qOld
Definition: HGcontroller.h:140
TimedDoubleSeq m_dq
Definition: HGcontroller.h:113
static const char * hgcontroller_spec[]
virtual bool isNew()
virtual bool write(DataType &value)
bool addInPort(const char *name, InPortBase &inport)
high gain joint controller component for OpenHRP
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
HGcontroller(RTC::Manager *manager)
Constructor.


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20