You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
{{ message }}
Repository navigation
Fix skipped/drifting frequency-limited handlers; keep speed when a moving omni agent gets a new goal - #76
1. Frequency-limited handlers run on a fixed grid; post-step callbacks
Callbacks and plugins with a frequency, collision_check_frequency and the DataMonitor refresh each decided whether they were due with sim_time - last >= interval, then set last to the time they ran. Two problems followed:
Skipped beats.sim_time accumulates one timestep at a time and carries float error. A 10 Hz callback on a 0.1 s timestep ran at gaps of 0.1 s and 0.2 s. A 1 Hz plugin accumulates 0.1 ten times, which is 0.9999999999999999, so it missed every other second.
Drift. Setting last to the time the handler actually ran let the phase drift whenever the interval was not a whole number of timesteps.
The fix is a new FrequencyGate (pybullet_fleet/_frequency.py), now used by all four:
Due times lie on a grid, k / frequency from the start of the run.
The comparison tolerates float error far below one timestep.
A handler runs at most once per step. After a gap (a pause, a late registration) it runs once at the next due time, not once for every due time it missed.
The first run is still one interval into the run, and dt is the simulated time since the previous run.
A DataMonitor with a GUI now refreshes every step, as documented. It used a strict > against one timestep before, so in practice it refreshed less often. Its default rate follows the final GUI mode: RECORD turns the GUI off in setup_pybullet(), and the monitor then refreshes once a second.
New API:
register_callback(cb, frequency=None, phase="step" | "post_step"). "post_step" runs right after the post_step event: poses are written, the active collision pairs are those of the new poses when the step ran a collision check, and the same frequency option applies, scheduled on the poses' time (step_end_time). This is the point at which a consumer that publishes or records each step's poses at a fixed rate wants to run. Before, it had to subscribe to POST_STEP and rate-limit by itself.
MultiRobotSimulationCore.step_end_time: the simulated time the bodies' poses belong to. Once a step has updated its objects (Phase-1 callbacks, plugin hooks, post_step, post-step callbacks), sim_time is still the time the step started at, while the poses are already those at its end. So step_end_time is sim_time + timestep from the object update on, sim_time in a pre_step subscriber (which runs before the update), and the elapsed time outside a step. Anything read from the bodies should be stamped with it.
One time for a pose and its trajectory. Position controllers (per-agent and batched, omni and differential) evaluate their trajectories at step_end_time and start a trajectory there, as kinematic joints already did; the core raises its pose-time flag before the object update so this holds during the update too. A step's pose is the trajectory at the time it is stamped with (from rest at 4 m/s² and 0.05 s steps: x = 0.08 at 0.20 s). Before, poses were evaluated at the step's start and so lagged their stamp by a step. Consequences: a goal set in a pre_step subscriber moves in that same step, a motion completes one step sooner, and a replaced goal is judged on the speed of the pose in effect, per-agent and batched alike, from whichever phase it is set.
The DataMonitor's default refresh (monitor_update_interval=None) follows the monitor window: every step while it is open (enable_monitor_gui, or gui when that is unset), once a second otherwise.
Docs: docs/architecture/two-phase-step.md covers the post-step phase, which time a pose belongs to (and that controllers work in it), and the schedule. plugins-events.md covers the plugin dt.
2. A goal replaced mid-motion continues the motion (omni) -- or is refused
OmniController built every new trajectory with build_tpi(p0=0, ...), which has no initial speed. On top of that, Agent.set_path added the current pose as an approach waypoint, which is a zero-length segment. So an agent cruising at full speed that got a goal further along the same line dropped to 0 m/s in one step and accelerated again. KinematicJoint already carries a replaced target's speed into the next trajectory; the omni controllers now do the same.
The rule: build the straight-line trajectory to the first waypoint from the agent's current state; take the goal if that trajectory exists within the configured limits, refuse it otherwise. The check and the motion use the same solver call, so they cannot disagree.
v0 is the signed component of the current velocity along the new line (magnitude clamped to vmax). A goal ahead keeps the speed; a goal behind is reached by slowing, reversing and driving back, with the velocity running continuously through zero. The slowing before a reversal is the solver's first phase and uses max_linear_accel.
Refused (no trajectory within the limits): a goal closer than the braking distance at max_linear_decel; a goal where the moving agent is; a goal off the line of motion, whose sideways velocity a straight-line trajectory cannot carry (a small documented tolerance absorbs numerical noise). Nothing ever brakes harder than configured and nothing restarts from rest mid-motion.
What a refusal does: nothing changes -- the current motion and path continue. Agent.set_path() / set_goal_pose() return False (True when taken; the AgentManager.set_goal_pose* wrappers pass it on) and the controller logs the reason at INFO. Agent.path_rejection(path) returns the reason without acting, as a PathRejection(reason, temporary): temporary is a refusal that lasts only while the agent moves; a fixed-base robot and an empty path are permanent. At rest every goal is taken, as before.
Callers inside the library retry a temporarily refused goal on their next update (a permanent refusal behaves as before): MoveAction goes back to NOT_STARTED, PatrolController advances only once a goal is taken, RandomWalkController picks again, and the behaviour tree's NavigateTo stays RUNNING (NavigationAdapter.start_navigation() may now return False; None still means started). FleetCommandDispatcher.navigate() checks first and reports a refusal in the ack's rejected map, before the command event, so nothing is mutated for it.
BatchOmniController applies the same rule through synchronized_set_path(), taking the velocity from the row's own profile rather than the reported (step-averaged) one. trapezoid_distance takes a per-row signed v0, and the evaluator no longer clips progress at the start, so a row that starts moving away from its goal runs behind its start before turning. Tests check per-agent and batch tracks against each other.
The initial velocity is used only while the agent is under way (a translation in progress or a velocity command), so the arrival step's leftover velocity never feeds a trajectory. Agent.set_path skips the approach waypoint in the same situation.
Solver fix: for a body starting away from its target, two-point-interpolation 1.3.1 can choose the smaller of two positive first-phase roots, which yields a trajectory that does not end at the target. try_build_tpi() now checks that every trajectory ends at its target and, if not, uses the larger root. Checkpoint restore goes through the same function. (Worth fixing in the library itself too.)
Navigation checkpoints record the forward trajectory's v0. The kinematic profile's controller state is now version 2.
DifferentialController turns in place before it drives, so it still starts from rest.
Validation
Full suite: 2202 passed, 12 skipped, 0 failed.
tests/test_pose_time.py (new, 14 tests): every pose stamped with step_end_time is the trajectory at that time, per-agent and batch, in post-step callbacks and outside a step; a goal replaced from each phase (outside, pre_step, Phase-1 callback, post_step event, post-step callback) is accepted or refused on the speed at step_end_time alike by both controllers; and the reported pre_step case (0.1 m ahead after five steps is inside the 0.125 m braking distance of 1.0 m/s; 0.13 m is taken). Evaluating at sim_time again fails 8 of them, raising the flag after the update fails 8, and the batch check on the next step's speed fails the 2 pre_step batch cases.
Follow-ons: the omni checkpoint proof checks the pose against the trajectory at the checkpoint's elapsed time and ends at 1.3 s (the arrival step); its evidence docs are updated, and the manipulation scenario's checkpoints move one step earlier. The per-agent/batch equivalence tests now compare the same path (the reference no longer gets the zero-length approach waypoint bc.set_path() does not add), and the batch buffered-write tests read the PyBullet pose by body_id.
tests/test_frequency_gate.py (new, 23 tests): the gate itself (no skipped beats, no drift at 3 Hz on a 0.1 s timestep, first run one interval in, one run after a gap, faster than the step rate), callbacks, plugins and collision_check_frequency through the core, the post-step phase order and rate, and step_end_time (including in a pre_step subscriber). The three core tests fail on main.
tests/test_goal_replacement_speed.py (new, 40 tests, per-agent and batch). Every motion test checks, per step, that the velocity vector never changes faster than the configured ramps:
speed kept toward a goal further ahead, and across repeated replacements;
stopping within reach at the configured deceleration, with no overshoot, reversal or lateral motion;
a goal behind is reached by reversing, continuously through zero (also from a velocity command);
refused, with the path and motion unchanged and the previous goal still reached: a goal inside the braking distance, at right angles, and where the agent is (also off a velocity command);
at rest any goal is taken; a goal set in the arrival step starts from rest;
MoveAction retries a refused path and completes it once the motion ends;
per-agent and batch produce the same track;
checkpoints keep the initial speed, including a reversal's negative one;
the solver's reversal trajectory ends at its target, and a reversal's checkpoint passes profile validation;
permanent refusals (fixed base) are not retried: MoveAction completes, NavigateTo counts as started.
Turning off the rejection, the signed v0, the root fix or the MoveAction retry each fails tests here.
tests/test_fleet_api.py: a refused goal is reported in the ack and the robot is not touched. A goal refused only after the command event (a handler of it changed the robot) is not acknowledged and not recorded as accepted. tests/test_behavior_tree.py: NavigateTo retries a refused start.
Existing tests that read the old last_exec / _accumulator fields now check the behavior: the schedule restarts after reset() / initialize_simulation(). The DataMonitor cadence test now pins exact counts, which were not deterministic before.
Benchmark (benchmark/run_benchmark.py --agents 500 --duration 10 --repetitions 2, alternating runs): step time 5.63–6.07 ms on main, 5.68–5.79 ms here, so no measurable change.
pre-commit run on the changed files (black, pyright, flake8, general hooks): passed. Sphinx with -W: passed.
Changelog
Updated CHANGELOG.md under [Unreleased] for this user-visible change.
No user-visible change; the reason is stated below.
Callbacks, plugins, the collision check and the DataMonitor refresh each
decided whether they were due with `sim_time - last >= interval` and then set
`last` to the time they ran. sim_time accumulates one timestep at a time and
carries float error, so a 10 Hz callback on a 0.1 s timestep ran at gaps of
0.1 s and 0.2 s, and a 1 Hz plugin (accumulating 0.1 ten times, which is
0.9999999999999999) missed every other second. Resetting `last` to the run
time also let the phase drift whenever the interval was not a whole number of
timesteps.
FrequencyGate schedules due times on a grid, k / frequency from the start of
the run, compares with a tolerance far below a timestep, runs at most once
per step, and after a gap moves on to the next due time instead of running
once per missed one. The first run is one interval in, as before, and dt is
the simulated time since the previous run.
register_callback() gains phase="post_step", for a frequency-limited callback
that runs after the post_step event, once poses are written and collisions
checked -- where a consumer publishing each step's poses wants to be.
step_end_time gives the time those poses belong to: during a step sim_time is
still the step's start.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Every new goal or path built its trajectory with build_tpi(p0=0, ...) and no
initial speed, and Agent.set_path put the current pose in front of it as an
approach waypoint -- a zero-length segment. An agent cruising at full speed
that received a goal further along the same line dropped to 0 m/s in one
step and accelerated again. A kinematic joint (KinematicJoint) already carries a replaced
target's speed into the next trajectory; the omni controllers now do too.
- build_tpi_from_speed() starts a trajectory at v0 (clamped to [0, vmax]).
When the goal is closer than the braking distance at the configured
deceleration it brakes exactly as hard as needed to stop there: a kinematic
agent has no other way to honour a goal without overshooting and coming
back.
- OmniController.set_path reads the agent's velocity before anything zeroes
it and starts the forward trajectory from its component toward the new
goal. The component across it, or away from it, is dropped at once, as
before. Later waypoints of a path still start from rest, as they arrive at
rest.
- BatchOmniController does the same; trapezoid_distance takes a per-row v0 so
the batched evaluation matches the per-agent TPI.
- Agent.set_path skips the approach waypoint while the agent is moving.
DifferentialController turns in place before driving and keeps starting from
rest.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
PRE_STEP is emitted before the object update, so its subscribers still read
the poses of the step's start; step_end_time returned the end for anything
inside step_once(). A flag set after the update loop now tells the two apart.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
The reason will be displayed to describe this comment to others. Learn more.
🔵 Needs a closer look
Two moderate frequency-gate transition issues remain unresolved.
No open issues 🎉
1 previously missed issue
In code that hasn't changed since last review
Frequency gate state persists across every-step transitions
pybullet_fleet/core_simulation.py:1171
When a plugin is changed from a positive frequency to None and then back to the same positive value, this branch leaves the old FrequencyGate attached. The positive-frequency branch only rebuilds when gate.frequency != freq, so it reuses the stale _next_index/_last_time and can invoke on_step immediately with the whole intervening interval instead of starting a fresh schedule. Clear _frequency_gate when entering the every-step branch (or track every frequency transition) before reusing it.
From review on the PR:
- A plugin switched from a frequency to every step and back kept its old
FrequencyGate and ran at once with the whole gap as dt. Every-step now
drops the gate, so a later frequency starts a fresh schedule.
- BatchOmniController carried the arrival step's average speed into a goal
set in the step the agent arrived (e.g. from a post-step callback); the
per-agent controller zeroes it on arrival. A new path now continues the
motion only while a translation is under way.
- set_collision_check_frequency() built the gate after storing the value, so
a negative frequency raised half-way. Non-positive values disable checks,
as they did before the gate, and nothing is stored until the gate is built.
- Post-step callbacks are scheduled on step_end_time, the time of the poses
they see, so a 10 Hz one runs when the poses reach 0.1 s, 0.2 s, ... The
first run of any schedule reports one interval (at least one step) as dt.
- The register_callback docstring says post-step callbacks see the new
poses' collisions only when the step ran a collision check.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Addressed the findings from the review summaries in 05dcbc1:
Plugin schedule across None (core_simulation.py:1164/1171): every-step now drops the plugin's gate, so a frequency set again later starts a fresh schedule. test_plugin_back_from_every_step_starts_a_fresh_schedule fails without it.
Stale terminal velocity in BatchOmniController (batch_omni.py:221): a new path continues the motion only while a translation is under way. A row that has just arrived starts from rest, as the per-agent controller does. test_a_goal_set_on_arrival_starts_from_rest[batch] sets the next goal from a post-step callback in the arrival step, and fails without the fix.
Collision frequency validation (core_simulation.py:1525): the gate is built before anything is stored. Non-positive values disable checks, as before the gate. Parametrised test for 0 and -1.
Gate post-step callbacks on step_end_time (core_simulation.py:1240): done. A 10 Hz post-step callback runs when the poses reach 0.1 s, 0.2 s, and so on. Phase-1 callbacks keep their start-time schedule. The first run of any schedule reports one interval (at least one step) as dt.
Collision freshness for post-step callbacks (nit): the docstring says the active pairs are the new poses' only when the step ran a collision check.
PRE_STEP timing in the docs (nit): covered by 182f107 (step_end_time is sim_time in a pre_step subscriber), in the docstring and in two-phase-step.md.
The reason will be displayed to describe this comment to others. Learn more.
🔵 Needs a closer look
Moderate issues remain in frequency-gate initialization and validation, plus restoration of nonzero trajectory speed.
No open issues 🎉
2 previously missed issues
In code that hasn't changed since last review
First callback skips an already-due grid point
pybullet_fleet/_frequency.py:64
The first-call baseline skips an already-due grid point because it always adds one after floor((now + slack) / interval). For example, after restore_completed_step_boundary() at 10.0 s, the first post-step callback sees now == step_end_time == 10.0 but initializes its next index to 11, so it misses the 10 Hz-grid point at 10.0 and does not run until 11.0; the same happens for a callback registered exactly on a due boundary. Initialize to the first index at or just before now (for example, ceil((now - slack) / interval), clamped to 1) so due() can consume the current grid point.
The new nonzero-v0 trajectory is not preserved by the existing navigation checkpoint path. capture_navigation_state() only copies t0, limits, and origin from capture_tpi_state() and restore_navigation_state() reconstructs the forward TPI with v0=0, so a checkpoint taken after a moving omni goal replacement resumes from rest with a different trajectory (and the profile validator currently rejects any added field). Include the initial speed in the checkpoint schema and restore it so this new behavior survives recording/restore.
From review:
- A schedule's first call took the grid point after the current time, so a
first call already on a due time skipped it: after a restore at 10.0 s a
10 Hz post-step callback skipped 10.0 s and first ran at 10.1 s, and a
10 Hz post-step callback in a fresh run (whose first time is one step in) first ran at 0.2 s. The first
call now takes the first grid point at or after now, never time zero, and
runs if it is due.
- Navigation checkpoints rebuilt the forward trajectory with v0 = 0, so a
checkpoint taken after a goal replaced mid-motion resumed on a different
trajectory. The forward state records v0 and restores it; the kinematic
profile's controller state is version 2 and validates the new field, and
the omni checkpoint proof expects v0 = 0 for its from-rest profile.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Addressed the two items from the latest review summary in 05da9a9:
First callback skips an already-due grid point (_frequency.py:64): the first call now takes the first grid point at or after now, never time zero, and runs if it is due. A 10 Hz post-step callback runs from the first step (pose time 0.1 s), and a schedule whose first call lands on a due time after a restore runs there. Phase-1 callbacks still first run one interval into a run. New tests test_a_first_call_on_a_due_time_runs_at_once and test_a_post_step_schedule_at_the_step_rate_runs_from_the_first_step both fail with the old baseline.
Navigation checkpoints lose nonzero initial velocity (controller.py:487): the forward state records v0 and restore_navigation_state rebuilds the trajectory with it. The kinematic profile's controller state is now version 2 and validates the field (finite, non-negative). The omni checkpoint proof expects v0 == 0 for its from-rest profile. test_a_checkpoint_keeps_the_initial_speed captures right after a goal is replaced at full speed and compares the restored trajectory; it fails when restoring with v0 = 0.
The reason will be displayed to describe this comment to others. Learn more.
🔵 Needs a closer look
Resolve the active-rotation goal replacement behavior and recompute monitor scheduling after final GUI/headless mode is established.
No open issues 🎉
1 previously missed issue
In code that hasn't changed since last review
Recompute monitor cadence after final GUI mode is set
pybullet_fleet/core_simulation.py:466
This gate is configured before setup_pybullet() runs, but that method can force self._params.gui from True to False when RECORD is set. With monitor_update_interval=None, the gate therefore remains in the GUI/every-step mode even though the simulation is now headless, regressing the documented 1 Hz headless DataMonitor cadence. Recompute this gate after the final GUI mode is established (or otherwise derive the mode at setup time).
The DataMonitor's default refresh rate depends on the GUI (every step with
one, once a second without), and its schedule was built in __init__ before
setup_pybullet(), which turns the GUI off when RECORD is set. A recording run
then refreshed the monitor every step. setup_pybullet() and
initialize_simulation() now rebuild the schedule from the final mode.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Addressed "Recompute monitor cadence after final GUI mode is set" (core_simulation.py:466) in 6c23733. setup_pybullet(), where RECORD turns the GUI off, and initialize_simulation() both rebuild the DataMonitor schedule from the final GUI mode. test_record_forced_headless_keeps_the_headless_monitor_rate requests a GUI with RECORD set and expects the headless 1 Hz refresh. It fails without the rebuild. Full suite: 2162 passed.
The summary also mentions an "active-rotation goal replacement behavior" but lists no finding for it. For reference: a goal replaced during a final in-place rotation starts the translation from rest, because the velocity during rotation is zero and only the component toward the new goal is carried. DifferentialController always rotates first and keeps starting from rest.
The reason will be displayed to describe this comment to others. Learn more.
🔵 Needs a closer look
Three unresolved moderate findings must be addressed before approval.
No open issues 🎉
1 previously missed issue
In code that hasn't changed since last review
Stale batch velocity suppresses waypoint at path completion
pybullet_fleet/agent.py:1393
This predicate is not reliable for batch agents: BatchKinematicController._apply_phase1() reports the previous step's average velocity, and a completed batch path changes _phase/is_moving to idle only after writing the final pose, so _current_velocity can still be nonzero immediately after arrival. Calling set_path() at that boundary therefore suppresses the approach waypoint even though the agent is no longer moving. Base this decision on the navigation state (self.is_moving) or clear the batch velocity when the path completes.
Addressed "Stale batch velocity suppresses waypoint at path completion" (agent.py:1393) in 9763736. Agent.set_path now decides on the navigation state (is_moving), not the reported velocity. test_a_path_set_on_arrival_keeps_its_approach_waypoint (per-agent and batch) sets the next path from a post-step callback in the arrival step and expects the approach waypoint. The batch case fails with the velocity check. Full suite: 2164 passed.
Agent.set_path skipped the approach waypoint when the reported velocity was
non-zero. A batch controller reports the arrival step's average speed until
its next flush, so a path set in the step the agent arrived lost its approach
waypoint although the agent had stopped. The decision now reads is_moving.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Use instantaneous endpoint velocity for batch replacements
pybullet_fleet/controllers/batch_omni.py:227
agent._current_velocity is not the same quantity here as in OmniController: BatchKinematicController._apply_phase1() derives it from the previous step's position delta divided by dt (an average velocity), whereas the per-agent controller stores the TPI's instantaneous velocity at the endpoint. If a goal is replaced during acceleration or braking, this v0 is therefore too low and the batch trajectory diverges from the per-agent trajectory (and does not actually continue the current speed). Derive the row's instantaneous profile speed from the batch trajectory, or retain that endpoint speed separately, before projecting it onto the new direction.`
BatchOmniController took a replaced goal's initial speed from the agent's
reported velocity, which the batch base derives from the last step's
displacement: an average, low while accelerating and high while braking. The
per-agent controller keeps the trajectory's instantaneous speed, so the two
diverged when a goal was replaced mid-ramp. The row's own trapezoid now gives
the speed at the time it was last evaluated.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
- Agent.path_rejection() returns a PathRejection(reason, temporary).
Temporary is a refusal that lasts only while the agent is moving (no
trajectory from its current velocity); a fixed-base robot and an empty path
are permanent. MoveAction, PatrolController, RandomWalkController and the
behaviour tree's navigation adapter retry only a temporary refusal, through
_replan.refused_for_now(); a permanent one behaves as before (MoveAction
completes, the adapter reports started). FleetCommandDispatcher reports the
reason string.
- The kinematic profile's checkpoint validation accepted only v0 >= 0; v0 is
signed now (a reversal records a negative one), so it checks it is finite.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
The reason will be displayed to describe this comment to others. Learn more.
🔵 Needs a closer look
Unresolved timing consistency and invalid-frequency handling issues remain.
No open issues 🎉
1 previously missed issue
In code that hasn't changed since last review
Preserve zero frequency validation for callbacks
pybullet_fleet/core_simulation.py:1242
frequency or None silently turns an explicit frequency=0 into an every-step callback, even though FrequencyGate rejects non-positive frequencies and the documented opt-in for every-step execution is None (the same argument is passed directly for plugins). This makes invalid callback configuration behave differently from the shared gate and can hide a bad rate; pass frequency through unchanged so zero is rejected consistently, or explicitly document zero as an alias and handle it in the gate.
two-point-interpolation 1.3.2 solves for the peak velocity directly, so a
body starting away from its target always gets a trajectory that ends there
(or a ValueError). try_build_tpi() no longer checks the end state and rebuilds
with the larger root; that copy of the solver is removed. With 1.3.1 installed
the reversal tests fail again, which is why the floor moves to 1.3.2.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
The reason will be displayed to describe this comment to others. Learn more.
Fixed in 5bb3e59, the first option: the per-agent and batched omni/differential controllers now evaluate their trajectories at step_end_time and start a trajectory there (pose_time() in controller.py), and the core raises the pose-time flag before the object update, so a pose and its stamp agree in every phase. tests/test_pose_time.py checks that every pose stamped with step_end_time is the trajectory at that time, per-agent and batch; evaluating at sim_time again fails 8 of its tests.
…ped with
From review: position controllers evaluated their trajectories at sim_time,
the step's start, while step_end_time stamped the resulting pose with the
step's end -- from rest at 4 m/s^2 with 0.05 s steps, the pose x = 0.08 (the
trajectory at 0.20 s) was stamped 0.25 s. And a batch row judged a goal set in
a pre_step subscriber on the speed at the advanced sim_time, the speed of the
step about to run, so per-agent accepted a goal the batched controller refused.
There is now one time for both. The core raises its pose-time flag before the
object update instead of after it, so step_end_time is the end of the step
during the update too; the per-agent and batched omni and differential
controllers evaluate their trajectories at it and start a trajectory at it
(pose_time() in controller.py, falling back to sim_time for a stand-in core).
Kinematic joints already evaluated at the end of the step. A goal set in a
pre_step subscriber now moves in that same step, a motion completes one step
sooner, and a replaced goal is judged on the speed of the pose in effect,
whichever phase sets it.
Follow-on changes:
- The omni checkpoint proof checks the checkpoint pose against the trajectory
at k*dt (the checkpoint's elapsed time) and ends at 1.3 s, the step that
now arrives; its evidence table and guide are updated. The manipulation
scenario's checkpoints move one step earlier with the arrival.
- The per-agent/batch equivalence tests compared different paths: the
reference agent got a zero-length approach waypoint from Agent.set_path that
bc.set_path() does not add, and the old one-step lag on the batch side hid
it. They now set the reference path with auto_approach=False.
- The batch buffered-write tests read the PyBullet pose by object_id instead
of body_id (a different body when a floor plane exists); they only passed
because nothing moved in the first step. They now read body_id and check the
row moved.
tests/test_pose_time.py: every pose stamped with step_end_time is the
trajectory at that time (per-agent and batch, in post-step callbacks and
outside a step; includes the reported 0.08 m at 0.20 s), and a goal replaced
from each phase -- outside, pre_step, Phase-1 callback, post_step event,
post-step callback -- is accepted or refused on the speed at step_end_time
alike by both controllers, plus the reported pre_step case.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
From review: the per-step default exists to keep the DataMonitor window
live, but it followed `gui` (the PyBullet viewer). A headless run that opens
the monitor window (enable_monitor_gui=True) refreshed it once a second, and a
viewer run that keeps it closed refreshed every step. The default now follows
the window's effective setting -- enable_monitor_gui, or gui when that is
None, the rule the DataMonitor itself uses. Docstring, configuration
reference and changelog updated; the new test fails with the old rule.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Copilot's review of 5bb3e59 lists one open point without an inline thread: the monitor's default refresh should follow the effective monitor window when enable_monitor_gui is on in a headless run. Done in ebb7ac8: the default (monitor_update_interval=None) is every step while the monitor window is open (enable_monitor_gui, or gui when that is None) and once a second otherwise. test_the_default_refresh_follows_the_monitor_window fails with the old gui-based rule. Full suite: 2201 passed.
Reconcile goal setter failure before emitting command acknowledgment
pybullet_fleet/fleet_api.py:141
_ack() emits FLEET_COMMAND_EVENT before these calls, so an event handler (or a concurrent command) can change the agent after the preflight check and make set_goal_pose() return False. That return value is ignored, leaving the already-emitted ack and recorded effective input claiming the robot was accepted even though the goal was refused. Apply the check and mutation under the same simulation-thread/command boundary, or reconcile the setter result before constructing the ack/event.
From review: navigate() emits the command event, then applies the goals, and
ignored set_goal_pose()'s result. A handler of that event can change a robot
so that it then refuses its goal, and the returned ack and the recorded
effective input still claimed it was accepted. The event is still emitted
before mutation (an existing contract); a goal refused after it is moved to the
ack's rejected map with its reason, left out of the recorded input, and logged
as a warning since the event already announced it.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Copilot's review of ebb7ac8 lists a previously missed point in fleet_api.py: navigate() emits the command event, then applies the goals and ignored set_goal_pose()'s result, so a goal refused after the event (a handler of it changed the robot) was still acknowledged and recorded as accepted. Fixed in 800e7c6. The event is still emitted before mutation, which is an existing contract (test_dispatcher_navigate_accepts_many_and_emits_before_mutation). A goal refused after it moves to the ack's rejected map with its reason, is left out of the recorded effective input, and is logged as a warning since the event already announced it. test_a_goal_refused_after_the_command_event_is_not_acknowledged covers it and fails without the fix. Full suite: 2202 passed.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
yuokamoto
left a comment
Summary
Two timing fixes, one commit each.
1. Frequency-limited handlers run on a fixed grid; post-step callbacks
Callbacks and plugins with a
frequency,collision_check_frequencyand the DataMonitor refresh each decided whether they were due withsim_time - last >= interval, then setlastto the time they ran. Two problems followed:sim_timeaccumulates one timestep at a time and carries float error. A 10 Hz callback on a 0.1 s timestep ran at gaps of 0.1 s and 0.2 s. A 1 Hz plugin accumulates 0.1 ten times, which is 0.9999999999999999, so it missed every other second.lastto the time the handler actually ran let the phase drift whenever the interval was not a whole number of timesteps.The fix is a new
FrequencyGate(pybullet_fleet/_frequency.py), now used by all four:k / frequencyfrom the start of the run.dtis the simulated time since the previous run.>against one timestep before, so in practice it refreshed less often. Its default rate follows the final GUI mode:RECORDturns the GUI off insetup_pybullet(), and the monitor then refreshes once a second.New API:
register_callback(cb, frequency=None, phase="step" | "post_step")."post_step"runs right after thepost_stepevent: poses are written, the active collision pairs are those of the new poses when the step ran a collision check, and the samefrequencyoption applies, scheduled on the poses' time (step_end_time). This is the point at which a consumer that publishes or records each step's poses at a fixed rate wants to run. Before, it had to subscribe toPOST_STEPand rate-limit by itself.MultiRobotSimulationCore.step_end_time: the simulated time the bodies' poses belong to. Once a step has updated its objects (Phase-1 callbacks, plugin hooks,post_step, post-step callbacks),sim_timeis still the time the step started at, while the poses are already those at its end. Sostep_end_timeissim_time + timestepfrom the object update on,sim_timein apre_stepsubscriber (which runs before the update), and the elapsed time outside a step. Anything read from the bodies should be stamped with it.One time for a pose and its trajectory. Position controllers (per-agent and batched, omni and differential) evaluate their trajectories at
step_end_timeand start a trajectory there, as kinematic joints already did; the core raises its pose-time flag before the object update so this holds during the update too. A step's pose is the trajectory at the time it is stamped with (from rest at 4 m/s² and 0.05 s steps: x = 0.08 at 0.20 s). Before, poses were evaluated at the step's start and so lagged their stamp by a step. Consequences: a goal set in apre_stepsubscriber moves in that same step, a motion completes one step sooner, and a replaced goal is judged on the speed of the pose in effect, per-agent and batched alike, from whichever phase it is set.The DataMonitor's default refresh (
monitor_update_interval=None) follows the monitor window: every step while it is open (enable_monitor_gui, orguiwhen that is unset), once a second otherwise.Docs:
docs/architecture/two-phase-step.mdcovers the post-step phase, which time a pose belongs to (and that controllers work in it), and the schedule.plugins-events.mdcovers the plugindt.2. A goal replaced mid-motion continues the motion (omni) -- or is refused
OmniControllerbuilt every new trajectory withbuild_tpi(p0=0, ...), which has no initial speed. On top of that,Agent.set_pathadded the current pose as an approach waypoint, which is a zero-length segment. So an agent cruising at full speed that got a goal further along the same line dropped to 0 m/s in one step and accelerated again.KinematicJointalready carries a replaced target's speed into the next trajectory; the omni controllers now do the same.The rule: build the straight-line trajectory to the first waypoint from the agent's current state; take the goal if that trajectory exists within the configured limits, refuse it otherwise. The check and the motion use the same solver call, so they cannot disagree.
v0is the signed component of the current velocity along the new line (magnitude clamped tovmax). A goal ahead keeps the speed; a goal behind is reached by slowing, reversing and driving back, with the velocity running continuously through zero. The slowing before a reversal is the solver's first phase and usesmax_linear_accel.max_linear_decel; a goal where the moving agent is; a goal off the line of motion, whose sideways velocity a straight-line trajectory cannot carry (a small documented tolerance absorbs numerical noise). Nothing ever brakes harder than configured and nothing restarts from rest mid-motion.Agent.set_path()/set_goal_pose()returnFalse(Truewhen taken; theAgentManager.set_goal_pose*wrappers pass it on) and the controller logs the reason at INFO.Agent.path_rejection(path)returns the reason without acting, as aPathRejection(reason, temporary): temporary is a refusal that lasts only while the agent moves; a fixed-base robot and an empty path are permanent. At rest every goal is taken, as before.MoveActiongoes back toNOT_STARTED,PatrolControlleradvances only once a goal is taken,RandomWalkControllerpicks again, and the behaviour tree'sNavigateTostaysRUNNING(NavigationAdapter.start_navigation()may now returnFalse;Nonestill means started).FleetCommandDispatcher.navigate()checks first and reports a refusal in the ack'srejectedmap, before the command event, so nothing is mutated for it.BatchOmniControllerapplies the same rule throughsynchronized_set_path(), taking the velocity from the row's own profile rather than the reported (step-averaged) one.trapezoid_distancetakes a per-row signedv0, and the evaluator no longer clips progress at the start, so a row that starts moving away from its goal runs behind its start before turning. Tests check per-agent and batch tracks against each other.Agent.set_pathskips the approach waypoint in the same situation.two-point-interpolation1.3.1 can choose the smaller of two positive first-phase roots, which yields a trajectory that does not end at the target.try_build_tpi()now checks that every trajectory ends at its target and, if not, uses the larger root. Checkpoint restore goes through the same function. (Worth fixing in the library itself too.)v0. The kinematic profile's controller state is now version 2.DifferentialControllerturns in place before it drives, so it still starts from rest.Validation
tests/test_pose_time.py(new, 14 tests): every pose stamped withstep_end_timeis the trajectory at that time, per-agent and batch, in post-step callbacks and outside a step; a goal replaced from each phase (outside,pre_step, Phase-1 callback,post_stepevent, post-step callback) is accepted or refused on the speed atstep_end_timealike by both controllers; and the reportedpre_stepcase (0.1 m ahead after five steps is inside the 0.125 m braking distance of 1.0 m/s; 0.13 m is taken). Evaluating atsim_timeagain fails 8 of them, raising the flag after the update fails 8, and the batch check on the next step's speed fails the 2pre_stepbatch cases.bc.set_path()does not add), and the batch buffered-write tests read the PyBullet pose bybody_id.tests/test_frequency_gate.py(new, 23 tests): the gate itself (no skipped beats, no drift at 3 Hz on a 0.1 s timestep, first run one interval in, one run after a gap, faster than the step rate), callbacks, plugins andcollision_check_frequencythrough the core, the post-step phase order and rate, andstep_end_time(including in apre_stepsubscriber). The three core tests fail onmain.tests/test_goal_replacement_speed.py(new, 40 tests, per-agent and batch). Every motion test checks, per step, that the velocity vector never changes faster than the configured ramps:MoveActionretries a refused path and completes it once the motion ends;MoveActioncompletes,NavigateTocounts as started.Turning off the rejection, the signed
v0, the root fix or theMoveActionretry each fails tests here.tests/test_fleet_api.py: a refused goal is reported in the ack and the robot is not touched. A goal refused only after the command event (a handler of it changed the robot) is not acknowledged and not recorded as accepted.tests/test_behavior_tree.py:NavigateToretries a refused start.last_exec/_accumulatorfields now check the behavior: the schedule restarts afterreset()/initialize_simulation(). The DataMonitor cadence test now pins exact counts, which were not deterministic before.benchmark/run_benchmark.py --agents 500 --duration 10 --repetitions 2, alternating runs): step time 5.63–6.07 ms onmain, 5.68–5.79 ms here, so no measurable change.pre-commit runon the changed files (black, pyright, flake8, general hooks): passed. Sphinx with-W: passed.Changelog
CHANGELOG.mdunder[Unreleased]for this user-visible change.🤖 Generated with Claude Code