RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
{
    static int loop = 0;
    loop++;
    if (m_qRefIn.isNew()) {
	m_qRefIn.read();

        TimedPosture tp;
        tp.time = 0;

	assert(m_qRef.data.length() == m_robot->numJoints());
        if ( m_use_viewer ) {
          for (int i=0; i<m_glbody->numLinks(); i++){
            ((GLlink *)m_glbody->link(i))->highlight(false);
          }
        }
        //set robot model's angle for collision check(two types)
        //  1. current safe angle .. check based on qRef
        //  2. recovery or collision angle .. check based on q'(m_recover_jointdata)
        if (m_safe_posture && m_recover_time == 0) {           // 1. current safe angle
          for ( int i = 0; i < m_robot->numJoints(); i++ ){
	    m_robot->joint(i)->q = m_qRef.data[i];
          }
        }else{   // recovery or collision angle
          for ( int i = 0; i < m_robot->numJoints(); i++ ){
	    m_robot->joint(i)->q = m_recover_jointdata[i];
          }
        }
        //collision check process in case of angle set above
	m_robot->calcForwardKinematics();
        m_safe_posture = true;
	coil::TimeValue tm1 = coil::gettimeofday();
        std::map<std::string, VclipLinkPairPtr>::iterator it = m_pair.begin();
	for (unsigned int i = 0; it != m_pair.end(); i++, it++){
	    VclipLinkPairPtr p = it->second;
            hrp::Vector3 point0(0,0,0), point1(0,0,0);
	    double d = p->computeDistance(point0.data(), point1.data());
            tp.lines.push_back(std::make_pair(point0, point1));
	    if ( d <= p->getTolerance() ) {
		m_safe_posture = false;
		hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
		std::cerr << i << "/" << m_pair.size() << " pair: " << p->link(0)->name << "/" << p->link(1)->name << "(" << jointPath->numJoints() << "), distance = " << d << std::endl;
              if ( m_use_viewer ) {
                ((GLlink *)p->link(0))->highlight(true);
                ((GLlink *)p->link(1))->highlight(true);
              }
	    }
	}
        //     mode : m_safe_posture : recover_time  : set as q
        // safe     :           true :            0  : qRef
        // collison :          false :         >  0  : q( do nothing)
        // recover  :           true :         >  0  : q'
        //std::cerr << "m_recover_time: " << m_recover_time << std::endl;
        coil::TimeValue tm2 = coil::gettimeofday();
        if (m_safe_posture && m_recover_time == 0){ // safe mode
          //std::cerr << "safe-------------- " << std::endl;
          for ( int i = 0; i < m_q.data.length(); i++ ) {
            m_q.data[i] = m_qRef.data[i];
          }
        } else {
          if(m_safe_posture){  //recover
            //std::cerr << "recover-------------- " << std::endl;
            for ( int i = 0; i < m_q.data.length(); i++ ) {
              m_q.data[i] = m_recover_jointdata[i];
            }
            m_recover_time = m_recover_time - i_dt;
          }else{ //collision
            //std::cerr << "collision-------------- " << std::endl;
            //do nothing (stay previous m_q)
            m_recover_time = default_recover_time;      // m_recover_time should be set based on difference between qRef and q
            m_interpolator->set(m_q.data.get_buffer()); //Set initial angle
          }
          //calc q'
#if 0
          //linear interpolation (dangerous)
          for ( int i = 0; i < m_q.data.length(); i++ ) {
            m_recover_jointdata[i] = m_q.data[i] + (m_qRef.data[i] - m_q.data[i]) / m_recover_time;
          }
#else
          //minjerk interpolation
          m_interpolator->setGoal(m_qRef.data.get_buffer(), m_recover_time);
          m_interpolator->get(m_recover_jointdata);
#endif
        }
        if ( DEBUGP ) {
          std::cerr << "check collisions for for " << m_pair.size() << " pairs in " << (tm2.sec()-tm1.sec())*1000+(tm2.usec()-tm1.usec())/1000.0 
                    << " [msec], safe = " << m_safe_posture << ", time = " << m_recover_time << " " << m_q.data[0] << " " << m_q.data[1] 
                    << " " << m_q.data[2] << " " << m_q.data[3] << " " << m_q.data[4] << " " << m_q.data[5] << " " << m_q.data[6] << std::endl;
        }
        //
        m_qOut.write();

        tp.posture.resize(m_qRef.data.length());
        for (size_t i=0; i<tp.posture.size(); i++) tp.posture[i] = m_q.data[i];
        m_log.add(tp);
    }
    if ( m_use_viewer ) m_window.oneStep();
    return RTC::RTC_OK;
}
Пример #2
0
RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
{
    static int loop = 0;
    loop++;

    // Connection check of m_beepCommand to BeeperRTC
    //   If m_beepCommand is not connected to BeeperRTC and sometimes, check connection.
    //   If once connection is found, never check connection.
    if (!is_beep_port_connected && (loop % 500 == 0) ) {
      if ( m_beepCommandOut.connectors().size() > 0 ) {
        is_beep_port_connected = true;
        std::cerr << "[" << m_profile.instance_name<< "] beepCommand data port connection found! Use BeeperRTC." << std::endl;
      }
    }

    if (m_servoStateIn.isNew()) {
        m_servoStateIn.read();
    }
    if ( ! m_enable ) {
        if ( DEBUGP || loop % 100 == 1) {
            std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving without checking self collision detection!!! please send enableCollisionDetection to CollisoinDetection RTC" << std::endl;
        }
        if ( m_qRefIn.isNew()) {
            m_qRefIn.read();
            for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
                m_q.data[i] = m_qRef.data[i];
            }
            m_q.tm = m_qRef.tm;
            m_qOut.write();
        }
    }
    if (m_enable && m_qRefIn.isNew()) {
	m_qRefIn.read();

        // check servo for collision beep sound
        bool has_servoOn = false;
        for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
          int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
          has_servoOn = has_servoOn || (servo_state == 1);
        }

        TimedPosture tp;

	assert(m_qRef.data.length() == m_robot->numJoints());
