diff --git a/CMakeLists.txt b/CMakeLists.txt index 8467bf38..1d85edb2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -295,6 +295,27 @@ install(TARGETS ${app} BUNDLE DESTINATION . RUNTIME DESTINATION bin) target_link_libraries(${app} grSim_core) option(BUILD_TESTING "Build simulator regression tests." OFF) +# Developer tool: capture fixed-step command timelines and replay without rendering. +add_executable(grSim_replay tools/physics_replay.cpp) +target_link_libraries(grSim_replay grSim_core) +find_package(Git QUIET) +set(GRSIM_REPLAY_REVISION "unknown") +if(GIT_FOUND) + execute_process(COMMAND "${GIT_EXECUTABLE}" describe --always --dirty "--exclude=*" + WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" + OUTPUT_VARIABLE GRSIM_REPLAY_GIT_REVISION OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_QUIET) + if(GRSIM_REPLAY_GIT_REVISION) + set(GRSIM_REPLAY_REVISION "${GRSIM_REPLAY_GIT_REVISION}") + endif() +endif() +target_compile_definitions(grSim_replay PRIVATE GRSIM_REPLAY_REVISION="${GRSIM_REPLAY_REVISION}") +if(TARGET ode_external) + target_include_directories(grSim_replay PRIVATE + "${PROJECT_SOURCE_DIR}/cmake/ode" ${GRSIM_ODE_MATH_INCLUDE_DIR}) + target_compile_definitions(grSim_replay PRIVATE + GRSIM_ODE_USE_BLAS=$) +endif() + if(BUILD_TESTING) enable_testing() # Exercise the actual application executable, including a relocated bundle. @@ -428,6 +449,13 @@ if(BUILD_TESTING) endif() find_program(GRSIM_PYTHON3_EXECUTABLE python3) if(GRSIM_PYTHON3_EXECUTABLE) + add_test(NAME measurement_comparison_regression + COMMAND ${GRSIM_PYTHON3_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tests/check_measurement_comparison.py) + set_tests_properties(measurement_comparison_regression PROPERTIES TIMEOUT 5) + add_test(NAME physics_replay_regression + COMMAND ${GRSIM_PYTHON3_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tests/check_physics_replay.py + $) + set_tests_properties(physics_replay_regression PROPERTIES TIMEOUT 120) add_test(NAME collision_filtering_trajectories COMMAND ${GRSIM_PYTHON3_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tests/check_collision_filtering.py $) diff --git a/Dockerfile b/Dockerfile index 25e6c5d8..29b93064 100644 --- a/Dockerfile +++ b/Dockerfile @@ -30,6 +30,7 @@ COPY config /grsim/config COPY include /grsim/include COPY resources /grsim/resources COPY src /grsim/src +COPY tools /grsim/tools COPY CMakeLists.txt README.md LICENSE.md /grsim/ RUN mkdir build && \ cd build && \ diff --git a/README.md b/README.md index cd05c8b9..f2a1ccf8 100644 --- a/README.md +++ b/README.md @@ -144,3 +144,37 @@ Kazunori Kimura and Jeremy Feltracco and Kenta Sato and Atousa Ahsani}, howpublished = {\url{https://github.com/RoboCup-SSL/grSim/}} } ``` + + +The `grSim_replay` developer executable records a fresh physics world's complete +settings, initial body states, random seed and accepted protocol commands by +simulation tick. Replay runs without rendering or wall-clock command scheduling: + +```sh +bin/grSim_replay --record /tmp/pushing.json --fixture pushing --ticks 180 +bin/grSim_replay --replay /tmp/pushing.json --helpers 0 > /tmp/serial.json +bin/grSim_replay --replay /tmp/pushing.json --helpers 3 > /tmp/workers.json +``` + +Both fixture modes use six robots per side. For a bounded loopback command +capture, use `--record FILE --duration-ms 5000 --blue-port 10301 --yellow-port +10302 --sim-port 10300`. The tool announces its ports, returns command feedback, +and records command-watchdog expiry as explicit events. `--settings FILE` uses +an existing grSim configuration when recording; replay reads its saved settings. +The capture is a fresh headless physics session, with no vision publication, +GUI edits, world configuration changes or arbitrary mid-match checkpoints. +Replays retain the direct double-precision solver and five substeps. Compare +numerical trajectories within a chosen tolerance across backends; identical +bits on different CPUs are not promised. + +For measured ball trajectories, `tools/compare_ball_measurements.py` accepts +the JSON output of `grSim_physics_experiments` and `--measured-csv FILE`. +CSV columns are `t_s,x_m,y_m`, optionally `z_m`, with strictly increasing +timestamps from one camera/track. It reports position errors without automatic +fitting or extrapolation; `--time-offset-s` explicitly aligns the two clocks. +The alternative `--reference-json tests/fixtures/er_force_2016_ball.json` +compares sliding/rolling deceleration and transition speed against the published +ER-Force 2016 fitted coefficients. These describe that team's field and ball, +not a calibration of another setup. `--require-agreement` makes a reference +mismatch return failure. Raw measurements and generated recordings should stay +outside Git. diff --git a/include/sslworld.h b/include/sslworld.h index 110df5e5..f9618a12 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -77,8 +77,6 @@ class SSLWorld : public QObject QList sendQueue; bool lastInfraredState[TEAM_COUNT][MAX_ROBOT_COUNT]{}; KickStatus lastKickState[TEAM_COUNT][MAX_ROBOT_COUNT]{}; - void processSimControl(const SimulatorCommand &simulatorCommand, SimulatorResponse &simulatorResponse); - void processRobotControl(const RobotControl &robotControl, RobotControlResponse &robotControlResponse, Team team); void processRobotSpec(const RobotSpecs &robotSpec) const; static void processRobotLimits(const RobotSpecs &robotSpec, RobotSettings *settings); static void processMoveCommand(RobotControlResponse &robotControlResponse, const RobotMoveCommand &robotCommand, @@ -86,7 +84,10 @@ class SSLWorld : public QObject void releaseBallHolds() const; void processTeleportBall(SimulatorResponse &simulatorResponse, const TeleportBall &teleBall) const; static void processTeleportRobot(const TeleportRobot &teleBot, Robot *robot); -public: +public: + // Apply decoded protocol commands between fixed steps, including offline replay. + void processRobotControl(const RobotControl &robotControl, RobotControlResponse &robotControlResponse, Team team); + void processSimControl(const SimulatorCommand &simulatorCommand, SimulatorResponse &simulatorResponse); dReal customDT; SSLWorld(QObject* parent, SimulationSettings* _cfg, RobotsFormation *form1, RobotsFormation *form2); ~SSLWorld() override; diff --git a/tests/check_measurement_comparison.py b/tests/check_measurement_comparison.py new file mode 100644 index 00000000..1455a114 --- /dev/null +++ b/tests/check_measurement_comparison.py @@ -0,0 +1,39 @@ +"""Independent arithmetic/negative controls for measurement comparison.""" +import csv +import importlib.util +from pathlib import Path +import tempfile +import sys +sys.dont_write_bytecode = True + +path = Path(__file__).resolve().parent.parent / "tools/compare_ball_measurements.py" +spec = importlib.util.spec_from_file_location("comparison", path) +module = importlib.util.module_from_spec(spec) +spec.loader.exec_module(module) +# Deliberately synthetic, analytically known traces test the comparator only. +# These are not passed off as measured or used to calibrate physics defaults. +document = {"success":True, "trajectory_columns":["t_s","ball_x_m","ball_y_m","ball_z_m", + "ball_vx_m_s","ball_vy_m_s","ball_wx_rad_s","ball_wy_rad_s"], + "trajectory":[[i, 2*i, -i, .02, 2, -1, 0, 0] for i in range(5)]} +with tempfile.TemporaryDirectory(prefix="grsim-measurements-") as directory: + path = Path(directory) / "samples.csv" + def write(rows): + with path.open("w", newline="") as handle: + writer = csv.writer(handle) + writer.writerow(["t_s","x_m","y_m"]) + writer.writerows(rows) + samples = [[t, 2*t + .03, -t - .04] for t in [.25, 1.25, 2.25, 3.25]] + write(samples) + result = module.compare_csv(document, path) + assert abs(result["xy_rmse_m"] - .05) < 1e-12 + assert abs(result["xy_max_error_m"] - .05) < 1e-12 + for rows in [samples[:2], list(reversed(samples)), samples[:3]+[samples[2]], + samples[:3]+[[10,0,0]], samples[:3]+[[float("nan"),0,0]]]: + write(rows) + try: + module.compare_csv(document, path) + except ValueError: + pass + else: + raise AssertionError("invalid/misaligned measurements were accepted") +print("PASS independent 3-4-5 error/interpolation, missing/duplicate/unordered/nonfinite/out-of-range measurement controls") diff --git a/tests/check_physics_replay.py b/tests/check_physics_replay.py new file mode 100644 index 00000000..a46a4c12 --- /dev/null +++ b/tests/check_physics_replay.py @@ -0,0 +1,165 @@ +"""Exercise actual recording/replay paths, including a live UDP capture.""" +import copy +import json +import math +import os +from pathlib import Path +import re +import select +import socket +import struct +import subprocess +import sys +import tempfile +import time + +binary = str(Path(sys.argv[1]).resolve()) +env = dict(os.environ, QT_QPA_PLATFORM="offscreen") + + +def run(args, accepted=True): + result = subprocess.run([binary, *args], env=env, capture_output=True, text=True, timeout=30) + if accepted: + assert result.returncode == 0, result.stderr + assert "ODE Message" not in result.stderr and "ODE Error" not in result.stderr, result.stderr + value = json.loads(result.stdout) + assert value["success"] and value["substeps"] == 5 + assert abs(value["step_seconds"] - 1 / 60) < 1e-12 + assert value["max_shell_penetration_m"] <= 0.002, value["max_shell_penetration_m"] + return value + assert result.returncode != 0, "invalid recording was accepted" + + +def difference(left, right): + assert len(left["checkpoints"]) == len(right["checkpoints"]) + maximum = 0 + for a, b in zip(left["checkpoints"], right["checkpoints"]): + assert a["tick"] == b["tick"] and len(a["robots"]) == len(b["robots"]) + for ra, rb in zip(a["robots"], b["robots"]): + assert ra["on"] == rb["on"] and ra["roller"] == rb["roller"] + maximum = max(maximum, max(abs(x-y) for x,y in zip(ra["wheel_commands"], rb["wheel_commands"]))) + assert len(a["bodies"]) == len(b["bodies"]) + for ba, bb in zip(a["bodies"], b["bodies"]): + for key in ba: + for x, y in zip(ba[key], bb[key]): + assert math.isfinite(x) and math.isfinite(y) + maximum = max(maximum, abs(x - y)) + return maximum + + +with tempfile.TemporaryDirectory(prefix="grsim-replay-") as directory: + directory = Path(directory) + for settings in [directory / "missing.xml", directory]: + output = directory / "invalid-settings-output.json" + run(["--record", str(output), "--fixture", "spaced", "--ticks", "1", + "--settings", str(settings)], accepted=False) + assert not output.exists(), "invalid settings produced a recording" + print("PASS missing/non-file explicit settings are rejected before recording", flush=True) + for fixture in ["spaced", "pushing"]: + path = directory / (fixture + ".json") + recorded = run(["--record", str(path), "--fixture", fixture, "--ticks", "180"]) + serial = run(["--replay", str(path), "--helpers", "0"]) + repeated = run(["--replay", str(path), "--helpers", "0"]) + workers = run(["--replay", str(path), "--helpers", "3"]) + assert difference(recorded, serial) <= 1e-12 + assert difference(serial, repeated) <= 1e-12 + delta = difference(serial, workers) + assert delta <= 1e-6, delta + print(f"PASS {fixture}: recorded/repeated serial <=1e-12, worker state delta {delta:.3g}", flush=True) + document = json.loads(path.read_text()) + assert document["settings"]["Robots_Count"] == 6 + assert document["initial"]["tick"] == 0 and document["seed"] == 166 + if fixture == "spaced": + delayed = copy.deepcopy(document) + for event in delayed["events"]: + if event["kind"] == "robot_control" and event["tick"] == 0: + event["tick"] = 3 + changed = directory / "delayed.json" + changed.write_text(json.dumps(delayed)) + altered = run(["--replay", str(changed)]) + # Compare physical states separately: wheel command histories can + # legitimately differ when a command's accepted tick changes. + delta = max(abs(x-y) for a,b in zip(serial["checkpoints"], altered["checkpoints"]) + for ba,bb in zip(a["bodies"],b["bodies"]) + for key in ["position", "velocity"] + for x,y in zip(ba[key],bb[key])) + assert delta > 1e-4, "changing accepted command ticks had no detectable effect" + print("PASS negative control detects shifted command ticks", flush=True) + variants = [] + wrong_format = copy.deepcopy(document); wrong_format["format"] = "unknown" + variants.append(wrong_format) + missing = copy.deepcopy(document); del missing["settings"]["BallMass"] + variants.append(missing) + unknown = copy.deepcopy(document); unknown["events"][0]["kind"] = "unknown" + variants.append(unknown) + payload = copy.deepcopy(document); payload["events"][0]["data"] = "not base64!" + variants.append(payload) + unordered = copy.deepcopy(document); unordered["events"][0]["tick"] = 180 + variants.append(unordered) + invalid = copy.deepcopy(document); invalid["settings"]["DeltaTime"] = -1 + variants.append(invalid) + wrong_initial = copy.deepcopy(document) + wrong_initial["initial"]["bodies"][0]["position"][0] += .001 + variants.append(wrong_initial) + for index, value in enumerate(variants): + changed.write_text(json.dumps(value)) + run(["--replay", str(changed)], accepted=False) + print("PASS malformed format/settings/payload/timeline controls are rejected", flush=True) + path = directory / "live.json" + process = subprocess.Popen([binary, "--record", str(path), "--duration-ms", "1700"], + env=env, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + try: + ready = "" + deadline = time.monotonic() + 10 + while time.monotonic() < deadline: + if select.select([process.stderr], [], [], .1)[0]: + line = process.stderr.readline() + if line.startswith("READY "): + ready = line + break + if process.poll() is not None: + break + assert ready, "capture did not announce its loopback ports" + port = int(re.search(r"blue=(\d+)", ready).group(1)) + simulator_port = int(re.search(r"simulator=(\d+)", ready).group(1)) + # Reject the whole batch before applying its ball or robot teleports, + # including explicitly present false-valued unsupported flags. + def message(field, payload): + assert len(payload) < 128 + return bytes([(field << 3) | 2, len(payload)]) + payload + identity = b"\x08\x00\x10\x02" # robot 0, BLUE + teleport_robot = message(1, identity) + b"\x15" + struct.pack("= b[0] for a, b in zip(rows, rows[1:])): + raise ValueError("simulation timestamps must be strictly increasing") + return rows + + +def compare_csv(document, filename, time_offset=0): + rows = trajectory(document) + with Path(filename).open(newline="") as handle: + reader = csv.DictReader(handle) + if not {"t_s", "x_m", "y_m"}.issubset(reader.fieldnames or []): + raise ValueError("CSV needs t_s,x_m,y_m in seconds and metres") + has_z = "z_m" in reader.fieldnames + measured = [[float(row[key]) for key in ["t_s", "x_m", "y_m"] + (["z_m"] if has_z else [])] + for row in reader] + if len(measured) < 4 or any(not math.isfinite(x) for row in measured for x in row): + raise ValueError("need at least four finite measurement rows") + if any(a[0] >= b[0] for a, b in zip(measured, measured[1:])): + raise ValueError("measurement timestamps must be strictly increasing; select one camera/track first") + times = [row[0] for row in rows] + errors, errors_z = [], [] + for sample in measured: + t = sample[0] + time_offset + if t < times[0] or t > times[-1]: + raise ValueError("measurement extends outside simulation; extrapolation is disabled") + index = min(max(1, bisect.bisect_left(times, t)), len(rows) - 1) + a, b = rows[index - 1], rows[index] + fraction = (t - a[0]) / (b[0] - a[0]) + predicted = [a[i] + fraction * (b[i] - a[i]) for i in range(1, 4)] + errors.append(math.hypot(predicted[0] - sample[1], predicted[1] - sample[2])) + if has_z: + errors_z.append(abs(predicted[2] - sample[3])) + return {"comparison":"measured trajectory samples", "measurement_rows":len(measured), + "time_offset_s":time_offset, "automatic_fitting":False, + "xy_rmse_m":math.sqrt(statistics.mean(x*x for x in errors)), + "xy_max_error_m":max(errors), + "z_rmse_m":math.sqrt(statistics.mean(x*x for x in errors_z)) if errors_z else None} + + +def compare_reference(document, reference): + rows = trajectory(document) + radius = float(document["parameters"]["ball_radius_m"]) + if radius <= 0 or not math.isfinite(radius): + raise ValueError("invalid ball radius") + sliding, rolling = [], [] + initial_speed = math.hypot(rows[0][4], rows[0][5]) + transition = None + was_sliding = False + for a, b in zip(rows, rows[1:]): + if abs(a[3] - radius) > .001 or abs(b[3] - radius) > .001: + continue + slip_a = math.hypot(a[4] - radius*a[7], a[5] + radius*a[6]) + slip_b = math.hypot(b[4] - radius*b[7], b[5] + radius*b[6]) + acceleration = (math.hypot(b[4], b[5]) - math.hypot(a[4], a[5])) / (b[0] - a[0]) + if slip_a > .03 and slip_b > .03: + sliding.append(acceleration) + was_sliding = True + if slip_a < .01 and slip_b < .01 and b[0] > .2 and math.hypot(b[4], b[5]) > .1: + rolling.append(acceleration) + if was_sliding and transition is None and slip_b < .01 and initial_speed > 0: + transition = math.hypot(b[4], b[5]) / initial_speed + observed = {"sliding_acceleration_m_s2":statistics.median(sliding) if sliding else None, + "rolling_acceleration_m_s2":statistics.median(rolling) if rolling else None, + "switch_speed_ratio":transition} + metrics = {} + for name, value in observed.items(): + expected = float(reference[name]) + tolerance = float(reference["comparison_tolerances"][name]) + metrics[name] = {"observed":value, "reference":expected, "tolerance":tolerance, + "within_tolerance":value is not None and abs(value-expected) <= tolerance, + "absolute_error":abs(value-expected) if value is not None else None} + return {"comparison":"published fitted coefficients, not raw measurement samples", + "reference":reference["reference"], "source_url":reference["source_url"], + "scope":reference["scope"], "tolerance_note":reference["tolerance_note"], + "agreement":all(value["within_tolerance"] for value in metrics.values()), + "metrics":metrics} + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("simulation_json", type=Path) + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument("--measured-csv", type=Path) + group.add_argument("--reference-json", type=Path) + parser.add_argument("--time-offset-s", type=float, default=0) + parser.add_argument("--require-agreement", action="store_true", + help="fail on published-reference mismatch; CSV comparisons report errors without inventing an acceptance threshold") + args = parser.parse_args() + try: + if not math.isfinite(args.time_offset_s): + raise ValueError("time offset must be finite") + if args.require_agreement and not args.reference_json: + raise ValueError("--require-agreement requires a coefficient reference") + document = json.loads(args.simulation_json.read_text()) + result = (compare_reference(document, json.loads(args.reference_json.read_text())) + if args.reference_json else compare_csv(document, args.measured_csv, args.time_offset_s)) + print(json.dumps(result, indent=2, allow_nan=False)) + return 1 if args.require_agreement and not result["agreement"] else 0 + except (ValueError, KeyError, TypeError, OSError) as error: + parser.error(str(error)) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/physics_replay.cpp b/tools/physics_replay.cpp new file mode 100644 index 00000000..62235a10 --- /dev/null +++ b/tools/physics_replay.cpp @@ -0,0 +1,618 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "configwidget.h" +#include "sslworld.h" +#ifdef GRSIM_ODE_USE_BLAS +#include "grsim_ode_math.h" +#endif +#ifndef GRSIM_REPLAY_REVISION +#define GRSIM_REPLAY_REVISION "unknown" +#endif +#ifndef GRSIM_ODE_MATH_BACKEND +#define GRSIM_ODE_MATH_BACKEND "ODE" +#endif + +#define ROBOT_FIELDS(X) \ + X(RobotCenterFromKicker) \ + X(RobotRadius) \ + X(RobotHeight) \ + X(BottomHeight) \ + X(KickerZ) \ + X(KickerThickness) \ + X(KickerWidth) \ + X(KickerHeight) \ + X(WheelRadius) \ + X(WheelThickness) \ + X(Wheel1Angle) \ + X(Wheel2Angle) \ + X(Wheel3Angle) \ + X(Wheel4Angle) \ + X(BodyMass) \ + X(WheelMass) \ + X(KickerMass) \ + X(KickerDampFactor) \ + X(RollerTorqueFactor) \ + X(RollerPerpendicularTorqueFactor) \ + X(Kicker_Friction) \ + X(WheelTangentFriction) \ + X(WheelPerpendicularFriction) \ + X(Wheel_Motor_FMax) \ + X(MaxLinearKickSpeed) \ + X(MaxChipKickSpeed) \ + X(AccSpeedupAbsoluteMax) \ + X(AccSpeedupAngularMax) \ + X(AccBrakeAbsoluteMax) \ + X(AccBrakeAngularMax) \ + X(VelAbsoluteMax) \ + X(VelAngularMax) +#define SIM_INT_FIELDS(X) \ + X(Robots_Count) \ + X(sendGeometryEvery) \ + X(VisionMulticastPort) \ + X(CommandListenPort) \ + X(BlueStatusSendPort) \ + X(YellowStatusSendPort) \ + X(SimControlListenPort) \ + X(BlueControlListenPort) \ + X(YellowControlListenPort) \ + X(sendDelay) +#define SIM_DOUBLE_FIELDS(X) \ + X(Camera_Height) \ + X(Camera_Focal_Length) \ + X(Camera_Scaling_Limit) \ + X(BallRadius) \ + X(BallMass) \ + X(BallFriction) \ + X(BallRollingDeceleration) \ + X(BallSlip) \ + X(BallBounce) \ + X(BallBounceVel) \ + X(BallLinearDamp) \ + X(BallAngularDamp) \ + X(BallModelTwoPhaseAccSlide) \ + X(BallModelTwoPhaseKSwitch) \ + X(BallModelChipFixedLossDampingXyFirstHop) \ + X(BallModelChipFixedLossDampingXyOtherHops) \ + X(BallModelChipFixedLossDampingZ) \ + X(DesiredFPS) \ + X(DeltaTime) \ + X(Gravity) \ + X(noiseDeviation_x) \ + X(noiseDeviation_y) \ + X(noiseDeviation_angle) \ + X(ball_vanishing) \ + X(blue_team_vanishing) \ + X(yellow_team_vanishing) \ + X(Field_Line_Width) \ + X(Field_Length) \ + X(Field_Width) \ + X(Field_Rad) \ + X(Field_Free_Kick) \ + X(Field_Penalty_Width) \ + X(Field_Penalty_Depth) \ + X(Field_Penalty_Point) \ + X(Field_Margin_Touch_Line) \ + X(Field_Margin_Goal_Line) \ + X(Field_Goal_Substitution_Area_Width) \ + X(Field_Referee_Margin) \ + X(Wall_Thickness) \ + X(Goal_Thickness) \ + X(Goal_Depth) \ + X(Goal_Width) \ + X(Goal_Height) +#define SIM_BOOL_FIELDS(X) \ + X(BallProjectAirborne) \ + X(LowSpecMode) \ + X(ResetTurnOver) \ + X(noise) \ + X(vanishing) +#define SIM_STRING_FIELDS(X) \ + X(VisionMulticastAddr) \ + X(ColorRobotBlue) \ + X(ColorRobotYellow) \ + X(Division) \ + X(YellowTeam) \ + X(BlueTeam) + +namespace { +double number(const QJsonObject& object, const char* name) +{ + const auto value = object.value(name); + if (!value.isDouble() || !std::isfinite(value.toDouble())) + throw std::runtime_error(std::string("Missing or nonfinite number: ") + name); + return value.toDouble(); +} +unsigned integer(const QJsonObject& object, const char* name, unsigned maximum) +{ + const double value = number(object, name); + if (value < 0 || value > maximum || value != std::floor(value)) + throw std::runtime_error(std::string("Invalid integer: ") + name); + return unsigned(value); +} +QJsonObject saveRobot(const RobotSettings& settings) +{ + QJsonObject result; +#define SAVE(name) result[#name] = settings.name; + ROBOT_FIELDS(SAVE) +#undef SAVE + return result; +} +RobotSettings loadRobot(const QJsonObject& object) +{ + RobotSettings result{}; +#define LOAD(name) result.name = number(object, #name); + ROBOT_FIELDS(LOAD) +#undef LOAD + if (result.BodyMass <= 0 || result.WheelMass <= 0 || result.KickerMass <= 0 || + result.RobotRadius <= 0 || result.RobotHeight <= 0 || + result.WheelRadius <= 0 || result.WheelThickness <= 0) + throw std::runtime_error("Invalid robot dimensions or mass"); + if (saveRobot(result).size() != object.size()) throw std::runtime_error("Unknown robot setting"); + return result; +} +QJsonObject saveSettings(const SimulationSettings& settings) +{ + QJsonObject result; +#define SAVE(name) result[#name] = settings.value_##name; + SIM_INT_FIELDS(SAVE) SIM_DOUBLE_FIELDS(SAVE) SIM_BOOL_FIELDS(SAVE) +#undef SAVE +#define SAVE(name) result[#name] = QString::fromStdString(settings.value_##name); + SIM_STRING_FIELDS(SAVE) +#undef SAVE + result["robotSettings"] = saveRobot(settings.robotSettings); + result["blueSettings"] = saveRobot(settings.blueSettings); + result["yellowSettings"] = saveRobot(settings.yellowSettings); + return result; +} +SimulationSettings loadSettings(const QJsonObject& object) +{ + SimulationSettings result; +#define LOAD(name) { const double value = number(object, #name); \ + if (value < std::numeric_limits::min() || value > std::numeric_limits::max() || value != std::floor(value)) \ + throw std::runtime_error("Invalid integer setting: " #name); result.value_##name = int(value); } + SIM_INT_FIELDS(LOAD) +#undef LOAD +#define LOAD(name) result.value_##name = number(object, #name); + SIM_DOUBLE_FIELDS(LOAD) +#undef LOAD +#define LOAD(name) if (!object[#name].isBool()) throw std::runtime_error("Invalid boolean: " #name); result.value_##name = object[#name].toBool(); + SIM_BOOL_FIELDS(LOAD) +#undef LOAD +#define LOAD(name) if (!object[#name].isString()) throw std::runtime_error("Invalid string: " #name); result.value_##name = object[#name].toString().toStdString(); + SIM_STRING_FIELDS(LOAD) +#undef LOAD + result.robotSettings = loadRobot(object["robotSettings"].toObject()); + result.blueSettings = loadRobot(object["blueSettings"].toObject()); + result.yellowSettings = loadRobot(object["yellowSettings"].toObject()); + if (result.Robots_Count() < 0 || result.Robots_Count() > MAX_ROBOT_COUNT || + result.DeltaTime() <= 0 || result.DeltaTime() > 0.1 || + result.BallRadius() <= 0 || result.BallMass() <= 0 || result.Gravity() < 0 || + result.BallRollingDeceleration() < 0) + throw std::runtime_error("Invalid physical settings"); + if (saveSettings(result).size() != object.size()) throw std::runtime_error("Unknown setting"); + return result; +} +QJsonArray values(const dReal* value, unsigned count) +{ + QJsonArray result; + for (unsigned i = 0; i < count; ++i) { + if (!std::isfinite(value[i])) throw std::runtime_error("Nonfinite body state"); + result.append(value[i]); + } + return result; +} +QJsonObject bodyState(dBodyID body) +{ + return {{"position", values(dBodyGetPosition(body), 3)}, + {"quaternion", values(dBodyGetQuaternion(body), 4)}, + {"velocity", values(dBodyGetLinearVel(body), 3)}, + {"angular_velocity", values(dBodyGetAngularVel(body), 3)}, + {"force", values(dBodyGetForce(body), 3)}, + {"torque", values(dBodyGetTorque(body), 3)}}; +} +QJsonObject state(const SSLWorld& world, unsigned tick) +{ + // Explicit construction order avoids pointer/hash-space traversal order. + QJsonArray bodies, robots; + bodies.append(bodyState(world.ball->body)); + for (int i = 0; i < world.cfg->Robots_Count() * 2; ++i) { + auto* robot = world.robots[i]; + bodies.append(bodyState(robot->chassis->body)); + QJsonArray speeds; + for (auto* wheel : robot->wheels) { + bodies.append(bodyState(wheel->cyl->body)); + speeds.append(wheel->speed); + } + bodies.append(bodyState(robot->kicker->box->body)); + robots.append(QJsonObject{{"on", robot->on}, {"wheel_commands", speeds}, + {"roller", robot->kicker->getRoller()}}); + } + return {{"tick", int(tick)}, {"bodies", bodies}, {"robots", robots}, + {"simulation_time", world.snapshot(0)->simulationTime}}; +} +bool finiteMessage(const google::protobuf::Message& message) +{ + const auto* reflection = message.GetReflection(); + std::vector fields; + reflection->ListFields(message, &fields); + for (const auto* field : fields) { + const int count = field->is_repeated() ? reflection->FieldSize(message, field) : 1; + for (int i = 0; i < count; ++i) { + using F = google::protobuf::FieldDescriptor; + if (field->cpp_type() == F::CPPTYPE_FLOAT) { + const auto value = field->is_repeated() ? reflection->GetRepeatedFloat(message, field, i) : reflection->GetFloat(message, field); + if (!std::isfinite(value)) return false; + } else if (field->cpp_type() == F::CPPTYPE_DOUBLE) { + const auto value = field->is_repeated() ? reflection->GetRepeatedDouble(message, field, i) : reflection->GetDouble(message, field); + if (!std::isfinite(value)) return false; + } else if (field->cpp_type() == F::CPPTYPE_MESSAGE) { + const auto& nested = field->is_repeated() ? reflection->GetRepeatedMessage(message, field, i) : reflection->GetMessage(message, field); + if (!finiteMessage(nested)) return false; + } + } + } + return true; +} +template Message decode(const QJsonObject& event) +{ + if (!event["data"].isString()) throw std::runtime_error("Missing command payload"); + const auto encoded = event["data"].toString().toLatin1(); + const auto data = QByteArray::fromBase64(encoded); + Message message; + if (data.toBase64() != encoded || !message.ParseFromArray(data.data(), data.size()) || + !message.IsInitialized() || !finiteMessage(message)) + throw std::runtime_error("Invalid protobuf command"); + return message; +} +QByteArray apply(SSLWorld& world, const QJsonObject& event) +{ + const auto kind = event["kind"].toString(); + if (kind == "robot_control") { + const unsigned team = integer(event, "team", 1); + RobotControlResponse response; + const auto command = decode(event); + for (const auto& robot : command.robot_commands()) + if (robot.has_move_command() && !robot.move_command().has_local_velocity() && + !robot.move_command().has_global_velocity() && !robot.move_command().has_wheel_velocity()) + throw std::runtime_error("Unsupported robot movement"); + world.processRobotControl(command, response, team ? YELLOW : BLUE); + if (response.errors_size()) throw std::runtime_error("Unsupported robot command"); + const auto data = response.SerializeAsString(); + return QByteArray(data.data(), int(data.size())); + } else if (kind == "simulator_control") { + const auto command = decode(event); + if (command.has_config()) throw std::runtime_error("Replay does not support world configuration changes"); + if (command.has_control() && command.control().has_teleport_ball()) { + const auto& ball = command.control().teleport_ball(); + // The live handler can apply these teleports before reporting an + // unsupported field. Reject the entire capture/replay batch first, + // including explicitly present false flags and sibling robot moves. + if (ball.has_teleport_safely() || ball.has_roll()) + throw std::runtime_error("Replay does not support safe or rolling ball teleports"); + } + SimulatorResponse response; + world.processSimControl(command, response); + if (response.errors_size()) throw std::runtime_error("Unsupported simulator command"); + const auto data = response.SerializeAsString(); + return QByteArray(data.data(), int(data.size())); + } else if (kind == "expire") { + const unsigned team = integer(event, "team", 1); + for (int id = 0; id < world.cfg->Robots_Count(); ++id) + world.robots[world.robotIndex(id, team)]->resetSpeeds(); + } else throw std::runtime_error("Unknown replay event"); + return {}; +} +QJsonObject commandEvent(unsigned tick, const char* kind, + const google::protobuf::Message& command, int team = -1) +{ + if (!command.IsInitialized() || !finiteMessage(command)) throw std::runtime_error("Invalid command"); + const auto data = command.SerializeAsString(); + QJsonObject result{{"tick", int(tick)}, {"kind", QString::fromLatin1(kind)}, + {"data", QString::fromLatin1(QByteArray(data.data(), int(data.size())).toBase64())}}; + if (team >= 0) result["team"] = team; + return result; +} +bool sameInitialState(const QJsonValue& left, const QJsonValue& right) +{ + if (left.type() != right.type()) return false; + if (left.isDouble()) + return std::isfinite(right.toDouble()) && std::abs(left.toDouble() - right.toDouble()) <= 1e-12; + if (left.isArray()) { + const auto a = left.toArray(), b = right.toArray(); + if (a.size() != b.size()) return false; + for (int i = 0; i < a.size(); ++i) if (!sameInitialState(a[i], b[i])) return false; + return true; + } + if (left.isObject()) { + const auto a = left.toObject(), b = right.toObject(); + if (a.keys() != b.keys()) return false; + for (auto i = a.begin(); i != a.end(); ++i) + if (!sameInitialState(i.value(), b[i.key()])) return false; + return true; + } + return left == right; +} +const char* triangularBackend() +{ +#ifdef GRSIM_ODE_USE_BLAS + return grsim_ode::triangularBackend(); +#else + return "ODE"; +#endif +} +QJsonObject readJson(const QString& filename) +{ + QFile file(filename); + if (!file.open(QIODevice::ReadOnly)) throw std::runtime_error("Could not open replay"); + QJsonParseError error; + const auto document = QJsonDocument::fromJson(file.readAll(), &error); + if (error.error != QJsonParseError::NoError || !document.isObject()) throw std::runtime_error("Invalid replay JSON"); + return document.object(); +} +void writeJson(const QString& filename, const QJsonObject& object) +{ + QSaveFile file(filename); + const auto data = QJsonDocument(object).toJson(QJsonDocument::Indented); + if (!file.open(QIODevice::WriteOnly) || file.write(data) != data.size() || !file.commit()) + throw std::runtime_error("Could not write replay"); +} +} + +int main(int argc, char** argv) +{ + if (qEnvironmentVariableIsEmpty("QT_QPA_PLATFORM")) qputenv("QT_QPA_PLATFORM", "offscreen"); + QApplication app(argc, argv); + QString recordFile, replayFile, settingsFile, fixture; + unsigned helpers = 0, seed = 166, ticks = 180, durationMs = 0; + unsigned ports[3] = {0, 0, 0}; + try { + for (int i = 1; i < argc; ++i) { + const QString option = QString::fromLocal8Bit(argv[i]); + if (++i == argc) throw std::runtime_error("Expected an option value"); + const QString value = QString::fromLocal8Bit(argv[i]); + if (option == "--record") recordFile = value; + else if (option == "--replay") replayFile = value; + else if (option == "--settings") settingsFile = value; + else if (option == "--fixture") fixture = value; + else { + bool valid = false; + const auto number = value.toUInt(&valid); + if (!valid) throw std::runtime_error("Invalid numeric option"); + if (option == "--helpers" && number <= 16) helpers = number; + else if (option == "--seed") seed = number; + else if (option == "--ticks" && number > 0 && number <= 1000000) ticks = number; + else if (option == "--duration-ms" && number >= 50 && number <= 120000) durationMs = number; + else if (option == "--blue-port" && number <= 65535) ports[0] = number; + else if (option == "--yellow-port" && number <= 65535) ports[1] = number; + else if (option == "--sim-port" && number <= 65535) ports[2] = number; + else throw std::runtime_error("Unknown or out-of-range option"); + } + } + if (recordFile.isEmpty() == replayFile.isEmpty() || + (!fixture.isEmpty() && fixture != "spaced" && fixture != "pushing") || + (!recordFile.isEmpty() && fixture.isEmpty() && !durationMs) || + (!fixture.isEmpty() && durationMs) || + (!replayFile.isEmpty() && (!fixture.isEmpty() || durationMs || !settingsFile.isEmpty()))) + throw std::runtime_error("Use --record FILE with --fixture spaced|pushing or --duration-ms N, or --replay FILE"); + QJsonObject recording; + SimulationSettings settings; + if (!replayFile.isEmpty()) { + recording = readJson(replayFile); + if (recording["format"] != "grsim-physics-replay-v1" || + recording["ode_version"] != QString::fromLatin1(dODE_VERSION) || + integer(recording, "substeps", 5) != 5) + throw std::runtime_error("Incompatible replay format or ODE version"); + seed = integer(recording, "seed", std::numeric_limits::max()); + ticks = integer(recording, "ticks", 1000000); + if (!ticks || !recording["events"].isArray() || !recording["initial"].isObject()) + throw std::runtime_error("Missing replay timeline"); + settings = loadSettings(recording["settings"].toObject()); + } else { + if (!settingsFile.isEmpty()) { + QFile input(settingsFile); + if (!QFileInfo(settingsFile).isFile() || !input.open(QIODevice::ReadOnly)) + throw std::runtime_error("Requested settings must be an existing readable regular file"); + } + ConfigWidget config(false, settingsFile); + if (settingsFile.isEmpty()) { + config.v_BlueTeam->setString("Parsian"); config.v_YellowTeam->setString("Parsian"); + config.loadRobotsSettings(); + config.v_Robots_Count->setInt(6); + config.v_DeltaTime->setDouble(1.0 / 60); + } + settings = loadSettings(saveSettings(config.simulationSettings())); + } + if (!dInitODE2(dInitFlagManualThreadCleanup) || !dAllocateODEDataForThread(dAllocateMaskAll)) + throw std::runtime_error("Could not initialize ODE"); + if (!dCheckConfiguration("ODE_double_precision")) { + dCleanupODEAllDataForThread(); dCloseODE(); + throw std::runtime_error("Replay requires double-precision ODE"); + } + QJsonObject result; + try { + dRandSetSeed(seed); + std::srand(seed); + RobotsFormation formation(FORMATION_INSIDE_1, &settings); + SSLWorld world(nullptr, &settings, &formation, &formation); + if (!world.p->setThreadCount(helpers)) throw std::runtime_error("Could not start ODE workers"); + const auto initial = state(world, 0); + if (!replayFile.isEmpty() && !sameInitialState(initial, recording["initial"])) + throw std::runtime_error("Initial world differs from the recorded configuration"); + if (!recordFile.isEmpty()) { + recording = {{"format", "grsim-physics-replay-v1"}, + {"build_revision", QString::fromLatin1(GRSIM_REPLAY_REVISION)}, + {"cpu_architecture", QSysInfo::currentCpuArchitecture()}, {"recorded_helpers", int(helpers)}, + {"ode_version", QString::fromLatin1(dODE_VERSION)}, + {"ode_configuration", QString::fromLatin1(dGetConfiguration())}, + {"math_backend", QString::fromLatin1(GRSIM_ODE_MATH_BACKEND)}, + {"triangular_backend", QString::fromLatin1(triangularBackend())}, + {"seed", double(seed)}, {"substeps", 5}, {"settings", saveSettings(settings)}, {"initial", initial}}; + } + QJsonArray events = recording["events"].toArray(), checkpoints; + checkpoints.append(initial); + unsigned tick = 0; + double maximumShellPenetration = 0; + PWorld::StepStatistics statistics; + statistics.contactObserver = [&](const dContact& contact) { + if (dGeomGetClass(contact.geom.g1) == dSphereClass && + dGeomGetClass(contact.geom.g2) == dSphereClass && + contact.geom.g1 != world.ball->geom && contact.geom.g2 != world.ball->geom) + maximumShellPenetration = std::max(maximumShellPenetration, double(contact.geom.depth)); + }; + auto step = [&]() { + world.stepPhysics(settings.DeltaTime(), PhysicsStepOptions(), &statistics); + ++tick; + if (tick % 30 == 0) checkpoints.append(state(world, tick)); + }; + auto accept = [&](const QJsonObject& event) { + const auto response = apply(world, event); + events.append(event); + return response; + }; + if (!fixture.isEmpty()) { + SimulatorCommand placement; + auto* ball = placement.mutable_control()->mutable_teleport_ball(); + ball->set_x(4); ball->set_y(-3); ball->set_z(settings.BallRadius()); + ball->set_vx(0); ball->set_vy(0); ball->set_vz(0); + for (int team = 0; team < 2; ++team) + for (int id = 0; id < settings.Robots_Count(); ++id) { + auto* robot = placement.mutable_control()->add_teleport_robot(); + robot->mutable_id()->set_id(id); robot->mutable_id()->set_team(team ? YELLOW : BLUE); + const int col = fixture == "pushing" ? (team ? 2 + id % 2 : 1 - id % 2) : team * 3 + id % 3; + robot->set_x((col - 2.5) * (fixture == "pushing" ? .185 : .7)); + robot->set_y((id / (fixture == "pushing" ? 2 : 3) - 1) * (fixture == "pushing" ? .185 : .7)); + robot->set_orientation(team ? M_PI : 0); + } + accept(commandEvent(tick, "simulator_control", placement)); + while (tick < ticks) { + if (tick % 30 == 0) { + for (int team = 0; team < 2; ++team) { + RobotControl command; + for (int id = 0; id < settings.Robots_Count(); ++id) { + auto* robot = command.add_robot_commands(); robot->set_id(id); + auto* velocity = robot->mutable_move_command()->mutable_local_velocity(); + velocity->set_forward(tick < ticks * 2 / 3 ? (fixture == "pushing" ? .5 : .15) : 0); + velocity->set_left(0); velocity->set_angular(fixture == "pushing" ? 0 : .2); + } + accept(commandEvent(tick, "robot_control", command, team)); + } + } + step(); + } + } else if (durationMs) { + QUdpSocket sockets[3]; + QElapsedTimer commandClock[2]; + bool expired[2] = {false, false}; + for (int i = 0; i < 3; ++i) { + if (!sockets[i].bind(QHostAddress(QHostAddress::LocalHost), quint16(ports[i]))) + throw std::runtime_error("Could not bind replay capture port"); + QObject::connect(&sockets[i], &QUdpSocket::readyRead, [&, i]() { + while (sockets[i].hasPendingDatagrams()) { + const auto datagram = sockets[i].receiveDatagram(); + try { + if (i < 2) { + RobotControl command; + if (!command.ParseFromArray(datagram.data().data(), datagram.data().size())) continue; + const auto response = accept(commandEvent(tick, "robot_control", command, i)); + sockets[i].writeDatagram(response, datagram.senderAddress(), datagram.senderPort()); + commandClock[i].restart(); expired[i] = false; + } else { + SimulatorCommand command; + if (!command.ParseFromArray(datagram.data().data(), datagram.data().size())) continue; + const auto response = accept(commandEvent(tick, "simulator_control", command)); + sockets[i].writeDatagram(response, datagram.senderAddress(), datagram.senderPort()); + } + } catch (const std::exception& error) { + std::fprintf(stderr, "Rejected capture command: %s\n", error.what()); + } + } + }); + } + std::fprintf(stderr, "READY blue=%u yellow=%u simulator=%u\n", + sockets[0].localPort(), sockets[1].localPort(), sockets[2].localPort()); + std::fflush(stderr); + QElapsedTimer clock; + clock.start(); + for (auto& elapsed : commandClock) elapsed.start(); + QTimer timer; + timer.setTimerType(Qt::PreciseTimer); timer.setInterval(1); + QObject::connect(&timer, &QTimer::timeout, [&]() { + try { + for (int team = 0; team < 2; ++team) + if (!expired[team] && commandClock[team].elapsed() > 1000) { + accept({{"tick", int(tick)}, {"kind", "expire"}, {"team", team}}); + expired[team] = true; + } + unsigned budget = 0; + while ((tick + 1) * settings.DeltaTime() <= clock.nsecsElapsed() / 1e9 && budget++ < 4) + step(); + } catch (const std::exception& error) { + std::fprintf(stderr, "Capture failed: %s\n", error.what()); app.exit(1); + } + }); + timer.start(); + QTimer::singleShot(durationMs, &app, &QCoreApplication::quit); + if (app.exec()) throw std::runtime_error("Capture failed"); + ticks = tick; + } else { + unsigned previous = 0; + for (const auto& value : events) { + if (!value.isObject()) throw std::runtime_error("Invalid replay event"); + const unsigned at = integer(value.toObject(), "tick", ticks); + if (at > ticks || at < previous) throw std::runtime_error("Unordered or out-of-range command tick"); + previous = at; + } + unsigned index = 0; + while (tick < ticks) { + while (index < unsigned(events.size()) && + integer(events[int(index)].toObject(), "tick", ticks) == tick) + apply(world, events[int(index++)].toObject()); + step(); + } + while (index < unsigned(events.size())) + apply(world, events[int(index++)].toObject()); + } + if (tick % 30 == 0) checkpoints.removeLast(); + checkpoints.append(state(world, tick)); + recording["ticks"] = int(tick); + recording["events"] = events; + if (!recordFile.isEmpty()) writeJson(recordFile, recording); + result = {{"success", true}, {"ticks", int(tick)}, {"helpers", int(helpers)}, + {"step_seconds", settings.DeltaTime()}, {"substeps", 5}, + {"ode_version", QString::fromLatin1(dODE_VERSION)}, + {"build_revision", QString::fromLatin1(GRSIM_REPLAY_REVISION)}, + {"cpu_architecture", QSysInfo::currentCpuArchitecture()}, + {"math_backend", QString::fromLatin1(GRSIM_ODE_MATH_BACKEND)}, + {"triangular_backend", QString::fromLatin1(triangularBackend())}, + {"checkpoints", checkpoints}, {"max_shell_penetration_m", maximumShellPenetration}, + {"contacts", double(statistics.contacts)}}; + } catch (...) { + dCleanupODEAllDataForThread(); dCloseODE(); throw; + } + dCleanupODEAllDataForThread(); dCloseODE(); + std::puts(QJsonDocument(result).toJson(QJsonDocument::Compact).constData()); + return 0; + } catch (const std::exception& error) { + std::fprintf(stderr, "%s\n", error.what()); + return 1; + } +}