#theoreticalengineering — Public Fediverse posts

Live and recent posts from across the Fediverse tagged #theoreticalengineering, aggregated by home.social.

pasjrwoctx👽 @[email protected] · 2026-10-03 · 02:50 UTC

MILK Protocol: A Predictive Kinematic Intelligence Framework for Autonomous Reality-State Modification

Author: pasjrwoctx👽
Concept Proposal by S*A*R*A*H Research Initiative

Version: 1.0
Field: Robotics, Cybernetics, Autonomous Systems, Control Theory, Digital Twins, AI

This paper introduces the Mechanized Intelligence Link Kinematically (MILK) Protocol, a generalized framework for autonomous systems that continuously model, predict, simulate, and modify physical environments through intelligent kinetic action.
Unlike traditional control architectures that optimize isolated actions, MILK treats every motion as a state-transforming event within a dynamic reality model. The protocol combines sensor fusion, predictive world modeling, digital-twin simulation, model predictive control (MPC), and machine learning into a unified architecture.
MILK defines quantitative metrics for measuring the influence of actions on future world states, enabling intelligent agents to maximize desired outcomes while minimizing uncertainty, energy expenditure, and risk.
A prototype implementation using a mobile robotic platform demonstrates how MILK can be experimentally validated under real-world conditions.
Keywords: #cybernetics, #robotics, #autonomoussystems, #digitaltwins, #worldmodels, #predictiveintelligence, #human-machineinteraction

Click to view full article

1. Introduction
Modern autonomous systems react to environments.
MILK proposes a stronger paradigm:
Every action is selected according to its projected influence on future reality states.
The protocol assumes:
    1. Every kinetic action produces measurable state transitions. 
    2. Future states can be estimated probabilistically. 
    3. Better predictions yield better interventions. 
    4. An autonomous agent should optimize future-state outcomes rather than immediate responses. 
This creates a closed-loop architecture capable of continuously shaping environments toward desired objectives.

2. Theoretical Foundation
Let a system state be represented as:
StS_tStâ€‹ 
where:
    • StS_tStâ€‹ = complete observable state at time t. 
An action:
AtA_tAtâ€‹ 
produces a transition:
St+1S_{t+1}St+1â€‹ 
such that:
St+1=f(St,At,Et)S_{t+1}=f(S_t,A_t,E_t)St+1â€‹=f(Stâ€‹,Atâ€‹,Etâ€‹) 
where:
    • EtE_tEtâ€‹ represents environmental factors. 

3. MILK Dynamic Equation
The original conceptual equation:
A+B(1/C)=XA + B(1/C)=XA+B(1/C)=X 
is formalized as:
Xt=At+KtUtX_t=A_t+\frac{K_t}{U_t}Xtâ€‹=Atâ€‹+Utâ€‹Ktâ€‹â€‹ 
where:
Variable	Meaning
Aâ‚œ	Intended action vector
Kâ‚œ	Environmental coupling factor
Uâ‚œ	Uncertainty score
Xâ‚œ	Predicted state change
Interpretation:
    • Strong environmental knowledge increases precision. 
    • Higher uncertainty reduces influence prediction accuracy. 
    • Outcome estimates improve as uncertainty approaches zero. 

4. Reality-State Modification Index
MILK introduces:
Reality Modification Index (RMI)
RMI=âˆ£âˆ£Sfutureâˆ’Scurrentâˆ£âˆ£RMI=||S_{future}-S_{current}||RMI=âˆ£âˆ£Sfutureâ€‹âˆ’Scurrentâ€‹âˆ£âˆ£ 
Where:
    • large values indicate substantial environmental change. 
    • small values indicate minimal influence. 
Examples:
Action	Approximate RMI
Pick up object	Low
Open door	Low
Rearrange room	Medium
Coordinate factory robots	High
Optimize city traffic	Very High
The RMI provides a measurable definition of "reality alteration."

5. Architecture
MILK consists of five primary layers.
Layer 1: Perception
Inputs:
    • Cameras 
    • LiDAR 
    • IMU 
    • Microphones 
    • Tactile sensors 
    • GPS 
Outputs:
WtW_tWtâ€‹ 
Current world model.

Layer 2: World Construction
Sensor fusion constructs:
Wt={Objects,Humans,Locations,Conditions}W_t = \{Objects,Humans,Locations,Conditions\}Wtâ€‹={Objects,Humans,Locations,Conditions} 
Methods:
    • SLAM 
    • Kalman filters 
    • Bayesian estimation 

Layer 3: Predictive Simulation
Generate:
Wt+1,Wt+2,...,Wt+nW_{t+1},W_{t+2},...,W_{t+n}Wt+1â€‹,Wt+2â€‹,...,Wt+nâ€‹ 
using:
    • Transformer world models 
    • Reinforcement learning 
    • Physics simulation 
    • Digital twins 

Layer 4: Kinematic Optimization
Find optimal action sequence:
Aâˆ—=argmin(J)A^*=argmin(J)Aâˆ—=argmin(J) 
where
J=Error+Risk+Energy+TimeJ=Error+Risk+Energy+TimeJ=Error+Risk+Energy+Time 

Layer 5: Reality Verification
After action execution:
Error=Sactualâˆ’SpredictedError=S_{actual}-S_{predicted}Error=Sactualâ€‹âˆ’Spredictedâ€‹ 
Model updates:
Modelnew=Modelold+Learning(Error)Model_{new}=Model_{old}+Learning(Error)Modelnewâ€‹=Modeloldâ€‹+Learning(Error) 

6. SARAH Autonomous Agent
SARAH (Simulated Augmented Reality Assistant Human)
is defined as a humanoid embodiment of MILK.
Core modules:
Self Localization
Maintains position estimate.
Predictive Cognition
Simulates future states.
Adaptive Learning
Updates behavior from errors.
Reality Synchronization Engine
Maintains consistency between:
    • Model 
    • Prediction 
    • Observation 

7. Experimental Hypothesis
Hypothesis:
A MILK-controlled robot will produce significantly lower state-transition error than a conventional reactive controller.
Independent Variable:
    • Control architecture 
Dependent Variables:
    • Path accuracy 
    • Task completion rate 
    • Energy consumption 
    • Prediction accuracy 
    • RMI efficiency 

8. Testable Prototype Design
Prototype Name
MILK-P1

Hardware
Compute
    • NVIDIA Jetson Orin Nano 
    • Raspberry Pi 5 
Sensors
    • Intel RealSense D455 
    • 9-axis IMU 
    • Wheel encoders 
    • Microphone array 
Mobility
    • Differential drive robot base 
Optional
    • 4 DOF robotic arm 
Estimated cost:
$800-$2500

Software Stack
Operating System
Ubuntu 24.04
Middleware
ROS2
Vision
OpenCV
AI
PyTorch
Simulation
Gazebo
Digital Twin
NVIDIA Isaac Sim

9. Experimental Environment
Construct a room containing:
    • Chairs 
    • Boxes 
    • Doors 
    • Human participants 
Robot objective:
Navigate from Point A to Point B while:
    • avoiding obstacles 
    • responding to environmental changes 
    • predicting future movement of agents 

10. Test Sequence
Trial 1
Reactive Controller
Robot responds only after detecting changes.
Measure:
    • collisions 
    • errors 
    • time 

Trial 2
MILK Controller
Robot predicts:
    • moving obstacles 
    • human paths 
    • object displacement 
before motion occurs.
Measure:
    • prediction accuracy 
    • RMI 
    • completion time 

11. Performance Metrics
Predictive Accuracy
PA=1âˆ’âˆ£Predictedâˆ’Actualâˆ£PA=1-|Predicted-Actual|PA=1âˆ’âˆ£Predictedâˆ’Actualâˆ£ 

Reality Modification Efficiency
RME=DesiredStateChangeEnergyUsedRME=\frac{DesiredStateChange}{EnergyUsed}RME=EnergyUsedDesiredStateChangeâ€‹ 

State Synchronization Error
SSE=âˆ£Sactualâˆ’Spredictedâˆ£SSE=|S_{actual}-S_{predicted}|SSE=âˆ£Sactualâ€‹âˆ’Spredictedâ€‹âˆ£ 

Autonomous Intelligence Score
AIS=PAÃ—RMESSEAIS=\frac{PA \times RME}{SSE}AIS=SSEPAÃ—RMEâ€‹ 
Higher is better.

12. Expected Outcomes
MILK should demonstrate:
    • Reduced path planning errors 
    • Better obstacle avoidance 
    • Lower energy expenditure 
    • More accurate future-state predictions 
    • Improved adaptation to dynamic environments 

13. Future Development
MILK-P2:
    • Full humanoid embodiment 
    • Whole-body control 
    • Multi-agent coordination 
MILK-P3:
    • Swarm intelligence 
    • Distributed digital twins 
    • Cloud synchronization 
MILK-P4:
    • Human cognitive state modeling 
    • Intent prediction 
    • Collaborative decision systems 

