update Walk.py and update model of Walk version 0.2.0

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 = 1.1
+        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 = 180000000
+        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}/'