Skip to content

Cannot change jump threshold for computeCartesianPath() #3658

@SphericalCowww

Description

@SphericalCowww

Description

There is no longer a viable way to change the jump threshold parameter for computeCartesianPath. I have tried multiple approaches to change the value to 0.15, but the output is as follows:

[move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability]: Attempting to follow 2 waypoints for link 'calfSphere' using a step of 0.001000 m and jump threshold 0.000000 (in global reference frame)

As a result, hardly any of the Cartesian solutions converge.

ROS Distro

Jazzy

OS and version

Ubuntu 24.04.3 LTS

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

ros-jazzy-moveit

Which RMW are you using?

None

Steps to Reproduce

  • Attempt 1: directly apply through computeCartesianPath() under /my_robot_commander:
my_robot_commander_class(std::shared_ptr<rclcpp::Node> node) {
    node_ = node;
    std::shared_ptr<MoveGroupInterface> leg1_interface_ = std::make_shared<MoveGroupInterface>(node_, "leg1");
    std::vector<geometry_msgs::msg::Pose> waypoints;
    waypoints.push_back(...)
    moveit_msgs::msg::RobotTrajectory trajectory;
    double fraction = leg1_interface_->computeCartesianPath(waypoints, 0.001, 0.15, trajectory);
}

During the compilation, it issues:

/home/kali/Documents/ROS_leggedRobot_testBed/src/my_robot_commander/src/my_robot_commander.cpp:95:72: warning: ‘double moveit::planning_interface::MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::msg::Pose_<std::allocator<void> > >&, double, double, moveit_msgs::msg::RobotTrajectory&, bool, moveit_msgs::msg::MoveItErrorCodes*)’ is deprecated: Drop jump_threshold [-Wdeprecated-declarations]

Deprecated as expected. But not only is it deprecated, but it also does not change the value at all. It should have been removed instead then. The result is:

[move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability]: Attempting to follow 2 waypoints for link 'calfSphere' using a step of 0.001000 m and jump threshold 0.000000 (in global reference frame)
  • Attempt 2: Editing from src/my_robot_moveit_config/config/kinematics.yaml
leg1:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3
  position_only_ik: True
  cartesian_path:
    jump_threshold: 0.15
    revolute_jump_threshold: 1.5
    prismatic_jump_threshold: 0.0

The result is:

[move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability]: Attempting to follow 2 waypoints for link 'calfSphere' using a step of 0.001000 m and jump threshold 0.000000 (in global reference frame)
  • Attempt 3: from launch file:
    ...
    moveit_config = (
        MoveItConfigsBuilder("my_robot", package_name="my_robot_moveit_config")
        .robot_description(file_path=urdf_path)
        .to_moveit_configs()
    )
    moveit_launcher = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(moveit_config_path),
        launch_arguments={}.items(),
    )
    commander_node = Node(
        package="my_robot_commander",
        executable="my_robot_commander",
        parameters=[
            moveit_config.robot_description,           # The URDF math
            moveit_config.robot_description_semantic,  # The SRDF (defines 'leg1')
            moveit_config.robot_description_kinematics,# The kinematics.yaml (THE MISSING PIECE)
            {"jump_threshold": 0.15},                  # for computeCartesianPath
        ],
    )
    return LaunchDescription([
        SetParameter(name='jump_threshold', value=0.15),
        ...
        moveit_launcher,
        commander_node,
        ...
    ])

In fact, this gives:

$ ros2 param get /move_group jump_threshold
Double value is: 0.15

However, the service still gives:

[move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability]: Attempting to follow 2 waypoints for link 'calfSphere' using a step of 0.001000 m and jump threshold 0.000000 (in global reference frame)

Expected behavior

Should at least return:

[move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability]: Attempting to follow 2 waypoints for link 'calfSphere' using a step of 0.001000 m and jump threshold 0.150000 (in global reference frame)

Actual behavior

Always resulting in:

[move_group.moveit.moveit.ros.move_group.cartesian_path_service_capability]: Attempting to follow 2 waypoints for link 'calfSphere' using a step of 0.001000 m and jump threshold 0.000000 (in global reference frame)

Backtrace or Console output

No response

Metadata

Metadata

Assignees

No one assigned

    Labels

    bugSomething isn't working

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions