Error Launching MoveIt2 Script for Moving the Robot: SRDF Not Found #3133

janperenic · 2024-11-26T10:05:31Z

janperenic
Nov 26, 2024

Hi, I’m experiencing an issue with my MoveIt2 project using a Yaskawa HC10DT robot. With the help of @jimmy-mcelwain, I was able to successfully plan a path and move the robot using the interactive marker in RViz. However, when I try to launch the script to move, I encounter an error that I can't resolve.

The script is a modified version of the hello_moveit tutorial script. Despite including both robot_description and robot_description_semantic in the parameter list and ensuring that the files are correctly formatted, I still encounter the following error:

This script previously worked with the official Panda tutorial and my setup from the Setup Assistant, where it successfully planned a path (the problem is this one doesn't work on real robot). I also tested it on your robot simulation but got the same error. The .srdf file is listed in the demo.launch.py file and appears correct.

Error message when launcing the script to move the robot:

janperenic@janperenic-OMEN:~/moveit2$ ros2 run hello_moveit hello_moveit_node
[ERROR] [1732271419.460509462] [hello_moveit]: Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds.
Error:   Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT ErrorID=13 (0xd) Line number=0
         at line 715 in ./src/model.cpp
[ERROR] [1732271419.478091279] [moveit_rdf_loader.rdf_loader]: Unable to parse SRDF
[FATAL] [1732271419.478178221] [move_group_interface]: Unable to construct robot model. Please make sure all needed information is on the parameter server.
terminate called after throwing an instance of 'std::runtime_error'
  what():  Unable to construct robot model. Please make sure all needed information is on the parameter server.
[ros2run]: Aborted

I am using Motors2 version & ROS2 distribution:
0.1.2
Humble

Robot Controller & Robot:
YRC1000
HC10

Terminal log:

janperenic@janperenic-OMEN:~/moveit2$ ros2 launch hc10dt_moveit_config demo.launch.py 
[INFO] [launch]: All log files can be found below /home/janperenic/.ros/log/2024-11-20-15-56-09-358892-janperenic-OMEN-114392
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [move_group-1]: process started with pid [114393]
[INFO] [rviz2-2]: process started with pid [114395]
[INFO] [robot_state_publisher-3]: process started with pid [114397]
[robot_state_publisher-3] [INFO] [1732114569.594887459] [robot_state_publisher]: got segment base
[robot_state_publisher-3] [INFO] [1732114569.595091440] [robot_state_publisher]: got segment base_link
[robot_state_publisher-3] [INFO] [1732114569.595099283] [robot_state_publisher]: got segment link_1
[robot_state_publisher-3] [INFO] [1732114569.595104090] [robot_state_publisher]: got segment link_2
[robot_state_publisher-3] [INFO] [1732114569.595108898] [robot_state_publisher]: got segment link_3
[robot_state_publisher-3] [INFO] [1732114569.595113155] [robot_state_publisher]: got segment link_4
[robot_state_publisher-3] [INFO] [1732114569.595117222] [robot_state_publisher]: got segment link_5
[robot_state_publisher-3] [INFO] [1732114569.595121959] [robot_state_publisher]: got segment link_6
[robot_state_publisher-3] [INFO] [1732114569.595126407] [robot_state_publisher]: got segment tool0
[move_group-1] [INFO] [1732114569.604070435] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0235261 seconds
[move_group-1] [INFO] [1732114569.604134729] [moveit_robot_model.robot_model]: Loading robot model 'motoman_hc10dt'...
[move_group-1] [INFO] [1732114569.640956340] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-1] [INFO] [1732114569.641143103] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-1] [INFO] [1732114569.641875791] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-1] [INFO] [1732114569.642119497] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-1] [INFO] [1732114569.642132187] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-1] [INFO] [1732114569.642328175] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-1] [INFO] [1732114569.642339453] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-1] [INFO] [1732114569.642559090] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-1] [INFO] [1732114569.642757281] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-1] [WARN] [1732114569.642944936] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-1] [ERROR] [1732114569.642956114] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[move_group-1] [INFO] [1732114569.644864830] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[move_group-1] [INFO] [1732114569.654970593] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-1] [INFO] [1732114569.657236799] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1732114569.657271205] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1732114569.657276173] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-1] [INFO] [1732114569.657292950] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-1] [INFO] [1732114569.657306572] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-1] [INFO] [1732114569.657312652] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1732114569.657321727] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1732114569.657327526] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was set to 0.050000
[move_group-1] [INFO] [1732114569.657332394] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-1] [INFO] [1732114569.657439127] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-1] [INFO] [1732114569.657444296] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-1] [INFO] [1732114569.657447501] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-1] [INFO] [1732114569.657451137] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-1] [INFO] [1732114569.657454322] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-1] [INFO] [1732114569.657459160] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-1] [INFO] [1732114569.659263697] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'pilz_industrial_motion_planner'
[move_group-1] [INFO] [1732114569.661265254] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-1] [INFO] [1732114569.663076943] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[move_group-1] [INFO] [1732114569.663088582] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[move_group-1] [INFO] [1732114569.663935716] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[move_group-1] [INFO] [1732114569.663945221] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[move_group-1] [INFO] [1732114569.664508265] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[move_group-1] [INFO] [1732114569.664516919] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[move_group-1] [INFO] [1732114569.665046498] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[move_group-1] [INFO] [1732114569.665056344] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[move_group-1] [INFO] [1732114569.665527117] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'chomp'
[move_group-1] [ERROR] [1732114569.666464348] [moveit.ros_planning.planning_pipeline]: Exception while loading planner 'chomp_interface/CHOMPPlanner': According to the loaded plugin descriptions the class chomp_interface/CHOMPPlanner with base class type planning_interface::PlannerManager does not exist. Declared types are  ompl_interface/OMPLPlanner pilz_industrial_motion_planner/CommandPlannerAvailable plugins: ompl_interface/OMPLPlanner, pilz_industrial_motion_planner/CommandPlanner
[move_group-1] [INFO] [1732114569.667323061] [moveit_ros.add_time_optimal_parameterization]: Param 'chomp.path_tolerance' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1732114569.667331254] [moveit_ros.add_time_optimal_parameterization]: Param 'chomp.resample_dt' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1732114569.667334840] [moveit_ros.add_time_optimal_parameterization]: Param 'chomp.min_angle_change' was not set. Using default value: 0.001000
[move_group-1] [INFO] [1732114569.667354882] [moveit_ros.fix_workspace_bounds]: Param 'chomp.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-1] [INFO] [1732114569.667365630] [moveit_ros.fix_start_state_bounds]: Param 'chomp.start_state_max_bounds_error' was set to 0.100000
[move_group-1] [INFO] [1732114569.667369456] [moveit_ros.fix_start_state_bounds]: Param 'chomp.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1732114569.667377699] [moveit_ros.fix_start_state_collision]: Param 'chomp.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1732114569.667381766] [moveit_ros.fix_start_state_collision]: Param 'chomp.jiggle_fraction' was set to 0.050000
[move_group-1] [INFO] [1732114569.667385702] [moveit_ros.fix_start_state_collision]: Param 'chomp.max_sampling_attempts' was not set. Using default value: 100
[move_group-1] [INFO] [1732114569.667395799] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-1] [INFO] [1732114569.667398794] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-1] [INFO] [1732114569.667401638] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-1] [INFO] [1732114569.667405074] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-1] [INFO] [1732114569.667407478] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-1] [INFO] [1732114569.667411043] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-1] [ERROR] [1732114569.667740027] [moveit.ros_planning_interface.moveit_cpp]: Failed to initialize planning pipeline 'chomp'.
[move_group-1] [INFO] [1732114569.689489708] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for follow_joint_trajectory
[move_group-1] [INFO] [1732114569.689712630] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1732114569.689732662] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1732114569.690088370] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-1] [INFO] [1732114569.690103154] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-1] [INFO] [1732114569.702036772] [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] [1732114569.702096869] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-1] [INFO] [1732114569.702105944] [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] [1732114569.850640350] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1732114569.850825941] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-2] [INFO] [1732114569.868951032] [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] [1732114572.956672772] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-2] [INFO] [1732114572.968170806] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-2] [INFO] [1732114573.040175715] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0195664 seconds
[rviz2-2] [INFO] [1732114573.040245438] [moveit_robot_model.robot_model]: Loading robot model 'motoman_hc10dt'...
[rviz2-2] [INFO] [1732114573.100962354] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-2] [INFO] [1732114573.101653741] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-2] [INFO] [1732114573.329440664] [interactive_marker_display_101501286873456]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[move_group-1] [WARN] [1732114573.334236718] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/base' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/base' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114573.334282422] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'world' to planning frame'base_link' (Could not find a connection between 'base_link' and 'world' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114573.334292779] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/flange' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/flange' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114573.334300231] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/tool0' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/tool0' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114573.334307453] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/tcp_15' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/tcp_15' because they are not part of the same tree.Tf has two or more unconnected trees.)
[rviz2-2] [INFO] [1732114573.335085801] [moveit_ros_visualization.motion_planning_frame]: group manipulator
[rviz2-2] [INFO] [1732114573.335103420] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'manipulator' in namespace ''
[rviz2-2] [INFO] [1732114573.342973943] [move_group_interface]: Ready to take commands for planning group manipulator.
[rviz2-2] [INFO] [1732114573.344037997] [moveit_ros_visualization.motion_planning_frame]: group manipulator
[rviz2-2] [INFO] [1732114573.344630643] [interactive_marker_display_101501286873456]: Sending request for interactive markers
[rviz2-2] [INFO] [1732114573.377712283] [interactive_marker_display_101501286873456]: Service response received for initialization
[rviz2-2] [INFO] [1732114584.785945070] [move_group_interface]: MoveGroup action client/server ready
[move_group-1] [INFO] [1732114584.786967816] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[rviz2-2] [INFO] [1732114584.787224318] [move_group_interface]: Planning request accepted
[move_group-1] [INFO] [1732114584.787236448] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-1] [WARN] [1732114584.804238224] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/base' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/base' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114584.804295459] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'world' to planning frame'base_link' (Could not find a connection between 'base_link' and 'world' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114584.804307719] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/flange' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/flange' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114584.804388011] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/tool0' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/tool0' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [WARN] [1732114584.804396495] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'r1/tcp_15' to planning frame'base_link' (Could not find a connection between 'base_link' and 'r1/tcp_15' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-1] [INFO] [1732114584.804498043] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-1] [INFO] [1732114584.804580899] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'pilz_industrial_motion_planner'
[move_group-1] [INFO] [1732114584.804673472] [moveit.pilz_industrial_motion_planner.trajectory_generator_ptp]: Initialized Point-to-Point Trajectory Generator.
[move_group-1] [INFO] [1732114584.804724676] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating PTP trajectory...
[move_group-1] [INFO] [1732114584.805673770] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
[rviz2-2] [INFO] [1732114584.806191643] [move_group_interface]: Planning request complete!
[rviz2-2] [INFO] [1732114584.806876762] [move_group_interface]: time taken to generate plan: 0.000292091 seconds
[move_group-1] [INFO] [1732114586.534683611] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-1] [INFO] [1732114586.534827228] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-1] [INFO] [1732114586.534887057] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1732114586.534907300] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[rviz2-2] [INFO] [1732114586.534912308] [move_group_interface]: Execute request accepted
[move_group-1] [INFO] [1732114586.534998451] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-1] [INFO] [1732114586.540882605] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-1] [INFO] [1732114586.540939990] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1732114586.540957439] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1732114586.541080562] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to follow_joint_trajectory
[move_group-1] [INFO] [1732114586.570922692] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: follow_joint_trajectory started execution
[move_group-1] [INFO] [1732114586.570979556] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-1] [WARN] [1732114589.076368877] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'follow_joint_trajectory' failed with error unknown error: Final position was outside tolerance. Check robot safety-limits that could be inhibiting motion. [joint_3: 0.0000 deviation] [joint_5: 0.0000 deviation]
[move_group-1] [WARN] [1732114589.076444883] [moveit_ros.trajectory_execution_manager]: Controller handle follow_joint_trajectory reports status ABORTED
[move_group-1] [INFO] [1732114589.076457794] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-1] [INFO] [1732114589.076523513] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED
[rviz2-2] [INFO] [1732114589.076854281] [move_group_interface]: Execute request aborted
[rviz2-2] [ERROR] [1732114589.076949709] [move_group_interface]: MoveGroupInterface::execute() failed or timeout reached

