EmergencyStopper.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "hrpsys/util/VectorConvert.h"
11 #include <rtm/CorbaNaming.h>
12 #include <hrpModel/ModelLoaderUtil.h>
13 #include <math.h>
14 #include <hrpModel/Link.h>
15 #include <hrpModel/Sensor.h>
16 #include "hrpsys/idl/RobotHardwareService.hh"
17 
18 #include "EmergencyStopper.h"
19 #include <iomanip>
20 
22 
23 // Module specification
24 // <rtc-template block="module_spec">
25 static const char* emergencystopper_spec[] =
26 {
27  "implementation_id", "EmergencyStopper",
28  "type_name", "EmergencyStopper",
29  "description", "emergency stopper",
30  "version", HRPSYS_PACKAGE_VERSION,
31  "vendor", "AIST",
32  "category", "example",
33  "activity_type", "DataFlowComponent",
34  "max_instance", "10",
35  "language", "C++",
36  "lang_type", "compile",
37  // Configuration variables
38  "conf.default.debugLevel", "0",
39  ""
40 };
41 // </rtc-template>
42 
43 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
44 {
45  int pre = os.precision();
46  os.setf(std::ios::fixed);
47  os << std::setprecision(6)
48  << (tm.sec + tm.nsec/1e9)
49  << std::setprecision(pre);
50  os.unsetf(std::ios::fixed);
51  return os;
52 }
53 
55  : RTC::DataFlowComponentBase(manager),
56  // <rtc-template block="initializer">
57  m_qRefIn("qRef", m_qRef),
58  m_emergencySignalIn("emergencySignal", m_emergencySignal),
59  m_servoStateIn("servoStateIn", m_servoState),
60  m_qOut("q", m_q),
61  m_emergencyModeOut("emergencyMode", m_emergencyMode),
62  m_beepCommandOut("beepCommand", m_beepCommand),
63  m_EmergencyStopperServicePort("EmergencyStopperService"),
64  // </rtc-template>
65  m_robot(hrp::BodyPtr()),
66  m_debugLevel(0),
67  loop(0),
68  emergency_stopper_beep_count(0),
69  dummy(0)
70 {
72 }
73 
75 {
76 }
77 
78 
79 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
80 RTC::ReturnCode_t EmergencyStopper::onInitialize()
81 {
82  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
83  // <rtc-template block="bind_config">
84  // Bind variables and configuration variable
85  bindParameter("debugLevel", m_debugLevel, "0");
86 
87  // Registration: InPort/OutPort/Service
88  // <rtc-template block="registration">
89  // Set InPort buffers
90  addInPort("qRef", m_qRefIn);
91  addInPort("emergencySignal", m_emergencySignalIn);
92  addInPort("servoStateIn", m_servoStateIn);
93 
94  // Set OutPort buffer
95  addOutPort("q", m_qOut);
96  addOutPort("emergencyMode", m_emergencyModeOut);
97  addOutPort("beepCommand", m_beepCommandOut);
98 
99  // Set service provider to Ports
100  m_EmergencyStopperServicePort.registerProvider("service0", "EmergencyStopperService", m_service0);
101 
102  // Set service consumers to Ports
103 
104  // Set CORBA Service Ports
106 
107  // </rtc-template>
108 
109  // Setup robot model
111  coil::stringTo(m_dt, prop["dt"].c_str());
112  m_robot = hrp::BodyPtr(new hrp::Body());
113 
114  RTC::Manager& rtcManager = RTC::Manager::instance();
115  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
116  int comPos = nameServer.find(",");
117  if (comPos < 0){
118  comPos = nameServer.length();
119  }
120  nameServer = nameServer.substr(0, comPos);
121  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
122  OpenHRP::BodyInfo_var binfo;
123  binfo = hrp::loadBodyInfo(prop["model"].c_str(),
124  CosNaming::NamingContext::_duplicate(naming.getRootContext()));
125  if (CORBA::is_nil(binfo)) {
126  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
127  << std::endl;
128  return RTC::RTC_ERROR;
129  }
130  if (!loadBodyFromBodyInfo(m_robot, binfo)) {
131  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
132  return RTC::RTC_ERROR;
133  }
134 
135  // Setting for wrench data ports (real + virtual)
136  OpenHRP::LinkInfoSequence_var lis = binfo->links();
137  std::vector<std::string> fsensor_names;
138  // find names for real force sensors
139  for ( unsigned int k = 0; k < lis->length(); k++ ) {
140  OpenHRP::SensorInfoSequence& sensors = lis[k].sensors;
141  for ( unsigned int l = 0; l < sensors.length(); l++ ) {
142  if ( std::string(sensors[l].type) == "Force" ) {
143  fsensor_names.push_back(std::string(sensors[l].name));
144  }
145  }
146  }
147  unsigned int npforce = fsensor_names.size();
148  // find names for virtual force sensors
149  coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
150  unsigned int nvforce = virtual_force_sensor.size()/10;
151  for (unsigned int i=0; i<nvforce; i++){
152  fsensor_names.push_back(virtual_force_sensor[i*10+0]);
153  }
154  // add ports for all force sensors
155  unsigned int nforce = npforce + nvforce;
156  m_wrenchesRef.resize(nforce);
157  m_wrenches.resize(nforce);
158  m_wrenchesIn.resize(nforce);
159  m_wrenchesOut.resize(nforce);
160  for (unsigned int i=0; i<nforce; i++){
161  m_wrenchesIn[i] = new InPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"In").c_str(), m_wrenchesRef[i]);
162  m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Out").c_str(), m_wrenches[i]);
163  m_wrenchesRef[i].data.length(6);
164  m_wrenchesRef[i].data[0] = m_wrenchesRef[i].data[1] = m_wrenchesRef[i].data[2] = 0.0;
165  m_wrenchesRef[i].data[3] = m_wrenchesRef[i].data[4] = m_wrenchesRef[i].data[5] = 0.0;
166  m_wrenches[i].data.length(6);
167  m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
168  m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
169  registerInPort(std::string(fsensor_names[i]+"In").c_str(), *m_wrenchesIn[i]);
170  registerOutPort(std::string(fsensor_names[i]+"Out").c_str(), *m_wrenchesOut[i]);
171  }
172 
173  // initialize member variables
175  is_initialized = false;
176 
178  recover_time_dt = 1.0;
181  //default_retrieve_time = 1.0/m_dt;
182  m_stop_posture = new double[m_robot->numJoints()];
183  m_stop_wrenches = new double[nforce*6];
184  m_tmp_wrenches = new double[nforce*6];
185  m_interpolator = new interpolator(m_robot->numJoints(), recover_time_dt);
186  m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
188  m_wrenches_interpolator->setName(std::string(m_profile.instance_name)+" interpolator wrenches");
189 
190  m_q.data.length(m_robot->numJoints());
191  for(unsigned int i=0; i<m_robot->numJoints(); i++){
192  m_q.data[i] = 0;
193  m_stop_posture[i] = 0;
194  }
195  for(unsigned int i=0; i<nforce; i++){
196  for(int j=0; j<6; j++){
197  m_wrenches[i].data[j] = 0;
198  m_stop_wrenches[i*6+j] = 0;
199  }
200  }
201 
202  m_servoState.data.length(m_robot->numJoints());
203  for(unsigned int i = 0; i < m_robot->numJoints(); i++) {
204  m_servoState.data[i].length(1);
205  int status = 0;
206  status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
207  status |= 1<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
208  status |= 1<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
209  status |= 0<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
210  status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
211  m_servoState.data[i][0] = status;
212  }
213 
214  emergency_stopper_beep_freq = static_cast<int>(1.0/(2.0*m_dt)); // 2 times / 1[s]
215  m_beepCommand.data.length(bc.get_num_beep_info());
216  return RTC::RTC_OK;
217 }
218 
219 
220 
221 
222 RTC::ReturnCode_t EmergencyStopper::onFinalize()
223 {
224  delete m_interpolator;
226  delete m_stop_posture;
227  delete m_stop_wrenches;
228  delete m_tmp_wrenches;
229  return RTC::RTC_OK;
230 }
231 
232 /*
233  RTC::ReturnCode_t EmergencyStopper::onStartup(RTC::UniqueId ec_id)
234  {
235  return RTC::RTC_OK;
236  }
237 */
238 
239 /*
240  RTC::ReturnCode_t EmergencyStopper::onShutdown(RTC::UniqueId ec_id)
241  {
242  return RTC::RTC_OK;
243  }
244 */
245 
247 {
248  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
249  return RTC::RTC_OK;
250 }
251 
253 {
254  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
255  Guard guard(m_mutex);
256  if (is_stop_mode) {
257  is_stop_mode = false;
258  recover_time = 0;
259  m_interpolator->setGoal(m_qRef.data.get_buffer(), m_dt);
260  m_interpolator->get(m_q.data.get_buffer());
261  }
262  return RTC::RTC_OK;
263 }
264 
266 {
267  unsigned int numJoints = m_robot->numJoints();
268  loop++;
269  if (m_servoStateIn.isNew()) {
271  }
272  if (!is_initialized) {
273  if (m_qRefIn.isNew()) {
274  m_qRefIn.read();
275  is_initialized = true;
276  } else {
277  return RTC::RTC_OK;
278  }
279  }
280 
281  // read data
282  if (m_qRefIn.isNew()) {
283  // joint angle
284  m_qRefIn.read();
285  assert(m_qRef.data.length() == numJoints);
286  std::vector<double> current_posture;
287  for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ) {
288  current_posture.push_back(m_qRef.data[i]);
289  }
290  m_input_posture_queue.push(current_posture);
291  while ((int)m_input_posture_queue.size() > default_retrieve_time) {
292  m_input_posture_queue.pop();
293  }
294  if (!is_stop_mode) {
295  for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ) {
296  if (recover_time > 0) { // Until releasing is finished, do not use m_stop_posture in input queue because too large error.
297  m_stop_posture[i] = m_q.data[i];
298  } else {
300  }
301  }
302  }
303  // wrench
304  for ( size_t i = 0; i < m_wrenchesIn.size(); i++ ) {
305  if ( m_wrenchesIn[i]->isNew() ) {
306  m_wrenchesIn[i]->read();
307  }
308  }
309  std::vector<double> current_wrench;
310  for ( unsigned int i= 0; i < m_wrenchesRef.size(); i++ ) {
311  for (int j = 0; j < 6; j++ ) {
312  current_wrench.push_back(m_wrenchesRef[i].data[j]);
313  }
314  }
315  m_input_wrenches_queue.push(current_wrench);
316  while ((int)m_input_wrenches_queue.size() > default_retrieve_time) {
318  }
319  if (!is_stop_mode) {
320  for ( unsigned int i= 0; i < m_wrenchesRef.size(); i++ ) {
321  for (int j = 0; j < 6; j++ ) {
322  if (recover_time > 0) {
323  m_stop_wrenches[i*6+j] = m_wrenches[i].data[j];
324  } else {
325  m_stop_wrenches[i*6+j] = m_input_wrenches_queue.front()[i*6+j];
326  }
327  }
328  }
329  }
330  }
331 
332  if (m_emergencySignalIn.isNew()){
334  if ( m_emergencySignal.data == 0 ) {
335  Guard guard(m_mutex);
336  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
337  << "] emergencySignal is reset!" << std::endl;
338  is_stop_mode = false;
339  } else if (!is_stop_mode) {
340  Guard guard(m_mutex);
341  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
342  << "] emergencySignal is set!" << std::endl;
343  is_stop_mode = true;
344  }
345  }
348  // Reflect current output joint angles to interpolator state
349  m_interpolator->set(m_q.data.get_buffer());
352  }
353 
354  if (DEBUGP) {
355  std::cerr << "[" << m_profile.instance_name << "] is_stop_mode : " << is_stop_mode << " recover_time : " << recover_time << "[s] retrieve_time : " << retrieve_time << "[s]" << std::endl;
356  }
357 
358  // mode : is_stop_mode : recover_time : set as q
359  // release : false : 0 : qRef
360  // recover : false : > 0 : q'
361  // stop : true : do not care : q(do nothing)
362  if (!is_stop_mode) {
363  if (recover_time > 0) {
365  m_interpolator->setGoal(m_qRef.data.get_buffer(), recover_time);
366  m_interpolator->get(m_q.data.get_buffer());
371  } else {
372  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
373  m_q.data[i] = m_qRef.data[i];
374  }
375  for ( unsigned int i = 0; i < m_wrenches.size(); i++ ) {
376  for ( int j = 0; j < 6; j++ ) {
377  m_wrenches[i].data[j] = m_wrenchesRef[i].data[j];
378  }
379  }
380  }
381  } else { // stop mode
383  if (retrieve_time > 0 ) {
386  m_interpolator->get(m_q.data.get_buffer());
390  } else {
391  // Do nothing
392  }
393  }
394 
395  // write data
396  if (DEBUGP) {
397  std::cerr << "q: ";
398  for (unsigned int i = 0; i < numJoints; i++) {
399  std::cerr << " " << m_q.data[i] ;
400  }
401  std::cerr << std::endl;
402  std::cerr << "wrenches: ";
403  for (unsigned int i = 0; i < m_wrenches.size(); i++) {
404  for (int j = 0; j < 6; j++ ) {
405  std::cerr << " " << m_wrenches[i].data[j];
406  }
407  }
408  std::cerr << std::endl;
409  }
410  m_q.tm = m_qRef.tm;
411  m_qOut.write();
412  for (size_t i = 0; i < m_wrenches.size(); i++) {
413  m_wrenches[i].tm = m_qRef.tm;
414  }
415  for (size_t i = 0; i < m_wrenchesOut.size(); i++) {
416  m_wrenchesOut[i]->write();
417  }
418 
420  m_emergencyMode.tm = m_qRef.tm;
422 
424 
425  // beep sound for emergency stop alert
426  // check servo for emergency stop beep sound
427  bool has_servoOn = false;
428  for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
429  int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
430  has_servoOn = has_servoOn || (servo_state == 1);
431  }
432  // beep
433  if ( is_stop_mode && has_servoOn ) { // If stop mode and some joint is servoOn
436  } else {
437  bc.stopBeep();
438  }
440  } else {
442  bc.stopBeep();
443  }
445  m_beepCommand.tm = m_qRef.tm;
447  return RTC::RTC_OK;
448 }
449 
450 /*
451  RTC::ReturnCode_t EmergencyStopper::onAborting(RTC::UniqueId ec_id)
452  {
453  return RTC::RTC_OK;
454  }
455 */
456 
457 /*
458  RTC::ReturnCode_t EmergencyStopper::onError(RTC::UniqueId ec_id)
459  {
460  return RTC::RTC_OK;
461  }
462 */
463 
464 /*
465  RTC::ReturnCode_t EmergencyStopper::onReset(RTC::UniqueId ec_id)
466  {
467  return RTC::RTC_OK;
468  }
469 */
470 
471 /*
472  RTC::ReturnCode_t EmergencyStopper::onStateUpdate(RTC::UniqueId ec_id)
473  {
474  return RTC::RTC_OK;
475  }
476 */
477 
478 /*
479  RTC::ReturnCode_t EmergencyStopper::onRateChanged(RTC::UniqueId ec_id)
480  {
481  return RTC::RTC_OK;
482  }
483 */
484 
486 {
487  Guard guard(m_mutex);
488  if (!is_stop_mode) {
489  is_stop_mode = true;
490  std::cerr << "[" << m_profile.instance_name << "] stopMotion is called" << std::endl;
491  }
492  return true;
493 }
494 
496 {
497  Guard guard(m_mutex);
498  if (is_stop_mode) {
499  is_stop_mode = false;
500  std::cerr << "[" << m_profile.instance_name << "] releaseMotion is called" << std::endl;
501  }
502  return true;
503 }
504 
505 bool EmergencyStopper::getEmergencyStopperParam(OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param)
506 {
507  std::cerr << "[" << m_profile.instance_name << "] getEmergencyStopperParam" << std::endl;
508  i_param.default_recover_time = default_recover_time*m_dt;
509  i_param.default_retrieve_time = default_retrieve_time*m_dt;
510  i_param.is_stop_mode = is_stop_mode;
511  return true;
512 };
513 
514 bool EmergencyStopper::setEmergencyStopperParam(const OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param)
515 {
516  std::cerr << "[" << m_profile.instance_name << "] setEmergencyStopperParam" << std::endl;
517  default_recover_time = i_param.default_recover_time/m_dt;
518  default_retrieve_time = i_param.default_retrieve_time/m_dt;
519  std::cerr << "[" << m_profile.instance_name << "] default_recover_time = " << default_recover_time*m_dt << "[s], default_retrieve_time = " << default_retrieve_time*m_dt << "[s]" << std::endl;
520  return true;
521 };
522 
523 extern "C"
524 {
525 
527  {
529  manager->registerFactory(profile,
530  RTC::Create<EmergencyStopper>,
531  RTC::Delete<EmergencyStopper>);
532  }
533 
534 };
535 
536 
ComponentProfile m_profile
virtual RTC::ReturnCode_t onFinalize()
png_infop png_charpp int png_charpp profile
static const char * emergencystopper_spec[]
bool setEmergencyStopperParam(const OpenHRP::EmergencyStopperService::EmergencyStopperParam &i_param)
bool getEmergencyStopperParam(OpenHRP::EmergencyStopperService::EmergencyStopperParam &i_param)
TimedDoubleSeq m_q
bool isWritable() const
Definition: beep.h:36
png_infop png_charp png_int_32 png_int_32 int * type
interpolator * m_wrenches_interpolator
HRPMODEL_API OpenHRP::BodyInfo_var loadBodyInfo(const char *url, int &argc, char *argv[])
OutPort< TimedLong > m_emergencyModeOut
bool stringTo(To &val, const char *str)
OutPort< TimedLongSeq > m_beepCommandOut
TimedDoubleSeq m_qRef
png_infop png_charpp name
TimedLong m_emergencySignal
TimedLong m_emergencyMode
void setName(const std::string &_name)
Definition: interpolator.h:63
void get_wrenches_array_from_data(const std::vector< TimedDoubleSeq > &wrenches_data, double *wrenches_array)
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
CORBA::ORB_ptr getORB()
coil::Guard< coil::Mutex > Guard
png_uint_32 i
TimedLongSeq m_beepCommand
coil::Properties & getProperties()
virtual RTC::ReturnCode_t onInitialize()
static Manager & instance()
RTC::CorbaPort m_EmergencyStopperServicePort
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
boost::shared_ptr< Body > BodyPtr
InPort< TimedDoubleSeq > m_qRefIn
EmergencyStopperService_impl m_service0
std::queue< std::vector< double > > m_input_posture_queue
std::vector< std::string > vstring
coil::Properties & getConfig()
hrp::BodyPtr m_robot
void startBeep(const int _freq, const int _length=50)
Definition: beep.h:22
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateIn
ExecutionContextHandle_t UniqueId
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
interpolator * m_interpolator
virtual ~EmergencyStopper()
Destructor.
std::vector< TimedDoubleSeq > m_wrenchesRef
OpenHRP::TimedLongSeqSeq m_servoState
std::queue< std::vector< double > > m_input_wrenches_queue
prop
static std::ostream & operator<<(std::ostream &os, const struct RTC::Time &tm)
std::vector< OutPort< TimedDoubleSeq > * > m_wrenchesOut
naming
void stopBeep()
Definition: beep.h:29
unsigned int m_debugLevel
int size(void) const
EmergencyStopper(RTC::Manager *manager)
Constructor.
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
void registerInPort(const char *name, InPortBase &inport)
virtual bool isNew()
void get(double *x_, bool popp=true)
bool addPort(PortBase &port)
virtual bool write(DataType &value)
hrp::BodyPtr m_robot
void emergencystopper(EmergencyStopper *i_emergencystopper)
int loadBodyFromBodyInfo(::World *world, const char *_name, BodyInfo_ptr bodyInfo)
JSAMPIMAGE data
std::vector< InPort< TimedDoubleSeq > * > m_wrenchesIn
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
void EmergencyStopperInit(RTC::Manager *manager)
bool addInPort(const char *name, InPortBase &inport)
void setGoal(const double *gx, const double *gv, double time, bool online=true)
void registerOutPort(const char *name, OutPortBase &outport)
InPort< TimedLong > m_emergencySignalIn
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void set_wrenches_data_from_array(std::vector< TimedDoubleSeq > &wrenches_data, const double *wrenches_array)
#define DEBUGP
void set(const double *x, const double *v=NULL)
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
OutPort< TimedDoubleSeq > m_qOut
emergency stopper
std::vector< TimedDoubleSeq > m_wrenches


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