diff --git a/README.md b/README.md index aa37f23f..0188dd9a 100644 --- a/README.md +++ b/README.md @@ -167,14 +167,22 @@ 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` 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 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. 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. @@ -207,6 +215,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. @@ -217,4 +229,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/config/Utama-Radio-1.ini b/config/Utama-Radio-1.ini index 9e7413e3..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 @@ -30,9 +31,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 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..9e61ef03 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -50,11 +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; + unsigned substeps = 2; // Minimum; activity and predicted impacts select finer steps. unsigned quickStepIterations = 0; bool legacyBallFriction = false; bool finiteWheelRotation = false; + IntegrationPolicy integrationPolicy = IntegrationPolicy::Adaptive; }; class SendingPacket { @@ -82,6 +85,12 @@ class SSLWorld : public QObject static void processMoveCommand(RobotControlResponse &robotControlResponse, const RobotMoveCommand &robotCommand, 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: @@ -96,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/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/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 98d500fd..bf89344d 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -407,12 +407,170 @@ 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::restartIntegrationSettling() +{ + adaptiveWarmupUntil = sim_time + 1; + quietFrames = resolutionDecayFrames = 0; + adaptiveResolution = 5; +} + +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; - for (unsigned kk=0; kk < options.substeps; ++kk) { + // 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) { for (auto* wheel : robots[k]->wheels) { @@ -442,7 +600,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"); } @@ -548,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; @@ -566,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; @@ -583,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(); } } @@ -667,10 +829,12 @@ 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(); processTeleportBall(simulatorResponse, teleBall); + teleported = true; } for(const auto &teleBot : simulatorCommand.control().teleport_robot()) { @@ -681,8 +845,10 @@ void SSLWorld::processSimControl(const SimulatorCommand &simulatorCommand, Simul auto robot = robots[id]; processTeleportRobot(teleBot, robot); + teleported = true; } } + if (teleported) restartIntegrationSettling(); if (simulatorCommand.has_config()) { if (simulatorCommand.config().has_geometry() @@ -1026,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/check_physics_replay.py b/tests/check_physics_replay.py index 7782d1fb..d6467556 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=2): 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 @@ -32,8 +32,12 @@ def run(args, accepted=True, step_seconds=1 / 60): 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): @@ -129,6 +133,97 @@ 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) + 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"]) @@ -179,8 +274,27 @@ 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(legacy) + fixed["integration_policy"] = "fixed-5-v1" + fixed["substeps"] = 5 + changed.write_text(json.dumps(fixed)) + 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)], substeps=5) + 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_defaults_regression.cpp b/tests/physics_defaults_regression.cpp index b3fcc56e..33b214dd 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,67 @@ 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 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"); } catch (...) { worker.stop(); throw; } worker.stop(); } diff --git a/tests/physics_experiments.cpp b/tests/physics_experiments.cpp index 835a94a5..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,6 +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.integrationPolicy = integrationPolicy; options.substeps = substeps; options.quickStepIterations = variant.iterations; options.legacyBallFriction = variant.rollingScale == 0; @@ -363,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}, @@ -411,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; @@ -423,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); @@ -434,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; @@ -444,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(); @@ -451,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 e2278264..327b14fe 100644 --- a/tools/physics_replay.cpp +++ b/tools/physics_replay.cpp @@ -423,19 +423,40 @@ 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" && 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 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 = "adaptive-v1"; 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) + const bool historical = recording["format"] == "grsim-physics-replay-v1"; + if ((!historical && recording["format"] != "grsim-physics-replay-v2") || + 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 (recordedSteps != 5 || (!policy.isUndefined() && policy != "fixed-5-v1")) + throw std::runtime_error("Unsupported historical integration policy"); + integrationPolicy = "fixed-5-v1"; + physicsOptions.integrationPolicy = IntegrationPolicy::Fixed; + } else { + 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" && policy != "adaptive-v1" && recordedSteps != 5) + throw std::runtime_error("Legacy integration policy requires five steps"); + integrationPolicy = policy.toString(); + 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()); ticks = integer(recording, "ticks", 1000000); if (!recording["events"].isArray() || !recording["initial"].isObject()) @@ -468,28 +489,40 @@ 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", int(physicsOptions.substeps)}, {"settings", saveSettings(settings)}, {"initial", initial}, + {"integration_policy", integrationPolicy}}; } 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(), PhysicsStepOptions(), &statistics); + world.stepPhysics(settings.DeltaTime(), physicsOptions, &statistics); ++tick; if (tick % checkpointEvery == 0) checkpoints.append(state(world, tick)); }; @@ -499,6 +532,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 +541,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 +556,15 @@ 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" || fixture == "pushing-turning-fast") { + velocity->set_forward(.5); + 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); + } } accept(commandEvent(tick, "robot_control", command, team)); } @@ -609,14 +650,17 @@ 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()}, {"math_backend", QString::fromLatin1(GRSIM_ODE_MATH_BACKEND)}, {"triangular_backend", QString::fromLatin1(triangularBackend())}, {"checkpoints", checkpoints}, {"max_shell_penetration_m", maximumShellPenetration}, - {"contacts", double(statistics.contacts)}}; + {"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 (...) { dCleanupODEAllDataForThread(); dCloseODE(); throw; }