File size: 4,228 Bytes
d8290eb | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 | """Versioned H2 planner packet tail shared by predictor and controller."""
from __future__ import annotations
import math
import numpy as np
LEGACY_FIELDS = 20
H2_PLANNER_PROTOCOL_VERSION = 3.0
H2_PLANNER_PACKET_LEN = 27
PROTOCOL_VERSION_INDEX = 20
ROBOT_SOURCE_MONOTONIC_INDEX = 21
BALL_SOURCE_MONOTONIC_INDEX = 22
ROBOT_BASE_ORIGIN_START_INDEX = 23
FRAME_CONVENTION_INDEX = 26
FRAME_CONVENTION_WORLD_X = 1.0
def source_age_s(now_s: float, source_stamp_s: float) -> float:
"""Return a fail-closed same-host monotonic source age."""
now = float(now_s)
stamp = float(source_stamp_s)
if not math.isfinite(now) or not math.isfinite(stamp) or stamp <= 0.0 or stamp > now + 0.01:
return math.inf
return max(0.0, now - stamp)
def build_h2_planner_packet(
*,
robot_pos_world,
robot_quat_xyzw,
time_to_hit_s: float,
hit_pos_robot,
racket_velocity_robot,
command_id: int,
incoming_velocity_robot,
landing_xy_robot,
robot_source_monotonic_s: float,
ball_source_monotonic_s: float,
robot_base_origin_world,
allow_missing_sources: bool = False,
) -> np.ndarray:
"""Build the exact 27-value H2 planner-v3 wire payload.
Both the ROS predictor and the isolated MuJoCo bridge call this function,
preventing a simulation-only packet layout from drifting away from the
real deployment producer.
"""
robot_pos = np.asarray(robot_pos_world, dtype=np.float64).reshape(3)
robot_quat = np.asarray(robot_quat_xyzw, dtype=np.float64).reshape(4)
hit_pos = np.asarray(hit_pos_robot, dtype=np.float64).reshape(3)
racket_velocity = np.asarray(racket_velocity_robot, dtype=np.float64).reshape(3)
incoming_velocity = np.asarray(incoming_velocity_robot, dtype=np.float64).reshape(3)
landing_xy = np.asarray(landing_xy_robot, dtype=np.float64).reshape(2)
origin = np.asarray(robot_base_origin_world, dtype=np.float64).reshape(3)
scalar_values = np.asarray(
[time_to_hit_s, robot_source_monotonic_s, ball_source_monotonic_s],
dtype=np.float64,
)
all_values = np.concatenate(
[robot_pos, robot_quat, scalar_values, hit_pos, racket_velocity,
incoming_velocity, landing_xy, origin]
)
if not np.isfinite(all_values).all():
raise ValueError("H2 planner packet inputs must all be finite")
if isinstance(command_id, bool) or int(command_id) != command_id or int(command_id) < 0:
raise ValueError("H2 planner command_id must be a non-negative integer")
quat_norm = float(np.linalg.norm(robot_quat))
if not 0.5 <= quat_norm <= 1.5:
raise ValueError(f"robot quaternion norm is invalid ({quat_norm:.3f})")
if not allow_missing_sources and (robot_source_monotonic_s <= 0.0 or ball_source_monotonic_s <= 0.0):
raise ValueError("H2 planner source timestamps must be positive monotonic seconds")
if allow_missing_sources and (
robot_source_monotonic_s < 0.0 or ball_source_monotonic_s < 0.0
or (ball_source_monotonic_s == 0.0 and command_id != 0)
):
raise ValueError("simulation missing-source packets use zero stamps and no strike before a ball")
packet = np.empty(H2_PLANNER_PACKET_LEN, dtype=np.float64)
packet[0:3] = robot_pos
packet[3:7] = robot_quat
packet[7] = float(time_to_hit_s)
packet[8:11] = hit_pos
packet[11:14] = racket_velocity
packet[14] = float(command_id)
packet[15:18] = incoming_velocity
packet[18:20] = landing_xy
packet[PROTOCOL_VERSION_INDEX] = H2_PLANNER_PROTOCOL_VERSION
packet[ROBOT_SOURCE_MONOTONIC_INDEX] = float(robot_source_monotonic_s)
packet[BALL_SOURCE_MONOTONIC_INDEX] = float(ball_source_monotonic_s)
packet[ROBOT_BASE_ORIGIN_START_INDEX : ROBOT_BASE_ORIGIN_START_INDEX + 3] = origin
packet[FRAME_CONVENTION_INDEX] = FRAME_CONVENTION_WORLD_X
return packet
__all__ = [
"BALL_SOURCE_MONOTONIC_INDEX",
"FRAME_CONVENTION_INDEX",
"FRAME_CONVENTION_WORLD_X",
"H2_PLANNER_PACKET_LEN",
"H2_PLANNER_PROTOCOL_VERSION",
"LEGACY_FIELDS",
"PROTOCOL_VERSION_INDEX",
"ROBOT_SOURCE_MONOTONIC_INDEX",
"ROBOT_BASE_ORIGIN_START_INDEX",
"build_h2_planner_packet",
"source_age_s",
]
|