diff --git a/README.md b/README.md index 3c1cd0f..8bbfb2b 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,9 @@ Todo List: - [x] [Learning-based Controller](controllers/rl_quadruped_controller/) - [x] Fully understand the RL Workflow - [x] ROS2 Humble Gazebo Classic Support +- [x] **[2025-02-23]** Add Gazebo Playground +- [ ] OCS2 controller for Gazebo Simulation +- [ ] Refactor FSM and Unitree Guide Controller Video for Unitree Guide Controller: [![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/) @@ -37,7 +40,7 @@ Video for RL Quadruped Controller: ``` * Compile the package ```bash - colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input + colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input --symlink-install ``` ### 1.1 Mujoco Simulator diff --git a/descriptions/unitree/go2_description/README.md b/descriptions/unitree/go2_description/README.md index a3ed477..4bdb7c2 100644 --- a/descriptions/unitree/go2_description/README.md +++ b/descriptions/unitree/go2_description/README.md @@ -11,14 +11,14 @@ Tested environment: * Ubuntu 22.04 * ROS2 Humble -## Build +## 1. Build ```bash cd ~/ros2_ws colcon build --packages-up-to go2_description --symlink-install ``` -## Visualize the robot +## 2. Visualize the robot To visualize and check the configuration of the robot in rviz, simply launch: @@ -27,9 +27,9 @@ source ~/ros2_ws/install/setup.bash ros2 launch go2_description visualize.launch.py ``` -## Launch ROS2 Control +## 3. Launch ROS2 Control -### Mujoco Simulator +### 3.1 Mujoco Simulator * Unitree Guide Controller ```bash @@ -47,14 +47,14 @@ ros2 launch go2_description visualize.launch.py ros2 launch rl_quadruped_controller mujoco.launch.py ``` -### Gazebo Classic 11 (ROS2 Humble) +### 3.2 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) +### 3.3 Gazebo Harmonic (ROS2 Jazzy) * Unitree Guide Controller ```bash @@ -67,17 +67,3 @@ ros2 launch go2_description visualize.launch.py ros2 launch go2_description gazebo_rl_control.launch.py ``` -## When used for isaac gym or other similiar engine - -Collision parameters in urdf can be amended to better train the robot: - -Open "go2_description.urdf" in "./go2_description/urdf", -and amend the ` box size="0.213 0.0245 0.034" ` in links of "FL_thigh", "FR_thigh", "RL_thigh", "RR_thigh". - -For example, change previous values to ` box size="0.11 0.0245 0.034" ` means the length of the thigh is shortened from -0.213 to 0.11, which can avoid unnecessary collision between the thigh link and the calf link. - -The collision model before and after the above amendment are shown as "Normal_collision_model.png" and " -Amended_collision_model.png" respectively. - - diff --git a/descriptions/unitree/go2_description/package.xml b/descriptions/unitree/go2_description/package.xml index 6535ac4..d1fcbb6 100644 --- a/descriptions/unitree/go2_description/package.xml +++ b/descriptions/unitree/go2_description/package.xml @@ -15,6 +15,7 @@ joint_state_publisher robot_state_publisher imu_sensor_broadcaster + joint_state_publisher_gui ament_cmake diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index 757b7b6..efc7afc 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -1,188 +1,219 @@ + + + gz_ros2_control/GazeboSimSystem + - - - gz_ros2_control/GazeboSimSystem - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + + + + + + + + - - - - - - - - - - - - - + + + $(find go2_description)/config/gazebo.yaml + + + + - - - $(find go2_description)/config/gazebo.yaml - - - - + + 0.6 + 0.6 + 1 + - - 0.6 - 0.6 - 1 - + + + 1 + 500 + true + imu + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + - - - 1 - 500 - true - imu - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - - - - true - + + true + + + + + + 2.094 + + 1280 + 720 + + + 0.1 + 15 + + + gaussian + + 0.0 + 0.007 + + front_camera + camera/camera_info + + true + 15 + true + camera/image + + + diff --git a/descriptions/unitree/go2_description/xacro/robot.xacro b/descriptions/unitree/go2_description/xacro/robot.xacro index a2459d9..a118c18 100755 --- a/descriptions/unitree/go2_description/xacro/robot.xacro +++ b/descriptions/unitree/go2_description/xacro/robot.xacro @@ -121,33 +121,16 @@ - + diff --git a/libraries/gz_quadruped_playground/CMakeLists.txt b/libraries/gz_quadruped_playground/CMakeLists.txt new file mode 100644 index 0000000..af2272e --- /dev/null +++ b/libraries/gz_quadruped_playground/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.8) +project(gz_quadruped_playground) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY worlds launch config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() \ No newline at end of file diff --git a/libraries/gz_quadruped_playground/README.md b/libraries/gz_quadruped_playground/README.md new file mode 100644 index 0000000..93473a1 --- /dev/null +++ b/libraries/gz_quadruped_playground/README.md @@ -0,0 +1,20 @@ +# Gazebo Quadruped Playground + +This folder contains the gazebo worlds and sensor models used for quadruped robot simulation. + +Tested environment: + +* Ubuntu 24.04 【ROS2 Jazzy】 + +## Build + +```bash +cd ~/ros2_ws +colcon build --packages-up-to gz_quadruped_playground --symlink-install +``` + +## Launch Simulation +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch gz_quadruped_playground gazebo.launch.py +``` \ No newline at end of file diff --git a/libraries/gz_quadruped_playground/config/rviz.rviz b/libraries/gz_quadruped_playground/config/rviz.rviz new file mode 100644 index 0000000..ab52823 --- /dev/null +++ b/libraries/gz_quadruped_playground/config/rviz.rviz @@ -0,0 +1,275 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 216 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + trunk: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/image + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.9871627688407898 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.011728689074516296 + Y: 0.011509218253195286 + Z: -0.10348724573850632 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5053979158401489 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.8203961253166199 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 786 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000258fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000019d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001de000000b50000001600ffffff000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000048d0000005efc0100000002fb0000000800540069006d006501000000000000048d0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003310000025800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1165 + X: 1159 + Y: 219 diff --git a/libraries/gz_quadruped_playground/launch/gazebo.launch.py b/libraries/gz_quadruped_playground/launch/gazebo.launch.py new file mode 100644 index 0000000..596aa63 --- /dev/null +++ b/libraries/gz_quadruped_playground/launch/gazebo.launch.py @@ -0,0 +1,151 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +import xacro + + +def launch_setup(context, *args, **kwargs): + # Gazebo World + world = context.launch_configurations['world'] + default_sdf_path = os.path.join(get_package_share_directory('gz_quadruped_playground'), 'worlds', world + '.sdf') + print(default_sdf_path) + + # Init Height When spawn the model + init_height = context.launch_configurations['height'] + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', '-name', + 'robot', '-allow_renaming', 'true', '-z', init_height], + ) + + # Robot Description + pkg_description = context.launch_configurations['pkg_description'] + pkg_path = os.path.join(get_package_share_directory(pkg_description)) + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') + robot_description = xacro.process_file(xacro_file, mappings={ + 'GAZEBO': 'true', + 'EXTERNAL_SENSORS': 'true' + }).toxml() + 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 + } + ], + ) + + rviz_config_file = os.path.join(get_package_share_directory('gz_quadruped_playground'), "config", "rviz.rviz") + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz', + output='screen', + arguments=["-d", rviz_config_file] + ) + + # Controllers + controller = context.launch_configurations['world'] + if controller == 'ocs2': + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([FindPackageShare('gz_quadruped_playground'), + 'launch', + 'ocs2.launch.py'])]) + ) + else: + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([FindPackageShare('gz_quadruped_playground'), + 'launch', + 'unitree_guide.launch.py'])]) + ) + + return [ + rviz, + robot_state_publisher, + gz_spawn_entity, + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py'])]), + launch_arguments=[('gz_args', [' -r -v 4 ', default_sdf_path])]), + controller_launch + ] + + +def generate_launch_description(): + world = DeclareLaunchArgument( + 'world', + default_value='default', + description='The world to load' + ) + + 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' + ) + + controller = DeclareLaunchArgument( + 'controller', + default_value='unitree_guide', + description='The ROS2-Control Controllers' + ) + + gz_bridge_node = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", + "/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", + ], + output="screen", + parameters=[ + {'use_sim_time': True}, + ] + ) + + gz_image_bridge_node = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=[ + "/camera/image", + ], + output="screen", + parameters=[ + {'use_sim_time': True, + 'camera.image.compressed.jpeg_quality': 75}, + ], + ) + + return LaunchDescription([ + world, + pkg_description, + height, + controller, + gz_bridge_node, + gz_image_bridge_node, + OpaqueFunction(function=launch_setup), + ]) diff --git a/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py b/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py new file mode 100644 index 0000000..0f7f718 --- /dev/null +++ b/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py @@ -0,0 +1,54 @@ +import xacro +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch_ros.actions import Node +def generate_launch_description(): + joint_state_publisher = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", + "--controller-manager", "/controller_manager"], + parameters=[ + {'use_sim_time': True}, + ] + ) + + imu_sensor_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["imu_sensor_broadcaster", + "--controller-manager", "/controller_manager"], + parameters=[ + {'use_sim_time': True}, + ] + ) + + leg_pd_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["leg_pd_controller", + "--controller-manager", "/controller_manager"], + parameters=[ + {'use_sim_time': True}, + ] + ) + + unitree_guide_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"], + parameters=[ + {'use_sim_time': True}, + ] + ) + + return LaunchDescription([ + leg_pd_controller, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=leg_pd_controller, + on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], + ) + ), + ]) diff --git a/libraries/gz_quadruped_playground/package.xml b/libraries/gz_quadruped_playground/package.xml new file mode 100644 index 0000000..46f27b8 --- /dev/null +++ b/libraries/gz_quadruped_playground/package.xml @@ -0,0 +1,19 @@ + + + gz_quadruped_playground + 0.0.0 + The gazebo playground for quadruped robots + + biao + Apache-2.0 + + xacro + joint_state_publisher + robot_state_publisher + imu_sensor_broadcaster + + + ament_cmake + + + diff --git a/libraries/gz_quadruped_playground/worlds/default.sdf b/libraries/gz_quadruped_playground/worlds/default.sdf new file mode 100644 index 0000000..5f681d9 --- /dev/null +++ b/libraries/gz_quadruped_playground/worlds/default.sdf @@ -0,0 +1,132 @@ + + + + + + 0.001 + 1.0 + + + ogre2 + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + \ No newline at end of file