CollisionDetector.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <iomanip>
11 #include <rtm/CorbaNaming.h>
12 #include <hrpModel/Link.h>
13 #include <hrpModel/JointPath.h>
14 #include <hrpUtil/Eigen3d.h>
15 #include <hrpUtil/Eigen4d.h>
17 #ifdef USE_HRPSYSUTIL
18 #include "hrpsys/util/GLbody.h"
19 #include "hrpsys/util/GLutil.h"
20 #endif // USE_HRPSYSUTIL
21 #include "hrpsys/util/BVutil.h"
22 #include "hrpsys/idl/RobotHardwareService.hh"
23 
24 #include "CollisionDetector.h"
25 
26 #define deg2rad(x) ((x)*M_PI/180)
27 #define rad2deg(x) ((x)*180/M_PI)
28 
29 // Module specification
30 // <rtc-template block="module_spec">
31 static const char* component_spec[] =
32 {
33  "implementation_id", "CollisionDetector",
34  "type_name", "CollisionDetector",
35  "description", "collisoin detector component",
36  "version", HRPSYS_PACKAGE_VERSION,
37  "vendor", "AIST",
38  "category", "example",
39  "activity_type", "DataFlowComponent",
40  "max_instance", "10",
41  "language", "C++",
42  "lang_type", "compile",
43  // Configuration variables
44  "conf.default.debugLevel", "0",
45  ""
46 };
47 // </rtc-template>
48 
49 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
50 {
51  int pre = os.precision();
52  os.setf(std::ios::fixed);
53  os << std::setprecision(6)
54  << (tm.sec + tm.nsec/1e9)
55  << std::setprecision(pre);
56  os.unsetf(std::ios::fixed);
57  return os;
58 }
59 
61  : RTC::DataFlowComponentBase(manager),
62  // <rtc-template block="initializer">
63  m_qRefIn("qRef", m_qRef),
64  m_qCurrentIn("qCurrent", m_qCurrent),
65  m_servoStateIn("servoStateIn", m_servoState),
66  m_qOut("q", m_q),
67  m_beepCommandOut("beepCommand", m_beepCommand),
68  m_CollisionDetectorServicePort("CollisionDetectorService"),
69  // </rtc-template>
70 #ifdef USE_HRPSYSUTIL
71  m_scene(&m_log),
72  m_window(&m_scene, &m_log),
73  m_glbody(NULL),
74 #endif // USE_HRPSYSUTIL
75  m_use_limb_collision(false),
76  m_use_viewer(false),
77  m_robot(hrp::BodyPtr()),
78  m_loop_for_check(0),
79  m_collision_loop(1),
80  m_debugLevel(0),
81  m_enable(true),
82  collision_beep_count(0),
83  is_beep_port_connected(false),
84  dummy(0)
85 {
86  m_service0.collision(this);
87 #ifdef USE_HRPSYSUTIL
88  m_log.enableRingBuffer(1);
89 #endif // USE_HRPSYSUTIL
90  init_beep();
91  start_beep(3136);
92 }
93 
95 {
96  quit_beep();
97 }
98 
99 
100 
102 {
103  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
104  // <rtc-template block="bind_config">
105  // Bind variables and configuration variable
106  bindParameter("debugLevel", m_debugLevel, "0");
107 
108  // </rtc-template>
109 
110  // Registration: InPort/OutPort/Service
111  // <rtc-template block="registration">
112  // Set InPort buffers
113  addInPort("qRef", m_qRefIn);
114  addInPort("qCurrent", m_qCurrentIn);
115  addInPort("servoStateIn", m_servoStateIn);
116 
117  // Set OutPort buffer
118  addOutPort("q", m_qOut);
119  addOutPort("beepCommand", m_beepCommandOut);
120 
121  // Set service provider to Ports
122  m_CollisionDetectorServicePort.registerProvider("service0", "CollisionDetectorService", m_service0);
123 
124  // Set service consumers to Ports
125 
126  // Set CORBA Service Ports
128 
129  // </rtc-template>
130 
131  //RTC::Properties& prop = getProperties();
132 
133  RTC::Manager& rtcManager = RTC::Manager::instance();
134  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
135  int comPos = nameServer.find(",");
136  if (comPos < 0){
137  comPos = nameServer.length();
138  }
139  nameServer = nameServer.substr(0, comPos);
140  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
141 
143 
144  coil::stringTo(m_dt, prop["dt"].c_str());
145 
146  if ( prop["collision_viewer"] == "true" ) {
147  m_use_viewer = true;
148  }
149 #ifdef USE_HRPSYSUTIL
150  m_glbody = new GLbody();
151  m_robot = hrp::BodyPtr(m_glbody);
152 #else
153  m_robot = hrp::BodyPtr(new hrp::Body());
154 #endif // USE_HRPSYSUTIL
155  //
156  OpenHRP::BodyInfo_var binfo;
157  binfo = hrp::loadBodyInfo(prop["model"].c_str(),
158  CosNaming::NamingContext::_duplicate(naming.getRootContext()));
159  if (CORBA::is_nil(binfo)){
160  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
161  << std::endl;
162  return RTC::RTC_ERROR;
163  }
164 #ifdef USE_HRPSYSUTIL
165  if (!loadBodyFromBodyInfo(m_robot, binfo, true, GLlinkFactory)) {
166 #else
167  if (!loadBodyFromBodyInfo(m_robot, binfo, true, hrplinkFactory)) {
168 #endif // USE_HRPSYSUTIL
169  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
170  << m_profile.instance_name << std::endl;
171  return RTC::RTC_ERROR;
172  }
173 #ifdef USE_HRPSYSUTIL
174  loadShapeFromBodyInfo(m_glbody, binfo);
175 #endif // USE_HRPSYSUTIL
176  if ( prop["collision_model"] == "AABB" ) {
178  } else if ( prop["collision_model"] == "convex hull" ||
179  prop["collision_model"] == "" ) { // set convex hull as default
181  }
183 
184  if ( prop["collision_pair"] != "" ) {
185  std::cerr << "[" << m_profile.instance_name << "] prop[collision_pair] ->" << prop["collision_pair"] << std::endl;
186  std::istringstream iss(prop["collision_pair"]);
187  std::string tmp;
188  while (getline(iss, tmp, ' ')) {
189  size_t pos = tmp.find_first_of(':');
190  std::string name1 = tmp.substr(0, pos), name2 = tmp.substr(pos+1);
191  if ( m_robot->link(name1)==NULL ) {
192  std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name1 << std::endl;
193  std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
194  for (unsigned int i=0; i < m_robot->numLinks(); i++) {
195  std::cerr << " " << m_robot->link(i)->name;
196  }
197  std::cerr << std::endl;
198  continue;
199  }
200  if ( m_robot->link(name2)==NULL ) {
201  std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name2 << std::endl;
202  std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
203  for (unsigned int i=0; i < m_robot->numLinks(); i++) {
204  std::cerr << " " << m_robot->link(i)->name;
205  }
206  std::cerr << std::endl;
207  continue;
208  }
209  std::cerr << "[" << m_profile.instance_name << "] check collisions between " << m_robot->link(name1)->name << " and " << m_robot->link(name2)->name << std::endl;
210  m_pair[tmp] = new CollisionLinkPair(new VclipLinkPair(m_robot->link(name1), m_VclipLinks[m_robot->link(name1)->index],
211  m_robot->link(name2), m_VclipLinks[m_robot->link(name2)->index], 0));
212  }
213  }
214 
215  if ( prop["collision_loop"] != "" ) {
216  coil::stringTo(m_collision_loop, prop["collision_loop"].c_str());
217  std::cerr << "[" << m_profile.instance_name << "] set collision_loop: " << m_collision_loop << std::endl;
218  }
219 #ifdef USE_HRPSYSUTIL
220  if ( m_use_viewer ) {
221  m_scene.addBody(m_robot);
223  }
224 #endif // USE_HRPSYSUTIL
225 
226  // true (1) do not move when collide,
227  // false(0) move even if collide
228  m_curr_collision_mask.resize(m_robot->numJoints()); // collision_mask used to select output 0: passthough reference data, 1 output safe data
229  std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0);
230  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
231  std::fill(m_init_collision_mask.begin(), m_init_collision_mask.end(), 1);
232  if ( prop["collision_mask"] != "" ) {
233  std::cerr << "[" << m_profile.instance_name << "] prop[collision_mask] ->" << prop["collision_mask"] << std::endl;
234  coil::vstring mask_str = coil::split(prop["collision_mask"], ",");
235  if (mask_str.size() == m_robot->numJoints()) {
236  for (size_t i = 0; i < m_robot->numJoints(); i++) {coil::stringTo(m_init_collision_mask[i], mask_str[i].c_str()); }
237  for (size_t i = 0; i < m_robot->numJoints(); i++) {
238  if ( m_init_collision_mask[i] == 0 ) {
239  std::cerr << "[" << m_profile.instance_name << "] CollisionDetector will not control " << m_robot->joint(i)->name << std::endl;
240  }
241  }
242  }else{
243  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;
244  }
245  }
246 
247  if ( prop["use_limb_collision"] != "" ) {
248  std::cerr << "[" << m_profile.instance_name << "] prop[use_limb_collision] -> " << prop["use_limb_collision"] << std::endl;
249  if ( prop["use_limb_collision"] == "true" ) {
250  m_use_limb_collision = true;
251  std::cerr << "[" << m_profile.instance_name << "] Enable use_limb_collision" << std::endl;
252  }
253  }
254 
255  // setup collision state
256  m_state.angle.length(m_robot->numJoints());
257  m_state.collide.length(m_robot->numLinks());
258 
259  // allocate memory for outPorts
260  m_q.data.length(m_robot->numJoints());
261  m_recover_time = 0;
262  m_safe_posture = true;
263  m_have_safe_posture = false;
264  i_dt = 1.0;
266  m_recover_jointdata = new double[m_robot->numJoints()];
267  m_lastsafe_jointdata = new double[m_robot->numJoints()];
268  m_interpolator = new interpolator(m_robot->numJoints(), i_dt);
269  m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
270  m_link_collision = new bool[m_robot->numLinks()];
271 
272  for(unsigned int i=0; i<m_robot->numJoints(); i++){
273  m_q.data[i] = 0;
274  }
275 
276  m_servoState.data.length(m_robot->numJoints());
277  for(unsigned int i = 0; i < m_robot->numJoints(); i++) {
278  m_servoState.data[i].length(1);
279  int status = 0;
280  status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
281  status |= 1<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
282  status |= 1<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
283  status |= 0<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
284  status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
285  m_servoState.data[i][0] = status;
286  }
287 
288  collision_beep_freq = static_cast<int>(1.0/(3.0*m_dt)); // 3 times / 1[s]
289  m_beepCommand.data.length(bc.get_num_beep_info());
290  return RTC::RTC_OK;
291 }
292 
293 
294 
295 RTC::ReturnCode_t CollisionDetector::onFinalize()
296 {
297  delete[] m_recover_jointdata;
298  delete[] m_lastsafe_jointdata;
299  delete m_interpolator;
300  delete[] m_link_collision;
301  return RTC::RTC_OK;
302 }
303 
304 /*
305  RTC::ReturnCode_t CollisionDetector::onStartup(RTC::UniqueId ec_id)
306  {
307  return RTC::RTC_OK;
308  }
309 */
310 
311 /*
312  RTC::ReturnCode_t CollisionDetector::onShutdown(RTC::UniqueId ec_id)
313  {
314  return RTC::RTC_OK;
315  }
316 */
317 
319 {
320  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
321  m_have_safe_posture = false;
322  return RTC::RTC_OK;
323 }
324 
326 {
327  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
328  return RTC::RTC_OK;
329 }
330 
331 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
333 {
334  static int loop = 0;
335  loop++;
336 
337  // Connection check of m_beepCommand to BeeperRTC
338  // If m_beepCommand is not connected to BeeperRTC and sometimes, check connection.
339  // If once connection is found, never check connection.
340  if (!is_beep_port_connected && (loop % 500 == 0) ) {
341  if ( m_beepCommandOut.connectors().size() > 0 ) {
342  is_beep_port_connected = true;
343  quit_beep();
344  std::cerr << "[" << m_profile.instance_name<< "] beepCommand data port connection found! Use BeeperRTC." << std::endl;
345  }
346  }
347 
348  if (m_servoStateIn.isNew()) {
350  }
351  if ( ! m_enable ) {
352  if ( DEBUGP || loop % 100 == 1) {
353  std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving without checking self collision detection!!! please send enableCollisionDetection to CollisoinDetection RTC" << std::endl;
354  }
355  if ( m_qRefIn.isNew()) {
356  m_qRefIn.read();
357  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
358  m_q.data[i] = m_qRef.data[i];
359  }
360  m_q.tm = m_qRef.tm;
361  m_qOut.write();
362  }
363  }
364  if (m_enable && m_qRefIn.isNew()) {
365  m_qRefIn.read();
366 
367  // check servo for collision beep sound
368  bool has_servoOn = false;
369  for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
370  int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
371  has_servoOn = has_servoOn || (servo_state == 1);
372  }
373 
374  TimedPosture tp;
375 
376  assert(m_qRef.data.length() == m_robot->numJoints());
377 #ifdef USE_HRPSYSUTIL
378  if ( m_use_viewer ) {
379  for (unsigned int i=0; i<m_glbody->numLinks(); i++){
380  ((GLlink *)m_glbody->link(i))->highlight(false);
381  }
382  }
383  for (unsigned int i=0; i<m_glbody->numLinks(); i++){
384  m_link_collision[m_glbody->link(i)->index] = false;
385  }
386 #else
387  for (unsigned int i=0; i<m_robot->numLinks(); i++){
388  m_link_collision[m_robot->link(i)->index] = false;
389  }
390 #endif // USE_HRPSYSUTIL
391 
392  //set robot model's angle for collision check(two types)
393  // 1. current safe angle or collision angle .. check based on qRef
394  // 2. recovery .. check based on q'(m_recover_jointdata)
395  if (m_recover_time == 0 || m_recover_time == default_recover_time ) { // 1. current safe angle or collision angle
396  if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
397  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
398  m_robot->joint(i)->q = m_qRef.data[i];
399  }
400  }
401  }else{ // 2. recovery
402  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
403  if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
404  if ( m_curr_collision_mask[i] == 1) {// joint with 1 (do not move when collide :default), need to be updated using recover(safe) data
405  m_robot->joint(i)->q = m_recover_jointdata[i];
406  }else{ // joint with 0 (move even if collide), need to be updated using reference(dangerous) data
407  m_robot->joint(i)->q = m_qRef.data[i];
408  }
409  }
410  }
411  }
412  // }
413  //collision check process in case of angle set above
414  m_robot->calcForwardKinematics();
416  std::map<std::string, CollisionLinkPair *>::iterator it = m_pair.begin();
417  for (int i = 0; it != m_pair.end(); it++, i++){
418  int sub_size = (m_pair.size() + m_collision_loop -1) / m_collision_loop; // 10 / 3 = 3 / floor
419  // 0 : 0 .. sub_size-1 // 0 .. 2
420  // 1 : sub_size ... sub_size*2-1 // 3 .. 5
421  // k : sub_size*k ... sub_size*(k+1)-1 // 6 .. 8
422  // n : sub_size*n ... m_pair.size() // 9 .. 10
423  if ( sub_size*m_loop_for_check <= i && i < sub_size*(m_loop_for_check+1) ) {
424  CollisionLinkPair* c = it->second;
425  c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data());
426  //std::cerr << i << ":" << (c->distance<=c->pair->getTolerance() ) << "/" << c->distance << " ";
427  }
428  }
429  if ( m_loop_for_check == m_collision_loop-1 ) {
430  bool last_safe_posture = m_safe_posture;
431  m_safe_posture = true;
432  it = m_pair.begin();
433  for (unsigned int i = 0; it != m_pair.end(); i++, it++){
434  CollisionLinkPair* c = it->second;
435  VclipLinkPairPtr p = c->pair;
436  tp.lines.push_back(std::make_pair(c->point0, c->point1));
437  if ( c->distance <= c->pair->getTolerance() ) {
438  m_safe_posture = false;
439  if ( loop%200==0 || last_safe_posture ) {
440  hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
441  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;
442  }
443  m_link_collision[p->link(0)->index] = true;
444  m_link_collision[p->link(1)->index] = true;
445  if ( m_use_limb_collision ) {
446  hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
447  bool stop_all = true;
448  // if all joint is within false(0:move even if collide) in initial mask ( for example leg to leg ) we stop them
449  // 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)
450  for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){ if ( m_init_collision_mask[jointPath->joint(i)->jointId] == 1) stop_all = false; }
451  for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){
452  int id = jointPath->joint(i)->jointId;
453  // collision_mask used to select output 0: passthough reference data, 1 output safe data
454  if ( stop_all ) {
455  m_curr_collision_mask[id] = 1; // true (1: output safe data) do not move when collide,
456  } else if (m_init_collision_mask[id] == 1) { // skip false (0: move even if collide)
458  }
459  }
460  } else {
461  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
462  }
463 #ifdef USE_HRPSYSUTIL
464  if ( m_use_viewer ) {
465  ((GLlink *)p->link(0))->highlight(true);
466  ((GLlink *)p->link(1))->highlight(true);
467  }
468 #endif // USE_HRPSYSUTIL
469  }
470  }
471  if ( m_safe_posture ) {
472  if (has_servoOn) {
473  if (! m_have_safe_posture ) {
474  // first transition collision -> safe
475  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
476  << "] set safe posture" << std::endl;
477  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
478  m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
479  }
480  m_interpolator->set(m_lastsafe_jointdata); // Set current angles as initial angle for recover
481  }
482  m_have_safe_posture = true;
483  }
485  // sefe or recover
486  // in collision, robot->q may differ from m_q.
487  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
488  m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
489  }
490  }
491  }else{
492  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
493  if ( m_curr_collision_mask[i] == 0 ) { // if collisoin_mask is 0 (move even if collide), we update lastsafe_joint_data from input data
494  m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
495  }
496  }
497  }
498  if ( m_use_limb_collision ) {
499  if ( loop%200==0 and ! m_safe_posture ) {
500  std::cerr << "[" << m_profile.instance_name << "] collision_mask : ";
501  for (size_t i = 0; i < m_robot->numJoints(); i++) {
502  std::cerr << m_robot->joint(i)->name << ":" << m_curr_collision_mask[i] << " ";
503  }
504  std::cerr << std::endl;
505  }
506  }
507  }
508  // mode : m_safe_posture : recover_time : set as q
509  // safe : true : 0 : qRef
510  // collison : false : > 0 : q( do nothing)
511  // recover : true : > 0 : q'
512  //std::cerr << "m_recover_time: " << m_recover_time << std::endl;
514  if (m_safe_posture && m_recover_time == 0){ // safe mode
515  //std::cerr << "safe-------------- " << std::endl;
516  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
517  m_q.data[i] = m_qRef.data[i];
518  }
519  // collision_mask used to select output 0: passthough reference data, 1 output safe data
520  std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0); // false(0) clear output data
521  } else {
522  static int collision_loop_recover = 0;
523  if(m_safe_posture){ //recover
524  //std::cerr << "recover-------------- " << std::endl;
525  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
526  m_q.data[i] = m_recover_jointdata[i];
527  }
529  } else if (m_collision_loop > 1 && (m_recover_time != default_recover_time)) { // collision with collision_loop
530  //std::cerr << "collision_loop-------------- " << std::endl;
531  collision_loop_recover = m_collision_loop;
532  m_recover_time = default_recover_time; // m_recover_time should be set based on difference between qRef and q
533  } else { //collision
534  //std::cerr << "collision-------------- " << std::endl;
535  //do nothing (stay previous m_q)
536  m_recover_time = default_recover_time; // m_recover_time should be set based on difference between qRef and q
537  m_interpolator->set(m_lastsafe_jointdata); //Set last safe joint data as initial angle
538  //m_interpolator->set(m_q.data.get_buffer()); //Set initial angle
539  }
540  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
541  if (m_curr_collision_mask[i] == 0) { // 0: passthough reference data, 1 output safe data, stop joints only joint with 1
542  m_q.data[i] = m_qRef.data[i];
543  }
544  }
545  //calc q'
546 #if 0
547  //linear interpolation (dangerous)
548  for ( int i = 0; i < m_q.data.length(); i++ ) {
549  m_recover_jointdata[i] = m_q.data[i] + (m_qRef.data[i] - m_q.data[i]) / m_recover_time;
550  }
551 #else
552  if (collision_loop_recover != 0) {
553  collision_loop_recover--;
554  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
555  m_q.data[i] =
556  m_lastsafe_jointdata[i] + collision_loop_recover * ((m_q.data[i] - m_lastsafe_jointdata[i])/m_collision_loop);
557  }
558  } else {
559  collision_loop_recover = 0;
560  //minjerk interpolation
561  m_interpolator->setGoal(m_qRef.data.get_buffer(), m_recover_time);
563  }
564 #endif
565  }
566  if ( DEBUGP ) {
567  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
568  << " [msec], safe = " << m_safe_posture << ", time = " << m_recover_time*m_dt << "[s], loop = " << m_loop_for_check << "/" << m_collision_loop << std::endl;
569  }
570  if ( m_pair.size() == 0 && ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) ) {
571  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;
572  }
573  if ( ! m_have_safe_posture && ! m_safe_posture ) {
574  if ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) {
575  std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving while collision detection!!!, since we do not get safe_posture yet" << std::endl;
576  }
577  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
578  m_lastsafe_jointdata[i] = m_recover_jointdata[i] = m_q.data[i] = m_qRef.data[i];
579  }
580  }
581  //
582  m_q.tm = m_qRef.tm;
583  m_qOut.write();
584 
585  // if servo off, we do not know last safe posture
586  if (! has_servoOn ) {
587  m_have_safe_posture = false;
588  }
589  // beep sound for collision alert
590  if ( !m_safe_posture && has_servoOn ) { // If collided and some joint is servoOn
593  else bc.stopBeep();
594  } else {
596  else stop_beep();
597  }
599  } else {
602  }
603 
605  tp.posture.resize(m_qRef.data.length());
606  for (size_t i=0; i<tp.posture.size(); i++) tp.posture[i] = m_q.data[i];
607 #ifdef USE_HRPSYSUTIL
608  m_log.add(tp);
609 #endif // USE_HRPSYSUTIL
610 
611  // set collisoin state
612  m_state.time = tm2;
613  for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){
614  m_state.angle[i] = m_robot->joint(i)->q;
615  }
616 
617  if ( m_loop_for_check == 0 ) {
618  for (unsigned int i = 0; i < m_robot->numLinks(); i++ ){
619  m_state.collide[i] = m_link_collision[i];
620  }
621 
622  m_state.lines.length(tp.lines.size());
623  for(unsigned int i = 0; i < tp.lines.size(); i++ ){
624  const std::pair<hrp::Vector3, hrp::Vector3>& line = tp.lines[i];
625  double *v;
626  m_state.lines[i].length(2);
627  m_state.lines[i].get_buffer()[0].length(3);
628  v = m_state.lines[i].get_buffer()[0].get_buffer();
629  v[0] = line.first.data()[0];
630  v[1] = line.first.data()[1];
631  v[2] = line.first.data()[2];
632  m_state.lines[i].get_buffer()[1].length(3);
633  v = m_state.lines[i].get_buffer()[1].get_buffer();
634  v[0] = line.second.data()[0];
635  v[1] = line.second.data()[1];
636  v[2] = line.second.data()[2];
637  }
638  }
639  m_state.computation_time = (tm2-tm1)*1000.0;
640  m_state.safe_posture = m_safe_posture;
641  m_state.recover_time = m_recover_time;
642  m_state.loop_for_check = m_loop_for_check;
643  }
647  }
648 #ifdef USE_HRPSYSUTIL
649  if ( m_use_viewer ) m_window.oneStep();
650 #endif // USE_HRPSYSUTIL
651  return RTC::RTC_OK;
652 }
653 
654 /*
655  RTC::ReturnCode_t CollisionDetector::onAborting(RTC::UniqueId ec_id)
656  {
657  return RTC::RTC_OK;
658  }
659 */
660 
661 /*
662  RTC::ReturnCode_t CollisionDetector::onError(RTC::UniqueId ec_id)
663  {
664  return RTC::RTC_OK;
665  }
666 */
667 
668 /*
669  RTC::ReturnCode_t CollisionDetector::onReset(RTC::UniqueId ec_id)
670  {
671  return RTC::RTC_OK;
672  }
673 */
674 
675 /*
676  RTC::ReturnCode_t CollisionDetector::onStateUpdate(RTC::UniqueId ec_id)
677  {
678  return RTC::RTC_OK;
679  }
680 */
681 
682 /*
683  RTC::ReturnCode_t CollisionDetector::onRateChanged(RTC::UniqueId ec_id)
684  {
685  return RTC::RTC_OK;
686  }
687 */
688 
689 bool CollisionDetector::setTolerance(const char *i_link_pair_name, double i_tolerance) {
690  if (strcmp(i_link_pair_name, "all") == 0 || strcmp(i_link_pair_name, "ALL") == 0){
691  for ( std::map<std::string, CollisionLinkPair *>::iterator it = m_pair.begin(); it != m_pair.end(); it++){
692  it->second->pair->setTolerance(i_tolerance);
693  }
694  }else if ( m_pair.find(std::string(i_link_pair_name)) != m_pair.end() ) {
695  m_pair[std::string(i_link_pair_name)]->pair->setTolerance(i_tolerance);
696  }else{
697  return false;
698  }
699  return true;
700 }
701 
703  if (input_loop > 0) {
704  m_collision_loop = input_loop;
705  return true;
706  }
707  return false;
708 }
709 
710 bool CollisionDetector::getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState &state)
711 {
712  state = m_state;
713  return true;
714 }
715 
717 {
718  m_VclipLinks.resize(i_body->numLinks());
719  //std::cerr << i_body->numLinks() << std::endl;
720  for (unsigned int i=0; i<i_body->numLinks(); i++) {
721  assert(i_body->link(i)->index == i);
722  setupVClipModel(i_body->link(i));
723  }
724 }
725 
727 {
728  for ( unsigned int i = 0; i < m_q.data.length(); i++ ) {
729  // If servoOn, check too large joint angle gap. Otherwise (servoOff), neglect too large joint angle gap.
730  int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; // enum SwitchStatus {SWITCH_ON, SWITCH_OFF};
731  if (servo_state == 1 && abs(m_q.data[i] - m_qRef.data[i]) > 0.017) return false;
732  }
733  return true;
734 }
735 
737 {
738  if (m_enable){
739  std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is already enabled." << std::endl;
740  return true;
741  }
742 
743  if (!checkIsSafeTransition()){
744  std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be enabled because of different reference joint angle" << std::endl;
745  return false;
746  }
747 
748  // check collision
749  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
750  m_robot->joint(i)->q = m_qRef.data[i];
751  }
752  m_robot->calcForwardKinematics();
753  std::map<std::string, CollisionLinkPair *>::iterator it = m_pair.begin();
754  for (unsigned int i = 0; it != m_pair.end(); it++, i++){
755  CollisionLinkPair* c = it->second;
756  VclipLinkPairPtr p = c->pair;
757  c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data());
758  if ( c->distance <= c->pair->getTolerance() ) {
759  hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
760  std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be enabled because of collision" << std::endl;
761  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;
762  return false;
763  }
764  }
765  std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is successfully enabled." << std::endl;
766 
767  m_safe_posture = true;
768  m_recover_time = 0;
769  m_loop_for_check = 0;
770  m_enable = true;
771  return true;
772 }
773 
775 {
776  if (!checkIsSafeTransition()){
777  std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be disabled because of different reference joint angle" << std::endl;
778  return false;
779  }
780  std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is successfully disabled." << std::endl;
781  m_enable = false;
782  return true;
783 }
784 
786 {
787  Vclip::Polyhedron* i_vclip_model = new Vclip::Polyhedron();
788  int n = i_link->coldetModel->getNumVertices();
789  float v[3];
790  Vclip::VertFaceName vertName;
791  for (int i = 0; i < n; i ++ ) {
792  i_link->coldetModel->getVertex(i, v[0], v[1], v[2]);
793  sprintf(vertName, "v%d", i);
794  i_vclip_model->addVertex(vertName, Vclip::Vect3(v[0], v[1], v[2]));
795  }
796  i_vclip_model->buildHull();
797  i_vclip_model->check();
798  fprintf(stderr, "[Vclip] build finished, vcliip mesh of %s, %d -> %d\n",
799  i_link->name.c_str(), n, (int)(i_vclip_model->verts().size()));
800  m_VclipLinks[i_link->index] = i_vclip_model;
801 }
802 
803 #ifndef USE_HRPSYSUTIL
805 {
806  return new hrp::Link();
807 }
808 #endif // USE_HRPSYSUTIL
809 
810 extern "C"
811 {
812 
814  {
816  manager->registerFactory(profile,
817  RTC::Create<CollisionDetector>,
818  RTC::Delete<CollisionDetector>);
819  }
820 
821 };
822 
823 
ComponentProfile m_profile
void convertToAABB(hrp::BodyPtr i_body)
Definition: BVutil.cpp:14
png_infop png_charpp int png_charpp profile
RTC::CorbaPort m_CollisionDetectorServicePort
boost::intrusive_ptr< VclipLinkPair > VclipLinkPairPtr
Definition: VclipLinkPair.h:21
InPort< TimedDoubleSeq > m_qCurrentIn
bool isWritable() const
Definition: beep.h:36
void collision(CollisionDetector *i_collision)
bool getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState &state)
HRPMODEL_API OpenHRP::BodyInfo_var loadBodyInfo(const char *url, int &argc, char *argv[])
bool stringTo(To &val, const char *str)
Vertex * addVertex(const char *name, const Vect3 &coords)
Definition: vclip.h:612
unsigned int m_debugLevel
void setupVClipModel(hrp::BodyPtr i_body)
std::vector< int > m_init_collision_mask
bool setCollisionLoop(int input_loop)
bool setTolerance(const char *i_link_pair_name, double i_tolerance)
void setName(const std::string &_name)
Definition: interpolator.h:63
void quit_beep()
Definition: beep.cpp:28
InPort< TimedDoubleSeq > m_qRefIn
void init_beep()
Definition: beep.cpp:6
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
CORBA::ORB_ptr getORB()
void loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo, GLshape *(*shapeFactory)())
Definition: GLutil.cpp:181
png_uint_32 i
coil::Properties & getProperties()
OutPort< TimedLongSeq > m_beepCommandOut
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
static Manager & instance()
interpolator * m_interpolator
bool addOutPort(const char *name, OutPortBase &outport)
void setDataPort(RTC::TimedLongSeq &out_data)
Definition: beep.h:43
int get_num_beep_info() const
Definition: beep.h:21
boost::shared_ptr< Body > BodyPtr
Definition: GLbody.h:11
TimedDoubleSeq m_q
std::vector< std::string > vstring
int gettimeofday(struct timeval *tv, struct timezone *tz)
coil::Properties & getConfig()
OutPort< TimedDoubleSeq > m_qOut
TimedDoubleSeq m_qRef
void startBeep(const int _freq, const int _length=50)
Definition: beep.h:22
ExecutionContextHandle_t UniqueId
std::map< std::string, CollisionLinkPair * > m_pair
std::vector< Vclip::Polyhedron * > m_VclipLinks
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
void convertToConvexHull(hrp::BodyPtr i_body)
Definition: BVutil.cpp:79
int check() const
void CollisionDetectorInit(RTC::Manager *manager)
const list< Vertex > & verts() const
Definition: vclip.h:416
CollisionDetector(RTC::Manager *manager)
Constructor.
char VertFaceName[VF_NAME_SZ]
Definition: vclip.h:61
OpenHRP::CollisionDetectorService::CollisionState m_state
virtual ~CollisionDetector()
Destructor.
n
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
static const char * component_spec[]
#define DEBUGP
virtual RTC::ReturnCode_t onInitialize()
bool checkIsSafeTransition(void)
prop
virtual RTC::ReturnCode_t onFinalize()
OpenHRP::TimedLongSeqSeq m_servoState
naming
void stopBeep()
Definition: beep.h:29
TimedLongSeq m_beepCommand
hrp::Link * hrplinkFactory()
typedef int
std::string sprintf(char const *__restrict fmt,...)
long int sec() const
virtual bool isNew()
void get(double *x_, bool popp=true)
static int id
bool addPort(PortBase &port)
long int usec() const
virtual bool write(DataType &value)
hrp::BodyPtr m_robot
void stop_beep()
Definition: beep.cpp:22
std::vector< std::pair< hrp::Vector3, hrp::Vector3 > > lines
Definition: TimedPosture.h:8
int loadBodyFromBodyInfo(::World *world, const char *_name, BodyInfo_ptr bodyInfo)
static std::ostream & operator<<(std::ostream &os, const struct RTC::Time &tm)
std::vector< int > m_curr_collision_mask
const std::vector< OutPortConnector *> & connectors()
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateIn
bool addInPort(const char *name, InPortBase &inport)
void setGoal(const double *gx, const double *gv, double time, bool online=true)
CollisionDetectorService_impl m_service0
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void start_beep(int freq, int length)
Definition: beep.cpp:16
void set(const double *x, const double *v=NULL)
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
std::vector< double > posture
Definition: TimedPosture.h:7


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20