Control

Virtual spring-damper laws on a measured [q; dq]: the rate-measured members of the classical family.

class minilink.control.impedance.ImpedanceController(dof=1, *, tracking_ref=False, Kp=10.0, Kd=1.0, u_min=-inf, u_max=inf, y_labels=None, y_units=None, u_labels=None, u_units=None)

Virtual spring-damper u = Kp (q_d − q) + Kd (dq_d − dq) on a measured [q; dq].

The measurement port y carries [q; dq] (dim 2·dof). The reference r is q_d (dim dof, regulation: dq_d = 0) or [q_d; dq_d] (dim 2·dof, tracking_ref=True). No state: the loop adds no pole.

What is measured decides the derivative: this block reads the rate, so it carries no filter state, where PD and PID filter the derivative of their one measurement.

Parameters:

• dof (int) – Number of axes.

• tracking_ref (bool) – If True, r is [q_d; dq_d]; otherwise q_d alone.

• Kp (float, vector or (dof, dof) array) – Stiffness and damping: a scalar or a per-axis vector is a diagonal matrix, a full matrix couples the axes. Stored in self.params as (dof, dof) arrays, the shape a value assigned there later must have.

• Kd (float, vector or (dof, dof) array) – Stiffness and damping: a scalar or a per-axis vector is a diagonal matrix, a full matrix couples the axes. Stored in self.params as (dof, dof) arrays, the shape a value assigned there later must have.

• u_min (float or vector) – Actuator limits per axis.

• u_max (float or vector) – Actuator limits per axis.

• y_labels (sequence of str, optional) – Display metadata for ports.

• y_units (sequence of str, optional) – Display metadata for ports.

• u_labels (sequence of str, optional) – Display metadata for ports.

• u_units (sequence of str, optional) – Display metadata for ports.

class minilink.control.impedance.ImpedanceIntegralController(dof=1, *, tracking_ref=False, Kp=10.0, Ki=1.0, Kd=1.0, u_min=-inf, u_max=inf, e_int_min=-inf, e_int_max=inf, y_labels=None, y_units=None, u_labels=None, u_units=None)

Spring-damper with integral action, u = Kp e + Ki ∫e + Kd ė, on a measured [q; dq].

ImpedanceController plus one integral state per axis on the position error e = q_d − q, held while the command or the integral state sits on a limit (the anti-windup of PID). Same ports and references.

Parameters:

• dof (as ImpedanceController.)

• tracking_ref (as ImpedanceController.)

• Kp (as ImpedanceController.)

• Kd (as ImpedanceController.)

• u_min (as ImpedanceController.)

• u_max (as ImpedanceController.)

• Ki (float or vector) – Integral gain per axis, never a matrix: the anti-windup hold pairs a command with its own error.

• e_int_min (float or vector) – End stops of the integral state per axis.

• e_int_max (float or vector) – End stops of the integral state per axis.

• y_labels (sequence of str, optional) – Display metadata for ports.

• y_units (sequence of str, optional) – Display metadata for ports.

• u_labels (sequence of str, optional) – Display metadata for ports.

• u_units (sequence of str, optional) – Display metadata for ports.

f(x, u, t=0, params=None)

State derivative dx = f(x, u, t; p).

Subclasses should treat this as a function of (x, u, t, params) only; the library does not verify purity.

Parameters:

• x (array of shape (n,))

• u (array of shape (m,))

• t (float)

• params (dict, optional) – None means “use self.params”.

Returns:

dx

Return type:

array of shape (n,)

Full-state linear feedback, constant or scheduled gain.

class minilink.control.state.StateFeedbackController(K, xbar=None, ubar=None)

Full-state feedback u = ubar - K (x - r) on a state reference.

The reference r is a state (dimension n); left unconnected it holds xbar, so the law regulates to xbar. This is the block minilink.control.design.lqr() and minilink.control.design.place() return. A reference on the output is OutputReferenceController.

Ports: the plant state x (dimension n) and the reference r. K has shape (m, n); ubar is the feedforward command.

This is state feedback on the full x port. For output feedback on a measured y (and @-operator wiring), use P.

class minilink.control.state.OutputReferenceController(K, N, xbar=None, ubar=None)

Full-state feedback u = ubar - K (x - xbar) + N r on an output reference.

The reference r is a desired output (dimension p, zero when unconnected), and N of shape (m, p) scales it to a command: the textbook’s N = -(C (A - B K)^-1 B)^-1 gives y = r at steady state for the plant (A, B, C) linearized about (xbar, ubar). A reference on the state is StateFeedbackController.

Ports: the plant state x (dimension n) and the reference r. K has shape (m, n); ubar is the feedforward command.

class minilink.control.state.TimeVaryingStateFeedbackController(t, K, xbar=None, ubar=None, K_after=None)

Full-state feedback with a gain schedule u = ubar - K(t) (x - r).

t holds N increasing sample times and K the matching gains, shape (N, m, n). Between samples the gain is interpolated linearly; before the first sample K[0] applies, and past the last sample the block keeps K[-1] — or applies K_after when one is given (for a finite-horizon design, the stationary gain). Ports are those of StateFeedbackController. This is the block minilink.control.design.lqr_finite_horizon() returns.

plot_gain_schedule(ax=None, show=True)

Draw every entry of the gain schedule K(t) against time.

class minilink.control.state.TrajectoryFeedbackController(trajectory, K)

Feedback around a reference trajectory u = u_d(t) - K(t) (x - x_d(t)).

