add ocs2 control for deep robotics

This commit is contained in:
Huang Zhenbiao 2024-10-18 16:23:49 +08:00
parent fe28277003
commit b8e6d1bec3
10 changed files with 771 additions and 387 deletions

View File

@ -30,7 +30,7 @@ ros2 launch lite3_description visualize.launch.py
* OCS2 Quadruped Controller * OCS2 Quadruped Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch a1_description ocs2_control.launch.py ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=lite3_description
``` ```
* Legged Gym Controller * Legged Gym Controller
```bash ```bash

View File

@ -1,22 +1,22 @@
targetDisplacementVelocity 0.5; targetDisplacementVelocity 0.5;
targetRotationVelocity 1.57; targetRotationVelocity 1.57;
comHeight 0.33 comHeight 0.39
defaultJointState defaultJointState
{ {
(0,0) -0.20 ; FL_hip_joint (0,0) 0.1 ; FL_HipX
(1,0) 0.72 ; FL_thigh_joint (1,0) -0.732 ; FL_HipY
(2,0) -1.44 ; FL_calf_joint (2,0) 1.361 ; FL_Knee
(3,0) 0.20 ; FR_hip_joint (3,0) -0.1 ; FR_HipX
(4,0) 0.72 ; FR_thigh_joint (4,0) -0.732 ; FR_HipY
(5,0) -1.44 ; FR_calf_joint (5,0) 1.361 ; FR_Knee
(6,0) -0.20 ; RL_hip_joint (6,0) 0.1 ; HL_HipX
(7,0) 0.72 ; RL_thigh_joint (7,0) -0.732 ; HL_HipY
(8,0) -1.44 ; RL_calf_joint (8,0) 1.361 ; HL_Knee
(9,0) 0.20 ; RR_hip_joint (9,0) -0.1 ; HR_HipX
(10,0) 0.72 ; RR_thigh_joint (10,0) -0.732 ; HR_HipY
(11,0) -1.44 ; RR_calf_joint (11,0) 1.361 ; HR_Knee
} }
initialModeSchedule initialModeSchedule

View File