demo.launch.py :

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
import xacro


def generate_launch_description():
      db_arg = DeclareLaunchArgument("warehouse_host", default_value="", description="Database connection path")

      robot_description_file_path = os.path.join(
        get_package_share_directory("hc10dt_moveit_config"), "config", "motoman_hc10dt.urdf.xacro"
      )

      robot_description_semantic_file_path = os.path.join(
        get_package_share_directory("hc10dt_moveit_config"), "config", "motoman_hc10dt.srdf"
      )

      trajectory_execution_file_path = os.path.join(
        get_package_share_directory("hc10dt_moveit_config"), "config", "moveit_controllers.yaml"
      )

      rviz_config_file_path = os.path.join(
        get_package_share_directory("hc10dt_moveit_config"), "config", "moveit.rviz"
      )

      robot_description_contents = xacro.process_file(robot_description_file_path).toxml()

      warehouse_ros_config = {
            "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
            "warehouse_host": LaunchConfiguration("warehouse_host")
      }

      moveit_config = (
            MoveItConfigsBuilder("motoman_hc10dt", 
			      package_name="hc10dt_moveit_config",)
        	  .robot_description(file_path=robot_description_file_path)
        	  .robot_description_semantic(file_path=robot_description_semantic_file_path)
    		    .trajectory_execution(file_path=trajectory_execution_file_path)
    		    .to_moveit_configs()
	  )
    

	  # Start the actual move_group node/action server
      move_group_node = Node(
        
            package="moveit_ros_move_group",
            executable="move_group",
            output="screen",
            parameters=[moveit_config.to_dict(), warehouse_ros_config],
            arguments=["--ros-args", "--log-level", "info"],
	  )


      rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file_path],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
            warehouse_ros_config,
        ],
      )

      robot_state_publisher_node = Node(
            package="robot_state_publisher",
            executable="robot_state_publisher",
            name="robot_state_publisher",
            output="both",
            parameters=[{'robot_description':robot_description_contents}]
            )


      return LaunchDescription(
        [
          db_arg,
          move_group_node, 
          rviz_node, 
          robot_state_publisher_node,
        ]
      )

