diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 09439fdb..a1384c0a 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -175,9 +175,18 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { collision_detection::DistanceResult result; if (with_world) +#if MOVEIT_MASTER state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); +#else + state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(), + robot); +#endif else +#if MOVEIT_MASTER state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); +#else + state->scene()->getCollisionRobot()->distanceSelf(request, result, robot); +#endif if (result.minimum_distance.distance <= 0) { return result.minimum_distance;