| |
| """Predict real-ball strikes and publish H2 policy-domain commands over ROS 2.""" |
|
|
| import time |
| import math |
| import argparse |
| from collections import Counter |
| import numpy as np |
| import zmq |
|
|
| import rclpy |
| from std_msgs.msg import Float64MultiArray |
|
|
|
|
| from runtime.prediction_filters import ( |
| CausalMarkerBallPhaseGate, |
| CausalPredictionStabilizer, |
| LEGACY_V2_HIT_VELOCITY_BIAS, |
| ) |
| from runtime.legacy_v2_predictor import HybridTrajectoryPredictor |
| from runtime.planner_protocol import H2_PLANNER_PACKET_LEN, build_h2_planner_packet |
| from runtime.provenance import verify_artifacts |
|
|
|
|
| def make_latest_sub_socket(context: zmq.Context, endpoint: str) -> zmq.Socket: |
| """Create a one-message latest-state SUB socket for combined mocap packets.""" |
| socket = context.socket(zmq.SUB) |
| socket.setsockopt(zmq.CONFLATE, 1) |
| socket.setsockopt(zmq.RCVHWM, 1) |
| socket.setsockopt(zmq.LINGER, 0) |
| socket.setsockopt_string(zmq.SUBSCRIBE, "") |
| socket.connect(endpoint) |
| return socket |
|
|
|
|
| def _source_frames_ready( |
| robot_source_monotonic_s: float, |
| ball_source_monotonic_s: float, |
| ) -> bool: |
| """Return true only after both real mocap sources delivered a first frame.""" |
| return ( |
| math.isfinite(float(robot_source_monotonic_s)) |
| and math.isfinite(float(ball_source_monotonic_s)) |
| and float(robot_source_monotonic_s) > 0.0 |
| and float(ball_source_monotonic_s) > 0.0 |
| ) |
|
|
|
|
| class PredictionGateDiagnostics: |
| """Rate-limited counters for otherwise silent prediction gates.""" |
|
|
| ORDER = ( |
| "ball_samples", |
| "velocity_invalid", |
| "phase_waiting", |
| "phase_incoming_slow", |
| "phase_outgoing", |
| "phase_missed", |
| "observation_x", |
| "observation_y", |
| "observation_z", |
| "input_pass", |
| "predictor_no_hit", |
| "predictor_nonfinite", |
| "ball_too_low", |
| "policy_domain", |
| "stabilizer_warmup", |
| "stable_prediction", |
| "wait_lock_far", |
| "lock_too_late", |
| "first_lock", |
| "lock_refresh", |
| "crossed_without_lock", |
| ) |
|
|
| def __init__(self, report_interval_s: float) -> None: |
| self.report_interval_s = max(0.0, float(report_interval_s)) |
| self.total = Counter() |
| self._last_report = Counter() |
| self._last_report_monotonic = time.monotonic() |
|
|
| def mark(self, reason: str) -> None: |
| self.total[reason] += 1 |
|
|
| def report_if_due(self, *, force: bool = False) -> None: |
| if self.report_interval_s <= 0.0: |
| return |
| now = time.monotonic() |
| elapsed = now - self._last_report_monotonic |
| if not force and elapsed < self.report_interval_s: |
| return |
| delta = { |
| key: self.total[key] - self._last_report[key] |
| for key in self.ORDER |
| if self.total[key] != self._last_report[key] |
| } |
| self._last_report = self.total.copy() |
| self._last_report_monotonic = now |
| if not delta: |
| return |
| fields = " ".join(f"{key}={value}" for key, value in delta.items()) |
| print(f"[PREDICT GATES {elapsed:.1f}s] {fields}") |
|
|
| def report_total(self) -> None: |
| if self.report_interval_s <= 0.0 or not self.total: |
| return |
| fields = " ".join( |
| f"{key}={self.total[key]}" for key in self.ORDER if self.total[key] |
| ) |
| print(f"[PREDICT GATES TOTAL] {fields}") |
|
|
|
|
| class LegacyV2HitPlanePredictor: |
| """Expose the fitted V2 model through this node's hit-plane API.""" |
|
|
| def __init__(self) -> None: |
| self._predictor = HybridTrajectoryPredictor() |
|
|
| def predict_hitting_point( |
| self, |
| p_init: np.ndarray, |
| v_init: np.ndarray, |
| target_x: float, |
| ): |
| trajectory, hit_velocity, flight_time = self._predictor.trajectory_to_x( |
| p_init, |
| v_init, |
| target_x=target_x, |
| table_contact_in_range=lambda pos: ( |
| -1.37 <= float(pos[0]) <= 0.0 and abs(float(pos[1])) <= 0.76 |
| ), |
| max_time=2.0, |
| dt=0.005, |
| ) |
| hit_position = trajectory[-1] if hit_velocity is not None else None |
| return hit_position, hit_velocity, flight_time |
|
|
|
|
| |
| |
| |
| def _solve_v_outgoing(predictor, p_racket: np.ndarray, p_landing: np.ndarray, |
| dt_flight: float, n_iter: int = 4) -> np.ndarray: |
| """Newton iteration: find v_out so the ball reaches p_landing in dt_flight |
| under the full physics model (drag + spin_az + gravity). |
| |
| Starts from the parabolic estimate with g_eff, then corrects via: |
| v += (p_target - p_predicted) / dt_flight |
| Converges in 3-4 iterations for typical outgoing trajectories (<10 m, <0.6 s). |
| """ |
| g_eff = predictor.g + predictor.spin_az |
| v = (p_landing - p_racket) / dt_flight + np.array([0.0, 0.0, 0.5 * g_eff * dt_flight]) |
| for _ in range(n_iter): |
| p_pred, _ = predictor.predict_state_at_time(p_racket, v, dt_flight) |
| v = v + (p_landing - p_pred) / dt_flight |
| return v |
|
|
|
|
| def calculate_racket_command(p_racket: np.ndarray, v_incoming: np.ndarray, |
| p_landing: np.ndarray, dt_flight: float, Cr: float = 0.70, |
| predictor=None): |
| """Compute required racket velocity for a given outgoing target. |
| |
| A predictor exposing ``g``, ``spin_az`` and ``predict_state_at_time`` uses |
| the full iterative flight solve. The standalone legacy-v2 wrapper only |
| predicts trajectories to an x plane, so it deliberately uses the |
| parabolic fallback for this compatibility-only racket-velocity field. |
| """ |
| supports_outgoing_solver = ( |
| predictor is not None |
| and hasattr(predictor, "g") |
| and hasattr(predictor, "spin_az") |
| and callable(getattr(predictor, "predict_state_at_time", None)) |
| ) |
| if supports_outgoing_solver: |
| v_outgoing = _solve_v_outgoing(predictor, p_racket, p_landing, dt_flight) |
| else: |
| g_eff = 9.81 |
| v_outgoing = (p_landing - p_racket) / dt_flight + np.array([0.0, 0.0, 0.5 * g_eff * dt_flight]) |
|
|
| v_diff = v_outgoing - v_incoming |
| norm_v_diff = np.linalg.norm(v_diff) |
|
|
| if norm_v_diff < 1e-6: |
| return np.zeros(3) |
|
|
| u = v_diff / norm_v_diff |
| v_racket_mag = (np.dot(v_outgoing, u) + Cr * np.dot(v_incoming, u)) / (1.0 + Cr) |
| return v_racket_mag * u |
|
|
|
|
| |
| |
| |
| class FastTrajectoryPredictor: |
| def __init__(self, table_height: float = 0.008): |
| |
| |
| self.m = 0.0027 |
| self.g = 9.81 |
| |
| |
| self.spin_az = 0.340 |
| self.Cd_x = 0.000447525 |
| self.Cd_y = 0.000684450 |
| self.Cd_z = 0.000590625 |
| self.table_height = table_height |
|
|
| |
| self.k = np.array([0.735, 0.446, -0.993]) |
| self.b = np.array([-0.110, -0.020, 0.0]) |
|
|
| def predict_state_at_time(self, p0: np.ndarray, v0: np.ndarray, t: float): |
| x0, y0, z0 = p0 |
| Vx0, Vy0, Vz0 = v0 |
| if t < 1e-9: return np.copy(p0), np.copy(v0) |
|
|
| Vx = self.m * Vx0 / (self.Cd_x * Vx0 * t + self.m) if Vx0 > 0 else self.m * Vx0 / (self.m - self.Cd_x * Vx0 * t) |
| x = x0 + (self.m / self.Cd_x) * np.log(1 + self.Cd_x * Vx0 * t / self.m) if Vx0 > 0 else x0 - (self.m / self.Cd_x) * np.log(1 - self.Cd_x * Vx0 * t / self.m) |
|
|
| Vy = self.m * Vy0 / (self.Cd_y * Vy0 * t + self.m) if Vy0 > 0 else self.m * Vy0 / (self.m - self.Cd_y * Vy0 * t) |
| y = y0 + (self.m / self.Cd_y) * np.log(1 + self.Cd_y * Vy0 * t / self.m) if Vy0 > 0 else y0 - (self.m / self.Cd_y) * np.log(1 - self.Cd_y * Vy0 * t / self.m) |
|
|
| g_eff = self.g + self.spin_az |
| mg_Cd = np.sqrt(self.m * g_eff / self.Cd_z) |
| sqrt_term = np.sqrt(self.Cd_z * g_eff / self.m) |
|
|
| if Vz0 >= 0: |
| t_peak = (1 / sqrt_term) * np.arctan(Vz0 / mg_Cd) if mg_Cd > 1e-9 else 0.0 |
| if t <= t_peak: |
| Vz = mg_Cd * np.tan(-sqrt_term * t + np.arctan(Vz0 / mg_Cd)) |
| log_val = np.cos(sqrt_term * t) + (Vz0 / mg_Cd) * np.sin(sqrt_term * t) |
| z = z0 + (self.m / self.Cd_z) * np.log(log_val) if log_val > 1e-9 else z0 |
| else: |
| p_peak, v_peak = self.predict_state_at_time(p0, v0, t_peak) |
| t_fall = t - t_peak |
| exp_term = np.exp(2 * t_fall * sqrt_term) |
| Vz = mg_Cd * (1 - exp_term) / (1 + exp_term) + v_peak[2] |
| z = p_peak[2] - (self.m / (2 * self.Cd_z)) * np.log(((1 + exp_term) ** 2) / (4 * exp_term)) + v_peak[2] * t_fall |
| else: |
| ratio_v = np.clip(-Vz0 / mg_Cd, 0.0, 1.0 - 1e-6) |
| phase = np.arctanh(ratio_v) |
| Vz = -mg_Cd * np.tanh(sqrt_term * t + phase) |
| z = z0 + (mg_Cd / sqrt_term) * (np.log(np.cosh(phase)) - np.log(np.cosh(sqrt_term * t + phase))) |
|
|
| return np.array([x, y, z]), np.array([Vx, Vy, Vz]) |
|
|
| def predict_hitting_point(self, p_init: np.ndarray, v_init: np.ndarray, target_x: float = -1.37, max_time: float = 2.0, dt: float = 0.01): |
| t_total = 0.0 |
| t_seg = 0.0 |
| max_bounces = 3 |
| bounce_count = 0 |
|
|
| p0, v0 = np.copy(p_init), np.copy(v_init) |
| p_curr, v_curr = p0, v0 |
|
|
| if p0[2] < self.table_height: |
| p0[2] = self.table_height |
| p_curr = p0.copy() |
|
|
| while t_total < max_time: |
| t_seg += dt |
| t_total += dt |
| p_next, v_next = self.predict_state_at_time(p0, v0, t_seg) |
|
|
| if p_curr[0] >= target_x and p_next[0] < target_x: |
| ratio = (target_x - p_curr[0]) / (p_next[0] - p_curr[0]) |
| hit_y = p_curr[1] + ratio * (p_next[1] - p_curr[1]) |
| hit_z = p_curr[2] + ratio * (p_next[2] - p_curr[2]) |
| hit_pos = np.array([target_x, hit_y, hit_z]) |
|
|
| hit_vx = v_curr[0] + ratio * (v_next[0] - v_curr[0]) |
| hit_vy = v_curr[1] + ratio * (v_next[1] - v_curr[1]) |
| hit_vz = v_curr[2] + ratio * (v_next[2] - v_curr[2]) |
| hit_vel = np.array([hit_vx, hit_vy, hit_vz]) |
|
|
| hit_t = (t_total - dt) + ratio * dt |
| return hit_pos, hit_vel, hit_t |
|
|
| if p_curr[2] >= self.table_height and p_next[2] < self.table_height: |
| ratio = (self.table_height - p_curr[2]) / (p_next[2] - p_curr[2]) |
| bounce_t_seg = (t_seg - dt) + ratio * dt |
|
|
| p_impact, v_impact = self.predict_state_at_time(p0, v0, bounce_t_seg) |
| p_impact[2] = self.table_height |
| v_reb = self.k * v_impact + self.b |
|
|
| if abs(v_reb[2]) < 0.1: |
| return None, None, None |
|
|
| bounce_count += 1 |
| if bounce_count > max_bounces: |
| return None, None, None |
|
|
| p0, v0 = p_impact, v_reb |
| t_seg = 0.0 |
| t_total = (t_total - dt) + ratio * dt |
| p_next, v_next = p0, v0 |
|
|
| p_curr, v_curr = p_next, v_next |
|
|
| return None, None, None |
|
|
|
|
| |
| |
| |
|
|
| def main(args=None): |
| parser = argparse.ArgumentParser(description="Table Tennis Strategy Node") |
| parser.add_argument("--sim-ball-only", action="store_true", |
| help="MuJoCo only: ignore robot mocap; publish idle packets before the first ball.") |
| parser.add_argument("--topic", default="/wbc_racket_command") |
| parser.add_argument("--zmq-endpoint", default="tcp://127.0.0.1:5556", |
| help="Adapter endpoint; one-click deployment passes its private endpoint.") |
| parser.add_argument("--debug-log", default=None, |
| help="Optional path for command publish logs, e.g. /tmp/tmp.txt") |
| parser.add_argument("--publish-hz", type=float, default=500.0, |
| help="Maximum command publish rate. Use <=0 to disable rate limiting.") |
| parser.add_argument( |
| "--gate-log-interval", |
| type=float, |
| default=1.0, |
| help="Seconds between prediction-gate summaries. Use <=0 to disable them.", |
| ) |
| parser.add_argument( |
| "--robot-base-origin-world", type=float, nargs=3, default=(-1.87, 0.0, -0.76), |
| metavar=("X", "Y", "Z"), help="Ready-pose origin in the mocap world frame.", |
| ) |
| parser.add_argument( |
| "--hit-plane-x", type=float, default=0.40, |
| help="Policy hit plane x in the robot court frame.", |
| ) |
| parser.add_argument( |
| "--hit-y-range", type=float, nargs=2, default=(-0.76, 0.76), |
| metavar=("MIN", "MAX"), help="Accepted policy hit y range in the robot court frame.", |
| ) |
| parser.add_argument( |
| "--hit-z-range", type=float, nargs=2, default=(0.80, 1.50), |
| metavar=("MIN", "MAX"), help="Accepted policy hit z range in the robot court frame.", |
| ) |
| parser.add_argument( |
| "--lock-time-threshold", type=float, default=0.52, |
| help="Start publishing an active strike command below this time-to-hit.", |
| ) |
| parser.add_argument( |
| "--prediction-model", choices=("legacy-v2", "deploy-closd"), default="legacy-v2", |
| help="Ball flight/bounce model. legacy-v2 matches the default MuJoCo replay model.", |
| ) |
| parser.add_argument( |
| "--incoming-velocity-bias", type=float, nargs=3, default=None, |
| metavar=("VX", "VY", "VZ"), |
| help=( |
| "Additive hit-plane velocity calibration in m/s. The default is " |
| "the recorded-ball legacy-v2 calibration, or zero for deploy-closd." |
| ), |
| ) |
| cli_args, remaining = parser.parse_known_args(args=args) |
| hit_y_range = tuple(sorted(map(float, cli_args.hit_y_range))) |
| hit_z_range = tuple(sorted(map(float, cli_args.hit_z_range))) |
| if hit_y_range[0] >= hit_y_range[1]: |
| parser.error("--hit-y-range must have MIN < MAX") |
| if hit_z_range[0] >= hit_z_range[1]: |
| parser.error("--hit-z-range must have MIN < MAX") |
| if cli_args.lock_time_threshold <= 0.0: |
| parser.error("--lock-time-threshold must be positive") |
|
|
| verified = verify_artifacts() |
| if "models/real_ball_hybrid_bounce.json" not in verified: |
| raise RuntimeError("the default calibrated ball model is not pinned in ARTIFACTS.sha256") |
|
|
| rclpy.init(args=remaining) |
| ros_node = rclpy.create_node('table_tennis_commander_node') |
|
|
| cmd_pub = ros_node.create_publisher(Float64MultiArray, cli_args.topic, 10) |
| ros_node.get_logger().info( |
| f"🏓 策略节点启动，将在 {cli_args.topic} 持续发布 H2 v3 指令 (Size=27)..." |
| ) |
|
|
| ZMQ_POLL_TIMEOUT_MS = 1 |
|
|
| context = zmq.Context() |
| socket = make_latest_sub_socket(context, cli_args.zmq_endpoint) |
|
|
| poller = zmq.Poller() |
| poller.register(socket, zmq.POLLIN) |
|
|
| print("[系统初始化] 已连接动捕数据源，等待接收数据...") |
|
|
| predictor = ( |
| LegacyV2HitPlanePredictor() |
| if cli_args.prediction_model == "legacy-v2" |
| else FastTrajectoryPredictor(table_height=0.008) |
| ) |
| incoming_velocity_bias = np.asarray( |
| cli_args.incoming_velocity_bias |
| if cli_args.incoming_velocity_bias is not None |
| else ( |
| LEGACY_V2_HIT_VELOCITY_BIAS |
| if cli_args.prediction_model == "legacy-v2" |
| else np.zeros(3, dtype=float) |
| ), |
| dtype=float, |
| ).reshape(3) |
|
|
| ROBOT_BASE_ORIGIN_WORLD = np.asarray(cli_args.robot_base_origin_world, dtype=float) |
| HIT_Y_RANGE = hit_y_range |
| HIT_Z_RANGE = hit_z_range |
| TARGET_HIT_X = float(ROBOT_BASE_ORIGIN_WORLD[0] + cli_args.hit_plane_x) |
| LOCK_TIME_THRESHOLD = float(cli_args.lock_time_threshold) |
| marker_phase = CausalMarkerBallPhaseGate(target_hit_x=TARGET_HIT_X) |
| gate_diagnostics = PredictionGateDiagnostics(cli_args.gate_log_interval) |
|
|
| p_landing_target = np.array([0.685, 0.0, 0.02]) |
| desired_flight_time = 0.6 |
|
|
| ball_incoming = False |
| has_crossed_target = False |
| pending_lock = False |
|
|
| rally_count = 0 |
| publish_count = 0 |
|
|
| |
| |
| has_ever_predicted = False |
|
|
| locked_hit_pos = None |
| locked_absolute_hit_time = None |
| locked_p_racket_relative = None |
| locked_v_racket_cmd_relative = None |
| locked_incoming_vel = None |
|
|
| tentative_hit_pos = None |
| tentative_hit_vel = None |
| tentative_absolute_hit_time = None |
| tentative_stamp = None |
| tentative_ball_pos = None |
| last_locked_tentative_stamp = None |
|
|
| prediction_stabilizer = None |
| domain_reject_reported = False |
|
|
| prev_pos_m = None |
| prev_mocap_time = None |
|
|
| current_robot_base_pos = np.array([-1.8, 0.0, 0.0]) |
| current_robot_base_quat = np.array([0.0, 0.0, 0.0, 1.0]) |
| last_ball_t = float("-inf") |
| last_robot_t = float("-inf") |
| last_robot_source_mono = 0.0 |
| last_ball_source_mono = 0.0 |
| last_mocap_time = None |
| wall_at_last_mocap = None |
|
|
| wall_time_at_lock = None |
| mocap_time_at_lock = None |
|
|
| |
| |
| |
| |
| |
| DEFAULT_T_STRIKE = -0.59 |
| DEFAULT_P_RACKET_RELATIVE = np.array([cli_args.hit_plane_x, 0.0, 1.0], dtype=float) |
| DEFAULT_V_RACKET_RELATIVE = np.array([1.1, 0.0, 0.3], dtype=float) |
|
|
| print( |
| "[策略域] " |
| f"model={cli_args.prediction_model}, " |
| f"vin_bias={np.array2string(incoming_velocity_bias, precision=3)}, " |
| f"world_x={TARGET_HIT_X:.3f}, court_x={cli_args.hit_plane_x:.3f}, " |
| f"y=[{HIT_Y_RANGE[0]:.2f},{HIT_Y_RANGE[1]:.2f}], " |
| f"z=[{HIT_Z_RANGE[0]:.2f},{HIT_Z_RANGE[1]:.2f}]" |
| ) |
| if cli_args.gate_log_interval > 0.0: |
| print( |
| f"[PREDICT GATES] 每 {cli_args.gate_log_interval:.1f}s 汇总一次；" |
| "计数单位是动捕帧；velocity/phase/observation 表示输入首先未通过的门，" |
| "其余是后续预测与锁定状态，不等同于丢球数量。" |
| ) |
|
|
| debug_log = open(cli_args.debug_log, "w", buffering=1) if cli_args.debug_log else None |
| if debug_log is not None: |
| debug_log.write("# timestamp ball_count t_strike " |
| "base_x base_y base_z qx qy qz qw " |
| "p_x p_y p_z v_racket_x v_racket_y v_racket_z " |
| "v_in_x v_in_y v_in_z\n") |
|
|
| ros_msg = Float64MultiArray() |
| publish_period = 1.0 / cli_args.publish_hz if cli_args.publish_hz > 0.0 else 0.0 |
| next_publish_mono = 0.0 |
|
|
| try: |
| while rclpy.ok(): |
| latest_robot_msg = None |
| latest_ball_msg = None |
|
|
| if poller.poll(ZMQ_POLL_TIMEOUT_MS): |
| try: |
| msg = socket.recv_pyobj(flags=zmq.NOBLOCK) |
| except zmq.Again: |
| msg = None |
|
|
| if msg is not None: |
| if msg.get("type") == "mocap": |
| robot_msg = None if cli_args.sim_ball_only else msg.get("robot") |
| ball_msg = msg.get("ball") |
|
|
| if robot_msg is not None: |
| robot_t = float(robot_msg["t"]) |
| if robot_t > last_robot_t: |
| latest_robot_msg = robot_msg |
| last_robot_t = robot_t |
|
|
| if ball_msg is not None: |
| ball_t = float(ball_msg["t"]) |
| if ball_t > last_ball_t: |
| latest_ball_msg = ball_msg |
| last_ball_t = ball_t |
|
|
| if latest_robot_msg is not None: |
| current_robot_base_pos = np.array(latest_robot_msg['pos'], dtype=float) / 1000.0 |
| current_robot_base_quat = np.array(latest_robot_msg['quat'], dtype=float) |
|
|
| wall_now = time.time() |
| mono_now = time.monotonic() |
| if latest_robot_msg is not None: |
| last_robot_source_mono = mono_now |
| if latest_ball_msg is not None: |
| last_ball_source_mono = mono_now |
|
|
| if latest_ball_msg is not None: |
| current_time = latest_ball_msg['t'] |
| pos_m = np.array(latest_ball_msg['pos'], dtype=float) / 1000.0 |
| vel_m = np.array(latest_ball_msg['vel'], dtype=float) / 1000.0 |
| gate_diagnostics.mark("ball_samples") |
| phase_transition = marker_phase.update(current_time, pos_m) |
| if phase_transition is not None: |
| previous_phase, current_phase = phase_transition |
| print( |
| f"[MARKER PHASE] {previous_phase}->{current_phase} " |
| f"vx={marker_phase.estimated_vx:+.3f}m/s x={pos_m[0]:+.3f}" |
| ) |
|
|
| velocity_valid = bool(latest_ball_msg.get("vel_valid", True)) |
| prediction_input_ready = False |
| if not velocity_valid: |
| gate_diagnostics.mark("velocity_invalid") |
| elif not marker_phase.allow_prediction_update: |
| phase_reason = { |
| marker_phase.WAITING: "phase_waiting", |
| marker_phase.INCOMING: "phase_incoming_slow", |
| marker_phase.OUTGOING: "phase_outgoing", |
| marker_phase.MISSED: "phase_missed", |
| }[marker_phase.state] |
| gate_diagnostics.mark(phase_reason) |
| elif not TARGET_HIT_X < pos_m[0] < 1.37: |
| gate_diagnostics.mark("observation_x") |
| elif not (-1.525 / 2) < pos_m[1] < (1.525 / 2): |
| gate_diagnostics.mark("observation_y") |
| elif not 0.0 < pos_m[2] < 1.0: |
| gate_diagnostics.mark("observation_z") |
| else: |
| gate_diagnostics.mark("input_pass") |
| prediction_input_ready = True |
|
|
| if prediction_input_ready: |
|
|
| if not ball_incoming: |
| ball_incoming = True |
| has_crossed_target = False |
| pending_lock = False |
| publish_count = -1 |
| prev_pos_m = None |
| prev_mocap_time = None |
| tentative_hit_pos = None |
| tentative_hit_vel = None |
| tentative_absolute_hit_time = None |
| tentative_stamp = None |
| tentative_ball_pos = None |
| last_locked_tentative_stamp = None |
| prediction_stabilizer = CausalPredictionStabilizer() |
| domain_reject_reported = False |
| print(f"\n[追踪中] 发现新来球，开始追踪...") |
|
|
| if pos_m[0] > TARGET_HIT_X: |
| hit_pos, hit_vel, flight_time = predictor.predict_hitting_point( |
| pos_m, vel_m, target_x=TARGET_HIT_X |
| ) |
| if hit_vel is not None: |
| hit_vel = np.asarray(hit_vel, dtype=float) + incoming_velocity_bias |
|
|
| if hit_pos is None: |
| gate_diagnostics.mark("predictor_no_hit") |
| elif hit_vel is None or flight_time is None or not ( |
| np.isfinite(hit_pos).all() |
| and np.isfinite(hit_vel).all() |
| and math.isfinite(float(flight_time)) |
| ): |
| gate_diagnostics.mark("predictor_nonfinite") |
| hit_pos = None |
| elif pos_m[2] < 0.1: |
| gate_diagnostics.mark("ball_too_low") |
| hit_pos = None |
|
|
| if hit_pos is not None: |
| hit_pos_relative = hit_pos - ROBOT_BASE_ORIGIN_WORLD |
| in_policy_domain = ( |
| HIT_Y_RANGE[0] <= hit_pos_relative[1] <= HIT_Y_RANGE[1] |
| and HIT_Z_RANGE[0] <= hit_pos_relative[2] <= HIT_Z_RANGE[1] |
| ) |
| if not in_policy_domain: |
| gate_diagnostics.mark("policy_domain") |
| if not domain_reject_reported: |
| print( |
| "[PREDICT REJECT] 击球点超出策略域 | " |
| f"y={hit_pos_relative[1]:+.3f} " |
| f"z={hit_pos_relative[2]:+.3f}" |
| ) |
| domain_reject_reported = True |
| hit_pos = None |
|
|
| if hit_pos is not None and prediction_stabilizer is not None: |
| |
| |
| |
| |
| |
| stable = prediction_stabilizer.update( |
| current_time, |
| current_time + flight_time, |
| hit_pos, |
| hit_vel, |
| ) |
| if stable is None: |
| gate_diagnostics.mark("stabilizer_warmup") |
| gate_diagnostics.report_if_due() |
| continue |
| gate_diagnostics.mark("stable_prediction") |
| ( |
| tentative_absolute_hit_time, |
| tentative_hit_pos, |
| tentative_hit_vel, |
| ) = stable |
| tentative_stamp = current_time |
| tentative_ball_pos = pos_m.copy() |
|
|
| |
| if ball_incoming and not has_crossed_target and prev_pos_m is not None: |
| if prev_pos_m[0] >= TARGET_HIT_X and pos_m[0] < TARGET_HIT_X: |
| has_crossed_target = True |
| ball_incoming = False |
|
|
| |
| ratio = (TARGET_HIT_X - prev_pos_m[0]) / (pos_m[0] - prev_pos_m[0]) |
| real_hit_y = prev_pos_m[1] + ratio * (pos_m[1] - prev_pos_m[1]) |
| real_hit_z = prev_pos_m[2] + ratio * (pos_m[2] - prev_pos_m[2]) |
|
|
| |
| if prev_mocap_time is not None: |
| real_hit_time = prev_mocap_time + ratio * (current_time - prev_mocap_time) |
| else: |
| real_hit_time = current_time |
|
|
| print(f"[Ball #{rally_count}] 事后校验 | " |
| f"真实穿越 Y={real_hit_y:.4f} Z={real_hit_z:.4f} " |
| f"T={real_hit_time:.4f}s", end="") |
| if pending_lock and locked_hit_pos is not None and locked_absolute_hit_time is not None: |
| error_y = real_hit_y - locked_hit_pos[1] |
| error_z = real_hit_z - locked_hit_pos[2] |
| time_error_ms = (locked_absolute_hit_time - real_hit_time) * 1000 |
| print(f" | 位置误差 {math.hypot(error_y, error_z) * 1000:.1f}mm" |
| f" (dY={error_y * 1000:+.1f} dZ={error_z * 1000:+.1f})" |
| f" | 时间误差 {time_error_ms:+.1f}ms") |
| else: |
| gate_diagnostics.mark("crossed_without_lock") |
| print(" | [PREDICT MISS] 穿越击球面前未锁定命令") |
|
|
| if marker_phase.terminal and ball_incoming: |
| ball_incoming = False |
|
|
| prev_pos_m = pos_m.copy() |
| prev_mocap_time = current_time |
| last_mocap_time = current_time |
| wall_at_last_mocap = wall_now |
|
|
| if ball_incoming and tentative_absolute_hit_time is not None \ |
| and last_mocap_time is not None and wall_at_last_mocap is not None: |
|
|
| if latest_ball_msg is not None: |
| estimated_mocap_now = last_mocap_time |
| else: |
| estimated_mocap_now = last_mocap_time + (wall_now - wall_at_last_mocap) |
|
|
| t_to_hit = tentative_absolute_hit_time - estimated_mocap_now |
| first_lock = (not pending_lock) and (0.0 < t_to_hit <= LOCK_TIME_THRESHOLD) |
| refresh_lock = pending_lock and (tentative_stamp != last_locked_tentative_stamp) |
| if latest_ball_msg is not None and not pending_lock: |
| if t_to_hit > LOCK_TIME_THRESHOLD: |
| gate_diagnostics.mark("wait_lock_far") |
| elif t_to_hit <= 0.0: |
| gate_diagnostics.mark("lock_too_late") |
|
|
| if first_lock or refresh_lock: |
| locked_hit_pos = tentative_hit_pos.copy() |
| locked_absolute_hit_time = tentative_absolute_hit_time |
| locked_incoming_vel = tentative_hit_vel.copy() |
|
|
| locked_v_racket_cmd = calculate_racket_command( |
| p_racket=locked_hit_pos, |
| v_incoming=tentative_hit_vel, |
| p_landing=p_landing_target, |
| dt_flight=desired_flight_time, |
| Cr=0.70, |
| predictor=predictor, |
| ) |
|
|
| locked_p_racket_relative = locked_hit_pos - ROBOT_BASE_ORIGIN_WORLD |
| locked_v_racket_cmd_relative = locked_v_racket_cmd.copy() |
| |
|
|
| has_ever_predicted = True |
| last_locked_tentative_stamp = tentative_stamp |
| wall_time_at_lock = wall_now |
| mocap_time_at_lock = estimated_mocap_now |
|
|
| if first_lock: |
| gate_diagnostics.mark("first_lock") |
| pending_lock = True |
| publish_count = 0 |
| gap_note = "" if latest_ball_msg is not None else "（丢帧期间由时间推算触发）" |
| bp = tentative_ball_pos |
| print(f"[追踪中] 首次锁定{gap_note} | t_to_hit={t_to_hit:.3f}s | " |
| f"击球点 Y={locked_hit_pos[1]:.3f} Z={locked_hit_pos[2]:.3f} | " |
| f"球位 ({bp[0]:+.3f}, {bp[1]:+.3f}, {bp[2]:+.3f})") |
| else: |
| gate_diagnostics.mark("lock_refresh") |
|
|
| gate_diagnostics.report_if_due() |
| |
| |
| |
| |
| if not cli_args.sim_ball_only and not _source_frames_ready(last_robot_source_mono, last_ball_source_mono): |
| continue |
| if publish_period > 0.0 and mono_now < next_publish_mono: |
| continue |
| next_publish_mono = mono_now + publish_period |
|
|
| |
| |
| |
| |
| |
| |
| publish_t_strike = DEFAULT_T_STRIKE |
| p_cmd_publish = DEFAULT_P_RACKET_RELATIVE |
| v_cmd_publish = DEFAULT_V_RACKET_RELATIVE |
| incoming_vel_publish = np.zeros(3, dtype=float) |
| has_prediction = ( |
| has_ever_predicted |
| and locked_hit_pos is not None |
| and locked_absolute_hit_time is not None |
| ) |
|
|
| if has_prediction: |
| if latest_ball_msg is not None: |
| dynamic_t_strike = locked_absolute_hit_time - current_time |
| else: |
| elapsed_wall = wall_now - wall_time_at_lock |
| estimated_mocap_time = mocap_time_at_lock + elapsed_wall |
| dynamic_t_strike = locked_absolute_hit_time - estimated_mocap_time |
|
|
| |
| |
| publish_t_strike = float(np.clip(dynamic_t_strike, DEFAULT_T_STRIKE, 0.8)) |
|
|
| p_cmd_publish = np.array([ |
| float(cli_args.hit_plane_x), |
| float(locked_p_racket_relative[1]), |
| float(locked_p_racket_relative[2]), |
| ]) |
| |
| |
| |
| v_cmd_raw = np.array(locked_v_racket_cmd_relative, dtype=float) |
| incoming_vel_publish = np.array(locked_incoming_vel, dtype=float) |
| if abs(p_cmd_publish[1]) > 0.20 and p_cmd_publish[2] < 0.91: |
| v_cmd_raw[0] += 0.4 |
| if p_cmd_publish[2] < 0.91: |
| v_cmd_publish = np.array([ |
| np.clip(v_cmd_raw[0], 1.31, 2.99), |
| np.clip(v_cmd_raw[1], -0.09, 0.09), |
| np.clip(v_cmd_raw[2], 0.01, 1.09) |
| ]) |
| else: |
| v_cmd_publish = np.array([ |
| np.clip(v_cmd_raw[0], 1.31, 2.99), |
| np.clip(v_cmd_raw[1], -0.09, 0.09), |
| np.clip(v_cmd_raw[2], 0.01, 0.39) |
| ]) |
|
|
| published_ball_count = rally_count + int(has_prediction and publish_count == 0) |
| landing_xy_relative = p_landing_target[:2] - ROBOT_BASE_ORIGIN_WORLD[:2] |
| cur_msg = build_h2_planner_packet( |
| robot_pos_world=current_robot_base_pos, |
| robot_quat_xyzw=current_robot_base_quat, |
| time_to_hit_s=publish_t_strike, |
| hit_pos_robot=p_cmd_publish, |
| racket_velocity_robot=v_cmd_publish, |
| command_id=published_ball_count, |
| incoming_velocity_robot=incoming_vel_publish, |
| landing_xy_robot=landing_xy_relative, |
| robot_source_monotonic_s=last_robot_source_mono, |
| ball_source_monotonic_s=last_ball_source_mono, |
| robot_base_origin_world=ROBOT_BASE_ORIGIN_WORLD, |
| allow_missing_sources=cli_args.sim_ball_only, |
| ).tolist() |
|
|
| if len(cur_msg) != H2_PLANNER_PACKET_LEN: |
| raise RuntimeError(f"internal H2 planner packet length changed: {len(cur_msg)}") |
|
|
| ros_msg.data = cur_msg |
| cmd_pub.publish(ros_msg) |
|
|
| if debug_log is not None: |
| debug_log.write( |
| f"{wall_now:.4f} ball={published_ball_count} " |
| f"t_strike={publish_t_strike:.4f} " |
| f"base=[{cur_msg[0]:.3f},{cur_msg[1]:.3f},{cur_msg[2]:.3f}] " |
| f"quat=[{cur_msg[3]:.3f},{cur_msg[4]:.3f},{cur_msg[5]:.3f},{cur_msg[6]:.3f}] " |
| f"p=[{p_cmd_publish[0]:.3f},{p_cmd_publish[1]:.3f},{p_cmd_publish[2]:.3f}] " |
| f"v_racket=[{v_cmd_publish[0]:.3f},{v_cmd_publish[1]:.3f},{v_cmd_publish[2]:.3f}] " |
| f"v_in=[{incoming_vel_publish[0]:.3f},{incoming_vel_publish[1]:.3f},{incoming_vel_publish[2]:.3f}]\n" |
| ) |
|
|
| if has_prediction and publish_count >= 0: |
| publish_count += 1 |
| if publish_count == 1: |
| rally_count += 1 |
| print(f"\n[Ball #{rally_count}] 首次发布成功 | " |
| f"击球点 Y={locked_p_racket_relative[1]:.3f} " |
| f"Z={locked_p_racket_relative[2]:.3f}") |
|
|
| except KeyboardInterrupt: |
| print("\n\n[系统退出] 检测到中止信号 (Ctrl+C)，正在安全关闭。") |
| finally: |
| gate_diagnostics.report_total() |
| if debug_log is not None: |
| debug_log.close() |
| socket.close(0) |
| context.term() |
| ros_node.destroy_node() |
| rclpy.try_shutdown() |
|
|
|
|
| if __name__ == "__main__": |
| main() |
|
|
