try to add gazebo classic simulation for rl

README.md

@@ -15,7 +15,7 @@ Todo List:
 - [x] [OCS2 Quadruped Control](controllers/ocs2_quadruped_controller)
 - [x] [Learning-based Controller](controllers/rl_quadruped_controller/)
 - [ ] Fully understand the RL Workflow
-- [ ] ROS2 Humble Gazebo Classic Support
+- [x] ROS2 Humble Gazebo Classic Support
 
 Video for Unitree Guide Controller:
 [![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/)

controllers/rl_quadruped_controller/README.md

@@ -11,13 +11,14 @@ Tested environment:
 ### 2.1 Installing libtorch
 ```bash
 cd ~/CLionProjects/
-wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.0.1%2Bcpu.zip
-unzip libtorch-cxx11-abi-shared-with-deps-2.0.1+cpu.zip -d ./
+wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.5.0%2Bcpu.zip
+unzip unzip libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip
 ```
 ```bash
 cd ~/CLionProjects/
-rm -rf libtorch-shared-with-deps-latest.zip
+rm -rf libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip
 echo 'export Torch_DIR=~/CLionProjects/libtorch' >> ~/.bashrc
+echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/CLionProjects/libtorch/lib' >> ~/.bashrc
 ```
 
 ### 2.2 Build Controller
@@ -27,14 +28,16 @@ colcon build --packages-up-to rl_quadruped_controller
 ```
 
 ## 3. Launch
-* Unitree A1 Robot
-  ```bash
-  source ~/ros2_ws/install/setup.bash
-  ros2 launch a1_description rl_control.launch.py
-  ```
 
-* Unitree Go2 Robot
-  ```bash
-  source ~/ros2_ws/install/setup.bash
-  ros2 launch go2_description rl_control.launch.py
-  ```
+### 3.1 Mujoco Simulation
+
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=go2_description
+```
+
+### 3.3 Gazebo Harmonic (ROS2 Jazzy)
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch rl_quadruped_controller gazebo.launch.py pkg_description:=go2_description
+```

controllers/rl_quadruped_controller/launch/gazebo_classic.launch.py

@@ -0,0 +1,119 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def launch_setup(context, *args, **kwargs):
+
+    package_description = context.launch_configurations['pkg_description']
+    init_height = context.launch_configurations['height']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true', 'CLASSIC': 'true'}).toxml()
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz_ocs2',
+        output='screen',
+        arguments=["-d", rviz_config_file]
+    )
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [PathJoinSubstitution([FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"])]
+        ),
+        launch_arguments={"verbose": "false"}.items(),
+    )
+
+    spawn_entity = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=["-topic", "robot_description", "-entity", "robot", "-z", init_height],
+        output="screen",
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    leg_pd_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["leg_pd_controller",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"]
+    )
+
+    return [
+        rviz,
+        robot_state_publisher,
+        gazebo,
+        spawn_entity,
+        leg_pd_controller,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=leg_pd_controller,
+                on_exit=[imu_sensor_broadcaster, joint_state_publisher, controller],
+            )
+        ),
+    ]
+
+
+def generate_launch_description():
+    pkg_description = DeclareLaunchArgument(
+        'pkg_description',
+        default_value='go2_description',
+        description='package for robot description'
+    )
+
+    height = DeclareLaunchArgument(
+        'height',
+        default_value='0.5',
+        description='Init height in simulation'
+    )
+
+    return LaunchDescription([
+        pkg_description,
+        height,
+        OpaqueFunction(function=launch_setup),
+    ])

descriptions/unitree/a1_description/README.md

@@ -50,9 +50,9 @@ ros2 launch a1_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=a1_description
+  ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=a1_description height:=0.43
   ```
-
+  
 ### Gazebo Harmonic (ROS2 Jazzy)
 
 * Unitree Guide Controller

descriptions/unitree/go2_description/config/robot_control.yaml

@@ -186,8 +186,8 @@ rl_quadruped_controller:
     foot_force_name: "foot_force"
     foot_force_interfaces:
       - FL
-      - RL
       - FR
+      - RL
       - RR
 
     imu_name: "imu_sensor"