diff --git a/scripts/gyms/Walk.py b/scripts/gyms/Walk.py index 1fb8430..21926c3 100755 --- a/scripts/gyms/Walk.py +++ b/scripts/gyms/Walk.py @@ -165,8 +165,8 @@ class WalkEnv(gym.Env): self.enable_reset_perturb = False self.reset_beam_yaw_range_deg = 180.0 self.reset_target_bearing_range_deg = 0.0 - self.reset_target_distance_min = 5 - self.reset_target_distance_max = 10 + self.reset_target_distance_min = 1.5 + self.reset_target_distance_max = 3.0 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_max, @@ -181,7 +181,7 @@ class WalkEnv(gym.Env): self.reward_forward_stability_gate = 0.35 self.reward_forward_tilt_hard_threshold = 0.50 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_penalty_scale = 3.0 self.stationary_start_steps = 20 @@ -222,29 +222,35 @@ class WalkEnv(gym.Env): self.reward_knee_explore_scale = 0.03 self.reward_knee_explore_delta_scale = 0.03 self.reward_knee_explore_cap = 0.10 - self.reward_hip_pitch_explore_scale = 0.07 - self.reward_hip_pitch_explore_delta_scale = 0.07 - self.reward_hip_pitch_explore_cap = 0.10 - self.reward_progress_scale = 18 - self.reward_survival_scale = 0.5 + self.reward_hip_pitch_explore_scale = 0.03 + self.reward_hip_pitch_explore_delta_scale = 0.03 + self.reward_hip_pitch_explore_cap = 0.05 + self.reward_progress_scale = 22.0 + self.reward_survival_scale = 0.3 self.reward_idle_penalty_scale = 0.6 - self.reward_accel_penalty_scale = 0.08 - self.reward_accel_penalty_cap = 0.40 + self.reward_accel_penalty_scale = 0.05 + self.reward_accel_penalty_cap = 0.25 self.reward_accel_abs_limit = 13.5 - self.reward_accel_abs_penalty_scale = 0.05 - self.reward_accel_abs_penalty_cap = 0.40 + self.reward_accel_abs_penalty_scale = 0.04 + self.reward_accel_abs_penalty_cap = 0.30 self.reward_heading_align_scale = 0.28 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_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_penalty_scale = 0.45 - self.reward_hip_overextend_threshold = 0.9 - self.reward_hip_overextend_penalty_scale = 1 - self.reward_leg_stretch_penalty_scale = 0.35 - self.reward_stretch_lean_combo_scale = 0.55 + self.reward_knee_straight_penalty_scale = 0.70 + self.reward_hip_overextend_threshold = 0.95 + self.reward_hip_overextend_penalty_scale = 1.30 + self.reward_leg_stretch_penalty_scale = 1.20 + self.reward_stretch_lean_combo_scale = 1.40 self.previous_action = 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]) 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])) right_knee_flex = abs(float(joint_pos[20])) avg_knee_flex = 0.5 * (left_knee_flex + right_knee_flex) @@ -555,6 +563,7 @@ class WalkEnv(gym.Env): # 脚踝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) @@ -562,6 +571,8 @@ class WalkEnv(gym.Env): # 惩罚两脚踝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角度 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 target_height = self.initial_height - height_error = height - target_height height_error = height - target_height - - height_penalty = -0.5 * (math.exp(15*abs(height_error))-1) + # Keep height shaping smooth and bounded to avoid exploding negatives. + 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() projected_gravity = orientation_quat_inv.apply(np.array([0.0, 0.0, -1.0])) 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. # Important: include leg overextension so upright torso cannot exploit progress reward. 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) 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) 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_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)) progress_reward = progress_reward_raw * posture_gate @@ -706,21 +726,23 @@ class WalkEnv(gym.Env): + split_penalty + inward_penalty + ankle_roll_penalty + + ankle_pitch_penalty + ankle_roll_cross_penalty + left_hip_yaw_penalty + right_hip_yaw_penalty + heading_align_reward + heading_error_penalty - # + knee_straight_penalty + + knee_straight_penalty + hip_overextend_penalty + leg_stretch_penalty + stretch_lean_combo_penalty # + exploration_bonus # + knee_explore_reward # + knee_lift_shortfall_penalty - # + hip_pitch_explore_reward + + hip_pitch_explore_reward + arrival_bonus + height_penalty + + forward_lean_penalty + posture_penalty ) @@ -744,6 +766,7 @@ class WalkEnv(gym.Env): f"split_penalty:{split_penalty:.4f}," f"inward_penalty:{inward_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"left_hip_yaw_penalty:{left_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"exploration_bonus:{exploration_bonus:.4f}," f"height_penalty:{height_penalty:.4f}," + f"forward_lean_penalty:{forward_lean_penalty:.4f}," # f"knee_explore_reward:{knee_explore_reward:.4f}," f"posture_penalty:{posture_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"total:{total:.4f}" ) @@ -790,8 +814,8 @@ class WalkEnv(gym.Env): action[20] = np.clip(action[20], -10.0, 0) # action[14] = 1 # the correct left knee sign # action[20] = -1 # the correct right knee sign - # action[11] = 2 - # action[17] = -2 + action[11] = np.clip(action[11], -8, 8) + action[17] = np.clip(action[17], -8, 8) # action[12] = -1 # action[18] = 1 # action[13] = -1.0 @@ -860,18 +884,18 @@ class Train(Train_Base): # --------------------------------------- Learning parameters n_envs = 20 server_warmup_sec = 3.0 - n_steps_per_env = 256 # RolloutBuffer is of size (n_steps_per_env * n_envs) - minibatch_size = 512 # should be a factor of (n_steps_per_env * n_envs) - total_steps = 90000000 - learning_rate = 2e-4 - ent_coef = 0.035 + n_steps_per_env = 1024 # RolloutBuffer is of size (n_steps_per_env * n_envs) + minibatch_size = 1024 # should be a factor of (n_steps_per_env * n_envs) + total_steps = 18000000 + learning_rate = 3e-4 + ent_coef = 0.05 clip_range = 0.2 - gamma = 0.97 - n_epochs = 3 + gamma = 0.99 + n_epochs = 5 enable_eval = True monitor_train_env = False - eval_freq_mult = 60 - save_freq_mult = 60 + eval_freq_mult = 15 + save_freq_mult = 15 eval_eps = 7 folder_name = f'Walk_R{self.robot_type}' model_path = f'./scripts/gyms/logs/{folder_name}/'