add support for gazebo classic simulator

README.md

@@ -23,7 +23,7 @@ Video for OCS2 Quadruped Controller:
 [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/)
 
 
-## Quick Start
+## 1. Quick Start
 * rosdep
     ```bash
     cd ~/ros2_ws
@@ -33,17 +33,52 @@ rosdep install --from-paths src --ignore-src -r -y
     ```bash
     colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input hardware_unitree_mujoco
     ```
+
+### 1.1 Mujoco Simulator
 * Launch the unitree mujoco go2 simulation
 * Launch the ros2-control
     ```bash
     source ~/ros2_ws/install/setup.bash
-ros2 launch go2_description unitree_guide.launch.py
+    ros2 launch unitree_guide_controller mujoco.launch.py
     ```
 * Run the keyboard control node
     ```bash
     source ~/ros2_ws/install/setup.bash
     ros2 run keyboard_input keyboard_input
     ```
+
+### 1.2 Gazebo Classic Simulator (ROS2 Humble)
+* Compile Leg PD Controller
+    ```bash
+    colcon build --packages-up-to leg_pd_controller
+    ```
+* Launch the ros2-control
+    ```bash
+    source ~/ros2_ws/install/setup.bash
+    ros2 launch unitree_guide_controller gazebo.launch.py
+    ```
+* Run the keyboard control node
+    ```bash
+    source ~/ros2_ws/install/setup.bash
+    ros2 run keyboard_input keyboard_input
+    ```
+
+### 1.3 Gazebo Harmonic Simulator (ROS2 Jazzy)
+* Compile Leg PD Controller
+    ```bash
+    colcon build --packages-up-to leg_pd_controller
+    ```
+* Launch the ros2-control
+    ```bash
+    source ~/ros2_ws/install/setup.bash
+    ros2 launch unitree_guide_controller gazebo_classic.launch.py
+    ```
+* Run the keyboard control node
+    ```bash
+    source ~/ros2_ws/install/setup.bash
+    ros2 run keyboard_input keyboard_input
+    ```
+
 For more details, please refer to the [unitree guide controller](controllers/unitree_guide_controller/) and [go2 description](descriptions/unitree/go2_description/).
 
 ## Reference

controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h

@@ -5,10 +5,13 @@
 #ifndef LEGPDCONTROLLER_H
 #define LEGPDCONTROLLER_H
 #include <controller_interface/chainable_controller_interface.hpp>
+#include "realtime_tools/realtime_buffer.h"
 #include <controller_interface/controller_interface.hpp>
+#include "std_msgs/msg/float64_multi_array.hpp"
 
 
 namespace leg_pd_controller {
+    using DataType = std_msgs::msg::Float64MultiArray;
     class LegPdController final : public controller_interface::ChainableControllerInterface {
     public:
         controller_interface::CallbackReturn on_init() override;
@@ -34,8 +37,10 @@ namespace leg_pd_controller {
     protected:
         std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
 
-        controller_interface::return_type update_reference_from_subscribers(
+        // controller_interface::return_type update_reference_from_subscribers(
-            const rclcpp::Time &time, const rclcpp::Duration &period) override;
+        //     const rclcpp::Time &time, const rclcpp::Duration &period);
+
+        controller_interface::return_type update_reference_from_subscribers() override;
 
         std::vector<double> joint_effort_command_;
         std::vector<double> joint_position_command_;
@@ -47,6 +52,7 @@ namespace leg_pd_controller {
 
         std::vector<std::string> state_interface_types_;
         std::vector<std::string> reference_interface_types_;
+        realtime_tools::RealtimeBuffer<std::shared_ptr<DataType>> rt_buffer_ptr_;
 
         std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
         joint_effort_command_interface_;

controllers/leg_pd_controller/src/LegPdController.cpp

@@ -4,17 +4,22 @@
 
 #include "leg_pd_controller/LegPdController.h"
 
-namespace leg_pd_controller {
+namespace leg_pd_controller
+{
     using config_type = controller_interface::interface_configuration_type;
 
-    controller_interface::CallbackReturn LegPdController::on_init() {
+    controller_interface::CallbackReturn LegPdController::on_init()
-        try {
+    {
+        try
+        {
             joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
             reference_interface_types_ =
                 auto_declare<std::vector<std::string>>("reference_interfaces", reference_interface_types_);
             state_interface_types_ = auto_declare<std::vector<
                 std::string>>("state_interfaces", state_interface_types_);
-        } catch (const std::exception &e) {
+        }
+        catch (const std::exception& e)
+        {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
         }
@@ -29,22 +34,27 @@ namespace leg_pd_controller {
         return CallbackReturn::SUCCESS;
     }
 
-    controller_interface::InterfaceConfiguration LegPdController::command_interface_configuration() const {
+    controller_interface::InterfaceConfiguration LegPdController::command_interface_configuration() const
+    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size());
-        for (const auto &joint_name: joint_names_) {
+        for (const auto& joint_name : joint_names_)
+        {
             conf.names.push_back(joint_name + "/effort");
         }
 
         return conf;
     }
 
-    controller_interface::InterfaceConfiguration LegPdController::state_interface_configuration() const {
+    controller_interface::InterfaceConfiguration LegPdController::state_interface_configuration() const
+    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
         conf.names.reserve(joint_names_.size() * state_interface_types_.size());
-        for (const auto &joint_name: joint_names_) {
+        for (const auto& joint_name : joint_names_)
-            for (const auto &interface_type: state_interface_types_) {
+        {
+            for (const auto& interface_type : state_interface_types_)
+            {
                 conf.names.push_back(joint_name + "/" += interface_type);
             }
         }
@@ -52,24 +62,28 @@ namespace leg_pd_controller {
     }
 
     controller_interface::CallbackReturn LegPdController::on_configure(
-        const rclcpp_lifecycle::State & /*previous_state*/) {
+        const rclcpp_lifecycle::State& /*previous_state*/)
+    {
+        reference_interfaces_.resize(joint_names_.size()*5, std::numeric_limits<double>::quiet_NaN());
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn LegPdController::on_activate(
-        const rclcpp_lifecycle::State & /*previous_state*/) {
+        const rclcpp_lifecycle::State& /*previous_state*/)
-
+    {
         joint_effort_command_interface_.clear();
         joint_position_state_interface_.clear();
         joint_velocity_state_interface_.clear();
 
         // assign effort command interface
-        for (auto &interface: command_interfaces_) {
+        for (auto& interface : command_interfaces_)
+        {
             joint_effort_command_interface_.emplace_back(interface);
         }
 
         // assign state interfaces
-        for (auto &interface: state_interfaces_) {
+        for (auto& interface : state_interfaces_)
+        {
             state_interface_map_[interface.get_interface_name()]->push_back(interface);
         }
 
@@ -77,36 +91,43 @@ namespace leg_pd_controller {
     }
 
     controller_interface::CallbackReturn LegPdController::on_deactivate(
-        const rclcpp_lifecycle::State & /*previous_state*/) {
+        const rclcpp_lifecycle::State& /*previous_state*/)
+    {
         release_interfaces();
         return CallbackReturn::SUCCESS;
     }
 
-    bool LegPdController::on_set_chained_mode(bool /*chained_mode*/) {
+    bool LegPdController::on_set_chained_mode(bool /*chained_mode*/)
+    {
         return true;
     }
 
     controller_interface::return_type LegPdController::update_and_write_commands(
-        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+        const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+    {
         if (joint_names_.size() != joint_effort_command_.size() ||
             joint_names_.size() != joint_kp_command_.size() ||
             joint_names_.size() != joint_position_command_.size() ||
             joint_names_.size() != joint_position_state_interface_.size() ||
             joint_names_.size() != joint_velocity_state_interface_.size() ||
-            joint_names_.size() != joint_effort_command_interface_.size()) {
+            joint_names_.size() != joint_effort_command_interface_.size())
-
+        {
             std::cout << "joint_names_.size() = " << joint_names_.size() << std::endl;
             std::cout << "joint_effort_command_.size() = " << joint_effort_command_.size() << std::endl;
             std::cout << "joint_kp_command_.size() = " << joint_kp_command_.size() << std::endl;
             std::cout << "joint_position_command_.size() = " << joint_position_command_.size() << std::endl;
-            std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() << std::endl;
+            std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() <<
-            std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() << std::endl;
+                std::endl;
-            std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() << std::endl;
+            std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() <<
+                std::endl;
+            std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() <<
+                std::endl;
 
             throw std::runtime_error("Mismatch in vector sizes in update_and_write_commands");
         }
 
-        for (size_t i = 0; i < joint_names_.size(); ++i) {
+        for (size_t i = 0; i < joint_names_.size(); ++i)
+        {
             // PD Controller
             const double torque = joint_effort_command_[i] + joint_kp_command_[i] * (
                     joint_position_command_[i] - joint_position_state_interface_[i].get().get_value())
@@ -120,12 +141,15 @@ namespace leg_pd_controller {
         return controller_interface::return_type::OK;
     }
 
-    std::vector<hardware_interface::CommandInterface> LegPdController::on_export_reference_interfaces() {
+    std::vector<hardware_interface::CommandInterface> LegPdController::on_export_reference_interfaces()
+    {
         std::vector<hardware_interface::CommandInterface> reference_interfaces;
 
         int ind = 0;
         std::string controller_name = get_node()->get_name();
-        for (const auto &joint_name: joint_names_) {
+        for (const auto& joint_name : joint_names_)
+        {
+            std::cout << joint_name << std::endl;
             reference_interfaces.emplace_back(controller_name, joint_name + "/position", &joint_position_command_[ind]);
             reference_interfaces.emplace_back(controller_name, joint_name + "/velocity",
                                               &joint_velocities_command_[ind]);
@@ -138,8 +162,13 @@ namespace leg_pd_controller {
         return reference_interfaces;
     }
 
-    controller_interface::return_type LegPdController::update_reference_from_subscribers(
+    // controller_interface::return_type LegPdController::update_reference_from_subscribers(
-        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+    //     const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+    //     return controller_interface::return_type::OK;
+    // }
+
+    controller_interface::return_type LegPdController::update_reference_from_subscribers()
+    {
         return controller_interface::return_type::OK;
     }
 }

controllers/unitree_guide_controller/CMakeLists.txt

@@ -77,6 +77,11 @@ install(
         RUNTIME DESTINATION bin
 )
 
+install(
+        DIRECTORY launch
+        DESTINATION share/${PROJECT_NAME}/
+)
+
 ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS})
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 

controllers/unitree_guide_controller/launch/gazebo.launch.py

@@ -8,35 +8,36 @@ 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
 
-package_description = "x30_description"
+package_description = "go2_description"
 
 
-def process_xacro():
+
+def launch_setup(context, *args, **kwargs):
+
+    package_description = context.launch_configurations['pkg_description']
     pkg_path = os.path.join(get_package_share_directory(package_description))
+
     xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
+    robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'}).toxml()
-    return robot_description_config.toxml()
 
-
-def generate_launch_description():
     rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
 
-    robot_description = process_xacro()
+    rviz = Node(
-
+        package='rviz2',
-    robot_controllers = PathJoinSubstitution(
+        executable='rviz2',
-        [
+        name='rviz_ocs2',
-            FindPackageShare(package_description),
+        output='screen',
-            "config",
+        arguments=["-d", rviz_config_file]
-            "robot_control.yaml",
-        ]
     )
 
-    controller_manager = Node(
+    gz_spawn_entity = Node(
-        package="controller_manager",
+        package='ros_gz_sim',
-        executable="ros2_control_node",
+        executable='create',
-        parameters=[robot_controllers],
+        output='screen',
-        output="both",
+        arguments=['-topic', 'robot_description', '-name',
+                   'robot', '-allow_renaming', 'true', '-z', '0.5'],
     )
 
     robot_state_publisher = Node(
@@ -67,27 +68,54 @@ def generate_launch_description():
                    "--controller-manager", "/controller_manager"],
     )
 
+    leg_pd_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["leg_pd_controller",
+                   "--controller-manager", "/controller_manager"],
+    )
+
     unitree_guide_controller = Node(
         package="controller_manager",
         executable="spawner",
         arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
     )
 
-    return LaunchDescription([
+    return [
-        Node(
+        rviz,
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
         robot_state_publisher,
-        controller_manager,
+        Node(
-        joint_state_publisher,
+            package='ros_gz_bridge',
+            executable='parameter_bridge',
+            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+            output='screen'
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
+                                       'launch',
+                                       'gz_sim.launch.py'])]),
+            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
+        robot_state_publisher,
+        gz_spawn_entity,
+        leg_pd_controller,
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=joint_state_publisher,
+                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, unitree_guide_controller],
+                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
             )
         ),
+    ]
+
+
+def generate_launch_description():
+    pkg_description = DeclareLaunchArgument(
+        'pkg_description',
+        default_value='go2_description',
+        description='package for robot description'
+    )
+
+    return LaunchDescription([
+        pkg_description,
+        OpaqueFunction(function=launch_setup),
     ])

controllers/unitree_guide_controller/launch/gazebo_classic.launch.py

@@ -8,26 +8,42 @@ 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
 
-package_description = "aliengo_description"
+package_description = "go2_description"
 
 
-def process_xacro(context):
-    robot_type_value = context.launch_configurations['robot_type']
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
-    return (robot_description_config.toxml(), robot_type_value)
-
 
 def launch_setup(context, *args, **kwargs):
-    (robot_description, robot_type) = process_xacro(context)
+
-    robot_controllers = PathJoinSubstitution(
+    package_description = context.launch_configurations['pkg_description']
-        [
+    pkg_path = os.path.join(get_package_share_directory(package_description))
-            FindPackageShare(package_description),
+
-            "config",
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-            "robot_control.yaml",
+    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", "0.5"],
+        output="screen",
     )
 
     robot_state_publisher = Node(
@@ -44,13 +60,6 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
-    controller_manager = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[robot_controllers],
-        output="both",
-    )
-
     joint_state_publisher = Node(
         package="controller_manager",
         executable="spawner",
@@ -65,6 +74,13 @@ def launch_setup(context, *args, **kwargs):
                    "--controller-manager", "/controller_manager"],
     )
 
+    leg_pd_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["leg_pd_controller",
+                   "--controller-manager", "/controller_manager"],
+    )
+
     unitree_guide_controller = Node(
         package="controller_manager",
         executable="spawner",
@@ -72,41 +88,28 @@ def launch_setup(context, *args, **kwargs):
     )
 
     return [
+        rviz,
         robot_state_publisher,
-        controller_manager,
+        gazebo,
-        joint_state_publisher,
+        spawn_entity,
+        leg_pd_controller,
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=joint_state_publisher,
+                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster],
+                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=imu_sensor_broadcaster,
-                on_exit=[unitree_guide_controller],
             )
         ),
     ]
 
 
 def generate_launch_description():
-    robot_type_arg = DeclareLaunchArgument(
+    pkg_description = DeclareLaunchArgument(
-        'robot_type',
+        'pkg_description',
-        default_value='aliengo',
+        default_value='go2_description',
-        description='Type of the robot'
+        description='package for robot description'
     )
 
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
     return LaunchDescription([
-        robot_type_arg,
+        pkg_description,
         OpaqueFunction(function=launch_setup),
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        )
     ])

controllers/unitree_guide_controller/launch/mujoco.launch.py

@@ -9,19 +9,18 @@ from launch.substitutions import PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
-package_description = "a1_description"
+package_description = "go2_description"
 
 
-def process_xacro(context):
-    robot_type_value = context.launch_configurations['robot_type']
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
-    return (robot_description_config.toxml(), robot_type_value)
-
 
 def launch_setup(context, *args, **kwargs):
-    (robot_description, robot_type) = process_xacro(context)
+
+    package_description = context.launch_configurations['pkg_description']
+    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).toxml()
+
     robot_controllers = PathJoinSubstitution(
         [
             FindPackageShare(package_description),
@@ -30,6 +29,16 @@ def launch_setup(context, *args, **kwargs):
         ]
     )
 
+    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]
+    )
+
     robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
@@ -48,6 +57,9 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_controllers],
+        remappings=[
+            ("~/robot_description", "/robot_description"),
+        ],
         output="both",
     )
 
@@ -72,6 +84,7 @@ def launch_setup(context, *args, **kwargs):
     )
 
     return [
+        rviz,
         robot_state_publisher,
         controller_manager,
         joint_state_publisher,
@@ -91,22 +104,13 @@ def launch_setup(context, *args, **kwargs):
 
 
 def generate_launch_description():
-    robot_type_arg = DeclareLaunchArgument(
+    pkg_description = DeclareLaunchArgument(
-        'robot_type',
+        'pkg_description',
-        default_value='a1',
+        default_value='go2_description',
-        description='Type of the robot'
+        description='package for robot description'
     )
 
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
     return LaunchDescription([
-        robot_type_arg,
+        pkg_description,
         OpaqueFunction(function=launch_setup),
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        )
     ])

descriptions/README.md

@@ -14,7 +14,7 @@ This folder contains the URDF and SRDF files for the quadruped robot.
     * [Lite 3](deep_robotics/lite3_description/)
     * [X30](deep_robotics/x30_description/)
 
-## Steps to transfer urdf to Mujoco model
+## 1. Steps to transfer urdf to Mujoco model
 
 * Install [Mujoco](https://github.com/google-deepmind/mujoco)
 * Transfer the mesh files to mujoco supported format, like stl.
@@ -29,9 +29,9 @@ This folder contains the URDF and SRDF files for the quadruped robot.
   compile robot.urdf robot.xml
   ```
 
