diff --git a/README.md b/README.md index 8bbfb2b..684ce63 100644 --- a/README.md +++ b/README.md @@ -9,15 +9,6 @@ This repository contains the ros2-control based controllers for the quadruped ro Todo List: -- [x] [Mujoco Simulation](hardwares/hardware_unitree_mujoco) -- [x] [Unitree Guide Controller](controllers/unitree_guide_controller) -- [x] [Gazebo Simulation](descriptions/quadruped_gazebo) -- [x] [Leg PD Controller](controllers/leg_pd_controller) -- [x] [Contact Sensor Simulation](https://github.com/legubiao/unitree_mujoco) -- [x] [OCS2 Quadruped Control](controllers/ocs2_quadruped_controller) -- [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 @@ -45,20 +36,26 @@ Video for RL Quadruped Controller: ### 1.1 Mujoco Simulator -Please use **C++ Simulation** in this [Mujoco Simulation](https://github.com/legubiao/unitree_mujoco) for more robot models and contact sensor. -* **Python Simulation** is also supported, but you still need to install [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2) +Please use **C++ Simulation** in this [Mujoco Simulation](https://github.com/legubiao/unitree_mujoco) for more robot +models and contact sensor. + +* **Python Simulation** is also supported, but you still need to + install [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2) > **Warning:** CycloneDDS ROS2 RMW may conflict with unitree_sdk2. If you cannot launch unitree mujoco simulation -> without `sudo`, then you cannot used `unitree_mujoco_hardware`. This conflict could be solved by one of below two methods: +> without `sudo`, then you cannot used `unitree_mujoco_hardware`. This conflict could be solved by one of below two +> methods: > 1. Uninstall CycloneDDS ROS2 RMW, or -> 2. Follow the guide in [unitree_ros2](https://github.com/unitreerobotics/unitree_ros2) to configure the ROS2 RMW by compiling cyclone dds. +> 2. Follow the guide in [unitree_ros2](https://github.com/unitreerobotics/unitree_ros2) to configure the ROS2 RMW by + compiling cyclone dds. * Compile Unitree Hardware Interfaces ```bash cd ~/ros2_ws colcon build --packages-up-to hardware_unitree_mujoco ``` -* Follow the guide in [unitree_mujoco](https://github.com/legubiao/unitree_mujoco) to launch the unitree mujoco go2 simulation +* Follow the guide in [unitree_mujoco](https://github.com/legubiao/unitree_mujoco) to launch the unitree mujoco go2 + simulation * Launch the ros2-control ```bash source ~/ros2_ws/install/setup.bash @@ -73,6 +70,7 @@ Please use **C++ Simulation** in this [Mujoco Simulation](https://github.com/leg ![mujoco](.images/mujoco.png) ### 1.2 Gazebo Classic Simulator (ROS2 Humble) + * Install Gazebo Classic ```bash sudo apt-get install ros-humble-gazebo-ros ros-humble-gazebo-ros2-control @@ -94,7 +92,6 @@ Please use **C++ Simulation** in this [Mujoco Simulation](https://github.com/leg ![gazebo classic](.images/gazebo_classic.png) - ### 1.3 Gazebo Harmonic Simulator (ROS2 Jazzy) * Install Gazebo @@ -102,25 +99,35 @@ Please use **C++ Simulation** in this [Mujoco Simulation](https://github.com/leg sudo apt-get install ros-jazzy-ros-gz ros-jazzy-gz-ros2-control ``` -* Compile Leg PD Controller - ```bash - colcon build --packages-up-to leg_pd_controller - ``` +* Compile Gazebo Playground + ```bash + colcon build --packages-up-to gz_quadruped_playground --symlink-install + ``` * Launch the ros2-control - ```bash - source ~/ros2_ws/install/setup.bash - ros2 launch unitree_guide_controller gazebo.launch.py - ``` + ```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 ``` + ![gazebo](.images/gazebo.png) For more details, please refer to the [unitree guide controller](controllers/unitree_guide_controller/) and [go2 description](descriptions/unitree/go2_description/). +## What's Next +Congratulations! You have successfully launched the quadruped robot in the simulation. Here are some suggestions for you to have a try: +* **More Robot Models** could be found at [description](descriptions/) +* **Try more controllers**. + * [OCS2 Quadruped Controller](controllers/ocs2_quadruped_controller): Robust MPC-based controller for quadruped robot + * [RL Quadruped Controller](controllers/rl_quadruped_controller): Reinforcement learning controller for quadruped robot +* **Simulate with more sensors** + * [Gazebo Quadruped Playground](libraries/gz_quadruped_playground): Provide gazebo simulation with lidar or depth camera. + ## Reference ### Conference Paper @@ -140,4 +147,5 @@ Available: [https://github.com/unitreerobotics/unitree_guide](https://github.com robots*. [Online]. Available: [https://github.com/qiayuanl/legged_control](https://github.com/qiayuanl/legged_control) [3] Ziqi Fan. *rl\_sar: Simulation Verification and Physical Deployment of Robot Reinforcement Learning Algorithm.* + 2024. Available: [https://github.com/fan-ziqi/rl_sar](https://github.com/fan-ziqi/rl_sar) \ No newline at end of file diff --git a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py index c3958b2..d870a28 100644 --- a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py +++ b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py @@ -89,20 +89,16 @@ def launch_setup(context, *args, **kwargs): 'launch', 'gz_sim.launch.py'])]), launch_arguments=[('gz_args', [' -r -v 4 ', default_sdf_path])]), - imu_sensor_broadcaster, RegisterEventHandler( event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[joint_state_publisher], + target_action=gz_spawn_entity, + on_exit=[joint_state_publisher,imu_sensor_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_publisher, - on_exit=[TimerAction( - period=10.0, # 延迟5秒启动 - actions=[ocs2_controller] - )], + on_exit=[ocs2_controller], ) ), ] diff --git a/controllers/unitree_guide_controller/launch/gazebo.launch.py b/controllers/unitree_guide_controller/launch/gazebo.launch.py index aac6fb9..0be90f8 100644 --- a/controllers/unitree_guide_controller/launch/gazebo.launch.py +++ b/controllers/unitree_guide_controller/launch/gazebo.launch.py @@ -65,13 +65,6 @@ 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", @@ -94,11 +87,16 @@ def launch_setup(context, *args, **kwargs): launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), robot_state_publisher, gz_spawn_entity, - leg_pd_controller, RegisterEventHandler( event_handler=OnProcessExit( - target_action=leg_pd_controller, - on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], + target_action=gz_spawn_entity, + on_exit=[imu_sensor_broadcaster, joint_state_publisher], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_publisher, + on_exit=[ unitree_guide_controller], ) ), ] diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index 24ecc3d..e408462 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -290,5 +290,35 @@ camera/image + + + scan + 10 + lidar + + + + 640 + 1 + 1.396263 + 4.8869 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.05 + 10.0 + 0.01 + + + 1 + true + + diff --git a/descriptions/unitree/go2_description/xacro/robot.xacro b/descriptions/unitree/go2_description/xacro/robot.xacro index a118c18..90ffa02 100755 --- a/descriptions/unitree/go2_description/xacro/robot.xacro +++ b/descriptions/unitree/go2_description/xacro/robot.xacro @@ -2,137 +2,148 @@ - - - + + + - - - + + + - - - - - - - - - - - + + + + + + + + + + + - + - - + + - - - - - - - - - + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - - + + + + + - - + + - - - - + + + + + + + + + + + + + + diff --git a/hardwares/gz_quadruped_hardware/src/gz_system.cpp b/hardwares/gz_quadruped_hardware/src/gz_system.cpp index 799d962..759a0fd 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_system.cpp +++ b/hardwares/gz_quadruped_hardware/src/gz_system.cpp @@ -572,12 +572,12 @@ namespace gz_quadruped_hardware return std::move(this->dataPtr->command_interfaces_); } - CallbackReturn GazeboSimSystem::on_activate(const rclcpp_lifecycle::State& previous_state) + CallbackReturn GazeboSimSystem::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { return CallbackReturn::SUCCESS; } - CallbackReturn GazeboSimSystem::on_deactivate(const rclcpp_lifecycle::State& previous_state) + CallbackReturn GazeboSimSystem::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { return CallbackReturn::SUCCESS; } diff --git a/hardwares/hardware_unitree_mujoco/package.xml b/hardwares/hardware_unitree_mujoco/package.xml index e3c1f75..b4c52cf 100644 --- a/hardwares/hardware_unitree_mujoco/package.xml +++ b/hardwares/hardware_unitree_mujoco/package.xml @@ -11,6 +11,7 @@ ament_lint_auto ament_lint_common + ros2_control ament_cmake diff --git a/libraries/gz_quadruped_playground/README.md b/libraries/gz_quadruped_playground/README.md index c0e712b..28ee699 100644 --- a/libraries/gz_quadruped_playground/README.md +++ b/libraries/gz_quadruped_playground/README.md @@ -16,15 +16,23 @@ colcon build --packages-up-to gz_quadruped_playground --symlink-install ## Launch Simulation * Unitree Guide Controller - ```bash - source ~/ros2_ws/install/setup.bash - ros2 launch gz_quadruped_playground gazebo.launch.py controller:=unitree_guide - ``` + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch gz_quadruped_playground gazebo.launch.py + ``` + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch gz_quadruped_playground gazebo.launch.py world:=warehouse + ``` * OCS2 Quadruped Controller - ```bash - source ~/ros2_ws/install/setup.bash - ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2 - ``` + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2 + ``` + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2 world:=warehouse + ``` ## Related Materials diff --git a/libraries/gz_quadruped_playground/config/rviz.rviz b/libraries/gz_quadruped_playground/config/rviz.rviz index cf6672b..69be719 100644 --- a/libraries/gz_quadruped_playground/config/rviz.rviz +++ b/libraries/gz_quadruped_playground/config/rviz.rviz @@ -8,8 +8,9 @@ Panels: - /Status1 - /Grid1 - /RobotModel1 + - /PointCloud21 Splitter Ratio: 0.5 - Tree Height: 216 + Tree Height: 138 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -27,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -161,6 +162,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + lidar: + Alpha: 1 + Show Axes: false + Show Trail: false trunk: Alpha: 1 Show Axes: false @@ -188,6 +193,39 @@ Visualization Manager: Reliability Policy: Reliable Value: /camera/image Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan/points + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 0; 0; 0 @@ -234,35 +272,35 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.8712024688720703 + Distance: 1.5249122381210327 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.004370270296931267 - Y: 0.06990259140729904 - Z: -0.18453647196292877 + X: 1.770981788635254 + Y: -0.6720151305198669 + Z: 2.1505401134490967 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.310397744178772 + Pitch: 0.42039743065834045 Target Frame: Value: Orbit (rviz) - Yaw: 0.8653964400291443 + Yaw: 3.25040864944458 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 786 + Height: 1431 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000258fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000019d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001de000000b50000001600ffffff000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000048d0000005efc0100000002fb0000000800540069006d006501000000000000048d0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003310000025800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f40000048dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000001860000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000202000002fb0000002800ffffff000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000068e0000005efc0100000002fb0000000800540069006d006501000000000000068e0000047a00fffffffb0000000800540069006d006501000000000000045000000000000000000000048e0000048d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -271,6 +309,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1165 - X: 1283 - Y: 199 + Width: 1678 + X: 2254 + Y: 268 diff --git a/libraries/gz_quadruped_playground/launch/gazebo.launch.py b/libraries/gz_quadruped_playground/launch/gazebo.launch.py index 0972f64..485cab9 100644 --- a/libraries/gz_quadruped_playground/launch/gazebo.launch.py +++ b/libraries/gz_quadruped_playground/launch/gazebo.launch.py @@ -120,6 +120,8 @@ def generate_launch_description(): arguments=[ "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", "/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", + "/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan", + "/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked", # "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry", # "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance", # "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V" diff --git a/libraries/gz_quadruped_playground/worlds/warehouse.sdf b/libraries/gz_quadruped_playground/worlds/warehouse.sdf new file mode 100644 index 0000000..9e2b403 --- /dev/null +++ b/libraries/gz_quadruped_playground/worlds/warehouse.sdf @@ -0,0 +1,401 @@ + + + + + 0.001 + 1.0 + + + ogre2 + + + + + + + + + + 1 + + + + + 0 0 1 + 1 1 + + + + + + + + + + + + 0 0 0 0 0 0 + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Depot + + 7.19009 1.09982 0 0 0 0 + + + + 1 + 7.19009 1.09982 0 0 0 0 + + 0 0 0 0 0 0 + + 0 -7.6129 4.5 0 0 0 + + + 30.167 0.08 9 + + + + + 0 7.2875 4.5 0 0 0 + + + 30.167 0.08 9 + + + + + -15 0 4.5 0 0 0 + + + 0.08 15.360 9 + + + + + 15 0 4.5 0 0 0 + + + 0.08 15.360 9 + + + + + 0.22268 -4.7268 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 3.1727 -4.7268 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 5.95268 -4.7268 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 8.55887 -4.7268 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 11.326 -4.7268 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 0.22268 -2.37448 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 3.1727 -2.37448 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 5.95268 -2.37448 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 8.55887 -2.37448 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + 11.326 -2.37448 0.68506 0 0 0 + + + 1.288 1.422 1.288 + + + + + -1.2268 4.1557 0.68506 0 0 -1.02799893 + + + 1.288 1.422 1.288 + + + + + -7.5402 3.6151 1 0 0 0 + + + 0.465 0.465 2 + + + + + 7.4575 3.6151 1 0 0 0 + + + 0.465 0.465 2 + + + + + -7.5402 -3.8857 1 0 0 0 + + + 0.465 0.465 2 + + + + + 7.4575 -3.8857 1 0 0 0 + + + 0.465 0.465 2 + + + + + -0.6144 -2.389 0.41838 0 0 0 + + + 0.363 0.440 0.719 + + + + + -1.6004 4.8225 0.41838 0 0 0 + + + 0.363 0.244 0.719 + + + + + 13.018 3.1652 0.25 0 0 0 + + + 1.299 0.6 0.5 + + + + + 1.4662 -0.017559 0.5 0 0 0 + + + 0.03 + 1 + + + + + 2.6483 -0.017559 0.5 0 0 0 + + + 0.03 + 1 + + + + + 5.3247 -0.017559 0.5 0 0 0 + + + 0.03 + 1 + + + + + 6.5063 -0.017559 0.5 0 0 0 + + + 0.03 + 1 + + + + + 9.0758 -0.017559 0.5 0 0 0 + + + 0.03 + 1 + + + + + 10.258 -0.017559 0.5 0 0 0 + + + 0.03 + 1 + + + + + 1.4662 2.5664 0.5 0 0 0 + + + 0.03 + 1 + + + + + 2.6483 2.5664 0.5 0 0 0 + + + 0.03 + 1 + + + + + 5.3247 2.5664 0.5 0 0 0 + + + 0.03 + 1 + + + + + 6.5063 2.5664 0.5 0 0 0 + + + 0.03 + 1 + + + + + 9.0758 2.5664 0.5 0 0 0 + + + 0.03 + 1 + + + + + 10.258 2.5664 0.5 0 0 0 + + + 0.03 + 1 + + + + + 1.4662 5.1497 0.5 0 0 0 + + + 0.03 + 1 + + + + + 2.6483 5.1497 0.5 0 0 0 + + + 0.03 + 1 + + + + + 5.3247 5.1497 0.5 0 0 0 + + + 0.03 + 1 + + + + + 6.5063 5.1497 0.5 0 0 0 + + + 0.03 + 1 + + + + + 9.0758 5.1497 0.5 0 0 0 + + + 0.03 + 1 + + + + + 10.258 5.1497 0.5 0 0 0 + + + 0.03 + 1 + + + + + + +