ezmsg.kinematics.min_jerk#

Minimum-jerk reach generator with continuous re-planning.

Stdlib-only, scalar in / scalar out, dataclass-backed state. Deliberately free of any framework dependency so the same generator can run inside a streaming graph, inside a task’s render loop, or in an offline analysis, and produce the same numbers in all three.

The classic minimum-jerk reach is a function of time: over a duration T, position follows p(t) = A + s(τ)·(B - A) with τ = t/T and s(τ) = 10τ³ - 15τ⁴ + 6τ⁵, giving the familiar symmetric bell-shaped speed profile. That is the whole story when the generator is what moves the effector. It is not, when something else is also pushing – a decoder, a participant, a disturbance – because the effector then ends up somewhere the plan never predicted, and a plan that only knows about elapsed time has nothing to say about it.

This generator runs a reach in two phases.

Ballistic. For ballistic_duration after the reach starts, the command is pure feedforward: the planned min-jerk velocity at the elapsed time, along the original start→target axis, regardless of where the effector actually is. Real reaches begin open-loop, and it means the launch command is decisive and identical every time – valuable if something downstream has to learn it.

Re-planning. After that, every step re-solves the minimum-jerk problem from where the effector is to the target, and emits the beginning of that fresh solution. The boundary conditions are the measured position, the current commanded velocity and acceleration, and rest at the target.

Re-planning is what makes the response to a disturbance fall out rather than have to be designed:

  • Undisturbed, it changes nothing. The tail of a minimum-jerk trajectory is itself the minimum-jerk solution for the remaining problem, so re-planning from a point on your own trajectory reproduces it exactly. An effector that was never pushed follows the original reach, including its deceleration onto the target.

  • Disturbed, it compensates partially, and by the right amount. A fresh plan cannot contain a velocity discontinuity, so it curves back toward the target rather than snapping to the new bearing. How hard it corrects is set by how much time is left, not by a blend weight anyone has to tune.

  • Speed and turning trade off on their own. A large heading error forces the new polynomial to spend its early effort turning, which suppresses forward speed for as long as the turn takes.

  • It always eases in. Rest at the target is a boundary condition of every re-plan, so there is no approach speed to tune and no overshoot to damp.

The remaining horizon is floored at min_horizon. Without a floor, an effector held away from the target as its deadline runs out demands unbounded speed – the time-to-go singularity. The floor costs exactness only in the final min_horizon of an undisturbed reach, where everything involved is already near zero. max_speed_ratio clamps the command as a second line of defence.

Time is passed in rather than read from a clock, so a caller can drive this from whatever timebase its data carries – a stream’s sample timestamps, a shared network clock, or a recording being replayed offline. Two processes given the same reach epoch and the same timestamps produce the same commands.

The command and the nominal reach are two different signals. step() is the command: ballistic, then re-planned against wherever the effector actually is. plan_position() and plan_velocity() are the reach as planned at the cue, on the original axis, unconditioned on what happened. They agree exactly through the ballistic phase and separate afterwards by however much re-planning had to correct. A caller recording both gets the trajectory that was asked for and the one that was commanded, and their difference is the effector’s tracking error – none of which is recoverable from the command alone.

Typical use:

reach = MinJerkReach()
begin_reach(reach, cue_time, *effector_xy, *target_xy, peak_speed=800.0)
while reaching:
    vx, vy = step(reach, now, *effector_xy, movement_allowed=True)

Functions

begin_reach(state, t0, start_x, start_y, target_x, target_y, peak_speed, *, ballistic_duration=0.5, min_horizon=0.3, max_speed_ratio=2.0, seed=ReplanSeed.COMMANDED)[source]#

(Re)initialize state for a reach from a start point to a target.

t0 is when the reach begins, in whatever timebase the caller passes to step(). Anchoring to a timestamp the caller already has – a cue event’s own arrival time, say – rather than to a clock read here is what lets two processes agree without exchanging state.

peak_speed is the peak of the planned velocity bell, in the caller’s distance unit per second. The duration is derived from it and the distance, so a longer reach takes proportionally longer rather than going faster.

ballistic_duration is how long the command ignores the effector’s position. 0 re-plans from the first step, which is well defined but gives up the identical, decisive launch. Values at or beyond the reach’s own duration make it purely feedforward.

min_horizon floors the time a re-plan is allowed to assume remains. max_speed_ratio clamps commanded speed to that multiple of peak_speed; 0 disables the clamp. Both guard the same failure – an effector held away from the target while its deadline expires.

