Execution stuck or timeout when running ur_moveit_config

[karimsiala]

Affected ROS2 Driver version(s)

2.2.9-1focal.20231213.212503 arm64

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel, Other Linux system

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR CB3 robot

Robot SW / URSim version(s)

3.15.8.106339

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

While setting up an Nvidia Jetson Orin NX board with a Universal Robots UR3 using the Universal_Robots_ROS2_Driver, I encountered an issue during trajectory execution through RViz with MoveIt2.

Issue details

After successfully configuring the Nvidia board with the UR3 and ensuring connectivity (I can ping the robot, and execute scripts that pops up a window in the Polyscope interface), I face an issue when attempting to execute trajectories through RViz. Specifically, upon launching the default MoveIt config launch file and trying to execute a trajectory, it either stuck in a planning loop or I receive an error the controller is taking too long to execute the trajectory.

Steps to Reproduce

1. Installed Universal_Robots_ROS2_Driver from binaries
2. Launch the driver: ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur3 robot_ip:=192.168.10.166 launch_rviz:=false kinematics_config:=~/my_robot_calibration.yaml
3. Launch the included MoveIt config: ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur3 launch_rviz:=true
4. Try to plan and execute a path

Expected Behavior

I expected the robot to execute the planned trajectory smoothly without any errors, as the setup was completed according to the provided documentation.

Actual Behavior

The trajectory execution either takes forever and stucks in a loop or fails with an error message from MoveIt, indicating for example that the controller is taking too long to execute the trajectory, specifically exceeding an expected upper bound of 5.029743 seconds. This prevents any trajectory execution through RViz, despite successful communication tests.

Thank you in advance and I hope I'm not creating a duplicate because I made sure to search everywhere for a solution.

Relevant log output