trajectory is the reference, a Trajectory with N samples of x_d and u_d, and K holds the matching gains, shape (N, m, n). Reference and gains are interpolated linearly between samples; past the last sample the block holds the final point and gain, so it keeps regulating where the trajectory ends — usually an equilibrium. The measurement is the full state and the reference is the trajectory itself, so there is no reference port. This is the block minilink.control.design.trajectory_lqr() returns.

plot_gain_schedule(ax=None, show=True)

Draw every entry of the gain schedule K(t) against time.

minilink.control.state.plot_gain_schedule(t, K, *, title='Gain schedule', ax=None, show=True)

Draw every entry of a gain schedule K(t), shape (N, m, n), against time.

Classical compensators: the decoupled P, PI, PD and PID laws on a tracking error.

class minilink.control.siso.P(Kp=1.0, *, dof=1, ports='error', u_min=-inf, u_max=inf)

Decoupled proportional compensator u_i = Kp_i e_i, the static member of the family.

ports="error" (default) is the compensator form: one input e, so P(...) @ plant inserts the Error block e = r - y and P(...) >> plant is the loop gain, with no pole added. ports="reference" is the controller form with inputs r and y, the one a hand-wired diagram and the sampled loop (P(...) % dt) read. A coupled static gain on an output error is Gain(K) @ plant.

Parameters:

• Kp (float or vector) – Per-axis gain (a scalar broadcasts to all axes).

• dof (int) – Number of independent scalar loops (default 1).

• ports ({"error", "reference"}) – Input layout, see above.

• u_min (float or vector) – Actuator limits per axis, infinite by default.

• u_max (float or vector) – Actuator limits per axis, infinite by default.

class minilink.control.siso.PID(Kp=1.0, Ki=0.0, Kd=0.0, tau=0.1, *, dof=1, ports='error', y_filt0=0.0, u_min=-inf, u_max=inf, e_int_min=-inf, e_int_max=inf)

Decoupled PID with filtered derivative and anti-windup.

ports="error" (default) is the compensator form: one input e, so PID(...) @ plant inserts the Error block e = r - y and PID(...) >> plant is the loop gain. ports="reference" is the controller form with inputs r and y. Per axis,

u_i = Kp_i e_i + Ki_i e_int_i + Kd_i d_i,

where d_i is the filtered derivative of e_i (error form) or of -y_i (reference form: no derivative kick on reference steps); the integrator stops while the command saturates.

One measurement per axis, so the derivative is filtered (a pole at -1/tau); the rate-measured members of the same family are in minilink.control.impedance.

The block carries the states its terms need — 2 dof here. Setting Ki=0 does not remove the integrator: the gain is tunable, so the state dimension cannot depend on it, and the dead integrator then shows up as an unobservable pole at the origin in every linearization. Use PI, PD or P when a term is structurally absent, so poles and zeros match the hand calculation.

Parameters:

• Kp (float or vector) – Per-axis gains (a scalar broadcasts to all axes).

• Ki (float or vector) – Per-axis gains (a scalar broadcasts to all axes).

• Kd (float or vector) – Per-axis gains (a scalar broadcasts to all axes).

• tau (float or vector) – Derivative filter time constant per axis [s]; must be positive.

• dof (int) – Number of independent scalar loops (default 1).

• ports ({"error", "reference"}) – Input layout, see above.

• y_filt0 (float or vector) – Initial filtered state per axis (the filtered y, or -e).

• u_min (float or vector) – Saturation limits per axis, infinite by default.

• u_max (float or vector) – Saturation limits per axis, infinite by default.

• e_int_min (float or vector) – Saturation limits per axis, infinite by default.

• e_int_max (float or vector) – Saturation limits per axis, infinite by default.

has_integrator = True

Which terms this form carries — the state layout follows.

f(x, u, t=0, params=None)

State derivative dx = f(x, u, t; p).

Subclasses should treat this as a function of (x, u, t, params) only; the library does not verify purity.

Parameters:

• x (array of shape (n,))

• u (array of shape (m,))

• t (float)

• params (dict, optional) – None means “use self.params”.

Returns:

dx

Return type:

array of shape (n,)

class minilink.control.siso.PI(Kp=1.0, Ki=0.0, *, dof=1, ports='error', u_min=-inf, u_max=inf, e_int_min=-inf, e_int_max=inf)

Decoupled PI compensator u_i = Kp_i e_i + Ki_i e_int_i.

PID without the derivative term and without its filter state, so a PI loop has exactly one pole per axis at the origin and no filter pole. Same ports and anti-windup as PID.

Parameters:

• Kp (float)

• Ki (float)

• dof (int)

• ports (str)

• u_min (float)

• u_max (float)

• e_int_min (float)

• e_int_max (float)

class minilink.control.siso.PD(Kp=1.0, Kd=0.0, tau=0.1, *, dof=1, ports='error', y_filt0=0.0, u_min=-inf, u_max=inf)

Decoupled PD compensator u_i = Kp_i e_i + Kd_i d_i, d filtered.

PID without the integrator, so a PD loop carries only the derivative-filter pole at -1/tau per axis and no pole at the origin. Without integral action the loop keeps a static error; that is the point of the comparison. An unfiltered Kp + Kd s is improper and has no block: filter the derivative (this block, or its frequency-domain twin Lead) or measure the rate (ImpedanceController). For the proportional law alone use P.

Parameters:

• Kp (float)

• Kd (float)

• tau (float)

• dof (int)

• ports (str)

• u_min (float)

• u_max (float)

has_integrator = False

Which terms this form carries — the state layout follows.