-## Dependencies for Gazebo Simulation
+## 2. Dependencies for Gazebo Simulation
 
-Gazebo Simulation only tested on ROS2 Jazzy. I didn't add support for ROS2 Humble because the package name is different.
+Gazebo Simulation only tested on ROS2 Jazzy. It add support for ROS2 Humble because the package name is different.
 
 * Gazebo Harmonic
   ```bash
@@ -46,3 +46,21 @@ Gazebo Simulation only tested on ROS2 Jazzy. I didn't add support for ROS2 Humbl
     cd ~/ros2_ws
     colcon build --packages-up-to leg_pd_controller
     ```
+
+## 2. Dependencies for Gazebo Classic Simulation
+
+Gazebo Classic (Gazebo11) Simulation only tested on ROS2 Humble.
+
+* Gazebo Classic
+  ```bash
+  sudo apt-get install ros-humble-gazebo-ros
+  ```
+* Ros2-Control for Gazebo
+  ```bash
+  sudo apt-get install ros-humble-gazebo-ros2-control
+  ``` 
+* Legged PD Controller
+    ```bash
+    cd ~/ros2_ws
+    colcon build --packages-up-to leg_pd_controller
+    ```

descriptions/deep_robotics/lite3_description/README.md

@@ -25,7 +25,7 @@ ros2 launch lite3_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch lite3_description unitree_guide.launch.py
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=lite3_description
   ```
 * OCS2 Quadruped Controller
   ```bash
