Autonomous control.
Supercharged by AI.

# 1. Import HRL Project Extreme Framework
from hrl_extreme.agent import HierarchicalAgent
from hrl_extreme.continuous_env import ContinuousGoalNavigationEnv
from hrl_extreme.reachability import ReachabilityProjector

# 2. Instantiate Environment and Hierarchical Agent
env = ContinuousGoalNavigationEnv(room_size=10.0, max_steps=200)
agent = HierarchicalAgent(
    obs_dim=6,          # [x, y, vx, vy, goal_x, goal_y]
    subgoal_dim=2,      # [g_x, g_y] continuous latent sub-goal
    action_dim=2,       # [a_x, a_y] continuous force in [-1.0, 1.0]
    continuous=True,
    c_step=8,           # Manager dilation horizon
    use_torch=True,
    use_reachability=True
)

# 3. Rollout & Autonomous Training Loop
obs = env.reset()
for episode in range(1000):
    done = False
    while not done:
        # Hierarchical Action Selection with GARA/STAR Reachability Guidance
        action, subgoal, log_prob, value = agent.select_action(obs, goal_pos=env.goal)
        next_obs, reward, done, info = env.step(action)
        
        # Intrinsic Cosine Alignment Motivation r_i
        r_i = agent.compute_intrinsic_reward(obs, next_obs, subgoal)
        
        # Store transition & optimize dual actor-critic networks
        agent.buffer.store_worker(obs, subgoal, action, r_i, next_obs, done, log_prob, value)
        obs = next_obs

    # Online batch policy gradient update
    losses = agent.train_batch(batch_size=32)
    print(f"Episode {episode} Complete | Worker Loss: {losses['worker_loss']:.4f} | Manager Loss: {losses['manager_loss']:.4f}")