Manipulator controllers: joint and task impedance, task-space kinematics (Pyro robotcontrollers.py).

class minilink.control.robotic.ModelJointImpedance(plant, *, tracking_ref=False, gravity_comp=False, gravity=None, Kp=None, Kd=None)

Joint-space impedance on a mechanical plant with optional gravity feedforward.

Law: τ = Kp e + Kd ė + g(q) when gravity_comp is enabled.

Parameters:

• plant (MechanicalSystem)

• tracking_ref (bool)

• gravity_comp (bool)

• gravity (GravityHook | None)

minilink.control.robotic.JointImpedance(plant_or_dof=1, *, dof=None, tracking_ref=False, gravity_comp=False, gravity=None, Kp=None, Kd=None, **impedance_kwargs)

Joint-space impedance: generic (dof only) or plant-aware.

JointImpedance(dof=n) returns a domain-neutral ImpedanceController.

JointImpedance(plant, gravity_comp=True) returns ModelJointImpedance with optional τ += g(q) (or a custom gravity(q) hook).

Parameters:

• tracking_ref (bool)

• gravity_comp (bool)

• gravity (Callable[[...], ndarray] | None)

class minilink.control.robotic.TaskImpedance(plant, *, tracking_ref=False, gravity_comp=False, gravity=None, Kp=None, Kd=None, show_task_force=False, task_force_scale=0.01, task_target_radius=0.04)

Task-space impedance using an internal kinematic model.

Joint measurements y = [q; dq] are mapped to task space via the plant’s forward_kinematics and J(q); no task-space plant ports are required for control (p / pdot remain for visualization).

τ = J(q)ᵀ (Kp e_p + Kd e_v) [+ g(q)]

Reference r has dim task_dim (regulation) or 2·task_dim for stacked [p_d; pdot_d] tracking.

Optional visualization (show_task_force=True) draws:

• a ball at the desired task point p_d (frame task_target);

• the commanded spring-damper task force f_task = Kp e + Kd ė at the current end-effector (frame task_force) — not gravity feedforward and not the mapped joint torque.

In a diagram those frames namespace to controller:task_target / controller:task_force.

Parameters:

• plant (Manipulator)

• tracking_ref (bool)

• gravity_comp (bool)

• gravity (GravityHook | None)

• show_task_force (bool)

• task_force_scale (float)

• task_target_radius (float)

task_quantities(u, params=None)

Return (q, p, p_d, J, f_task) for the current reference and measurement.

tf(x, u, t=0, params=None)

World transforms as dict[str, 4x4] (the forward-kinematics hook).

Return only named articulation frames (body, link0, …). Do not emit "world": I — the animator injects an implicit identity root for geometry keyed to "world".

get_dynamic_geometry(x, u, t=0, params=None)

Per-frame geometry as dict[str, list[primitive]] (rebuilt each frame).

class minilink.control.robotic.TaskKinematic(plant, *, Kp=None)

Task-space kinematic controller for velocity-controlled manipulators.

Joint measurements y = q; output u = dq drives a SpeedControlledManipulator.

v_task = Kp (p_d − p) dq = J(q)⁺ v_task

Reference r is the desired end-effector position p_d (dim task_dim).

class minilink.control.robotic.TaskKinematicNullspace(plant, *, Kp=None, K_null=None)

Kinematic controller with a secondary joint posture in the nullspace.

dq = J⁺ v_task + (I − J⁺ J) K_null (q_null − q)

Primary reference r is p_d; secondary reference r_null is q_null. Gain K_null lives in params.

Model-based joint control: computed torque and sliding mode on the plant’s own model.

class minilink.control.modelbased.ComputedTorqueController(plant, *, tracking_ref=True, Kp=None, Kd=None)

Computed torque with built-in outer PD on joint space (Pyro nonlinear.py).

Desired acceleration qdd = Kp (q_d - q) + Kd (dq_d - dq) (regulation uses dq_d = 0), then τ = inverse_dynamics(q, dq, qdd).

y is the plant’s state [q; dq] (ctl @ plant); reference r is q_d (dim dof) or stacked [q_d; dq_d] when tracking_ref=True.

The embedded model (inverse_dynamics) reads the referenced plant’s live self.params; the controller’s own params carry gains only. See DESIGN §4 (Embedded-model params rule).

Parameters:

• plant (MechanicalSystem)

• tracking_ref (bool)

ctl(x, u, t=0, params=None)

Evaluate the bound regulation or tracking law.

class minilink.control.modelbased.SlidingModeController(plant, *, tracking_ref=True, lam=None, gain=None, nab=None)

Joint-space sliding-mode law on a mechanical plant (Pyro nonlinear.py).

Inherits computed-torque port layout (r, y = [q; dq]). Fixed-goal regulation uses dq_d = ddq_d = 0; stacked r = [q_d; dq_d] when tracking_ref=True.

The law drives the tracking error onto the sliding surface s = ė + λ e with a computed-torque reaching term and a discontinuous gain (the equations are written in ctl_regulation() and ctl_tracking()).

Parameters:

• plant (MechanicalSystem)

• tracking_ref (bool)

Linear state-feedback design: a model or arrays in, a controller block out.

minilink.control.design.lqr_gain(A, B, Q, R)

Return the optimal feedback gain K of the law u = -K x minimizing ∫ xᵀQx + uᵀRu dt.

minilink.control.design.place_gain(A, B, poles)

Return the gain K of the law u = -K x that puts the eigenvalues of A − B K at poles.

