From 4c024bc49ef20369f01d363c5884556932bf86da Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Mon, 22 Jan 2024 12:38:22 +0100 Subject: [PATCH] avoid a relative jump threshold of 0.0 (#2654) --- .../cartesian_path_service_capability.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 8e2c696672..87a4e00778 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -177,11 +177,15 @@ bool MoveGroupCartesianPathService::computeService( "and jump threshold %lf (in %s reference frame)", static_cast(waypoints.size()), link_name.c_str(), req->max_step, req->jump_threshold, global_frame ? "global" : "link"); + moveit::core::JumpThreshold jump_threshold = moveit::core::JumpThreshold::disabled(); + if (req->jump_threshold > 0.0) + { + jump_threshold = moveit::core::JumpThreshold::relative(req->jump_threshold); + } std::vector traj; res->fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, - moveit::core::MaxEEFStep(req->max_step), moveit::core::JumpThreshold::relative(req->jump_threshold), - constraint_fn); + moveit::core::MaxEEFStep(req->max_step), jump_threshold, constraint_fn); moveit::core::robotStateToRobotStateMsg(start_state, res->start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name);