-
Notifications
You must be signed in to change notification settings - Fork 698
Open
Labels
bugSomething isn't workingSomething isn't working
Description
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
Labels
bugSomething isn't workingSomething isn't working