minilink.control.design.lqr(sys, Q, R, *, x_bar=None, u_bar=None, t=0.0, params=None, method='auto', eps=1e-06)

Design an LQR on sys and return a StateFeedbackController.

sys is a linear model (an LTISystem, used as is with the operating point it carries) or any plant, linearized first about its command port at (x_bar, u_bar), the nominal state and inputs when omitted. The block implements u = ubar - K (x - r) with r defaulting to xbar, the operating point of the model; wire the plant state into its x port. On arrays the gain is lqr_gain(), and the model LTISystem(A, B). Only the command port enters B (u, else the one input that is not a disturbance d or a noise n); every other input is held at its nominal value.

Parameters:

• sys (System) – The linear model or the plant.

• Q (array-like) – State and input weights of ∫ xᵀQx + uᵀRu dt.

• R (array-like) – State and input weights of ∫ xᵀQx + uᵀRu dt.

• x_bar (array-like, optional) – Operating point to linearize a plant about, the nominal by default; a linear model carries its own.

• u_bar (array-like, optional) – Operating point to linearize a plant about, the nominal by default; a linear model carries its own.

• t (optional) – Linearization time, parameter set, differentiation backend and central-difference step, as in linearize().

• params (optional) – Linearization time, parameter set, differentiation backend and central-difference step, as in linearize().

• method (optional) – Linearization time, parameter set, differentiation backend and central-difference step, as in linearize().

• eps (optional) – Linearization time, parameter set, differentiation backend and central-difference step, as in linearize().

minilink.control.design.place(sys, poles, *, x_bar=None, u_bar=None, t=0.0, params=None, method='auto', eps=1e-06)

Place the closed-loop poles of sys and return a StateFeedbackController.

sys is a linear model or any plant, linearized first about its command port at (x_bar, u_bar), as in lqr(), with poles (n of them, complex ones in conjugate pairs) in place of Q, R. The block implements u = ubar - K (x - r) with r defaulting to xbar. On arrays the gain is place_gain().

minilink.control.design.lqg(sys, Q, R, Q_n, R_n, *, x_bar=None, u_bar=None, t=0.0, params=None, method='auto', eps=1e-06)

Design an LQG compensator on sys, an LQR gain on a Kalman estimate, and return it as one controller box.

sys is a linear model or any plant measured through y, as in kalman(). Q, R are the LQR weights, Q_n, R_n the process and measurement noise intensities of kalman(). The box holds the StateObserver under observer and the StateFeedbackController under gain, the estimate wired into the gain and the command back into the observer, with r, y -> u on its edge: box @ plant closes the loop, and the closed-loop poles are the gain’s and the observer’s together (the separation principle).

minilink.control.design.lqr_gain_schedule(A, B, Q, R, S_f, tf, *, n_steps=1001)

Return (t, K, S): the finite-horizon gain K(t) of the law u = -K(t) x and the Riccati matrix S(t).

S is swept backward from the terminal weight S(t_f) = S_f, exactly on a uniform grid, through the Hamiltonian system of the co-state (riccati_step()): no integration error, no stiffness, and S stays symmetric. t holds n_steps samples from 0 to tf; K has shape (n_steps, m, n) and S shape (n_steps, n, n).

minilink.control.design.lqr_finite_horizon(sys, Q, R, S_f, tf, *, n_steps=1001, after='hold', x_bar=None, u_bar=None, t=0.0, params=None, method='auto', eps=1e-06)

Design a finite-horizon LQR on sys and return a TimeVaryingStateFeedbackController.

sys is a linear model or any plant, as in lqr(). The block implements u = ubar - K(t) (x - r) with K(t) from lqr_gain_schedule(), minimizing ∫₀^tf xᵀQx + uᵀRu dt + x(tf)ᵀ S_f x(tf) for the linear model. Past the horizon the block keeps K(tf) (after="hold") or switches to the infinite-horizon gain of lqr_gain() (after="stationary").

minilink.control.design.trajectory_lqr(sys, trajectory, Q, R, S_f=None, *, params=None, method='auto', eps=1e-06)

Design an LQR along a reference trajectory and return a TrajectoryFeedbackController.

sys is linearized at every sample (x_d(t_k), u_d(t_k)) of trajectory, and the Riccati differential equation of the error x - x_d is swept backward along the samples, the dynamics frozen on each interval and each interval exact, as in lqr_gain_schedule(). The gains give the law u = u_d(t) - K(t) (x - x_d(t)). S_f defaults to the algebraic Riccati solution at the final sample, so the gain held past the horizon is the stationary LQR of the end point.

Parameters:

• sys (System) – Plant to stabilize.

• trajectory (Trajectory) – Reference (t, x_d, u_d), N samples at increasing times.

• Q (array-like) – LQR weights on the tracking error and on the input deviation.

• R (array-like) – LQR weights on the tracking error and on the input deviation.

• S_f (array-like, optional) – Terminal weight S(t_f); default the stationary solution at the end.

• params (dict, optional) – Parameter dict forwarded to sys.f.

• method ({"auto", "fd", "jax"}, optional) – Differentiation backend, see jacobian().

• eps (float, optional) – Central-difference step.

Pure pursuit: steer along the arc through a look-ahead point of a waypoint path.

minilink.control.geometric.MIN_REACH = 1e-06

[m] floor on the distance to the target, so the arc stays finite if the target ever lands on the axle itself (a path shorter than one lookahead, driven to its end).

class minilink.control.geometric.PurePursuit(waypoints, wheelbase=0.34, lookahead=0.6, lookahead_gain=0.25, delta_max=0.52, rear_offset=0.0, closed=None, state_dim=9)

