Skip to content

Commit

Permalink
Remove/replace deprecated use of JumpThreshold (#984)
Browse files Browse the repository at this point in the history
  • Loading branch information
sea-bass authored Nov 4, 2024
1 parent 1fe7db4 commit 59b03cc
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -216,14 +216,15 @@ int main(int argc, char** argv)
auto start_state = move_group.getCurrentState(10.0);
std::vector<moveit::core::RobotStatePtr> 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);

Expand Down

0 comments on commit 59b03cc

Please sign in to comment.