hello_moveit.py :

#include <memory>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char *argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Create a ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // Spin up a SingleThreadedExecutor for MoveItVisualTools to interact with ROS
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // Create the MoveIt MoveGroup Interface
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "manipulator");

  // Construct and initialize MoveItVisualTools
  auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
    node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
    move_group_interface.getRobotModel()};
  moveit_visual_tools.deleteAllMarkers();
  moveit_visual_tools.loadRemoteControl();

  // Create closures for visualization
  auto const draw_title = [&moveit_visual_tools](auto text) {
    auto const text_pose = [] {
      auto msg = Eigen::Isometry3d::Identity();
      msg.translation().z() = 1.5;  // Place text 1.5m above the base link
      return msg;
    }();
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
                                    rviz_visual_tools::XLARGE);
  };
  auto const prompt = [&moveit_visual_tools](auto text) {
    moveit_visual_tools.prompt(text);
  };
  auto const draw_trajectory_tool_path =
      [&moveit_visual_tools,
      jmg = move_group_interface.getRobotModel()->getJointModelGroup(
          "manipulator")](auto const trajectory) {
        moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
      };

  // Example Cartesian Path Integration Section (refer to the previous answer)
  std::vector<geometry_msgs::msg::Pose> waypoints;
  // geometry_msgs::msg::Pose start_pose = move_group_interface.getCurrentPose().pose;

  // Set start_pose as the first waypoint
  // Initial point (bottom-left corner of the square)
  geometry_msgs::msg::Pose start_pose;
  start_pose.position.x = 0.5;
  start_pose.position.y = -0.176;
  start_pose.position.z = 0.85;
  start_pose.orientation.x = 0.0;
  start_pose.orientation.y = 0.0;
  start_pose.orientation.z = 0.0;
  start_pose.orientation.w = 1.0;
  waypoints.push_back(start_pose);

  // Define other waypoints
  geometry_msgs::msg::Pose target_pose = start_pose;
  target_pose.position.z -= 0.2;  // Move down 20 cm
  waypoints.push_back(target_pose);

  target_pose.position.z += 0.2;  // Move up 20 cm back to origin
  waypoints.push_back(target_pose);

  target_pose.position.z += 0.2;  // Move up 20 cm
  waypoints.push_back(target_pose);

  target_pose.position.z -= 0.2;  // Move down 20 cm back to origin
  waypoints.push_back(target_pose);

  target_pose.position.y -= 0.2;  // Move right 20 cm
  waypoints.push_back(target_pose);

  target_pose.position.z += 0.2;  // Move up 20 cm
  target_pose.position.y += 0.2;  // Move back to original Y
  target_pose.position.x -= 0.2;  // Move left 20 cm
  waypoints.push_back(target_pose);

  // Compute Cartesian Path
  moveit_msgs::msg::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;
  const double eef_step = 0.01;
  double fraction = move_group_interface.computeCartesianPath(
      waypoints, eef_step, jump_threshold, trajectory);
  RCLCPP_INFO(logger, "Visualizing plan (Cartesian path) (%.2f%% achieved)",
              fraction * 100.0);

  // Visualize Cartesian Path in RViz
  moveit_visual_tools.deleteAllMarkers();
  moveit_visual_tools.publishText(start_pose, "Cartesian_Path", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
  moveit_visual_tools.publishPath(waypoints, rviz_visual_tools::LIME_GREEN, rviz_visual_tools::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    moveit_visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rviz_visual_tools::SMALL);
  moveit_visual_tools.trigger();
  prompt("Press 'next' in the RvizVisualToolsGui window to execute Cartesian Path");

  // Execute Cartesian Path
  move_group_interface.execute(trajectory);

  // Shutdown ROS
  rclcpp::shutdown();
  spinner.join();
  return 0;
}

