KurisiTOMA's picture
chore: publish H2 real-world RL system
d8290eb verified
Raw History Blame Contribute Delete
18.3 kB
"""Measured-ball adjudication. No outgoing simulation or virtual contact model."""
from __future__ import annotations
from collections import deque
from dataclasses import dataclass, replace
import math
import queue
import threading
import time
import numpy as np
from ..ball import BALL_RADIUS, TABLE_HEIGHT, TABLE_X_RANGE, TABLE_HALF_WIDTH, NET_X, NET_HEIGHT
from .config import RLConfig
from .position_reward import RacketHistory
ADJUDICATION_VERSION = "adapter-velocity-first-landing-v2"
@dataclass(frozen=True)
class BallSample:
source_s: float
receipt_s: float
monotonic_s: float
position: np.ndarray
velocity: np.ndarray
velocity_valid: bool
def __post_init__(self):
for name in ("position", "velocity"):
value = np.asarray(getattr(self, name), dtype=np.float64)
if value.shape != (3,) or not np.isfinite(value).all():
raise ValueError(f"invalid ball {name}")
object.__setattr__(self, name, value.copy())
if not isinstance(self.velocity_valid, (bool, np.bool_)):
raise ValueError("invalid ball velocity_valid: expected bool")
if not np.isfinite([self.source_s, self.receipt_s, self.monotonic_s]).all():
raise ValueError("invalid ball timestamps")
def row(self):
return [self.source_s, self.receipt_s, self.monotonic_s, *self.position.tolist()]
def velocity_row(self):
return [*self.velocity.tolist(), int(self.velocity_valid)]
@dataclass(frozen=True)
class BallEvent:
name: str
source_s: float
monotonic_s: float
confirmed_s: float
position: tuple[float, float, float]
def to_dict(self):
return dict(event=self.name, source_s=self.source_s, monotonic_s=self.monotonic_s,
confirmed_s=self.confirmed_s, position=list(self.position))
class BallDecoder:
def __init__(self, origin):
self.origin = np.asarray(origin, dtype=np.float64).reshape(3)
if not np.isfinite(self.origin).all():
raise ValueError("invalid mocap origin")
self.last_source_s = None
self.offset_s = None
def decode(self, payload, receipt_s):
if not isinstance(payload, dict) or payload.get("type") != "mocap":
raise ValueError("expected combined type=mocap payload")
ball = payload.get("ball")
if ball is None:
return None
if not isinstance(ball, dict) or ball.get("type") != "ball" or ball.get("unit") != "mm":
raise ValueError("expected nested type=ball, unit=mm")
stamp = float(ball.get("t", math.nan))
pos = np.asarray(ball.get("pos"), dtype=np.float64)
if not math.isfinite(stamp) or stamp <= 0 or pos.shape != (3,) or not np.isfinite(pos).all():
raise ValueError("invalid ball position/source timestamp")
velocity = np.asarray(ball.get("vel"), dtype=np.float64)
velocity_valid = ball.get("vel_valid")
if velocity.shape != (3,) or not np.isfinite(velocity).all():
raise ValueError("invalid_ball_velocity: expected finite vel[3]")
if not isinstance(velocity_valid, bool):
raise ValueError("invalid_ball_velocity_valid: expected bool vel_valid")
if self.last_source_s is not None:
if stamp < self.last_source_s - 1e-9:
self.last_source_s = stamp
self.offset_s = receipt_s - stamp
raise ValueError("ball_time_backwards")
if stamp <= self.last_source_s + 1e-9:
return None # robot-only updates cannot refresh ball freshness
self.last_source_s = stamp
offset = receipt_s - stamp
# Nested source stamps can use an epoch unrelated to monotonic time.
# Minimum observed transport offset removes queueing delay; source and
# receipt stamps are both retained so the alignment remains auditable.
self.offset_s = offset if self.offset_s is None else min(self.offset_s, offset)
return BallSample(stamp, receipt_s, stamp + self.offset_s, pos * 0.001 - self.origin,
velocity * 0.001, velocity_valid)
class BallStream:
"""Dedicated ordered SUB thread; never CONFLATE or run ROS on the policy thread."""
def __init__(self, config: RLConfig, origin):
self.config = config
self.decoder = BallDecoder(origin)
self.racket = RacketHistory(origin)
self.latest_ball = None
self.pair_lock = threading.Lock()
self.samples = queue.Queue(maxsize=config.ball_queue_capacity)
self.stop = threading.Event()
self.ready = threading.Event()
self.last_receipt_s = 0.0
self.error_generation = 0
self.last_error = ""
self.thread = threading.Thread(target=self._run, name="h2-real-ball-sub", daemon=True)
self.thread.start()
def _error(self, reason):
self.last_error = str(reason)
self.error_generation += 1
def _run(self):
import zmq
context = zmq.Context()
socket = context.socket(zmq.SUB)
socket.setsockopt(zmq.RCVHWM, self.config.ball_queue_capacity)
socket.setsockopt(zmq.LINGER, 0)
socket.setsockopt_string(zmq.SUBSCRIBE, "")
try:
socket.connect(self.config.zmq_endpoint)
self.ready.set()
while not self.stop.is_set():
if not socket.poll(10):
continue
try:
payload, receipt = socket.recv_pyobj(), time.monotonic()
with self.pair_lock:
sample = self.decoder.decode(payload, receipt)
# Decode racket-only snapshots even when the ball stamp
# repeats; ROS callback order must not lose paired poses.
self.racket.update(payload, receipt)
if sample is not None:
self.latest_ball = sample
if sample is None:
continue
self.last_receipt_s = sample.receipt_s
self.samples.put_nowait(sample)
except queue.Full:
self._error("ball_queue_overflow")
except (ValueError, TypeError, KeyError, OverflowError) as error:
self._error(error)
except Exception as error:
self._error(f"ball_source_failed:{error}")
finally:
socket.close()
context.term()
def drain(self):
result = []
for _ in range(self.config.ball_queue_capacity):
try:
result.append(self.samples.get_nowait())
except queue.Empty:
break
return result
def racket_distance(self, now_s):
with self.pair_lock:
return self.racket.distance(self.latest_ball, now_s, self.config.max_ball_gap_s)
def close(self):
self.stop.set()
self.thread.join(timeout=2)
@dataclass(frozen=True)
class CourtGeometry:
table_x: tuple[float, float] = TABLE_X_RANGE
table_half_width: float = TABLE_HALF_WIDTH
table_center_y: float = 0.0
contact_z: float = TABLE_HEIGHT + BALL_RADIUS
net_x: float = NET_X
net_top: float = NET_HEIGHT
radius: float = BALL_RADIUS
hit_x: tuple[float, float] = (0.35, 0.45)
hit_y: tuple[float, float] = (-0.76, 0.76)
hit_z: tuple[float, float] = (0.80, 1.50)
@classmethod
def from_spec(cls, spec, origin=(-1.87, 0.0, -0.76)):
# Re-express the audited physical court if CLI calibration changes.
shift = np.array([-1.87, 0.0, -0.76]) - np.asarray(origin, dtype=float)
return cls(table_x=tuple(float(x+shift[0]) for x in TABLE_X_RANGE),
table_center_y=float(shift[1]), contact_z=float(TABLE_HEIGHT+BALL_RADIUS+shift[2]),
net_x=float(NET_X+shift[0]), net_top=float(NET_HEIGHT+shift[2]),
hit_x=spec.hit_x_range, hit_y=spec.hit_y_range, hit_z=spec.hit_z_range)
def on_table(self, position):
x, y = position[:2]
return self.table_x[0] <= x <= self.table_x[1] and abs(y-self.table_center_y) <= self.table_half_width
def hit_region(self, position):
return all(lo <= value <= hi for value, (lo, hi) in zip(position, (self.hit_x, self.hit_y, self.hit_z)))
class RealBallDetector:
"""Conservative trajectory evidence; ambiguous outcomes are discarded upstream."""
def __init__(self, config: RLConfig, geometry: CourtGeometry):
self.config, self.geometry = config, geometry
# velocity_window is retained for checkpoint/config compatibility. It
# now controls only the local position window used to locate a landing.
self.history = deque(maxlen=config.velocity_window)
self.before_return = []
self.last_sample = None
self.phase = "pre_hit"
self.incoming_seen = False
self.outgoing_count = 0
self.miss_candidate = None
self.hit_event = None
self.terminal = None
self.invalid_reason = None
self.last_bounce_source = -math.inf
self.net_reverse_count = 0
self.net_turning = None
self.terminal_candidates = []
def _event(self, name, sample, *, source_s=None, position=None):
source_s = sample.source_s if source_s is None else source_s
position = sample.position if position is None else position
return BallEvent(name, source_s, sample.monotonic_s - (sample.source_s - source_s),
sample.receipt_s, tuple(float(v) for v in position))
@staticmethod
def _fit_positions(samples):
"""Local geometric interpolation for landing time/XY only."""
times = np.array([s.source_s for s in samples])
design = np.column_stack((times - times[-1], np.ones(len(times))))
positions = np.array([s.position for s in samples])
slope, intercept = np.linalg.lstsq(design, positions, rcond=None)[0]
return slope, intercept
def _bounce(self, sample):
window = list(self.history)[-self.config.velocity_window:]
if len(window) < self.config.velocity_window:
return None
zs = np.array([s.position[2] for s in window])
i = int(np.argmin(zs))
if i == 0 or i == len(window)-1 or window[i].source_s <= self.last_bounce_source:
return None
g = self.geometry
if abs(zs[i] - g.contact_z) > self.config.table_height_tolerance_m:
return None
before, after = window[:i+1], window[i:]
v0, p0 = self._fit_positions(before)
v1, p1 = self._fit_positions(after)
if v0[2] >= 0 or v1[2] <= 0:
return None
# Intersection of the two measured local vertical fits, bounded by the
# samples adjacent to the observed minimum. No ballistic rollout.
ref = window[-1].source_s
b0 = p0[2] + v0[2] * (ref - before[-1].source_s)
b1 = p1[2] + v1[2] * (ref - after[-1].source_s)
t = ref + (b1-b0) / (v0[2]-v1[2])
if not window[i-1].source_s <= t <= window[i+1].source_s:
self.invalid_reason = "ambiguous_bounce_fit"
return None
fit_z = b0 + v0[2] * (t-ref)
if abs(fit_z - g.contact_z) > self.config.table_height_tolerance_m:
self.invalid_reason = "ambiguous_bounce_height"
return None
slope, intercept = self._fit_positions(window)
position = intercept + slope * (t-ref)
position[2] = g.contact_z
self.last_bounce_source = window[i].source_s
if not g.on_table(position):
return self._event("out", sample, source_s=t, position=position)
name = "bounce_opponent" if position[0] > g.net_x else "bounce_ours"
return self._event(name, sample, source_s=t, position=position)
def update(self, sample: BallSample, hit_deadline_s: float):
if self.invalid_reason or self.terminal is not None:
return []
previous = self.last_sample
if previous is not None:
gap = sample.source_s - previous.source_s
if gap <= 0 or gap > self.config.max_ball_gap_s + 1e-9:
self.invalid_reason = "ball_time_backwards" if gap <= 0 else "ball_source_gap"
return []
self.last_sample = sample
if self.phase == "post_hit":
return self._update_post_hit(sample)
c, g = self.config, self.geometry
if len(self.before_return) >= c.ball_queue_capacity:
self.invalid_reason = "ball_history_overflow"
return []
self.before_return.append(sample)
vx = float(sample.velocity[0]) if sample.velocity_valid else None
self.incoming_seen |= vx is not None and vx < c.incoming_vx_mps
self.outgoing_count = (self.outgoing_count + 1 if self.incoming_seen and
vx is not None and vx > c.outgoing_vx_mps else 0)
if self.outgoing_count >= c.outgoing_confirmations:
turning_index = min(range(len(self.before_return)),
key=lambda i: self.before_return[i].position[0])
turning = self.before_return[turning_index]
# Keep the legacy log name, but this is a confirmed return direction,
# not an estimate of racket contact or an additional reward gate.
self.hit_event = replace(self._event("our_hit", turning), confirmed_s=sample.receipt_s)
self.phase = "post_hit"
result = [self.hit_event]
# Adapter velocity may confirm the return after the first landing.
# Replay positions from the X turn, in source order, exactly once.
for earlier in self.before_return[turning_index:]:
result.extend(self._update_post_hit(earlier, confirmed_s=sample.receipt_s))
if self.terminal is not None or self.invalid_reason:
break
self.before_return.clear()
return result
miss_evidence = (self.incoming_seen and vx is not None and vx < c.incoming_vx_mps and
sample.monotonic_s >= hit_deadline_s and
sample.position[0] < g.hit_x[0] - g.radius and
previous is not None and sample.position[0] < previous.position[0])
if miss_evidence:
if not (g.hit_y[0] <= sample.position[1] <= g.hit_y[1] and
g.hit_z[0] <= sample.position[2] <= g.hit_z[1]):
self.invalid_reason = "incoming_outside_h2_hit_region"
return []
if self.miss_candidate is not None:
self.terminal = replace(self._event("no_hit", self.miss_candidate),
confirmed_s=sample.receipt_s)
return [self.terminal]
self.miss_candidate = sample
else:
self.miss_candidate = None
return []
def _update_post_hit(self, sample, *, confirmed_s=None):
self.history.append(sample)
if len(self.history) < 2:
return []
c, g = self.config, self.geometry
candidates = []
bounce = self._bounce(sample)
if self.invalid_reason:
return []
if bounce and bounce.source_s > self.hit_event.source_s:
candidates.append(bounce)
prev = self.history[-2]
p, q = prev.position, sample.position
# Observed intersection with the physical net does not require valid
# velocity. Invalid adapter velocity never counts as a net reversal.
if p[0] < g.net_x <= q[0] and q[0] > p[0]:
alpha = (g.net_x-p[0])/(q[0]-p[0])
cross = p + alpha*(q-p)
if abs(cross[1]-g.table_center_y) <= g.table_half_width and g.contact_z-g.radius <= cross[2] <= g.net_top + g.radius:
candidates.append(self._event("net_contact", sample,
source_s=prev.source_s + alpha*(sample.source_s-prev.source_s), position=cross))
if self.net_turning is None or sample.position[0] > self.net_turning.position[0]:
self.net_turning = sample
self.net_reverse_count = (self.net_reverse_count + 1 if sample.velocity_valid and
sample.velocity[0] < 0 else 0)
if self.net_reverse_count >= c.outgoing_confirmations:
turning = self.net_turning
r = turning.position
if (abs(r[0]-g.net_x) <= c.net_x_tolerance_m and abs(r[1]-g.table_center_y) <= g.table_half_width and
g.contact_z-g.radius <= r[2] <= g.net_top + g.radius and
turning.source_s > self.hit_event.source_s):
candidates.append(self._event("net_contact", sample,
source_s=turning.source_s, position=r))
# A measured downward crossing outside the exact table footprint is out.
if p[2] > g.contact_z >= q[2] and q[2] < p[2]:
alpha = (g.contact_z-p[2])/(q[2]-p[2])
cross = p + alpha*(q-p)
if not g.on_table(cross):
candidates.append(self._event("out", sample,
source_s=prev.source_s + alpha*(sample.source_s-prev.source_s), position=cross))
self.terminal_candidates.extend(candidates)
if not self.terminal_candidates:
return []
first = min(self.terminal_candidates, key=lambda e: e.source_s)
if any(e.name != first.name and abs(e.source_s-first.source_s) <= 1e-6
for e in self.terminal_candidates):
self.invalid_reason = "conflicting_terminal_events"
return []
# Let the local landing window pass the earliest candidate before
# committing: an earlier rebound can need more frames than a net/out
# crossing. This keeps physical event order, not detection order.
if len(self.history) == c.velocity_window and first.source_s <= self.history[0].source_s + 1e-9:
self.terminal = replace(first, confirmed_s=max(sample.receipt_s,
confirmed_s if confirmed_s is not None else sample.receipt_s))
return [self.terminal]
return []