EmergencyStopper.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "hrpsys/util/VectorConvert.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/ModelLoaderUtil.h>
00013 #include <math.h>
00014 #include <hrpModel/Link.h>
00015 #include <hrpModel/Sensor.h>
00016 #include "hrpsys/idl/RobotHardwareService.hh"
00017 
00018 #include "EmergencyStopper.h"
00019 #include <iomanip>
00020 
00021 typedef coil::Guard<coil::Mutex> Guard;
00022 
00023 // Module specification
00024 // <rtc-template block="module_spec">
00025 static const char* emergencystopper_spec[] =
00026 {
00027     "implementation_id", "EmergencyStopper",
00028     "type_name",         "EmergencyStopper",
00029     "description",       "emergency stopper",
00030     "version",           HRPSYS_PACKAGE_VERSION,
00031     "vendor",            "AIST",
00032     "category",          "example",
00033     "activity_type",     "DataFlowComponent",
00034     "max_instance",      "10",
00035     "language",          "C++",
00036     "lang_type",         "compile",
00037     // Configuration variables
00038     "conf.default.debugLevel", "0",
00039     ""
00040 };
00041 // </rtc-template>
00042 
00043 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
00044 {
00045     int pre = os.precision();
00046     os.setf(std::ios::fixed);
00047     os << std::setprecision(6)
00048        << (tm.sec + tm.nsec/1e9)
00049        << std::setprecision(pre);
00050     os.unsetf(std::ios::fixed);
00051     return os;
00052 }
00053 
00054 EmergencyStopper::EmergencyStopper(RTC::Manager* manager)
00055     : RTC::DataFlowComponentBase(manager),
00056       // <rtc-template block="initializer">
00057       m_qRefIn("qRef", m_qRef),
00058       m_emergencySignalIn("emergencySignal", m_emergencySignal),
00059       m_servoStateIn("servoStateIn", m_servoState),
00060       m_qOut("q", m_q),
00061       m_emergencyModeOut("emergencyMode", m_emergencyMode),
00062       m_beepCommandOut("beepCommand", m_beepCommand),
00063       m_EmergencyStopperServicePort("EmergencyStopperService"),
00064       // </rtc-template>
00065       m_robot(hrp::BodyPtr()),
00066       m_debugLevel(0),
00067       loop(0),
00068       emergency_stopper_beep_count(0),
00069       dummy(0)
00070 {
00071     m_service0.emergencystopper(this);
00072 }
00073 
00074 EmergencyStopper::~EmergencyStopper()
00075 {
00076 }
00077 
00078 
00079 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00080 RTC::ReturnCode_t EmergencyStopper::onInitialize()
00081 {
00082     std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00083     // <rtc-template block="bind_config">
00084     // Bind variables and configuration variable
00085     bindParameter("debugLevel", m_debugLevel, "0");
00086 
00087     // Registration: InPort/OutPort/Service
00088     // <rtc-template block="registration">
00089     // Set InPort buffers
00090     addInPort("qRef", m_qRefIn);
00091     addInPort("emergencySignal", m_emergencySignalIn);
00092     addInPort("servoStateIn", m_servoStateIn);
00093 
00094     // Set OutPort buffer
00095     addOutPort("q", m_qOut);
00096     addOutPort("emergencyMode", m_emergencyModeOut);
00097     addOutPort("beepCommand", m_beepCommandOut);
00098 
00099     // Set service provider to Ports
00100     m_EmergencyStopperServicePort.registerProvider("service0", "EmergencyStopperService", m_service0);
00101 
00102     // Set service consumers to Ports
00103 
00104     // Set CORBA Service Ports
00105     addPort(m_EmergencyStopperServicePort);
00106 
00107     // </rtc-template>
00108 
00109     // Setup robot model
00110     RTC::Properties& prop = getProperties();
00111     coil::stringTo(m_dt, prop["dt"].c_str());
00112     m_robot = hrp::BodyPtr(new hrp::Body());
00113 
00114     RTC::Manager& rtcManager = RTC::Manager::instance();
00115     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00116     int comPos = nameServer.find(",");
00117     if (comPos < 0){
00118         comPos = nameServer.length();
00119     }
00120     nameServer = nameServer.substr(0, comPos);
00121     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00122     OpenHRP::BodyInfo_var binfo;
00123     binfo = hrp::loadBodyInfo(prop["model"].c_str(),
00124                               CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00125     if (CORBA::is_nil(binfo)) {
00126         std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
00127                   << std::endl;
00128         return RTC::RTC_ERROR;
00129     }
00130     if (!loadBodyFromBodyInfo(m_robot, binfo)) {
00131         std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
00132         return RTC::RTC_ERROR;
00133     }
00134 
00135     // Setting for wrench data ports (real + virtual)
00136     OpenHRP::LinkInfoSequence_var lis = binfo->links();
00137     std::vector<std::string> fsensor_names;
00138     //   find names for real force sensors
00139     for ( unsigned int k = 0; k < lis->length(); k++ ) {
00140         OpenHRP::SensorInfoSequence& sensors = lis[k].sensors;
00141         for ( unsigned int l = 0; l < sensors.length(); l++ ) {
00142             if ( std::string(sensors[l].type) == "Force" ) {
00143                 fsensor_names.push_back(std::string(sensors[l].name));
00144             }
00145         }
00146     }
00147     unsigned int npforce = fsensor_names.size();
00148     //   find names for virtual force sensors
00149     coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
00150     unsigned int nvforce = virtual_force_sensor.size()/10;
00151     for (unsigned int i=0; i<nvforce; i++){
00152         fsensor_names.push_back(virtual_force_sensor[i*10+0]);
00153     }
00154     //   add ports for all force sensors
00155     unsigned int nforce  = npforce + nvforce;
00156     m_wrenchesRef.resize(nforce);
00157     m_wrenches.resize(nforce);
00158     m_wrenchesIn.resize(nforce);
00159     m_wrenchesOut.resize(nforce);
00160     for (unsigned int i=0; i<nforce; i++){
00161         m_wrenchesIn[i] = new InPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"In").c_str(), m_wrenchesRef[i]);
00162         m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Out").c_str(), m_wrenches[i]);
00163         m_wrenchesRef[i].data.length(6);
00164         m_wrenchesRef[i].data[0] = m_wrenchesRef[i].data[1] = m_wrenchesRef[i].data[2] = 0.0;
00165         m_wrenchesRef[i].data[3] = m_wrenchesRef[i].data[4] = m_wrenchesRef[i].data[5] = 0.0;
00166         m_wrenches[i].data.length(6);
00167         m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
00168         m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
00169         registerInPort(std::string(fsensor_names[i]+"In").c_str(), *m_wrenchesIn[i]);
00170         registerOutPort(std::string(fsensor_names[i]+"Out").c_str(), *m_wrenchesOut[i]);
00171     }
00172 
00173     // initialize member variables
00174     is_stop_mode = prev_is_stop_mode = false;
00175     is_initialized = false;
00176 
00177     recover_time = retrieve_time = 0;
00178     recover_time_dt = 1.0;
00179     default_recover_time = 2.5/m_dt;
00180     default_retrieve_time = 1;
00181     //default_retrieve_time = 1.0/m_dt;
00182     m_stop_posture = new double[m_robot->numJoints()];
00183     m_stop_wrenches = new double[nforce*6];
00184     m_tmp_wrenches = new double[nforce*6];
00185     m_interpolator = new interpolator(m_robot->numJoints(), recover_time_dt);
00186     m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
00187     m_wrenches_interpolator = new interpolator(nforce*6, recover_time_dt);
00188     m_wrenches_interpolator->setName(std::string(m_profile.instance_name)+" interpolator wrenches");
00189 
00190     m_q.data.length(m_robot->numJoints());
00191     for(unsigned int i=0; i<m_robot->numJoints(); i++){
00192         m_q.data[i] = 0;
00193         m_stop_posture[i] = 0;
00194     }
00195     for(unsigned int i=0; i<nforce; i++){
00196         for(int j=0; j<6; j++){
00197             m_wrenches[i].data[j] = 0;
00198             m_stop_wrenches[i*6+j] = 0;
00199         }
00200     }
00201 
00202     m_servoState.data.length(m_robot->numJoints());
00203     for(unsigned int i = 0; i < m_robot->numJoints(); i++) {
00204         m_servoState.data[i].length(1);
00205         int status = 0;
00206         status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
00207         status |= 1<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
00208         status |= 1<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
00209         status |= 0<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
00210         status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
00211         m_servoState.data[i][0] = status;
00212     }
00213 
00214     emergency_stopper_beep_freq = static_cast<int>(1.0/(2.0*m_dt)); // 2 times / 1[s]
00215     m_beepCommand.data.length(bc.get_num_beep_info());
00216     return RTC::RTC_OK;
00217 }
00218 
00219 
00220 
00221 
00222 RTC::ReturnCode_t EmergencyStopper::onFinalize()
00223 {
00224     delete m_interpolator;
00225     delete m_wrenches_interpolator;
00226     delete m_stop_posture;
00227     delete m_stop_wrenches;
00228     delete m_tmp_wrenches;
00229     return RTC::RTC_OK;
00230 }
00231 
00232 /*
00233   RTC::ReturnCode_t EmergencyStopper::onStartup(RTC::UniqueId ec_id)
00234   {
00235   return RTC::RTC_OK;
00236   }
00237 */
00238 
00239 /*
00240   RTC::ReturnCode_t EmergencyStopper::onShutdown(RTC::UniqueId ec_id)
00241   {
00242   return RTC::RTC_OK;
00243   }
00244 */
00245 
00246 RTC::ReturnCode_t EmergencyStopper::onActivated(RTC::UniqueId ec_id)
00247 {
00248     std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00249     return RTC::RTC_OK;
00250 }
00251 
00252 RTC::ReturnCode_t EmergencyStopper::onDeactivated(RTC::UniqueId ec_id)
00253 {
00254     std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00255     Guard guard(m_mutex);
00256     if (is_stop_mode) {
00257         is_stop_mode = false;
00258         recover_time = 0;
00259         m_interpolator->setGoal(m_qRef.data.get_buffer(), m_dt);
00260         m_interpolator->get(m_q.data.get_buffer());
00261     }
00262     return RTC::RTC_OK;
00263 }
00264 
00265 RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id)
00266 {
00267     unsigned int numJoints = m_robot->numJoints();
00268     loop++;
00269     if (m_servoStateIn.isNew()) {
00270         m_servoStateIn.read();
00271     }
00272     if (!is_initialized) {
00273         if (m_qRefIn.isNew()) {
00274             m_qRefIn.read();
00275             is_initialized = true;
00276         } else {
00277             return RTC::RTC_OK;
00278         }
00279     }
00280 
00281     // read data
00282     if (m_qRefIn.isNew()) {
00283         // joint angle
00284         m_qRefIn.read();
00285         assert(m_qRef.data.length() == numJoints);
00286         std::vector<double> current_posture;
00287         for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ) {
00288             current_posture.push_back(m_qRef.data[i]);
00289         }
00290         m_input_posture_queue.push(current_posture);
00291         while ((int)m_input_posture_queue.size() > default_retrieve_time) {
00292             m_input_posture_queue.pop();
00293         }
00294         if (!is_stop_mode) {
00295             for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ) {
00296                 if (recover_time > 0) { // Until releasing is finished, do not use m_stop_posture in input queue because too large error.
00297                     m_stop_posture[i] = m_q.data[i];
00298                 } else {
00299                     m_stop_posture[i] = m_input_posture_queue.front()[i];
00300                 }
00301             }
00302         }
00303         // wrench
00304         for ( size_t i = 0; i < m_wrenchesIn.size(); i++ ) {
00305             if ( m_wrenchesIn[i]->isNew() ) {
00306                 m_wrenchesIn[i]->read();
00307             }
00308         }
00309         std::vector<double> current_wrench;
00310         for ( unsigned int i= 0; i < m_wrenchesRef.size(); i++ ) {
00311             for (int j = 0; j < 6; j++ ) {
00312                 current_wrench.push_back(m_wrenchesRef[i].data[j]);
00313             }
00314         }
00315         m_input_wrenches_queue.push(current_wrench);
00316         while ((int)m_input_wrenches_queue.size() > default_retrieve_time) {
00317             m_input_wrenches_queue.pop();
00318         }
00319         if (!is_stop_mode) {
00320             for ( unsigned int i= 0; i < m_wrenchesRef.size(); i++ ) {
00321                 for (int j = 0; j < 6; j++ ) {
00322                     if (recover_time > 0) {
00323                         m_stop_wrenches[i*6+j] = m_wrenches[i].data[j];
00324                     } else {
00325                         m_stop_wrenches[i*6+j] = m_input_wrenches_queue.front()[i*6+j];
00326                     }
00327                 }
00328             }
00329         }
00330     }
00331 
00332     if (m_emergencySignalIn.isNew()){
00333         m_emergencySignalIn.read();
00334         if ( m_emergencySignal.data == 0 ) {
00335             Guard guard(m_mutex);
00336             std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
00337                       << "] emergencySignal is reset!" << std::endl;
00338             is_stop_mode = false;
00339         } else if (!is_stop_mode) {
00340             Guard guard(m_mutex);
00341             std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
00342                       << "] emergencySignal is set!" << std::endl;
00343             is_stop_mode = true;
00344         }
00345     }
00346     if (is_stop_mode && !prev_is_stop_mode) {
00347         retrieve_time = default_retrieve_time;
00348         // Reflect current output joint angles to interpolator state
00349         m_interpolator->set(m_q.data.get_buffer());
00350         get_wrenches_array_from_data(m_wrenches, m_tmp_wrenches);
00351         m_wrenches_interpolator->set(m_tmp_wrenches);
00352     }
00353 
00354     if (DEBUGP) {
00355         std::cerr << "[" << m_profile.instance_name << "] is_stop_mode : " << is_stop_mode << " recover_time : "  << recover_time << "[s] retrieve_time : " << retrieve_time << "[s]" << std::endl;
00356     }
00357 
00358     //     mode : is_stop_mode : recover_time  : set as q
00359     // release  :        false :            0  : qRef
00360     // recover  :        false :         >  0  : q'
00361     // stop     :         true :  do not care  : q(do nothing)
00362     if (!is_stop_mode) {
00363         if (recover_time > 0) {
00364             recover_time = recover_time - recover_time_dt;
00365             m_interpolator->setGoal(m_qRef.data.get_buffer(), recover_time);
00366             m_interpolator->get(m_q.data.get_buffer());
00367             get_wrenches_array_from_data(m_wrenchesRef, m_tmp_wrenches);
00368             m_wrenches_interpolator->setGoal(m_tmp_wrenches, recover_time);
00369             m_wrenches_interpolator->get(m_tmp_wrenches);
00370             set_wrenches_data_from_array(m_wrenches, m_tmp_wrenches);
00371         } else {
00372             for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00373                 m_q.data[i] = m_qRef.data[i];
00374             }
00375             for ( unsigned int i = 0; i < m_wrenches.size(); i++ ) {
00376                 for ( int j = 0; j < 6; j++ ) {
00377                     m_wrenches[i].data[j] = m_wrenchesRef[i].data[j];
00378                 }
00379             }
00380         }
00381     } else { // stop mode
00382         recover_time = default_recover_time;
00383         if (retrieve_time > 0 ) {
00384             retrieve_time = retrieve_time - recover_time_dt;
00385             m_interpolator->setGoal(m_stop_posture, retrieve_time);
00386             m_interpolator->get(m_q.data.get_buffer());
00387             m_wrenches_interpolator->setGoal(m_stop_wrenches, retrieve_time);
00388             m_wrenches_interpolator->get(m_tmp_wrenches);
00389             set_wrenches_data_from_array(m_wrenches, m_tmp_wrenches);
00390         } else {
00391             // Do nothing
00392         }
00393     }
00394 
00395     // write data
00396     if (DEBUGP) {
00397         std::cerr << "q: ";
00398         for (unsigned int i = 0; i < numJoints; i++) {
00399             std::cerr << " " << m_q.data[i] ;
00400         }
00401         std::cerr << std::endl;
00402         std::cerr << "wrenches: ";
00403         for (unsigned int i = 0; i < m_wrenches.size(); i++) {
00404             for (int j = 0; j < 6; j++ ) {
00405                 std::cerr << " " << m_wrenches[i].data[j];
00406             }
00407         }
00408         std::cerr << std::endl;
00409     }
00410     m_q.tm = m_qRef.tm;
00411     m_qOut.write();
00412     for (size_t i = 0; i < m_wrenches.size(); i++) {
00413       m_wrenches[i].tm = m_qRef.tm;
00414     }
00415     for (size_t i = 0; i < m_wrenchesOut.size(); i++) {
00416       m_wrenchesOut[i]->write();
00417     }
00418 
00419     m_emergencyMode.data = is_stop_mode;
00420     m_emergencyMode.tm = m_qRef.tm;
00421     m_emergencyModeOut.write();
00422 
00423     prev_is_stop_mode = is_stop_mode;
00424 
00425     // beep sound for emergency stop alert
00426     //  check servo for emergency stop beep sound
00427     bool has_servoOn = false;
00428     for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00429         int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
00430         has_servoOn = has_servoOn || (servo_state == 1);
00431     }
00432     //  beep
00433     if ( is_stop_mode && has_servoOn ) { // If stop mode and some joint is servoOn
00434       if ( emergency_stopper_beep_count % emergency_stopper_beep_freq == 0 && emergency_stopper_beep_count % (emergency_stopper_beep_freq * 3) != 0 ) {
00435         bc.startBeep(2352, emergency_stopper_beep_freq*0.7);
00436       } else {
00437         bc.stopBeep();
00438       }
00439         emergency_stopper_beep_count++;
00440     } else {
00441         emergency_stopper_beep_count = 0;
00442         bc.stopBeep();
00443     }
00444     bc.setDataPort(m_beepCommand);
00445     m_beepCommand.tm = m_qRef.tm;
00446     if (bc.isWritable()) m_beepCommandOut.write();
00447     return RTC::RTC_OK;
00448 }
00449 
00450 /*
00451   RTC::ReturnCode_t EmergencyStopper::onAborting(RTC::UniqueId ec_id)
00452   {
00453   return RTC::RTC_OK;
00454   }
00455 */
00456 
00457 /*
00458   RTC::ReturnCode_t EmergencyStopper::onError(RTC::UniqueId ec_id)
00459   {
00460   return RTC::RTC_OK;
00461   }
00462 */
00463 
00464 /*
00465   RTC::ReturnCode_t EmergencyStopper::onReset(RTC::UniqueId ec_id)
00466   {
00467   return RTC::RTC_OK;
00468   }
00469 */
00470 
00471 /*
00472   RTC::ReturnCode_t EmergencyStopper::onStateUpdate(RTC::UniqueId ec_id)
00473   {
00474   return RTC::RTC_OK;
00475   }
00476 */
00477 
00478 /*
00479   RTC::ReturnCode_t EmergencyStopper::onRateChanged(RTC::UniqueId ec_id)
00480   {
00481   return RTC::RTC_OK;
00482   }
00483 */
00484 
00485 bool EmergencyStopper::stopMotion()
00486 {
00487     Guard guard(m_mutex);
00488     if (!is_stop_mode) {
00489         is_stop_mode = true;
00490         std::cerr << "[" << m_profile.instance_name << "] stopMotion is called" << std::endl;
00491     }
00492     return true;
00493 }
00494 
00495 bool EmergencyStopper::releaseMotion()
00496 {
00497     Guard guard(m_mutex);
00498     if (is_stop_mode) {
00499         is_stop_mode = false;
00500         std::cerr << "[" << m_profile.instance_name << "] releaseMotion is called" << std::endl;
00501     }
00502     return true;
00503 }
00504 
00505 bool EmergencyStopper::getEmergencyStopperParam(OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param)
00506 {
00507     std::cerr << "[" << m_profile.instance_name << "] getEmergencyStopperParam" << std::endl;
00508     i_param.default_recover_time = default_recover_time*m_dt;
00509     i_param.default_retrieve_time = default_retrieve_time*m_dt;
00510     i_param.is_stop_mode = is_stop_mode;
00511     return true;
00512 };
00513 
00514 bool EmergencyStopper::setEmergencyStopperParam(const OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param)
00515 {
00516     std::cerr << "[" << m_profile.instance_name << "] setEmergencyStopperParam" << std::endl;
00517     default_recover_time = i_param.default_recover_time/m_dt;
00518     default_retrieve_time = i_param.default_retrieve_time/m_dt;
00519     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;
00520     return true;
00521 };
00522 
00523 extern "C"
00524 {
00525 
00526     void EmergencyStopperInit(RTC::Manager* manager)
00527     {
00528         RTC::Properties profile(emergencystopper_spec);
00529         manager->registerFactory(profile,
00530                                  RTC::Create<EmergencyStopper>,
00531                                  RTC::Delete<EmergencyStopper>);
00532     }
00533 
00534 };
00535 
00536 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:54