#ifdef USE_HRPSYSUTIL
        if ( m_use_viewer ) {
          for (unsigned int i=0; i<m_glbody->numLinks(); i++){
            ((GLlink *)m_glbody->link(i))->highlight(false);
          }
        }
        for (unsigned int i=0; i<m_glbody->numLinks(); i++){
            m_link_collision[m_glbody->link(i)->index] = false;
        }
#else
        for (unsigned int i=0; i<m_robot->numLinks(); i++){
            m_link_collision[m_robot->link(i)->index] = false;
        }
#endif // USE_HRPSYSUTIL

        //set robot model's angle for collision check(two types)
        //  1. current safe angle or collision angle  .. check based on qRef
        //  2. recovery                               .. check based on q'(m_recover_jointdata)
        if (m_recover_time == 0 || m_recover_time == default_recover_time ) {  // 1. current safe angle or collision angle
            if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
                for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
                    m_robot->joint(i)->q = m_qRef.data[i];
                }
            }
        }else{  // 2. recovery
          for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
            if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
              if ( m_curr_collision_mask[i] == 1) {// joint with 1 (do not move when collide :default), need to be updated using recover(safe) data
                  m_robot->joint(i)->q = m_recover_jointdata[i];
              }else{                               // joint with 0 (move even if collide), need to be updated using reference(dangerous) data
                  m_robot->joint(i)->q = m_qRef.data[i];
              }
            }
          }
        }
        //        }
        //collision check process in case of angle set above
	m_robot->calcForwardKinematics();
	coil::TimeValue tm1 = coil::gettimeofday();
        std::map<std::string, CollisionLinkPair *>::iterator it = m_pair.begin();
	for (int i = 0; it != m_pair.end(); it++, i++){
            int sub_size = (m_pair.size() + m_collision_loop -1) / m_collision_loop;  // 10 / 3 = 3  / floor
            // 0 : 0 .. sub_size-1                            // 0 .. 2
            // 1 : sub_size ... sub_size*2-1                  // 3 .. 5
            // k : sub_size*k ... sub_size*(k+1)-1            // 6 .. 8
            // n : sub_size*n ... m_pair.size()               // 9 .. 10
            if ( sub_size*m_loop_for_check <= i && i < sub_size*(m_loop_for_check+1) ) {
                CollisionLinkPair* c = it->second;
                c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data());
                //std::cerr << i << ":" << (c->distance<=c->pair->getTolerance() ) << "/" << c->distance << " ";
            }
        }
        if ( m_loop_for_check == m_collision_loop-1 ) {
            bool last_safe_posture = m_safe_posture;
            m_safe_posture = true;
            it = m_pair.begin();
            for (unsigned int i = 0; it != m_pair.end(); i++, it++){
                CollisionLinkPair* c = it->second;
                VclipLinkPairPtr p = c->pair;
                tp.lines.push_back(std::make_pair(c->point0, c->point1));
                if ( c->distance <= c->pair->getTolerance() ) {
                    m_safe_posture = false;
                    if ( loop%200==0 || last_safe_posture ) {
                        hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
                        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;
                    }
                    m_link_collision[p->link(0)->index] = true;
                    m_link_collision[p->link(1)->index] = true;
                    if ( m_use_limb_collision ) {
                        hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
                        bool stop_all = true;
                        // if all joint is within false(0:move even if collide) in initial mask ( for example leg to leg ) we stop them
                        // 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)
                        for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){ if ( m_init_collision_mask[jointPath->joint(i)->jointId] == 1) stop_all = false; }
                        for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){
                            int id = jointPath->joint(i)->jointId;
                            // collision_mask used to select output                0: passthough reference data, 1 output safe data
                            if ( stop_all ) {
                                m_curr_collision_mask[id] = 1;            // true (1: output safe data) do not move when collide,
                            } else if (m_init_collision_mask[id] == 1) {  // skip false (0: move even if collide)
                                m_curr_collision_mask[id] = 1;
                            }
                        }
                    } else {
                        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
                    }
#ifdef USE_HRPSYSUTIL
                    if ( m_use_viewer ) {
                        ((GLlink *)p->link(0))->highlight(true);
                        ((GLlink *)p->link(1))->highlight(true);
                    }
#endif // USE_HRPSYSUTIL
                }
            }
            if ( m_safe_posture ) {
                if (has_servoOn) {
                if (! m_have_safe_posture ) {
                    // first transition collision -> safe
                    std::cerr << "[" << m_profile.instance_name << "] set safe posture" << std::endl;
                    for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
                        m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
                    }
                    m_interpolator->set(m_lastsafe_jointdata); // Set current angles as initial angle for recover
                }
                m_have_safe_posture = true;
                }
                if (m_recover_time != default_recover_time) {
                    // sefe or recover
                    // in collision, robot->q may differ from m_q.
                    for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
                        m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
                    }
                }
            }else{
                for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
                    if ( m_curr_collision_mask[i] == 0 ) { // if collisoin_mask is 0 (move even if collide), we update lastsafe_joint_data from input data
                        m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
                    }
                }
            }
            if ( m_use_limb_collision ) {
               if ( loop%200==0 and ! m_safe_posture ) {
                   std::cerr << "[" << m_profile.instance_name << "] collision_mask : ";
                    for (size_t i = 0; i < m_robot->numJoints(); i++) {
                        std::cerr << m_robot->joint(i)->name << ":"  << m_curr_collision_mask[i] << " ";
                    }
                    std::cerr << std::endl;
                }
            }
        }
        //     mode : m_safe_posture : recover_time  : set as q
        // safe     :           true :            0  : qRef
        // collison :          false :         >  0  : q( do nothing)
        // recover  :           true :         >  0  : q'
        //std::cerr << "m_recover_time: " << m_recover_time << std::endl;
        coil::TimeValue tm2 = coil::gettimeofday();
        if (m_safe_posture && m_recover_time == 0){ // safe mode
          //std::cerr << "safe-------------- " << std::endl;
          for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
            m_q.data[i] = m_qRef.data[i];
          }
          // collision_mask used to select output                0: passthough reference data, 1 output safe data
          std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0); // false(0) clear output data
        } else {
          static int collision_loop_recover = 0;
          if(m_safe_posture){  //recover
            //std::cerr << "recover-------------- " << std::endl;
            for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
              m_q.data[i] = m_recover_jointdata[i];
            }
            m_recover_time = m_recover_time - i_dt;
          } else if (m_collision_loop > 1 && (m_recover_time != default_recover_time)) { // collision with collision_loop
            //std::cerr << "collision_loop-------------- " << std::endl;
            collision_loop_recover = m_collision_loop;
            m_recover_time = default_recover_time;      // m_recover_time should be set based on difference between qRef and q
          } else { //collision
            //std::cerr << "collision-------------- " << std::endl;
            //do nothing (stay previous m_q)
            m_recover_time = default_recover_time;      // m_recover_time should be set based on difference between qRef and q
            m_interpolator->set(m_lastsafe_jointdata); //Set last safe joint data as initial angle
            //m_interpolator->set(m_q.data.get_buffer()); //Set initial angle
          }
          for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
              if (m_curr_collision_mask[i] == 0) { // 0: passthough reference data, 1 output safe data, stop joints only joint with 1
                  m_q.data[i] = m_qRef.data[i];
              }
          }
          //calc q'