Steer toward the point of the path one lookahead distance ahead.

The law is geometry only. Write the target in the body frame at the rear axle, (x_t, y_t), at distance d; the circle through the axle and that point, tangent to the car, has curvature 2 y_t / d^2, and the kinematic bicycle holds that curvature at delta = arctan(wheelbase * curvature). No path derivative and no gain to tune apart from the lookahead — which grows with speed, so a fast car aims further ahead and stops weaving.

Input port y: the vehicle state in the minilink vehicle layout, i.e. [x, y, theta, vx, ...]. Output port u: the steer command [rad].

The target is the first waypoint at least one lookahead ahead of the closest one, counted along the path. Counting along the path rather than by array index is what makes a closed circuit work: at the seam the waypoints ahead of the car are the ones at the start of the array, and an index comparison finds none of them and falls back to the last waypoint — which sits under the car, where the arc is undefined.

Parameters:

• waypoints (array of shape (N, 2)) – Path samples [m], in the order they are driven. A closed loop ends one spacing short of its first point (circuit_waypoints()); it is not closed by repeating that point, which would leave a zero-length segment behind.

• wheelbase (float) – Distance between the axles [m].

• lookahead (float) – Lookahead distance at standstill [m].

• lookahead_gain (float) – Added lookahead per unit speed [s], so L_d = lookahead + gain * |vx|.

• delta_max (float) – Steer command limit [rad].

• rear_offset (float) – Distance from the point the state locates back to the rear axle [m], where the pursuit geometry is written: zero when the state already sits at the rear axle, and the centre-of-gravity-to-rear-axle distance for a model that carries its state at the centre of gravity.

• closed (bool or None) – Whether the path loops. None decides from the waypoints themselves: the path is closed when the gap from its last point back to its first is no wider than the spacing between neighbours (is_closed()).

• state_dim (int) – Dimension of the state the block reads (the plant’s y port).

minilink.control.geometric.is_closed(waypoints)

True when the polyline loops back on itself within one waypoint spacing.

A closed path is written without repeating its first point at the end — the repeat would leave a zero-length segment with no tangent — so the loop shows up as a last-to-first gap no wider than the other steps.

Neural state-feedback controllers: the law u = pi(x) that policy search trains.

class minilink.control.neural.NeuralPolicyController(sys, *, features=None, hidden=(64, 64), activation='tanh', normalize=True, squash='clip', seed=0, name='Neural Policy Controller', show_setpoint=False, setpoint_plant=None, setpoint_radius=0.04, task_target=None, task_target_radius=0.04)

State feedback u = u_mid + u_half * clip(MLP(z(x)), -1, 1).

The law reinforcement learning, or any policy search, trains: features of the state, a multilayer perceptron, and a map from the network’s normalized output to the input-port bounds. Its input port is named x, so ctl @ plant closes the loop on the plant state, plot_control_law draws the law, and the weights are ordinary params entries that a planner updates in place.

Parameters:

• sys (System) – Plant whose state bounds and action-port bounds ("u", or its single input port, e.g. the reference r of an inner loop) define the feature normalization and the action range.

• features (callable, optional) – Observation map z = features(x) (must trace under JAX). Use it to make angles periodic or to give the policy task-relative sensing. Default: the state scaled by its bounds when normalize is true.

• hidden (tuple of int) – Hidden widths of the policy network.

• activation ({"tanh", "relu"}) – Hidden nonlinearity.

• normalize (bool) – Scale the default features by the state box (needs finite bounds).

• squash ({"clip", "tanh"}) – How the network output is kept in [-1, 1]: clipped (PPO family) or squashed by tanh (SAC family).

• seed (int) – Weight initialization seed.

• show_setpoint (bool) – Draw a ball at the commanded set-point (default False).

• setpoint_plant (System, optional) – Plant whose forward_kinematics maps a joint action to a point. Default: sys itself, or the first manipulator leaf in a diagram.

• setpoint_radius (float) – Ball radius in metres (default 0.04).

• task_target (array, optional) – Fixed task point p*. When set, a second ball is drawn there.

• task_target_radius (float) – Radius of the task ball (default 0.04).

Notes

The trainable weights are params["mlp"]; the normalized-action range [-1, 1] maps onto the u port bounds, which is what makes exploration cover the whole input range from the first step.

Optional visualization: show_setpoint=True draws a ball at the commanded set-point (forward_kinematics(r) when the action is a joint reference, or the action itself when it is already a task point). task_target draws a second, fixed ball at the task point.

observe(x)

Features z(x) fed to the network.

mean_action(x, params=None)

Normalized network output a = MLP(z(x)) (unclipped).

action(x, params=None)

Plant input u = u_mid + u_half * squash(a) with squash clip or tanh.

ctl(x, u, t=0, params=None)

State feedback; the x input port carries the plant state in u.

setpoint_position(x, params=None)

Cartesian point for the commanded set-point, or None.

tf(x, u, t=0, params=None)

World transforms as dict[str, 4x4] (the forward-kinematics hook).

Return only named articulation frames (body, link0, …). Do not emit "world": I — the animator injects an implicit identity root for geometry keyed to "world".

get_dynamic_geometry(x, u, t=0, params=None)

Per-frame geometry as dict[str, list[primitive]] (rebuilt each frame).

minilink.control.neural.angle_features(angles, scales=None)

Feature map making the listed state angles periodic: cos, sin per angle.

