planning scene collision distance

Question

Hi,

I am using Groovy with Ubuntu 12.04.

I am trying to call the planning_planning_scene::PlanningScene::distanceToCollision functions, but the returned value is always a constant huge number. The call to selfCollisionCheck does work perfectly.

... // Collision check

robot_state::RobotState& current_state = planning_scene_ptr_->getCurrentStateNonConst(); current_state = *kinematic_state_; collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; collision_request.group_name = "arm_tcp";
collision_result.clear(); planning_scene_ptr_->checkSelfCollision(collision_request, collision_result, current_state); if (collision_result.collision) ROS_ERROR("Robot in SELF-COLLISION");

double distance_to_collision = planning_scene_ptr_->distanceToCollision( current_state);

...

a working example measuring the distance to the nearest collision would be much appreciated.

Thanks and best regards,

Roberto

---

Originally posted by Robotnik on ROS Answers with karma: 71 on 2013-10-16

Post score: 0

Answer 1

Solved. The problem was related with the collision matrix generated from the setup_assistant. Just manually disabling some additional joints on the SRDF file, the following code does work:

 // Collision check
  robot_state::RobotState& current_state = planning_scene_ptr_->getCurrentStateNonConst();
  current_state = *kinematic_state_;
  collision_detection::CollisionRequest collision_request;
  collision_detection::CollisionResult collision_result;
  collision_request.group_name = "arm_tcp";  
  collision_request.distance = true;
  collision_result.clear();
  planning_scene_ptr_->checkSelfCollision(collision_request, collision_result, current_state);
  if (collision_result.collision) ROS_ERROR("Robot in SELF-COLLISION");
  ROS_INFO("Collision result %5.10f", collision_result.distance);

---

Originally posted by Robotnik with karma: 71 on 2013-10-17

This answer was ACCEPTED on the original site

Post score: 0