UniversalRobots / Universal_Robots_ROS2_Gazebo_Simulation

BSD 3-Clause "New" or "Revised" License
63 stars 27 forks source link

Having trouble getting the scaled_joint_trajectory_controller to work with ur_sim_moveit.launch.py #14

Closed haavarpb closed 9 months ago

haavarpb commented 2 years ago

Hi,

after finally being able to run the gazebo simulation (needed to update ur_msgs dependency like in #10, update the locale LC_NUMERIC which was related to MoveIt2) I am having difficulties with moving the gazebo ur robot.

Starting the driver using ros2 launch ur_simulation_gazebo ur_sim_moveit.launch.py starts the system with the following controllers loaded:

haavard@ThinkPad-X1:~/ros2_galactic_ws$ ros2 control list_controllers
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active    
joint_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active 

If I now move the ur robot using the Rviz with MoveIt, and hit Plan everything looks fine:

[move_group-7] [INFO] [1654174394.525417745] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-7] [INFO] [1654174394.525607760] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-7] [INFO] [1654174394.525716098] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-7] [INFO] [1654174394.525734318] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'move_group'
[move_group-7] [INFO] [1654174394.527467288] [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-7] [INFO] [1654174394.544790690] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.

However, when I click Execute, this happens:

[move_group-7] [INFO] [1654174487.326601726] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-7] [INFO] [1654174487.326863641] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-7] [INFO] [1654174487.326926137] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-7] [INFO] [1654174487.326946210] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-7] [INFO] [1654174487.327084820] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-7] [INFO] [1654174487.327174635] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-7] [INFO] [1654174487.327229781] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-7] [INFO] [1654174487.327245519] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-7] [ERROR] [1654174487.327394428] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected to action server: scaled_joint_trajectory_controller/follow_joint_trajectory
[move_group-7] [ERROR] [1654174487.327397340] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller scaled_joint_trajectory_controller
[move_group-7] [INFO] [1654174487.327404627] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-7] [INFO] [1654174487.327496313] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED

Ok, so it looks like MoveIt wants to talk to the scaled_joint_trajectory_controller(SJTC), while it's actually joint_trajectory_controller (JTC) that's loaded by ros2 control. Fair enough, after switching the default controller in ur_moveit_config/config/controllers.yaml from the SJTC to JTC, I am able to move the gazebo robot!

Now, as an exercise, I looked into if I could manage to use the SJTC instead. First thing I tried is to unload JTC, and then load-configure-start the SJTC through ros2 control. This results in the following messages:

[gzserver-4] [INFO] [1654175946.435835764] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller'
[gzserver-4] [INFO] [1654175946.453069320] [controller_manager]: Configuring controller 'scaled_joint_trajectory_controller'
[gzserver-4] [INFO] [1654175946.453128926] [scaled_joint_trajectory_controller]: Command interfaces are [position] and and state interfaces are [position velocity].
[gzserver-4] [INFO] [1654175946.453418972] [scaled_joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[gzserver-4] [INFO] [1654175946.453716297] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[gzserver-4] [ERROR] [1654175971.846511008] [controller_manager]: Can't activate controller 'scaled_joint_trajectory_controller': State interface with key 'speed_scaling/speed_scaling_factor' does not exist

Based on these messages, the ros control architecture and the explanation of the different ur_controllers here I decided to try and start the speed_scaling_state_broadcaster instead of the joint_state_broadcaster hoping that these are equivalent besides that the former also publishes the required speed_scaling/speed_scaling_factor. Hope is not enough, as it didn't work...

I am at my limit of my ROS2 knowledge here, but I will continue to try. However, I am dropping this here to see if anyone knows how I can configure the UR robot to use the SJTC.

PS: I feel like using the ros2 control interface is the wrong thing to do here, as moveit adds a node called /moveit_simple_controller_manager which seems to invoke ros2 control, however I have not found a way to tell it to switch controllers. Looking into the moveit repo, it seems that there might be an implementation of a MoveItControllerManager that could help, but I am unable to see how I would approach incorporating it. It seems that this thread is relevant as well.

Thanks for any help, and sorry if this is inappropriate.

fmauch commented 2 years ago

Thank you for your report and your excellent debugging skills. Indeed, the scaled interface is not available for the gazebo simulation as this is currently more or less a ur-specific feature. We hope to change this in the future, but we don't have any particular time frame ATM.

padhupradheep commented 2 years ago

To circumvent this issue, it would be nice to use the use_fake_hardware when we include the launch for moveit in ur_sim_moveit.launch.py

ur_moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [FindPackageShare("ur_moveit_config"), "/launch", "/ur_moveit.launch.py"]
        ),
        launch_arguments={
            "ur_type": ur_type,
            "safety_limits": safety_limits,
            "description_package": description_package,
            "description_file": description_file,
            "moveit_config_package": moveit_config_package,
            "moveit_config_file": moveit_config_file,
            "prefix": prefix,
            "use_sim_time": "true",
            "use_fake_hardware": "True",
            "launch_rviz": "true",
        }.items(),
    )

Hope this helps! I'll try to give in a PR, if the maintainers are interested.

fmauch commented 2 years ago

@padhupradheep thanks for suggesting this. This will probably see some rework in the near future, since we've found the reason for https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/390. Hence, the automatic switch of the default controller will at least be renamed rather soon. When this is done, it is indeed a good idea to change the launchfile in here.

padhupradheep commented 2 years ago

No problem.

        "use_fake_hardware": "True",

Another interesting thing here is that, you definitely need to have capital T for "True". Else, there will be an error, when you use them with the Context. It could be interesting for other params as well during the rework.

gachiemchiep commented 1 year ago

I am using humble, the ur_sim_moveit.launch.py file has the following check

    change_controllers = context.perform_substitution(use_fake_hardware)
    if change_controllers == "true":
        controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
        controllers_yaml["joint_trajectory_controller"]["default"] = True

so I think for now "use_fake_hardware": "True", need to be "use_fake_hardware": "true",

fmauch commented 9 months ago

This seems to boil down to a duplicate of #8, so closing this.