Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 28 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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=$<BOOL:${GRSIM_ODE_MATH_AVAILABLE}>)
endif()

if(BUILD_TESTING)
enable_testing()
# Exercise the actual application executable, including a relocated bundle.
Expand Down Expand Up @@ -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
$<TARGET_FILE:grSim_replay>)
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
$<TARGET_FILE:grSim_ode_performance_experiments>)
Expand Down
1 change: 1 addition & 0 deletions Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -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 && \
Expand Down
34 changes: 34 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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.
7 changes: 4 additions & 3 deletions include/sslworld.h
Original file line number Diff line number Diff line change
Expand Up @@ -77,16 +77,17 @@ class SSLWorld : public QObject
QList<SendingPacket*> 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,
Robot *robot) ;
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;
Expand Down
39 changes: 39 additions & 0 deletions tests/check_measurement_comparison.py
Original file line number Diff line number Diff line change
@@ -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")
165 changes: 165 additions & 0 deletions tests/check_physics_replay.py
Original file line number Diff line number Diff line change
@@ -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("<f", 4)
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as simulator:
for field in [7, 8]: # teleport_safely, roll
for enabled in [False, True]:
teleport_ball = b"\x0d" + struct.pack("<f", 2) + bytes([field << 3, int(enabled)])
control = message(1, teleport_ball) + message(2, teleport_robot)
simulator.sendto(message(1, control), ("127.0.0.1", simulator_port))
# Minimal valid RobotControl/RobotCommand/MoveLocalVelocity wire packet.
# Required float fields use protobuf fixed32 little-endian wire type 5.
velocity = b"\x0d" + struct.pack("<f", .2) + b"\x15" + struct.pack("<f", 0) + b"\x1d" + struct.pack("<f", 0)
move = b"\x12" + bytes([len(velocity)]) + velocity
robot = b"\x08\x00\x12" + bytes([len(move)]) + move
packet = b"\x0a" + bytes([len(robot)]) + robot
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sender:
sender.settimeout(2)
for _ in range(3):
sender.sendto(packet, ("127.0.0.1", port))
response, _ = sender.recvfrom(4096)
assert response, "capture omitted command feedback"
time.sleep(.04)
output, errors = process.communicate(timeout=15)
assert process.returncode == 0, errors
recorded = json.loads(output)
replayed = run(["--replay", str(path)])
assert difference(recorded, replayed) <= 1e-12
document = json.loads(path.read_text())
assert errors.count("Rejected capture command:") == 4, errors
assert not any(event["kind"] == "simulator_control" for event in document["events"])
assert any(event["kind"] == "robot_control" for event in document["events"])
assert any(event["kind"] == "expire" and event["team"] == 0 for event in document["events"])
print("PASS rejected teleport batches leave state unchanged; live UDP feedback/expiry replay exactly", flush=True)
finally:
if process.poll() is None:
process.kill()
process.wait()
15 changes: 15 additions & 0 deletions tests/fixtures/er_force_2016_ball.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
{
"reference": "ER-Force Extended Team Description Paper, RoboCup 2016, section 5.2",
"source_url": "https://ssl.robocup.org/wp-content/uploads/2019/01/2016_ETDP_ER-Force.pdf",
"measurement_kind": "Author-reported fitted coefficients; raw shot samples and confidence intervals are not supplied here.",
"scope": "The authors' field and golf ball. This is not calibration of the user's robot or field.",
"sliding_acceleration_m_s2": -2.5,
"rolling_acceleration_m_s2": -0.3,
"switch_speed_ratio": 0.6,
"comparison_tolerances": {
"sliding_acceleration_m_s2": 0.3,
"rolling_acceleration_m_s2": 0.03,
"switch_speed_ratio": 0.05
},
"tolerance_note": "Diagnostic acceptance tolerances chosen for this comparison, not experimental uncertainty reported by the authors."
}
Loading
Loading