angles are state indices; every other component is passed through, multiplied by scales (a dict {index: gain}, e.g. {1: 0.1} to bring a rate to order one). Works on NumPy and traces under JAX.

Model predictive control as a System block: one trajectory-optimization solve per replan tick.

minilink.control.mpc.controller.ModelPredictiveController(planner, *, dt_mpc, warm_start=True, verbose=False, t0=0.0, debug=False)

Build the product MPC System family block.

Parameters:

• planner (TrajectoryOptimizationPlanner) – Trajopt planner (auto-compiles parametric NLP when needed).

• dt_mpc (float) – Replan period (required for both warm-start modes).

• warm_start (bool, optional) – If True, return a StepSystem with packed z on Computer.x. If False, algebraic System.

• verbose (bool) – Latch printouts, absolute-time epoch, and live debug figure auto-update.

• t0 (float) – Latch printouts, absolute-time epoch, and live debug figure auto-update.

• debug (bool) – Latch printouts, absolute-time epoch, and live debug figure auto-update.

class minilink.control.mpc.controller.Command(solution, k, t_solve, u_ff, x_ff, z, success)

One replan tick result for deploy or debugging.

solution.trajectory.t is plan-local time with zero at the solve instant. Absolute solve time is t_solve.

Parameters:

• solution (PlanningSolution)

• k (int)

• t_solve (float)

• u_ff (ndarray)

• x_ff (ndarray)

• z (ndarray)

• success (bool)

property solver

The optimizer’s record of this tick’s solve (solution.solver).

class minilink.control.mpc.controller.ModelPredictiveControllerMixin(planner, *, dt_mpc, t0=0.0, verbose=False, debug=False)

Shared deploy / export / debug surface for MPC System family blocks.

A concrete block calls this constructor beside its System base’s, then declares its ports with declare_mpc_ports().

Parameters:

• planner (TrajectoryOptimizationPlanner)

• dt_mpc (float)

• t0 (float)

• verbose (bool)

• debug (bool)

planner

The trajopt planner the block solves with.

Type:

TrajectoryOptimizationPlanner

latch

The per-tick solve memo every port (and a stateful block’s step) shares.

Type:

MPCTickLatch

dt_mpc

Replan period (warm-start shift and Computer schedule).

Type:

float

t0

Absolute-time epoch: t_solve = t0 + k * dt_mpc.

Type:

float

debug

Whether every command redraws the live debug figure.

Type:

bool

deploy_k

The deploy counter: the tick of the next compute_command().

Type:

int

last_command

Most recent compute_command() result, if any.

Type:

Command or None

debug_handles

The live debug figure’s handles, once init_debug_figure() built it.

Type:

dict or None

nominal_cache

The latched plan’s interpolator, once generate_nominal_interpolator() built it.

Type:

NominalCache or None

replan_divisor

Base ticks per replan tick (a dual-rate computer sets it).

Type:

int

Hooks a concrete block defines

Type:

compute_u_ff, compute_x_ff and

``compute_z``, the port functions ``(x, u, t, params)``, and

``measurement(u)``, the measured state from the block input. Hooks the

mixin provides, which a block may override

Type:

z_warm_for_command(), the

packed ``z`` that warm-starts the next deploy command (``None`` here);

``replan_k(k_tick)``, the replan index of a Computer base tick; and

``require_nominal_cache()``, the nominal cache, refused before it is built.

A port looks its hook up on the block each time it is evaluated, so a

subclass override is honoured.

compute_command(y, *, params=None, k=None, t=None)

Deploy replan tick: measure y → NLP → Command.

Does not build the nominal interpolator; call generate_nominal_interpolator() explicitly for dual-rate / broadcast.

Parameters:

• y (array_like) – Measured plant state (diagram input y).

• params (mapping, optional) – Forwarded to solve_trajectory_from() via the tick latch (None/{} = x0-only; the key scene raises NotImplementedError).

• k (int, optional) – Tick index. Default: increment internal deploy counter.

• t (float, optional) – Absolute solve time. Default: t0 + k * dt_mpc.

Return type:

Command

get_solver_record()

The last NLP solve’s record for deploy / logging (ROS-agnostic).

Prefers last_command; otherwise the planner’s last_solution (e.g. after a hybrid tick with no deploy call). None if nothing has been solved yet.

reset()

Clear deploy counter, last command, nominal cache, and tick latch.

Return type:

None

generate_nominal_interpolator(*, derivatives=True)

Build the fast nominal cache from the last latched plan (opt-in).

Call after compute_command() (or a hybrid replan tick) when using high-rate get_nominal_u() / dual-rate broadcast. Not invoked by compute_command() itself.

Parameters:

derivatives (bool, optional) – If True, attach FD knot rates for get_nominal_*_dot.

Return type:

NominalCache

get_nominal_u(t)

Linear interp of latched plan u at absolute time t.

Parameters:

t (float)

Return type:

ndarray

get_nominal_x(t)

Linear interp of latched plan x at absolute time t.

Parameters:

t (float)

Return type:

ndarray

get_nominal_u_dot(t)

Interp of precomputed u rates (requires derivatives=True).

Parameters:

t (float)

Return type:

ndarray

get_nominal_x_dot(t)

Interp of precomputed x rates (requires derivatives=True).

Parameters:

t (float)

Return type:

ndarray

export_to_computer(schedule=None)

Build a single-rate Computer (u_ff ZOH).

dual_rate_computer(dt_broadcast)

Advanced multi-rate Computer.

Replan leaf at dt_mpc; broadcast leaf at dt_broadcast applying get_nominal_u(). Requires dt_mpc / dt_broadcast to be a positive integer. Does not change default export_to_computer() / @.

