SoftErrorLimiter.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "SoftErrorLimiter.h"
11 #include "hrpsys/util/VectorConvert.h"
12 #include "hrpsys/idl/RobotHardwareService.hh"
13 #include <rtm/CorbaNaming.h>
14 #include <hrpModel/ModelLoaderUtil.h>
15 
16 #include <math.h>
17 #include <vector>
18 #include <limits>
19 #include <iomanip>
20 #define deg2rad(x)((x)*M_PI/180)
21 
22 // Module specification
23 // <rtc-template block="module_spec">
24 static const char* softerrorlimiter_spec[] =
25  {
26  "implementation_id", "SoftErrorLimiter",
27  "type_name", "SoftErrorLimiter",
28  "description", "soft error limiter",
29  "version", HRPSYS_PACKAGE_VERSION,
30  "vendor", "AIST",
31  "category", "example",
32  "activity_type", "DataFlowComponent",
33  "max_instance", "10",
34  "language", "C++",
35  "lang_type", "compile",
36  // Configuration variables
37  "conf.default.debugLevel", "0",
38  ""
39  };
40 // </rtc-template>
41 
42 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
43 {
44  int pre = os.precision();
45  os.setf(std::ios::fixed);
46  os << std::setprecision(6)
47  << (tm.sec + tm.nsec/1e9)
48  << std::setprecision(pre);
49  os.unsetf(std::ios::fixed);
50  return os;
51 }
52 
54  : RTC::DataFlowComponentBase(manager),
55  // <rtc-template block="initializer">
56  m_qRefIn("qRef", m_qRef),
57  m_qCurrentIn("qCurrent", m_qCurrent),
58  m_servoStateIn("servoStateIn", m_servoState),
59  m_qOut("q", m_qRef),
60  m_servoStateOut("servoStateOut", m_servoState),
61  m_beepCommandOut("beepCommand", m_beepCommand),
62  m_SoftErrorLimiterServicePort("SoftErrorLimiterService"),
63  // </rtc-template>
64  m_debugLevel(0),
65  dummy(0),
66  is_beep_port_connected(false)
67 {
68  init_beep();
69  start_beep(3136);
70 }
71 
73 {
74  quit_beep();
75 }
76 
77 
78 
79 RTC::ReturnCode_t SoftErrorLimiter::onInitialize()
80 {
81  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
82  // <rtc-template block="bind_config">
83  // Bind variables and configuration variable
84  bindParameter("debugLevel", m_debugLevel, "0");
85 
86  // </rtc-template>
87 
88  // Registration: InPort/OutPort/Service
89  // <rtc-template block="registration">
90  // Set InPort buffers
91  addInPort("qRef", m_qRefIn);
92  addInPort("qCurrent", m_qCurrentIn);
93  addInPort("servoState", m_servoStateIn);
94 
95  // Set OutPort buffer
96  addOutPort("q", m_qOut);
97  addOutPort("servoState", m_servoStateOut);
98  addOutPort("beepCommand", m_beepCommandOut);
99 
100  // Set service provider to Ports
101  m_SoftErrorLimiterServicePort.registerProvider("service0", "SoftErrorLimiterService", m_service0);
102 
103  // Set service consumers to Ports
104 
105  // Set CORBA Service Ports
107 
108  // </rtc-template>
109 
112 
113  RTC::Manager& rtcManager = RTC::Manager::instance();
114  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
115  int comPos = nameServer.find(",");
116  if (comPos < 0){
117  comPos = nameServer.length();
118  }
119  nameServer = nameServer.substr(0, comPos);
120  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
121  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
122  CosNaming::NamingContext::_duplicate(naming.getRootContext())
123  )){
124  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
125  << m_profile.instance_name << std::endl;
126  return RTC::RTC_ERROR;
127  }
128 
129  std::cout << "[" << m_profile.instance_name << "] dof = " << m_robot->numJoints() << std::endl;
130  if (!m_robot->init()) return RTC::RTC_ERROR;
132  m_servoState.data.length(m_robot->numJoints());
133  for(unsigned int i = 0; i < m_robot->numJoints(); i++) {
134  m_servoState.data[i].length(1);
135  int status = 0;
136  status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
137  status |= 1<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
138  status |= 1<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
139  status |= 0<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
140  status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
141  m_servoState.data[i][0] = status;
142  }
143 
144  /* Calculate count for beep sound frequency */
145  coil::stringTo(dt, prop["dt"].c_str());
146  soft_limit_error_beep_freq = static_cast<int>(1.0/(4.0*dt)); // soft limit error => 4 times / 1[s]
147  position_limit_error_beep_freq = static_cast<int>(1.0/(2.0*dt)); // position limit error => 2 times / 1[s]
148  debug_print_freq = static_cast<int>(0.2/dt); // once per 0.2 [s]
149  /* If you print debug message for all controller loop, please comment in here */
150  // debug_print_freq = 1;
151  m_beepCommand.data.length(bc.get_num_beep_info());
152 
153  // load joint limit table
154  hrp::readJointLimitTableFromProperties (joint_limit_tables, m_robot, prop["joint_limit_table"], std::string(m_profile.instance_name));
155 
156  // read ignored joint
157  m_joint_mask.resize(m_robot->numJoints(), false);
158  coil::vstring ijoints = coil::split(prop["mask_joint_limit"], ",");
159  for(int i = 0; i < ijoints.size(); i++) {
160  hrp::Link *lk = m_robot->link(std::string(ijoints[i]));
161  if((!!lk) && (lk->jointId >= 0)) {
162  std::cout << "[" << m_profile.instance_name << "] "
163  << "disable ErrorLimit, joint : " << ijoints[i]
164  << " (id = " << lk->jointId << ")" << std::endl;
165  m_joint_mask[lk->jointId] = true;
166  }
167  }
168 
169  return RTC::RTC_OK;
170 }
171 
172 /*
173 RTC::ReturnCode_t SoftErrorLimiter::onFinalize()
174 {
175  return RTC::RTC_OK;
176 }
177 */
178 
179 /*
180 RTC::ReturnCode_t SoftErrorLimiter::onStartup(RTC::UniqueId ec_id)
181 {
182  return RTC::RTC_OK;
183 }
184 */
185 
186 /*
187 RTC::ReturnCode_t SoftErrorLimiter::onShutdown(RTC::UniqueId ec_id)
188 {
189  return RTC::RTC_OK;
190 }
191 */
192 
194 {
195  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
196  return RTC::RTC_OK;
197 }
198 
200 {
201  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
202  return RTC::RTC_OK;
203 }
204 
206 {
207  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
208  static int loop = 0;
209  static bool debug_print_velocity_first = false;
210  static bool debug_print_position_first = false;
211  static bool debug_print_error_first = false;
212  loop ++;
213 
214  // Connection check of m_beepCommand to BeeperRTC
215  // If m_beepCommand is not connected to BeeperRTC and sometimes, check connection.
216  // If once connection is found, never check connection.
217  if (!is_beep_port_connected && (loop % 500 == 0) ) {
218  if ( m_beepCommandOut.connectors().size() > 0 ) {
219  is_beep_port_connected = true;
220  quit_beep();
221  std::cerr << "[" << m_profile.instance_name<< "] beepCommand data port connection found! Use BeeperRTC." << std::endl;
222  }
223  }
224 
225  if (m_qRefIn.isNew()) {
226  m_qRefIn.read();
227  }
228  if (m_qCurrentIn.isNew()) {
229  m_qCurrentIn.read();
230  }
231  if (m_servoStateIn.isNew()) {
233  }
234 
235  /*
236  0x001 : 'SS_OVER_VOLTAGE',
237  0x002 : 'SS_OVER_LOAD',
238  0x004 : 'SS_OVER_VELOCITY',
239  0x008 : 'SS_OVER_CURRENT',
240  0x010 : 'SS_OVER_HEAT',
241  0x020 : 'SS_TORQUE_LIMIT',
242  0x040 : 'SS_VELOCITY_LIMIT',
243  0x080 : 'SS_FORWARD_LIMIT',
244  0x100 : 'SS_REVERSE_LIMIT',
245  0x200 : 'SS_POSITION_ERROR',
246  0x300 : 'SS_ENCODER_ERROR',
247  0x800 : 'SS_OTHER'
248  */
249  bool soft_limit_error = false;
250  bool velocity_limit_error = false;
251  bool position_limit_error = false;
252  if ( m_qRef.data.length() == m_qCurrent.data.length() &&
253  m_qRef.data.length() == m_servoState.data.length() ) {
254  // prev_angle is previous output
255  static std::vector<double> prev_angle;
256  if ( prev_angle.size() != m_qRef.data.length() ) { // initialize prev_angle
257  prev_angle.resize(m_qRef.data.length(), 0);
258  for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){
259  prev_angle[i] = m_qCurrent.data[i];
260  }
261  }
262  std::vector<int> servo_state;
263  servo_state.resize(m_qRef.data.length(), 0);
264  for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){
265  servo_state[i] = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; // enum SwitchStatus {SWITCH_ON, SWITCH_OFF};
266  }
267 
268  /*
269  From hrpModel/Body.h
270  inline Link* joint(int id) const
271  This function returns a link that has a given joint ID.
272  If there is no link that has a given joint ID,
273  the function returns a dummy link object whose ID is minus one.
274  The maximum id can be obtained by numJoints().
275 
276  inline Link* link(int index) const
277  This function returns the link of a given index in the whole link sequence.
278  The order of the sequence corresponds to a link-tree traverse from the root link.
279  The size of the sequence can be obtained by numLinks().
280 
281  So use m_robot->joint(i) for llimit/ulimit, lvlimit/ulimit
282  */
283 
284  // Velocity limitation for reference joint angles
285  for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){
286  if(m_joint_mask[i]) continue;
287  // Determin total upper-lower limit considering velocity, position, and error limits.
288  // e.g.,
289  // total lower limit = max (vel, pos, err) <= severest lower limit
290  // total upper limit = min (vel, pos, err) <= severest upper limit
291  double total_upper_limit = std::numeric_limits<double>::max(), total_lower_limit = -std::numeric_limits<double>::max();
292  {
293  double qvel = (m_qRef.data[i] - prev_angle[i]) / dt;
294  double lvlimit = m_robot->joint(i)->lvlimit + 0.000175; // 0.01 deg / sec
295  double uvlimit = m_robot->joint(i)->uvlimit - 0.000175;
296  // fixed joint has ulimit = vlimit
297  if ( servo_state[i] == 1 && (lvlimit < uvlimit) && ((lvlimit > qvel) || (uvlimit < qvel)) ) {
298  if (loop % debug_print_freq == 0 || debug_print_velocity_first ) {
299  std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm
300  << "] velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel
301  << ", lvlimit =" << lvlimit
302  << ", uvlimit =" << uvlimit
303  << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF");
304  }
305  double limited;
306  // fix joint angle
307  if ( lvlimit > qvel ) {
308  limited = total_lower_limit = std::max(prev_angle[i] + lvlimit * dt, total_lower_limit);
309  }
310  if ( uvlimit < qvel ) {
311  limited = total_upper_limit = std::min(prev_angle[i] + uvlimit * dt, total_upper_limit);
312  }
313  if (loop % debug_print_freq == 0 || debug_print_velocity_first ) {
314  std::cerr << ", q(limited) = " << limited << std::endl;
315  }
316  velocity_limit_error = true;
317  }
318  }
319 
320  // Position limitation for reference joint angles
321  {
322  double llimit = m_robot->joint(i)->llimit;
323  double ulimit = m_robot->joint(i)->ulimit;
324  if (joint_limit_tables.find(m_robot->joint(i)->name) != joint_limit_tables.end()) {
325  std::map<std::string, hrp::JointLimitTable>::iterator it = joint_limit_tables.find(m_robot->joint(i)->name);
326  llimit = it->second.getLlimit(m_qRef.data[it->second.getTargetJointId()]);
327  ulimit = it->second.getUlimit(m_qRef.data[it->second.getTargetJointId()]);
328  }
329  // fixed joint have vlimit = ulimit
330  bool servo_limit_state = (llimit < ulimit) && ((llimit > m_qRef.data[i]) || (ulimit < m_qRef.data[i]));
331  if ( servo_state[i] == 1 && servo_limit_state ) {
332  if (loop % debug_print_freq == 0 || debug_print_position_first) {
333  std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm
334  << "] position limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i]
335  << ", llimit =" << llimit
336  << ", ulimit =" << ulimit
337  << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF")
338  << ", prev_angle = " << prev_angle[i];
339  }
340  double limited;
341  // fix joint angle
342  if ( llimit > m_qRef.data[i] && prev_angle[i] > m_qRef.data[i] ) { // ref < llimit and prev < ref -> OK
343  limited = total_lower_limit = std::max(llimit, total_lower_limit);
344  }
345  if ( ulimit < m_qRef.data[i] && prev_angle[i] < m_qRef.data[i] ) { // ulimit < ref and ref < prev -> OK
346  limited = total_upper_limit = std::min(ulimit, total_upper_limit);
347  }
348  if (loop % debug_print_freq == 0 || debug_print_position_first ) {
349  std::cerr << ", q(limited) = " << limited << std::endl;
350  }
351  m_servoState.data[i][0] |= (0x200 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT);
352  position_limit_error = true;
353  }
354  }
355 
356  // Servo error limitation between reference joint angles and actual joint angles
357  // pos_vel_limited_angle is current output joint angle which arleady finished position limit and velocity limit.
358  // Check and limit error between pos_vel_limited_angle and current actual joint angle.
359  {
360  double pos_vel_limited_angle = std::min(total_upper_limit, std::max(total_lower_limit, m_qRef.data[i]));
361  double limit = m_robot->m_servoErrorLimit[i];
362  double error = pos_vel_limited_angle - m_qCurrent.data[i];
363  if ( servo_state[i] == 1 && fabs(error) > limit ) {
364  if (loop % debug_print_freq == 0 || debug_print_error_first ) {
365  std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm
366  << "] error limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << pos_vel_limited_angle
367  << ", qCurrent=" << m_qCurrent.data[i] << " "
368  << ", Error=" << error << " > " << limit << " (limit)"
369  << ", servo_state = " << ( 1 ? "ON" : "OFF");
370  }
371  double limited;
372  if ( error > limit ) {
373  limited = total_upper_limit = std::min(m_qCurrent.data[i] + limit, total_upper_limit);
374  } else {
375  limited = total_lower_limit = std::max(m_qCurrent.data[i] - limit, total_lower_limit);
376  }
377  if (loop % debug_print_freq == 0 || debug_print_error_first ) {
378  std::cerr << ", q(limited) = " << limited << std::endl;
379  }
380  m_servoState.data[i][0] |= (0x040 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT);
381  soft_limit_error = true;
382  }
383  }
384 
385  // Limitation of current output considering total upper and lower limits
386  prev_angle[i] = m_qRef.data[i] = std::min(total_upper_limit, std::max(total_lower_limit, m_qRef.data[i]));
387  }
388  // display error info if no error found
389  debug_print_velocity_first = !velocity_limit_error;
390  debug_print_position_first = !position_limit_error;
391  debug_print_error_first = !soft_limit_error;
392 
393  // Beep sound
394  if ( soft_limit_error ) { // play beep
397  else bc.stopBeep();
398  } else {
400  }
401  }else if ( position_limit_error || velocity_limit_error ) { // play beep
404  else bc.stopBeep();
405  } else {
407  }
408  } else {
410  bc.stopBeep();
411  } else {
412  stop_beep();
413  }
414  }
415  m_qOut.write();
417  } else {
419  bc.startBeep(3136);
420  } else {
421  start_beep(3136);
422  }
423  if ( loop % 100 == 1 ) {
424  std::cerr << "SoftErrorLimiter is not working..." << std::endl;
425  std::cerr << " m_qRef " << m_qRef.data.length() << std::endl;
426  std::cerr << " m_qCurrent " << m_qCurrent.data.length() << std::endl;
427  std::cerr << " m_servoState " << m_servoState.data.length() << std::endl;
428  }
429  }
433  }
434 
435  return RTC::RTC_OK;
436 }
437 
438 /*
439 RTC::ReturnCode_t SoftErrorLimiter::onAborting(RTC::UniqueId ec_id)
440 {
441  return RTC::RTC_OK;
442 }
443 */
444 
445 /*
446 RTC::ReturnCode_t SoftErrorLimiter::onError(RTC::UniqueId ec_id)
447 {
448  return RTC::RTC_OK;
449 }
450 */
451 
452 /*
453 RTC::ReturnCode_t SoftErrorLimiter::onReset(RTC::UniqueId ec_id)
454 {
455  return RTC::RTC_OK;
456 }
457 */
458 
459 /*
460 RTC::ReturnCode_t SoftErrorLimiter::onStateUpdate(RTC::UniqueId ec_id)
461 {
462  return RTC::RTC_OK;
463 }
464 */
465 
466 /*
467 RTC::ReturnCode_t SoftErrorLimiter::onRateChanged(RTC::UniqueId ec_id)
468 {
469  return RTC::RTC_OK;
470 }
471 */
472 
473 
474 
475 extern "C"
476 {
477 
479  {
481  manager->registerFactory(profile,
482  RTC::Create<SoftErrorLimiter>,
483  RTC::Delete<SoftErrorLimiter>);
484  }
485 
486 };
487 
488 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
#define max(a, b)
bool isWritable() const
Definition: beep.h:36
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
void SoftErrorLimiterInit(RTC::Manager *manager)
std::vector< bool > m_joint_mask
bool stringTo(To &val, const char *str)
OutPort< TimedLongSeq > m_beepCommandOut
static std::ostream & operator<<(std::ostream &os, const struct RTC::Time &tm)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
void quit_beep()
Definition: beep.cpp:28
TimedLongSeq m_beepCommand
void init_beep()
Definition: beep.cpp:6
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
void error(char *msg) const
CORBA::ORB_ptr getORB()
null component
png_uint_32 i
coil::Properties & getProperties()
SoftErrorLimiter(RTC::Manager *manager)
Constructor.
void readJointLimitTableFromProperties(std::map< std::string, hrp::JointLimitTable > &joint_mm_tables, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
static Manager & instance()
bool addOutPort(const char *name, OutPortBase &outport)
void setDataPort(RTC::TimedLongSeq &out_data)
Definition: beep.h:43
int get_num_beep_info() const
Definition: beep.h:21
void setRobot(boost::shared_ptr< robot > &i_robot)
#define min(a, b)
std::vector< std::string > vstring
SoftErrorLimiterService_impl m_service0
coil::Properties & getConfig()
OutPort< OpenHRP::TimedLongSeqSeq > m_servoStateOut
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateIn
void startBeep(const int _freq, const int _length=50)
Definition: beep.h:22
ExecutionContextHandle_t UniqueId
boost::shared_ptr< robot > m_robot
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
TimedDoubleSeq m_qRef
OpenHRP::TimedLongSeqSeq m_servoState
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
OutPort< TimedDoubleSeq > m_qOut
std::map< std::string, hrp::JointLimitTable > joint_limit_tables
unsigned int m_debugLevel
static const char * softerrorlimiter_spec[]
prop
InPort< TimedDoubleSeq > m_qCurrentIn
naming
void stopBeep()
Definition: beep.h:29
InPort< TimedDoubleSeq > m_qRefIn
TimedDoubleSeq m_qCurrent
virtual bool isNew()
bool addPort(PortBase &port)
virtual bool write(DataType &value)
RTC::CorbaPort m_SoftErrorLimiterServicePort
void stop_beep()
Definition: beep.cpp:22
virtual RTC::ReturnCode_t onInitialize()
const std::vector< OutPortConnector *> & connectors()
bool addInPort(const char *name, InPortBase &inport)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void start_beep(int freq, int length)
Definition: beep.cpp:16
virtual ~SoftErrorLimiter()
Destructor.
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)


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