Apollo3D/Gym_CPU
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update Walk.py

Browse Source
This commit is contained in:
xxh
2026-04-18 10:50:53 -04:00
parent 29adc4b3df
commit 2ff69d64be
1 changed files with 62 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
scripts/gyms/Walk.py
View File

@@ -165,8 +165,8 @@ class WalkEnv(gym.Env):
self.enable_reset_perturb = False self.enable_reset_perturb = False
self.reset_beam_yaw_range_deg = 180.0 self.reset_beam_yaw_range_deg = 180.0
self.reset_target_bearing_range_deg = 0.0 self.reset_target_bearing_range_deg = 0.0
self.reset_target_distance_min = 5 self.reset_target_distance_min = 1.5
self.reset_target_distance_max = 10 self.reset_target_distance_max = 3.0
if self.reset_target_distance_min > self.reset_target_distance_max: if self.reset_target_distance_min > self.reset_target_distance_max:
self.reset_target_distance_min, self.reset_target_distance_max = ( self.reset_target_distance_min, self.reset_target_distance_max = (
self.reset_target_distance_max, self.reset_target_distance_max,
@@ -181,7 +181,7 @@ class WalkEnv(gym.Env):
self.reward_forward_stability_gate = 0.35 self.reward_forward_stability_gate = 0.35
self.reward_forward_tilt_hard_threshold = 0.50 self.reward_forward_tilt_hard_threshold = 0.50
self.reward_forward_tilt_hard_scale = 0.20 self.reward_forward_tilt_hard_scale = 0.20
self.reward_head_toward_bonus = 1.0 self.reward_head_toward_bonus = 0.8
self.turn_stationary_radius = 0.2 self.turn_stationary_radius = 0.2
self.turn_stationary_penalty_scale = 3.0 self.turn_stationary_penalty_scale = 3.0
self.stationary_start_steps = 20 self.stationary_start_steps = 20
@@ -222,29 +222,35 @@ class WalkEnv(gym.Env):
self.reward_knee_explore_scale = 0.03 self.reward_knee_explore_scale = 0.03
self.reward_knee_explore_delta_scale = 0.03 self.reward_knee_explore_delta_scale = 0.03
self.reward_knee_explore_cap = 0.10 self.reward_knee_explore_cap = 0.10
self.reward_hip_pitch_explore_scale = 0.07 self.reward_hip_pitch_explore_scale = 0.03
self.reward_hip_pitch_explore_delta_scale = 0.07 self.reward_hip_pitch_explore_delta_scale = 0.03
self.reward_hip_pitch_explore_cap = 0.10 self.reward_hip_pitch_explore_cap = 0.05
self.reward_progress_scale = 18 self.reward_progress_scale = 22.0
self.reward_survival_scale = 0.5 self.reward_survival_scale = 0.3
self.reward_idle_penalty_scale = 0.6 self.reward_idle_penalty_scale = 0.6
self.reward_accel_penalty_scale = 0.08 self.reward_accel_penalty_scale = 0.05
self.reward_accel_penalty_cap = 0.40 self.reward_accel_penalty_cap = 0.25
self.reward_accel_abs_limit = 13.5 self.reward_accel_abs_limit = 13.5
self.reward_accel_abs_penalty_scale = 0.05 self.reward_accel_abs_penalty_scale = 0.04
self.reward_accel_abs_penalty_cap = 0.40 self.reward_accel_abs_penalty_cap = 0.30
self.reward_heading_align_scale = 0.28 self.reward_heading_align_scale = 0.28
self.reward_heading_error_scale = 0.05 self.reward_heading_error_scale = 0.05
self.reward_progress_tilt_gate_start = 0.28 self.reward_progress_tilt_gate_start = 0.20
self.reward_progress_knee_gate_min = 0.16 self.reward_progress_knee_gate_min = 0.16
self.reward_progress_hip_gate_over = 0.18 self.reward_progress_hip_gate_over = 0.18
self.reward_progress_gate_floor = 0.25 self.reward_progress_gate_floor = 0.10
self.reward_height_deadband = 0.02
self.reward_height_penalty_scale = 10.0
self.reward_height_penalty_cap = 1.2
self.reward_forward_lean_threshold = 0.20
self.reward_forward_lean_penalty_scale = 0.9
self.reward_forward_lean_penalty_cap = 0.7
self.reward_knee_straight_threshold = 0.18 self.reward_knee_straight_threshold = 0.18
self.reward_knee_straight_penalty_scale = 0.45 self.reward_knee_straight_penalty_scale = 0.70
self.reward_hip_overextend_threshold = 0.9 self.reward_hip_overextend_threshold = 0.95
self.reward_hip_overextend_penalty_scale = 1 self.reward_hip_overextend_penalty_scale = 1.30
self.reward_leg_stretch_penalty_scale = 0.35 self.reward_leg_stretch_penalty_scale = 1.20
self.reward_stretch_lean_combo_scale = 0.55 self.reward_stretch_lean_combo_scale = 1.40
self.previous_action = np.zeros(len(self.Player.robot.ROBOT_MOTORS)) self.previous_action = np.zeros(len(self.Player.robot.ROBOT_MOTORS))
self.last_action_for_reward = np.zeros(len(self.Player.robot.ROBOT_MOTORS)) self.last_action_for_reward = np.zeros(len(self.Player.robot.ROBOT_MOTORS))
@@ -540,6 +546,8 @@ class WalkEnv(gym.Env):
left_ankle_roll = -float(joint_pos[16]) left_ankle_roll = -float(joint_pos[16])
right_ankle_roll = float(joint_pos[22]) right_ankle_roll = float(joint_pos[22])
left_ankle_pitch = -float(joint_pos[15])
right_ankle_pitch = float(joint_pos[21])
left_knee_flex = abs(float(joint_pos[14])) left_knee_flex = abs(float(joint_pos[14]))
right_knee_flex = abs(float(joint_pos[20])) right_knee_flex = abs(float(joint_pos[20]))
avg_knee_flex = 0.5 * (left_knee_flex + right_knee_flex) avg_knee_flex = 0.5 * (left_knee_flex + right_knee_flex)
@@ -555,6 +563,7 @@ class WalkEnv(gym.Env):
# 脚踝roll角度检测：防止过度外翻或内翻 # 脚踝roll角度检测：防止过度外翻或内翻
max_ankle_roll = 0.15 # 最大允许的脚踝roll角度 max_ankle_roll = 0.15 # 最大允许的脚踝roll角度
max_ankle_pitch = 0.25 # 最大允许的脚踝pitch角度
# 惩罚脚踝过度外翻/内翻（绝对值过大） # 惩罚脚踝过度外翻/内翻（绝对值过大）
ankle_roll_penalty = -0.12 * max(0.0, (abs(left_ankle_roll) + abs(right_ankle_roll) - 2 * max_ankle_roll) / max_ankle_roll) ankle_roll_penalty = -0.12 * max(0.0, (abs(left_ankle_roll) + abs(right_ankle_roll) - 2 * max_ankle_roll) / max_ankle_roll)
@@ -562,6 +571,8 @@ class WalkEnv(gym.Env):
# 惩罚两脚踝roll方向相反（不稳定姿势） # 惩罚两脚踝roll方向相反（不稳定姿势）
ankle_roll_cross_penalty = -0.12 * max(0.0, -(left_ankle_roll * right_ankle_roll)) ankle_roll_cross_penalty = -0.12 * max(0.0, -(left_ankle_roll * right_ankle_roll))
ankle_pitch_penalty = -0.12 * max(0.0, (abs(left_ankle_pitch) + abs(right_ankle_pitch) - 2 * max_ankle_pitch) / max_ankle_pitch)
# 分别惩罚左右大腿过度转动 # 分别惩罚左右大腿过度转动
max_hip_yaw = 0.2 # 最大允许的yaw角度 max_hip_yaw = 0.2 # 最大允许的yaw角度
left_hip_yaw_penalty = -0.6 * max(0.0, abs(left_hip_yaw) - max_hip_yaw) left_hip_yaw_penalty = -0.6 * max(0.0, abs(left_hip_yaw) - max_hip_yaw)
@@ -658,27 +669,36 @@ class WalkEnv(gym.Env):
arrival_bonus = 0.0 arrival_bonus = 0.0
target_height = self.initial_height target_height = self.initial_height
height_error = height - target_height
height_error = height - target_height height_error = height - target_height
# Keep height shaping smooth and bounded to avoid exploding negatives.
height_penalty = -0.5 * (math.exp(15*abs(height_error))-1) height_dev = max(0.0, abs(height_error) - self.reward_height_deadband)
height_penalty = -min(
self.reward_height_penalty_cap,
self.reward_height_penalty_scale * (height_dev ** 2),
)
orientation_quat_inv = R.from_quat(robot._global_cheat_orientation).inv() orientation_quat_inv = R.from_quat(robot._global_cheat_orientation).inv()
projected_gravity = orientation_quat_inv.apply(np.array([0.0, 0.0, -1.0])) projected_gravity = orientation_quat_inv.apply(np.array([0.0, 0.0, -1.0]))
tilt_mag = float(np.linalg.norm(projected_gravity[:2])) tilt_mag = float(np.linalg.norm(projected_gravity[:2]))
pitch_lean = abs(float(projected_gravity[0]))
forward_lean_excess = max(0.0, pitch_lean - self.reward_forward_lean_threshold)
forward_lean_penalty = -min(
self.reward_forward_lean_penalty_cap,
self.reward_forward_lean_penalty_scale * (forward_lean_excess ** 2),
)
# Gate progress reward when posture quality is poor. # Gate progress reward when posture quality is poor.
# Important: include leg overextension so upright torso cannot exploit progress reward. # Important: include leg overextension so upright torso cannot exploit progress reward.
tilt_excess = max(0.0, tilt_mag - self.reward_progress_tilt_gate_start) tilt_excess = max(0.0, tilt_mag - self.reward_progress_tilt_gate_start)
knee_gate_excess = max(0.0, self.reward_progress_knee_gate_min - avg_knee_flex) knee_gate_excess = max(0.0, self.reward_progress_knee_gate_min - avg_knee_flex)
left_hip_pitch = float(joint_pos[11]) left_hip_pitch = float(joint_pos[11])
right_hip_pitch = float(joint_pos[17]) right_hip_pitch = -float(joint_pos[17])
left_hip_over = max(0.0, abs(left_hip_pitch) - self.reward_hip_overextend_threshold) left_hip_over = max(0.0, abs(left_hip_pitch) - self.reward_hip_overextend_threshold)
right_hip_over = max(0.0, abs(right_hip_pitch) - self.reward_hip_overextend_threshold) right_hip_over = max(0.0, abs(right_hip_pitch) - self.reward_hip_overextend_threshold)
hip_over_mean = 0.5 * (left_hip_over + right_hip_over) hip_over_mean = 0.5 * (left_hip_over + right_hip_over)
hip_gate_excess = max(0.0, hip_over_mean - self.reward_progress_hip_gate_over) hip_gate_excess = max(0.0, hip_over_mean - self.reward_progress_hip_gate_over)
posture_gate = 1.0 - 1.2 * tilt_excess - 2.0 * knee_gate_excess - 1.8 * hip_gate_excess posture_gate = 1.0 - 1.2 * tilt_excess - 2.2 * knee_gate_excess - 2.4 * hip_gate_excess
posture_gate = float(np.clip(posture_gate, self.reward_progress_gate_floor, 1.0)) posture_gate = float(np.clip(posture_gate, self.reward_progress_gate_floor, 1.0))
progress_reward = progress_reward_raw * posture_gate progress_reward = progress_reward_raw * posture_gate
@@ -706,21 +726,23 @@ class WalkEnv(gym.Env):
+ split_penalty + split_penalty
+ inward_penalty + inward_penalty
+ ankle_roll_penalty + ankle_roll_penalty
+ ankle_pitch_penalty
+ ankle_roll_cross_penalty + ankle_roll_cross_penalty
+ left_hip_yaw_penalty + left_hip_yaw_penalty
+ right_hip_yaw_penalty + right_hip_yaw_penalty
+ heading_align_reward + heading_align_reward
+ heading_error_penalty + heading_error_penalty
# + knee_straight_penalty + knee_straight_penalty
+ hip_overextend_penalty + hip_overextend_penalty
+ leg_stretch_penalty + leg_stretch_penalty
+ stretch_lean_combo_penalty + stretch_lean_combo_penalty
# + exploration_bonus # + exploration_bonus
# + knee_explore_reward # + knee_explore_reward
# + knee_lift_shortfall_penalty # + knee_lift_shortfall_penalty
# + hip_pitch_explore_reward + hip_pitch_explore_reward
+ arrival_bonus + arrival_bonus
+ height_penalty + height_penalty
+ forward_lean_penalty
+ posture_penalty + posture_penalty
) )
@@ -744,6 +766,7 @@ class WalkEnv(gym.Env):
f"split_penalty:{split_penalty:.4f}," f"split_penalty:{split_penalty:.4f},"
f"inward_penalty:{inward_penalty:.4f}," f"inward_penalty:{inward_penalty:.4f},"
f"ankle_roll_penalty:{ankle_roll_penalty:.4f}," f"ankle_roll_penalty:{ankle_roll_penalty:.4f},"
f"ankle_pitch_penalty:{ankle_pitch_penalty:.4f},"
f"ankle_roll_cross_penalty:{ankle_roll_cross_penalty:.4f}," f"ankle_roll_cross_penalty:{ankle_roll_cross_penalty:.4f},"
f"left_hip_yaw_penalty:{left_hip_yaw_penalty:.4f}," f"left_hip_yaw_penalty:{left_hip_yaw_penalty:.4f},"
f"right_hip_yaw_penalty:{right_hip_yaw_penalty:.4f}," f"right_hip_yaw_penalty:{right_hip_yaw_penalty:.4f},"
@@ -757,10 +780,11 @@ class WalkEnv(gym.Env):
f"progress_reward_raw:{progress_reward_raw:.4f}," f"progress_reward_raw:{progress_reward_raw:.4f},"
# f"exploration_bonus:{exploration_bonus:.4f}," # f"exploration_bonus:{exploration_bonus:.4f},"
f"height_penalty:{height_penalty:.4f}," f"height_penalty:{height_penalty:.4f},"
f"forward_lean_penalty:{forward_lean_penalty:.4f},"
# f"knee_explore_reward:{knee_explore_reward:.4f}," # f"knee_explore_reward:{knee_explore_reward:.4f},"
f"posture_penalty:{posture_penalty:.4f}," f"posture_penalty:{posture_penalty:.4f},"
# f"knee_lift_shortfall_penalty:{knee_lift_shortfall_penalty:.4f}," # f"knee_lift_shortfall_penalty:{knee_lift_shortfall_penalty:.4f},"
# f"hip_pitch_explore_reward:{hip_pitch_explore_reward:.4f}," f"hip_pitch_explore_reward:{hip_pitch_explore_reward:.4f},"
f"arrival_bonus:{arrival_bonus:.4f}," f"arrival_bonus:{arrival_bonus:.4f},"
f"total:{total:.4f}" f"total:{total:.4f}"
) )
@@ -790,8 +814,8 @@ class WalkEnv(gym.Env):
action[20] = np.clip(action[20], -10.0, 0) action[20] = np.clip(action[20], -10.0, 0)
# action[14] = 1 # the correct left knee sign # action[14] = 1 # the correct left knee sign
# action[20] = -1 # the correct right knee sign # action[20] = -1 # the correct right knee sign
# action[11] = 2 action[11] = np.clip(action[11], -8, 8)
# action[17] = -2 action[17] = np.clip(action[17], -8, 8)
# action[12] = -1 # action[12] = -1
# action[18] = 1 # action[18] = 1
# action[13] = -1.0 # action[13] = -1.0
@@ -860,18 +884,18 @@ class Train(Train_Base):
# --------------------------------------- Learning parameters # --------------------------------------- Learning parameters
n_envs = 20 n_envs = 20
server_warmup_sec = 3.0 server_warmup_sec = 3.0
n_steps_per_env = 256 # RolloutBuffer is of size (n_steps_per_env * n_envs) n_steps_per_env = 1024 # RolloutBuffer is of size (n_steps_per_env * n_envs)
minibatch_size = 512 # should be a factor of (n_steps_per_env * n_envs) minibatch_size = 1024 # should be a factor of (n_steps_per_env * n_envs)
total_steps = 90000000 total_steps = 18000000
learning_rate = 2e-4 learning_rate = 3e-4
ent_coef = 0.035 ent_coef = 0.05
clip_range = 0.2 clip_range = 0.2
gamma = 0.97 gamma = 0.99
n_epochs = 3 n_epochs = 5
enable_eval = True enable_eval = True
monitor_train_env = False monitor_train_env = False
eval_freq_mult = 60 eval_freq_mult = 15
save_freq_mult = 60 save_freq_mult = 15
eval_eps = 7 eval_eps = 7
folder_name = f'Walk_R{self.robot_type}' folder_name = f'Walk_R{self.robot_type}'
model_path = f'./scripts/gyms/logs/{folder_name}/' model_path = f'./scripts/gyms/logs/{folder_name}/'