Conclusion
The MILK Protocol transforms the philosophical concept of "reality alteration" into a measurable engineering framework based on state-space control, predictive simulation, digital twins, and autonomous learning.
Rather than altering reality in a supernatural sense, MILK quantifies how intelligent actions reshape future physical states and provides a mathematical basis for designing systems, such as SARAH, that can optimize those state transitions with increasing precision. The proposed MILK-P1 prototype is immediately testable using existing robotics hardware and modern AI infrastructure, making the protocol falsifiable, measurable, and suitable for academic research and experimental validation.

Click to view code

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
MILK Protocol v1.0 -- Reference Implementation
==============================================

A Predictive Kinematic Intelligence Framework for
Autonomous Reality-State Modification.

This module implements the architecture described in:

    "MILK Protocol: A Predictive Kinematic Intelligence Framework
     for Autonomous Reality-State Modification"
     Concept Proposal by SARAH Research Initiative, v1.0

Contents
--------
  Layer 1  PerceptionLayer          -- sensors -> SensorFrame
  Layer 2  WorldModel               -- sensor fusion -> W_t (tracked objects)
  Layer 3  PredictiveSimulator      -- W_t -> W_{t+1} .. W_{t+n}
  Layer 4  KinematicOptimizer       -- argmin J = Error+Risk+Energy+Time
  Layer 5  RealityVerifier          -- SSE -> model adaptation
  Math     milk_dynamic_equation    -- X_t = A_t + K_t / U_t
  Metric   reality_modification_index (RMI), PA, RME, SSE, AIS
  Agent    SARAH                    -- humanoid embodiment of MILK

Run:
    python milk_protocol.py --help
    python milk_protocol.py --demo                 # single rendered episode
    python milk_protocol.py --trials 5             # full experiment
    python milk_protocol.py --selftest             # unit checks