Leaves share this controller’s latch / nominal cache (no port edge between them). Deploy truth remains compute_command() + generate_nominal_interpolator() + get_nominal_u().

Parameters:

dt_broadcast (float)

Return type:

Computer

init_debug_figure(sys, **kwargs)

Create a matplotlib figure for live plan visualization.

Under a non-interactive backend (e.g. Agg), still builds axes so tests can smoke the path; update_debug_figure redraws plan knots.

update_debug_figure(cmd=None)

Redraw debug axes from cmd or last_command.

Parameters:

cmd (Command | None)

class minilink.control.mpc.controller.MPCStatelessController(planner, *, dt_mpc, verbose=False, t0=0.0, debug=False)

Algebraic MPC feedforward block (warm_start=False product sibling).

Owns dt_mpc for Computer schedule / @. Outputs u_ff, x_ff, and z come from one NLP per integer tick k (latch-memoized).

Parameters:

• planner (TrajectoryOptimizationPlanner)

• dt_mpc (float)

• verbose (bool)

• t0 (float)

• debug (bool)

class minilink.control.mpc.controller.MPCStatefulController(planner, *, dt_mpc, verbose=False, t0=0.0, debug=False)

Warm-start MPC block with packed optimizer state on Computer.x.

warm_start=True product sibling. State x is packed z from the previous tick. Ports and step() share one NLP per tick via the latch.

Parameters:

• planner (TrajectoryOptimizationPlanner)

• dt_mpc (float)

• verbose (bool)

• t0 (float)

• debug (bool)

step(x, u, k=0, params=None)

The next packed z: the replan tick’s solve, warm-started from z = x.

class minilink.control.mpc.controller.MPCTickSolve(plan, z, u_ff, x_ff, t_solve, k)

One MPC fire: reconstructed plan and feedforward slices.

Parameters:

• plan (Trajectory)

• z (ndarray)

• u_ff (ndarray)

• x_ff (ndarray)

• t_solve (float)

• k (int)

class minilink.control.mpc.controller.MPCTickLatch(planner, *, verbose=False, dt_mpc=None, t0=0.0)

Memoize one planner solve per integer replan tick k and measurement y.

Port compute paths on MPC blocks call solve_for_tick(); the first call at a new (k, y) runs the NLP via solve_trajectory_from(), later calls with the same k and an equal y read the latch. Inside a Computer tick every port sees the same y, so the block still solves once per tick.

Parameters:

• planner (TrajectoryOptimizationPlanner)

• verbose (bool)

• dt_mpc (float | None)

• t0 (float)

property last_t_solve: float | None

Absolute solve time of the latched tick, if any.

solve_for_tick(k, y, *, z_warm=None, dt_mpc=None, initial_guess=None, params=None)

The tick’s solve: the latched one at the same (k, y), else a new NLP.

An explicit initial_guess always runs a new NLP from that seed, and latches its solve in place of the tick’s earlier one.

Return type:

MPCTickSolve

set_after_solve(callback)

Optional hook after a new NLP latch (e.g. dual-rate interpolator).

Return type:

None

reset_latch()

Clear tick memo (e.g. after Computer.reset).

Return type:

None

minilink.control.mpc.controller.export_mpc_to_computer(block, schedule=None, *, dt_mpc=None)

Build a single-rate Computer from an MPC block.

schedule defaults to dt_mpc (the block’s own when not passed); a given schedule must tick at dt_mpc. Once the schedule is accepted, the export undoes the dual-rate hooks a dual_rate_computer call left on the block; a rejected schedule leaves the block as it was.

Parameters:

• schedule (StepSchedule | float | None)

• dt_mpc (float | None)

Return type:

Computer

minilink.control.mpc.controller.export_mpc_dual_rate_computer(block, *, dt_broadcast)

Multi-rate Computer: replan at block.dt_mpc, broadcast at dt_broadcast.

Boundary ports: y → replan, u_nom from broadcast (primary control).

Coupling between replan and broadcast is a shared latch on block (generate_nominal_interpolator after NLP, then get_nominal_*) — not a diagram port. Graphviz correctly shows no edge.

Parameters:

dt_broadcast (float)

Return type:

Computer

class minilink.control.mpc.controller.MPCBroadcastController(mpc, *, dt_broadcast, t0=0.0)

Sample latched nominals at Computer base ticks (no NLP).

Absolute time is t0 + k * dt_broadcast where k is the Computer tick passed as the port t argument.

Holds a reference to the replan MPC (shared NominalCache) — not a port input. See dual-rate packaging in DESIGN.md (MPC hybrid block).

Parameters:

• dt_broadcast (float)

• t0 (float)

mpc

The replan block whose nominal cache the leaf samples.

Type:

ModelPredictiveControllerMixin

dt_broadcast

Broadcast period: one base tick of the Computer.

Type:

float

t0

Absolute-time epoch of base tick zero.

Type:

float

Hooks a broadcast leaf subclass overrides or calls

Type:

compute_u_nom and

``compute_x_nom``, the port functions ``(x, u, t, params)``, and

``abs_t(k_tick)``, the absolute time of a base tick. A port looks its hook

up on the leaf each time it is evaluated, so a subclass override is

honoured.

minilink.control.mpc.controller.validate_mpc_planner(planner)

Validate a plan-producing planner and return (n, m, n_z).

The contract an MPC planner meets: a duck-typed TrajectoryOptimizationPlanner (or compatible) with a problem, a transcription and options, and the verbs compile_parametric_program, has_parametric_program and solve_trajectory_from. A planner missing any of them is a TypeError.

