Consolidate test URDF and launch files

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
 ```

test/config/mujoco_pid.yaml

@@ -3,35 +3,35 @@
     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
           i_clamp_min: -10.0
     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

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,12 @@
 
 
 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"
+    )
+
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -40,6 +48,9 @@ def generate_launch_description():
                     "test_robot.urdf",
                 ]
             ),
+            " ",
+            "use_pid:=",
+            LaunchConfiguration("use_pid"),
         ]
     )
 
@@ -91,6 +102,7 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
+            use_pid,
             robot_state_publisher_node,
             control_node,
             spawn_joint_state_broadcaster,