A degenerate reach – zero distance, or a non-positive peak_speed – leaves the state inactive rather than raising, so a caller driving this from live events need not pre-filter a target that coincides with the effector.

Return type:

None

Parameters:
plan_position(state, t)[source]#

Nominal position of the reach at time t: A + s(τ)·(B - A).

The reach as planned at the cue, on the original start→target axis, independent of where the effector actually went. It is the exact antiderivative of the ballistic command, so during the ballistic phase d/dt plan_position is step()’s output to machine precision. After that phase the two separate, and the difference is precisely the correction re-planning applied – which is the useful part, not an inconsistency.

Publish this alongside step() when something downstream needs the reach’s ground truth: the trajectory the task asked for, against which the effector’s actual path is an error signal. Recovering it from the commanded velocity is not equivalent – integrating a command that re-plans against a lagging effector runs past the target and keeps going, because each step’s velocity is anchored to the effector rather than to the integral.

τ is clamped to [0, 1], so a time before the cue gives the start point and a time past the nominal duration gives the target.

Returns None when no reach is active, rather than an origin that is indistinguishable from a real coordinate. step() can return (0, 0) for “no command” because rest is the unambiguous zero of a velocity; position has no such zero. A caller wanting an unbroken signal should substitute the effector’s own position – with no reach, where it is is where it should be.

Note that the nominal trajectory is a function of elapsed wall time. A caller that gates movement mid-reach should reset() and re-plan, not expect this to pause.

Return type:

tuple[float, float] | None

Parameters:
plan_velocity(state, t)[source]#

Nominal velocity of the reach at time t – the derivative of plan_position().

This is the ballistic command, evaluated for the whole reach rather than only for ballistic_duration. Use it when you want the planned reach unconditioned on what the effector did; use step() when you want the command to actually issue, which re-plans once the ballistic phase ends.

Returns (0, 0) for an inactive reach, matching step() – unlike plan_position(), which has no unambiguous zero to return.

Return type:

tuple[float, float]

Parameters:
reset(state)[source]#

Mark the reach inactive; subsequent step() calls return zero.

Return type:

None

Parameters:

state (MinJerkReach)

step(state, t, pos_x, pos_y, movement_allowed)[source]#

Velocity (vx, vy) for an effector at (pos_x, pos_y) at time t.

t is in the same timebase as the t0 given to begin_reach().

Returns (0.0, 0.0) when movement is not allowed or no reach is active. While movement is disallowed the command is held at rest and the clock reference is carried forward, so resuming does not produce a step proportional to how long the pause lasted.

Return type:

tuple[float, float]

Parameters:

Classes

class MinJerkReach(t0=0.0, start_x=0.0, start_y=0.0, target_x=0.0, target_y=0.0, D=0.0, T=0.0, ballistic_duration=0.5, min_horizon=0.3, max_speed=0.0, seed=ReplanSeed.COMMANDED, cmd_vx=0.0, cmd_vy=0.0, cmd_ax=0.0, cmd_ay=0.0, last_x=0.0, last_y=0.0, t_prev=0.0, active=False)[source]#

Bases: object

State for one minimum-jerk reach.

Lifecycle:

  • begin_reach() starts (or restarts) a reach. Call it whenever movement becomes allowed and whenever the target moves mid-reach.

  • step() returns a velocity for the current time and effector position. While movement is gated off, pass movement_allowed=False – the command goes to zero and the reach waits.

  • reset() is optional; it marks the reach inactive so a later step() returns zero even with movement_allowed=True.

Fields:

t0: timestamp the reach started, in the caller’s timebase. start_x, start_y: effector position captured at begin_reach. target_x, target_y: target captured at begin_reach. D: straight-line distance from start to target. T: nominal reach duration. Chosen so peak speed equals peak_speed. ballistic_duration: how long the feedforward phase lasts. min_horizon: floor on the remaining time a re-plan is given. max_speed: absolute clamp on commanded speed. 0 disables. seed: which velocity a re-plan continues from. cmd_vx, cmd_vy: velocity last commanded – the plan’s own motor state. cmd_ax, cmd_ay: acceleration last commanded. last_x, last_y: effector position at the previous step, for the

MEASURED seed.

t_prev: timestamp of the previous step. active: False before begin_reach or after reset.

