From 5c3d13395fea9ccb96c802a6dc6383875775c426 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Tue, 3 Nov 2026 17:28:22 +0000 Subject: [PATCH 1/8] Refine Utama radio 1 angular response from expanded log checks --- README.md | 4 ++++ config/Utama-Radio-1.ini | 5 +++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index aa37f23f..5c17334e 100644 --- a/README.md +++ b/README.md @@ -207,6 +207,10 @@ Geometry and masses remain assumptions from Parsian; these logs do not measure them. Response is validated for the observed drives (up to 1 m/s) and turns (1–4 rad/s), not all possible commands. +The radio 1 profile also uses 125 rad/s² angular speedup/braking limits, selected +from comparisons that held out entire recordings. This is a fitted simulator +control setting; it is not a direct measurement of hardware acceleration. + Local/global body-velocity commands use separate forward/left gains and angular gain `AngularVelocityScale + AngularVelocityScaleSlope * abs(requested_turn)`. Teleport velocity targets and raw wheel-speed commands keep their existing meaning. diff --git a/config/Utama-Radio-1.ini b/config/Utama-Radio-1.ini index 9e7413e3..52ac77bd 100644 --- a/config/Utama-Radio-1.ini +++ b/config/Utama-Radio-1.ini @@ -30,9 +30,10 @@ WheelMotorMaximumApplyingTorque= 0.2 MaxLinearKickSpeed=10 MaxChipKickSpeed=10 AccSpeedupAbsoluteMax=4 -AccSpeedupAngularMax=50 +; Fitted simulator limits from grouped turn/stop checks, not direct hardware measurements. +AccSpeedupAngularMax=125 AccBrakeAbsoluteMax=4 -AccBrakeAngularMax=50 +AccBrakeAngularMax=125 VelAbsoluteMax=5 VelAngularMax=20 From 34fe632ef58531a4f6fc41e5024ec627338d7dab Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Tue, 3 Nov 2026 17:30:06 +0000 Subject: [PATCH 2/8] Clarify the fitted scope of the radio 1 profile --- config/Utama-Radio-1.ini | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/config/Utama-Radio-1.ini b/config/Utama-Radio-1.ini index 52ac77bd..fa7839fc 100644 --- a/config/Utama-Radio-1.ini +++ b/config/Utama-Radio-1.ini @@ -1,5 +1,6 @@ ; Utama physical robot on radio 1, 27-28 October2026 field logs. -; Geometry/mass are inherited Parsian assumptions; only body-response gains were fitted. +; Geometry/mass are inherited Parsian assumptions. +; Body-response gains and angular control limits were fitted. [Geometery] CenterFromKicker= 0.073 Radius = 0.09 From 4e74fea154dff86ef72124069fa38b8c9144ab48 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Tue, 3 Nov 2026 18:11:25 +0000 Subject: [PATCH 3/8] Refine first-impact shell contacts and version replay integration --- README.md | 10 +++++--- include/physics/pworld.h | 1 + include/sslworld.h | 4 ++- src/physics/pworld.cpp | 1 + src/sslworld.cpp | 34 ++++++++++++++++++++++++-- tests/check_physics_replay.py | 39 ++++++++++++++++++++++++++++- tests/physics_experiments.cpp | 1 + tools/physics_replay.cpp | 46 ++++++++++++++++++++++++++--------- 8 files changed, 117 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 5c17334e..150fb704 100644 --- a/README.md +++ b/README.md @@ -167,14 +167,18 @@ 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 +All fixture modes use six robots per side. `pushing-turning` adds sustained +pushing with alternating turns for first-impact collision coverage. 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 +Replays retain the direct double-precision solver. New v2 recordings store the +integration policy: five steps per fixed tick in free motion, six near robot +shells for finer first-impact resolution. Historical v1 recordings retain their +fixed-five path; older binaries reject v2 rather than silently changing it. Compare numerical trajectories within a chosen tolerance across backends; identical bits on different CPUs are not promised. @@ -221,4 +225,4 @@ an external hardware-noise layer. Use one actuator-response model in the stack. For physics comparisons, `grSim_replay --replay FILE --checkpoint-every 1` saves every simulation tick. The default interval remains 30 ticks; diagnostics do not -change the fixed timestep or five ODE substeps. +change the fixed timestep or recorded integration policy. diff --git a/include/physics/pworld.h b/include/physics/pworld.h index 05453cb8..73c0a628 100644 --- a/include/physics/pworld.h +++ b/include/physics/pworld.h @@ -31,6 +31,7 @@ class PWorld public: struct StepStatistics { unsigned contacts = 0; + unsigned integrationSteps = 0; double maxPenetration = 0; unsigned callbacks = 0, narrowPhaseCalls = 0, createdContacts = 0; bool measureTimings = false; diff --git a/include/sslworld.h b/include/sslworld.h index f9618a12..a77a77f0 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -51,10 +51,11 @@ class RobotsFormation; // Alternate values support controlled offline experiments without changing // networking or personal settings. Legacy friction is only a comparison mode. struct PhysicsStepOptions { - unsigned substeps = 5; + unsigned substeps = 5; // Minimum integration steps per fixed tick. unsigned quickStepIterations = 0; bool legacyBallFriction = false; bool finiteWheelRotation = false; + bool refineShellImpacts = true; // Nearby shells use at least six steps. }; class SendingPacket { @@ -82,6 +83,7 @@ class SSLWorld : public QObject static void processMoveCommand(RobotControlResponse &robotControlResponse, const RobotMoveCommand &robotCommand, Robot *robot) ; void releaseBallHolds() const; + bool shellsMayMeet(dReal frameStep) const; void processTeleportBall(SimulatorResponse &simulatorResponse, const TeleportBall &teleBall) const; static void processTeleportRobot(const TeleportRobot &teleBot, Robot *robot); public: diff --git a/src/physics/pworld.cpp b/src/physics/pworld.cpp index a1b371d2..0c623624 100644 --- a/src/physics/pworld.cpp +++ b/src/physics/pworld.cpp @@ -338,6 +338,7 @@ PSurface* PWorld::findSurface(PObject* o1,PObject* o2) bool PWorld::step(dReal dt, unsigned quickStepIterations, StepStatistics* statistics) { activeStatistics = statistics; + if (statistics) ++statistics->integrationSteps; bool success = false; try { updateCollisionMasks(); diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 98d500fd..966ebf5d 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -407,12 +407,42 @@ void SSLWorld::stepSimulation(dReal dt) { frame_num ++; } +bool SSLWorld::shellsMayMeet(dReal frameStep) const +{ + // A frame of lookahead plus 5cm makes the refinement decision before + // first impact, rather than switching on an existing contact's depth. + const int count = cfg->Robots_Count() * 2; + for (int first = 0; first < count; ++first) { + const auto* p1 = dGeomGetPosition(robots[first]->dummy->geom); + const auto* v1 = dBodyGetLinearVel(robots[first]->chassis->body); + const dReal speed1 = std::sqrt(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]); + for (int second = first + 1; second < count; ++second) { + const auto* v2 = dBodyGetLinearVel(robots[second]->chassis->body); + const dReal speed2 = std::sqrt(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]); + const dReal reach = robots[first]->settings.RobotRadius + + robots[second]->settings.RobotRadius + 0.05 + frameStep*(speed1 + speed2); + const auto* p2 = dGeomGetPosition(robots[second]->dummy->geom); + dReal distanceSquared = 0; + for (int axis = 0; axis < 3; ++axis) { + const dReal separation = p1[axis] - p2[axis]; + distanceSquared += separation * separation; + } + if (distanceSquared <= reach * reach) return true; + } + } + return false; +} + void SSLWorld::stepPhysics(dReal dt, const PhysicsStepOptions& options, PWorld::StepStatistics* statistics) { if (!std::isfinite(dt) || dt <= 0 || !options.substeps) throw std::invalid_argument("Invalid physics timestep"); legacyBallFrictionActive = options.legacyBallFriction; - for (unsigned kk=0; kk < options.substeps; ++kk) { + // Keep free motion on the nominal schedule. Nearby shells use six even + // steps for the entire frame, avoiding a velocity-dependent per-contact mesh. + const unsigned substeps = options.refineShellImpacts && shellsMayMeet(dt) + ? std::max(options.substeps, 6u) : options.substeps; + for (unsigned kk=0; kk < substeps; ++kk) { if (options.finiteWheelRotation) { for (int k=0; k < cfg->Robots_Count() * 2; ++k) { for (auto* wheel : robots[k]->wheels) { @@ -442,7 +472,7 @@ void SSLWorld::stepPhysics(dReal dt, const PhysicsStepOptions& options, dBodySetLinearVel(ball->body, 0, 0, 0); } } - if (!p->step(dt/options.substeps, options.quickStepIterations, statistics)) + if (!p->step(dt/substeps, options.quickStepIterations, statistics)) throw std::runtime_error("ODE physics step failed"); } diff --git a/tests/check_physics_replay.py b/tests/check_physics_replay.py index 7782d1fb..497a633a 100644 --- a/tests/check_physics_replay.py +++ b/tests/check_physics_replay.py @@ -129,6 +129,22 @@ def difference(left, right): assert tuple(field_settings[name][key] for key in ["ForwardVelocityScale", "LeftVelocityScale", "AngularVelocityScale", "AngularVelocityScaleSlope"]) == expected print("PASS physical robot profiles and measured ball settings load and record explicitly", flush=True) + # Fast initial shell contact must meet the same overlap bound while turning. + turning_path = directory / "pushing-turning.json" + turning = run(["--record", str(turning_path), "--fixture", "pushing-turning", + "--ticks", "900", "--settings", str(profile_path)]) + turning_workers = run(["--replay", str(turning_path), "--helpers", "3"]) + # Keep the existing 3s cross-worker bound. Long forced-contact paths amplify + # roundoff even with the historical fixed-five integrator; full serial runs + # must instead repeat exactly, while both modes retain the 2mm contact bound. + turning_repeated = run(["--replay", str(turning_path), "--helpers", "0"]) + assert difference(turning, turning_repeated) <= 1e-12 + short_serial = dict(turning, checkpoints=[p for p in turning["checkpoints"] if p["tick"] <= 180]) + short_workers = dict(turning_workers, checkpoints=[p for p in turning_workers["checkpoints"] if p["tick"] <= 180]) + assert difference(short_serial, short_workers) <= 1e-6 + assert turning["integration_policy"] == "near-shell-6-v1" + assert turning["integration_steps"] > turning["ticks"] * turning["substeps"] + print("PASS 6v6 pushing/turning stays within 2mm with serial and threaded ODE", flush=True) for fixture in ["spaced", "pushing"]: path = directory / (fixture + ".json") recorded = run(["--record", str(path), "--fixture", fixture, "--ticks", "180"]) @@ -179,8 +195,29 @@ def difference(left, right): del legacy["settings"][model][key] changed.write_text(json.dumps(legacy)) assert difference(serial, run(["--replay", str(changed)])) <= 1e-12 - print("PASS legacy v1 recordings retain exact unit-response physics", flush=True) + print("PASS omitted response fields retain exact unit-response physics", flush=True) + fixed = copy.deepcopy(document) + fixed["integration_policy"] = "fixed-5-v1" + changed.write_text(json.dumps(fixed)) + fixed_result = run(["--replay", str(changed)]) + del fixed["integration_policy"] + fixed["format"] = "grsim-physics-replay-v1" + for model in ["robotSettings", "blueSettings", "yellowSettings"]: + for key in ["ForwardVelocityScale", "LeftVelocityScale", "AngularVelocityScale", "AngularVelocityScaleSlope"]: + del fixed["settings"][model][key] + changed.write_text(json.dumps(fixed)) + historical = run(["--replay", str(changed)]) + assert historical["integration_policy"] == "fixed-5-v1" + assert historical["integration_steps"] == historical["ticks"] * 5 + assert difference(fixed_result, historical) <= 1e-12 + print("PASS recordings without integration metadata retain fixed-five physics", flush=True) variants = [] + unknown_policy = copy.deepcopy(document); unknown_policy["integration_policy"] = "unknown" + variants.append(unknown_policy) + missing_policy = copy.deepcopy(document); del missing_policy["integration_policy"] + variants.append(missing_policy) + wrong_legacy_policy = copy.deepcopy(document); wrong_legacy_policy["format"] = "grsim-physics-replay-v1" + variants.append(wrong_legacy_policy) for key, value in [("ForwardVelocityScale", 0), ("LeftVelocityScale", -1), ("AngularVelocityScale", "not numeric"), ("AngularVelocityScaleSlope", -1)]: invalid_gain = copy.deepcopy(document) diff --git a/tests/physics_experiments.cpp b/tests/physics_experiments.cpp index 835a94a5..b35edf4f 100644 --- a/tests/physics_experiments.cpp +++ b/tests/physics_experiments.cpp @@ -279,6 +279,7 @@ QJsonObject runScenario(SimulationSettings settings, const Variant& variant, con std::vector> contexts; const double complianceScale=applyVariant(world, variant, contexts, substeps, normalizeCompliance); PhysicsStepOptions options; + options.refineShellImpacts = variant.systemDefaults; options.substeps = substeps; options.quickStepIterations = variant.iterations; options.legacyBallFriction = variant.rollingScale == 0; diff --git a/tools/physics_replay.cpp b/tools/physics_replay.cpp index e2278264..0d66dbeb 100644 --- a/tools/physics_replay.cpp +++ b/tools/physics_replay.cpp @@ -423,19 +423,32 @@ int main(int argc, char** argv) } } if (recordFile.isEmpty() == replayFile.isEmpty() || - (!fixture.isEmpty() && fixture != "spaced" && fixture != "pushing") || + (!fixture.isEmpty() && fixture != "spaced" && fixture != "pushing" && fixture != "pushing-turning") || (!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"); + throw std::runtime_error("Use --record FILE with --fixture spaced|pushing|pushing-turning or --duration-ms N, or --replay FILE"); QJsonObject recording; SimulationSettings settings; + PhysicsStepOptions physicsOptions; if (!replayFile.isEmpty()) { recording = readJson(replayFile); - if (recording["format"] != "grsim-physics-replay-v1" || + const bool historical = recording["format"] == "grsim-physics-replay-v1"; + if ((!historical && recording["format"] != "grsim-physics-replay-v2") || recording["ode_version"] != QString::fromLatin1(dODE_VERSION) || integer(recording, "substeps", 5) != 5) throw std::runtime_error("Incompatible replay format or ODE version"); + // Missing policy is the historical fixed-five integrator. + const auto policy = recording.value("integration_policy"); + if (historical) { + if (!policy.isUndefined() && policy != "fixed-5-v1") + throw std::runtime_error("Unsupported historical integration policy"); + physicsOptions.refineShellImpacts = false; + } else { + if (policy != "fixed-5-v1" && policy != "near-shell-6-v1") + throw std::runtime_error("Missing or unsupported integration policy"); + physicsOptions.refineShellImpacts = policy == "near-shell-6-v1"; + } seed = integer(recording, "seed", std::numeric_limits::max()); ticks = integer(recording, "ticks", 1000000); if (!recording["events"].isArray() || !recording["initial"].isObject()) @@ -468,14 +481,15 @@ int main(int argc, char** argv) 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"}, + recording = {{"format", "grsim-physics-replay-v2"}, {"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}}; + {"seed", double(seed)}, {"substeps", 5}, {"settings", saveSettings(settings)}, {"initial", initial}, + {"integration_policy", "near-shell-6-v1"}}; } QJsonArray events = recording["events"].toArray(), checkpoints; checkpoints.append(initial); @@ -489,7 +503,7 @@ int main(int argc, char** argv) maximumShellPenetration = std::max(maximumShellPenetration, double(contact.geom.depth)); }; auto step = [&]() { - world.stepPhysics(settings.DeltaTime(), PhysicsStepOptions(), &statistics); + world.stepPhysics(settings.DeltaTime(), physicsOptions, &statistics); ++tick; if (tick % checkpointEvery == 0) checkpoints.append(state(world, tick)); }; @@ -499,6 +513,7 @@ int main(int argc, char** argv) return response; }; if (!fixture.isEmpty()) { + const bool dense = fixture != "spaced"; SimulatorCommand placement; auto* ball = placement.mutable_control()->mutable_teleport_ball(); ball->set_x(4); ball->set_y(-3); ball->set_z(settings.BallRadius()); @@ -507,9 +522,9 @@ int main(int argc, char** argv) 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)); + const int col = dense ? (team ? 2 + id % 2 : 1 - id % 2) : team * 3 + id % 3; + robot->set_x((col - 2.5) * (dense ? .185 : .7)); + robot->set_y((id / (dense ? 2 : 3) - 1) * (dense ? .185 : .7)); robot->set_orientation(team ? M_PI : 0); } accept(commandEvent(tick, "simulator_control", placement)); @@ -522,8 +537,14 @@ int main(int argc, char** argv) 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); + velocity->set_left(0); + if (fixture == "pushing-turning") { + velocity->set_forward(.5); + velocity->set_angular(id % 2 ? -2 : 2); + } else { + velocity->set_forward(tick < ticks * 2 / 3 ? (dense ? .5 : .15) : 0); + velocity->set_angular(dense ? 0 : .2); + } } accept(commandEvent(tick, "robot_control", command, team)); } @@ -616,7 +637,8 @@ int main(int argc, char** argv) {"math_backend", QString::fromLatin1(GRSIM_ODE_MATH_BACKEND)}, {"triangular_backend", QString::fromLatin1(triangularBackend())}, {"checkpoints", checkpoints}, {"max_shell_penetration_m", maximumShellPenetration}, - {"contacts", double(statistics.contacts)}}; + {"contacts", double(statistics.contacts)}, {"integration_steps", double(statistics.integrationSteps)}, + {"integration_policy", physicsOptions.refineShellImpacts ? "near-shell-6-v1" : "fixed-5-v1"}}; } catch (...) { dCleanupODEAllDataForThread(); dCloseODE(); throw; } From 764a77d9151df8f0dcdfd56afb752b9bf672e279 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Tue, 3 Nov 2026 18:13:28 +0000 Subject: [PATCH 4/8] Reuse the legacy settings fixture in compatibility checks --- tests/check_physics_replay.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/tests/check_physics_replay.py b/tests/check_physics_replay.py index 497a633a..5272cb9d 100644 --- a/tests/check_physics_replay.py +++ b/tests/check_physics_replay.py @@ -196,15 +196,12 @@ def difference(left, right): changed.write_text(json.dumps(legacy)) assert difference(serial, run(["--replay", str(changed)])) <= 1e-12 print("PASS omitted response fields retain exact unit-response physics", flush=True) - fixed = copy.deepcopy(document) + fixed = copy.deepcopy(legacy) fixed["integration_policy"] = "fixed-5-v1" changed.write_text(json.dumps(fixed)) fixed_result = run(["--replay", str(changed)]) del fixed["integration_policy"] fixed["format"] = "grsim-physics-replay-v1" - for model in ["robotSettings", "blueSettings", "yellowSettings"]: - for key in ["ForwardVelocityScale", "LeftVelocityScale", "AngularVelocityScale", "AngularVelocityScaleSlope"]: - del fixed["settings"][model][key] changed.write_text(json.dumps(fixed)) historical = run(["--replay", str(changed)]) assert historical["integration_policy"] == "fixed-5-v1" From 85b576240fdb99538c313378c1fefa8fc7cf48e7 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 4 Nov 2026 00:53:33 +0000 Subject: [PATCH 5/8] Support explicit fixed-step replay experiments --- tests/check_physics_replay.py | 19 +++++++++++++++++-- tools/physics_replay.cpp | 23 +++++++++++++++-------- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/tests/check_physics_replay.py b/tests/check_physics_replay.py index 5272cb9d..d8ca6de4 100644 --- a/tests/check_physics_replay.py +++ b/tests/check_physics_replay.py @@ -18,13 +18,13 @@ env = dict(os.environ, QT_QPA_PLATFORM="offscreen") -def run(args, accepted=True, step_seconds=1 / 60): +def run(args, accepted=True, step_seconds=1 / 60, substeps=5): 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 value["success"] and value["substeps"] == substeps assert abs(value["step_seconds"] - step_seconds) < 1e-12 assert value["max_shell_penetration_m"] <= 0.002, value["max_shell_penetration_m"] return value @@ -129,6 +129,21 @@ def difference(left, right): assert tuple(field_settings[name][key] for key in ["ForwardVelocityScale", "LeftVelocityScale", "AngularVelocityScale", "AngularVelocityScaleSlope"]) == expected print("PASS physical robot profiles and measured ball settings load and record explicitly", flush=True) + fixed_document = json.loads(profile_recording.read_text()) + fixed_document["integration_policy"] = "fixed-v1" + fixed_results = {} + fixed_path = directory / "fixed-step-count.json" + for count in [1, 2, 3, 5]: + fixed_document["substeps"] = count + fixed_path.write_text(json.dumps(fixed_document)) + fixed_results[count] = run(["--replay", str(fixed_path)], substeps=count) + assert fixed_results[count]["integration_steps"] == fixed_results[count]["ticks"] * count + assert difference(fixed_results[1], fixed_results[5]) > 1e-8 + for count in [0, 33]: + fixed_document["substeps"] = count + fixed_path.write_text(json.dumps(fixed_document)) + run(["--replay", str(fixed_path)], accepted=False) + print("PASS explicit fixed step counts alter integration without changing the tick clock", flush=True) # Fast initial shell contact must meet the same overlap bound while turning. turning_path = directory / "pushing-turning.json" turning = run(["--record", str(turning_path), "--fixture", "pushing-turning", diff --git a/tools/physics_replay.cpp b/tools/physics_replay.cpp index 0d66dbeb..54abc0f9 100644 --- a/tools/physics_replay.cpp +++ b/tools/physics_replay.cpp @@ -431,24 +431,31 @@ int main(int argc, char** argv) QJsonObject recording; SimulationSettings settings; PhysicsStepOptions physicsOptions; + QString integrationPolicy = "near-shell-6-v1"; if (!replayFile.isEmpty()) { recording = readJson(replayFile); const bool historical = recording["format"] == "grsim-physics-replay-v1"; if ((!historical && recording["format"] != "grsim-physics-replay-v2") || - recording["ode_version"] != QString::fromLatin1(dODE_VERSION) || - integer(recording, "substeps", 5) != 5) + recording["ode_version"] != QString::fromLatin1(dODE_VERSION)) throw std::runtime_error("Incompatible replay format or ODE version"); // Missing policy is the historical fixed-five integrator. + const unsigned recordedSteps = integer(recording, "substeps", 32); + if (recordedSteps == 0) throw std::runtime_error("Invalid integration step count"); const auto policy = recording.value("integration_policy"); if (historical) { - if (!policy.isUndefined() && policy != "fixed-5-v1") + if (recordedSteps != 5 || (!policy.isUndefined() && policy != "fixed-5-v1")) throw std::runtime_error("Unsupported historical integration policy"); + integrationPolicy = "fixed-5-v1"; physicsOptions.refineShellImpacts = false; } else { - if (policy != "fixed-5-v1" && policy != "near-shell-6-v1") + if (policy != "fixed-5-v1" && policy != "near-shell-6-v1" && policy != "fixed-v1") throw std::runtime_error("Missing or unsupported integration policy"); + if (policy != "fixed-v1" && recordedSteps != 5) + throw std::runtime_error("Legacy integration policy requires five steps"); + integrationPolicy = policy.toString(); physicsOptions.refineShellImpacts = policy == "near-shell-6-v1"; } + physicsOptions.substeps = recordedSteps; seed = integer(recording, "seed", std::numeric_limits::max()); ticks = integer(recording, "ticks", 1000000); if (!recording["events"].isArray() || !recording["initial"].isObject()) @@ -488,8 +495,8 @@ int main(int argc, char** argv) {"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}, - {"integration_policy", "near-shell-6-v1"}}; + {"seed", double(seed)}, {"substeps", int(physicsOptions.substeps)}, {"settings", saveSettings(settings)}, {"initial", initial}, + {"integration_policy", integrationPolicy}}; } QJsonArray events = recording["events"].toArray(), checkpoints; checkpoints.append(initial); @@ -630,7 +637,7 @@ int main(int argc, char** argv) recording["events"] = events; if (!recordFile.isEmpty()) writeJson(recordFile, recording); result = {{"success", true}, {"ticks", int(tick)}, {"helpers", int(helpers)}, - {"step_seconds", settings.DeltaTime()}, {"substeps", 5}, + {"step_seconds", settings.DeltaTime()}, {"substeps", int(physicsOptions.substeps)}, {"ode_version", QString::fromLatin1(dODE_VERSION)}, {"build_revision", QString::fromLatin1(GRSIM_REPLAY_REVISION)}, {"cpu_architecture", QSysInfo::currentCpuArchitecture()}, @@ -638,7 +645,7 @@ int main(int argc, char** argv) {"triangular_backend", QString::fromLatin1(triangularBackend())}, {"checkpoints", checkpoints}, {"max_shell_penetration_m", maximumShellPenetration}, {"contacts", double(statistics.contacts)}, {"integration_steps", double(statistics.integrationSteps)}, - {"integration_policy", physicsOptions.refineShellImpacts ? "near-shell-6-v1" : "fixed-5-v1"}}; + {"integration_policy", integrationPolicy}}; } catch (...) { dCleanupODEAllDataForThread(); dCloseODE(); throw; } From c043809f21680f3a82df71e8c681bd0526786fb8 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 4 Nov 2026 01:28:38 +0000 Subject: [PATCH 6/8] Adapt integration resolution to motion and predicted impacts --- README.md | 12 +-- include/sslworld.h | 11 ++- src/sslworld.cpp | 138 +++++++++++++++++++++++++++++++++- tests/check_physics_replay.py | 107 ++++++++++++++++++++------ tests/physics_experiments.cpp | 26 +++++-- tools/physics_replay.cpp | 35 ++++++--- 6 files changed, 280 insertions(+), 49 deletions(-) diff --git a/README.md b/README.md index 150fb704..aea53ffa 100644 --- a/README.md +++ b/README.md @@ -123,7 +123,7 @@ Citing If you use this in your research, please cite the original paper: ``` -@inproceedings{Monajjemi2011grSimR, +`inproceedings{Monajjemi2011grSimR, title={grSim - RoboCup Small Size Robot Soccer Simulator}, author={Valiallah Monajjemi and A. Koochakzadeh and S. S. Ghidary}, booktitle={RoboCup}, @@ -134,7 +134,7 @@ If you use this in your research, please cite the original paper: If you wish to cite this repo with it's modifications specifically, please cite: ``` -@misc{grsim2021, +`misc{grsim2021, author = {Mohammad Mahdi Rahimi and Jan Segre and Valiallah Monajjemi and A. Koochakzadeh and Sepehr MohaimenianPour and Nicolai Ommer and Avatar Kazunori Kimura and Jeremy Feltracco and Kenta Sato and Atousa Ahsani}, title = {GRSIM}, @@ -176,9 +176,11 @@ 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. New v2 recordings store the -integration policy: five steps per fixed tick in free motion, six near robot -shells for finer first-impact resolution. Historical v1 recordings retain their -fixed-five path; older binaries reject v2 rather than silently changing it. Compare +integration policy. The default adaptive policy uses two steps for settled, +distant motion, at least five during acceleration, turning, sliding or flight, +and finer steps for predicted robot, ball and wall impacts. All bodies share +the same evenly divided fixed tick; short settling guards avoid rapidly +switching the resolution. Historical v1 recordings retain their fixed-five path; older binaries reject v2 rather than silently changing it. Compare numerical trajectories within a chosen tolerance across backends; identical bits on different CPUs are not promised. diff --git a/include/sslworld.h b/include/sslworld.h index a77a77f0..16e84760 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -50,12 +50,14 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) class RobotsFormation; // Alternate values support controlled offline experiments without changing // networking or personal settings. Legacy friction is only a comparison mode. +enum class IntegrationPolicy { Fixed, NearShellSix, Adaptive }; + struct PhysicsStepOptions { - unsigned substeps = 5; // Minimum integration steps per fixed tick. + unsigned substeps = 2; // Minimum; activity and predicted impacts select finer steps. unsigned quickStepIterations = 0; bool legacyBallFriction = false; bool finiteWheelRotation = false; - bool refineShellImpacts = true; // Nearby shells use at least six steps. + IntegrationPolicy integrationPolicy = IntegrationPolicy::Adaptive; }; class SendingPacket { @@ -84,6 +86,11 @@ class SSLWorld : public QObject Robot *robot) ; void releaseBallHolds() const; bool shellsMayMeet(dReal frameStep) const; + unsigned adaptiveSubsteps(dReal frameStep, unsigned minimum); + dReal previousWheelSpeeds[MAX_ROBOT_COUNT*2][4]{}; + dReal previousHorizontalVelocity[MAX_ROBOT_COUNT*2][2]{}; + unsigned quietFrames = 0, resolutionDecayFrames = 0, adaptiveResolution = 5; + dReal adaptiveWarmupUntil = 1; void processTeleportBall(SimulatorResponse &simulatorResponse, const TeleportBall &teleBall) const; static void processTeleportRobot(const TeleportRobot &teleBot, Robot *robot); public: diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 966ebf5d..b40bf7ff 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -433,15 +433,136 @@ bool SSLWorld::shellsMayMeet(dReal frameStep) const return false; } +unsigned SSLWorld::adaptiveSubsteps(dReal frameStep, unsigned minimum) +{ + constexpr unsigned normalSteps = 5; + constexpr dReal travelBudget = 0.001; + constexpr dReal contactMargin = 0.005; + unsigned required = minimum; + bool changingMotion = sim_time < adaptiveWarmupUntil; + const int count = cfg->Robots_Count() * 2; + for (int robot = 0; robot < count; ++robot) { + for (int wheel = 0; wheel < 4; ++wheel) { + const dReal target = robots[robot]->getSpeed(wheel); + changingMotion |= std::abs(target-previousWheelSpeeds[robot][wheel]) > 0.001; + previousWheelSpeeds[robot][wheel] = target; + } + const auto* v = dBodyGetLinearVel(robots[robot]->chassis->body); + const auto* w = dBodyGetAngularVel(robots[robot]->chassis->body); + for (int axis = 0; axis < 2; ++axis) { + changingMotion |= std::abs(v[axis]-previousHorizontalVelocity[robot][axis]) > frameStep*0.1; + previousHorizontalVelocity[robot][axis] = v[axis]; + } + changingMotion |= std::abs(v[2]) > 0.02 || + std::max({std::abs(w[0]), std::abs(w[1]), std::abs(w[2])}) > 0.05; + } + quietFrames = changingMotion ? 0 : std::min(quietFrames+1, 6u); + if (quietFrames < 6) required = std::max(required, normalSteps); + // One shared, even subdivision for the entire fixed tick. Predict impacts + // using relative motion and a bounded acceleration allowance. + auto impact = [&](dReal gap, dReal closing, dReal acceleration) { + closing = std::max(dReal(0), closing); + const dReal travel = frameStep*(closing + acceleration*frameStep); + if (gap > contactMargin + travel) return; + required = std::max(required, normalSteps); + if (gap > travel + travelBudget) return; + const dReal needed = std::ceil(travel/travelBudget); + const unsigned bounded = unsigned(std::min(dReal(64), std::max(dReal(1), needed))); + for (unsigned level : {5u, 8u, 12u, 20u, 32u, 64u}) + if (level >= bounded) { required = std::max(required, level); break; } + }; + for (int first = 0; first < count; ++first) { + const auto* p1 = dGeomGetPosition(robots[first]->dummy->geom); + const auto* v1 = dBodyGetLinearVel(robots[first]->chassis->body); + const auto* w1 = dBodyGetAngularVel(robots[first]->chassis->body); + for (int second = first+1; second < count; ++second) { + const auto* p2 = dGeomGetPosition(robots[second]->dummy->geom); + const auto* v2 = dBodyGetLinearVel(robots[second]->chassis->body); + const auto* w2 = dBodyGetAngularVel(robots[second]->chassis->body); + dReal squared = 0, relative = 0; + for (int axis = 0; axis < 3; ++axis) { + const dReal d = p1[axis]-p2[axis]; + squared += d*d; relative += d*(v1[axis]-v2[axis]); + } + const dReal distance = std::sqrt(squared); + if (distance <= 0) { required = std::max(required, 20u); continue; } + const dReal a1 = robots[first]->settings.AccSpeedupAbsoluteMax + + std::hypot(v1[0],v1[1])*std::abs(w1[2]); + const dReal a2 = robots[second]->settings.AccSpeedupAbsoluteMax + + std::hypot(v2[0],v2[1])*std::abs(w2[2]); + impact(distance-robots[first]->settings.RobotRadius-robots[second]->settings.RobotRadius, + -relative/distance, a1+a2); + } + } + const auto* bp = dGeomGetPosition(ball->geom); + const auto* bv = dBodyGetLinearVel(ball->body); + const auto* bw = dBodyGetAngularVel(ball->body); + const dReal radius = cfg->BallRadius(); + // Airborne/sliding balls keep the baseline; settled rolling was validated + // separately. Floor impact and nearby robots/walls can demand finer steps. + if (bp[2] > radius+0.001 || std::abs(bv[2]) > 0.01 || + std::hypot(bv[0]-bw[1]*radius, bv[1]+bw[0]*radius) > 0.01) + required = std::max(required, normalSteps); + if (bv[2] < -0.01 || bp[2] > radius+0.001) + impact(bp[2]-radius, -bv[2], cfg->Gravity()); + for (int robot = 0; robot < count; ++robot) { + const auto* p = dGeomGetPosition(robots[robot]->dummy->geom); + const auto* v = dBodyGetLinearVel(robots[robot]->chassis->body); + const auto* w = dBodyGetAngularVel(robots[robot]->chassis->body); + // The hull is a cylinder, not the shell-contact sphere: use its + // horizontal radius conservatively even for low ball contacts. + dReal squared = 0, relative = 0; + for (int axis = 0; axis < 2; ++axis) { + const dReal d = bp[axis]-p[axis]; squared += d*d; + relative += d*(bv[axis]-v[axis]); + } + const dReal distance = std::sqrt(squared); + if (distance > 0) + impact(distance-radius-robots[robot]->settings.RobotRadius, + -relative/distance + robots[robot]->settings.RobotRadius*std::abs(w[2]), + robots[robot]->settings.AccSpeedupAbsoluteMax); + } + for (auto* wall : walls) { + dReal bounds[6]; + dGeomGetAABB(wall->geom, bounds); + auto wallImpact = [&](const dReal* position, const dReal* velocity, + dReal bodyRadius, dReal acceleration) { + dReal squared = 0, relative = 0; + for (int axis = 0; axis < 3; ++axis) { + const dReal nearest = std::max(bounds[2*axis], std::min(position[axis], bounds[2*axis+1])); + const dReal distance = position[axis]-nearest; + squared += distance*distance; + relative += distance*velocity[axis]; + } + const dReal distance = std::sqrt(squared); + if (distance > 0) impact(distance-bodyRadius, -relative/distance, acceleration); + else required = std::max(required, 20u); + }; + wallImpact(bp, bv, radius, 0); + for (int robot = 0; robot < count; ++robot) + wallImpact(dGeomGetPosition(robots[robot]->dummy->geom), + dBodyGetLinearVel(robots[robot]->chassis->body), + robots[robot]->settings.RobotRadius, robots[robot]->settings.AccSpeedupAbsoluteMax); + } + if (required >= adaptiveResolution) { + adaptiveResolution = required; resolutionDecayFrames = 0; + } else if (++resolutionDecayFrames >= 2) { + adaptiveResolution = required; resolutionDecayFrames = 0; + } + return adaptiveResolution; +} + void SSLWorld::stepPhysics(dReal dt, const PhysicsStepOptions& options, PWorld::StepStatistics* statistics) { if (!std::isfinite(dt) || dt <= 0 || !options.substeps) throw std::invalid_argument("Invalid physics timestep"); legacyBallFrictionActive = options.legacyBallFriction; - // Keep free motion on the nominal schedule. Nearby shells use six even - // steps for the entire frame, avoiding a velocity-dependent per-contact mesh. - const unsigned substeps = options.refineShellImpacts && shellsMayMeet(dt) - ? std::max(options.substeps, 6u) : options.substeps; + // Every body shares one even subdivision of the fixed tick. Historical + // policies remain explicit so saved recordings keep their original physics. + const unsigned substeps = options.integrationPolicy == IntegrationPolicy::Adaptive + ? adaptiveSubsteps(dt, options.substeps) + : (options.integrationPolicy == IntegrationPolicy::NearShellSix && shellsMayMeet(dt) + ? std::max(options.substeps, 6u) : options.substeps); for (unsigned kk=0; kk < substeps; ++kk) { if (options.finiteWheelRotation) { for (int k=0; k < cfg->Robots_Count() * 2; ++k) { @@ -697,10 +818,13 @@ void SSLWorld::yellowControlSocketReady() { void SSLWorld::processSimControl(const SimulatorCommand &simulatorCommand, SimulatorResponse &simulatorResponse) { + bool teleported = false; if(simulatorCommand.has_control()) { if(simulatorCommand.control().has_teleport_ball()) { auto teleBall = simulatorCommand.control().teleport_ball(); + const int previousErrors = simulatorResponse.errors_size(); processTeleportBall(simulatorResponse, teleBall); + teleported = simulatorResponse.errors_size() == previousErrors; } for(const auto &teleBot : simulatorCommand.control().teleport_robot()) { @@ -711,8 +835,14 @@ void SSLWorld::processSimControl(const SimulatorCommand &simulatorCommand, Simul auto robot = robots[id]; processTeleportRobot(teleBot, robot); + teleported = true; } } + if (teleported) { + adaptiveWarmupUntil = sim_time + 1; + quietFrames = resolutionDecayFrames = 0; + adaptiveResolution = 5; + } if (simulatorCommand.has_config()) { if (simulatorCommand.config().has_geometry() diff --git a/tests/check_physics_replay.py b/tests/check_physics_replay.py index d8ca6de4..d6467556 100644 --- a/tests/check_physics_replay.py +++ b/tests/check_physics_replay.py @@ -18,7 +18,7 @@ env = dict(os.environ, QT_QPA_PLATFORM="offscreen") -def run(args, accepted=True, step_seconds=1 / 60, substeps=5): +def run(args, accepted=True, step_seconds=1 / 60, substeps=2): result = subprocess.run([binary, *args], env=env, capture_output=True, text=True, timeout=30) if accepted: assert result.returncode == 0, result.stderr @@ -32,8 +32,12 @@ def run(args, accepted=True, step_seconds=1 / 60, substeps=5): def message(field, payload): - assert len(payload) < 128 - return bytes([(field << 3) | 2, len(payload)]) + payload + length = len(payload) + encoded = [] + while length > 127: + encoded.append((length & 127) | 128) + length >>= 7 + return bytes([(field << 3) | 2, *encoded, length]) + payload def difference(left, right): @@ -144,22 +148,82 @@ def difference(left, right): fixed_path.write_text(json.dumps(fixed_document)) run(["--replay", str(fixed_path)], accepted=False) print("PASS explicit fixed step counts alter integration without changing the tick clock", flush=True) - # Fast initial shell contact must meet the same overlap bound while turning. - turning_path = directory / "pushing-turning.json" - turning = run(["--record", str(turning_path), "--fixture", "pushing-turning", - "--ticks", "900", "--settings", str(profile_path)]) - turning_workers = run(["--replay", str(turning_path), "--helpers", "3"]) - # Keep the existing 3s cross-worker bound. Long forced-contact paths amplify - # roundoff even with the historical fixed-five integrator; full serial runs - # must instead repeat exactly, while both modes retain the 2mm contact bound. - turning_repeated = run(["--replay", str(turning_path), "--helpers", "0"]) - assert difference(turning, turning_repeated) <= 1e-12 - short_serial = dict(turning, checkpoints=[p for p in turning["checkpoints"] if p["tick"] <= 180]) - short_workers = dict(turning_workers, checkpoints=[p for p in turning_workers["checkpoints"] if p["tick"] <= 180]) - assert difference(short_serial, short_workers) <= 1e-6 - assert turning["integration_policy"] == "near-shell-6-v1" - assert turning["integration_steps"] > turning["ticks"] * turning["substeps"] - print("PASS 6v6 pushing/turning stays within 2mm with serial and threaded ODE", flush=True) + def floats(*values): + return b"".join(bytes([(field << 3) | 5]) + struct.pack(" 0 + if case == "ball-wall": + assert min(p["bodies"][0]["velocity"][1] for p in serial["checkpoints"][90:]) < -.1 + elif case == "robot-wall": + assert serial["max_robot_wall_penetration_m"] > 0 + assert serial["checkpoints"][-1]["bodies"][1]["position"][1] < wall_y + else: + assert serial["integration_steps"] < 180 * 4 + assert serial["checkpoints"][-1]["bodies"][1]["position"][0] > -1.2 + print(f"PASS {case}: bounded impacts, serial repeat and worker agreement", flush=True) + + # Both observed turn speeds must retain the 2 mm bound, including the + # previously failing 4 rad/s first-impact case. + for fixture in ["pushing-turning", "pushing-turning-fast"]: + turning_path = directory / (fixture + ".json") + turning = run(["--record", str(turning_path), "--fixture", fixture, + "--ticks", "900", "--settings", str(profile_path)]) + turning_workers = run(["--replay", str(turning_path), "--helpers", "3"]) + turning_repeated = run(["--replay", str(turning_path), "--helpers", "0"]) + assert difference(turning, turning_repeated) <= 1e-12 + short_serial = dict(turning, checkpoints=[p for p in turning["checkpoints"] if p["tick"] <= 180]) + short_workers = dict(turning_workers, checkpoints=[p for p in turning_workers["checkpoints"] if p["tick"] <= 180]) + assert difference(short_serial, short_workers) <= 1e-6 + assert turning["integration_policy"] == "adaptive-v1" + assert turning["integration_steps"] > turning["ticks"] * 5 + print(f"PASS 6v6 {fixture}: <2mm contacts, exact serial repeat and short worker agreement", flush=True) for fixture in ["spaced", "pushing"]: path = directory / (fixture + ".json") recorded = run(["--record", str(path), "--fixture", fixture, "--ticks", "180"]) @@ -213,12 +277,13 @@ def difference(left, right): print("PASS omitted response fields retain exact unit-response physics", flush=True) fixed = copy.deepcopy(legacy) fixed["integration_policy"] = "fixed-5-v1" + fixed["substeps"] = 5 changed.write_text(json.dumps(fixed)) - fixed_result = run(["--replay", str(changed)]) + fixed_result = run(["--replay", str(changed)], substeps=5) del fixed["integration_policy"] fixed["format"] = "grsim-physics-replay-v1" changed.write_text(json.dumps(fixed)) - historical = run(["--replay", str(changed)]) + historical = run(["--replay", str(changed)], substeps=5) assert historical["integration_policy"] == "fixed-5-v1" assert historical["integration_steps"] == historical["ticks"] * 5 assert difference(fixed_result, historical) <= 1e-12 diff --git a/tests/physics_experiments.cpp b/tests/physics_experiments.cpp index b35edf4f..f0258cc2 100644 --- a/tests/physics_experiments.cpp +++ b/tests/physics_experiments.cpp @@ -264,7 +264,8 @@ QJsonObject torqueExperiment(const SimulationSettings& settings, const Variant& } QJsonObject runScenario(SimulationSettings settings, const Variant& variant, const QString& scenario, - unsigned substeps, int requestedSteps, bool normalizeCompliance, double initialSpeed) + unsigned substeps, int requestedSteps, bool normalizeCompliance, double initialSpeed, + IntegrationPolicy integrationPolicy) { if (scenario == "torque-chassis" || scenario == "torque-wheel") return torqueExperiment(settings, variant, scenario == "torque-wheel", substeps); @@ -279,7 +280,7 @@ QJsonObject runScenario(SimulationSettings settings, const Variant& variant, con std::vector> contexts; const double complianceScale=applyVariant(world, variant, contexts, substeps, normalizeCompliance); PhysicsStepOptions options; - options.refineShellImpacts = variant.systemDefaults; + options.integrationPolicy = integrationPolicy; options.substeps = substeps; options.quickStepIterations = variant.iterations; options.legacyBallFriction = variant.rollingScale == 0; @@ -364,6 +365,7 @@ QJsonObject runScenario(SimulationSettings settings, const Variant& variant, con {"rms_joint_anchor_error_m", diagnostics.jointSamples ? std::sqrt(diagnostics.sumJointSquared/diagnostics.jointSamples) : 0}, {"max_wheel_axis_error_deg", diagnostics.maxAxisDegrees}, {"max_contact_penetration_m", contacts.maxPenetration}, {"detected_contacts",double(contacts.contacts)}, + {"integration_steps",double(contacts.integrationSteps)}, {"max_body_linear_speed_m_s", diagnostics.maxLinearSpeed}, {"max_body_angular_speed_rad_s",diagnostics.maxAngularSpeed}, {"min_chassis_z_m",robots ? diagnostics.minChassisZ : 0}, {"ball_runout_x_m",position[0]+2}, {"ball_stop_time_s",stopTime}, {"ball_first_rebound_height_m",firstBounceHeight}, @@ -412,10 +414,10 @@ int main(int argc,char** argv) { QApplication app(argc,argv); QString variantName="baseline", scenario="ball-flight"; - QString settingsFile; + QString settingsFile, integrationPolicyName; double rollingDeceleration = -1, initialSpeed = 2; unsigned substeps=5, seed=166; - bool normalizeCompliance=false; + bool normalizeCompliance=false, explicitSubsteps=false; int steps=0; for (int i=1; i=argc) return 2; @@ -424,6 +426,7 @@ int main(int argc,char** argv) if (option=="--variant") variantName=value; else if (option=="--scenario") scenario=value; else if (option=="--settings-file") settingsFile=value; + else if (option=="--integration-policy") integrationPolicyName=value; else if (option=="--rolling-deceleration" || option=="--ball-speed") { bool valid = false; const double number = value.toDouble(&valid); @@ -435,7 +438,7 @@ int main(int argc,char** argv) bool valid=false; const unsigned number=value.toUInt(&valid); if (!valid) return 2; - if (option=="--substeps") substeps=number; + if (option=="--substeps") { substeps=number; explicitSubsteps=true; } else if (option=="--steps") steps=number; else if (option=="--seed") seed=number; else if (option=="--normalize-contact-stiffness") normalizeCompliance=number != 0; @@ -445,6 +448,15 @@ int main(int argc,char** argv) const QStringList scenarios={"ball-flight","ball-rolling","ball-sliding","ball-spin","ball-drop", "torque-chassis","torque-wheel","robot-drive","robot-turn","robot-rest","crowded-contact", "perf-11","perf-16","perf-crowded"}; + if (integrationPolicyName.isEmpty()) + integrationPolicyName = variantName == "system" && !explicitSubsteps && !scenario.startsWith("torque-") + ? "adaptive" : "fixed"; + if (integrationPolicyName != "adaptive" && integrationPolicyName != "fixed" && integrationPolicyName != "near-shell-six") return 2; + const auto integrationPolicy = integrationPolicyName == "adaptive" ? IntegrationPolicy::Adaptive : + (integrationPolicyName == "near-shell-six" ? IntegrationPolicy::NearShellSix : IntegrationPolicy::Fixed); + if (!explicitSubsteps && integrationPolicy == IntegrationPolicy::Adaptive) substeps = PhysicsStepOptions().substeps; + if (normalizeCompliance && integrationPolicy != IntegrationPolicy::Fixed) return 2; + if (scenario.startsWith("torque-") && integrationPolicy != IntegrationPolicy::Fixed) return 2; if (!substeps || substeps>120 || !scenarios.contains(scenario)) return 2; ConfigWidget config(false, settingsFile); auto settings = config.simulationSettings(); @@ -452,12 +464,12 @@ int main(int argc,char** argv) if (!dInitODE2(dInitFlagManualThreadCleanup) || !dAllocateODEDataForThread(dAllocateMaskAll)) return 2; int status=0; QJsonObject output{{"collision_model","chassis-hull-2026"}, {"variant",variantName}, {"scenario",scenario}, {"seed",int(seed)}, - {"substeps",int(substeps)}, {"normalized_contact_stiffness",normalizeCompliance}, + {"substeps",int(substeps)}, {"integration_policy",integrationPolicyName}, {"normalized_contact_stiffness",normalizeCompliance}, {"ode_version",QString::fromLatin1(dODE_VERSION)}}; try { dRandSetSeed(seed); const auto result=runScenario(settings, variantFor(variantName), scenario, substeps, steps, - normalizeCompliance, initialSpeed); + normalizeCompliance, initialSpeed, integrationPolicy); for (auto i=result.begin(); i!=result.end(); ++i) output[i.key()]=i.value(); output["success"]=true; } catch (const std::exception& error) { diff --git a/tools/physics_replay.cpp b/tools/physics_replay.cpp index 54abc0f9..327b14fe 100644 --- a/tools/physics_replay.cpp +++ b/tools/physics_replay.cpp @@ -423,15 +423,15 @@ int main(int argc, char** argv) } } if (recordFile.isEmpty() == replayFile.isEmpty() || - (!fixture.isEmpty() && fixture != "spaced" && fixture != "pushing" && fixture != "pushing-turning") || + (!fixture.isEmpty() && fixture != "spaced" && fixture != "pushing" && fixture != "pushing-turning" && fixture != "pushing-turning-fast") || (!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|pushing-turning or --duration-ms N, or --replay FILE"); + throw std::runtime_error("Use --record FILE with --fixture spaced|pushing|pushing-turning|pushing-turning-fast or --duration-ms N, or --replay FILE"); QJsonObject recording; SimulationSettings settings; PhysicsStepOptions physicsOptions; - QString integrationPolicy = "near-shell-6-v1"; + QString integrationPolicy = "adaptive-v1"; if (!replayFile.isEmpty()) { recording = readJson(replayFile); const bool historical = recording["format"] == "grsim-physics-replay-v1"; @@ -446,14 +446,15 @@ int main(int argc, char** argv) if (recordedSteps != 5 || (!policy.isUndefined() && policy != "fixed-5-v1")) throw std::runtime_error("Unsupported historical integration policy"); integrationPolicy = "fixed-5-v1"; - physicsOptions.refineShellImpacts = false; + physicsOptions.integrationPolicy = IntegrationPolicy::Fixed; } else { - if (policy != "fixed-5-v1" && policy != "near-shell-6-v1" && policy != "fixed-v1") + if (policy != "fixed-5-v1" && policy != "near-shell-6-v1" && policy != "fixed-v1" && policy != "adaptive-v1") throw std::runtime_error("Missing or unsupported integration policy"); - if (policy != "fixed-v1" && recordedSteps != 5) + if (policy != "fixed-v1" && policy != "adaptive-v1" && recordedSteps != 5) throw std::runtime_error("Legacy integration policy requires five steps"); integrationPolicy = policy.toString(); - physicsOptions.refineShellImpacts = policy == "near-shell-6-v1"; + physicsOptions.integrationPolicy = policy == "near-shell-6-v1" ? IntegrationPolicy::NearShellSix : + (policy == "adaptive-v1" ? IntegrationPolicy::Adaptive : IntegrationPolicy::Fixed); } physicsOptions.substeps = recordedSteps; seed = integer(recording, "seed", std::numeric_limits::max()); @@ -501,13 +502,24 @@ int main(int argc, char** argv) QJsonArray events = recording["events"].toArray(), checkpoints; checkpoints.append(initial); unsigned tick = 0; - double maximumShellPenetration = 0; + double maximumShellPenetration = 0, maximumBallSolidPenetration = 0, maximumRobotWallPenetration = 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)); + const bool firstBall = contact.geom.g1 == world.ball->geom; + const bool secondBall = contact.geom.g2 == world.ball->geom; + if ((firstBall && contact.geom.g2 != world.ground->geom) || + (secondBall && contact.geom.g1 != world.ground->geom)) + maximumBallSolidPenetration = std::max(maximumBallSolidPenetration, double(contact.geom.depth)); + if (!firstBall && !secondBall && + (dGeomGetClass(contact.geom.g1) == dBoxClass || dGeomGetClass(contact.geom.g2) == dBoxClass)) { + for (auto* wall : world.walls) + if (contact.geom.g1 == wall->geom || contact.geom.g2 == wall->geom) + maximumRobotWallPenetration = std::max(maximumRobotWallPenetration, double(contact.geom.depth)); + } }; auto step = [&]() { world.stepPhysics(settings.DeltaTime(), physicsOptions, &statistics); @@ -545,9 +557,10 @@ int main(int argc, char** argv) auto* robot = command.add_robot_commands(); robot->set_id(id); auto* velocity = robot->mutable_move_command()->mutable_local_velocity(); velocity->set_left(0); - if (fixture == "pushing-turning") { + if (fixture == "pushing-turning" || fixture == "pushing-turning-fast") { velocity->set_forward(.5); - velocity->set_angular(id % 2 ? -2 : 2); + const double turn = fixture == "pushing-turning-fast" ? 4 : 2; + velocity->set_angular(id % 2 ? -turn : turn); } else { velocity->set_forward(tick < ticks * 2 / 3 ? (dense ? .5 : .15) : 0); velocity->set_angular(dense ? 0 : .2); @@ -644,6 +657,8 @@ int main(int argc, char** argv) {"math_backend", QString::fromLatin1(GRSIM_ODE_MATH_BACKEND)}, {"triangular_backend", QString::fromLatin1(triangularBackend())}, {"checkpoints", checkpoints}, {"max_shell_penetration_m", maximumShellPenetration}, + {"max_ball_solid_penetration_m", maximumBallSolidPenetration}, + {"max_robot_wall_penetration_m", maximumRobotWallPenetration}, {"contacts", double(statistics.contacts)}, {"integration_steps", double(statistics.integrationSteps)}, {"integration_policy", integrationPolicy}}; } catch (...) { From 3877bc01b1989788f251e1ed087a8d55aa94c2c5 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 4 Nov 2026 01:34:51 +0000 Subject: [PATCH 7/8] Restart adaptive settling for every placement entry point --- README.md | 12 ++--- include/sslworld.h | 2 + src/simulation.cpp | 13 +++++- src/sslworld.cpp | 20 ++++++--- tests/physics_defaults_regression.cpp | 64 ++++++++++++++++++++++++++- 5 files changed, 96 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index aea53ffa..0188dd9a 100644 --- a/README.md +++ b/README.md @@ -123,7 +123,7 @@ Citing If you use this in your research, please cite the original paper: ``` -`inproceedings{Monajjemi2011grSimR, +@inproceedings{Monajjemi2011grSimR, title={grSim - RoboCup Small Size Robot Soccer Simulator}, author={Valiallah Monajjemi and A. Koochakzadeh and S. S. Ghidary}, booktitle={RoboCup}, @@ -134,7 +134,7 @@ If you use this in your research, please cite the original paper: If you wish to cite this repo with it's modifications specifically, please cite: ``` -`misc{grsim2021, +@misc{grsim2021, author = {Mohammad Mahdi Rahimi and Jan Segre and Valiallah Monajjemi and A. Koochakzadeh and Sepehr MohaimenianPour and Nicolai Ommer and Avatar Kazunori Kimura and Jeremy Feltracco and Kenta Sato and Atousa Ahsani}, title = {GRSIM}, @@ -167,8 +167,9 @@ bin/grSim_replay --replay /tmp/pushing.json --helpers 0 > /tmp/serial.json bin/grSim_replay --replay /tmp/pushing.json --helpers 3 > /tmp/workers.json ``` -All fixture modes use six robots per side. `pushing-turning` adds sustained -pushing with alternating turns for first-impact collision coverage. For a bounded loopback command +All fixture modes use six robots per side. `pushing-turning` and +`pushing-turning-fast` add sustained pushing with alternating 2 and 4 rad/s +turn commands for first-impact collision coverage. 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 @@ -180,7 +181,8 @@ integration policy. The default adaptive policy uses two steps for settled, distant motion, at least five during acceleration, turning, sliding or flight, and finer steps for predicted robot, ball and wall impacts. All bodies share the same evenly divided fixed tick; short settling guards avoid rapidly -switching the resolution. Historical v1 recordings retain their fixed-five path; older binaries reject v2 rather than silently changing it. Compare +switching the resolution. Historical v1 recordings retain their fixed-five path; older binaries reject v2 rather than silently changing it. +Compare numerical trajectories within a chosen tolerance across backends; identical bits on different CPUs are not promised. diff --git a/include/sslworld.h b/include/sslworld.h index 16e84760..9e61ef03 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -105,6 +105,8 @@ class SSLWorld : public QObject PWorld::StepStatistics* statistics = nullptr); // Apply the same ball material, mass and optional damping on startup/edit. void updateBallPhysics(); + // Restart fine integration after any externally applied pose/reset edit. + void restartIntegrationSettling(); std::shared_ptr snapshot(quint64 generation) const; SSL_WrapperPacket* generatePacket(int cam_id=0); void addFieldLinesArcs(SSL_GeometryFieldSize *field); diff --git a/src/simulation.cpp b/src/simulation.cpp index b0daf503..966eded4 100644 --- a/src/simulation.cpp +++ b/src/simulation.cpp @@ -268,19 +268,26 @@ void SimulationWorker::act(SimulationAction action) auto releaseBall = [this]() { for (int i = 0; i < settings.Robots_Count() * 2; ++i) world->robots[i]->kicker->unholdBall(); }; + bool placed = false; switch (action.kind) { - case SimulationActionKind::ResetRobot: if (robot) robot->resetRobot(); break; - case SimulationActionKind::SetRobotEnabled: if (robot) robot->on = action.enabled; break; + case SimulationActionKind::ResetRobot: + if (robot) { robot->resetRobot(); placed = true; } + break; + case SimulationActionKind::SetRobotEnabled: + if (robot) { robot->on = action.enabled; placed = true; } + break; case SimulationActionKind::PlaceRobot: if (robot) { robot->setXY(action.x, action.y); if (action.reset) robot->resetRobot(); if (action.reset) robot->setDir(action.direction); + placed = true; } break; case SimulationActionKind::PlaceBall: releaseBall(); world->ball->setBodyPosition(action.x, action.y, action.z); + placed = true; dBodySetForce(world->ball->body, 0, 0, 0); dBodySetTorque(world->ball->body, 0, 0, 0); dBodySetLinearVel(world->ball->body, 0, 0, 0); @@ -318,6 +325,7 @@ void SimulationWorker::act(SimulationAction action) if (action.team < 0 || action.team > 1 || action.formation < 0 || action.formation > 3) break; RobotsFormation formation(static_cast(action.formation), &settings); formation.resetRobots(world->robots, action.team); + placed = true; break; } case SimulationActionKind::EnableTeam: @@ -325,6 +333,7 @@ void SimulationWorker::act(SimulationAction action) for (int i = 0; i < count; ++i) world->robots[world->robotIndex(i, action.team)]->on = action.enabled; break; } + if (placed) world->restartIntegrationSettling(); publish(); } diff --git a/src/sslworld.cpp b/src/sslworld.cpp index b40bf7ff..dac9dc1a 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -433,6 +433,13 @@ bool SSLWorld::shellsMayMeet(dReal frameStep) const return false; } +void SSLWorld::restartIntegrationSettling() +{ + adaptiveWarmupUntil = sim_time + 1; + quietFrames = resolutionDecayFrames = 0; + adaptiveResolution = 5; +} + unsigned SSLWorld::adaptiveSubsteps(dReal frameStep, unsigned minimum) { constexpr unsigned normalSteps = 5; @@ -699,6 +706,7 @@ void SSLWorld::recvActions() { } } if (grSimPacket.has_replacement()) { + bool replaced = false; for (int i=0; i < grSimPacket.replacement().robots_size(); i++) { int team = 0; if (grSimPacket.replacement().robots(i).has_yellowteam() && grSimPacket.replacement().robots(i).yellowteam()) team = 1; @@ -717,6 +725,7 @@ void SSLWorld::recvActions() { robots[id]->resetRobot(); robots[id]->setDir(dir); robots[id]->on = turn_on; + replaced = true; } if (grSimPacket.replacement().has_ball()) { dReal x = 0, y = 0, z = 0, vx = 0, vy = 0; @@ -734,7 +743,9 @@ void SSLWorld::recvActions() { ball->setBodyPosition(x,y,cfg->BallRadius()*1.2); dBodySetLinearVel(ball->body,vx,vy,0); dBodySetAngularVel(ball->body,0,0,0); + replaced = true; } + if (replaced) restartIntegrationSettling(); } } @@ -822,9 +833,8 @@ void SSLWorld::processSimControl(const SimulatorCommand &simulatorCommand, Simul if(simulatorCommand.has_control()) { if(simulatorCommand.control().has_teleport_ball()) { auto teleBall = simulatorCommand.control().teleport_ball(); - const int previousErrors = simulatorResponse.errors_size(); processTeleportBall(simulatorResponse, teleBall); - teleported = simulatorResponse.errors_size() == previousErrors; + teleported = true; } for(const auto &teleBot : simulatorCommand.control().teleport_robot()) { @@ -838,11 +848,7 @@ void SSLWorld::processSimControl(const SimulatorCommand &simulatorCommand, Simul teleported = true; } } - if (teleported) { - adaptiveWarmupUntil = sim_time + 1; - quietFrames = resolutionDecayFrames = 0; - adaptiveResolution = 5; - } + if (teleported) restartIntegrationSettling(); if (simulatorCommand.has_config()) { if (simulatorCommand.config().has_geometry() diff --git a/tests/physics_defaults_regression.cpp b/tests/physics_defaults_regression.cpp index b3fcc56e..1aca0f62 100644 --- a/tests/physics_defaults_regression.cpp +++ b/tests/physics_defaults_regression.cpp @@ -7,6 +7,7 @@ #include "configwidget.h" #include "simulation.h" #include "sslworld.h" +#include "grSim_Packet.pb.h" namespace { constexpr double tick = 1.0 / 60; @@ -203,7 +204,8 @@ void workerSettings(SimulationSettings settings) settings.value_Robots_Count = 1; settings.value_CommandListenPort = settings.value_SimControlListenPort = 0; settings.value_BlueControlListenPort = settings.value_YellowControlListenPort = 0; - SimulationWorker worker(std::make_shared()); + auto mailbox = std::make_shared(); + SimulationWorker worker(mailbox); worker.start(settings); auto configure = [&](const SimulationSettings& next, bool rebuild) { SimulationSettingsEdit edit; @@ -240,6 +242,66 @@ void workerSettings(SimulationSettings settings) configure(settings, false); checkBallSettings(*worker.findChild(), settings); std::puts("PASS startup/live/rebuilt ball settings, radius changes and zero resistance"); + auto* world = worker.findChild(); + auto oneTick = [&]() { + PWorld::StepStatistics statistics; + world->stepPhysics(tick, PhysicsStepOptions(), &statistics); + return statistics.integrationSteps; + }; + auto settle = [&]() { + SimulationAction park; + park.kind = SimulationActionKind::PlaceBall; + park.generation = mailbox->latest()->generation; + park.x = 4; park.y = -3; park.z = settings.BallRadius(); + worker.act(park); + for (int i = 0; i < 150; ++i) oneTick(); + require(oneTick() == 2, "placement guard fixture must first reach two settled steps"); + }; + // These edits run through the real GUI/worker action entry point. + for (auto kind : {SimulationActionKind::PlaceRobot, SimulationActionKind::ResetRobot, + SimulationActionKind::PlaceBall, SimulationActionKind::Formation}) { + settle(); + SimulationAction action; + action.generation = mailbox->latest()->generation; + action.kind = kind; + action.robot = 0; action.team = 0; action.formation = FORMATION_INSIDE_1; + action.x = -2; action.y = 0; action.z = settings.BallRadius(); + worker.act(action); + require(oneTick() >= 5, "GUI placement/reset must restart fine settling"); + } + settle(); + SimulatorCommand command; + auto* robot = command.mutable_control()->add_teleport_robot(); + robot->mutable_id()->set_id(0); robot->mutable_id()->set_team(BLUE); + robot->set_x(-2); robot->set_y(0); + SimulatorResponse response; + world->processSimControl(command, response); + require(response.errors_size() == 0 && oneTick() >= 5, "modern robot teleport must restart fine settling"); + for (bool ball : {false, true}) { + settle(); + grSim_Packet packet; + if (ball) { + packet.mutable_replacement()->mutable_ball()->set_x(4); + packet.mutable_replacement()->mutable_ball()->set_y(-2); + } else { + auto* replacement = packet.mutable_replacement()->add_robots(); + replacement->set_id(0); replacement->set_yellowteam(false); + replacement->set_x(-1.8); replacement->set_y(0); replacement->set_dir(0); + } + QUdpSocket sender; + const auto data = packet.SerializeAsString(); + require(sender.writeDatagram(data.data(), int(data.size()), QHostAddress::LocalHost, + world->commandSocket->localPort()) == qint64(data.size()), + "legacy placement datagram must be sent"); + world->commandSocket->waitForReadyRead(1000); + // readyRead normally drains synchronously through the worker; drain + // explicitly as well if the platform deferred signal delivery. + if (world->commandSocket->hasPendingDatagrams()) world->recvActions(); + const double x = dBodyGetPosition(ball ? world->ball->body : world->robots[0]->chassis->body)[0]; + require(std::abs(x - (ball ? 4 : -1.8)) < 1e-6, "legacy placement must actually reach the world"); + require(oneTick() >= 5, "legacy placement must restart fine settling"); + } + std::puts("PASS modern/legacy and GUI placement/reset restart fine integration after settled motion"); } catch (...) { worker.stop(); throw; } worker.stop(); } From b88877db326e56c5aa78338b397c3a41031a7e61 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 4 Nov 2026 01:35:54 +0000 Subject: [PATCH 8/8] Cover automatic reset and verify legacy ball movement --- src/sslworld.cpp | 5 ++++- tests/physics_defaults_regression.cpp | 5 +++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/sslworld.cpp b/src/sslworld.cpp index dac9dc1a..bf89344d 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -1192,7 +1192,10 @@ SSL_WrapperPacket* SSLWorld::generatePacket(int cam_id) { dir = robots[i]->getDir(k); // reset when the robot has turned over - if (cfg->ResetTurnOver() && k < 0.9) robots[i]->resetRobot(); + if (cfg->ResetTurnOver() && k < 0.9) { + robots[i]->resetRobot(); + restartIntegrationSettling(); + } if (visibleInCam(cam_id, x, y)) { SSL_DetectionRobot* rob = (is_blue) ? pPacket->mutable_detection()->add_robots_blue() diff --git a/tests/physics_defaults_regression.cpp b/tests/physics_defaults_regression.cpp index 1aca0f62..33b214dd 100644 --- a/tests/physics_defaults_regression.cpp +++ b/tests/physics_defaults_regression.cpp @@ -297,8 +297,9 @@ void workerSettings(SimulationSettings settings) // readyRead normally drains synchronously through the worker; drain // explicitly as well if the platform deferred signal delivery. if (world->commandSocket->hasPendingDatagrams()) world->recvActions(); - const double x = dBodyGetPosition(ball ? world->ball->body : world->robots[0]->chassis->body)[0]; - require(std::abs(x - (ball ? 4 : -1.8)) < 1e-6, "legacy placement must actually reach the world"); + const auto* position = dBodyGetPosition(ball ? world->ball->body : world->robots[0]->chassis->body); + require(std::abs(position[ball ? 1 : 0] - (ball ? -2 : -1.8)) < 1e-6, + "legacy placement must actually reach the world"); require(oneTick() >= 5, "legacy placement must restart fine settling"); } std::puts("PASS modern/legacy and GUI placement/reset restart fine integration after settled motion");