search

UniversalRobots / Universal_Robots_ROS2_Driver

Universal Robots ROS2 driver supporting CB3 and e-Series
BSD 3-Clause "New" or "Revised" License
422 stars 219 forks source link

Trajectory execution failed with error: PATH_TOLERANCE_VIOLATED #1072

Closed Yazan-Soliman closed 1 month ago

Yazan-Soliman commented 2 months ago

Affected ROS2 Driver version(s)

2.2.14-1

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR E-series robot, Real robot, URSim in docker

Robot SW / URSim version(s)

5.16.0

How is the ROS driver used.

Headless without using the teach pendant

Issue details

Summary

moving the robot (ur10e) over scaled_joint_trajectory_controller when in using ursim docker or on the real robot (ur10e) returning the following error

[ERROR] [1722985034.879335744] [ur10e_trajectory_planner]: Trajectory execution failed with error: PATH_TOLERANCE_VIOLATED

Issue details

Detailed description help us understand the problem. Code are welcome!

Steps to Reproduce

launch the ur_robot_driver to a real robot or a ursim docker execute test trajectory like in ur_robot_driver/scripts/example_move.py

Expected Behavior

when only using rviz with use?fake_hardware:= the robot follows the desired trajectories. but when connected to the real robot or ursim docker the above error occurs.

Actual Behavior

robot does not move

Relevant log output

`[ur_ros2_control_node-1] [INFO] [1722985006.778117990] [scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-1] [INFO] [1722985006.778220070] [scaled_joint_trajectory_controller]: Accepted new action goal
[ur_ros2_control_node-1] [WARN] [1722985034.829688924] [scaled_joint_trajectory_controller]: Aborted due to state tolerance violation
`

Accept Public visibility

fmauch commented 2 months ago

What kind of trajectory are you sending to the robot? Is it unfeasible in any way? Does using the example_move.py script run without errors?

Yazan-Soliman commented 2 months ago

What kind of trajectory are you sending to the robot? Is it unfeasible in any way? Does using the example_move.py script run without errors?

The trajectories are feasible from what I see when they are executed when use_fake_hardware:=true on rviz, on the real hardware unfortunately not. example_move.py shows the same error 

[INFO] [1723022412.532784810] [jtc_client]: Waiting for action server on scaled_joint_trajectory_controller/follow_joint_trajectory
[INFO] [1723022412.533669464] [jtc_client]: Executing trajectory traj0
[INFO] [1723022413.086584979] [jtc_client]: Done with result: PATH_TOLERANCE_VIOLATED
Traceback (most recent call last):
  File "Universal_Robots_ROS2_Driver/ur_robot_driver/scripts/example_move.py", line 190, in <module>
    main()
  File "Universal_Robots_ROS2_Driver/ur_robot_driver/scripts/example_move.py", line 182, in main
    rclpy.spin(jtc_client)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/__init__.py", line 222, in spin
    executor.spin_once()
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 739, in spin_once
    self._spin_once_impl(timeout_sec)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 736, in _spin_once_impl
    raise handler.exception()
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 251, in __call__
    self.set_result(self._handler(*self._args, **self._kwargs))
  File "Universal_Robots_ROS2_Driver/ur_robot_driver/scripts/example_move.py", line 159, in get_result_callback
    raise RuntimeError("Executing trajectory failed")
