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; }