CollisionDetector.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <iomanip>
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/Link.h>
00013 #include <hrpModel/JointPath.h>
00014 #include <hrpUtil/Eigen3d.h>
00015 #include <hrpUtil/Eigen4d.h>
00016 #include <hrpCollision/ColdetModel.h>
00017 #ifdef USE_HRPSYSUTIL
00018 #include "hrpsys/util/GLbody.h"
00019 #include "hrpsys/util/GLutil.h"
00020 #endif // USE_HRPSYSUTIL
00021 #include "hrpsys/util/BVutil.h"
00022 #include "hrpsys/idl/RobotHardwareService.hh"
00023 
00024 #include "CollisionDetector.h"
00025 
00026 #define deg2rad(x)      ((x)*M_PI/180)
00027 #define rad2deg(x)      ((x)*180/M_PI)
00028 
00029 // Module specification
00030 // <rtc-template block="module_spec">
00031 static const char* component_spec[] =
00032 {
00033     "implementation_id", "CollisionDetector",
00034     "type_name",         "CollisionDetector",
00035     "description",       "collisoin detector component",
00036     "version",           HRPSYS_PACKAGE_VERSION,
00037     "vendor",            "AIST",
00038     "category",          "example",
00039     "activity_type",     "DataFlowComponent",
00040     "max_instance",      "10",
00041     "language",          "C++",
00042     "lang_type",         "compile",
00043     // Configuration variables
00044     "conf.default.debugLevel", "0",
00045     ""
00046 };
00047 // </rtc-template>
00048 
00049 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
00050 {
00051     int pre = os.precision();
00052     os.setf(std::ios::fixed);
00053     os << std::setprecision(6)
00054        << (tm.sec + tm.nsec/1e9)
00055        << std::setprecision(pre);
00056     os.unsetf(std::ios::fixed);
00057     return os;
00058 }
00059 
00060 CollisionDetector::CollisionDetector(RTC::Manager* manager)
00061     : RTC::DataFlowComponentBase(manager),
00062       // <rtc-template block="initializer">
00063       m_qRefIn("qRef", m_qRef),
00064       m_qCurrentIn("qCurrent", m_qCurrent),
00065       m_servoStateIn("servoStateIn", m_servoState),
00066       m_qOut("q", m_q),
00067       m_beepCommandOut("beepCommand", m_beepCommand),
00068       m_CollisionDetectorServicePort("CollisionDetectorService"),
00069       // </rtc-template>
00070 #ifdef USE_HRPSYSUTIL
00071       m_scene(&m_log),
00072       m_window(&m_scene, &m_log),
00073       m_glbody(NULL),
00074 #endif // USE_HRPSYSUTIL
00075       m_use_limb_collision(false),
00076       m_use_viewer(false),
00077       m_robot(hrp::BodyPtr()),
00078       m_loop_for_check(0),
00079       m_collision_loop(1),
00080       m_debugLevel(0),
00081       m_enable(true),
00082       collision_beep_count(0),
00083       is_beep_port_connected(false),
00084       dummy(0)
00085 {
00086     m_service0.collision(this);
00087 #ifdef USE_HRPSYSUTIL
00088     m_log.enableRingBuffer(1);
00089 #endif // USE_HRPSYSUTIL
00090     init_beep();
00091     start_beep(3136);
00092 }
00093 
00094 CollisionDetector::~CollisionDetector()
00095 {
00096   quit_beep();
00097 }
00098 
00099 
00100 
00101 RTC::ReturnCode_t CollisionDetector::onInitialize()
00102 {
00103     std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00104     // <rtc-template block="bind_config">
00105     // Bind variables and configuration variable
00106     bindParameter("debugLevel", m_debugLevel, "0");
00107   
00108     // </rtc-template>
00109 
00110     // Registration: InPort/OutPort/Service
00111     // <rtc-template block="registration">
00112     // Set InPort buffers
00113     addInPort("qRef", m_qRefIn);
00114     addInPort("qCurrent", m_qCurrentIn);
00115     addInPort("servoStateIn", m_servoStateIn);
00116 
00117     // Set OutPort buffer
00118     addOutPort("q", m_qOut);
00119     addOutPort("beepCommand", m_beepCommandOut);
00120   
00121     // Set service provider to Ports
00122     m_CollisionDetectorServicePort.registerProvider("service0", "CollisionDetectorService", m_service0);
00123   
00124     // Set service consumers to Ports
00125   
00126     // Set CORBA Service Ports
00127     addPort(m_CollisionDetectorServicePort);
00128   
00129     // </rtc-template>
00130 
00131     //RTC::Properties& prop = getProperties();
00132 
00133     RTC::Manager& rtcManager = RTC::Manager::instance();
00134     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00135     int comPos = nameServer.find(",");
00136     if (comPos < 0){
00137         comPos = nameServer.length();
00138     }
00139     nameServer = nameServer.substr(0, comPos);
00140     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00141 
00142     RTC::Properties& prop = getProperties();
00143 
00144     coil::stringTo(m_dt, prop["dt"].c_str());
00145 
00146     if ( prop["collision_viewer"] == "true" ) {
00147         m_use_viewer = true;
00148     }
00149 #ifdef USE_HRPSYSUTIL
00150     m_glbody = new GLbody();
00151     m_robot = hrp::BodyPtr(m_glbody);
00152 #else
00153     m_robot = hrp::BodyPtr(new hrp::Body());
00154 #endif // USE_HRPSYSUTIL
00155     //
00156     OpenHRP::BodyInfo_var binfo;
00157     binfo = hrp::loadBodyInfo(prop["model"].c_str(),
00158                               CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00159     if (CORBA::is_nil(binfo)){
00160         std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
00161                   << std::endl;
00162         return RTC::RTC_ERROR;
00163     }
00164 #ifdef USE_HRPSYSUTIL
00165     if (!loadBodyFromBodyInfo(m_robot, binfo, true, GLlinkFactory)) {
00166 #else
00167     if (!loadBodyFromBodyInfo(m_robot, binfo, true, hrplinkFactory)) {
00168 #endif // USE_HRPSYSUTIL
00169       std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
00170                 << m_profile.instance_name << std::endl;
00171       return RTC::RTC_ERROR;
00172     }
00173 #ifdef USE_HRPSYSUTIL
00174     loadShapeFromBodyInfo(m_glbody, binfo);
00175 #endif // USE_HRPSYSUTIL
00176     if ( prop["collision_model"] == "AABB" ) {
00177         convertToAABB(m_robot);
00178     } else if ( prop["collision_model"] == "convex hull" ||
00179                 prop["collision_model"] == "" ) { // set convex hull as default
00180         convertToConvexHull(m_robot);
00181     }
00182     setupVClipModel(m_robot);
00183 
00184     if ( prop["collision_pair"] != "" ) {
00185         std::cerr << "[" << m_profile.instance_name << "] prop[collision_pair] ->" << prop["collision_pair"] << std::endl;
00186         std::istringstream iss(prop["collision_pair"]);
00187         std::string tmp;
00188         while (getline(iss, tmp, ' ')) {
00189             size_t pos = tmp.find_first_of(':');
00190             std::string name1 = tmp.substr(0, pos), name2 = tmp.substr(pos+1);
00191             if ( m_robot->link(name1)==NULL ) {
00192                 std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name1 << std::endl;
00193                 std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
00194                 for (unsigned int i=0; i < m_robot->numLinks(); i++) {
00195                   std::cerr << " " << m_robot->link(i)->name;
00196                 }
00197                 std::cerr << std::endl;
00198                 continue;
00199             }
00200             if ( m_robot->link(name2)==NULL ) {
00201                 std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name2 << std::endl;
00202                 std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
00203                 for (unsigned int i=0; i < m_robot->numLinks(); i++) {
00204                   std::cerr << " " << m_robot->link(i)->name;
00205                 }
00206                 std::cerr << std::endl;
00207                 continue;
00208             }
00209             std::cerr << "[" << m_profile.instance_name << "] check collisions between " << m_robot->link(name1)->name << " and " <<  m_robot->link(name2)->name << std::endl;
00210             m_pair[tmp] = new CollisionLinkPair(new VclipLinkPair(m_robot->link(name1), m_VclipLinks[m_robot->link(name1)->index],
00211                                                                   m_robot->link(name2), m_VclipLinks[m_robot->link(name2)->index], 0));
00212         }
00213     }
00214 
00215     if ( prop["collision_loop"] != "" ) {
00216         coil::stringTo(m_collision_loop, prop["collision_loop"].c_str());
00217         std::cerr << "[" << m_profile.instance_name << "] set collision_loop: " << m_collision_loop << std::endl;
00218     }
00219 #ifdef USE_HRPSYSUTIL
00220     if ( m_use_viewer ) {
00221       m_scene.addBody(m_robot);
00222       GLlink::drawMode(GLlink::DM_COLLISION);
00223     }
00224 #endif // USE_HRPSYSUTIL
00225 
00226     // true (1) do not move when collide,
00227     // false(0) move even if collide
00228     m_curr_collision_mask.resize(m_robot->numJoints()); // collision_mask used to select output                0: passthough reference data, 1 output safe data
00229     std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0);
00230     m_init_collision_mask.resize(m_robot->numJoints()); // collision_mask defined in .conf as [collisoin_mask] 0: move even if collide, 1: do not move when collide
00231     std::fill(m_init_collision_mask.begin(), m_init_collision_mask.end(), 1);
00232     if ( prop["collision_mask"] != "" ) {
00233         std::cerr << "[" << m_profile.instance_name << "] prop[collision_mask] ->" << prop["collision_mask"] << std::endl;
00234         coil::vstring mask_str = coil::split(prop["collision_mask"], ",");
00235         if (mask_str.size() == m_robot->numJoints()) {
00236             for (size_t i = 0; i < m_robot->numJoints(); i++) {coil::stringTo(m_init_collision_mask[i], mask_str[i].c_str()); }
00237             for (size_t i = 0; i < m_robot->numJoints(); i++) {
00238                 if ( m_init_collision_mask[i] == 0 ) {
00239                     std::cerr << "[" << m_profile.instance_name << "] CollisionDetector will not control " << m_robot->joint(i)->name << std::endl;
00240                 }
00241             }
00242         }else{
00243             std::cerr << "[" << m_profile.instance_name << "] ERROR size of collision_mask is differ from robot joint number .. " << mask_str.size()  << ", " << m_robot->numJoints() << std::endl;
00244         }
00245     }
00246 
00247     if ( prop["use_limb_collision"] != "" ) {
00248         std::cerr << "[" << m_profile.instance_name << "] prop[use_limb_collision] -> " << prop["use_limb_collision"] << std::endl;
00249         if ( prop["use_limb_collision"] == "true" ) {
00250             m_use_limb_collision = true;
00251             std::cerr << "[" << m_profile.instance_name << "] Enable use_limb_collision" << std::endl;
00252         }
00253     }
00254 
00255     // setup collision state
00256     m_state.angle.length(m_robot->numJoints());
00257     m_state.collide.length(m_robot->numLinks());
00258 
00259     // allocate memory for outPorts
00260     m_q.data.length(m_robot->numJoints());
00261     m_recover_time = 0;
00262     m_safe_posture = true;
00263     m_have_safe_posture = false;
00264     i_dt = 1.0;
00265     default_recover_time = 2.5/m_dt;
00266     m_recover_jointdata = new double[m_robot->numJoints()];
00267     m_lastsafe_jointdata = new double[m_robot->numJoints()];
00268     m_interpolator = new interpolator(m_robot->numJoints(), i_dt);
00269     m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
00270     m_link_collision = new bool[m_robot->numLinks()];
00271 
00272     for(unsigned int i=0; i<m_robot->numJoints(); i++){
00273       m_q.data[i] = 0;
00274     }
00275 
00276     m_servoState.data.length(m_robot->numJoints());
00277     for(unsigned int i = 0; i < m_robot->numJoints(); i++) {
00278         m_servoState.data[i].length(1);
00279         int status = 0;
00280         status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
00281         status |= 1<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
00282         status |= 1<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
00283         status |= 0<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
00284         status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
00285         m_servoState.data[i][0] = status;
00286     }
00287 
00288     collision_beep_freq = static_cast<int>(1.0/(3.0*m_dt)); // 3 times / 1[s]
00289     m_beepCommand.data.length(bc.get_num_beep_info());
00290     return RTC::RTC_OK;
00291 }
00292 
00293 
00294 
00295 RTC::ReturnCode_t CollisionDetector::onFinalize()
00296 {
00297     delete[] m_recover_jointdata;
00298     delete[] m_lastsafe_jointdata;
00299     delete m_interpolator;
00300     delete[] m_link_collision;
00301     return RTC::RTC_OK;
00302 }
00303 
00304 /*
00305   RTC::ReturnCode_t CollisionDetector::onStartup(RTC::UniqueId ec_id)
00306   {
00307   return RTC::RTC_OK;
00308   }
00309 */
00310 
00311 /*
00312   RTC::ReturnCode_t CollisionDetector::onShutdown(RTC::UniqueId ec_id)
00313   {
00314   return RTC::RTC_OK;
00315   }
00316 */
00317 
00318 RTC::ReturnCode_t CollisionDetector::onActivated(RTC::UniqueId ec_id)
00319 {
00320     std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00321     m_have_safe_posture = false;
00322     return RTC::RTC_OK;
00323 }
00324 
00325 RTC::ReturnCode_t CollisionDetector::onDeactivated(RTC::UniqueId ec_id)
00326 {
00327     std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00328     return RTC::RTC_OK;
00329 }
00330 
00331 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00332 RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
00333 {
00334     static int loop = 0;
00335     loop++;
00336 
00337     // Connection check of m_beepCommand to BeeperRTC
00338     //   If m_beepCommand is not connected to BeeperRTC and sometimes, check connection.
00339     //   If once connection is found, never check connection.
00340     if (!is_beep_port_connected && (loop % 500 == 0) ) {
00341       if ( m_beepCommandOut.connectors().size() > 0 ) {
00342         is_beep_port_connected = true;
00343         quit_beep();
00344         std::cerr << "[" << m_profile.instance_name<< "] beepCommand data port connection found! Use BeeperRTC." << std::endl;
00345       }
00346     }
00347 
00348     if (m_servoStateIn.isNew()) {
00349         m_servoStateIn.read();
00350     }
00351     if ( ! m_enable ) {
00352         if ( DEBUGP || loop % 100 == 1) {
00353             std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving without checking self collision detection!!! please send enableCollisionDetection to CollisoinDetection RTC" << std::endl;
00354         }
00355         if ( m_qRefIn.isNew()) {
00356             m_qRefIn.read();
00357             for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00358                 m_q.data[i] = m_qRef.data[i];
00359             }
00360             m_q.tm = m_qRef.tm;
00361             m_qOut.write();
00362         }
00363     }
00364     if (m_enable && m_qRefIn.isNew()) {
00365         m_qRefIn.read();
00366 
00367         // check servo for collision beep sound
00368         bool has_servoOn = false;
00369         for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00370           int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
00371           has_servoOn = has_servoOn || (servo_state == 1);
00372         }
00373 
00374         TimedPosture tp;
00375 
00376         assert(m_qRef.data.length() == m_robot->numJoints());
00377 #ifdef USE_HRPSYSUTIL
00378         if ( m_use_viewer ) {
00379           for (unsigned int i=0; i<m_glbody->numLinks(); i++){
00380             ((GLlink *)m_glbody->link(i))->highlight(false);
00381           }
00382         }
00383         for (unsigned int i=0; i<m_glbody->numLinks(); i++){
00384             m_link_collision[m_glbody->link(i)->index] = false;
00385         }
00386 #else
00387         for (unsigned int i=0; i<m_robot->numLinks(); i++){
00388             m_link_collision[m_robot->link(i)->index] = false;
00389         }
00390 #endif // USE_HRPSYSUTIL
00391 
00392         //set robot model's angle for collision check(two types)
00393         //  1. current safe angle or collision angle  .. check based on qRef
00394         //  2. recovery                               .. check based on q'(m_recover_jointdata)
00395         if (m_recover_time == 0 || m_recover_time == default_recover_time ) {  // 1. current safe angle or collision angle
00396             if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
00397                 for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00398                     m_robot->joint(i)->q = m_qRef.data[i];
00399                 }
00400             }
00401         }else{  // 2. recovery
00402           for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00403             if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
00404               if ( m_curr_collision_mask[i] == 1) {// joint with 1 (do not move when collide :default), need to be updated using recover(safe) data
00405                   m_robot->joint(i)->q = m_recover_jointdata[i];
00406               }else{                               // joint with 0 (move even if collide), need to be updated using reference(dangerous) data
00407                   m_robot->joint(i)->q = m_qRef.data[i];
00408               }
00409             }
00410           }
00411         }
00412         //        }
00413         //collision check process in case of angle set above
00414         m_robot->calcForwardKinematics();
00415         coil::TimeValue tm1 = coil::gettimeofday();
00416         std::map<std::string, CollisionLinkPair *>::iterator it = m_pair.begin();
00417         for (int i = 0; it != m_pair.end(); it++, i++){
00418             int sub_size = (m_pair.size() + m_collision_loop -1) / m_collision_loop;  // 10 / 3 = 3  / floor
00419             // 0 : 0 .. sub_size-1                            // 0 .. 2
00420             // 1 : sub_size ... sub_size*2-1                  // 3 .. 5
00421             // k : sub_size*k ... sub_size*(k+1)-1            // 6 .. 8
00422             // n : sub_size*n ... m_pair.size()               // 9 .. 10
00423             if ( sub_size*m_loop_for_check <= i && i < sub_size*(m_loop_for_check+1) ) {
00424                 CollisionLinkPair* c = it->second;
00425                 c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data());
00426                 //std::cerr << i << ":" << (c->distance<=c->pair->getTolerance() ) << "/" << c->distance << " ";
00427             }
00428         }
00429         if ( m_loop_for_check == m_collision_loop-1 ) {
00430             bool last_safe_posture = m_safe_posture;
00431             m_safe_posture = true;
00432             it = m_pair.begin();
00433             for (unsigned int i = 0; it != m_pair.end(); i++, it++){
00434                 CollisionLinkPair* c = it->second;
00435                 VclipLinkPairPtr p = c->pair;
00436                 tp.lines.push_back(std::make_pair(c->point0, c->point1));
00437                 if ( c->distance <= c->pair->getTolerance() ) {
00438                     m_safe_posture = false;
00439                     if ( loop%200==0 || last_safe_posture ) {
00440                         hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
00441                         std::cerr << "[" << m_profile.instance_name << "] " << i << "/" << m_pair.size() << " pair: " << p->link(0)->name << "/" << p->link(1)->name << "(" << jointPath->numJoints() << "), distance = " << c->distance << std::endl;
00442                     }
00443                     m_link_collision[p->link(0)->index] = true;
00444                     m_link_collision[p->link(1)->index] = true;
00445                     if ( m_use_limb_collision ) {
00446                         hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
00447                         bool stop_all = true;
00448                         // if all joint is within false(0:move even if collide) in initial mask ( for example leg to leg ) we stop them
00449                         // if some joint is not within true(1:do not move within collide) on initial mask, stop only true joint (for exmple leg to arm)
00450                         for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){ if ( m_init_collision_mask[jointPath->joint(i)->jointId] == 1) stop_all = false; }
00451                         for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){
00452                             int id = jointPath->joint(i)->jointId;
00453                             // collision_mask used to select output                0: passthough reference data, 1 output safe data
00454                             if ( stop_all ) {
00455                                 m_curr_collision_mask[id] = 1;            // true (1: output safe data) do not move when collide,
00456                             } else if (m_init_collision_mask[id] == 1) {  // skip false (0: move even if collide)
00457                                 m_curr_collision_mask[id] = 1;
00458                             }
00459                         }
00460                     } else {
00461                         std::copy(m_init_collision_mask.begin(), m_init_collision_mask.end(), m_curr_collision_mask.begin()); // copy init_collision_mask to curr_collision_mask
00462                     }
00463 #ifdef USE_HRPSYSUTIL
00464                     if ( m_use_viewer ) {
00465                         ((GLlink *)p->link(0))->highlight(true);
00466                         ((GLlink *)p->link(1))->highlight(true);
00467                     }
00468 #endif // USE_HRPSYSUTIL
00469                 }
00470             }
00471             if ( m_safe_posture ) {
00472                 if (has_servoOn) {
00473                 if (! m_have_safe_posture ) {
00474                     // first transition collision -> safe
00475                     std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
00476                               << "] set safe posture" << std::endl;
00477                     for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00478                         m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
00479                     }
00480                     m_interpolator->set(m_lastsafe_jointdata); // Set current angles as initial angle for recover
00481                 }
00482                 m_have_safe_posture = true;
00483                 }
00484                 if (m_recover_time != default_recover_time) {
00485                     // sefe or recover
00486                     // in collision, robot->q may differ from m_q.
00487                     for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00488                         m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
00489                     }
00490                 }
00491             }else{
00492                 for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00493                     if ( m_curr_collision_mask[i] == 0 ) { // if collisoin_mask is 0 (move even if collide), we update lastsafe_joint_data from input data
00494                         m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
00495                     }
00496                 }
00497             }
00498             if ( m_use_limb_collision ) {
00499                if ( loop%200==0 and ! m_safe_posture ) {
00500                    std::cerr << "[" << m_profile.instance_name << "] collision_mask : ";
00501                     for (size_t i = 0; i < m_robot->numJoints(); i++) {
00502                         std::cerr << m_robot->joint(i)->name << ":"  << m_curr_collision_mask[i] << " ";
00503                     }
00504                     std::cerr << std::endl;
00505                 }
00506             }
00507         }
00508         //     mode : m_safe_posture : recover_time  : set as q
00509         // safe     :           true :            0  : qRef
00510         // collison :          false :         >  0  : q( do nothing)
00511         // recover  :           true :         >  0  : q'
00512         //std::cerr << "m_recover_time: " << m_recover_time << std::endl;
00513         coil::TimeValue tm2 = coil::gettimeofday();
00514         if (m_safe_posture && m_recover_time == 0){ // safe mode
00515           //std::cerr << "safe-------------- " << std::endl;
00516           for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00517             m_q.data[i] = m_qRef.data[i];
00518           }
00519           // collision_mask used to select output                0: passthough reference data, 1 output safe data
00520           std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0); // false(0) clear output data
00521         } else {
00522           static int collision_loop_recover = 0;
00523           if(m_safe_posture){  //recover
00524             //std::cerr << "recover-------------- " << std::endl;
00525             for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00526               m_q.data[i] = m_recover_jointdata[i];
00527             }
00528             m_recover_time = m_recover_time - i_dt;
00529           } else if (m_collision_loop > 1 && (m_recover_time != default_recover_time)) { // collision with collision_loop
00530             //std::cerr << "collision_loop-------------- " << std::endl;
00531             collision_loop_recover = m_collision_loop;
00532             m_recover_time = default_recover_time;      // m_recover_time should be set based on difference between qRef and q
00533           } else { //collision
00534             //std::cerr << "collision-------------- " << std::endl;
00535             //do nothing (stay previous m_q)
00536             m_recover_time = default_recover_time;      // m_recover_time should be set based on difference between qRef and q
00537             m_interpolator->set(m_lastsafe_jointdata); //Set last safe joint data as initial angle
00538             //m_interpolator->set(m_q.data.get_buffer()); //Set initial angle
00539           }
00540           for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00541               if (m_curr_collision_mask[i] == 0) { // 0: passthough reference data, 1 output safe data, stop joints only joint with 1
00542                   m_q.data[i] = m_qRef.data[i];
00543               }
00544           }
00545           //calc q'
00546 #if 0
00547           //linear interpolation (dangerous)
00548           for ( int i = 0; i < m_q.data.length(); i++ ) {
00549             m_recover_jointdata[i] = m_q.data[i] + (m_qRef.data[i] - m_q.data[i]) / m_recover_time;
00550           }
00551 #else
00552           if (collision_loop_recover != 0) {
00553             collision_loop_recover--;
00554             for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00555               m_q.data[i] =
00556                   m_lastsafe_jointdata[i] + collision_loop_recover * ((m_q.data[i] - m_lastsafe_jointdata[i])/m_collision_loop);
00557             }
00558           } else {
00559           collision_loop_recover = 0;
00560           //minjerk interpolation
00561           m_interpolator->setGoal(m_qRef.data.get_buffer(), m_recover_time);
00562           m_interpolator->get(m_recover_jointdata);
00563           }
00564 #endif
00565         }
00566         if ( DEBUGP ) {
00567           std::cerr << "[" << m_profile.instance_name << "] check collisions for " << m_pair.size() << " pairs in " << (tm2.sec()-tm1.sec())*1000+(tm2.usec()-tm1.usec())/1000.0 
00568                     << " [msec], safe = " << m_safe_posture << ", time = " << m_recover_time*m_dt << "[s], loop = " << m_loop_for_check << "/" << m_collision_loop << std::endl;
00569         }
00570         if ( m_pair.size() == 0 && ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) ) {
00571             std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving without checking self collision detection!!! please define collision_pair in configuration file" << std::endl;
00572         }
00573         if ( ! m_have_safe_posture && ! m_safe_posture ) {
00574             if ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) {
00575                 std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving while collision detection!!!, since we do not get safe_posture yet" << std::endl;
00576             }
00577             for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00578                 m_lastsafe_jointdata[i] = m_recover_jointdata[i] = m_q.data[i] = m_qRef.data[i];
00579             }
00580         }
00581         //
00582         m_q.tm = m_qRef.tm;
00583         m_qOut.write();
00584 
00585         // if servo off, we do not know last safe posture
00586         if (! has_servoOn ) {
00587             m_have_safe_posture = false;
00588         }
00589         // beep sound for collision alert
00590         if ( !m_safe_posture && has_servoOn ) { // If collided and some joint is servoOn
00591           if (is_beep_port_connected) {
00592             if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) bc.startBeep(2352, collision_beep_freq*0.7);
00593             else bc.stopBeep();
00594           } else {
00595             if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) start_beep(2352, collision_beep_freq*0.7);
00596             else stop_beep();
00597           }
00598           collision_beep_count++;
00599         } else {
00600           if (is_beep_port_connected) bc.stopBeep();
00601           collision_beep_count = 0;
00602         }
00603 
00604         if ( ++m_loop_for_check >= m_collision_loop ) m_loop_for_check = 0;
00605         tp.posture.resize(m_qRef.data.length());
00606         for (size_t i=0; i<tp.posture.size(); i++) tp.posture[i] = m_q.data[i];
00607 #ifdef USE_HRPSYSUTIL
00608         m_log.add(tp);
00609 #endif // USE_HRPSYSUTIL
00610 
00611         // set collisoin state
00612         m_state.time = tm2;
00613         for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00614             m_state.angle[i] = m_robot->joint(i)->q;
00615         }
00616 
00617         if ( m_loop_for_check == 0 ) {
00618             for (unsigned int i = 0; i < m_robot->numLinks(); i++ ){
00619                 m_state.collide[i] = m_link_collision[i];
00620             }
00621 
00622             m_state.lines.length(tp.lines.size());
00623             for(unsigned int i = 0; i < tp.lines.size(); i++ ){
00624                 const std::pair<hrp::Vector3, hrp::Vector3>& line = tp.lines[i];
00625                 double *v;
00626                 m_state.lines[i].length(2);
00627                 m_state.lines[i].get_buffer()[0].length(3);
00628                 v = m_state.lines[i].get_buffer()[0].get_buffer();
00629                 v[0] = line.first.data()[0];
00630                 v[1] = line.first.data()[1];
00631                 v[2] = line.first.data()[2];
00632                 m_state.lines[i].get_buffer()[1].length(3);
00633                 v = m_state.lines[i].get_buffer()[1].get_buffer();
00634                 v[0] = line.second.data()[0];
00635                 v[1] = line.second.data()[1];
00636                 v[2] = line.second.data()[2];
00637             }
00638         }
00639          m_state.computation_time = (tm2-tm1)*1000.0;
00640         m_state.safe_posture = m_safe_posture;
00641         m_state.recover_time = m_recover_time;
00642         m_state.loop_for_check = m_loop_for_check;
00643     }
00644     if (is_beep_port_connected) {
00645       bc.setDataPort(m_beepCommand);
00646       if (bc.isWritable()) m_beepCommandOut.write();
00647     }
00648 #ifdef USE_HRPSYSUTIL
00649     if ( m_use_viewer ) m_window.oneStep();
00650 #endif // USE_HRPSYSUTIL
00651     return RTC::RTC_OK;
00652 }
00653 
00654 /*
00655   RTC::ReturnCode_t CollisionDetector::onAborting(RTC::UniqueId ec_id)
00656   {
00657   return RTC::RTC_OK;
00658   }
00659 */
00660 
00661 /*
00662   RTC::ReturnCode_t CollisionDetector::onError(RTC::UniqueId ec_id)
00663   {
00664   return RTC::RTC_OK;
00665   }
00666 */
00667 
00668 /*
00669   RTC::ReturnCode_t CollisionDetector::onReset(RTC::UniqueId ec_id)
00670   {
00671   return RTC::RTC_OK;
00672   }
00673 */
00674 
00675 /*
00676   RTC::ReturnCode_t CollisionDetector::onStateUpdate(RTC::UniqueId ec_id)
00677   {
00678   return RTC::RTC_OK;
00679   }
00680 */
00681 
00682 /*
00683   RTC::ReturnCode_t CollisionDetector::onRateChanged(RTC::UniqueId ec_id)
00684   {
00685   return RTC::RTC_OK;
00686   }
00687 */
00688 
00689 bool CollisionDetector::setTolerance(const char *i_link_pair_name, double i_tolerance) {
00690     if (strcmp(i_link_pair_name, "all") == 0 || strcmp(i_link_pair_name, "ALL") == 0){
00691         for ( std::map<std::string, CollisionLinkPair *>::iterator it = m_pair.begin();  it != m_pair.end(); it++){
00692             it->second->pair->setTolerance(i_tolerance);
00693         }
00694     }else if ( m_pair.find(std::string(i_link_pair_name)) != m_pair.end() ) {
00695         m_pair[std::string(i_link_pair_name)]->pair->setTolerance(i_tolerance);
00696     }else{
00697         return false;
00698     }
00699     return true;
00700 }
00701 
00702 bool CollisionDetector::setCollisionLoop(int input_loop) {
00703     if (input_loop > 0) {
00704         m_collision_loop = input_loop;
00705         return true;
00706     }
00707     return false;
00708 }
00709 
00710 bool CollisionDetector::getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState &state)
00711 {
00712     state = m_state;
00713     return true;
00714 }
00715 
00716 void CollisionDetector::setupVClipModel(hrp::BodyPtr i_body)
00717 {
00718     m_VclipLinks.resize(i_body->numLinks());
00719     //std::cerr << i_body->numLinks() << std::endl;
00720     for (unsigned int i=0; i<i_body->numLinks(); i++) {
00721       assert(i_body->link(i)->index == i);
00722       setupVClipModel(i_body->link(i));
00723     }
00724 }
00725 
00726 bool CollisionDetector::checkIsSafeTransition(void)
00727 {
00728     for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
00729         // If servoOn, check too large joint angle gap. Otherwise (servoOff), neglect too large joint angle gap.
00730         int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; // enum SwitchStatus {SWITCH_ON, SWITCH_OFF};
00731         if (servo_state == 1 && abs(m_q.data[i] - m_qRef.data[i]) > 0.017) return false;
00732     }
00733     return true;
00734 }
00735 
00736 bool CollisionDetector::enable(void)
00737 {
00738     if (m_enable){
00739         std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is already enabled." << std::endl;
00740         return true;
00741     }
00742 
00743     if (!checkIsSafeTransition()){
00744         std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be enabled because of different reference joint angle" << std::endl;
00745         return false;
00746     }
00747 
00748     // check collision
00749     for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00750         m_robot->joint(i)->q = m_qRef.data[i];
00751     }
00752     m_robot->calcForwardKinematics();
00753     std::map<std::string, CollisionLinkPair *>::iterator it = m_pair.begin();
00754     for (unsigned int i = 0; it != m_pair.end(); it++, i++){
00755         CollisionLinkPair* c = it->second;
00756         VclipLinkPairPtr p = c->pair;
00757         c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data());
00758         if ( c->distance <= c->pair->getTolerance() ) {
00759             hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
00760             std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be enabled because of collision" << std::endl;
00761             std::cerr << "[" << m_profile.instance_name << "] " << i << "/" << m_pair.size() << " pair: " << p->link(0)->name << "/" << p->link(1)->name << "(" << jointPath->numJoints() << "), distance = " << c->distance << std::endl;
00762             return false;
00763         }
00764     }
00765     std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is successfully enabled." << std::endl;
00766 
00767     m_safe_posture = true;
00768     m_recover_time = 0;
00769     m_loop_for_check = 0;
00770     m_enable = true;
00771     return true;
00772 }
00773 
00774 bool CollisionDetector::disable(void)
00775 {
00776     if (!checkIsSafeTransition()){
00777         std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be disabled because of different reference joint angle" << std::endl;
00778         return false;
00779     }
00780     std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is successfully disabled." << std::endl;
00781     m_enable = false;
00782     return true;
00783 }
00784 
00785 void CollisionDetector::setupVClipModel(hrp::Link *i_link)
00786 {
00787     Vclip::Polyhedron* i_vclip_model = new Vclip::Polyhedron();
00788     int n = i_link->coldetModel->getNumVertices();
00789     float v[3];
00790     Vclip::VertFaceName vertName;
00791     for (int i = 0; i < n; i ++ ) {
00792         i_link->coldetModel->getVertex(i, v[0], v[1], v[2]);
00793         sprintf(vertName, "v%d", i);
00794         i_vclip_model->addVertex(vertName, Vclip::Vect3(v[0], v[1], v[2]));
00795     }
00796     i_vclip_model->buildHull();
00797     i_vclip_model->check();
00798     fprintf(stderr, "[Vclip] build finished, vcliip mesh of %s, %d -> %d\n",
00799             i_link->name.c_str(), n, (int)(i_vclip_model->verts().size()));
00800     m_VclipLinks[i_link->index] = i_vclip_model;
00801 }
00802 
00803 #ifndef USE_HRPSYSUTIL
00804 hrp::Link *hrplinkFactory()
00805 {
00806   return new hrp::Link();
00807 }
00808 #endif // USE_HRPSYSUTIL
00809 
00810 extern "C"
00811 {
00812 
00813     void CollisionDetectorInit(RTC::Manager* manager)
00814     {
00815         RTC::Properties profile(component_spec);
00816         manager->registerFactory(profile,
00817                                  RTC::Create<CollisionDetector>,
00818                                  RTC::Delete<CollisionDetector>);
00819     }
00820 
00821 };
00822 
00823 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:17