Parameters:
t0: float = 0.0#
start_x: float = 0.0#
start_y: float = 0.0#
target_x: float = 0.0#
target_y: float = 0.0#
D: float = 0.0#
T: float = 0.0#
ballistic_duration: float = 0.5#
min_horizon: float = 0.3#
max_speed: float = 0.0#
seed: ReplanSeed = 'commanded'#
cmd_vx: float = 0.0#
cmd_vy: float = 0.0#
cmd_ax: float = 0.0#
cmd_ay: float = 0.0#
last_x: float = 0.0#
last_y: float = 0.0#
t_prev: float = 0.0#
active: bool = False#
__init__(t0=0.0, start_x=0.0, start_y=0.0, target_x=0.0, target_y=0.0, D=0.0, T=0.0, ballistic_duration=0.5, min_horizon=0.3, max_speed=0.0, seed=ReplanSeed.COMMANDED, cmd_vx=0.0, cmd_vy=0.0, cmd_ax=0.0, cmd_ay=0.0, last_x=0.0, last_y=0.0, t_prev=0.0, active=False)#
Parameters:
Return type:

None

class ReplanSeed(*values)[source]#

Bases: str, Enum

Which velocity a re-plan treats as the effector’s current velocity.

This is a modelling choice, not a tuning knob, and it matters whenever something other than this generator is moving the effector.

COMMANDED continues from the velocity this generator last asked for. The plan stays continuous with its own intent and treats the effector’s position as feedback about where that intent ended up. Appropriate when the output represents what someone or something wants – a disturbance moved the effector, but it did not move the intent. This is the default.

MEASURED continues from the effector’s observed velocity, differentiated from successive positions. The plan stays continuous with what is physically happening. Appropriate when the output drives the effector directly and you want the generator to own the real dynamics. Noisier, and if the output is being used as a training label it will carry whatever error the disturbance injected.

COMMANDED = 'commanded'#
MEASURED = 'measured'#
class MinJerkReach(t0=0.0, start_x=0.0, start_y=0.0, target_x=0.0, target_y=0.0, D=0.0, T=0.0, ballistic_duration=0.5, min_horizon=0.3, max_speed=0.0, seed=ReplanSeed.COMMANDED, cmd_vx=0.0, cmd_vy=0.0, cmd_ax=0.0, cmd_ay=0.0, last_x=0.0, last_y=0.0, t_prev=0.0, active=False)[source]#

Bases: object

State for one minimum-jerk reach.

Lifecycle:

  • begin_reach() starts (or restarts) a reach. Call it whenever movement becomes allowed and whenever the target moves mid-reach.

  • step() returns a velocity for the current time and effector position. While movement is gated off, pass movement_allowed=False – the command goes to zero and the reach waits.

  • reset() is optional; it marks the reach inactive so a later step() returns zero even with movement_allowed=True.

Fields:

t0: timestamp the reach started, in the caller’s timebase. start_x, start_y: effector position captured at begin_reach. target_x, target_y: target captured at begin_reach. D: straight-line distance from start to target. T: nominal reach duration. Chosen so peak speed equals peak_speed. ballistic_duration: how long the feedforward phase lasts. min_horizon: floor on the remaining time a re-plan is given. max_speed: absolute clamp on commanded speed. 0 disables. seed: which velocity a re-plan continues from. cmd_vx, cmd_vy: velocity last commanded – the plan’s own motor state. cmd_ax, cmd_ay: acceleration last commanded. last_x, last_y: effector position at the previous step, for the

MEASURED seed.

t_prev: timestamp of the previous step. active: False before begin_reach or after reset.

Parameters:
t0: float = 0.0#
start_x: float = 0.0#
start_y: float = 0.0#
target_x: float = 0.0#
target_y: float = 0.0#
D: float = 0.0#
T: float = 0.0#
ballistic_duration: float = 0.5#
min_horizon: float = 0.3#
max_speed: float = 0.0#
seed: ReplanSeed = 'commanded'#
cmd_vx: float = 0.0#
cmd_vy: float = 0.0#
cmd_ax: float = 0.0#
cmd_ay: float = 0.0#
last_x: float = 0.0#
last_y: float = 0.0#
t_prev: float = 0.0#
active: bool = False#
__init__(t0=0.0, start_x=0.0, start_y=0.0, target_x=0.0, target_y=0.0, D=0.0, T=0.0, ballistic_duration=0.5, min_horizon=0.3, max_speed=0.0, seed=ReplanSeed.COMMANDED, cmd_vx=0.0, cmd_vy=0.0, cmd_ax=0.0, cmd_ay=0.0, last_x=0.0, last_y=0.0, t_prev=0.0, active=False)#
Parameters:
Return type:

None

