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
```
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..c6b7880 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,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,
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_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
-
-
-
-
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
-