#if 0
          //linear interpolation (dangerous)
          for ( int i = 0; i < m_q.data.length(); i++ ) {
            m_recover_jointdata[i] = m_q.data[i] + (m_qRef.data[i] - m_q.data[i]) / m_recover_time;
          }
#else
          if (collision_loop_recover != 0) {
            collision_loop_recover--;
            for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
              m_q.data[i] =
                  m_lastsafe_jointdata[i] + collision_loop_recover * ((m_q.data[i] - m_lastsafe_jointdata[i])/m_collision_loop);
            }
          } else {
          collision_loop_recover = 0;
          //minjerk interpolation
          m_interpolator->setGoal(m_qRef.data.get_buffer(), m_recover_time);
          m_interpolator->get(m_recover_jointdata);
          }
#endif
        }
        if ( DEBUGP ) {
          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 
                    << " [msec], safe = " << m_safe_posture << ", time = " << m_recover_time*m_dt << "[s], loop = " << m_loop_for_check << "/" << m_collision_loop << std::endl;
        }
        if ( m_pair.size() == 0 && ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) ) {
            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;
        }
        if ( ! m_have_safe_posture && ! m_safe_posture ) {
            if ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) {
                std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving while collision detection!!!, since we do not get safe_posture yet" << std::endl;
            }
            for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
                m_lastsafe_jointdata[i] = m_recover_jointdata[i] = m_q.data[i] = m_qRef.data[i];
            }
        }
        //
        m_q.tm = m_qRef.tm;
        m_qOut.write();

        // if servo off, we do not know last safe posture
        if (! has_servoOn ) {
            m_have_safe_posture = false;
        }
        // beep sound for collision alert
        if ( !m_safe_posture && has_servoOn ) { // If collided and some joint is servoOn
          if (is_beep_port_connected) {
            if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) bc.startBeep(2352, collision_beep_freq*0.7);
            else bc.stopBeep();
          } else {
            if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) start_beep(2352, collision_beep_freq*0.7);
            else stop_beep();
          }
          collision_beep_count++;
        } else {
          if (is_beep_port_connected) bc.stopBeep();
          collision_beep_count = 0;
        }

        if ( ++m_loop_for_check >= m_collision_loop ) m_loop_for_check = 0;
        tp.posture.resize(m_qRef.data.length());
        for (size_t i=0; i<tp.posture.size(); i++) tp.posture[i] = m_q.data[i];
