Revision history [back]

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