Download runtime/rl/events.py from BAAI/PingPongRealWorldRL: direct link, hf CLI and curl.
- Browser
- Download file 18.3 kB
-
https://huggingface.co/BAAI/PingPongRealWorldRL/resolve/main/runtime/rl/events.py
- Command line
-
hf download hf://BAAI/PingPongRealWorldRL/runtime/rl/events.py
-
curl -L -o events.py https://huggingface.co/BAAI/PingPongRealWorldRL/resolve/main/runtime/rl/events.py
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" | |
| 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)] | |
| 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) | |
| 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) | |
| 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)) | |
| 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 [] | |