compile_backend='jax' auto-compiles a parametric NLP once (bind + solve each tick). compile_backend='numpy' or 'direct' skips parametric compile; each tick rebuilds via solve_trajectory_from().

Return type:

tuple[int, int, int]

Receding-horizon bookkeeping: the warm-start guesses and the nominal plan cache of MPC.

minilink.control.mpc.utilities.mpc_warm_start_guess(z_prev, y, planner, *, dt_mpc, k=0)

Build an initial_guess Trajectory for solve_trajectory_from().

At tick k == 0 (or when shift fails), returns the default collocation guess. Otherwise unpacks z_prev, shifts on the transcription grid, and pins the first state node to the current measurement y.

Parameters:

• planner (TrajectoryOptimizationPlanner)

• dt_mpc (float)

• k (int)

Return type:

Trajectory

minilink.control.mpc.utilities.warm_start_guess_from_prev_plan(prev_plan, x_meas, planner, *, dt_shift, horizon, t_anchor)

Warm-start guess from a previous solved plan (manual MPC demo contract).

Mirrors a hand-loop compute_command + RK4 ZOH deploy pattern: shift prev_plan by dt_shift, pin x[:, 0] = x_meas, re-time with t_anchor (simulation time at the MPC fire). Without a previous plan of at least three samples, or when the shifted window is too short, returns the default collocation guess.

Parameters:

• prev_plan (Trajectory | None)

• planner (TrajectoryOptimizationPlanner)

• dt_shift (float)

• horizon (float)

• t_anchor (float)

Return type:

Trajectory

minilink.control.mpc.utilities.mpc_default_computer_x0(planner)

Packed default decision vector for Computer.reset / x0_computer.

Parameters:

planner (TrajectoryOptimizationPlanner)

Return type:

ndarray

class minilink.control.mpc.utilities.NominalCache(t_solve, tau, x, u, x_dot=None, u_dot=None)

Plan-local interpolant data built after an NLP replan.

Parameters:

• t_solve (float)

• tau (ndarray)

• x (ndarray)

• u (ndarray)

• x_dot (ndarray | None)

• u_dot (ndarray | None)

property tau_end: float

the last knot, 0 for an empty plan.

Type:

The plan-local horizon T

minilink.control.mpc.utilities.build_nominal_cache(trajectory, t_solve, *, derivatives=True)

Build a NominalCache from a latched solution’s trajectory.

Parameters:

• trajectory (Trajectory) – The latched solve’s schedule (plan-local t).

• t_solve (float) – Absolute solve time (τ = 0).

• derivatives (bool, optional) – If True, attach FD knot rates x_dot / u_dot.

Return type:

NominalCache

minilink.control.mpc.utilities.eval_signal(cache, t, signal)

Linear interp of rows of signal (n, N) at absolute t.

Parameters:

• cache (NominalCache)

• t (float)

• signal (ndarray)

Return type:

ndarray

MPC visualization: the receding horizons of a hybrid rollout, as animation overlays.

minilink.control.mpc.viz.mpc_animation_overlays(result, planner, *, scene=None, track=None, traj=None, trail=True, reference_pad=None, t0=0.0, dt_mpc=None, scene_color='tab:red', scene_opacity=0.45)

Build animate(overlays=[...]) layers for a hybrid MPC rollout.

Reconstructs horizon polylines from the computer tick history, optionally an executed-path trail, and optionally a straight reference line, obstacle scene skin, or track corridor.

Parameters:

• result (HybridSimResult) – Output of compute_trajectory().

• planner – Trajopt planner (transcription and template problem).

• scene (Scene, optional) – Collision scene drawn via as_visualizer().

• track (Track, optional) – 2-D track corridor edges and centerline.

• traj (Trajectory, optional) – Executed path for the trail. Defaults to result.plant. Pass the leaf-car trajectory when the hybrid plant is a nested diagram, so the trail is pose (x, y) and not the first two diagram states.

• trail (bool, optional) – Draw the executed-path polyline. False keeps the receding horizon and any track/scene layers without the growing trail.

• reference_pad (float, optional) – When set, draw a dashed world y=0 reference spanning the executed path in x by this padding on each end.

• t0 (float, optional) – Simulation start time for horizon alignment.

• dt_mpc (float, optional) – MPC sample period. Defaults to result.hybrid.computer.schedule.dt_base.

• scene_color (str) – Passed to as_visualizer().

• scene_opacity (float) – Passed to as_visualizer().

Returns:

Overlay drawables ready for animate().

Return type:

list

minilink.control.mpc.viz.mpc_plans_from_rollout(computer, transcription, problem, *, z_source='signals', signal_name='z', t0=0.0, dt_mpc)

Build absolute-time MPC plans from per-tick packed decision vectors.

Parameters:

• computer (StepRollout) – Tick-indexed computer rollout from HybridSimulator.

• transcription (DirectCollocationTranscription) – Collocation transcription used by the MPC block.

• problem (PlanningProblem) – Template planning problem (system / cost layout).

• z_source (str, optional) – "signals" reads computer.signals[signal_name]; "x" reads computer.x (warm-start step block state).

• signal_name (str, optional) – Signal key when z_source="signals".

• t0 (float, optional) – Simulation start time.

• dt_mpc (float) – MPC sample period (Computer.schedule.dt_base).

Returns:

(t_solve, plan) pairs suitable for HorizonPolyline.

Return type:

list of (float, Trajectory)