csia-jetson@csiajetson-desktop:~$ LC_NUMERIC=en_US.UTF-8 ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur3 launch_rviz:=true
[INFO] [launch]: All log files can be found below /home/csia-jetson/.ros/log/2024-02-26-09-14-09-115003-csiajetson-desktop-3701
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [move_group-1]: process started with pid [3710]
[INFO] [rviz2-2]: process started with pid [3712]
[INFO] [servo_node_main-3]: process started with pid [3714]
[servo_node_main-3] [WARN] [1708935253.200570380] [moveit_servo.servo_node]: Intra-process communication is disabled, consider enabling it by adding: 
[servo_node_main-3] extra_arguments=[{'use_intra_process_comms' : True}]
[servo_node_main-3] to the Servo composable node in the launch file
[move_group-1] [WARN] [1708935253.207353087] [move_group.move_group]: Falling back to using the the move_group node namespace (deprecated behavior).
[move_group-1] [INFO] [1708935253.213797100] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00577856 seconds
[move_group-1] [INFO] [1708935253.213873038] [moveit_robot_model.robot_model]: Loading robot model 'ur'...
[move_group-1] [INFO] [1708935253.213917134] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[servo_node_main-3] [INFO] [1708935253.223048041] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0059552 seconds
[servo_node_main-3] [INFO] [1708935253.223135531] [moveit_robot_model.robot_model]: Loading robot model 'ur'...
[servo_node_main-3] [INFO] [1708935253.223164651] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[servo_node_main-3] [WARN] [1708935253.307897739] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[servo_node_main-3] [INFO] [1708935253.508706494] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[servo_node_main-3] [INFO] [1708935253.519589271] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[servo_node_main-3] [INFO] [1708935253.519640344] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[servo_node_main-3] [INFO] [1708935253.521201970] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[servo_node_main-3] [INFO] [1708935253.523844671] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on '/servo_node/publish_planning_scene'
[servo_node_main-3] [WARN] [1708935253.541483211] [moveit_servo.servo_calcs]: No kinematics solver instantiated for group 'ur_manipulator'. Will use inverse Jacobian for servo calculations instead.
[servo_node_main-3] [WARN] [1708935253.541568492] [moveit_servo.collision_check]: Collision check rate is low, increase it in yaml file if CPU allows
[move_group-1] [INFO] [1708935253.544298619] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-1] [INFO] [1708935253.544695585] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-1] [INFO] [1708935253.547748597] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-1] [INFO] [1708935253.548628452] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-1] [INFO] [1708935253.548671749] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-1] [INFO] [1708935253.550016988] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-1] [INFO] [1708935253.550055068] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-1] [INFO] [1708935253.554354405] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-1] [INFO] [1708935253.555467672] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-1] [WARN] [1708935253.555814078] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-1] [ERROR] [1708935253.555848511] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[move_group-1] [INFO] [1708935253.644147419] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'move_group'
[move_group-1] [INFO] [1708935253.715904030] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-1] [INFO] [1708935253.722062502] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.path_tolerance' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1708935253.722117191] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.resample_dt' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1708935253.722126791] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.min_angle_change' was not set. Using default value: 0.001000
[move_group-1] [INFO] [1708935253.722153736] [moveit_ros.fix_workspace_bounds]: Param 'move_group.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-1] [INFO] [1708935253.722177544] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_bounds_error' was set to 0.100000
[move_group-1] [INFO] [1708935253.722186184] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1708935253.722201737] [moveit_ros.fix_start_state_collision]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1708935253.722208713] [moveit_ros.fix_start_state_collision]: Param 'move_group.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-1] [INFO] [1708935253.722233769] [moveit_ros.fix_start_state_collision]: Param 'move_group.max_sampling_attempts' was not set. Using default value: 100
[move_group-1] [INFO] [1708935253.722251114] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-1] [INFO] [1708935253.722260554] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-1] [INFO] [1708935253.722265098] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-1] [INFO] [1708935253.722268906] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-1] [INFO] [1708935253.722340619] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-1] [INFO] [1708935253.769376490] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller
[move_group-1] [INFO] [1708935253.774889256] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller
[move_group-1] [INFO] [1708935253.775215245] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1708935253.775309935] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1708935253.776255999] [moveit_ros.trajectory_execution_manager]: Trajectory execution is not managing controllers
[move_group-1] [INFO] [1708935253.776292832] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-1] [INFO] [1708935253.813897630] [move_group.move_group]: 
[move_group-1] 
[move_group-1] ********************************************************
[move_group-1] * MoveGroup using: 
[move_group-1] *     - ApplyPlanningSceneService
[move_group-1] *     - ClearOctomapService
[move_group-1] *     - CartesianPathService
[move_group-1] *     - ExecuteTrajectoryAction
[move_group-1] *     - GetPlanningSceneService
[move_group-1] *     - KinematicsService
[move_group-1] *     - MoveAction
[move_group-1] *     - MotionPlanService
[move_group-1] *     - QueryPlannersService
[move_group-1] *     - StateValidationService
[move_group-1] ********************************************************
[move_group-1] 
[move_group-1] [INFO] [1708935253.813999776] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-1] [INFO] [1708935253.814017408] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-1] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-1] Loading 'move_group/ClearOctomapService'...
[move_group-1] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-1] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-1] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-1] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-1] Loading 'move_group/MoveGroupMoveAction'...
[move_group-1] Loading 'move_group/MoveGroupPlanService'...
[move_group-1] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-1] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-1] 
[move_group-1] You can start planning now!
[move_group-1] 
[rviz2-2] [INFO] [1708935254.508561883] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1708935254.508765790] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-2] [INFO] [1708935254.603165879] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-2]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[rviz2-2] [ERROR] [1708935258.001243051] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-2] [INFO] [1708935258.031799169] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-2] [INFO] [1708935258.103379128] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0053869 seconds
[rviz2-2] [INFO] [1708935258.103461945] [moveit_robot_model.robot_model]: Loading robot model 'ur'...
[rviz2-2] [INFO] [1708935258.103490073] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-2] [INFO] [1708935258.398480835] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-2] [INFO] [1708935258.400764840] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-2] [INFO] [1708935258.733654016] [interactive_marker_display_187650783914800]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-2] [INFO] [1708935258.750554709] [moveit_ros_visualization.motion_planning_frame]: group ur_manipulator
[rviz2-2] [INFO] [1708935258.750598678] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'ur_manipulator' in namespace ''
[rviz2-2] [INFO] [1708935258.766280439] [move_group_interface]: Ready to take commands for planning group ur_manipulator.
[rviz2-2] [INFO] [1708935258.771766513] [interactive_marker_display_187650783914800]: Sending request for interactive markers
[rviz2-2] [INFO] [1708935258.806991251] [interactive_marker_display_187650783914800]: Service response received for initialization
[move_group-1] [INFO] [1708935285.727438212] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[rviz2-2] [INFO] [1708935285.727813097] [move_group_interface]: Plan and Execute request accepted
[move_group-1] [INFO] [1708935285.727866474] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-1] [INFO] [1708935285.730251692] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-1] [INFO] [1708935285.730530575] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-1] [INFO] [1708935285.730561488] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'move_group'
[move_group-1] [INFO] [1708935285.734967181] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-1] [WARN] [1708935285.804538266] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
[move_group-1] [INFO] [1708935285.809828164] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1708935285.809942822] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1708935285.810160489] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-1] [INFO] [1708935285.818591551] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-1] [INFO] [1708935285.818807874] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1708935285.818849666] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1708935285.819272936] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller
[move_group-1] [INFO] [1708935285.820847838] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution
[move_group-1] [INFO] [1708935285.820891423] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-1] [WARN] [1708935290.664641115] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: waitForExecution timed out
[move_group-1] [ERROR] [1708935290.664711932] [moveit_ros.trajectory_execution_manager]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 4.843375 seconds). Stopping trajectory.
[move_group-1] [INFO] [1708935290.664933439] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Cancelling execution for scaled_joint_trajectory_controller

Accept Public visibility

• I agree to make this context public

[fmauch]

Well, did you start the external control program on the robot? Do you see the Robot ready to receive control commands output in the driver's shell?

Also, when executing a trajectory, the driver should print about that event, so that log output might be relevant as well, to see what's going on.

[karimsiala]

I was unable to further troubleshoot the issue due to a sudden change in the project hardware. Thank you so much for your assistance.
I will close the issue.