diff --git a/README.md b/README.md index 65af517..e3ee431 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,8 @@ Todo List: - [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) -- [ ] Learning-based Controller +- [x] [Learning-based Controller](controllers/rl_quadruped_controller/) +- [ ] Fully understand the RL Workflow Video for Unitree Guide Controller: [![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/) @@ -55,3 +56,4 @@ For more details, please refer to the [unitree guide controller](controllers/uni [2] Qiayuan Liao. *legged\_control: An open-source NMPC, WBC, state estimation, and sim2real framework for legged 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/legged_gym_controller/CMakeLists.txt b/controllers/rl_quadruped_controller/CMakeLists.txt similarity index 95% rename from controllers/legged_gym_controller/CMakeLists.txt rename to controllers/rl_quadruped_controller/CMakeLists.txt index 38b4d67..c071149 100644 --- a/controllers/legged_gym_controller/CMakeLists.txt +++ b/controllers/rl_quadruped_controller/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.18 FATAL_ERROR) set(CMAKE_CXX_STANDARD 17) -project(legged_gym_controller) +project(rl_quadruped_controller) if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -58,7 +58,7 @@ ament_target_dependencies( ${dependencies} ) -pluginlib_export_plugin_description_file(controller_interface legged_gym_controller.xml) +pluginlib_export_plugin_description_file(controller_interface rl_quadruped_controller.xml) install( DIRECTORY include/ diff --git a/controllers/legged_gym_controller/README.md b/controllers/rl_quadruped_controller/README.md similarity index 85% rename from controllers/legged_gym_controller/README.md rename to controllers/rl_quadruped_controller/README.md index d8501b5..3a7f317 100644 --- a/controllers/legged_gym_controller/README.md +++ b/controllers/rl_quadruped_controller/README.md @@ -1,4 +1,4 @@ -# Legged Gym Controller +# RL Quadruped Controller This repository contains the reinforcement learning based controllers for the quadruped robot. @@ -20,10 +20,10 @@ rm -rf libtorch-shared-with-deps-latest.zip echo 'export Torch_DIR=~/CLionProjects/libtorch' >> ~/.bashrc ``` -### 2.2 Build Legged Gym Controller +### 2.2 Build Controller ```bash cd ~/ros2_ws -colcon build --packages-up-to legged_gym_controller +colcon build --packages-up-to rl_quadruped_controller ``` ## 3. Launch diff --git a/controllers/legged_gym_controller/include/legged_gym_controller/FSM/FSMState.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h similarity index 86% rename from controllers/legged_gym_controller/include/legged_gym_controller/FSM/FSMState.h rename to controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h index cfed755..32b780d 100644 --- a/controllers/legged_gym_controller/include/legged_gym_controller/FSM/FSMState.h +++ b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h @@ -9,8 +9,8 @@ #include #include -#include "legged_gym_controller/common/enumClass.h" -#include "legged_gym_controller/control/CtrlComponent.h" +#include "rl_quadruped_controller/common/enumClass.h" +#include "rl_quadruped_controller/control/CtrlComponent.h" class FSMState { public: diff --git a/controllers/legged_gym_controller/include/legged_gym_controller/FSM/StateFixedDown.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h similarity index 100% rename from controllers/legged_gym_controller/include/legged_gym_controller/FSM/StateFixedDown.h rename to controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h diff --git a/controllers/legged_gym_controller/include/legged_gym_controller/FSM/StateFixedStand.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h similarity index 100% rename from controllers/legged_gym_controller/include/legged_gym_controller/FSM/StateFixedStand.h rename to controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h diff --git a/controllers/legged_gym_controller/include/legged_gym_controller/FSM/StatePassive.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StatePassive.h similarity index 100% rename from controllers/legged_gym_controller/include/legged_gym_controller/FSM/StatePassive.h rename to controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StatePassive.h diff --git a/controllers/legged_gym_controller/include/legged_gym_controller/FSM/StateRL.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h similarity index 100% rename from controllers/legged_gym_controller/include/legged_gym_controller/FSM/StateRL.h rename to controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h diff --git a/controllers/legged_gym_controller/include/legged_gym_controller/common/enumClass.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/enumClass.h similarity index 100% rename from controllers/legged_gym_controller/include/legged_gym_controller/common/enumClass.h rename to controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/enumClass.h diff --git a/controllers/legged_gym_controller/include/legged_gym_controller/control/CtrlComponent.h b/controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h similarity index 100% rename from controllers/legged_gym_controller/include/legged_gym_controller/control/CtrlComponent.h rename to controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h diff --git a/controllers/legged_gym_controller/package.xml b/controllers/rl_quadruped_controller/package.xml similarity index 74% rename from controllers/legged_gym_controller/package.xml rename to controllers/rl_quadruped_controller/package.xml index ca4e9ac..a0f0bcd 100644 --- a/controllers/legged_gym_controller/package.xml +++ b/controllers/rl_quadruped_controller/package.xml @@ -1,10 +1,10 @@ - legged_gym_controller + rl_quadruped_controller 0.0.0 - TODO: Package description - biao + A Quadruped robot controller relied on .pt RL network + Huang Zhenbiao Apache-2.0 ament_cmake diff --git a/controllers/legged_gym_controller/legged_gym_controller.xml b/controllers/rl_quadruped_controller/rl_quadruped_controller.xml similarity index 55% rename from controllers/legged_gym_controller/legged_gym_controller.xml rename to controllers/rl_quadruped_controller/rl_quadruped_controller.xml index 9c6aafb..f238a6b 100644 --- a/controllers/legged_gym_controller/legged_gym_controller.xml +++ b/controllers/rl_quadruped_controller/rl_quadruped_controller.xml @@ -1,6 +1,6 @@ - - + Quadruped Controller used RL-model trained in Legged Gym. diff --git a/controllers/legged_gym_controller/src/FSM/StateFixedDown.cpp b/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp similarity index 96% rename from controllers/legged_gym_controller/src/FSM/StateFixedDown.cpp rename to controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp index 95ff36f..a7637dd 100644 --- a/controllers/legged_gym_controller/src/FSM/StateFixedDown.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp @@ -2,7 +2,7 @@ // Created by tlab-uav on 24-9-11. // -#include "legged_gym_controller/FSM/StateFixedDown.h" +#include "rl_quadruped_controller/FSM/StateFixedDown.h" #include diff --git a/controllers/legged_gym_controller/src/FSM/StateFixedStand.cpp b/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp similarity index 96% rename from controllers/legged_gym_controller/src/FSM/StateFixedStand.cpp rename to controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp index 77f24fa..102c0c1 100644 --- a/controllers/legged_gym_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp @@ -2,7 +2,7 @@ // Created by biao on 24-9-10. // -#include "legged_gym_controller/FSM/StateFixedStand.h" +#include "rl_quadruped_controller/FSM/StateFixedStand.h" #include diff --git a/controllers/legged_gym_controller/src/FSM/StatePassive.cpp b/controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp similarity index 94% rename from controllers/legged_gym_controller/src/FSM/StatePassive.cpp rename to controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp index fe25be5..9622cc6 100644 --- a/controllers/legged_gym_controller/src/FSM/StatePassive.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp @@ -2,7 +2,7 @@ // Created by tlab-uav on 24-9-6. // -#include "legged_gym_controller/FSM/StatePassive.h" +#include "rl_quadruped_controller/FSM/StatePassive.h" #include diff --git a/controllers/legged_gym_controller/src/FSM/StateRL.cpp b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp similarity index 99% rename from controllers/legged_gym_controller/src/FSM/StateRL.cpp rename to controllers/rl_quadruped_controller/src/FSM/StateRL.cpp index 58f99e6..2692276 100644 --- a/controllers/legged_gym_controller/src/FSM/StateRL.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp @@ -2,7 +2,7 @@ // Created by biao on 24-10-6. // -#include "legged_gym_controller/FSM/StateRL.h" +#include "rl_quadruped_controller/FSM/StateRL.h" #include #include diff --git a/controllers/legged_gym_controller/src/LeggedGymController.cpp b/controllers/rl_quadruped_controller/src/LeggedGymController.cpp similarity index 98% rename from controllers/legged_gym_controller/src/LeggedGymController.cpp rename to controllers/rl_quadruped_controller/src/LeggedGymController.cpp index 99d1194..e6232f5 100644 --- a/controllers/legged_gym_controller/src/LeggedGymController.cpp +++ b/controllers/rl_quadruped_controller/src/LeggedGymController.cpp @@ -4,7 +4,7 @@ #include "LeggedGymController.h" -namespace legged_gym_controller { +namespace rl_quadruped_controller { using config_type = controller_interface::interface_configuration_type; controller_interface::InterfaceConfiguration LeggedGymController::command_interface_configuration() const { @@ -191,4 +191,4 @@ namespace legged_gym_controller { } #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(legged_gym_controller::LeggedGymController, controller_interface::ControllerInterface); +PLUGINLIB_EXPORT_CLASS(rl_quadruped_controller::LeggedGymController, controller_interface::ControllerInterface); diff --git a/controllers/legged_gym_controller/src/LeggedGymController.h b/controllers/rl_quadruped_controller/src/LeggedGymController.h similarity index 92% rename from controllers/legged_gym_controller/src/LeggedGymController.h rename to controllers/rl_quadruped_controller/src/LeggedGymController.h index 086ec35..083cb47 100644 --- a/controllers/legged_gym_controller/src/LeggedGymController.h +++ b/controllers/rl_quadruped_controller/src/LeggedGymController.h @@ -5,14 +5,14 @@ #ifndef LEGGEDGYMCONTROLLER_H #define LEGGEDGYMCONTROLLER_H #include -#include +#include -#include "legged_gym_controller/FSM/StateFixedStand.h" -#include "legged_gym_controller/FSM/StateFixedDown.h" -#include "legged_gym_controller/FSM/StatePassive.h" -#include "legged_gym_controller/control/CtrlComponent.h" +#include "rl_quadruped_controller/FSM/StateFixedStand.h" +#include "rl_quadruped_controller/FSM/StateFixedDown.h" +#include "rl_quadruped_controller/FSM/StatePassive.h" +#include "rl_quadruped_controller/control/CtrlComponent.h" -namespace legged_gym_controller { +namespace rl_quadruped_controller { struct FSMStateList { std::shared_ptr invalid; std::shared_ptr passive; diff --git a/controllers/legged_gym_controller/src/common/ObservationBuffer.cpp b/controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp similarity index 100% rename from controllers/legged_gym_controller/src/common/ObservationBuffer.cpp rename to controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp diff --git a/controllers/legged_gym_controller/src/common/ObservationBuffer.h b/controllers/rl_quadruped_controller/src/common/ObservationBuffer.h similarity index 100% rename from controllers/legged_gym_controller/src/common/ObservationBuffer.h rename to controllers/rl_quadruped_controller/src/common/ObservationBuffer.h diff --git a/descriptions/README.md b/descriptions/README.md index ed0e009..aafd84d 100644 --- a/descriptions/README.md +++ b/descriptions/README.md @@ -10,6 +10,8 @@ This folder contains the URDF and SRDF files for the quadruped robot. * [B2](unitree/b2_description/) * Xiaomi * [Cyberdog](xiaomi/cyberdog_description/) +* Deep Robotics + * [Lite 3](deep_robotics/lite3_description/) ## Steps to transfer urdf to Mujoco model @@ -27,7 +29,9 @@ This folder contains the URDF and SRDF files for the quadruped robot. ``` ## 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 Harmonic ```bash sudo apt-get install ros-jazzy-ros-gz diff --git a/descriptions/deep_robotics/lite3_description/config/gazebo.yaml b/descriptions/deep_robotics/lite3_description/config/gazebo.yaml index 04709ac..559e61c 100644 --- a/descriptions/deep_robotics/lite3_description/config/gazebo.yaml +++ b/descriptions/deep_robotics/lite3_description/config/gazebo.yaml @@ -16,8 +16,8 @@ controller_manager: unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController - legged_gym_controller: - type: legged_gym_controller/LeggedGymController + rl_quadruped_controller: + type: rl_quadruped_controller/LeggedGymController imu_sensor_broadcaster: ros__parameters: @@ -98,7 +98,7 @@ unitree_guide_controller: - linear_acceleration.y - linear_acceleration.z -legged_gym_controller: +rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym" diff --git a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml b/descriptions/deep_robotics/lite3_description/config/robot_control.yaml index 98eecf8..4798138 100644 --- a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml +++ b/descriptions/deep_robotics/lite3_description/config/robot_control.yaml @@ -16,8 +16,8 @@ controller_manager: ocs2_quadruped_controller: type: ocs2_quadruped_controller/Ocs2QuadrupedController - legged_gym_controller: - type: legged_gym_controller/LeggedGymController + rl_quadruped_controller: + type: rl_quadruped_controller/LeggedGymController imu_sensor_broadcaster: ros__parameters: diff --git a/descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py b/descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py index 825dab9..51bce4a 100644 --- a/descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py +++ b/descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py @@ -4,7 +4,6 @@ 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.launch_description_sources import PythonLaunchDescriptionSource from launch.event_handlers import OnProcessExit from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node diff --git a/descriptions/unitree/a1_description/README.md b/descriptions/unitree/a1_description/README.md index 90bcab8..0ab7484 100644 --- a/descriptions/unitree/a1_description/README.md +++ b/descriptions/unitree/a1_description/README.md @@ -32,7 +32,7 @@ ros2 launch a1_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch a1_description ocs2_control.launch.py ``` -* Legged Gym Controller +* RL Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash ros2 launch a1_description rl_control.launch.py @@ -44,7 +44,7 @@ ros2 launch a1_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch a1_description gazebo_unitree_guide.launch.py ``` -* Legged Gym Controller +* RL Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash ros2 launch a1_description gazebo_rl_control.launch.py diff --git a/descriptions/unitree/a1_description/config/gazebo.yaml b/descriptions/unitree/a1_description/config/gazebo.yaml index ea18712..32bcfda 100644 --- a/descriptions/unitree/a1_description/config/gazebo.yaml +++ b/descriptions/unitree/a1_description/config/gazebo.yaml @@ -16,8 +16,8 @@ controller_manager: unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController - legged_gym_controller: - type: legged_gym_controller/LeggedGymController + rl_quadruped_controller: + type: rl_quadruped_controller/LeggedGymController imu_sensor_broadcaster: ros__parameters: @@ -97,10 +97,10 @@ unitree_guide_controller: - linear_acceleration.y - linear_acceleration.z -legged_gym_controller: +rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz - config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym" + config_folder: "/home/biao/ros2_ws/install/a1_description/share/a1_description/config/issacgym" command_prefix: "leg_pd_controller" joints: - FL_hip_joint diff --git a/descriptions/unitree/a1_description/config/issacgym/config.yaml b/descriptions/unitree/a1_description/config/issacgym/config.yaml index 4aade37..4ac2b9b 100644 --- a/descriptions/unitree/a1_description/config/issacgym/config.yaml +++ b/descriptions/unitree/a1_description/config/issacgym/config.yaml @@ -15,14 +15,14 @@ clip_actions_upper: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -rl_kp: [13.5, 13.5, 13.5, - 13.5, 13.5, 13.5, - 13.5, 13.5, 13.5, - 13.5, 13.5, 13.5] -rl_kd: [0.41, 0.41, 0.41, - 0.41, 0.41, 0.41, - 0.41, 0.41, 0.41, - 0.41, 0.41, 0.41] +rl_kp: [20.1, 20.1, 20.1, + 20.1, 20.1, 20.1, + 20.1, 20.1, 20.1, + 20.1, 20.1, 20.1] +rl_kd: [0.54, 0.54, 0.54, + 0.54, 0.54, 0.54, + 0.54, 0.54, 0.54, + 0.54, 0.54, 0.54] fixed_kp: [80, 80, 80, 80, 80, 80, 80, 80, 80, diff --git a/descriptions/unitree/a1_description/config/robot_control.yaml b/descriptions/unitree/a1_description/config/robot_control.yaml index c1afad2..c4180bc 100644 --- a/descriptions/unitree/a1_description/config/robot_control.yaml +++ b/descriptions/unitree/a1_description/config/robot_control.yaml @@ -16,8 +16,8 @@ controller_manager: ocs2_quadruped_controller: type: ocs2_quadruped_controller/Ocs2QuadrupedController - legged_gym_controller: - type: legged_gym_controller/LeggedGymController + rl_quadruped_controller: + type: rl_quadruped_controller/LeggedGymController imu_sensor_broadcaster: ros__parameters: @@ -133,7 +133,7 @@ ocs2_quadruped_controller: - FR - RR -legged_gym_controller: +rl_quadruped_controller: ros__parameters: update_rate: 100 # Hz joints: diff --git a/descriptions/unitree/a1_description/launch/gazebo_rl_control.launch.py b/descriptions/unitree/a1_description/launch/gazebo_rl_control.launch.py index 20c469a..33338b2 100644 --- a/descriptions/unitree/a1_description/launch/gazebo_rl_control.launch.py +++ b/descriptions/unitree/a1_description/launch/gazebo_rl_control.launch.py @@ -71,7 +71,7 @@ def generate_launch_description(): controller = Node( package="controller_manager", executable="spawner", - arguments=["legged_gym_controller", "--controller-manager", "/controller_manager"], + arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"], parameters=[ { 'config_folder': os.path.join(get_package_share_directory(package_description), 'config', diff --git a/descriptions/unitree/a1_description/launch/rl_control.launch.py b/descriptions/unitree/a1_description/launch/rl_control.launch.py index 149fa70..e65cd04 100644 --- a/descriptions/unitree/a1_description/launch/rl_control.launch.py +++ b/descriptions/unitree/a1_description/launch/rl_control.launch.py @@ -12,16 +12,18 @@ from launch_ros.substitutions import FindPackageShare package_description = "a1_description" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] +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, mappings={'robot_type': robot_type_value}) - return (robot_description_config.toxml(), robot_type_value) + 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() -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -72,10 +74,17 @@ def launch_setup(context, *args, **kwargs): controller = Node( package="controller_manager", executable="spawner", - arguments=["legged_gym_controller", "--controller-manager", "/controller_manager"], + arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"], ) - return [ + 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, @@ -91,26 +100,4 @@ def launch_setup(context, *args, **kwargs): on_exit=[controller], ) ), - ] - - -def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='a1', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") - - return LaunchDescription([ - robot_type_arg, - OpaqueFunction(function=launch_setup), - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ) ]) diff --git a/descriptions/unitree/a1_description/xacro/gazebo.xacro b/descriptions/unitree/a1_description/xacro/gazebo.xacro index 781a520..e8ce4f7 100644 --- a/descriptions/unitree/a1_description/xacro/gazebo.xacro +++ b/descriptions/unitree/a1_description/xacro/gazebo.xacro @@ -117,13 +117,6 @@ 0.6 0.6 1 - - - .05 .05 .05 1.0 - .05 .05 .05 1.0 - .05 .05 .05 1.0 - - @@ -135,54 +128,54 @@ - - - - - - + + 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 + diff --git a/descriptions/unitree/a1_description/xacro/leg.xacro b/descriptions/unitree/a1_description/xacro/leg.xacro index ac6fa93..df95aeb 100644 --- a/descriptions/unitree/a1_description/xacro/leg.xacro +++ b/descriptions/unitree/a1_description/xacro/leg.xacro @@ -174,25 +174,12 @@ 0.2 0.2 - - - .5 .5 .5 1.0 - .5 .5 .5 1.0 - .5 .5 .5 1.0 - - + 0.2 0.2 0 - - - .175 .175 .175 1.0 - .175 .175 .175 1.0 - .175 .175 .175 1.0 - - @@ -201,26 +188,12 @@ 0.6 0.6 1 - - - .5 .5 .5 1.0 - .5 .5 .5 1.0 - .5 .5 .5 1.0 - - 0.6 0.6 1 - - - .175 .175 .175 1.0 - .175 .175 .175 1.0 - .175 .175 .175 1.0 - - diff --git a/descriptions/unitree/a1_description/xacro/robot.xacro b/descriptions/unitree/a1_description/xacro/robot.xacro index ca333aa..451fa5a 100644 --- a/descriptions/unitree/a1_description/xacro/robot.xacro +++ b/descriptions/unitree/a1_description/xacro/robot.xacro @@ -53,7 +53,6 @@ - diff --git a/descriptions/unitree/go1_description/README.md b/descriptions/unitree/go1_description/README.md index 67308e8..4f5de2a 100644 --- a/descriptions/unitree/go1_description/README.md +++ b/descriptions/unitree/go1_description/README.md @@ -6,7 +6,7 @@ This repository contains the urdf model of go1. ## Build ```bash cd ~/ros2_ws -colcon build --packages-up-to go1_description +colcon build --packages-up-to go1_description --symlink-install ``` ## Visualize the robot diff --git a/descriptions/unitree/go2_description/README.md b/descriptions/unitree/go2_description/README.md index d333f85..f7711eb 100644 --- a/descriptions/unitree/go2_description/README.md +++ b/descriptions/unitree/go2_description/README.md @@ -34,6 +34,11 @@ ros2 launch go2_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch go2_description ocs2_control.launch.py ``` +* RL Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch go2_description rl_control.launch.py + ``` ### Gazebo Simulator * Unitree Guide Controller @@ -41,7 +46,12 @@ ros2 launch go2_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch go2_description gazebo_unitree_guide.launch.py ``` - +* RL Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + 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: diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index ea18712..5f0da8d 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -16,8 +16,8 @@ controller_manager: unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController - legged_gym_controller: - type: legged_gym_controller/LeggedGymController + rl_quadruped_controller: + type: rl_quadruped_controller/LeggedGymController imu_sensor_broadcaster: ros__parameters: @@ -97,10 +97,10 @@ unitree_guide_controller: - linear_acceleration.y - linear_acceleration.z -legged_gym_controller: +rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz - config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym" + config_folder: "/home/biao/ros2_ws/install/go2_description/share/go2_description/config/issacgym" command_prefix: "leg_pd_controller" joints: - FL_hip_joint diff --git a/descriptions/unitree/go2_description/config/issacgym/config.yaml b/descriptions/unitree/go2_description/config/issacgym/config.yaml new file mode 100644 index 0000000..10be7ba --- /dev/null +++ b/descriptions/unitree/go2_description/config/issacgym/config.yaml @@ -0,0 +1,50 @@ +model_name: "policy.pt" +framework: "isaacgym" +rows: 4 +cols: 3 +use_history: True +decimation: 4 +num_observations: 45 +observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] +clip_obs: 100.0 +clip_actions_lower: [-100, -100, -100, + -100, -100, -100, + -100, -100, -100, + -100, -100, -100] +clip_actions_upper: [100, 100, 100, + 100, 100, 100, + 100, 100, 100, + 100, 100, 100] +rl_kp: [20.1, 20.1, 20.1, + 20.1, 20.1, 20.1, + 20.1, 20.1, 20.1, + 20.1, 20.1, 20.1] +rl_kd: [0.61, 0.61, 0.61, + 0.61, 0.61, 0.61, + 0.61, 0.61, 0.61, + 0.61, 0.61, 0.61] +fixed_kp: [80, 80, 80, + 80, 80, 80, + 80, 80, 80, + 80, 80, 80] +fixed_kd: [3, 3, 3, + 3, 3, 3, + 3, 3, 3, + 3, 3, 3] +hip_scale_reduction: 0.5 +hip_scale_reduction_indices: [0, 3, 6, 9] +num_of_dofs: 12 +action_scale: 0.25 +lin_vel_scale: 2.0 +ang_vel_scale: 0.25 +dof_pos_scale: 1.0 +dof_vel_scale: 0.05 +commands_scale: [2.0, 2.0, 1.0] +torque_limits: [33.5, 33.5, 33.5, + 33.5, 33.5, 33.5, + 33.5, 33.5, 33.5, + 33.5, 33.5, 33.5] +default_dof_pos: [ 0.1000, 0.8000, -1.5000, + -0.1000, 0.8000, -1.5000, + 0.1000, 1.0000, -1.5000, + -0.1000, 1.0000, -1.5000] diff --git a/descriptions/unitree/go2_description/config/issacgym/policy.pt b/descriptions/unitree/go2_description/config/issacgym/policy.pt new file mode 100644 index 0000000..9963d6b Binary files /dev/null and b/descriptions/unitree/go2_description/config/issacgym/policy.pt differ diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 4d51433..7134243 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/descriptions/unitree/go2_description/config/robot_control.yaml @@ -1,7 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 500 # Hz + update_rate: 1000 # Hz # Define the available controllers joint_state_broadcaster: @@ -16,6 +16,9 @@ controller_manager: ocs2_quadruped_controller: type: ocs2_quadruped_controller/Ocs2QuadrupedController + rl_quadruped_controller: + type: rl_quadruped_controller/LeggedGymController + imu_sensor_broadcaster: ros__parameters: sensor_name: "imu_sensor" @@ -73,7 +76,7 @@ unitree_guide_controller: ocs2_quadruped_controller: ros__parameters: - update_rate: 500 # Hz + update_rate: 100 # Hz joints: - FL_hip_joint @@ -127,4 +130,47 @@ ocs2_quadruped_controller: - FL - RL - FR - - RR \ No newline at end of file + - RR + +rl_quadruped_controller: + ros__parameters: + update_rate: 200 # Hz + joints: + - FL_hip_joint + - FL_thigh_joint + - FL_calf_joint + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + imu_name: "imu_sensor" + + imu_interfaces: + - orientation.w + - orientation.x + - orientation.y + - orientation.z + - angular_velocity.x + - angular_velocity.y + - angular_velocity.z + - linear_acceleration.x + - linear_acceleration.y + - linear_acceleration.z \ No newline at end of file diff --git a/descriptions/unitree/go2_description/launch/gazebo_rl_control.launch.py b/descriptions/unitree/go2_description/launch/gazebo_rl_control.launch.py new file mode 100644 index 0000000..6b90504 --- /dev/null +++ b/descriptions/unitree/go2_description/launch/gazebo_rl_control.launch.py @@ -0,0 +1,110 @@ +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.launch_description_sources import PythonLaunchDescriptionSource +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 = "go2_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, mappings={'GAZEBO': 'true'}) + 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() + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', '-name', + 'go2', '-allow_renaming', 'true', '-z', '0.4'], + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[ + { + 'publish_frequency': 20.0, + 'use_tf_static': True, + 'robot_description': robot_description, + 'ignore_timestamp': True + } + ], + ) + + joint_state_publisher = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + imu_sensor_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["imu_sensor_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + leg_pd_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["leg_pd_controller", + "--controller-manager", "/controller_manager"], + ) + + controller = Node( + package="controller_manager", + executable="spawner", + arguments=["rl_quadruped_controller", "--controller-manager", "/controller_manager"], + parameters=[ + { + 'config_folder': os.path.join(get_package_share_directory(package_description), 'config', + 'issacgym'), + }], + ) + + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + Node( + 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=leg_pd_controller, + on_exit=[controller, imu_sensor_broadcaster, joint_state_publisher], + ) + ), + ]) diff --git a/descriptions/unitree/go2_description/launch/rl_control.launch.py b/descriptions/unitree/go2_description/launch/rl_control.launch.py new file mode 100644 index 0000000..ee91582 --- /dev/null +++ b/descriptions/unitree/go2_description/launch/rl_control.launch.py @@ -0,0 +1,103 @@ +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 = "go2_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", + ] + ) + + 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 + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers, + { + 'config_folder': os.path.join(get_package_share_directory(package_description), 'config', + 'issacgym'), + }], + output="both", + ) + + 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"], + ) + + controller = Node( + package="controller_manager", + executable="spawner", + arguments=["rl_quadruped_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], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=imu_sensor_broadcaster, + on_exit=[controller], + ) + ), + ]) diff --git a/descriptions/unitree/go2_description/launch/unitree_guide.launch.py b/descriptions/unitree/go2_description/launch/unitree_guide.launch.py index ae7513c..4741397 100644 --- a/descriptions/unitree/go2_description/launch/unitree_guide.launch.py +++ b/descriptions/unitree/go2_description/launch/unitree_guide.launch.py @@ -12,16 +12,18 @@ from launch_ros.substitutions import FindPackageShare package_description = "go2_description" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] +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, mappings={'robot_type': robot_type_value}) - return (robot_description_config.toxml(), robot_type_value) + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) +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), @@ -30,6 +32,13 @@ def launch_setup(context, *args, **kwargs): ] ) + 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', @@ -44,13 +53,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", @@ -71,42 +73,21 @@ def launch_setup(context, *args, **kwargs): arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"], ) - return [ - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[unitree_guide_controller], - ) - ), - ] - - -def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='go2', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") - return LaunchDescription([ - robot_type_arg, - OpaqueFunction(function=launch_setup), 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], + ) + ), ]) diff --git a/descriptions/unitree/go2_description/launch/visualize.launch.py b/descriptions/unitree/go2_description/launch/visualize.launch.py index f044aba..579800d 100644 --- a/descriptions/unitree/go2_description/launch/visualize.launch.py +++ b/descriptions/unitree/go2_description/launch/visualize.launch.py @@ -3,23 +3,30 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch_ros.actions import Node import xacro package_description = "go2_description" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] +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, mappings={'robot_type': robot_type_value}) + robot_description_config = xacro.process_file(xacro_file) return robot_description_config.toxml() +def generate_launch_description(): -def launch_setup(context, *args, **kwargs): - robot_description = process_xacro(context) - return [ + rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") + robot_description = process_xacro() + + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -39,27 +46,4 @@ def launch_setup(context, *args, **kwargs): name='joint_state_publisher', output='screen', ) - ] - - -def generate_launch_description(): - - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='go2', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") - - return LaunchDescription([ - robot_type_arg, - OpaqueFunction(function=launch_setup), - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ) ]) \ No newline at end of file diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index 1b1fb45..6a6be5e 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -106,7 +106,7 @@ - $(find a1_description)/config/gazebo.yaml + $(find go2_description)/config/gazebo.yaml @@ -128,54 +128,54 @@ - - - - - - + + 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 +