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