Regarding goal tolerance in moveit_py api bindings

[yqkqknct]

Description

I was using the python binding for moveit2 library, but unfortunately I am unable to make the trajectory planning stable and it often shows the error "Unable to solve the planning problem" or timeout (over 20s) as follows:

Besides, the robot is a 5-DOF robot arm with addition 1 DOF for gripper.

moveit_controller-1] [INFO] [1733616772.991477379] [moveit_controller]: Target pose: geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=0.0, y=0.12, z=0.07), orientation=geometry_msgs.msg.Quaternion(x=-0.7071067811865476, y=0.0, z=0.0, w=0.7071067811865476))
[moveit_controller-1] [INFO] [1733616772.991932086] [moveit_controller]: Update current joint degrees: {'link1_to_base_link': 0.0, 'link2_to_link1': 0.0, 'link3_to_link2': 0.0, 'link4_to_link3': 0.0, 'link5_to_link4': 0.0, 'link6_to_link5': 0.0}
[moveit_controller-1] [INFO] [1733616772.993141512] [moveit_controller]: Current pose: geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.0005580000000000001, y=0.32107, z=0.041141000000000004), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))
[moveit_controller-1] [INFO] [1733616772.994579686] [moveit_controller]: Planning trajectory
[moveit_controller-1] [INFO] [1733616772.994876643] [moveit_controller]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[moveit_controller-1] [INFO] [1733616772.995129184] [moveit_controller]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[moveit_controller-1] [INFO] [1733616772.995151500] [moveit_controller]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[moveit_controller-1] [INFO] [1733616772.995191994] [moveit_controller]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[moveit_controller-1] [WARN] [1733616772.995374200] [moveit_2119459007.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead.
[moveit_controller-1] [INFO] [1733616772.996140903] [moveit_2119459007.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[moveit_controller-1] [INFO] [1733616772.996362339] [moveit_controller]: Calling Planner 'OMPL'
[moveit_controller-1] [ERROR] [1733616792.998795001] [moveit_2119459007.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - manipulator/manipulator: Unable to sample any valid states for goal tree
[moveit_controller-1] [ERROR] [1733616793.001731750] [moveit_2119459007.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - manipulator/manipulator: Unable to sample any valid states for goal tree
[moveit_controller-1] [ERROR] [1733616793.003247415] [moveit_2119459007.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - manipulator/manipulator: Unable to sample any valid states for goal tree
[moveit_controller-1] [ERROR] [1733616793.003307984] [moveit_2119459007.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - manipulator/manipulator: Unable to sample any valid states for goal tree
[moveit_controller-1] [WARN] [1733616793.003438425] [moveit_2119459007.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 20.006969 seconds
[moveit_controller-1] [ERROR] [1733616793.038515293] [moveit_2119459007.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem
[moveit_controller-1] [ERROR] [1733616793.038612219] [moveit_controller]: Planner 'OMPL' failed with error code FAILURE

I couldn't find any APIs to set the goal constraints easily, though I noticed that there is a kinematic_constraints where there is a constructJointConstraint function, however I think that's different from the capability provided from MoveGroupInterface where I just need to set a scalar.

Therefore, I managed to change all my code to C++ version with all API available to use. Surprisingly, I found the planning goes well, I tested it on the same script, where multiple tries always succeed to find a trajectory result.

I also take a look the default tolerance value by

    RCLCPP_INFO(get_logger(), "Planning pipeline id: %s", move_group_interface_->getPlanningPipelineId().c_str());
    RCLCPP_INFO(get_logger(), "Default planning pipeline id: %s",
                move_group_interface_->getDefaultPlanningPipelineId().c_str());
    RCLCPP_INFO(get_logger(), "Planner id: %s", move_group_interface_->getPlannerId().c_str());
    RCLCPP_INFO(get_logger(), "Planner time: %f", move_group_interface_->getPlanningTime());
    RCLCPP_INFO(get_logger(), "Goal joint tolerance: %f", move_group_interface_->getGoalJointTolerance());
    RCLCPP_INFO(get_logger(), "Goal position tolerance: %f", move_group_interface_->getGoalPositionTolerance());
    RCLCPP_INFO(get_logger(), "Goal orientation tolerance: %f", move_group_interface_->getGoalOrientationTolerance());
    RCLCPP_INFO(get_logger(), "Max velocity scaling factor: %f", move_group_interface_->getMaxVelocityScalingFactor());
    RCLCPP_INFO(get_logger(), "Max acceleration scaling factor: %f",
                move_group_interface_->getMaxAccelerationScalingFactor());

The output is as below, where there is default goal tolerance 1e-4:

[INFO] [1733626853.009494624] [robot_controller]: Planning pipeline id: 
[INFO] [1733626853.009504824] [robot_controller]: Default planning pipeline id: 
[INFO] [1733626853.009510093] [robot_controller]: Planner id: 
[INFO] [1733626853.009514944] [robot_controller]: Planner time: 5.000000
[INFO] [1733626853.009521385] [robot_controller]: Goal joint tolerance: 0.000100
[INFO] [1733626853.009527377] [robot_controller]: Goal position tolerance: 0.000100
[INFO] [1733626853.009532477] [robot_controller]: Goal orientation tolerance: 0.001000
[INFO] [1733626853.009537848] [robot_controller]: Max velocity scaling factor: 0.100000
[INFO] [1733626853.009542965] [robot_controller]: Max acceleration scaling factor: 0.100000

Then, I realized that the Python API is a bit different from C++ part. It uses MoveGroupInterface in C++, but in Python, it uses PlanningComponent.

I also tried to set the goal tolerance to 0 in C++ implementation. The planning become unable to generate reliable plan and often failed. (the code to set goal tolerance is below)

move_group_interface_->setGoalJointTolerance(0);
    move_group_interface_->setGoalPositionTolerance(0);
    move_group_interface_->setGoalOrientationTolerance(0);

I think the inability to generate a reliable plan in Python API is due to the lack of similar API support on goal tolerance. Is it something in the roadmap, or is it something that due to the design philosophy in Python API so it won't be supported in this way?

Some refs:

• PlanningComponent: https://moveit.picknik.ai/main/api/html/classmoveit__cpp_1_1PlanningComponent.html#abb85bebb94c2fb211b62ce34e49500f1
• MoveGroupInterface: https://moveit.picknik.ai/main/api/html/classmoveit_1_1planning__interface_1_1MoveGroupInterface_1_1MoveGroupInterfaceImpl.html

Thank you very much.

ROS Distro

Jazzy

OS and version

Ubuntu 24.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

main

Which RMW are you using?

None

Steps to Reproduce

Follow the Python guide, and use a 5-DOF robot URDF

Expected behavior

It should be able to set goal tolerance to make planning more reliable.

Actual behavior

I couldn't find the api related to goal tolerance on Python.

Backtrace or Console output

No response

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.