class ReplanSeed(*values)[source]#

Bases: str, Enum

Which velocity a re-plan treats as the effector’s current velocity.

This is a modelling choice, not a tuning knob, and it matters whenever something other than this generator is moving the effector.

COMMANDED continues from the velocity this generator last asked for. The plan stays continuous with its own intent and treats the effector’s position as feedback about where that intent ended up. Appropriate when the output represents what someone or something wants – a disturbance moved the effector, but it did not move the intent. This is the default.

MEASURED continues from the effector’s observed velocity, differentiated from successive positions. The plan stays continuous with what is physically happening. Appropriate when the output drives the effector directly and you want the generator to own the real dynamics. Noisier, and if the output is being used as a training label it will carry whatever error the disturbance injected.

COMMANDED = 'commanded'#
MEASURED = 'measured'#
begin_reach(state, t0, start_x, start_y, target_x, target_y, peak_speed, *, ballistic_duration=0.5, min_horizon=0.3, max_speed_ratio=2.0, seed=ReplanSeed.COMMANDED)[source]#

(Re)initialize state for a reach from a start point to a target.

t0 is when the reach begins, in whatever timebase the caller passes to step(). Anchoring to a timestamp the caller already has – a cue event’s own arrival time, say – rather than to a clock read here is what lets two processes agree without exchanging state.

peak_speed is the peak of the planned velocity bell, in the caller’s distance unit per second. The duration is derived from it and the distance, so a longer reach takes proportionally longer rather than going faster.

ballistic_duration is how long the command ignores the effector’s position. 0 re-plans from the first step, which is well defined but gives up the identical, decisive launch. Values at or beyond the reach’s own duration make it purely feedforward.

min_horizon floors the time a re-plan is allowed to assume remains. max_speed_ratio clamps commanded speed to that multiple of peak_speed; 0 disables the clamp. Both guard the same failure – an effector held away from the target while its deadline expires.

A degenerate reach – zero distance, or a non-positive peak_speed – leaves the state inactive rather than raising, so a caller driving this from live events need not pre-filter a target that coincides with the effector.

Return type:

None

Parameters:
plan_position(state, t)[source]#

Nominal position of the reach at time t: A + s(τ)·(B - A).

The reach as planned at the cue, on the original start→target axis, independent of where the effector actually went. It is the exact antiderivative of the ballistic command, so during the ballistic phase d/dt plan_position is step()’s output to machine precision. After that phase the two separate, and the difference is precisely the correction re-planning applied – which is the useful part, not an inconsistency.

Publish this alongside step() when something downstream needs the reach’s ground truth: the trajectory the task asked for, against which the effector’s actual path is an error signal. Recovering it from the commanded velocity is not equivalent – integrating a command that re-plans against a lagging effector runs past the target and keeps going, because each step’s velocity is anchored to the effector rather than to the integral.

τ is clamped to [0, 1], so a time before the cue gives the start point and a time past the nominal duration gives the target.

Returns None when no reach is active, rather than an origin that is indistinguishable from a real coordinate. step() can return (0, 0) for “no command” because rest is the unambiguous zero of a velocity; position has no such zero. A caller wanting an unbroken signal should substitute the effector’s own position – with no reach, where it is is where it should be.

Note that the nominal trajectory is a function of elapsed wall time. A caller that gates movement mid-reach should reset() and re-plan, not expect this to pause.

Return type:

tuple[float, float] | None

Parameters:
plan_velocity(state, t)[source]#

Nominal velocity of the reach at time t – the derivative of plan_position().

This is the ballistic command, evaluated for the whole reach rather than only for ballistic_duration. Use it when you want the planned reach unconditioned on what the effector did; use step() when you want the command to actually issue, which re-plans once the ballistic phase ends.

Returns (0, 0) for an inactive reach, matching step() – unlike plan_position(), which has no unambiguous zero to return.

Return type:

tuple[float, float]

Parameters:
reset(state)[source]#

Mark the reach inactive; subsequent step() calls return zero.

Return type:

None

Parameters:

state (MinJerkReach)

step(state, t, pos_x, pos_y, movement_allowed)[source]#

Velocity (vx, vy) for an effector at (pos_x, pos_y) at time t.

t is in the same timebase as the t0 given to begin_reach().

Returns (0.0, 0.0) when movement is not allowed or no reach is active. While movement is disallowed the command is held at rest and the clock reference is carried forward, so resuming does not produce a step proportional to how long the pause lasted.

Return type:

tuple[float, float]

Parameters: