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
