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