diff --git a/README.md b/README.md index 095ce2f..702fb57 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ Todo List: - [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/) -- [ ] Fully understand the RL Workflow +- [x] Fully understand the RL Workflow - [x] ROS2 Humble Gazebo Classic Support Video for Unitree Guide Controller: diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h index 184b2d9..82aa31e 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h @@ -9,9 +9,7 @@ #include #include -#include #include -#include namespace ocs2::legged_robot { class KalmanFilterEstimate final : public StateEstimateBase { diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h index 33eb4bd..607ee2b 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h @@ -7,7 +7,6 @@ #include #include -#include #include #include diff --git a/descriptions/anybotics/anymal_c_description/README.md b/descriptions/anybotics/anymal_c_description/README.md index fad8e27..4502524 100644 --- a/descriptions/anybotics/anymal_c_description/README.md +++ b/descriptions/anybotics/anymal_c_description/README.md @@ -22,13 +22,18 @@ ros2 launch anymal_c_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:=anymal_c_description + ``` * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description ``` * Legged Gym Controller - ```bash + ```bash source ~/ros2_ws/install/setup.bash - ros2 launch anymal_c_description rl_control.launch.py + ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description ``` \ No newline at end of file diff --git a/descriptions/anybotics/anymal_c_description/config/robot_control.yaml b/descriptions/anybotics/anymal_c_description/config/robot_control.yaml index 5b46889..c734dad 100644 --- a/descriptions/anybotics/anymal_c_description/config/robot_control.yaml +++ b/descriptions/anybotics/anymal_c_description/config/robot_control.yaml @@ -1,7 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 500 # Hz + update_rate: 200 # Hz # Define the available controllers joint_state_broadcaster: @@ -17,13 +17,91 @@ controller_manager: type: ocs2_quadruped_controller/Ocs2QuadrupedController rl_quadruped_controller: - type: LH_quadruped_controller/LeggedGymController + type: rl_quadruped_controller/LeggedGymController imu_sensor_broadcaster: ros__parameters: sensor_name: "imu_sensor" frame_id: "imu_link" +unitree_guide_controller: + ros__parameters: + update_rate: 200 # Hz + joints: + - RF_HAA + - RF_HFE + - RF_KFE + - LF_HAA + - LF_HFE + - LF_KFE + - RH_HAA + - RH_HFE + - RH_KFE + - LH_HAA + - LH_HFE + - LH_KFE + + down_pos: + - -0.0 + - 1.41 + - -2.58 + - 0.0 + - 1.41 + - -2.58 + - -0.0 + - -1.41 + - 2.58 + - 0.0 + - -1.41 + - 2.58 + + stand_pos: + - 0.2 + - 0.6 + - -0.85 + - -0.2 + - 0.6 + - -0.85 + - 0.2 + - -0.6 + - 0.85 + - -0.2 + - -0.6 + - 0.85 + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet_names: + - LF_FOOT + - RF_FOOT + - LH_FOOT + - RH_FOOT + + imu_name: "imu_sensor" + base_name: "base" + + 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 + ocs2_quadruped_controller: ros__parameters: update_rate: 100 # Hz @@ -82,3 +160,90 @@ ocs2_quadruped_controller: - RF - LH - RH + +rl_quadruped_controller: + ros__parameters: + update_rate: 200 # Hz + robot_pkg: "anymal_c_description" + model_folder: "legged_gym" + joints: + - LF_HAA + - LF_HFE + - LF_KFE + - RF_HAA + - RF_HFE + - RF_KFE + - LH_HAA + - LH_HFE + - LH_KFE + - RH_HAA + - RH_HFE + - RH_KFE + + down_pos: + - -0.0 + - 1.41 + - -2.58 + - 0.0 + - 1.41 + - -2.58 + - -0.0 + - -1.41 + - 2.58 + - 0.0 + - -1.41 + - 2.58 + + stand_pos: + - 0.2 + - 0.6 + - -0.85 + - -0.2 + - 0.6 + - -0.85 + - 0.2 + - -0.6 + - 0.85 + - -0.2 + - -0.6 + - 0.85 + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet_names: + - LF_FOOT + - RF_FOOT + - LH_FOOT + - RH_FOOT + + foot_force_name: "foot_force" + foot_force_interfaces: + - LF + - RF + - LH + - RH + + imu_name: "imu_sensor" + base_name: "base" + + 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/deep_robotics/lite3_description/README.md b/descriptions/deep_robotics/lite3_description/README.md index bedbf12..f2b0d00 100644 --- a/descriptions/deep_robotics/lite3_description/README.md +++ b/descriptions/deep_robotics/lite3_description/README.md @@ -42,7 +42,7 @@ ros2 launch lite3_description visualize.launch.py * RL Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=lite3_description model_folder:=legged_gym + ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=lite3_description ``` ### Gazebo Classic 11 (ROS2 Humble) diff --git a/descriptions/deep_robotics/x30_description/README.md b/descriptions/deep_robotics/x30_description/README.md index c50d7d6..1f14c75 100644 --- a/descriptions/deep_robotics/x30_description/README.md +++ b/descriptions/deep_robotics/x30_description/README.md @@ -39,6 +39,12 @@ ros2 launch x30_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=x30_description model_folder:=legged_gym ``` +* RL Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=x30_description + ``` + ### Gazebo Classic 11 (ROS2 Humble) diff --git a/descriptions/deep_robotics/x30_description/config/legged_gym/config.yaml b/descriptions/deep_robotics/x30_description/config/legged_gym/config.yaml index 6be4dab..890499e 100644 --- a/descriptions/deep_robotics/x30_description/config/legged_gym/config.yaml +++ b/descriptions/deep_robotics/x30_description/config/legged_gym/config.yaml @@ -15,18 +15,18 @@ clip_actions_upper: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] -rl_kp: [160, 160, 160, - 160, 160, 160, - 160, 160, 160, - 160, 160, 160,] -rl_kd: [8.0, 8.0, 8.0, - 8.0, 8.0, 8.0, - 8.0, 8.0, 8.0, - 8.0, 8.0, 8.0] +rl_kp: [150.0, 150.0, 150.0, + 150.0, 150.0, 150.0, + 150.0, 150.0, 150.0, + 150.0, 150.0, 150.0] +rl_kd: [2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0] hip_scale_reduction: 1.0 hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 -action_scale: 0.25 +action_scale: 0.4 lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/descriptions/deep_robotics/x30_description/config/legged_gym/policy.pt b/descriptions/deep_robotics/x30_description/config/legged_gym/policy.pt old mode 100644 new mode 100755 index 8b494ea..7c165b8 Binary files a/descriptions/deep_robotics/x30_description/config/legged_gym/policy.pt and b/descriptions/deep_robotics/x30_description/config/legged_gym/policy.pt differ diff --git a/descriptions/deep_robotics/x30_description/config/robot_control.yaml b/descriptions/deep_robotics/x30_description/config/robot_control.yaml index 7ea88db..82b85e2 100644 --- a/descriptions/deep_robotics/x30_description/config/robot_control.yaml +++ b/descriptions/deep_robotics/x30_description/config/robot_control.yaml @@ -165,6 +165,95 @@ ocs2_quadruped_controller: - FR - HR +rl_quadruped_controller: + ros__parameters: + update_rate: 200 # Hz + robot_pkg: "x30_description" + model_folder: "legged_gym" + stand_kp: 500.0 + stand_kd: 20.0 + joints: + - FL_HipX + - FL_HipY + - FL_Knee + - FR_HipX + - FR_HipY + - FR_Knee + - HL_HipX + - HL_HipY + - HL_Knee + - HR_HipX + - HR_HipY + - HR_Knee + + down_pos: + - 0.0 + - -1.22 + - 2.61 + - 0.0 + - -1.22 + - 2.61 + - 0.0 + - -1.22 + - 2.61 + - 0.0 + - -1.22 + - 2.61 + + stand_pos: + - 0.0 + - -1.0 + - 1.8 + - 0.0 + - -1.0 + - 1.8 + - 0.0 + - -1.0 + - 1.8 + - 0.0 + - -1.0 + - 1.8 + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet_names: + - FL_FOOT + - FR_FOOT + - HL_FOOT + - HR_FOOT + + foot_force_name: "foot_force" + foot_force_interfaces: + - FL + - FR + - HL + - HR + + imu_name: "imu_sensor" + base_name: "base" + + 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 + rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz diff --git a/descriptions/unitree/aliengo_description/config/legged_gym/config.yaml b/descriptions/unitree/aliengo_description/config/legged_gym/config.yaml new file mode 100644 index 0000000..0f1cf3a --- /dev/null +++ b/descriptions/unitree/aliengo_description/config/legged_gym/config.yaml @@ -0,0 +1,41 @@ +model_name: "policy.pt" +framework: "isaacgym" +rows: 4 +cols: 3 +decimation: 4 +num_observations: 48 +observations: ["lin_vel", "ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] +#observations_history: [6, 5, 4, 3, 2, 1, 0] +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, 20, 20, + 20, 20, 20, + 20, 20, 20, + 20, 20, 20] +rl_kd: [0.75, 0.75, 0.75, + 0.75, 0.75, 0.75, + 0.75, 0.75, 0.75, + 0.75, 0.75, 0.75] +hip_scale_reduction: 1.0 +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, 0.25] + +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] \ No newline at end of file diff --git a/descriptions/unitree/aliengo_description/config/legged_gym/policy.pt b/descriptions/unitree/aliengo_description/config/legged_gym/policy.pt new file mode 100755 index 0000000..a7905ad Binary files /dev/null and b/descriptions/unitree/aliengo_description/config/legged_gym/policy.pt differ diff --git a/descriptions/unitree/aliengo_description/config/robot_control.yaml b/descriptions/unitree/aliengo_description/config/robot_control.yaml index 691eb6e..b2f2bec 100644 --- a/descriptions/unitree/aliengo_description/config/robot_control.yaml +++ b/descriptions/unitree/aliengo_description/config/robot_control.yaml @@ -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" @@ -131,4 +134,77 @@ ocs2_quadruped_controller: - FL - RL - FR - - RR \ No newline at end of file + - RR + +rl_quadruped_controller: + ros__parameters: + update_rate: 200 # Hz + robot_pkg: "aliengo_description" + model_folder: "legged_gym" + 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 + + stand_pos: + - 0.1000 + - 0.8000 + - -1.5000 + - -0.1000 + - 0.8000 + - -1.5000 + - 0.1000 + - 0.8000 + - -1.5000 + - -0.1000 + - 0.8000 + - -1.5000 + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet_names: + - FL_foot + - FR_foot + - RL_foot + - RR_foot + + foot_force_name: "foot_force" + foot_force_interfaces: + - FL + - RL + - FR + - RR + + imu_name: "imu_sensor" + base_name: "base" + + 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/aliengo_description/urdf/robot.urdf b/descriptions/unitree/aliengo_description/urdf/robot.urdf index 04034cb..6f4c1f3 100644 --- a/descriptions/unitree/aliengo_description/urdf/robot.urdf +++ b/descriptions/unitree/aliengo_description/urdf/robot.urdf @@ -1,6 +1,6 @@ - + @@ -174,7 +174,6 @@ - @@ -196,7 +195,6 @@ - @@ -254,7 +252,6 @@ - @@ -301,7 +298,6 @@ - @@ -397,6 +393,30 @@ + + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission @@ -446,7 +466,6 @@ - @@ -493,7 +512,6 @@ - @@ -589,6 +607,30 @@ + + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission @@ -638,7 +680,6 @@ - @@ -685,7 +726,6 @@ - @@ -781,6 +821,30 @@ + + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission @@ -830,7 +894,6 @@ - @@ -877,7 +940,6 @@ - @@ -973,6 +1035,30 @@ + + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission diff --git a/descriptions/unitree/go1_description/urdf/robot.urdf b/descriptions/unitree/go1_description/urdf/robot.urdf index ae2ca1e..42f9e18 100644 --- a/descriptions/unitree/go1_description/urdf/robot.urdf +++ b/descriptions/unitree/go1_description/urdf/robot.urdf @@ -1,6 +1,6 @@ - + @@ -13,9 +13,15 @@ + + + + + + @@ -196,7 +202,7 @@ - + @@ -254,7 +260,7 @@ - + @@ -274,7 +280,6 @@ - @@ -301,6 +306,7 @@ + @@ -320,7 +326,6 @@ - @@ -347,6 +352,7 @@ + @@ -396,6 +402,29 @@ + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission @@ -445,7 +474,7 @@ - + @@ -465,7 +494,6 @@ - @@ -492,6 +520,7 @@ + @@ -511,7 +540,6 @@ - @@ -538,6 +566,7 @@ + @@ -587,6 +616,29 @@ + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission @@ -636,7 +688,7 @@ - + @@ -656,7 +708,6 @@ - @@ -683,6 +734,7 @@ + @@ -702,7 +754,6 @@ - @@ -729,6 +780,7 @@ + @@ -778,6 +830,29 @@ + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission @@ -827,7 +902,7 @@ - + @@ -847,7 +922,6 @@ - @@ -874,6 +948,7 @@ + @@ -893,7 +968,6 @@ - @@ -920,6 +994,7 @@ + @@ -969,6 +1044,29 @@ + + 0.2 + 0.2 + + + 0.2 + 0.2 + 1 + + + + + 0.2 + 0.2 + 1 + + + 0.6 + 0.6 + 1 + + + transmission_interface/SimpleTransmission @@ -1030,49 +1128,49 @@ - - - - 16 - - 2.094 - - 928 - 800 - R8G8B8 - - - 0.1 - 5 - - - - 0.025 - true - 0.0 - camera_face_ir - /camera_face/color/image_raw - /camera_face/color/camera_info - /camera_face/depth/image_raw - /camera_face/depth/camera_info - - /cam1/point_cloud_face - camera_optical_face - 0.1 - 1.5 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0 - 0.0045 - 0.0039 - 0 - 0 - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1104,49 +1202,49 @@ - - - - 16 - - 2.094 - - 928 - 800 - R8G8B8 - - - 0.1 - 5 - - - - 0.025 - true - 0.0 - camera_chin_ir - /camera_chin/color/image_raw - /camera_chin/color/camera_info - /camera_chin/depth/image_raw - /camera_chin/depth/camera_info - - /cam2/point_cloud_chin - camera_optical_chin - 0.1 - 1.5 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0 - 0.0045 - 0.0039 - 0 - 0 - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1178,49 +1276,49 @@ - - - - 16 - - 2.094 - - 928 - 800 - R8G8B8 - - - 0.1 - 5 - - - - 0.025 - true - 0.0 - camera_left_ir - /camera_left/color/image_raw - /camera_left/color/camera_info - /camera_left/depth/image_raw - /camera_left/depth/camera_info - - /cam3/point_cloud_left - camera_optical_left - 0.1 - 1.5 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0 - 0.0045 - 0.0039 - 0 - 0 - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1252,49 +1350,49 @@ - - - - 16 - - 2.094 - - 928 - 800 - R8G8B8 - - - 0.1 - 5 - - - - 0.025 - true - 0.0 - camera_right_ir - /camera_right/color/image_raw - /camera_right/color/camera_info - /camera_right/depth/image_raw - /camera_right/depth/camera_info - - /cam4/point_cloud_right - camera_optical_right - 0.1 - 1.5 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0 - 0.0045 - 0.0039 - 0 - 0 - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1326,49 +1424,49 @@ - - - - 16 - - 2.094 - - 928 - 800 - R8G8B8 - - - 0.1 - 5 - - - - 0.025 - true - 0.0 - camera_rearDown_ir - /camera_rearDown/color/image_raw - /camera_rearDown/color/camera_info - /camera_rearDown/depth/image_raw - /camera_rearDown/depth/camera_info - - /cam5/point_cloud_rearDown - camera_optical_rearDown - 0.1 - 1.5 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0 - 0.0045 - 0.0039 - 0 - 0 - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index bb44c60..0de34a4 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: 200 # Hz + update_rate: 1000 # Hz # Define the available controllers joint_state_broadcaster: