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

turn_around training history

Browse Source
This commit is contained in:
xxh
2026-03-28 05:55:43 -04:00
parent 8ab57840ba
commit 05db95385d
19 changed files with 13349 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

267
scripts/gyms/Walk.py
View File

@@ -53,6 +53,10 @@ class WalkEnv(gym.Env):
self.route_completed = False
self.debug_every_n_steps = 5
self.enable_debug_joint_status = False
self.reward_debug_interval_sec = float(os.environ.get("GYM_CPU_REWARD_DEBUG_INTERVAL_SEC", "600"))
self.reward_debug_burst_steps = int(os.environ.get("GYM_CPU_REWARD_DEBUG_BURST_STEPS", "10"))
self._reward_debug_last_time = time.time()
self._reward_debug_steps_left = 0
self.calibrate_nominal_from_neutral = True
self.auto_calibrate_train_sim_flip = True
self.nominal_calibrated_once = False
@@ -154,11 +158,23 @@ class WalkEnv(gym.Env):
# Small reset perturbations for robustness training.
self.enable_reset_perturb = False
self.reset_beam_yaw_range_deg = 180 # randomize target direction fully to encourage learning a real walk instead of a fixed gait
self.reset_beam_yaw_range_deg = float(os.environ.get("GYM_CPU_RESET_BEAM_YAW_RANGE_DEG", "180"))
self.reset_target_bearing_range_deg = float(os.environ.get("GYM_CPU_RESET_TARGET_BEARING_RANGE_DEG", "45"))
self.reset_target_distance_min = float(os.environ.get("GYM_CPU_RESET_TARGET_DISTANCE_MIN", "1.2"))
self.reset_target_distance_max = float(os.environ.get("GYM_CPU_RESET_TARGET_DISTANCE_MAX", "2.8"))
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,
self.reset_target_distance_min,
)
self.reset_joint_noise_rad = 0.025
self.reset_perturb_steps = 4
self.reset_recover_steps = 8
self.reward_smoothness_scale = float(os.environ.get("GYM_CPU_REWARD_SMOOTHNESS_SCALE", "0.06"))
self.reward_smoothness_cap = float(os.environ.get("GYM_CPU_REWARD_SMOOTHNESS_CAP", "0.45"))
self.reward_head_toward_bonus = float(os.environ.get("GYM_CPU_REWARD_HEAD_TOWARD_BONUS", "0.7"))
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.previous_pos = np.array([0.0, 0.0]) # Track previous position
@@ -317,8 +333,8 @@ class WalkEnv(gym.Env):
if seed is not None:
np.random.seed(seed)
target_distance = np.random.uniform(1.2, 2.8)
target_bearing_deg = np.random.uniform(-180.0, 180.0)
target_distance = np.random.uniform(self.reset_target_distance_min, self.reset_target_distance_max)
target_bearing_deg = np.random.uniform(-self.reset_target_bearing_range_deg, self.reset_target_bearing_range_deg)
self.step_counter = 0
self.waypoint_index = 0
@@ -328,6 +344,7 @@ class WalkEnv(gym.Env):
self.previous_pos = np.array([0.0, 0.0]) # Initialize for first step
self.last_yaw_error = None
self.walk_cycle_step = 0
self._reward_debug_steps_left = 0
# 随机 beam 目标位置和朝向，增加训练多样性
beam_x = (random() - 0.5) * 10
@@ -403,17 +420,24 @@ class WalkEnv(gym.Env):
height = float(self.Player.world.global_position[2])
robot = self.Player.robot
joint_pos_rad = np.deg2rad(
[robot.motor_positions[motor] for motor in robot.ROBOT_MOTORS]
)
joint_speed_rad = np.deg2rad(
[robot.motor_speeds[motor] for motor in robot.ROBOT_MOTORS]
)
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]))
ang_vel = np.deg2rad(robot.gyroscope)
ang_vel_mag = float(np.linalg.norm(ang_vel))
rp_ang_vel_mag = float(np.linalg.norm(ang_vel[:2]))
is_fallen = height < 0.55
if is_fallen:
# remain = max(0, 800 - self.step_counter)
# return -8.0 - 0.01 * remain
return -2
# is_fallen = height < 0.55
# if is_fallen:
# remain = max(0, 800 - self.step_counter)
# # Strong terminal penalty discourages risky turn-and-fall behaviors.
# return -1
@@ -428,48 +452,20 @@ class WalkEnv(gym.Env):
# forward_step = float(np.dot(delta_pos, forward_dir))
# lateral_step = float(np.linalg.norm(delta_pos - forward_dir * forward_step))
alive_bonus = 0.5
# Keep reward simple: turn correctly, stay stable, avoid jerky actions.
delta_action_norm = float(np.linalg.norm(action - self.last_action_for_reward))
# Cap smoothness penalty so it regularizes behavior without dominating total reward.
smoothness_penalty = -min(self.reward_smoothness_cap, self.reward_smoothness_scale * delta_action_norm)
# action_penalty = -0.01 * float(np.linalg.norm(action))
smoothness_penalty = -0.06 * float(np.linalg.norm(action - self.last_action_for_reward))
posture_penalty = -0.45 * tilt_mag
# Penalize roll/pitch rotational shake but do not penalize yaw turning directly.
ang_vel_penalty = -0.04 * rp_ang_vel_mag
posture_penalty = -0.5 * (tilt_mag)
ang_vel_penalty = -0.04 * ang_vel_mag
# Turn-to-target shaping.
to_target = self.target_position - current_pos
dist_to_target = float(np.linalg.norm(to_target))
if dist_to_target > 1e-6:
target_yaw = math.atan2(float(to_target[1]), float(to_target[0]))
else:
target_yaw = 0.0
robot_yaw = math.radians(float(robot.global_orientation_euler[2]))
yaw_error = self._wrap_to_pi(target_yaw - robot_yaw)
# Dense alignment reward in [-1, 1], max when facing target.
heading_align_reward = 1.6 * math.cos(yaw_error)
# Reward reducing heading error across consecutive control steps.
if self.last_yaw_error is None:
heading_progress_reward = 0.0
else:
heading_progress_reward = 1.2 * (abs(self.last_yaw_error) - abs(yaw_error))
self.last_yaw_error = yaw_error
# Encourage yaw rotation in the correct direction while far from alignment.
yaw_rate = float(np.deg2rad(robot.gyroscope[2]))
turn_dir = float(np.sign(yaw_error))
turn_cap = max(0.03, 0.08 * abs(yaw_error))
turn_rate_reward = float(np.clip(0.35 * turn_dir * yaw_rate, -turn_cap, turn_cap)) if abs(yaw_error) > math.radians(10.0) else 0.0
# Small bonus for holding a good heading; prevents oscillation near target angle.
heading_hold_bonus = 0.25 if abs(yaw_error) < math.radians(10.0) else 0.0
# Use simulator joint readings in training frame to shape lateral stance.
joint_pos = np.deg2rad(
[robot.motor_positions[motor] for motor in robot.ROBOT_MOTORS]
) * self.train_sim_flip
left_hip_roll = float(joint_pos[12])
right_hip_roll = float(joint_pos[18])
left_ankle_roll = float(joint_pos[16])
@@ -483,59 +479,140 @@ class WalkEnv(gym.Env):
stance_collapse_penalty = -4.0 * max(0.0, self.min_stance_rad - stance_metric)
cross_leg_penalty = -1.2 * max(0.0, -(hip_spread * ankle_spread))
target_height = self.initial_height
height_error = height - target_height
height_penalty = -0.5 * abs(height_error) # 惩罚高度偏离，系数可调
# # 在 compute_reward 中
# if self.step_counter > 50:
# avg_prev_action = np.mean(self.prev_action_history, axis=0)
# novelty = float(np.linalg.norm(action - avg_prev_action))
# exploration_bonus = 0.05 * novelty
# else:
# exploration_bonus = 0
# Torso-lower-body linkage: reward coordinated turning, punish waist-only spinning.
waist_speed = abs(float(joint_speed_rad[10]))
lower_body_speed = float(np.mean(np.abs(joint_speed_rad[11:23])))
lower_body_follow_ratio = lower_body_speed / (waist_speed + 1e-4)
linkage_reward = 0.24 * min(1.0, lower_body_follow_ratio) * min(1.0, waist_speed / 1.2)
waist_only_turn_penalty = -0.20 * max(0.0, waist_speed - 1.35 * lower_body_speed)
# self.prev_action_history[self.history_idx] = action
# self.history_idx = (self.history_idx + 1) % 50
# Extra posture linkage in yaw joints to avoid decoupled torso twist.
waist_yaw = abs(float(joint_pos_rad[10]))
hip_yaw_mean = 0.5 * (abs(float(joint_pos_rad[13])) + abs(float(joint_pos_rad[19])))
yaw_link_reward = 0.12 * math.exp(-abs(waist_yaw - hip_yaw_mean) / 0.22)
# Turn-to-target shaping.
to_target = self.target_position - current_pos
dist_to_target = float(np.linalg.norm(to_target))
if dist_to_target > 1e-6:
target_yaw = math.atan2(float(to_target[1]), float(to_target[0]))
else:
target_yaw = 0.0
robot_yaw = math.radians(float(robot.global_orientation_euler[2]))
yaw_error = self._wrap_to_pi(target_yaw - robot_yaw)
# Main heading objective: face the target direction.
# heading_align_reward = 1.0 * math.cos(yaw_error)
abs_yaw_error = abs(yaw_error)
# Reward reducing heading error between consecutive steps.
# Use a deadzone and smaller gain to avoid high-frequency jitter near alignment.
if self.last_yaw_error is None:
heading_progress_reward = 0.0
else:
prev_abs_yaw_error = abs(self.last_yaw_error)
yaw_err_delta = prev_abs_yaw_error - abs_yaw_error
progress_gate = 1.0 if abs_yaw_error > math.radians(4.0) else 0.0
heading_progress_reward = 0.30 * progress_gate * yaw_err_delta
heading_progress_reward = float(np.clip(heading_progress_reward, -0.12, 0.12))
self.last_yaw_error = yaw_error
yaw_rate = float(np.deg2rad(robot.gyroscope[2]))
yaw_rate_abs = abs(yaw_rate)
turn_dir = float(np.sign(yaw_error))
# Continuous turn shaping prevents reward discontinuity near small heading error.
turn_gate = min(1.0, abs_yaw_error / math.radians(45.0))
turn_rate_reward = 0.45 * turn_gate * math.tanh(2.0 * turn_dir * yaw_rate)
head_toward_bonus = self.reward_head_toward_bonus if abs_yaw_error < math.radians(8.0) else 0.0
# After roughly aligning with target, prioritize standing stability over continued aggressive turning.
aligned_gate = max(0.0, 1.0 - abs_yaw_error / math.radians(18.0))
post_turn_ang_vel_penalty = -0.10 * aligned_gate * min(rp_ang_vel_mag, math.radians(60.0))
lower_body_speed_mag = float(np.mean(np.abs(joint_speed_rad[11:23])))
post_turn_pose_bonus = 0.30 * aligned_gate * math.exp(-tilt_mag / 0.20) * math.exp(-lower_body_speed_mag / 1.10)
# Keep feet separation when aligned so robot does not collapse stance after turning.
aligned_stance_bonus = 0.10 * aligned_gate * min(1.0, stance_metric / max(self.min_stance_rad, 1e-4))
# Once roughly aligned, damp yaw oscillation and reward keeping a stable stance.
anti_oscillation_penalty = -0.08 * min(yaw_rate_abs, math.radians(35.0)) if abs_yaw_error < math.radians(7.0) else 0.0
stabilize_bonus = 0.45 if (
abs_yaw_error < math.radians(12.0)
and yaw_rate_abs < math.radians(10.0)
and tilt_mag < 0.28
) else 0.0
alive_bonus = max(0.5, 1.5 * math.cos(yaw_error)) # Encourage facing target, but give some baseline reward for not falling even if not facing target yet.
total = (
# progress_reward +
alive_bonus +
# lateral_penalty +
# action_penalty +
smoothness_penalty +
posture_penalty
+ ang_vel_penalty
+ height_penalty
+ stance_collapse_penalty
+ cross_leg_penalty
+ heading_align_reward
+ heading_progress_reward
+ turn_rate_reward
+ heading_hold_bonus
# + exploration_bonus
# + height_down_penalty
)
if time.time() - self.start_time >= 600:
self.start_time = time.time()
print(
# f"progress_reward:{progress_reward:.4f}",
# f"lateral_penalty:{lateral_penalty:.4f}",
# f"action_penalty:{action_penalty:.4f}"s,
f"height_penalty:{height_penalty:.4f}",
f"smoothness_penalty:{smoothness_penalty:.4f},",
f"posture_penalty:{posture_penalty:.4f}",
f"stance_collapse_penalty:{stance_collapse_penalty:.4f}",
f"cross_leg_penalty:{cross_leg_penalty:.4f}",
f"heading_align_reward:{heading_align_reward:.4f}",
f"heading_progress_reward:{heading_progress_reward:.4f}",
f"turn_rate_reward:{turn_rate_reward:.4f}",
f"heading_hold_bonus:{heading_hold_bonus:.4f}",
# f"ang_vel_penalty:{ang_vel_penalty:.4f}",
# f"height_down_penalty:{height_down_penalty:.4f}",
# f"exploration_bonus:{exploration_bonus:.4f}"
)
alive_bonus
+ smoothness_penalty
+ posture_penalty
+ ang_vel_penalty
+ linkage_reward
+ waist_only_turn_penalty
+ yaw_link_reward
+ head_toward_bonus
+ heading_progress_reward
+ anti_oscillation_penalty
+ stabilize_bonus
+ post_turn_ang_vel_penalty
+ post_turn_pose_bonus
+ aligned_stance_bonus
# + heading_align_reward
+ turn_rate_reward
+ stance_collapse_penalty
+ cross_leg_penalty
)
now = time.time()
if self.reward_debug_interval_sec > 0 and now - self._reward_debug_last_time >= self.reward_debug_interval_sec:
self._reward_debug_last_time = now
self._reward_debug_steps_left = max(1, self.reward_debug_burst_steps)
if self._reward_debug_steps_left > 0:
self._reward_debug_steps_left -= 1
# print(
# f"reward_debug: step={self.step_counter}, "
# f"alive_bonus:{alive_bonus:.4f}, "
# # f"heading_align_reward:{heading_align_reward:.4f}, "
# # f"heading_progress_reward:{heading_progress_reward:.4f}, "
# f"head_towards_bonus:{head_toward_bonus},"
# f"posture_penalty:{posture_penalty:.4f}, "
# f"ang_vel_penalty:{ang_vel_penalty:.4f}, "
# f"smoothness_penalty:{smoothness_penalty:.4f}, "
# f"linkage_reward:{linkage_reward:.4f}, "
# f"waist_only_turn_penalty:{waist_only_turn_penalty:.4f}, "
# f"yaw_link_reward:{yaw_link_reward:.4f}, "
# f"anti_oscillation_penalty:{anti_oscillation_penalty:.4f}, "
# f"stabilize_bonus:{stabilize_bonus:.4f}, "
# f"turn_rate_reward:{turn_rate_reward:.4f}, "
# f"total:{total:.4f}"
# )
self.debug_log(
f"reward_debug: step={self.step_counter}, "
f"alive_bonus:{alive_bonus:.4f}, "
# f"heading_align_reward:{heading_align_reward:.4f}, "
# f"heading_progress_reward:{heading_progress_reward:.4f}, "
f"head_towards_bonus:{head_toward_bonus},"
f"posture_penalty:{posture_penalty:.4f}, "
f"ang_vel_penalty:{ang_vel_penalty:.4f}, "
f"smoothness_penalty:{smoothness_penalty:.4f}, "
f"heading_progress_reward:{heading_progress_reward:.4f}, "
f"linkage_reward:{linkage_reward:.4f}, "
f"waist_only_turn_penalty:{waist_only_turn_penalty:.4f}, "
f"yaw_link_reward:{yaw_link_reward:.4f}, "
f"anti_oscillation_penalty:{anti_oscillation_penalty:.4f}, "
f"stabilize_bonus:{stabilize_bonus:.4f}, "
f"post_turn_ang_vel_penalty:{post_turn_ang_vel_penalty:.4f}, "
f"post_turn_pose_bonus:{post_turn_pose_bonus:.4f}, "
f"aligned_stance_bonus:{aligned_stance_bonus:.4f}, "
f"turn_rate_reward:{turn_rate_reward:.4f}, "
f"stance_collapse_penalty:{stance_collapse_penalty:.4f}, "
f"cross_leg_penalty:{cross_leg_penalty:.4f}, "
f"total:{total:.4f}"
)
return total
@@ -554,7 +631,7 @@ class WalkEnv(gym.Env):
for idx, target in enumerate(self.target_joint_positions):
r.set_motor_target_position(
r.ROBOT_MOTORS[idx], target * 180 / math.pi, kp=40, kd=1.0
r.ROBOT_MOTORS[idx], target * 180 / math.pi, kp=25, kd=0.6
)
self.previous_action = action