AccelerationChecker.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <math.h>
11 #include <iomanip>
12 #include <stdio.h>
13 #include "AccelerationChecker.h"
14 #include "hrpsys/idl/RobotHardwareService.hh"
15 
16 // Module specification
17 // <rtc-template block="module_spec">
18 static const char* spec[] =
19  {
20  "implementation_id", "AccelerationChecker",
21  "type_name", "AccelerationChecker",
22  "description", "Joint acceleration checker",
23  "version", HRPSYS_PACKAGE_VERSION,
24  "vendor", "AIST",
25  "category", "example",
26  "activity_type", "DataFlowComponent",
27  "max_instance", "10",
28  "language", "C++",
29  "lang_type", "compile",
30  // Configuration variables
31  "conf.default.thd", "1000",
32  "conf.default.print", "0",
33 
34  ""
35  };
36 // </rtc-template>
37 
39  : RTC::DataFlowComponentBase(manager),
40  // <rtc-template block="initializer">
41  m_qIn("qIn", m_q),
42  m_servoStateIn("servoState", m_servoState),
43  m_qOut("qOut", m_q),
44  // </rtc-template>
45  dummy(0)
46 {
47 }
48 
50 {
51 }
52 
53 
54 
56 {
57  //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
58  // <rtc-template block="bind_config">
59  // Bind variables and configuration variable
60  bindParameter("thd", m_thd, "1000");
61  bindParameter("print", m_print, "0");
62 
63  // </rtc-template>
64 
65  // Registration: InPort/OutPort/Service
66  // <rtc-template block="registration">
67  // Set InPort buffers
68  addInPort("qIn", m_qIn);
69  addInPort("servoState", m_servoStateIn);
70 
71  // Set OutPort buffer
72  addOutPort("qOut", m_qOut);
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 
83  m_dt = 0;
84  coil::stringTo(m_dt, prop["dt"].c_str());
85  if (m_dt == 0){
86  std::cerr << m_profile.instance_name << ": dt is not defined in the conf"
87  << std::endl;
88  return RTC::RTC_ERROR;
89  }
90 
91  return RTC::RTC_OK;
92 }
93 
94 
95 
96 /*
97 RTC::ReturnCode_t AccelerationChecker::onFinalize()
98 {
99  return RTC::RTC_OK;
100 }
101 */
102 
103 /*
104 RTC::ReturnCode_t AccelerationChecker::onStartup(RTC::UniqueId ec_id)
105 {
106  return RTC::RTC_OK;
107 }
108 */
109 
110 /*
111 RTC::ReturnCode_t AccelerationChecker::onShutdown(RTC::UniqueId ec_id)
112 {
113  return RTC::RTC_OK;
114 }
115 */
116 
118 {
119  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
120  return RTC::RTC_OK;
121 }
122 
124 {
125  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
126  return RTC::RTC_OK;
127 }
128 
130 {
131  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
132 
133  if (m_qIn.isNew()){
134  m_qIn.read();
136  if (!m_dq.data.length()){ // first time
137  m_qOld.data.length(m_q.data.length());
138  m_qOld = m_q;
139  m_dqOld.data.length(m_q.data.length());
140  m_ddqMax.data.length(m_q.data.length());
141  for (unsigned int i=0; i<m_dqOld.data.length(); i++){
142  m_dqOld.data[i] = 0.0;
143  m_ddqMax.data[i] = 0.0;
144  }
145  m_dq.data.length(m_q.data.length());
146  }
147  for (unsigned int i=0; i<m_q.data.length(); i++){
148  m_dq.data[i] = (m_q.data[i] - m_qOld.data[i])/m_dt;
149  double ddq = (m_dq.data[i] - m_dqOld.data[i])/m_dt;
150  bool servo = true;
151  if (m_servoState.data.length()==m_q.data.length()){
152  servo = m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK;
153  }
154  if (fabs(ddq) > m_ddqMax.data[i]) m_ddqMax.data[i] = fabs(ddq);
155  if (servo && fabs(ddq) > m_thd){
156  time_t now = time(NULL);
157  struct tm *tm_now = localtime(&now);
158  char *datetime = asctime(tm_now);
159  datetime[strlen(datetime)-1] = '\0';
160  std::cout << "["
161  << datetime
162  << "] Warning: too big joint acceleration for "
163  << i << "th joint(" << ddq << "[rad/m^2])" << std::endl;
164  }
165  }
166  m_qOld = m_q;
167  m_dqOld = m_dq;
168 
169  if (m_print){
170  printf("jid: max acc[rad/m^2]\n");
171  for (unsigned int i=0; i<m_ddqMax.data.length(); i++){
172  printf("%2d: %8f\n", i, m_ddqMax.data[i]);
173  }
174  m_print = false;
175  }
176 
177  m_qOut.write();
178  }
179 
180  return RTC::RTC_OK;
181 }
182 
183 /*
184 RTC::ReturnCode_t AccelerationChecker::onAborting(RTC::UniqueId ec_id)
185 {
186  return RTC::RTC_OK;
187 }
188 */
189 
190 /*
191 RTC::ReturnCode_t AccelerationChecker::onError(RTC::UniqueId ec_id)
192 {
193  return RTC::RTC_OK;
194 }
195 */
196 
197 /*
198 RTC::ReturnCode_t AccelerationChecker::onReset(RTC::UniqueId ec_id)
199 {
200  return RTC::RTC_OK;
201 }
202 */
203 
204 /*
205 RTC::ReturnCode_t AccelerationChecker::onStateUpdate(RTC::UniqueId ec_id)
206 {
207  return RTC::RTC_OK;
208 }
209 */
210 
211 /*
212 RTC::ReturnCode_t AccelerationChecker::onRateChanged(RTC::UniqueId ec_id)
213 {
214  return RTC::RTC_OK;
215 }
216 */
217 
218 
219 
220 extern "C"
221 {
222 
224  {
226  manager->registerFactory(profile,
227  RTC::Create<AccelerationChecker>,
228  RTC::Delete<AccelerationChecker>);
229  }
230 
231 };
232 
233 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
bool stringTo(To &val, const char *str)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
OutPort< TimedDoubleSeq > m_qOut
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateIn
png_uint_32 i
coil::Properties & getProperties()
virtual ~AccelerationChecker()
Destructor.
bool addOutPort(const char *name, OutPortBase &outport)
static const char * spec[]
OpenHRP::TimedLongSeqSeq m_servoState
ExecutionContextHandle_t UniqueId
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
virtual RTC::ReturnCode_t onInitialize()
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
static std::vector< int > servo
Definition: iob.cpp:17
prop
virtual bool isNew()
virtual bool write(DataType &value)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
AccelerationChecker(RTC::Manager *manager)
Constructor.
bool addInPort(const char *name, InPortBase &inport)
InPort< TimedDoubleSeq > m_qIn
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void AccelerationCheckerInit(RTC::Manager *manager)
joint acceleration checker


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