RuntimeError: Executing trajectory failed`

from the running terminal


`[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ur_ros2_control_node-1]: process started with pid [95974]
[INFO] [robot_state_publisher-2]: process started with pid [95976]
[INFO] [rviz2-3]: process started with pid [95978]
[INFO] [spawner-4]: process started with pid [95980]
[INFO] [spawner-5]: process started with pid [95982]
[INFO] [spawner-6]: process started with pid [95984]
[INFO] [spawner-7]: process started with pid [95986]
[INFO] [spawner-8]: process started with pid [95988]
[INFO] [spawner-9]: process started with pid [95990]
[ur_ros2_control_node-1] [WARN] [1723021998.289086273] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ur_ros2_control_node-1] text not specified in the tf_prefix tag
[ur_ros2_control_node-1] [INFO] [1723021998.289590025] [resource_manager]: Loading hardware 'ur10e' 
[ur_ros2_control_node-1] [INFO] [1723021998.292222499] [resource_manager]: Initialize hardware 'ur10e' 
[ur_ros2_control_node-1] [INFO] [1723021998.292397606] [resource_manager]: Successful initialization of hardware 'ur10e'
[ur_ros2_control_node-1] [INFO] [1723021998.292634663] [resource_manager]: 'configure' hardware 'ur10e' 
[ur_ros2_control_node-1] [INFO] [1723021998.292658973] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-1] [INFO] [1723021998.292679962] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-1] [WARN] [1723021998.293509758] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[robot_state_publisher-2] [INFO] [1723021998.294664304] [robot_state_publisher]: got segment base
[robot_state_publisher-2] [INFO] [1723021998.295051369] [robot_state_publisher]: got segment base_link
[robot_state_publisher-2] [INFO] [1723021998.295088458] [robot_state_publisher]: got segment base_link_inertia
[robot_state_publisher-2] [INFO] [1723021998.295098827] [robot_state_publisher]: got segment cell_base_link
[robot_state_publisher-2] [INFO] [1723021998.295105588] [robot_state_publisher]: got segment flange
[robot_state_publisher-2] [INFO] [1723021998.295111816] [robot_state_publisher]: got segment forearm_link
[robot_state_publisher-2] [INFO] [1723021998.295120256] [robot_state_publisher]: got segment ft_frame
[robot_state_publisher-2] [INFO] [1723021998.295126142] [robot_state_publisher]: got segment robot_mount_link
[robot_state_publisher-2] [INFO] [1723021998.295131539] [robot_state_publisher]: got segment second_link_1
[robot_state_publisher-2] [INFO] [1723021998.295138156] [robot_state_publisher]: got segment shoulder_link
[robot_state_publisher-2] [INFO] [1723021998.295144374] [robot_state_publisher]: got segment tool0
[robot_state_publisher-2] [INFO] [1723021998.295150344] [robot_state_publisher]: got segment upper_arm_link
[robot_state_publisher-2] [INFO] [1723021998.295156881] [robot_state_publisher]: got segment world
[robot_state_publisher-2] [INFO] [1723021998.295163184] [robot_state_publisher]: got segment wrist_1_link
[robot_state_publisher-2] [INFO] [1723021998.295169362] [robot_state_publisher]: got segment wrist_2_link
[robot_state_publisher-2] [INFO] [1723021998.295175492] [robot_state_publisher]: got segment wrist_3_link
[rviz2-3] [INFO] [1723021998.835684166] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1723021998.836061367] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[ur_ros2_control_node-1] [INFO] [1723021998.857990575] [UR_Client_Library:]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-1] [INFO] [1723021998.858335443] [UR_Client_Library:]: Setting up RTDE communication with frequency 500.000000
[rviz2-3] [INFO] [1723021998.869332072] [rviz2]: Stereo is NOT SUPPORTED
[ur_ros2_control_node-1] [INFO] [1723021999.869640687] [URPositionHardwareInterface]: Calibration checksum: 'calib_6826641031304328834'.
[spawner-5] [INFO] [1723022000.667077919] [spawner_speed_scaling_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1723022000.764111400] [spawner_force_torque_sensor_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-7] [INFO] [1723022000.772108852] [spawner_scaled_joint_trajectory_controller]: Waiting for '/controller_manager' services to be available
[spawner-8] [INFO] [1723022000.831249389] [spawner_cartesian_motion_controller]: Waiting for '/controller_manager' services to be available
[spawner-9] [INFO] [1723022000.845301419] [spawner_motion_control_handle]: Waiting for '/controller_manager' services to be available
[spawner-4] [INFO] [1723022000.864202709] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[ur_ros2_control_node-1] [INFO] [1723022000.871084776] [URPositionHardwareInterface]: Calibration checked successfully.
[ur_ros2_control_node-1] [INFO] [1723022000.871341144] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-1] [INFO] [1723022000.871426871] [resource_manager]: Successful 'configure' of hardware 'ur10e'
[ur_ros2_control_node-1] [INFO] [1723022000.871542236] [resource_manager]: 'activate' hardware 'ur10e' 
[ur_ros2_control_node-1] [INFO] [1723022000.871558348] [URPositionHardwareInterface]: Activating HW interface
[ur_ros2_control_node-1] [INFO] [1723022000.871566902] [resource_manager]: Successful 'activate' of hardware 'ur10e'
[ur_ros2_control_node-1] [WARN] [1723022000.884080167] [controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-1] [WARN] [1723022000.884240878] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ur_ros2_control_node-1] [INFO] [1723022000.975423361] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster'
[spawner-6] [INFO] [1723022000.987453907] [spawner_force_torque_sensor_broadcaster]: Loaded force_torque_sensor_broadcaster
[ur_ros2_control_node-1] [INFO] [1723022000.988237850] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-1] [WARN] [1723022001.006615406] [scaled_joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ur_ros2_control_node-1] [INFO] [1723022001.008956795] [controller_manager]: Configuring controller 'force_torque_sensor_broadcaster'
[spawner-7] [INFO] [1723022001.010329672] [spawner_scaled_joint_trajectory_controller]: Loaded scaled_joint_trajectory_controller
[ur_ros2_control_node-1] [INFO] [1723022001.012525057] [controller_manager]: Configuring controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-1] [INFO] [1723022001.012770908] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ur_ros2_control_node-1] [INFO] [1723022001.012802988] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ur_ros2_control_node-1] [INFO] [1723022001.012815099] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method.
[ur_ros2_control_node-1] [INFO] [1723022001.014410933] [scaled_joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[ur_ros2_control_node-1] [INFO] [1723022001.017214842] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-6] [INFO] [1723022001.031208717] [spawner_force_torque_sensor_broadcaster]: Configured and activated force_torque_sensor_broadcaster
[spawner-7] [INFO] [1723022001.034229576] [spawner_scaled_joint_trajectory_controller]: Configured and activated scaled_joint_trajectory_controller
[ur_ros2_control_node-1] [INFO] [1723022001.062835800] [controller_manager]: Loading controller 'motion_control_handle'
[spawner-9] [INFO] [1723022001.088832316] [spawner_motion_control_handle]: Loaded motion_control_handle
[ur_ros2_control_node-1] [INFO] [1723022001.093079770] [controller_manager]: Loading controller 'joint_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1723022001.108560803] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster'
[spawner-4] [INFO] [1723022001.110310579] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1723022001.117848626] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: 
[ur_ros2_control_node-1] [INFO] [1723022001.118627328] [controller_manager]: Configuring controller 'motion_control_handle'
[spawner-5] [INFO] [1723022001.119726381] [spawner_speed_scaling_state_broadcaster]: Loaded speed_scaling_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1723022001.130782547] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1723022001.131037253] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[ur_ros2_control_node-1] [INFO] [1723022001.137183119] [controller_manager]: Configuring controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1723022001.137373794] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz
[ur_ros2_control_node-1] [INFO] [1723022001.144825596] [controller_manager]: Loading controller 'cartesian_motion_controller'
[spawner-8] [INFO] [1723022001.168092300] [spawner_cartesian_motion_controller]: Loaded cartesian_motion_controller
[spawner-4] [INFO] [1723022001.173861880] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1723022001.175229569] [controller_manager]: Configuring controller 'cartesian_motion_controller'
[spawner-5] [INFO] [1723022001.176373558] [spawner_speed_scaling_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1723022001.184134526] [cartesian_motion_controller]: Forward dynamics solver initialized
[ur_ros2_control_node-1] [INFO] [1723022001.184240786] [cartesian_motion_controller]: Forward dynamics solver has control over 6 joints
[INFO] [spawner-7]: process has finished cleanly [pid 95986]
[INFO] [spawner-6]: process has finished cleanly [pid 95984]
[INFO] [spawner-9]: process has finished cleanly [pid 95990]
[INFO] [spawner-4]: process has finished cleanly [pid 95980]
[INFO] [spawner-5]: process has finished cleanly [pid 95982]
[INFO] [spawner-8]: process has finished cleanly [pid 95988]
[ur_ros2_control_node-1] [INFO] [1723022254.008600285] [scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-1] [INFO] [1723022254.008643713] [scaled_joint_trajectory_controller]: Accepted new action goal
[ur_ros2_control_node-1] [INFO] [1723022294.001695308] [UR_Client_Library:]: Robot requested program
[ur_ros2_control_node-1] [INFO] [1723022294.001795412] [UR_Client_Library:]: Sent program to robot
[ur_ros2_control_node-1] [WARN] [1723022295.344203311] [scaled_joint_trajectory_controller]: Aborted due to state tolerance violation
[ur_ros2_control_node-1] [INFO] [1723022300.574945972] [scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-1] [INFO] [1723022300.574997106] [scaled_joint_trajectory_controller]: Accepted new action goal
[ur_ros2_control_node-1] [WARN] [1723022301.602219145] [scaled_joint_trajectory_controller]: Aborted due to state tolerance violation
[ur_ros2_control_node-1] [INFO] [1723022412.534407237] [scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-1] [INFO] [1723022412.534483626] [scaled_joint_trajectory_controller]: Accepted new action goal
[ur_ros2_control_node-1] [WARN] [1723022413.050250036] [scaled_joint_trajectory_controller]: Aborted due to state tolerance violation
`
Yazan-Soliman commented 2 months ago

I managed to solve the problem – it was a firewall issue! I'm still not entirely sure how the error of state tolerance violation relates to the firewall, but here's what tipped me off:

Testing on a different laptop I noticed the following log entry: [ur_ros2_control_node-1] [INFO] [1723119165.138680125] [UR_Client_Library:]: Robot connected to reverse interface. Ready to receive control commands. I did not see this log entry on my primary machine, also [ur_ros2_control_node-1] [WARN] [1723022413.050250036] [scaled_joint_trajectory_controller]: Aborted due to state tolerance violation was very misleading initially.

VinDp commented 1 month ago

Since the issue seems to have been solved, I will close it. Feel free to reopen if still relevant.