I would appreciate any helpful input on how to solve the issue.
Thanks

Answered by Danilrivero

Nov 29, 2024

Hello,

A few points to note here, this happens because RViz has access to the parameters due to being passed through moveit_config and in this case being able to handle the robot_description & robot_description_semantic through params. This makes you be able to move it.

This is an interesting read to better understand this.

the robot description and semantics are loaded using the RDFLoader class, which will attempt to read the parameters from the node, and if that fails, will attempt to subscribe to the String topic for a short period of time. If both methods fail to get the parameter, then a warning will be printed to the console.

All the components of MoveIt that use the RobotModel n…

View full answer

Danilrivero · 2024-11-29T09:14:41Z

Danilrivero
Nov 29, 2024

Hello,

A few points to note here, this happens because RViz has access to the parameters due to being passed through moveit_config and in this case being able to handle the robot_description & robot_description_semantic through params. This makes you be able to move it.

This is an interesting read to better understand this.

the robot description and semantics are loaded using the RDFLoader class, which will attempt to read the parameters from the node, and if that fails, will attempt to subscribe to the String topic for a short period of time. If both methods fail to get the parameter, then a warning will be printed to the console.

All the components of MoveIt that use the RobotModel need to have access to the URDF and SRDF to function properly. In ROS 1, this was accomplished by loading the XML of each into a string parameter (/robot_description and /robot_description_semantic respectively) into the global parameter server. ROS 2 does not have a global parameter server, so making sure all the appropriate nodes have access requires a little more work.