@@ -43,7 +43,7 @@ ros2 launch lite3_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch lite3_description gazebo_unitree_guide.launch.py
+  ros2 launch lite3_description gazebo.launch.py
   ```
 * Legged Gym Controller
   ```bash

descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py

@@ -1,93 +0,0 @@
-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
-
-package_description = "lite3_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare(package_description),
-            "config",
-            "robot_control.yaml",
-        ]
-    )
-
-    controller_manager = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[robot_controllers],
-        output="both",
-    )
-
-    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"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        robot_state_publisher,
-        controller_manager,
-        joint_state_publisher,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=joint_state_publisher,
-                on_exit=[imu_sensor_broadcaster, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/deep_robotics/x30_description/README.md

@@ -21,12 +21,18 @@ ros2 launch x30_description visualize.launch.py
 ```
 
 ## Launch ROS2 Control
+### Mujoco Simulator
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=x30_description
+  ```
 
 ### Gazebo Simulator
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch x30_description gazebo_unitree_guide.launch.py
+  ros2 launch x30_description gazebo.launch.py
   ```
 * Legged Gym Controller
   ```bash

descriptions/unitree/a1_description/README.md

@@ -25,7 +25,7 @@ ros2 launch a1_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch a1_description unitree_guide.launch.py
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=a1_description
   ```
 * OCS2 Quadruped Controller
   ```bash
@@ -38,11 +38,18 @@ ros2 launch a1_description visualize.launch.py
   ros2 launch a1_description rl_control.launch.py
   ```
 
-### Gazebo Simulator
+### Gazebo Classic 11 (ROS2 Humble)
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch a1_description gazebo_unitree_guide.launch.py
+  ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=a1_description
+  ```
+
+### Gazebo Harmonic (ROS2 Jazzy)
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch a1_description gazebo.launch.py
   ```
 * RL Quadruped Controller
   ```bash

descriptions/unitree/a1_description/config/gazebo.yaml

@@ -26,6 +26,7 @@ imu_sensor_broadcaster:
 
 leg_pd_controller:
   ros__parameters:
+    update_rate: 500  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint
@@ -50,6 +51,7 @@ leg_pd_controller:
 unitree_guide_controller:
   ros__parameters:
     command_prefix: "leg_pd_controller"
+    update_rate: 500  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint

descriptions/unitree/a1_description/xacro/gazebo_classic.xacro

@@ -0,0 +1,139 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+
+    <joint name="FR_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+      <state_interface name="angular_velocity.x"/>
+      <state_interface name="angular_velocity.y"/>
+      <state_interface name="angular_velocity.z"/>
+      <state_interface name="linear_acceleration.x"/>
+      <state_interface name="linear_acceleration.y"/>
+      <state_interface name="linear_acceleration.z"/>
+    </sensor>
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <parameters>$(find go2_description)/config/gazebo.yaml</parameters>
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="trunk">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+
+  <gazebo reference="imu_link">
+      <gravity>true</gravity>
+      <sensor name="imu_sensor" type="imu">
+          <always_on>true</always_on>
+          <update_rate>1000</update_rate>
+          <visualize>true</visualize>
+          <topic>__default_topic__</topic>
+          <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+              <topicName>trunk_imu</topicName>
+              <bodyName>imu_link</bodyName>
+              <updateRateHZ>1000.0</updateRateHZ>
+              <gaussianNoise>0.0</gaussianNoise>
+              <xyzOffset>0 0 0</xyzOffset>
+              <rpyOffset>0 0 0</rpyOffset>
+              <frameName>imu_link</frameName>
+          </plugin>
+          <pose>0 0 0 0 0 0</pose>
+      </sensor>
+  </gazebo>
+
+</robot>

descriptions/unitree/a1_description/xacro/robot.xacro

@@ -4,6 +4,7 @@
 
     <xacro:arg name="DEBUG" default="false"/>
     <xacro:arg name="GAZEBO" default="false"/>
+    <xacro:arg name="CLASSIC" default="false"/>
 
     <xacro:include filename="$(find a1_description)/xacro/const.xacro"/>
     <xacro:include filename="$(find a1_description)/xacro/materials.xacro"/>
@@ -11,7 +12,12 @@
     <xacro:include filename="$(find a1_description)/xacro/stairs.xacro"/>
 
     <xacro:if value="$(arg GAZEBO)">
-        <xacro:include filename="$(find a1_description)/xacro/gazebo.xacro"/>
+        <xacro:if value="$(arg CLASSIC)">
+            <xacro:include filename="$(find go2_description)/xacro/gazebo_classic.xacro"/>
+        </xacro:if>
+         <xacro:unless value="$(arg CLASSIC)">
+              <xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
+          </xacro:unless>
     </xacro:if>
     <xacro:unless value="$(arg GAZEBO)">
         <xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>

descriptions/unitree/aliengo_description/README.md

@@ -32,7 +32,7 @@ ros2 launch aliengo_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch aliengo_description unitree_guide.launch.py
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=aliengo_description
   ```
 * OCS2 Quadruped Controller
   ```bash
@@ -45,5 +45,5 @@ ros2 launch aliengo_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch aliengo_description gazebo_unitree_guide.launch.py
+  ros2 launch aliengo_description gazebo.launch.py
   ```

descriptions/unitree/b2_description/README.md

@@ -27,7 +27,7 @@ ros2 launch b2_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch b2_description unitree_guide.launch.py
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=b2_description
   ```
 * OCS2 Quadruped Controller
   ```bash
@@ -40,5 +40,5 @@ ros2 launch b2_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch b2_description gazebo_unitree_guide.launch.py
+  ros2 launch b2_description gazebo.launch.py
   ```

descriptions/unitree/b2_description/launch/unitree_guide.launch.py

@@ -1,93 +0,0 @@
-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
-
-package_description = "b2_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare(package_description),
-            "config",
-            "robot_control.yaml",
-        ]
-    )
-
-    controller_manager = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[robot_controllers],
-        output="both",
-    )
-
-    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"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        robot_state_publisher,
-        controller_manager,
-        joint_state_publisher,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=joint_state_publisher,
-                on_exit=[imu_sensor_broadcaster, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/unitree/go1_description/README.md

@@ -27,7 +27,7 @@ ros2 launch go1_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch go1_description unitree_guide.launch.py
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=go1_description
   ```
 * OCS2 Quadruped Controller
   ```bash
@@ -40,5 +40,5 @@ ros2 launch go1_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch go1_description gazebo_unitree_guide.launch.py
+  ros2 launch go1_description gazebo.launch.py
   ```

descriptions/unitree/go1_description/launch/unitree_guide.launch.py

@@ -1,93 +0,0 @@
-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
-
-package_description = "go1_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare(package_description),
-            "config",
-            "robot_control.yaml",
-        ]
-    )
-
-    controller_manager = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[robot_controllers],
-        output="both",
-    )
-
-    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"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        robot_state_publisher,
-        controller_manager,
-        joint_state_publisher,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=joint_state_publisher,
-                on_exit=[imu_sensor_broadcaster, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/unitree/go2_description/README.md

@@ -34,7 +34,7 @@ ros2 launch go2_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch go2_description unitree_guide.launch.py
+  ros2 launch unitree_guide_controller mujoco.launch.py
   ```
 * OCS2 Quadruped Controller
   ```bash
@@ -47,12 +47,19 @@ ros2 launch go2_description visualize.launch.py
   ros2 launch go2_description rl_control.launch.py
   ```
 
-### Gazebo Simulator
+### Gazebo Classic 11 (ROS2 Humble)
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch unitree_guide_controller gazebo_classic.launch.py
+  ```
+
+### Gazebo Harmonic (ROS2 Jazzy)
 
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch go2_description gazebo_unitree_guide.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py
   ```
 * RL Quadruped Controller
   ```bash

descriptions/unitree/go2_description/config/gazebo.yaml

@@ -26,6 +26,7 @@ imu_sensor_broadcaster:
 
 leg_pd_controller:
   ros__parameters:
+    update_rate: 500  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint
@@ -49,6 +50,7 @@ leg_pd_controller:
 
 unitree_guide_controller:
   ros__parameters:
+    update_rate: 500  # Hz
     command_prefix: "leg_pd_controller"
     joints:
       - FR_hip_joint

descriptions/unitree/go2_description/launch/gazebo_classic.launch.py

@@ -8,38 +8,42 @@ 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
 
 package_description = "go2_description"
 
 
-def process_xacro():
+
+def launch_setup(context, *args, **kwargs):
+
+    package_description = context.launch_configurations['pkg_description']
     pkg_path = os.path.join(get_package_share_directory(package_description))
+
     xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
+    robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true', 'CLASSIC': 'true'}).toxml()
-    return robot_description_config.toxml()
 
-
-def generate_launch_description():
     rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
 
-    robot_description = process_xacro()
+    rviz = Node(
-
+        package='rviz2',
-    robot_controllers = PathJoinSubstitution(
+        executable='rviz2',
-        [
+        name='rviz_ocs2',
-            FindPackageShare(package_description),
+        output='screen',
-            "config",
+        arguments=["-d", rviz_config_file]
-            "robot_control.yaml",
-        ]
     )
 
-    controller_manager = Node(
+    gazebo = IncludeLaunchDescription(
-        package="controller_manager",
+        PythonLaunchDescriptionSource(
-        executable="ros2_control_node",
+            [PathJoinSubstitution([FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"])]
-        parameters=[robot_controllers],
+        ),
-        output="both",
+        launch_arguments={"verbose": "false"}.items(),
-        remappings=[
+    )
-            ("~/robot_description", "/robot_description"),
+
-        ],
+    spawn_entity = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=["-topic", "robot_description", "-entity", "robot", "-z", "0.5"],
+        output="screen",
     )
 
     robot_state_publisher = Node(
@@ -70,27 +74,42 @@ def generate_launch_description():
                    "--controller-manager", "/controller_manager"],
     )
 
+    leg_pd_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["leg_pd_controller",
+                   "--controller-manager", "/controller_manager"],
+    )
+
     unitree_guide_controller = Node(
         package="controller_manager",
         executable="spawner",
         arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
     )
 
-    return LaunchDescription([
+    return [
-        Node(
+        rviz,
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
         robot_state_publisher,
-        controller_manager,
+        gazebo,
-        joint_state_publisher,
+        spawn_entity,
+        leg_pd_controller,
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=joint_state_publisher,
+                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, unitree_guide_controller],
+                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
             )
         ),
+    ]
+
+
+def generate_launch_description():
+    pkg_description = DeclareLaunchArgument(
+        'pkg_description',
+        default_value='go2_description',
+        description='package for robot description'
+    )
+
+    return LaunchDescription([
+        pkg_description,
+        OpaqueFunction(function=launch_setup),
     ])

descriptions/unitree/go2_description/xacro/gazebo_classic.xacro

@@ -0,0 +1,139 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+
+    <joint name="FR_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_hip_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_thigh_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_calf_joint">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+      <state_interface name="angular_velocity.x"/>
+      <state_interface name="angular_velocity.y"/>
+      <state_interface name="angular_velocity.z"/>
+      <state_interface name="linear_acceleration.x"/>
+      <state_interface name="linear_acceleration.y"/>
+      <state_interface name="linear_acceleration.z"/>
+    </sensor>
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <parameters>$(find go2_description)/config/gazebo.yaml</parameters>
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="trunk">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+
+  <gazebo reference="imu_link">
+      <gravity>true</gravity>
+      <sensor name="imu_sensor" type="imu">
+          <always_on>true</always_on>
+          <update_rate>1000</update_rate>
+          <visualize>true</visualize>
+          <topic>__default_topic__</topic>
+          <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+              <topicName>trunk_imu</topicName>
+              <bodyName>imu_link</bodyName>
+              <updateRateHZ>1000.0</updateRateHZ>
+              <gaussianNoise>0.0</gaussianNoise>
+              <xyzOffset>0 0 0</xyzOffset>
+              <rpyOffset>0 0 0</rpyOffset>
+              <frameName>imu_link</frameName>
+          </plugin>
+          <pose>0 0 0 0 0 0</pose>
+      </sensor>
+  </gazebo>
+
+</robot>

descriptions/unitree/go2_description/xacro/robot.xacro

@@ -4,13 +4,19 @@
 
     <xacro:arg name="DEBUG" default="false"/>
     <xacro:arg name="GAZEBO" default="false"/>
+    <xacro:arg name="CLASSIC" default="false"/>
 
     <xacro:include filename="$(find go2_description)/xacro/const.xacro"/>
     <xacro:include filename="$(find go2_description)/xacro/materials.xacro"/>
     <xacro:include filename="$(find go2_description)/xacro/leg.xacro"/>
 
     <xacro:if value="$(arg GAZEBO)">
+        <xacro:if value="$(arg CLASSIC)">
+            <xacro:include filename="$(find go2_description)/xacro/gazebo_classic.xacro"/>
+        </xacro:if>
+         <xacro:unless value="$(arg CLASSIC)">
               <xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
+          </xacro:unless>
     </xacro:if>
     <xacro:unless value="$(arg GAZEBO)">
         <xacro:include filename="$(find go2_description)/xacro/ros2_control.xacro"/>

descriptions/xiaomi/cyberdog_description/README.md

@@ -28,7 +28,7 @@ ros2 launch cyberdog_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch cyberdog_description unitree_guide.launch.py
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=cyberdog_description
   ```
 * OCS2 Quadruped Controller
   ```bash
@@ -41,5 +41,5 @@ ros2 launch cyberdog_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch cyberdog_description gazebo_unitree_guide.launch.py
+  ros2 launch cyberdog_description gazebo.launch.py
   ```

descriptions/xiaomi/cyberdog_description/launch/unitree_guide.launch.py

@@ -1,93 +0,0 @@
-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
-
-package_description = "cyberdog_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare(package_description),
-            "config",
-            "robot_control.yaml",
-        ]
-    )
-
-    controller_manager = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[robot_controllers],
-        output="both",
-    )
-
-    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"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        robot_state_publisher,
-        controller_manager,
-        joint_state_publisher,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=joint_state_publisher,
-                on_exit=[imu_sensor_broadcaster, unitree_guide_controller],
-            )
-        ),
-    ])