Wrist_3_joint exceeds position tolerance

[ChandimaFernando]

Robot: ur3e
ROS2 version: humble
OS version: Ubuntu 22.04
Planner: Moveit2 RRTConnect
Trajectory controller: scaled_joint_trajectory_controller

Trajectory execution fails after planning with the following error on ur_ros2_control node (launch via ur_robot_driver).

[ur_ros2_control_node-1] [INFO] [1733778715.688537686] [scaled_joint_trajectory_controller]: Accepted new action goa
[ur_ros2_control_node-1] [ERROR] [1733778715.689473971] [tolerances]: State tolerances failed for joint 5:
[ur_ros2_control_node-1] [ERROR] [1733778715.689491616] [tolerances]: Position Error: -6.283177, Position Tolerance: 0.200000
[ur_ros2_control_node-1] [WARN] [1733778715.689507842] [scaled_joint_trajectory_controller]: Aborted due to state tolerance violation

Below is the error trace on move_group :

[move_group-1] [INFO] [1733778715.688660404] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution
[move_group-1] [INFO] [1733778715.688686778] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-1] [WARN] [1733778715.738984041] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' failed with error PATH_TOLERANCE_VIOLATED: Aborted due to path tolerance violation
[move_group-1] [WARN] [1733778715.739030144] [moveit_ros.trajectory_execution_manager]: Controller handle scaled_joint_trajectory_controller reports status ABORTED
[move_group-1] [INFO] [1733778715.739046027] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-1] [INFO] [1733778715.739168229] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED

Observed the error most likely after #196 . In my trajectory, starting state and the target state for wrist 3 is pi. Moveit planned trajectory after move_group_interface.plan() starts from -pi when fails happen. -pi for start state is acceptable at the wrist 3 as wrist 3 is now a continuous joint as per #196 . Tracked the error from here to here.

[urfeex]

Thank you for reporting this. This sounds likely that we need to update things at other places, as well. I assume, that this will be more affecting the driver repo, though. I will move the issue once that is more clear.

Edit: I have a bit of trouble reproducing this. @ChandimaFernando could you maybe provide a trajectory producing this issue?

[ChandimaFernando]

Hi, Thanks for the response. The error happens at random. I tried a bunch of times today to reproduce the error on a specific trajectory, but could not reproduce it with the limited time I had. I will try again and keep you posted.