@ -12,7 +12,7 @@ model_settings
verboseCppAd true verboseCppAd true
recompileLibrariesCppAd false recompileLibrariesCppAd false
modelFolderCppAd /tmp/ocs2_quadruped_controller/a1 modelFolderCppAd /tmp/ocs2_quadruped_controller/lite3
} }
swing_trajectory_config swing_trajectory_config
@ -154,24 +154,24 @@ initialState
;; Base Pose: [position, orientation] ;; ;; Base Pose: [position, orientation] ;;
(6,0) 0.0 ; p_base_x (6,0) 0.0 ; p_base_x
(7,0) 0.0 ; p_base_y (7,0) 0.0 ; p_base_y
(8,0) 0.33 ; p_base_z (8,0) 0.39 ; p_base_z
(9,0) 0.0 ; theta_base_z (9,0) 0.0 ; theta_base_z
(10,0) 0.0 ; theta_base_y (10,0) 0.0 ; theta_base_y
(11,0) 0.0 ; theta_base_x (11,0) 0.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;; ;; Leg Joint Positions: [FL, HL, FR, HR] ;;
(12,0) -0.20 ; FL_hip_joint (12,0) 0.1 ; FL_HipX
(13,0) 0.72 ; FL_thigh_joint (13,0) -0.732 ; FL_HipY
(14,0) -1.44 ; FL_calf_joint (14,0) 1.361 ; FL_Knee
(15,0) -0.20 ; RL_hip_joint (15,0) 0.1 ; HL_HipX
(16,0) 0.72 ; RL_thigh_joint (16,0) -0.732 ; HL_HipY
(17,0) -1.44 ; RL_calf_joint (17,0) 1.361 ; HL_Knee
(18,0) 0.20 ; FR_hip_joint (18,0) -0.1 ; FR_HipX
(19,0) 0.72 ; FR_thigh_joint (19,0) -0.732 ; FR_HipY
(20,0) -1.44 ; FR_calf_joint (20,0) 1.361 ; FR_Knee
(21,0) 0.20 ; RR_hip_joint (21,0) -0.1 ; HR_HipX
(22,0) 0.72 ; RR_thigh_joint (22,0) -0.732 ; HR_HipY
(23,0) -1.44 ; RR_calf_joint (23,0) 1.361 ; HR_Knee
} }
; standard state weight matrix ; standard state weight matrix
@ -195,19 +195,19 @@ Q
(10,10) 300.0 ; theta_base_y (10,10) 300.0 ; theta_base_y
(11,11) 300.0 ; theta_base_x (11,11) 300.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;; ;; Leg Joint Positions: [FL, HL, FR, HR] ;;
(12,12) 5.0 ; FL_hip_joint (12,12) 5.0 ; FL_HipX
(13,13) 5.0 ; FL_thigh_joint (13,13) 5.0 ; FL_HipY
(14,14) 2.5 ; FL_calf_joint (14,14) 2.5 ; FL_Knee
(15,15) 5.0 ; RL_hip_joint (15,15) 5.0 ; HL_HipX
(16,16) 5.0 ; RL_thigh_joint (16,16) 5.0 ; HL_HipY
(17,17) 2.5 ; RL_calf_joint (17,17) 2.5 ; HL_Knee
(18,18) 5.0 ; FR_hip_joint (18,18) 5.0 ; FR_HipX
(19,19) 5.0 ; FR_thigh_joint (19,19) 5.0 ; FR_HipY
(20,20) 2.5 ; FR_calf_joint (20,20) 2.5 ; HR_Knee
(21,21) 5.0 ; RR_hip_joint (21,21) 5.0 ; HR_HipX
(22,22) 5.0 ; RR_thigh_joint (22,22) 5.0 ; HR_HipY
(23,23) 2.5 ; RR_calf_joint (23,23) 2.5 ; HR_Knee
} }
; control weight matrix ; control weight matrix
@ -215,7 +215,7 @@ R
{ {
scaling 1e-3 scaling 1e-3
;; Feet Contact Forces: [FL, FR, RL, RR] ;; ;; Feet Contact Forces: [FL, FR, HL, HR] ;;
(0,0) 1.0 ; front_left_force (0,0) 1.0 ; front_left_force
(1,1) 1.0 ; front_left_force (1,1) 1.0 ; front_left_force
(2,2) 1.0 ; front_left_force (2,2) 1.0 ; front_left_force
@ -229,7 +229,7 @@ R
(10,10) 1.0 ; rear_right_force (10,10) 1.0 ; rear_right_force
(11,11) 1.0 ; rear_right_force (11,11) 1.0 ; rear_right_force
;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;; ;; foot velocity relative to base: [FL, HL, FR, HR] (uses the Jacobian at nominal configuration) ;;
(12,12) 5000.0 ; x (12,12) 5000.0 ; x
(13,13) 5000.0 ; y (13,13) 5000.0 ; y
(14,14) 5000.0 ; z (14,14) 5000.0 ; z
@ -263,14 +263,14 @@ selfCollision
; Self Collision pairs ; Self Collision pairs
collisionLinkPairs collisionLinkPairs
{ {
[0] "FL_calf, FR_calf" [0] "FL_SHANK, FR_SHANK"
[1] "RL_calf, RR_calf" [1] "HL_SHANK, HR_SHANK"
[2] "FL_calf, RL_calf" [2] "FL_SHANK, HL_SHANK"
[3] "FR_calf, RR_calf" [3] "FR_SHANK, HR_SHANK"
[4] "FL_foot, FR_foot" [4] "FL_FOOT, FR_FOOT"
[5] "RL_foot, RR_foot" [5] "HL_FOOT, HR_FOOT"
[6] "FL_foot, RL_foot" [6] "FL_FOOT, HL_FOOT"
[7] "FR_foot, RR_foot" [7] "FR_FOOT, HR_FOOT"
} }
minimumDistance 0.05 minimumDistance 0.05
@ -283,9 +283,9 @@ selfCollision
; Whole body control ; Whole body control
torqueLimitsTask torqueLimitsTask
{ {
(0,0) 33.5 ; HAA (0,0) 24.0 ; HAA
(1,0) 33.5 ; HFE (1,0) 24.0 ; HFE
(2,0) 33.5 ; KFE (2,0) 36.0 ; KFE
} }
frictionConeTask frictionConeTask
@ -309,7 +309,7 @@ weight
; State Estimation ; State Estimation
kalmanFilter kalmanFilter
{ {
footRadius 0.02 footRadius 0.01
imuProcessNoisePosition 0.02 imuProcessNoisePosition 0.02
imuProcessNoiseVelocity 0.02 imuProcessNoiseVelocity 0.02
footProcessNoisePosition 0.002 footProcessNoisePosition 0.002

View File

@ -74,3 +74,62 @@ unitree_guide_controller:
- linear_acceleration.x - linear_acceleration.x
- linear_acceleration.y - linear_acceleration.y
- linear_acceleration.z - linear_acceleration.z
ocs2_quadruped_controller:
ros__parameters:
update_rate: 500 # Hz
default_kd: 1.2
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
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet:
- FL_FOOT
- FR_FOOT
- HL_FOOT
- HR_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
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- HL
- FR
- HR

View File

@ -27,6 +27,11 @@ ros2 launch x30_description visualize.launch.py
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=x30_description ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=x30_description
``` ```
* OCS2 Quadruped Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=x30_description
```
### Gazebo Simulator ### Gazebo Simulator
* Unitree Guide Controller * Unitree Guide Controller

View File

@ -1,22 +1,22 @@
targetDisplacementVelocity 0.5; targetDisplacementVelocity 0.5;
targetRotationVelocity 1.57; targetRotationVelocity 1.57;
comHeight 0.33 comHeight 0.54;
defaultJointState defaultJointState
{ {
(0,0) -0.20 ; FL_hip_joint (0,0) 0.0 ; FL_HipX
(1,0) 0.72 ; FL_thigh_joint (1,0) -0.732 ; FL_HipY
(2,0) -1.44 ; FL_calf_joint (2,0) 1.361 ; FL_Knee
(3,0) 0.20 ; FR_hip_joint (3,0) -0.0 ; FR_HipX
(4,0) 0.72 ; FR_thigh_joint (4,0) -0.732 ; FR_HipY
(5,0) -1.44 ; FR_calf_joint (5,0) 1.361 ; FR_Knee
(6,0) -0.20 ; RL_hip_joint (6,0) 0.0 ; HL_HipX
(7,0) 0.72 ; RL_thigh_joint (7,0) -0.732 ; HL_HipY
(8,0) -1.44 ; RL_calf_joint (8,0) 1.361 ; HL_Knee
(9,0) 0.20 ; RR_hip_joint (9,0) -0.0 ; HR_HipX
(10,0) 0.72 ; RR_thigh_joint (10,0) -0.732 ; HR_HipY
(11,0) -1.44 ; RR_calf_joint (11,0) 1.361 ; HR_Knee
} }
initialModeSchedule initialModeSchedule

View File

@ -12,14 +12,14 @@ model_settings
verboseCppAd true verboseCppAd true
recompileLibrariesCppAd false recompileLibrariesCppAd false
modelFolderCppAd /tmp/ocs2_quadruped_controller/a1 modelFolderCppAd /tmp/ocs2_quadruped_controller/x30
} }
swing_trajectory_config swing_trajectory_config
{ {
liftOffVelocity 0.05 liftOffVelocity 0.05
touchDownVelocity -0.1 touchDownVelocity -0.1
swingHeight 0.1 swingHeight 0.16
swingTimeScale 0.15 swingTimeScale 0.15
} }
@ -154,24 +154,24 @@ initialState
;; Base Pose: [position, orientation] ;; ;; Base Pose: [position, orientation] ;;
(6,0) 0.0 ; p_base_x (6,0) 0.0 ; p_base_x
(7,0) 0.0 ; p_base_y (7,0) 0.0 ; p_base_y
(8,0) 0.33 ; p_base_z (8,0) 0.54 ; p_base_z
(9,0) 0.0 ; theta_base_z (9,0) 0.0 ; theta_base_z
(10,0) 0.0 ; theta_base_y (10,0) 0.0 ; theta_base_y
(11,0) 0.0 ; theta_base_x (11,0) 0.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;; ;; Leg Joint Positions: [FL, HL, FR, HR] ;;
(12,0) -0.20 ; FL_hip_joint (12,0) 0.0 ; FL_HipX
(13,0) 0.72 ; FL_thigh_joint (13,0) -0.732 ; FL_HipY
(14,0) -1.44 ; FL_calf_joint (14,0) 1.361 ; FL_Knee
(15,0) -0.20 ; RL_hip_joint (15,0) 0.0 ; HL_HipX
(16,0) 0.72 ; RL_thigh_joint (16,0) -0.732 ; HL_HipY
(17,0) -1.44 ; RL_calf_joint (17,0) 1.361 ; HL_Knee
(18,0) 0.20 ; FR_hip_joint (18,0) -0.0 ; FR_HipX
(19,0) 0.72 ; FR_thigh_joint (19,0) -0.732 ; FR_HipY
(20,0) -1.44 ; FR_calf_joint (20,0) 1.361 ; FR_Knee
(21,0) 0.20 ; RR_hip_joint (21,0) -0.0 ; HR_HipX
(22,0) 0.72 ; RR_thigh_joint (22,0) -0.732 ; HR_HipY
(23,0) -1.44 ; RR_calf_joint (23,0) 1.361 ; HR_Knee
} }
; standard state weight matrix ; standard state weight matrix
@ -195,19 +195,19 @@ Q
(10,10) 300.0 ; theta_base_y (10,10) 300.0 ; theta_base_y
(11,11) 300.0 ; theta_base_x (11,11) 300.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;; ;; Leg Joint Positions: [FL, HL, FR, HR] ;;
(12,12) 5.0 ; FL_hip_joint (12,12) 5.0 ; FL_HipX
(13,13) 5.0 ; FL_thigh_joint (13,13) 5.0 ; FL_HipY
(14,14) 2.5 ; FL_calf_joint (14,14) 2.5 ; FL_Knee
(15,15) 5.0 ; RL_hip_joint (15,15) 5.0 ; HL_HipX
(16,16) 5.0 ; RL_thigh_joint (16,16) 5.0 ; HL_HipY
(17,17) 2.5 ; RL_calf_joint (17,17) 2.5 ; HL_Knee
(18,18) 5.0 ; FR_hip_joint (18,18) 5.0 ; FR_HipX
(19,19) 5.0 ; FR_thigh_joint (19,19) 5.0 ; FR_HipY
(20,20) 2.5 ; FR_calf_joint (20,20) 2.5 ; HR_Knee
(21,21) 5.0 ; RR_hip_joint (21,21) 5.0 ; HR_HipX
(22,22) 5.0 ; RR_thigh_joint (22,22) 5.0 ; HR_HipY
(23,23) 2.5 ; RR_calf_joint (23,23) 2.5 ; HR_Knee
} }
; control weight matrix ; control weight matrix
@ -215,7 +215,7 @@ R
{ {
scaling 1e-3 scaling 1e-3
;; Feet Contact Forces: [FL, FR, RL, RR] ;; ;; Feet Contact Forces: [FL, FR, HL, HR] ;;
(0,0) 1.0 ; front_left_force (0,0) 1.0 ; front_left_force
(1,1) 1.0 ; front_left_force (1,1) 1.0 ; front_left_force
(2,2) 1.0 ; front_left_force (2,2) 1.0 ; front_left_force
@ -229,7 +229,7 @@ R
(10,10) 1.0 ; rear_right_force (10,10) 1.0 ; rear_right_force
(11,11) 1.0 ; rear_right_force (11,11) 1.0 ; rear_right_force
;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;; ;; foot velocity relative to base: [FL, HL, FR, HR] (uses the Jacobian at nominal configuration) ;;
(12,12) 5000.0 ; x (12,12) 5000.0 ; x
(13,13) 5000.0 ; y (13,13) 5000.0 ; y
(14,14) 5000.0 ; z (14,14) 5000.0 ; z
@ -263,14 +263,14 @@ selfCollision
; Self Collision pairs ; Self Collision pairs
collisionLinkPairs collisionLinkPairs
{ {
[0] "FL_calf, FR_calf" [0] "FL_SHANK, FR_SHANK"
[1] "RL_calf, RR_calf" [1] "HL_SHANK, HR_SHANK"
[2] "FL_calf, RL_calf" [2] "FL_SHANK, HL_SHANK"
[3] "FR_calf, RR_calf" [3] "FR_SHANK, HR_SHANK"
[4] "FL_foot, FR_foot" [4] "FL_FOOT, FR_FOOT"
[5] "RL_foot, RR_foot" [5] "HL_FOOT, HR_FOOT"
[6] "FL_foot, RL_foot" [6] "FL_FOOT, HL_FOOT"
[7] "FR_foot, RR_foot" [7] "FR_FOOT, HR_FOOT"
} }
minimumDistance 0.05 minimumDistance 0.05
@ -283,9 +283,9 @@ selfCollision
; Whole body control ; Whole body control
torqueLimitsTask torqueLimitsTask
{ {
(0,0) 33.5 ; HAA (0,0) 84.0 ; Hip_X
(1,0) 33.5 ; HFE (1,0) 84.0 ; Hip_Y
(2,0) 33.5 ; KFE (2,0) 160.0 ; Knee
} }
frictionConeTask frictionConeTask
@ -309,7 +309,7 @@ weight
; State Estimation ; State Estimation
kalmanFilter kalmanFilter
{ {
footRadius 0.02 footRadius 0.0165
imuProcessNoisePosition 0.02 imuProcessNoisePosition 0.02
imuProcessNoiseVelocity 0.02 imuProcessNoiseVelocity 0.02
footProcessNoisePosition 0.002 footProcessNoisePosition 0.002

View File

@ -74,3 +74,62 @@ unitree_guide_controller:
- linear_acceleration.x - linear_acceleration.x
- linear_acceleration.y - linear_acceleration.y
- linear_acceleration.z - linear_acceleration.z
ocs2_quadruped_controller:
ros__parameters:
update_rate: 500 # Hz
default_kd: 8.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
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet:
- FL_FOOT
- FR_FOOT
- HL_FOOT
- HR_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
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- HL
- FR
- HR

File diff suppressed because it is too large Load Diff

View File

@ -58,7 +58,7 @@
<parent link="TORSO"/> <parent link="TORSO"/>
<child link="FL_HIP"/> <child link="FL_HIP"/>
<axis xyz="-1 0 0"/> <axis xyz="-1 0 0"/>
<limit lower="-0.323" upper="0.585" effort="84" velocity="17.5"/> <limit lower="-0.323" upper="0.323" effort="84" velocity="17.5"/>
</joint> </joint>
<link name="FL_THIGH"> <link name="FL_THIGH">
@ -126,7 +126,7 @@
<parent link="FL_THIGH" /> <parent link="FL_THIGH" />
<child link="FL_SHANK" /> <child link="FL_SHANK" />
<axis xyz="0 -1 0" /> <axis xyz="0 -1 0" />
<limit lower="0.349" upper="2.531" effort="180" velocity="16.1" /> <limit lower="0.349" upper="2.531" effort="160" velocity="16.1" />
</joint> </joint>
<link name="FL_FOOT"> <link name="FL_FOOT">
@ -166,7 +166,7 @@
<parent link="TORSO" /> <parent link="TORSO" />
<child link="FR_HIP" /> <child link="FR_HIP" />
<axis xyz="-1 0 0" /> <axis xyz="-1 0 0" />
<limit lower="-0.585" upper="0.323" effort="84" velocity="17.5" /> <limit lower="-0.323" upper="0.323" effort="84" velocity="17.5" />
</joint> </joint>
<link name="FR_THIGH"> <link name="FR_THIGH">
@ -234,7 +234,7 @@
<parent link="FR_THIGH" /> <parent link="FR_THIGH" />
<child link="FR_SHANK" /> <child link="FR_SHANK" />
<axis xyz="0 -1 0" /> <axis xyz="0 -1 0" />
<limit lower="0.349" upper="2.531" effort="180" velocity="16.1" /> <limit lower="0.349" upper="2.531" effort="160" velocity="16.1" />
</joint> </joint>
<link name="FR_FOOT"> <link name="FR_FOOT">
@ -274,7 +274,7 @@
<parent link="TORSO" /> <parent link="TORSO" />
<child link="HL_HIP" /> <child link="HL_HIP" />
<axis xyz="-1 0 0" /> <axis xyz="-1 0 0" />
<limit lower="-0.323" upper="0.585" effort="84" velocity="17.5" /> <limit lower="-0.323" upper="0.323" effort="84" velocity="17.5" />
</joint> </joint>
<link name="HL_THIGH"> <link name="HL_THIGH">
@ -342,7 +342,7 @@
<parent link="HL_THIGH" /> <parent link="HL_THIGH" />
<child link="HL_SHANK" /> <child link="HL_SHANK" />
<axis xyz="0 -1 0" /> <axis xyz="0 -1 0" />
<limit lower="0.349" upper="2.531" effort="180" velocity="16.1" /> <limit lower="0.349" upper="2.531" effort="160" velocity="16.1" />
</joint> </joint>
<link name="HL_FOOT"> <link name="HL_FOOT">
@ -382,7 +382,7 @@
<parent link="TORSO" /> <parent link="TORSO" />
<child link="HR_HIP" /> <child link="HR_HIP" />
<axis xyz="-1 0 0" /> <axis xyz="-1 0 0" />
<limit lower="-0.585" upper="0.323" effort="84" velocity="17.5" /> <limit lower="-0.323" upper="0.323" effort="84" velocity="17.5" />
</joint> </joint>
<link name="HR_THIGH"> <link name="HR_THIGH">
@ -450,7 +450,7 @@
<parent link="HR_THIGH" /> <parent link="HR_THIGH" />
<child link="HR_SHANK" /> <child link="HR_SHANK" />
<axis xyz="0 -1 0" /> <axis xyz="0 -1 0" />
<limit lower="0.349" upper="2.531" effort="180" velocity="16.1" /> <limit lower="0.349" upper="2.531" effort="160" velocity="16.1" />
</joint> </joint>
<link name="HR_FOOT"> <link name="HR_FOOT">