Source

In this case your node needs access to these parameters (due to instance of MoveGroupInterface being created) and when running your node you are not passing these parameters to the node.

A few alternatives exist:

You add the node to your launch file and pass the moveit_config dict. as a parameter to be able to read them.
If handling them separately you can publish the topics but that would also require you to tweak some parts of your launch.

    moveit_config = (MoveItConfigsBuilder("robot", package_name="robot_moveit_description")
                     .planning_pipelines(pipelines=["ompl"])
                     .robot_description(file_path="config/robot.urdf.xacro")
                     .robot_description_semantic(file_path="config/robot.srdf")
                     .trajectory_execution(file_path="config/moveit_controllers.yaml")
                     .robot_description_kinematics(file_path="config/kinematics.yaml")
                     .planning_scene_monitor(
        publish_robot_description_semantic=True
    )
        .to_moveit_configs()
    )

Notice you will publish the topic and can read with an echo to verify this. It is not being published by default as shown here.

Your robot_description is being published by your robot_state_publisher.

This also takes into account that now nodes such as RViz would be able to read it through topic so it is best to tweak your code to this:

     rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file_path],
        parameters=[
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
            warehouse_ros_config,
        ],
      )

This applies to all nodes that require access to this information, let it be through parameters or topics.

Also, a few things you may want to read1 read2 on how to launch alternatively your move_group node through MoveItConfigBuilder.

8 replies

Danilrivero Dec 5, 2024

First you have to pass the planning_pipelines which is correct.

.planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])

This error:

[move_group-1] [INFO] [1733323390.225113604] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'pilz_industrial_motion_planner'
[move_group-1] terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterTypeException'
[move_group-1] what(): parameter 'pilz_industrial_motion_planner.request_adapters' has invalid type: expected [string] got [string_array]

