Setting up simulation in Gazebo with cartesian_motion_controller
Closed this issue · 5 comments
Problem description
First of all, thank you for this package.
I'm working on a Universal Robot UR10e. I've been able to teleop the robot using the cartesian motion controller and it works flawlessly (from what I can see in RViZ). I'd like now to have a reproduction of what happens in RViz (moving the end-effector in space) in Gazebo, so that I can simulate some tasks (like object pick-and-place). I've tried implementing this, starting from the launch file provided by Universal Robots and the robot shows up correctly in Gazebo (and the joint trajectory controller loads fine). However, when I swap the joint trajectory controller with your cartesian motion controller, the latter goes into "unconfigured" state and I am not able to use it. In the terminal I can find this error:
[gazebo-3] [INFO] [1701122711.743714910] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gazebo-3] [INFO] [1701122711.789006800] [controller_manager]: Loading controller 'cartesian_motion_controller'
[gazebo-3] [INFO] [1701122711.800404981] [controller_manager]: Setting use_sim_time=True for cartesian_motion_controller to match controller manager (see ros2_control#325 for details)
[spawner-5] [INFO] [1701122711.804800088] [spawner_cartesian_motion_controller]: Loaded cartesian_motion_controller
[gazebo-3] [INFO] [1701122711.805252525] [controller_manager]: Configuring controller 'cartesian_motion_controller'
[gazebo-3] [ERROR] [1701122711.809993187] [cartesian_motion_controller]: robot_description is empty
[gazebo-3] [WARN] [1701122711.810205463] [cartesian_motion_controller]: Error occurred while doing error handling.
[gazebo-3] [ERROR] [1701122711.810232888] [controller_manager]: After configuring, controller 'cartesian_motion_controller' is in state 'unconfigured' , expected inactive.
[spawner-5] [ERROR] [1701122711.810626414] [spawner_cartesian_motion_controller]: Failed to configure controller
The error robot_description is empty in particular is what caught my attention: I think the controller isn´t properly getting the URDF description of the robot from the robot_description topic. I verified the topic was correctly echoing the robot description, and it did, but I haven't been able to fix this problem. I don't have this problem with the "default" joint trajectory controller.
I could be wrong, but I thought this had to do with the fact that while for the RViZ teleop I actually passed the robot_description to the ur_ros2_control_node (I've used your starting UR script to do so), here I didn't do so, because what is starting the controller is the gazebo_ros2_control inside the URDF from Universal Robots (which I pass the cartesian_motion_controller to, as a parameter):
<xacro:if value="$(arg sim_gazebo)">
<!-- Gazebo plugins -->
<gazebo reference="world">
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(arg simulation_controllers)</parameters>
</plugin>
</gazebo>
</xacro:if>
This is my launch file (simplified version omitting, among the others, parameters definitions):
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]
),
" ",
"safety_limits:=",
safety_limits,
" ",
"name:=",
"ur",
" ",
"ur_type:=",
ur_type,
" ",
"prefix:=",
prefix,
" ",
"sim_gazebo:=true",
" ",
"simulation_controllers:=",
initial_joint_controllers
]
)
robot_description = {"robot_description": robot_description_content}
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[{"use_sim_time": True}, robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
condition=IfCondition(launch_rviz),
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
# Workaround to launch Gazebo, due to issue: https://github.com/ros-simulation/gazebo_ros_pkgs/issues/1050
gazebo = ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'],
output='screen'
)
gazebo_spawn_robot = Node(
package='gazebo_ros',
executable='spawn_entity.py',
name='spawn_ur',
output='screen',
arguments=["-entity", "ur", "-topic", "robot_description"]
)
cartesian_motion_controller_spawner = Node(
package="controller_manager",
executable="spawner",
output='screen',
arguments=["cartesian_motion_controller", "--controller-manager", "/controller_manager"],
)
nodes_to_start = [
robot_state_publisher_node,
joint_state_broadcaster_spawner,
gazebo,
gazebo_spawn_robot,
cartesian_motion_controller_spawner,
]
return nodes_to_start
Software versions
- Commit: 9682c7199c8f48e8793c6fa03cb43324d4c81dee
- OS: Ubuntu 22.04
- ROS version: ROS2 Humble
- Robot: Universal Robot UR10e
Expected behavior
Loading cartesian motion controller and be able to use it to control the arm inside Gazebo, as I do in RViZ.
Thank you.
Update 29/11
Looking at the source code of the controller, I think I understood the problem. The controller expects the robot_description as a parameter: in fact, it seems that it's not reading it from the /robot_description topic. It seems this is going deprecated, and I'm looking for a way to pass it as a param or another way to launch it using the gazebo ros2 control library.
Update 05/12
I've been able to move the robot in Gazebo Ignition. To do so, I've added a subscription (local transient) in the controller code to the robot_description topic (so that it gets fetched from robot_state_publisher node) and made the following remapping under the plugin in the URDF file:
<ros><remapping>/cartesian_motion_controller/target_frame:=/target_frame</remapping></ros>
I don't know if this approach is optimal, but it's working. If it is correct, I'll be happy to share the code.
Thanks @fornaroale
It's a parameter at the moment, yes. I'll have to check what the most recent (recommended) policy is for fetching the robot_description inside ROS2 controllers..
I got same problem when simulating in gazebo . After following your sollution, it is still not working. Can you share your code with me? @fornaroale Thank you
I got same problem when simulating in gazebo . After following your sollution, it is still not working. Can you share your code with me? @fornaroale Thank you
It is on my Github, here. Please be aware that it could be not the best solution/the right way to make it work, especially in terms of best practices.
I got same problem when simulating in gazebo . After following your sollution, it is still not working. Can you share your code with me? @fornaroale Thank you
It is on my Github, here. Please be aware that it could be not the best solution/the right way to make it work, especially in terms of best practices.
thanks for your reply. can i ask one more question: how can you get data feedback from gazebo to the controller?