From 160c48b924cac51a922952345549e59e23298ef2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 15 Jul 2024 15:05:19 +0200 Subject: [PATCH] MoveRelative: handle equal min/max distance When min_distance == max_distance > 0.0, the minimal distance might be missed due to numerical errors. To avoid this, deactivate minimal distance check and run the full distance as given by max_distance. --- core/src/stages/move_relative.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 3f1b915ed..634c8b56f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -185,6 +185,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning double max_distance = props.get("max_distance"); double min_distance = props.get("min_distance"); + + // if min_distance == max_distance, resort to moving full distance (negative min_distance) + if (max_distance > 0.0 && std::fabs(max_distance - min_distance) < std::numeric_limits::epsilon()) + min_distance = -1.0; + const auto& path_constraints = props.get("path_constraints"); robot_trajectory::RobotTrajectoryPtr robot_trajectory;