adjusted rewards and requirements
This commit is contained in:
@@ -46,12 +46,14 @@ class JackBotEnv(gym.Env):
|
||||
self.start_pose = start_pose
|
||||
self.max_episode_steps = max_episode_steps
|
||||
self.urdf_path = urdf_path
|
||||
self.max_robot_speed = 0.8
|
||||
|
||||
self.episode_count = 0
|
||||
self.step_count = 0
|
||||
self.total_steps = 0
|
||||
self.cumulative_reward = 0.0
|
||||
self.robot_rewards = [0.0]
|
||||
self.consecutive_still_steps = [0]
|
||||
self.failed_robots_mask = [False]
|
||||
self.episode_height_sum = 0.0
|
||||
self._first_reset = True
|
||||
@@ -106,7 +108,6 @@ class JackBotEnv(gym.Env):
|
||||
self.curriculum_stage_requirements = {
|
||||
CurriculumPhase.FORWARD: {
|
||||
"survival_steps": 400, # Must survive ~8 seconds
|
||||
"max_displacement": 0.25, # Must remain within 0.25m radius
|
||||
"min_avg_height_ratio": 0.90, # Average height >= 90% of target
|
||||
"stability_roll_pitch": 0.18, # Max ~10 degrees tilt
|
||||
},
|
||||
@@ -150,17 +151,24 @@ class JackBotEnv(gym.Env):
|
||||
vz = 0.0
|
||||
omega = 0.0
|
||||
elif phase == CurriculumPhase.FORWARD:
|
||||
# Phase 1: Straight Forward Walking
|
||||
vx = np.random.uniform(0.5, 1.0)
|
||||
vy = 0.0
|
||||
vz = 0.0
|
||||
omega = 0.0
|
||||
# Phase 1: Straight Forward Walking + Standing Checks
|
||||
if np.random.random() < 0.30:
|
||||
# 30% chance: Stand Still Command (vx = 0)
|
||||
vx = 0.0
|
||||
else:
|
||||
# 70% chance: Forward Walk Command
|
||||
vx = np.random.uniform(0.15, 0.50)
|
||||
|
||||
vy, vz, omega = 0.0, 0.0, 0.0
|
||||
|
||||
elif phase == CurriculumPhase.TURN_AND_DIRECTION:
|
||||
# Phase 2: Forward/Backward + Turning
|
||||
vx = np.random.uniform(-1.0, 1.0)
|
||||
vy = 0.0
|
||||
vz = 0.0
|
||||
omega = np.random.uniform(-0.8, 0.8)
|
||||
# Phase 2: Forward/Backward + Turning + Standing
|
||||
if np.random.random() < 0.20:
|
||||
vx, omega = 0.0, 0.0
|
||||
else:
|
||||
vx = np.random.uniform(-1.0, 1.0)
|
||||
omega = np.random.uniform(-0.8, 0.8)
|
||||
vy, vz = 0.0, 0.0
|
||||
elif phase == CurriculumPhase.OMNI_DIRECTION:
|
||||
# Phase 3: Full Omnidirectional Movement
|
||||
vx = np.random.uniform(-1.0, 1.0)
|
||||
@@ -195,6 +203,7 @@ class JackBotEnv(gym.Env):
|
||||
if self.use_gui:
|
||||
self.sim_manager.set_robot_color(pb_id, [1.0, 1.0, 1.0, 1.0])
|
||||
|
||||
self.consecutive_still_steps = [0 for _ in range(len(self.pb_robots))]
|
||||
self.last_action = np.zeros(self.action_space.shape[0], dtype=np.float32)
|
||||
self.max_distance_from_start = [0.0]
|
||||
self.max_survival_steps = 0
|
||||
@@ -406,56 +415,78 @@ class JackBotEnv(gym.Env):
|
||||
linear_vel, angular_vel = self.sim_manager.get_robot_velocity(pb_id)
|
||||
command = self.commands[idx]
|
||||
|
||||
cmd_vx = command[0]
|
||||
cmd_vy = command[1]
|
||||
cmd_vx = command[0] # Normalized -1.0 to 1.0
|
||||
cmd_vy = command[1] # Normalized -1.0 to 1.0
|
||||
cmd_yaw = command[3]
|
||||
|
||||
# 1. LINEAR VECTOR SPEED MAXIMIZATION
|
||||
cmd_dir = np.array([cmd_vx, cmd_vy], dtype=np.float32)
|
||||
cmd_norm = np.linalg.norm(cmd_dir)
|
||||
|
||||
actual_vel_2d = np.array([linear_vel[0], linear_vel[1]], dtype=np.float32)
|
||||
|
||||
# -----------------------------------------------------------------
|
||||
# 1. NORMALIZED SPEED TRACKING (1.0 = v_max)
|
||||
# -----------------------------------------------------------------
|
||||
if cmd_norm > 0.05:
|
||||
unit_cmd_dir = cmd_dir / cmd_norm
|
||||
actual_vel_2d = np.array([linear_vel[0], linear_vel[1]], dtype=np.float32)
|
||||
|
||||
aligned_speed = float(np.dot(actual_vel_2d, unit_cmd_dir))
|
||||
linear_speed_reward = 2.5 * aligned_speed
|
||||
|
||||
target_speed = cmd_norm * self.max_robot_speed # e.g., 0.35 * 0.8 = 0.28 m/s
|
||||
|
||||
if aligned_speed <= 0.0:
|
||||
# Zero or backward movement gets ZERO reward
|
||||
linear_speed_reward = 0.0
|
||||
elif aligned_speed <= target_speed:
|
||||
# Smoothly scales from 0.0 to +5.0 as speed approaches target
|
||||
linear_speed_reward = 5.0 * (aligned_speed / target_speed)
|
||||
else:
|
||||
# Gently penalize overspeeding beyond target
|
||||
overspeed_ratio = (aligned_speed - target_speed) / target_speed
|
||||
linear_speed_reward = max(0.0, 5.0 - 2.5 * overspeed_ratio)
|
||||
|
||||
# Drift penalty for sideways sliding
|
||||
perp_vel = actual_vel_2d - aligned_speed * unit_cmd_dir
|
||||
drift_penalty = 0.35 * float(np.dot(perp_vel, perp_vel))
|
||||
else:
|
||||
aligned_speed = 0.0
|
||||
linear_speed_reward = 0.0
|
||||
drift_penalty = 1.0 * (linear_vel[0]**2 + linear_vel[1]**2)
|
||||
|
||||
# 2. TURNING SPEED MAXIMIZATION
|
||||
# -----------------------------------------------------------------
|
||||
# 2. TIME-ACCUMULATING STILLNESS PENALTY
|
||||
# -----------------------------------------------------------------
|
||||
actual_yaw_rate = angular_vel[2]
|
||||
is_moving = abs(aligned_speed) > 0.03 or abs(actual_yaw_rate) > 0.08
|
||||
|
||||
if (cmd_norm > 0.05 or abs(cmd_yaw) > 0.05) and not is_moving:
|
||||
# Increment counter every step the robot stalls under command
|
||||
self.consecutive_still_steps[idx] += 1
|
||||
else:
|
||||
# Reset counter as soon as robot makes a valid move!
|
||||
self.consecutive_still_steps[idx] = 0
|
||||
|
||||
# Penalty grows by 0.05 every step frozen, capped at -3.5 per step
|
||||
still_penalty = min(3.5, 0.05 * self.consecutive_still_steps[idx])
|
||||
|
||||
# -----------------------------------------------------------------
|
||||
# 3. TURNING & POSTURE PENALTIES
|
||||
# -----------------------------------------------------------------
|
||||
if abs(cmd_yaw) > 0.05:
|
||||
turning_reward = 1.5 * (actual_yaw_rate * cmd_yaw)
|
||||
else:
|
||||
turning_reward = -0.6 * (actual_yaw_rate ** 2)
|
||||
|
||||
# 3. SMALL ALIVE BONUS + AGE-RAMPED STILLNESS PENALTY
|
||||
alive_reward = 0.5
|
||||
|
||||
age_ratio = min(1.0, self.step_count / max(1, self.curriculum_episode_limit))
|
||||
still_penalty = 0.0
|
||||
if (cmd_norm > 0.1 or abs(cmd_yaw) > 0.1) and (abs(linear_vel[0]) < 0.02 and abs(actual_yaw_rate) < 0.05):
|
||||
still_penalty = 0.35 + 0.85 * age_ratio
|
||||
|
||||
# 4. POSTURE & STABILITY PENALTIES
|
||||
height_penalty : float
|
||||
min_valid_h = self.target_height * 0.90 # 90% threshold
|
||||
alive_reward = 0.20
|
||||
|
||||
# Height drop penalty
|
||||
min_valid_h = self.target_height * 0.90
|
||||
if pos[2] < min_valid_h:
|
||||
# Normalized drop below the 90% mark
|
||||
drop = (min_valid_h - pos[2]) / self.target_height
|
||||
# Linear + quadratic penalty that rapidly outweighs the +0.50 alive bonus
|
||||
height_penalty = 4.0 * drop + 20.0 * (drop ** 2)
|
||||
else:
|
||||
# Zero penalty inside the valid 90% - 110% zone!
|
||||
height_penalty = 0.0
|
||||
stability_penalty = 1.5 * (roll**2 + pitch**2)
|
||||
|
||||
stability_penalty = 8 * (roll**2 + pitch**2)
|
||||
|
||||
control_delta = np.abs(current_actions[idx] - previous_actions[idx])
|
||||
large_delta_mask = control_delta > 0.12
|
||||
@@ -474,7 +505,7 @@ class JackBotEnv(gym.Env):
|
||||
rewards.append(r_step)
|
||||
|
||||
return float(np.sum(rewards)), rewards
|
||||
|
||||
|
||||
def _is_done(self) -> bool:
|
||||
"""Returns True when the robot has entered a failed state."""
|
||||
return bool(self.failed_robots_mask[0]) if self.failed_robots_mask else False
|
||||
|
||||
Reference in New Issue
Block a user