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.
This commit is contained in:
Robert Haschke 2024-07-16 08:24:55 +02:00 committed by GitHub
parent 8d2baf2739
commit 2e9a223827
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194

View File

@ -185,6 +185,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
double max_distance = props.get<double>("max_distance");
double min_distance = props.get<double>("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<double>::epsilon())
min_distance = -1.0;
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr robot_trajectory;