#ifdef USE_HRPSYSUTIL
        m_log.add(tp);
#endif // USE_HRPSYSUTIL

        // set collisoin state
        m_state.time = tm2;
        for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
            m_state.angle[i] = m_robot->joint(i)->q;
        }

        if ( m_loop_for_check == 0 ) {
            for (unsigned int i = 0; i < m_robot->numLinks(); i++ ){
                m_state.collide[i] = m_link_collision[i];
            }

            m_state.lines.length(tp.lines.size());
            for(unsigned int i = 0; i < tp.lines.size(); i++ ){
                const std::pair<hrp::Vector3, hrp::Vector3>& line = tp.lines[i];
                double *v;
                m_state.lines[i].length(2);
                m_state.lines[i].get_buffer()[0].length(3);
                v = m_state.lines[i].get_buffer()[0].get_buffer();
                v[0] = line.first.data()[0];
                v[1] = line.first.data()[1];
                v[2] = line.first.data()[2];
                m_state.lines[i].get_buffer()[1].length(3);
                v = m_state.lines[i].get_buffer()[1].get_buffer();
                v[0] = line.second.data()[0];
                v[1] = line.second.data()[1];
                v[2] = line.second.data()[2];
            }
        }
         m_state.computation_time = (tm2-tm1)*1000.0;
        m_state.safe_posture = m_safe_posture;
        m_state.recover_time = m_recover_time;
        m_state.loop_for_check = m_loop_for_check;
    }