From 2e9a223827b60b316b3d2c763ce5201e7b09d377 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 Jul 2024 08:24:55 +0200 Subject: [PATCH] MoveRelative: handle equal min/max distance (#593) When min_distance == max_distance > 0.0, the minimal distance might be missed due to numerical errors. To avoid this, deactivate the 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;