From 7719f7f6294a4afc52d6925e72d42e8a756d9336 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 3 Dec 2025 17:28:16 -0500 Subject: [PATCH 1/6] Consolidate the URDFs for PID and non-PID control --- test/config/mujoco_pid.yaml | 24 ++--- test/launch/test_robot.launch.py | 13 +++ test/launch/test_robot_pid.launch.py | 100 ------------------ test/test_resources/test_robot.urdf | 146 +++++++++++++++------------ 4 files changed, 105 insertions(+), 178 deletions(-) delete mode 100644 test/launch/test_robot_pid.launch.py diff --git a/test/config/mujoco_pid.yaml b/test/config/mujoco_pid.yaml index d321078..bffbac3 100644 --- a/test/config/mujoco_pid.yaml +++ b/test/config/mujoco_pid.yaml @@ -3,17 +3,17 @@ pid_gains: position: joint1: - p: 200.0 - i: 0.005 - d: 50.0 + p: 100.0 + i: 0.0 + d: 30.0 u_clamp_max: 100.0 u_clamp_min: -100.0 i_clamp_max: 10.0 i_clamp_min: -10.0 joint2: - p: 200.0 - i: 0.005 - d: 50.0 + p: 100.0 + i: 0.0 + d: 30.0 u_clamp_max: 100.0 u_clamp_min: -100.0 i_clamp_max: 10.0 @@ -21,17 +21,17 @@ pid_gains: velocity: joint1: - p: 100.0 - i: 0.001 - d: 5.0 + p: 30.0 + i: 0.1 + d: 1.0 u_clamp_max: 100.0 u_clamp_min: -100.0 i_clamp_max: 10.0 i_clamp_min: -10.0 joint2: - p: 100.0 - i: 0.001 - d: 5.0 + p: 30.0 + i: 0.1 + d: 1.0 u_clamp_max: 100.0 u_clamp_min: -100.0 i_clamp_max: 10.0 diff --git a/test/launch/test_robot.launch.py b/test/launch/test_robot.launch.py index cc30370..4078425 100644 --- a/test/launch/test_robot.launch.py +++ b/test/launch/test_robot.launch.py @@ -18,9 +18,11 @@ # under the License. from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument from launch.substitutions import ( Command, FindExecutable, + LaunchConfiguration, PathJoinSubstitution, ) from launch_ros.actions import Node @@ -29,6 +31,13 @@ def generate_launch_description(): + + use_pid = DeclareLaunchArgument( + 'use_pid', + default_value='false', + description='If we should use PID control' + ) + robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -40,6 +49,9 @@ def generate_launch_description(): "test_robot.urdf", ] ), + " ", + "use_pid:=", + LaunchConfiguration('use_pid'), ] ) @@ -91,6 +103,7 @@ def generate_launch_description(): return LaunchDescription( [ + use_pid, robot_state_publisher_node, control_node, spawn_joint_state_broadcaster, diff --git a/test/launch/test_robot_pid.launch.py b/test/launch/test_robot_pid.launch.py deleted file mode 100644 index 492b590..0000000 --- a/test/launch/test_robot_pid.launch.py +++ /dev/null @@ -1,100 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2025, PAL Robotics S.L. -# -# All rights reserved. -# -# This software is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -from launch import LaunchDescription -from launch.substitutions import ( - Command, - FindExecutable, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue, ParameterFile -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("mujoco_ros2_simulation"), - "test_resources", - "test_pid", - "test_robot_pid.urdf", - ] - ), - ] - ) - - robot_description = {"robot_description": ParameterValue(value=robot_description_content, value_type=str)} - - controller_parameters_pid = ParameterFile( - PathJoinSubstitution([FindPackageShare("mujoco_ros2_simulation"), "config", "controllers.yaml"]), - ) - - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[ - robot_description, - {"use_sim_time": True}, - ], - ) - - control_node = Node( - package="mujoco_ros2_simulation", - executable="ros2_control_node", - output="both", - parameters=[ - {"use_sim_time": True}, - controller_parameters_pid, - ], - emulate_tty=True, - ) - - spawn_joint_state_broadcaster = Node( - package="controller_manager", - executable="spawner", - name="spawn_joint_state_broadcaster", - arguments=[ - "joint_state_broadcaster", - ], - output="both", - ) - - spawn_position_controller = Node( - package="controller_manager", - executable="spawner", - name="spawn_position_controller", - arguments=[ - "position_controller", - ], - output="both", - ) - - return LaunchDescription( - [ - robot_state_publisher_node, - control_node, - spawn_joint_state_broadcaster, - spawn_position_controller, - ] - ) diff --git a/test/test_resources/test_robot.urdf b/test/test_resources/test_robot.urdf index ea1be8c..c994984 100644 --- a/test/test_resources/test_robot.urdf +++ b/test/test_resources/test_robot.urdf @@ -1,82 +1,82 @@ - + + + + + - - - + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - + + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - - + + + + + - - - + + + @@ -123,15 +123,24 @@ - - - + + + + mujoco_ros2_simulation/MujocoSystemInterface - $(find mujoco_ros2_simulation)/test_resources/scene.xml + + + + $(find mujoco_ros2_simulation)/test_resources/test_pid/scene_pid.xml + $(find mujoco_ros2_simulation)/config/mujoco_pid.yaml + + + $(find mujoco_ros2_simulation)/test_resources/scene.xml + @@ -143,10 +152,13 @@ 6.0 - 10.0 + 1.0 + + + 0.0 @@ -155,6 +167,9 @@ + + + 0.0 @@ -185,5 +200,4 @@ /scan - From 55ecd77d3be6685ca092f2ef6346033c2b2cd7b7 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 3 Dec 2025 17:29:51 -0500 Subject: [PATCH 2/6] Update README --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 1199ed1..a12f1f0 100644 --- a/README.md +++ b/README.md @@ -419,6 +419,9 @@ For now, built the drivers with testing enabled, then the test robot system can # Brings up the hardware drivers and mujoco interface, along with a single position controller ros2 launch mujoco_ros2_simulation test_robot.launch.py +# Or optionally include the PID controller as mentioned above +ros2 launch mujoco_ros2_simulation test_robot.launch.py use_pid:=true + # Launch an rviz2 window with the provided configuration rviz2 -d $(ros2 pkg prefix --share mujoco_ros2_simulation)/config/test_robot.rviz ``` From 211d8a54f5386f15bbefc0c4fe5644c0b0f6648c Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 3 Dec 2025 17:35:09 -0500 Subject: [PATCH 3/6] Remove the test PID --- .../test_pid/test_robot_pid.urdf | 192 ------------------ 1 file changed, 192 deletions(-) delete mode 100644 test/test_resources/test_pid/test_robot_pid.urdf diff --git a/test/test_resources/test_pid/test_robot_pid.urdf b/test/test_resources/test_pid/test_robot_pid.urdf deleted file mode 100644 index be78798..0000000 --- a/test/test_resources/test_pid/test_robot_pid.urdf +++ /dev/null @@ -1,192 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - mujoco_ros2_simulation/MujocoSystemInterface - $(find mujoco_ros2_simulation)/test_resources/test_pid/scene_pid.xml - - $(find mujoco_ros2_simulation)/config/mujoco_pid.yaml - - - 3.0 - - - 6.0 - - - 10.0 - - - - - - - 0.0 - - - - - - - - - - 0.0 - - - - - - - - - camera_color_mujoco_frame - /camera/color/camera_info - /camera/color/image_raw - /camera/aligned_depth_to_color/image_raw - - - - - - - - lidar_sensor_frame - 0.025 - -0.3 - 0.3 - 0.05 - 10 - /scan - - - - From 27065d3c47d574557b7e4d7703986e17fcca0695 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 3 Dec 2025 17:39:37 -0500 Subject: [PATCH 4/6] Pre-commit fixes --- test/launch/test_robot.launch.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/test/launch/test_robot.launch.py b/test/launch/test_robot.launch.py index 4078425..e23bbbc 100644 --- a/test/launch/test_robot.launch.py +++ b/test/launch/test_robot.launch.py @@ -32,11 +32,7 @@ def generate_launch_description(): - use_pid = DeclareLaunchArgument( - 'use_pid', - default_value='false', - description='If we should use PID control' - ) + use_pid = DeclareLaunchArgument("use_pid", default_value="false", description="If we should use PID control") robot_description_content = Command( [ @@ -51,7 +47,7 @@ def generate_launch_description(): ), " ", "use_pid:=", - LaunchConfiguration('use_pid'), + LaunchConfiguration("use_pid"), ] ) From c3ffd44992635f46791bbd64e759a85e0f8663eb Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Thu, 4 Dec 2025 09:41:55 -0500 Subject: [PATCH 5/6] Update test/launch/test_robot.launch.py Co-authored-by: Sai Kishor Kothakota --- test/launch/test_robot.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/launch/test_robot.launch.py b/test/launch/test_robot.launch.py index e23bbbc..b382aea 100644 --- a/test/launch/test_robot.launch.py +++ b/test/launch/test_robot.launch.py @@ -32,7 +32,8 @@ def generate_launch_description(): - use_pid = DeclareLaunchArgument("use_pid", default_value="false", description="If we should use PID control") + # Refer https://github.com/ros-controls/mujoco_ros2_simulation?tab=readme-ov-file#joints + use_pid = DeclareLaunchArgument("use_pid", default_value="false", description="If we should use PID control to enable other control modes") robot_description_content = Command( [ From 131c6dac97e85962871be7ac08e8c05e096775f3 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Thu, 4 Dec 2025 09:43:13 -0500 Subject: [PATCH 6/6] Format fixes --- test/launch/test_robot.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test/launch/test_robot.launch.py b/test/launch/test_robot.launch.py index b382aea..c6b7880 100644 --- a/test/launch/test_robot.launch.py +++ b/test/launch/test_robot.launch.py @@ -33,7 +33,9 @@ def generate_launch_description(): # Refer https://github.com/ros-controls/mujoco_ros2_simulation?tab=readme-ov-file#joints - use_pid = DeclareLaunchArgument("use_pid", default_value="false", description="If we should use PID control to enable other control modes") + use_pid = DeclareLaunchArgument( + "use_pid", default_value="false", description="If we should use PID control to enable other control modes" + ) robot_description_content = Command( [