Seems to be related to this config. file:

https://github.com/moveit/moveit2/blob/main/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml

Verify that you are using the format either for Humble or Jazzy (aka main right now until #3116 makes a new dedicated branch) or whatever version you use.

Also verify all the other config. files while you are at it.

If you are switching between plans with OMPL and PILZ make sure you swiftly change between them using methods such as...

  move_group->setPlanningPipelineId("pilz_industrial_motion_planner");
  move_group->setPlannerId("LIN");

janperenic Dec 6, 2024
Author

Hi, thanks for your response. I discovered that the issue was with the pilz_industrial_motion_planner_planning.yaml file. When I removed it from the config folder, everything actually started running. However, I'm still getting warnings about tolerances, and I'm not exactly sure where to configure them. I tried modifying the parameters in rqt_gui, but it didn't seem to help, or perhaps I didn't find the correct configuration.

[move_group-1] [INFO] [1733472997.271969985] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-1] [INFO] [1733472997.271993389] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'pilz_industrial_motion_planner'
[move_group-1] [INFO] [1733472997.272011533] [moveit.pilz_industrial_motion_planner.trajectory_generator_ptp]: Initialized Point-to-Point Trajectory Generator.
[move_group-1] [INFO] [1733472997.272038935] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating PTP trajectory...
[move_group-1] [INFO] [1733472997.272482108] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
[move_group-1] [INFO] [1733472997.274072758] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-1] [INFO] [1733472997.274135667] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-1] [INFO] [1733472997.274153220] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1733472997.274167637] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1733472997.274209776] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 1.5
[move_group-1] [INFO] [1733472997.296283253] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-1] [INFO] [1733472997.296365849] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1733472997.296390826] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [1733472997.296471758] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to follow_joint_trajectory
[move_group-1] [INFO] [1733472997.336230833] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: follow_joint_trajectory started execution
[move_group-1] [INFO] [1733472997.336257804] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-1] [WARN] [1733472998.841422592] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'follow_joint_trajectory' failed with error unknown error: Final position was outside tolerance. Check robot safety-limits that could be inhibiting motion. [joint_1: 0.0000 deviation] [joint_2: 0.0000 deviation] [joint_3: 0.0000 deviation] [joint_6: 0.0000 deviation]
[move_group-1] [WARN] [1733472998.841505959] [moveit_ros.trajectory_execution_manager]: Controller handle follow_joint_trajectory reports status ABORTED
[move_group-1] [INFO] [1733472998.841528161] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-1] [INFO] [1733472998.841629251] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED

Danilrivero Dec 9, 2024

There could be several reasons (SW / HW) but I would start by lowering the velocities at pilz_cartesian_limit.yaml in order to match the vel/accel you require for your application while checking if your robot is capable of executing these plans. Keep in mind that having higher than expected velocities / accel could get into conflict with your joint_limits.yaml configuration which does not seem to be the case here.

janperenic Dec 9, 2024
Author

Thank you! You helped me a lot. I owe you a beer😁

gavanderhoorn Dec 9, 2024
Collaborator

@janperenic: I believe you are using MotoROS2? If so: please make sure you have at least version 0.1.3 installed. You may be running into what Yaskawa-Global/motoros2#241 attempted to fix.

If you are running 0.1.3, please open a support request over at yaskawa-global/motoros2.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

MoveIt

Error Launching MoveIt2 Script for Moving the Robot: SRDF Not Found #3133

{{title}}

Replies: 1 comment 8 replies

{{title}}

{{title}}

{{title}}

{{title}}

{{title}}

{{title}}

Select a reply

MoveIt

Error Launching MoveIt2 Script for Moving the Robot: SRDF Not Found #3133

janperenic Nov 26, 2024

Replies: 1 comment · 8 replies

Danilrivero Nov 29, 2024

Danilrivero Dec 5, 2024

janperenic Dec 6, 2024 Author

Danilrivero Dec 9, 2024

janperenic Dec 9, 2024 Author

gavanderhoorn Dec 9, 2024 Collaborator

janperenic
Nov 26, 2024

Replies: 1 comment 8 replies

Danilrivero
Nov 29, 2024

janperenic Dec 6, 2024
Author

janperenic Dec 9, 2024
Author

gavanderhoorn Dec 9, 2024
Collaborator