Merge pull request #18 from InigoMoreno/self-collisions

Take self collisions into account in distance_penalty
ros-industrial · Jun 7, 2024 · 95aff84 · 95aff84
2 parents 421461c + 52ce335
commit 95aff84
Showing 1 changed file with 7 additions and 2 deletions.
diff --git a/src/evaluation/distance_penalty_moveit.cpp b/src/evaluation/distance_penalty_moveit.cpp
@@ -64,8 +64,13 @@ double DistancePenaltyMoveIt::calculateScore(const std::map<std::string, double>
   state.setJointGroupPositions(jmg_, pose_subset);
   state.update();
 
-  const double dist = scene_->distanceToCollision(state, scene_->getAllowedCollisionMatrix());
-  const double clipped_distance = std::min(std::abs(dist / dist_threshold_), 1.0);
+  collision_detection::CollisionRequest collision_request;
+  collision_request.distance = true;
+  collision_detection::CollisionResult collision_result;
+  scene_->checkCollision(collision_request, collision_result, state);
+  if (collision_result.collision)
+    return 0.0;
+  const double clipped_distance = std::min(collision_result.distance / dist_threshold_, 1.0);
   return std::pow(clipped_distance, exponent_);
 }