Dependencies: numpy only.
"""

from __future__ import annotations

import argparse
import json
import math
import sys
import time
from dataclasses import dataclass, field, asdict
from typing import Dict, List, Optional, Sequence, Tuple

import numpy as np

# =====================================================================
# 0.  Utilities
# =====================================================================

EPS = 1e-9


def wrap_angle(a: float) -> float:
    """Wrap an angle to (-pi, pi]."""
    return (a + math.pi) % (2.0 * math.pi) - math.pi


def integrate_diff_drive(pose: np.ndarray,
                         vel: np.ndarray,
                         action: np.ndarray,
                         cfg: "MILKConfig") -> Tuple[np.ndarray, np.ndarray]:
    """
    Shared differential-drive integrator used by BOTH the environment and
    the predictive simulator.  Keeping them identical means any residual
    prediction error comes from sensing noise / unmodelled slip, not from a
    model mismatch -- which is exactly what the Reality Verification layer
    (Layer 5) is supposed to measure.

    pose   : (x, y, theta)
    vel    : (v, omega)
    action : (v_cmd, omega_cmd)  -- rate limited by a_max / alpha_max
    """
    dt = cfg.dt
    v = float(vel[0]) + float(np.clip(action[0] - vel[0], -cfg.a_max * dt, cfg.a_max * dt))
    w = float(vel[1]) + float(np.clip(action[1] - vel[1], -cfg.alpha_max * dt, cfg.alpha_max * dt))
    x = float(pose[0]) + v * math.cos(pose[2]) * dt
    y = float(pose[1]) + v * math.sin(pose[2]) * dt
    th = wrap_angle(float(pose[2]) + w * dt)
    return np.array([x, y, th]), np.array([v, w])


def ray_circle(ox: float, oy: float, dx: float, dy: float,
               cx: float, cy: float, r: float) -> float:
    """Distance along unit ray (dx,dy) from (ox,oy) to circle, or inf."""
    fx, fy = ox - cx, oy - cy
    b = 2.0 * (fx * dx + fy * dy)
    c = fx * fx + fy * fy - r * r
    disc = b * b - 4.0 * c
    if disc < 0.0:
        return float("inf")
    sq = math.sqrt(disc)
    t1 = (-b - sq) / 2.0
    t2 = (-b + sq) / 2.0
    if t1 > 1e-6:
        return t1
    if t2 > 1e-6:
        return t2
    return float("inf")


# =====================================================================
# 1.  Configuration
# =====================================================================

@dataclass
class MILKConfig:
    """All tunable parameters of the MILK stack."""

    # -- timing -------------------------------------------------------
    dt: float = 0.10
    horizon: int = 12

    # -- robot limits -------------------------------------------------
    v_max: float = 1.20
    omega_max: float = 1.80
    a_max: float = 2.00
    alpha_max: float = 4.00
    robot_radius: float = 0.22

    # -- sensor model -------------------------------------------------
    sensor_range: float = 6.00
    n_rays: int = 72
    range_sigma: float = 0.020
    gps_sigma: float = 0.040
    compass_sigma: float = 0.030
    encoder_sigma: float = 0.020
    gyro_sigma: float = 0.030
    detect_sigma: float = 0.080
    p_detect: float = 0.92

    # -- environment process noise (wheel slip, unmodelled dynamics) --
    slip_v: float = 0.020
    slip_w: float = 0.030

    # -- MILK dynamic equation ---------------------------------------
    u_min: float = 1e-3          # floor on uncertainty (avoids K/U blow-up)

    # -- optimizer weights  (J = Error + Risk + Energy + Time) --------
    w_error: float = 1.00
    w_risk: float = 6.00
    w_energy: float = 0.20
    w_time: float = 0.05
    w_smooth: float = 0.30

    # -- optimizer sampling -------------------------------------------
    n_v_samples: int = 5
    n_w_samples: int = 11
    n_random: int = 60

    # -- world model ---------------------------------------------------
    track_q: float = 0.35        # KF process noise
    track_timeout: int = 8       # frames before a track is dropped

    # -- metrics -------------------------------------------------------
    w_sse_pose: float = 1.00
    w_sse_obj: float = 1.00
    sse_scale: float = 0.50      # normalisation for Predictive Accuracy

    # -- misc ----------------------------------------------------------
    seed: int = 0


# =====================================================================
# 2.  Environment  (the "real world")
# =====================================================================

@dataclass
class Circle:
    x: float
    y: float
    r: float


@dataclass
class Human:
    id: int
    x: float
    y: float
    vx: float
    vy: float
    r: float = 0.30


class RoomEnvironment:
    """
    Ground-truth simulator.  A rectangular room containing static circular
    obstacles and moving humans.  The robot is a differential-drive base.

    Nothing in this class is visible to the controllers except through the
    PerceptionLayer.
    """

    def __init__(self, cfg: MILKConfig, seed: int = 0,
                 n_static: int = 6, n_humans: int = 2):
        self.cfg = cfg
        self.rng = np.random.default_rng(seed)

        self.width = 10.0
        self.height = 8.0

        self.start = np.array([1.0, 1.0, 0.0])
        self.goal = np.array([self.width - 1.0, self.height - 1.0])

        self.robot_pose = self.start.copy()
        self.robot_vel = np.zeros(2)

        self.static: List[Circle] = []
        self._build_static(n_static)

        self.humans: List[Human] = []
        self._build_humans(n_humans)

        self.t = 0.0
        self.collision_events = 0
        self._in_collision = False

    # ------------------------------------------------------------------
    def _build_static(self, n: int) -> None:
        tries = 0
        while len(self.static) < n and tries < 2000:
            tries += 1
            r = float(self.rng.uniform(0.30, 0.60))
            x = float(self.rng.uniform(r + 0.3, self.width - r - 0.3))
            y = float(self.rng.uniform(r + 0.3, self.height - r - 0.3))
            if math.hypot(x - self.start[0], y - self.start[1]) < 1.4:
                continue
            if math.hypot(x - self.goal[0], y - self.goal[1]) < 1.4:
                continue
            if any(math.hypot(x - c.x, y - c.y) < r + c.r + 0.7 for c in self.static):
                continue
            self.static.append(Circle(x, y, r))

    def _build_humans(self, n: int) -> None:
        for i in range(n):
            x = float(self.rng.uniform(2.0, self.width - 2.0))
            y = float(self.rng.uniform(2.0, self.height - 2.0))
            ang = float(self.rng.uniform(0, 2 * math.pi))
            sp = float(self.rng.uniform(0.25, 0.55))
            self.humans.append(Human(i, x, y, sp * math.cos(ang), sp * math.sin(ang)))

    # ------------------------------------------------------------------
    # Kinematics / dynamics
    # ------------------------------------------------------------------
    def step(self, action: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
        """Advance the world by one dt. Returns (new_pose, new_vel)."""
        cfg = self.cfg
        new_pose, new_vel = integrate_diff_drive(self.robot_pose, self.robot_vel, action, cfg)

        # unmodelled slip / process noise -- this is what makes prediction hard
        new_vel = new_vel + self.rng.normal(0.0, [cfg.slip_v, cfg.slip_w])
        new_pose[2] = wrap_angle(new_pose[2] + self.rng.normal(0.0, 0.01))

        self.robot_pose = new_pose
        self.robot_vel = new_vel

        self._step_humans(cfg.dt)

        self.t += cfg.dt

        # collision bookkeeping
        hit = self.check_collision()
        if hit and not self._in_collision:
            self.collision_events += 1
        self._in_collision = hit

        return self.robot_pose.copy(), self.robot_vel.copy()

    def _step_humans(self, dt: float) -> None:
        for h in self.humans:
            h.vx += float(self.rng.normal(0.0, 0.25)) * dt
            h.vy += float(self.rng.normal(0.0, 0.25)) * dt
            sp = math.hypot(h.vx, h.vy)
            if sp > 0.85:
                h.vx *= 0.85 / sp
                h.vy *= 0.85 / sp
            h.x += h.vx * dt
            h.y += h.vy * dt
            if h.x < h.r:
                h.x = h.r
                h.vx = abs(h.vx)
            elif h.x > self.width - h.r:
                h.x = self.width - h.r
                h.vx = -abs(h.vx)
            if h.y < h.r:
                h.y = h.r
                h.vy = abs(h.vy)
            elif h.y > self.height - h.r:
                h.y = self.height - h.r
                h.vy = -abs(h.vy)

    # ------------------------------------------------------------------
    # Sensing primitives (used by the PerceptionLayer)
    # ------------------------------------------------------------------
    def raycast(self, pose: np.ndarray, angles: np.ndarray) -> np.ndarray:
        """Ideal (noise-free) range readings for a fan of rays."""
        ox, oy, oth = float(pose[0]), float(pose[1]), float(pose[2])
        rng_max = self.cfg.sensor_range
        out = np.full(len(angles), rng_max, dtype=float)
        targets = [(c.x, c.y, c.r) for c in self.static]
        targets += [(h.x, h.y, h.r) for h in self.humans]

        for i, a in enumerate(angles):
            ang = oth + float(a)
            dx, dy = math.cos(ang), math.sin(ang)
            t = self._wall_distance(ox, oy, dx, dy)
            for (cx, cy, cr) in targets:
                tc = ray_circle(ox, oy, dx, dy, cx, cy, cr)
                if tc < t:
                    t = tc
            out[i] = min(t, rng_max)
        return out

    def _wall_distance(self, ox: float, oy: float, dx: float, dy: float) -> float:
        ts = []
        if dx > EPS:
            ts.append((self.width - ox) / dx)
        elif dx < -EPS:
            ts.append((0.0 - ox) / dx)
        if dy > EPS:
            ts.append((self.height - oy) / dy)
        elif dy < -EPS:
            ts.append((0.0 - oy) / dy)
        ts = [t for t in ts if t > EPS]
        return min(ts) if ts else float("inf")

    def check_collision(self) -> bool:
        rx, ry = float(self.robot_pose[0]), float(self.robot_pose[1])
        rr = self.cfg.robot_radius
        for c in self.static:
            if math.hypot(rx - c.x, ry - c.y) < rr + c.r:
                return True
        for h in self.humans:
            if math.hypot(rx - h.x, ry - h.y) < rr + h.r:
                return True
        if rx < rr or rx > self.width - rr or ry < rr or ry > self.height - rr:
            return True
        return False

    # ------------------------------------------------------------------
    def ground_truth(self) -> Dict:
        """The state the controller is trying to predict."""
        return {
            "pose": self.robot_pose.copy(),
            "vel": self.robot_vel.copy(),
            "objects": {h.id: np.array([h.x, h.y]) for h in self.humans},
        }

    def goal_distance(self) -> float:
        return float(np.linalg.norm(self.robot_pose[:2] - self.goal))


# =====================================================================
# 3.  Layer 1 -- Perception
# =====================================================================

@dataclass
class SensorFrame:
    t: float
    dt: float
    angles: np.ndarray
    ranges: np.ndarray
    gps_xy: np.ndarray
    compass_theta: float
    encoder_v: float
    gyro_w: float
    detections: Dict[int, np.ndarray]   # object id -> noisy (x, y)


class PerceptionLayer:
    """Layer 1: raw, noisy, partial observation of the world."""

    def __init__(self, cfg: MILKConfig, seed: int = 0):
        self.cfg = cfg
        self.rng = np.random.default_rng(seed + 1234)
        self.angles = np.linspace(-math.pi, math.pi, cfg.n_rays, endpoint=False)

    def sense(self, env: RoomEnvironment) -> SensorFrame:
        cfg = self.cfg
        pose = env.robot_pose

        # --- LiDAR / depth -------------------------------------------
        ranges = env.raycast(pose, self.angles)
        ranges = np.clip(ranges + self.rng.normal(0.0, cfg.range_sigma, ranges.shape),
                         0.0, cfg.sensor_range)

        # --- GPS ------------------------------------------------------
        gps = pose[:2] + self.rng.normal(0.0, cfg.gps_sigma, 2)

        # --- IMU / compass -------------------------------------------
        compass = wrap_angle(pose[2] + float(self.rng.normal(0.0, cfg.compass_sigma)))
        gyro = float(env.robot_vel[1] + self.rng.normal(0.0, cfg.gyro_sigma))

        # --- wheel encoders ------------------------------------------
        enc = float(env.robot_vel[0] + self.rng.normal(0.0, cfg.encoder_sigma))

        # --- object detector (people / dynamic agents) ---------------
        detections: Dict[int, np.ndarray] = {}
        for h in env.humans:
            d = math.hypot(h.x - pose[0], h.y - pose[1])
            if d > cfg.sensor_range:
                continue
            if self.rng.random() > cfg.p_detect:
                continue
            z = np.array([h.x, h.y]) + self.rng.normal(0.0, cfg.detect_sigma, 2)
            detections[h.id] = z

        return SensorFrame(
            t=env.t, dt=cfg.dt, angles=self.angles, ranges=ranges,
            gps_xy=gps, compass_theta=compass, encoder_v=enc, gyro_w=gyro,
            detections=detections,
        )


# =====================================================================
# 4.  Layer 2 -- World Construction  (sensor fusion -> W_t)
# =====================================================================

class TrackedObject:
    """Constant-velocity Kalman filter: state = [x, y, vx, vy]."""

    def __init__(self, oid: int, x: float, y: float,
                 vx: float = 0.0, vy: float = 0.0):
        self.id = oid
        self.x = np.array([x, y, vx, vy], dtype=float)
        self.P = np.diag([0.25, 0.25, 1.00, 1.00])
        self.missed = 0

    def predict(self, dt: float, q: float) -> None:
        F = np.array([[1, 0, dt, 0],
                      [0, 1, 0, dt],
                      [0, 0, 1, 0],
                      [0, 0, 0, 1]], dtype=float)
        Q = q * np.diag([dt ** 4 / 4.0, dt ** 4 / 4.0, dt ** 2, dt ** 2])
        self.x = F @ self.x
        self.P = F @ self.P @ F.T + Q

    def update(self, z: np.ndarray, R: np.ndarray) -> None:
        H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=float)
        y = z - H @ self.x
        S = H @ self.P @ H.T + R
        K = self.P @ H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(4) - K @ H) @ self.P
        self.missed = 0

    # -- convenience ---------------------------------------------------
    @property
    def position(self) -> np.ndarray:
        return self.x[:2].copy()

    @property
    def velocity(self) -> np.ndarray:
        return self.x[2:].copy()

    @property
    def pos_var(self) -> float:
        return float(self.P[0, 0] + self.P[1, 1])

    @property
    def vel_var(self) -> float:
        return float(self.P[2, 2] + self.P[3, 3])


class WorldModel:
    """
    Layer 2: builds the current world model
        W_t = { Objects, Humans, Locations, Conditions }
    from noisy sensor frames using a pose EKF + per-object Kalman filters.
    """

    def __init__(self, cfg: MILKConfig):
        self.cfg = cfg
        self.pose = np.zeros(3)
        self.pose_cov = np.diag([1.0, 1.0, 0.5])
        self.vel = np.zeros(2)
        self.objects: Dict[int, TrackedObject] = {}
        self.q_scale = 1.0          # adapted by Layer 5
        self.initialised = False
        self.t = 0.0

    # ------------------------------------------------------------------
    def fuse(self, frame: SensorFrame) -> None:
        cfg = self.cfg
        dt = frame.dt

        if not self.initialised:
            self.pose = np.array([frame.gps_xy[0], frame.gps_xy[1], frame.compass_theta])
            self.initialised = True
        else:
            self._predict_pose(dt, frame.encoder_v, frame.gyro_w)

        # predict all tracks forward to the current instant
        for o in self.objects.values():
            o.predict(dt, cfg.track_q * self.q_scale)

        # measurement updates
        self._update_pose(frame.gps_xy, frame.compass_theta)

        R = np.eye(2) * (cfg.detect_sigma ** 2)
        for oid, z in frame.detections.items():
            if oid in self.objects:
                self.objects[oid].update(z, R)
            else:
                self.objects[oid] = TrackedObject(oid, float(z[0]), float(z[1]))

        # age out stale tracks
        dead = []
        for oid, o in self.objects.items():
            if oid not in frame.detections:
                o.missed += 1
                if o.missed > cfg.track_timeout:
                    dead.append(oid)
        for oid in dead:
            del self.objects[oid]

        self.vel = np.array([frame.encoder_v, frame.gyro_w])
        self.t = frame.t

    # ------------------------------------------------------------------
    def _predict_pose(self, dt: float, v: float, w: float) -> None:
        x, y, th = self.pose
        F = np.array([[1.0, 0.0, -v * math.sin(th) * dt],
                      [0.0, 1.0, v * math.cos(th) * dt],
                      [0.0, 0.0, 1.0]])
        self.pose = np.array([x + v * math.cos(th) * dt,
                              y + v * math.sin(th) * dt,
                              wrap_angle(th + w * dt)])
        Q = self.q_scale * np.diag([0.010, 0.010, 0.004])
        self.pose_cov = F @ self.pose_cov @ F.T + Q

    def _update_pose(self, z_xy: np.ndarray, z_th: float) -> None:
        cfg = self.cfg
        R = np.diag([cfg.gps_sigma ** 2, cfg.gps_sigma ** 2, cfg.compass_sigma ** 2])
        y = np.array([z_xy[0] - self.pose[0],
                      z_xy[1] - self.pose[1],
                      wrap_angle(z_th - self.pose[2])])
        S = self.pose_cov + R
        K = self.pose_cov @ np.linalg.inv(S)
        self.pose = self.pose + K @ y
        self.pose[2] = wrap_angle(self.pose[2])
        self.pose_cov = (np.eye(3) - K) @ self.pose_cov

    # ------------------------------------------------------------------
    def object_positions(self) -> Dict[int, np.ndarray]:
        return {oid: o.position for oid, o in self.objects.items()}

    def localisation_sigma(self) -> float:
        return math.sqrt(max(0.0, float(self.pose_cov[0, 0] + self.pose_cov[1, 1])))


# =====================================================================
# 5.  MILK Mathematics
# =====================================================================

@dataclass
class MILKInfluence:
    """Container for the terms of the MILK dynamic equation."""
    A: np.ndarray      # intended action vector
    K: float           # environmental coupling factor
    U: float           # uncertainty score
    X: np.ndarray      # predicted state change

    @property
    def magnitude(self) -> float:
        return float(np.linalg.norm(self.X))

    def as_dict(self) -> Dict:
        return {"A": self.A.tolist(), "K": self.K, "U": self.U,
                "X": self.X.tolist(), "|X|": self.magnitude}


def milk_dynamic_equation(A, K: float, U: float, u_min: float = 1e-3):
    """
    The MILK dynamic equation:

        X_t = A_t + K_t / U_t

    A_t : intended action vector
    K_t : environmental coupling factor  (how strongly the agent's action
          couples into the environment)
    U_t : uncertainty score              (floored at u_min)

    NOTE ON NUMERICS
    ----------------
    As U -> 0 the term K/U diverges, exactly as the source document states
    ("outcome estimates improve as uncertainty approaches zero").  In a
    physical implementation U is floored at u_min, and X is used as a
    *relative influence score* -- not as a literal pose delta.
    """
    A = np.asarray(A, dtype=float)
    U_eff = max(float(U), float(u_min))
    return A + (float(K) / U_eff)


def _sse_weights(n_objects: int, cfg: MILKConfig) -> np.ndarray:
    base = np.array([1.0, 1.0, 0.5,        # pose  (x, y, theta)
                     0.2, 0.2,             # velocity (v, omega)
                     1.0, 1.0])            # goal
    obj = np.ones(2 * n_objects)
    return np.concatenate([base, obj])


def state_vector(pose, vel, goal, objects: Dict[int, np.ndarray]) -> np.ndarray:
    """
    Canonical flat state vector used for RMI / SSE computations.
    Object ordering is by ascending id so vectors are comparable.
    """
    parts = [np.asarray(pose, float)[:3],
             np.asarray(vel, float)[:2],
             np.asarray(goal, float)[:2]]
    for k in sorted(objects):
        parts.append(np.asarray(objects[k], float)[:2])
    return np.concatenate(parts)


def reality_modification_index(s_a: np.ndarray,
                               s_b: np.ndarray,
                               weights: Optional[np.ndarray] = None) -> float:
    """
    Layer metric -- Reality Modification Index:

        RMI = || S_future - S_current ||

    Large values => substantial environmental change.
    Small values => minimal influence.
    """
    a = np.asarray(s_a, float)
    b = np.asarray(s_b, float)
    n = min(len(a), len(b))
    d = b[:n] - a[:n]
    if weights is not None:
        d = d * np.asarray(weights, float)[:n]
    return float(np.linalg.norm(d))


# =====================================================================
# 6.  Layer 3 -- Predictive Simulation
# =====================================================================

class PredictiveSimulator:
    """
    Layer 3: generate W_{t+1} .. W_{t+n} using the world model,
    a constant-velocity motion model for dynamic agents, and exact
    differential-drive kinematics for the ego robot.

    (A production system would swap this for a transformer world model or
    a PhysX/Isaac digital twin; the interface stays identical.)
    """

    def __init__(self, cfg: MILKConfig):
        self.cfg = cfg

    # ------------------------------------------------------------------
    def rollout_robot(self,
                      pose: np.ndarray,
                      vel: np.ndarray,
                      action_seq: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
        """Roll the ego robot forward under a candidate action sequence."""
        cfg = self.cfg
        traj = np.empty((len(action_seq) + 1, 3), dtype=float)
        vels = np.empty(len(action_seq), dtype=float)

        p = np.asarray(pose, float).copy()
        v = np.asarray(vel, float).copy()
        traj[0] = p
        for k in range(len(action_seq)):
            p, v = integrate_diff_drive(p, v, action_seq[k], cfg)
            traj[k + 1] = p
            vels[k] = v[0]
        return traj, vels

    # ------------------------------------------------------------------
    def predict_objects(self,
                        world: WorldModel) -> Dict[int, Tuple[np.ndarray, np.ndarray, float]]:
        """
        Predict each tracked dynamic object over the horizon.

        Returns {id: (positions (H+1,2), variances (H+1,), radius)}
        """
        cfg = self.cfg
        H = cfg.horizon
        dt = cfg.dt
        F = np.array([[1, 0, dt, 0],
                      [0, 1, 0, dt],
                      [0, 0, 1, 0],
                      [0, 0, 0, 1]], dtype=float)
        Q = cfg.track_q * np.diag([dt ** 4 / 4, dt ** 4 / 4, dt ** 2, dt ** 2])

        out: Dict[int, Tuple[np.ndarray, np.ndarray, float]] = {}
        for oid, obj in world.objects.items():
            x = obj.x.copy()
            P = obj.P.copy()
            pos = np.empty((H + 1, 2))
            var = np.empty(H + 1)
            pos[0] = x[:2]
            var[0] = P[0, 0] + P[1, 1]
            for k in range(H):
                x = F @ x
                P = F @ P @ F.T + Q
                pos[k + 1] = x[:2]
                var[k + 1] = P[0, 0] + P[1, 1]
            out[oid] = (pos, var, 0.30)   # 0.30 m nominal agent radius
        return out

    # ------------------------------------------------------------------
    def predict_next_objects(self,
                             world: WorldModel) -> Dict[int, np.ndarray]:
        """One-step-ahead object prediction (used for the SSE metric)."""
        preds = self.predict_objects(world)
        return {oid: v[0][1].copy() for oid, v in preds.items()}


# =====================================================================
# 7.  Layer 4 -- Kinematic Optimization
# =====================================================================

class KinematicOptimizer:
    """
    Layer 4: find the action sequence minimising

        J = Error + Risk + Energy + Time   (+ smoothness regulariser)

    via sampling-based receding-horizon (MPC) optimisation.
    """

    def __init__(self, cfg: MILKConfig, sim: PredictiveSimulator, seed: int = 0):
        self.cfg = cfg
        self.sim = sim
        self.rng = np.random.default_rng(seed + 99)
        self.last_cost = float("inf")
        self.n_evaluated = 0

    # ------------------------------------------------------------------
    def _candidates(self, prev_action: np.ndarray) -> List[np.ndarray]:
        cfg = self.cfg
        H = cfg.horizon
        cands: List[np.ndarray] = []

        vs = np.linspace(0.0, cfg.v_max, cfg.n_v_samples)
        ws = np.linspace(-cfg.omega_max, cfg.omega_max, cfg.n_w_samples)
        for v in vs:
            for w in ws:
                cands.append(np.tile([v, w], (H, 1)))

        # a handful of two-phase manoeuvres (turn-then-drive)
        half = max(1, H // 2)
        for _ in range(cfg.n_random):
            v1 = float(self.rng.uniform(0.0, cfg.v_max))
            w1 = float(self.rng.uniform(-cfg.omega_max, cfg.omega_max))
            v2 = float(self.rng.uniform(0.0, cfg.v_max))
            w2 = float(self.rng.uniform(-cfg.omega_max, cfg.omega_max))
            seq = np.vstack([np.tile([v1, w1], (half, 1)),
                             np.tile([v2, w2], (H - half, 1))])
            cands.append(seq)

        # always include "brake hard"
        cands.append(np.tile([0.0, 0.0], (H, 1)))
        return cands

    # ------------------------------------------------------------------
    def _cost(self,
              traj: np.ndarray,
              vels: np.ndarray,
              goal: np.ndarray,
              pred_objs: Dict[int, Tuple[np.ndarray, np.ndarray, float]],
              action_seq: np.ndarray,
              prev_action: np.ndarray,
              bounds: Tuple[float, float, float, float],
              risk_gain: float) -> float:
        cfg = self.cfg
        dt = cfg.dt
        H = len(action_seq)

        # ---- Error : terminal distance + heading misalignment --------
        final = traj[-1]
        d_goal = float(np.linalg.norm(final[:2] - goal))
        desired = math.atan2(goal[1] - final[1], goal[0] - final[0])
        head_err = abs(wrap_angle(desired - final[2]))
        error = d_goal + 0.25 * head_err

        # ---- Risk : predicted collision exposure ---------------------
        risk = 0.0
        for oid, (pos, var, orad) in pred_objs.items():
            n = min(len(pos), len(traj))
            d = np.linalg.norm(pos[:n] - traj[:n, :2], axis=1)
            clearance = d - (cfg.robot_radius + orad)
            sigma = np.sqrt(var[:n]) + 0.15
            risk += float(np.sum(np.exp(-np.maximum(clearance, 0.0) ** 2 / (2.0 * sigma ** 2))))
            risk += 100.0 * float(np.sum(clearance < 0.0))

        # ---- wall risk ------------------------------------------------
        x0, x1, y0, y1 = bounds
        margin = cfg.robot_radius + 0.05
        outside = ((traj[:, 0] < x0 + margin) | (traj[:, 0] > x1 - margin) |
                   (traj[:, 1] < y0 + margin) | (traj[:, 1] > y1 - margin))
        wall_risk = 100.0 * float(np.sum(outside))

        # ---- Energy ---------------------------------------------------
        w_cmd = action_seq[:, 1]
        energy = float(np.sum(vels ** 2 + 0.30 * w_cmd ** 2) * dt)

        # ---- Time : expected remaining time to goal -------------------
        v_avg = max(float(np.mean(np.abs(vels))), 0.20)
        time_term = d_goal / v_avg

        # ---- Smoothness ----------------------------------------------
        smooth = float(np.linalg.norm(action_seq[0] - prev_action))

        return (cfg.w_error * error
                + cfg.w_risk * risk_gain * (risk + wall_risk)
                + cfg.w_energy * energy
                + cfg.w_time * time_term
                + cfg.w_smooth * smooth)

    # ------------------------------------------------------------------
    def optimize(self,
                 pose: np.ndarray,
                 vel: np.ndarray,
                 goal: np.ndarray,
                 pred_objs: Dict[int, Tuple[np.ndarray, np.ndarray, float]],
                 prev_action: np.ndarray,
                 bounds: Tuple[float, float, float, float],
                 risk_gain: float = 1.0):
        """Returns (best_action, info_dict)."""
        best_seq = None
        best_cost = float("inf")
        best_traj = None
        best_vels = None

        for seq in self._candidates(prev_action):
            traj, vels = self.sim.rollout_robot(pose, vel, seq)
            c = self._cost(traj, vels, goal, pred_objs, seq, prev_action,
                           bounds, risk_gain)
            if c < best_cost:
                best_cost = c
                best_seq = seq
                best_traj = traj
                best_vels = vels

        self.last_cost = best_cost
        self.n_evaluated += 1

        info = {
            "cost": best_cost,
            "trajectory": best_traj,
            "vels": best_vels,
            "sequence": best_seq,
        }
        return best_seq[0].copy(), info


# =====================================================================
# 8.  Layer 5 -- Reality Verification
# =====================================================================

class RealityVerifier:
    """
    Layer 5: compare predicted vs. actual state and adapt the world model.

        Error       = S_actual - S_predicted
        Model_new   = Model_old + Learning(Error)

    Adaptation here adjusts the Kalman process-noise scale: persistent
    under-prediction of motion raises q, persistent over-prediction lowers it.
    """

    def __init__(self, cfg: MILKConfig):
        self.cfg = cfg
        self.history: List[float] = []
        self.q_scale = 1.0
        self.lr = 0.08
        self.target = 0.08

    def verify(self, error: float) -> float:
        self.history.append(float(error))
        return float(error)

    def learn(self) -> float:
        if not self.history:
            return self.q_scale
        e = self.history[-1]
        self.q_scale *= (1.0 + self.lr * (e - self.target))
        self.q_scale = float(np.clip(self.q_scale, 0.25, 8.0))
        return self.q_scale

    def mean_error(self) -> float:
        return float(np.mean(self.history)) if self.history else 0.0


def prediction_error(pred: Dict, gt: Dict, cfg: MILKConfig) -> Tuple[float, float, float]:
    """
    Compute the State Synchronization Error between a prediction snapshot
    and ground truth.

        SSE = w_pose * ||pose_pred - pose_actual||
            + w_obj  * mean ||obj_pred - obj_actual||

    Returns (sse_total, pose_error, object_error)
    """
    p_pose = np.asarray(pred["pose"], float)
    a_pose = np.asarray(gt["pose"], float)
    e_pose = float(np.linalg.norm(p_pose[:2] - a_pose[:2]))

    e_objs = []
    for oid, p in pred.get("objects", {}).items():
        if oid in gt["objects"]:
            e_objs.append(float(np.linalg.norm(np.asarray(p, float)[:2]
                                               - np.asarray(gt["objects"][oid], float)[:2])))
    e_obj = float(np.mean(e_objs)) if e_objs else 0.0

    total = cfg.w_sse_pose * e_pose + cfg.w_sse_obj * e_obj
    return total, e_pose, e_obj


# =====================================================================
# 9.  SARAH -- Simulated Augmented Reality Assistant Human
# =====================================================================

class SelfLocalizationModule:
    """Maintains the position estimate of the embodiment."""

    def __init__(self, world: WorldModel):
        self.world = world

    @property
    def pose(self) -> np.ndarray:
        return self.world.pose

    @property
    def covariance(self) -> np.ndarray:
        return self.world.pose_cov

    def report(self) -> Dict:
        return {
            "pose": self.world.pose.tolist(),
            "sigma": self.world.localisation_sigma(),
        }


class PredictiveCognitionModule:
    """Simulates future states of self and others."""

    def __init__(self, sim: PredictiveSimulator):
        self.sim = sim

    def simulate_self(self, pose, vel, action_seq):
        return self.sim.rollout_robot(pose, vel, action_seq)

    def simulate_others(self, world: WorldModel):
        return self.sim.predict_objects(world)


class AdaptiveLearningModule:
    """Updates behaviour from observed errors."""

    def __init__(self, verifier: RealityVerifier):
        self.verifier = verifier

    def learn(self) -> float:
        return self.verifier.learn()

    def report(self) -> Dict:
        return {"q_scale": self.verifier.q_scale,
                "mean_sse": self.verifier.mean_error(),
                "n": len(self.verifier.history)}


class RealitySynchronizationEngine:
    """
    Keeps Model / Prediction / Observation mutually consistent and
    flags divergence.
    """

    def __init__(self, tol: float = 0.25):
        self.tol = tol
        self.log: List[Dict] = []

    def synchronize(self, model_pose, predicted_pose, observed_pose) -> Dict:
        mp = np.asarray(model_pose, float)[:2]
        pp = np.asarray(predicted_pose, float)[:2]
        op = np.asarray(observed_pose, float)[:2]
        rec = {
            "model_prediction": float(np.linalg.norm(mp - pp)),
            "prediction_observation": float(np.linalg.norm(pp - op)),
            "model_observation": float(np.linalg.norm(mp - op)),
        }
        rec["synchronized"] = bool(rec["prediction_observation"] < self.tol)
        self.log.append(rec)
        return rec

    def sync_rate(self) -> float:
        if not self.log:
            return 0.0
        return float(np.mean([r["synchronized"] for r in self.log]))


# =====================================================================
# 10.  Controllers
# =====================================================================

class BaseController:
    """Common perception + world-model plumbing."""

    name = "BASE"

    def __init__(self, cfg: MILKConfig, seed: int = 0):
        self.cfg = cfg
        self.seed = seed
        self.perception = PerceptionLayer(cfg, seed)
        self.world = WorldModel(cfg)
        self.prev_action = np.zeros(2)
        self.predicted_pose = np.zeros(3)
        self.predicted_objects: Dict[int, np.ndarray] = {}
        self.last_frame: Optional[SensorFrame] = None

    # -- to be overridden ---------------------------------------------
    def act(self, goal: np.ndarray) -> np.ndarray:
        raise NotImplementedError

    def verify(self, pred: Dict, gt: Dict) -> float:
        return 0.0

    # ------------------------------------------------------------------
    def observe(self, env: RoomEnvironment) -> None:
        frame = self.perception.sense(env)
        self.last_frame = frame
        self.world.fuse(frame)

    def prediction_snapshot(self) -> Dict:
        return {"pose": self.predicted_pose.copy(),
                "objects": {k: v.copy() for k, v in self.predicted_objects.items()}}

    def diagnostics(self) -> Dict:
        return {}


# ---------------------------------------------------------------------
class ReactiveController(BaseController):
    """
    Baseline: reacts to the world as it currently is.

    * heads straight for the goal
    * turns away from anything currently within a fixed radius
    * no rollout, no prediction of agent motion, no verification layer

    Its implicit prediction for the next timestep is "the world stays
    exactly as I currently estimate it".
    """

    name = "REACTIVE"

    def __init__(self, cfg: MILKConfig, seed: int = 0):
        super().__init__(cfg, seed)
        self.avoid_radius = 1.0

    def act(self, goal: np.ndarray) -> np.ndarray:
        cfg = self.cfg
        pose = self.world.pose

        # --- pure pursuit ---------------------------------------------
        desired = math.atan2(goal[1] - pose[1], goal[0] - pose[0])
        err = wrap_angle(desired - pose[2])
        omega = float(np.clip(2.0 * err, -cfg.omega_max, cfg.omega_max))
        v = cfg.v_max * max(0.0, 1.0 - abs(err) / 1.4)

        # --- reflexive obstacle avoidance -----------------------------
        for obj in self.world.objects.values():
            rel = obj.position - pose[:2]
            d = float(np.linalg.norm(rel))
            if d > self.avoid_radius:
                continue
            bearing = wrap_angle(math.atan2(rel[1], rel[0]) - pose[2])
            if abs(bearing) < 0.8:
                omega = -math.copysign(cfg.omega_max * 0.85, bearing)
                v = min(v, 0.12)

        action = np.array([v, omega])

        # --- the reactive "prediction": the world is frozen ------------
        self.predicted_pose, _ = integrate_diff_drive(pose, self.world.vel, action, cfg)
        self.predicted_objects = self.world.object_positions()

        self.prev_action = action
        return action


# ---------------------------------------------------------------------
class MILKController(BaseController):
    """
    Full MILK stack:

        Layer 1  PerceptionLayer
        Layer 2  WorldModel
        Layer 3  PredictiveSimulator
        Layer 4  KinematicOptimizer
        Layer 5  RealityVerifier
        + MILK dynamic equation and RMI bookkeeping
    """

    name = "MILK"

    def __init__(self, cfg: MILKConfig, seed: int = 0):
        super().__init__(cfg, seed)
        self.sim = PredictiveSimulator(cfg)
        self.optimizer = KinematicOptimizer(cfg, self.sim, seed)
        self.verifier = RealityVerifier(cfg)

        self.uncertainty = 1.0
        self.coupling = 0.0
        self.influence: Optional[MILKInfluence] = None
        self.rmi = 0.0
        self.pred_traj: Optional[np.ndarray] = None
        self.last_cost = float("inf")

    # ------------------------------------------------------------------
    #  Uncertainty (U_t) and environmental coupling (K_t)
    # ------------------------------------------------------------------
    def _compute_uncertainty(self) -> float:
        """
        U_t : scalar uncertainty over the horizon.

        Combines localisation variance with the propagated position
        uncertainty of every tracked dynamic object.
        """
        cfg = self.cfg
        loc = self.world.localisation_sigma()

        terms = []
        for o in self.world.objects.values():
            growth = (cfg.horizon * cfg.dt) ** 2 * o.vel_var
            terms.append(o.pos_var + growth)
        obj = math.sqrt(float(np.mean(terms))) if terms else 0.0

        return float(max(loc + 0.5 * obj, cfg.u_min))

    def _compute_coupling(self) -> float:
        """
        K_t : environmental coupling factor in [0, 1].

        How strongly the agent's actions can couple into the environment:
        high when nearby, confidently-tracked objects are present;
        low in empty, featureless space.
        """
        cfg = self.cfg
        objs = list(self.world.objects.values())
        if not objs:
            return 0.15

        ds = np.array([float(np.linalg.norm(o.position - self.world.pose[:2]))
                       for o in objs])
        proximity = float(np.mean(np.exp(-ds / cfg.sensor_range)))
        confidence = float(np.mean([math.exp(-0.5 * o.pos_var / 0.25) for o in objs]))
        return float(np.clip(proximity * confidence, 0.0, 1.0))

    # ------------------------------------------------------------------
    def act(self, goal: np.ndarray) -> np.ndarray:
        cfg = self.cfg
        pose = self.world.pose
        vel = self.world.vel

        # --- Layer 3 : simulate the future ---------------------------
        pred_objs = self.sim.predict_objects(self.world)

        # --- Layer 4 : optimise the action ---------------------------
        self.uncertainty = self._compute_uncertainty()
        self.coupling = self._compute_coupling()

        # Higher uncertainty -> more conservative risk weighting.
        risk_gain = float(np.clip(1.0 + 0.8 * (self.uncertainty - 0.15), 1.0, 3.0))

        bounds = (0.0, 20.0, 0.0, 20.0)   # generous; wall cost handles margins
        action, info = self.optimizer.optimize(
            pose, vel, goal, pred_objs, self.prev_action, bounds, risk_gain)

        self.pred_traj = info["trajectory"]
        self.last_cost = info["cost"]

        # --- MILK dynamic equation : X_t = A_t + K_t / U_t -----------
        A = np.array([action[0] * cfg.dt, action[1] * cfg.dt])
        X = milk_dynamic_equation(A, self.coupling, self.uncertainty, cfg.u_min)
        self.influence = MILKInfluence(A=A, K=self.coupling,
                                       U=self.uncertainty, X=X)

        # --- Reality Modification Index ------------------------------
        cur_objs = self.world.object_positions()
        fut_objs = {oid: v[0][-1] for oid, v in pred_objs.items()}
        s_now = state_vector(self.world.pose, self.world.vel, goal, cur_objs)
        s_fut = state_vector(self.pred_traj[-1],
                             [float(info["vels"][-1]), action[1]],
                             goal, fut_objs)
        n_obj = len(set(cur_objs) | set(fut_objs))
        self.rmi = reality_modification_index(s_now, s_fut, _sse_weights(n_obj, cfg))

        # --- one-step predictions (for the SSE metric) ---------------
        self.predicted_pose = self.pred_traj[1].copy()
        self.predicted_objects = {oid: v[0][1].copy() for oid, v in pred_objs.items()}

        self.prev_action = action
        return action

    # ------------------------------------------------------------------
    def verify(self, pred: Dict, gt: Dict) -> float:
        """Layer 5: measure error and adapt the world model."""
        sse, _, _ = prediction_error(pred, gt, self.cfg)
        self.verifier.verify(sse)
        self.world.q_scale = self.verifier.learn()
        return sse

    def diagnostics(self) -> Dict:
        return {
            "U": self.uncertainty,
            "K": self.coupling,
            "X_norm": self.influence.magnitude if self.influence else 0.0,
            "RMI": self.rmi,
            "cost": self.last_cost,
            "q_scale": self.world.q_scale,
        }


# ---------------------------------------------------------------------
class SARAH:
    """
    SARAH -- Simulated Augmented Reality Assistant Human.

    The humanoid embodiment of the MILK Protocol.  Composes the four
    named core modules on top of the MILK control stack.
    """

    name = "SARAH/MILK"

    def __init__(self, cfg: MILKConfig, seed: int = 0):
        self.cfg = cfg
        self.engine = MILKController(cfg, seed)

        # -- the four core modules of SARAH ---------------------------
        self.self_localization = SelfLocalizationModule(self.engine.world)
        self.predictive_cognition = PredictiveCognitionModule(self.engine.sim)
        self.adaptive_learning = AdaptiveLearningModule(self.engine.verifier)
        self.reality_sync = RealitySynchronizationEngine(tol=0.30)

    # -- MILK interface ------------------------------------------------
    def observe(self, env: RoomEnvironment) -> None:
        self.engine.observe(env)

    def act(self, goal: np.ndarray) -> np.ndarray:
        return self.engine.act(goal)

    def prediction_snapshot(self) -> Dict:
        return self.engine.prediction_snapshot()

    def verify(self, pred: Dict, gt: Dict) -> float:
        sse = self.engine.verify(pred, gt)
        self.reality_sync.synchronize(self.engine.world.pose,
                                      pred["pose"],
                                      gt["pose"])
        return sse

    def diagnostics(self) -> Dict:
        d = self.engine.diagnostics()
        d["sync_rate"] = self.reality_sync.sync_rate()
        return d


# =====================================================================
# 11.  Metrics
# =====================================================================

@dataclass
class TrialMetrics:
    controller: str
    seed: int
    success: bool
    steps: int
    time_to_goal: float
    collisions: int
    path_length: float
    energy: float
    mean_sse: float
    mean_pa: float
    mean_rmi: float
    rme: float
    ais: float
    final_goal_distance: float

    def as_row(self) -> str:
        return (f"{self.controller:<10} | {str(self.success):<5} | "
                f"{self.collisions:^10} | {self.time_to_goal:^7.2f} | "
                f"{self.path_length:^11.2f} | {self.energy:^6.2f} | "
                f"{self.mean_sse:^8.3f} | {self.mean_pa:^7.3f} | "
                f"{self.mean_rmi:^8.2f} | {self.rme:^6.3f} | {self.ais:^6.2f}")


class MetricsRecorder:
    """Accumulates the performance metrics defined in section 11."""

    def __init__(self, label: str, cfg: MILKConfig,
                 start_goal_distance: float, seed: int):
        self.label = label
        self.cfg = cfg
        self.start_goal_distance = float(start_goal_distance)
        self.seed = seed

        self.sse: List[float] = []
        self.rmi: List[float] = []
        self.energy = 0.0
        self.path_length = 0.0
        self.steps = 0
        self.prev_xy: Optional[np.ndarray] = None
        self.time_to_goal = float("nan")

    # ------------------------------------------------------------------
    def step(self,
             sse: float,
             rmi: float,
             action: np.ndarray,
             pose_xy: np.ndarray) -> None:
        cfg = self.cfg
        self.sse.append(float(sse))
        self.rmi.append(float(rmi))

        # energy proxy for a differential drive: v^2 + k * omega^2
        self.energy += float(action[0] ** 2 + 0.30 * action[1] ** 2) * cfg.dt

        if self.prev_xy is not None:
            self.path_length += float(np.linalg.norm(pose_xy - self.prev_xy))
        self.prev_xy = np.asarray(pose_xy, float).copy()

        self.steps += 1

    # ------------------------------------------------------------------
    def finalize(self, env: RoomEnvironment, success: bool) -> TrialMetrics:
        cfg = self.cfg

        mean_sse = float(np.mean(self.sse)) if self.sse else 0.0
        mean_rmi = float(np.mean(self.rmi)) if self.rmi else 0.0

        # Predictive Accuracy:  PA = 1 - |Predicted - Actual|  (normalised)
        mean_pa = float(np.clip(1.0 - mean_sse / cfg.sse_scale, 0.0, 1.0))

        # Reality Modification Efficiency:  RME = DesiredStateChange / Energy
        desired_change = max(0.0, self.start_goal_distance - env.goal_distance())
        energy = max(self.energy, EPS)
        rme = desired_change / energy

        # Autonomous Intelligence Score:  AIS = PA * RME / SSE
        ais = (mean_pa * rme) / max(mean_sse, 1e-4)

        return TrialMetrics(
            controller=self.label,
            seed=self.seed,
            success=bool(success),
            steps=self.steps,
            time_to_goal=self.time_to_goal,
            collisions=env.collision_events,
            path_length=self.path_length,
            energy=self.energy,
            mean_sse=mean_sse,
            mean_pa=mean_pa,
            mean_rmi=mean_rmi,
            rme=rme,
            ais=ais,
            final_goal_distance=env.goal_distance(),
        )


# =====================================================================
# 12.  Terminal renderer
# =====================================================================

class ASCIIRenderer:
    """Minimal top-down visualisation for terminals."""

    def __init__(self, env: RoomEnvironment, cols: int = 76, rows: int = 22):
        self.env = env
        self.cols = cols
        self.rows = rows

    def _cell(self, x: float, y: float) -> Tuple[int, int]:
        c = int(x / self.env.width * (self.cols - 1))
        r = int((1.0 - y / self.env.height) * (self.rows - 1))
        return (max(0, min(self.cols - 1, c)), max(0, min(self.rows - 1, r)))

    def render(self, controller: Optional[BaseController] = None,
               goal: Optional[np.ndarray] = None,
               header: str = "") -> str:
        env = self.env
        grid = [[" "] * self.cols for _ in range(self.rows)]

        for c in range(self.cols):
            grid[0][c] = "-"
            grid[self.rows - 1][c] = "-"
        for r in range(self.rows):
            grid[r][0] = "|"
            grid[r][self.cols - 1] = "|"

        # static obstacles
        for ob in env.static:
            c0, r0 = self._cell(ob.x, ob.y)
            grid[r0][c0] = "#"

        # predicted object positions (MILK only)
        if controller is not None:
            for oid, p in controller.predicted_objects.items():
                c0, r0 = self._cell(float(p[0]), float(p[1]))
                if grid[r0][c0] == " ":
                    grid[r0][c0] = "o"

            # predicted ego trajectory
            if getattr(controller, "pred_traj", None) is not None:
                for p in controller.pred_traj[1:]:
                    c0, r0 = self._cell(float(p[0]), float(p[1]))
                    if grid[r0][c0] == " ":
                        grid[r0][c0] = "."

        # humans (ground truth)
        for h in env.humans:
            c0, r0 = self._cell(h.x, h.y)
            grid[r0][c0] = "H"

        # goal
        g = env.goal if goal is None else goal
        c0, r0 = self._cell(float(g[0]), float(g[1]))
        grid[r0][c0] = "G"

        # robot
        c0, r0 = self._cell(float(env.robot_pose[0]), float(env.robot_pose[1]))
        grid[r0][c0] = "R"

        lines = [header] if header else []
        lines += ["".join(row) for row in grid]
        return "\n".join(lines)


# =====================================================================
# 13.  Trial runner
# =====================================================================

def make_controller(kind: str, cfg: MILKConfig, seed: int):
    kind = kind.upper()
    if kind in ("REACTIVE", "BASELINE"):
        return ReactiveController(cfg, seed)
    if kind in ("MILK", "SARAH"):
        return SARAH(cfg, seed)
    raise ValueError(f"Unknown controller kind: {kind}")


def run_trial(kind: str,
              cfg: MILKConfig,
              seed: int,
              max_steps: int = 400,
              render: bool = False,
              render_every: int = 6,
              verbose: bool = False) -> TrialMetrics:
    """Run one episode and return the resulting metrics."""

    env = RoomEnvironment(cfg, seed=seed)
    controller = make_controller(kind, cfg, seed)

    start_dist = env.goal_distance()
    rec = MetricsRecorder(controller.name, cfg, start_dist, seed)

    goal = env.goal
    success = False
    renderer = ASCIIRenderer(env) if render else None

    for step in range(max_steps):
        controller.observe(env)
        action = controller.act(goal)

        # -- snapshot the prediction BEFORE the world moves -----------
        pred = controller.prediction_snapshot()

        # -- execute ---------------------------------------------------
        env.step(action)

        # -- ground truth AFTER the step -------------------------------
        gt = env.ground_truth()
        sse, e_pose, e_obj = prediction_error(pred, gt, cfg)

        # -- Layer 5 ---------------------------------------------------
        controller.verify(pred, gt)

        # -- bookkeeping ----------------------------------------------
        rmi = getattr(controller, "rmi", 0.0)
        if not isinstance(controller, SARAH):
            rmi = 0.0
        else:
            rmi = controller.engine.rmi

        rec.step(sse, rmi, action, env.robot_pose[:2])

        if render and (step % render_every == 0):
            diag = controller.diagnostics()
            hdr = (f"[{controller.name}] step {step:03d}  "
                   f"d_goal={env.goal_distance():5.2f}  "
                   f"SSE={sse:.3f}  PA={1 - min(sse / cfg.sse_scale, 1):.3f}  "
                   f"RMI={rmi:6.2f}  "
                   f"U={diag.get('U', 0):.3f}  K={diag.get('K', 0):.3f}")
            print("\033[H\033[J" + renderer.render(controller, goal, hdr))
            time.sleep(0.02)

        # -- termination ----------------------------------------------
        if env.goal_distance() < 0.35:
            success = True
            rec.time_to_goal = (step + 1) * cfg.dt
            break

    if not success:
        rec.time_to_goal = float("nan")

    metrics = rec.finalize(env, success)

    if verbose:
        print(f"  trial seed={seed} {controller.name}: "
              f"success={success} steps={metrics.steps} "
              f"SSE={metrics.mean_sse:.3f} PA={metrics.mean_pa:.3f} "
              f"AIS={metrics.ais:.2f}")

    return metrics


def run_experiment(cfg: MILKConfig,
                   n_trials: int = 3,
                   max_steps: int = 400,
                   verbose: bool = True) -> Dict[str, List[TrialMetrics]]:
    """Run Trial 1 (reactive) and Trial 2 (MILK) over matched seeds."""

    results: Dict[str, List[TrialMetrics]] = {"REACTIVE": [], "SARAH/MILK": []}

    print("=" * 108)
    print("MILK PROTOCOL v1.0 -- EXPERIMENTAL VALIDATION")
    print("=" * 108)

    for seed in range(n_trials):
        env_seed = cfg.seed + seed
        print(f"\n-- Trial pair {seed + 1}/{n_trials} (env seed {env_seed}) --")

        m1 = run_trial("REACTIVE", cfg, env_seed, max_steps, verbose=verbose)
        m2 = run_trial("MILK", cfg, env_seed, max_steps, verbose=verbose)

        results["REACTIVE"].append(m1)
        results["SARAH/MILK"].append(m2)

    print("\n" + "=" * 108)
    print("RESULTS")
    print("=" * 108)
    print(f"{'CTRL':<10} | {'OK':<5} | {'COLLISIONS':^10} | {'T[s]':^7} | "
          f"{'PATH[m]':^11} | {'E':^6} | {'SSE':^8} | {'PA':^7} | "
          f"{'RMI':^8} | {'RME':^6} | {'AIS':^6}")
    print("-" * 108)
    for group in results.values():
        for m in group:
            print(m.as_row())
    print("-" * 108)

    print("\nSUMMARY (mean over trials)")
    print("-" * 108)
    for name, group in results.items():
        print(f"{name:<12} | "
              f"success={np.mean([m.success for m in group]):.2f} | "
              f"collisions={np.mean([m.collisions for m in group]):5.2f} | "
              f"SSE={np.mean([m.mean_sse for m in group]):.3f} | "
              f"PA={np.mean([m.mean_pa for m in group]):.3f} | "
              f"RME={np.mean([m.rme for m in group]):.3f} | "
              f"AIS={np.mean([m.ais for m in group]):.2f}")

    # -- hypothesis test ------------------------------------------------
    r_sse = np.mean([m.mean_sse for m in results["REACTIVE"]])
    m_sse = np.mean([m.mean_sse for m in results["SARAH/MILK"]])
    r_col = np.mean([m.collisions for m in results["REACTIVE"]])
    m_col = np.mean([m.collisions for m in results["SARAH/MILK"]])

    print("\nHYPOTHESIS: MILK produces lower state-transition error than a")
    print("            conventional reactive controller.")
    print(f"  mean SSE  reactive = {r_sse:.4f}   MILK = {m_sse:.4f}   "
          f"-> {'SUPPORTED' if m_sse < r_sse else 'NOT SUPPORTED'}")
    print(f"  mean collisions  reactive = {r_col:.2f}   MILK = {m_col:.2f}   "
          f"-> {'SUPPORTED' if m_col <= r_col else 'NOT SUPPORTED'}")
    print("=" * 108)

    return results


# =====================================================================
# 14.  Self-test
# =====================================================================

def selftest() -> bool:
    """Sanity checks on the core MILK mathematics and components."""
    ok = True

    def check(name: str, cond: bool, detail: str = "") -> None:
        nonlocal ok
        status = "PASS" if cond else "FAIL"
        print(f"[{status}] {name} {detail}")
        ok = ok and cond

    cfg = MILKConfig()

    # --- MILK dynamic equation ----------------------------------------
    X = milk_dynamic_equation(np.array([0.1, 0.1]), 0.5, 0.5, cfg.u_min)
    check("MILK eq. basic", np.allclose(X, [1.1, 1.1]), f"X={X}")

    X1 = milk_dynamic_equation(np.array([0.0]), 0.5, 0.1, cfg.u_min)
    X2 = milk_dynamic_equation(np.array([0.0]), 0.5, 0.9, cfg.u_min)
    check("MILK eq. uncertainty reduces influence", float(X1[0]) > float(X2[0]),
          f"{X1[0]:.2f} > {X2[0]:.2f}")

    Xf = milk_dynamic_equation(np.array([0.0]), 0.5, 0.0, cfg.u_min)
    check("MILK eq. finite at U=0", np.isfinite(Xf[0]), f"X={Xf[0]:.1f}")

    # --- RMI ------------------------------------------------------------
    a = state_vector([0, 0, 0], [0, 0], [1, 1], {0: np.array([2.0, 2.0])})
    b = state_vector([0, 0, 0], [0, 0], [1, 1], {0: np.array([2.0, 2.0])})
    c = state_vector([3, 4, 0], [0, 0], [1, 1], {0: np.array([2.0, 2.0])})
    check("RMI identical states == 0", abs(reality_modification_index(a, b)) < 1e-9)
    check("RMI grows with displacement", reality_modification_index(a, c) > 4.9)

    # --- differential drive --------------------------------------------
    pose = np.array([0.0, 0.0, 0.0])
    vel = np.array([0.0, 0.0])
    p1, v1 = integrate_diff_drive(pose, vel, np.array([1.0, 0.0]), cfg)
    check("diff-drive straight line", abs(p1[0] - 0.02) < 1e-9 and abs(p1[1]) < 1e-9,
          f"pose={p1}")

    # --- Kalman filter convergence -------------------------------------
    obj = TrackedObject(0, 0.0, 0.0)
    rng = np.random.default_rng(0)
    tx, ty = 1.0, 2.0
    vx, vy = 0.5, 0.2
    for k in range(60):
        obj.predict(0.1, 0.35)
        tx += vx * 0.1
        ty += vy * 0.1
        obj.update(np.array([tx, ty]) + rng.normal(0, 0.08, 2),
                   np.eye(2) * 0.08 ** 2)
    check("KF converges to true position",
          float(np.linalg.norm(obj.position - [tx, ty])) < 0.15,
          f"err={np.linalg.norm(obj.position - [tx,ty]):.4f}")
    check("KF estimates velocity",
          float(np.linalg.norm(obj.velocity - [vx, vy])) < 0.20,
          f"err={np.linalg.norm(obj.velocity - [vx,vy]):.4f}")

    # --- environment -----------------------------------------------------
    env = RoomEnvironment(cfg, seed=1)
    readings = env.raycast(env.robot_pose, np.linspace(0, 2 * math.pi, 16, endpoint=False))
    check("raycast within sensor range",
          bool(np.all(readings >= 0) and np.all(readings <= cfg.sensor_range + 1e-9)))

    # --- short smoke run --------------------------------------------------
    m = run_trial("MILK", cfg, seed=0, max_steps=60, verbose=False)
    check("MILK produces finite metrics",
          math.isfinite(m.mean_sse) and math.isfinite(m.ais))

    m2 = run_trial("REACTIVE", cfg, seed=0, max_steps=60, verbose=False)
    check("Reactive produces finite metrics",
          math.isfinite(m2.mean_sse) and math.isfinite(m2.ais))

    print("\nSELFTEST:", "ALL PASS" if ok else "FAILURES DETECTED")
    return ok


# =====================================================================
# 15.  Entry point
# =====================================================================

def main() -> int:
    parser = argparse.ArgumentParser(
        description="MILK Protocol v1.0 -- reference implementation")
    parser.add_argument("--trials", type=int, default=3,
                        help="number of trial pairs (default: 3)")
    parser.add_argument("--seed", type=int, default=0,
                        help="base random seed")
    parser.add_argument("--steps", type=int, default=400,
                        help="max steps per trial")
    parser.add_argument("--demo", action="store_true",
                        help="render a single MILK episode in the terminal")
    parser.add_argument("--render", action="store_true",
                        help="render during the experiment (slow)")
    parser.add_argument("--controller", type=str, default="MILK",
                        choices=["MILK", "REACTIVE"],
                        help="controller used with --demo")
    parser.add_argument("--json", type=str, default=None,
                        help="write results to a JSON file")
    parser.add_argument("--selftest", action="store_true",
                        help="run internal sanity checks and exit")
    parser.add_argument("--horizon", type=int, default=None,
                        help="override prediction horizon")
    args = parser.parse_args()

    cfg = MILKConfig(seed=args.seed)
    if args.horizon is not None:
        cfg.horizon = args.horizon

    if args.selftest:
        return 0 if selftest() else 1

    if args.demo:
        print(f"Rendering a single episode with the {args.controller} "
              f"controller. Ctrl-C to stop.\n")
        run_trial(args.controller, cfg, seed=args.seed,
                  max_steps=args.steps, render=True, render_every=4)
        return 0

    results = run_experiment(cfg, n_trials=args.trials,
                             max_steps=args.steps, verbose=True)

    if args.json:
        payload = {
            name: [asdict(m) for m in group]
            for name, group in results.items()
        }
        with open(args.json, "w") as fh:
            json.dump(payload, fh, indent=2)
        print(f"\nWrote {args.json}")

    return 0


if __name__ == "__main__":
    sys.exit(main())

You can encourage my continued useless #poetry, creativity and expression of self, #commentary, random thoughts, #philosophy and ideas, and by doing so your helping to feed, house and clothe a #disabled man living in #poverty, $5-10-15 It All Helps, via #cashapp at $woctxphotog or via #paypal at paypal.com/donate?campaign_id=…

#TheoreticalEngineering, #TheoreticalComputing, ##TheoreticalRobotics, #TheoreticalAi

#humanmachineinteraction #predictiveintelligence #theoreticalai #theoreticalcomputing #theoreticalengineering #worldmodels