diff --git a/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp index 57fd2626f5..6a523a873a 100644 --- a/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp @@ -304,15 +304,11 @@ int main(int argc, char** argv) target_pose3.position.x -= 0.2; waypoints.push_back(target_pose3); // up and left - // We want the Cartesian path to be interpolated at a resolution of 1 cm - // which is why we will specify 0.01 as the max step in Cartesian - // translation. We will specify the jump threshold as 0.0, effectively disabling it. - // Warning - disabling the jump threshold while operating real hardware can cause - // large unpredictable motions of redundant joints and could be a safety issue - moveit_msgs::msg::RobotTrajectory trajectory; - const double jump_threshold = 0.0; + // We want the Cartesian path to be interpolated at a resolution of 1 cm, + // which is why we will specify 0.01 as the max step in Cartesian translation. const double eef_step = 0.01; - double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); + moveit_msgs::msg::RobotTrajectory trajectory; + double fraction = move_group.computeCartesianPath(waypoints, eef_step, trajectory); RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); // Visualize the plan in RViz diff --git a/doc/how_to_guides/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp b/doc/how_to_guides/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp index 4b4385c26e..b23ef352d1 100644 --- a/doc/how_to_guides/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp +++ b/doc/how_to_guides/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp @@ -216,14 +216,15 @@ int main(int argc, char** argv) auto start_state = move_group.getCurrentState(10.0); std::vector traj; moveit::core::MaxEEFStep max_eef_step(0.01, 0.1); - // Here, we're effectively disabling the jump threshold for joints. This is not recommended on real hardware. - const auto jump_thresh = moveit::core::JumpThreshold::disabled(); + moveit::core::CartesianPrecision cartesian_precision{ .translational = 0.001, + .rotational = 0.01, + .max_resolution = 1e-3 }; // The trajectory, traj, passed to computeCartesianPath will contain several waypoints toward // the goal pose, target. For each of these waypoints, the IK solver is queried with the given cost function. const auto frac = moveit::core::CartesianInterpolator::computeCartesianPath( start_state.get(), joint_model_group, traj, joint_model_group->getLinkModel("panda_link8"), target, true, - max_eef_step, jump_thresh, callback_fn, opts, cost_fn); + max_eef_step, cartesian_precision, callback_fn, opts, cost_fn); RCLCPP_INFO(LOGGER, "Computed %f percent of cartesian path.", frac.value * 100.0);