Repository navigation
Add trajectory generators and quadrotor plant/controller support - #29
Merged
Merged
Conversation
…mixer Implement the Quadrotor plant (issue #15): standalone analytical dynamics with four rotor-speed inputs and an optional configurable rotor placement table. - State [x, y, z, roll, pitch, yaw, vx, vy, vz, p, q, r]; control is the four rotor angular speeds. Translational dynamics apply body thrust through R = Rz Ry Rx; rotational dynamics are Euler's rigid-body equations (gyroscopic term included) with Euler kinematics eta_dot = T(phi, theta) w. - Batch-capable, backend-routed dynamics (column idiom) so the same function backs step, finite-difference get_model, and MPPI lowering; R/T are expanded into closed-form components (the strictly-2D graph cannot hold (N,3,3)). - Rotor placement is a configurable mixer: RotorConfig (x, y, spin) and an optional [[rotors]] table. The default "+" layout reduces exactly to the previous closed-form torque mixing. - get_model defaults u0 to the hover rotor speed sqrt(m g / 4k) (zero u0 gives B = 0 because rotor forces are quadratic); optional state_bounds; from_config restores the frozen Config and abstract methods dropped in ba22350, which had left `import shinro` failing. - MuJoCo engine mode is still TODO: step raises while an engine is attached. Tests: TestQuadrotor in tests/test_plants.py and Quadrotor added to TestBatchCapableDynamics. samples/plants/quadrotor.toml and docs updated.
- TestQuadrotorControllers (tests/test_controllers.py): pair the Quadrotor with LQR, MPC_LTI / MPC_LTI_DeltaU (via get_model), MPPI (attach_plant) and SMC (f_x + linearized g_x), plus PID as four cascaded per-channel loops. LQR is verified to place every closed-loop mode inside the unit circle; the rest are wiring/shape tests. - demos/demo_quadrotor_smc.py: a multi-surface sliding-mode closed loop that stabilizes altitude + roll/pitch/yaw. One surface per input-matched DOF makes C g a square decoupling matrix, so the rotor commands are fully determined (no min-norm). Contrast sections show single-surface SMC drifting away and det(C g) collapsing to 0 at stopped rotors / gimbal lock. - Also tidies pre-existing type-check warnings in test_controllers.py.
Replace the demo's bespoke multi-surface law with four instances of the framework SlidingModeController, one per controlled DOF (altitude, roll, pitch, yaw), matching the documented multi-axis pattern (the built-in SMC is single-surface by design). Each instance is fed its two-state channel (pos, rate) with the plant's gravity / gyroscopic / Euler-kinematics terms embedded in a reduced f and g, so its scalar law returns the channel's virtual control (F, tau_x, tau_y, tau_z). Those map back to rotor speeds through the plant's constant mixer (built from plant.rotors, so a configured [[rotors]] layout works). M^-1 is precomputed once, so per tick the only linear algebra is four scalar divisions inside the controllers — no matrix inverse, no new ops, no framework change. The closed loop is unchanged (z, roll, pitch, yaw -> ~1e-4, thrust == weight). The contrast (one 12-state surface drifts away) and the built-in c^T g guard at gimbal lock are kept.
Port the standalone draft into a registered, config-driven component.
The curve is built per-axis in a local frame and rotated back into the
world frame with an optional rotation matrix R (kept so a robotics user
can orient the figure), which is what makes pos(0)=start / pos(T)=end
hold for any rotation; odd harmonics keep both boundary velocities zero.
- backend-agnostic via self.bk (no np. in component code)
- frozen LissajousConfig + @register_trajectory("lissajous")
- from_config samples position_at into a (steps, N) waypoint schedule
- loud ValueError on len(k) != N or a non-orthogonal R
- N-dimensional (k has one entry per axis, R is NxN)
- tests (numpy + torch), sample config, docs row, lab note
Control-point-in generator: degree = len(control_points) - 1, spatial
dimension inferred from the point length, endpoints interpolated exactly.
- evaluates the Bernstein form through one shared helper; velocity and
acceleration are lower-degree curves over the scaled first/second
control-point differences (math.comb coefficients)
- backend-agnostic via self.bk (no np. in component code)
- frozen BezierConfig + @register_trajectory("bezier")
- from_config samples position_at into a (steps, d) waypoint schedule
- loud ValueError on <2 points, ragged dimensions, non-positive duration,
or a declared start/end that disagrees with the control-list ends
- two-point (degree-1) curves return exact-zero acceleration; duplicate
the end points to pin boundary velocity to zero
- tests (numpy + torch), sample config, docs row, lab note
Completes the BSpline draft: `_bspline_derv` computes the k-th derivative
curve by degree reduction (Q_i = p(P_{i+1}-P_i)/(u_{i+p+1}-u_{i+1}), one
knot chopped per side), so derivatives evaluate through the same Cox–de Boor
basis recursion. `generate` validates the knot/control relation and
precomputes the velocity/acceleration curves; `position_at` returns
(pos, vel, acc) like BezierCurve. Adds the frozen BSplineConfig + from_config
required by the strict registry decorator, threads the reduced knot vector
through `_bspline_basis`, and exports BSpline from trajectories/__init__.
Verified: clamped cubic == cubic Bezier to 4.4e-16; randomized 300-curve
sweep vs scipy.interpolate.BSpline (pos + derivative 1/2) to 3.6e-12;
`make lint` clean.
TestBSpline (19 cases x numpy/torch): clamped cubic vs cubic Bezier on pos/vel/acc, clamped endpoints and boundary velocities, finite-difference derivative consistency, partition of unity, degree-0/1 behaviour, duplicate-endpoint zero boundary velocity, uniform-knot derivative consistency (guards the reduced-knot threading), time clamping, validation errors, and from_config. TestCreateBspline loads the shipped sample; samples/trajectories/bspline.toml documents the knot-count rule and that the knot vector is the time parametrization (domain must span [0, duration]); docs/components.md gains the bspline row. Widened BSpline.__init__'s control_points/time_knot_vector annotations to collections.abc.Sequence (read-only inputs, so tuple constants typecheck). Verified: pytest test_trajectories+test_factories 174 passed; make test 1597 passed, 5 skipped, 96 deselected; make lint clean.
The framework had no smooth waypoint pass-through: `waypoints` holds each
position and `cubic_segments`/`quintic_segments` generate each segment
independently with zero boundary velocity, so the registered behaviour is
stop-and-go at every waypoint. `CatmullRom` derives a C1 tangent at each
interior waypoint (v_i = (P_{i+1} - P_{i-1}) / (d_{i-1} + d_i)) and builds one
CubicPolynomial per hop, so it passes through the waypoints without stopping
(closed form, no linear solve). Endpoint tangents default to zero
(rest-to-rest); endpoint_tangent="one_sided" gives the classic Catmull-Rom
ends. Config is `start` + `[[waypoints]]{duration, position}`, registered as
`catmull_rom`.
Verified: pytest test_trajectories+test_factories 204 passed; make test 1627
passed, 5 skipped, 96 deselected; make lint clean.
…pt-in) from_config discarded the velocity/acceleration the generators already compute, so a tracking controller had only a position reference. Adds shinro.trajectories.sample_schedule / sample_segments (shared helpers that return stacked (steps, N) position/velocity/acceleration arrays) and a `derivatives = true` opt-in on the six curve/segment Configs (bezier, bspline, catmull_rom, lissajous, cubic_segments, quintic_segments): from_config then returns the dict instead of the position schedule. The default output is unchanged, so the closed-loop runner and TrajectoryFactory are untouched (no controller consumes a reference derivative yet — that is controller-side work). The helpers also remove the duplicated sampling loop from every from_config; those polymorphic from_config returns are annotated -> Any. Dropped the last np. usage in quintic_polynomial (int(np.round(...)) -> round). Verified: pytest test_trajectories+test_factories 246 passed; make test 1669 passed, 5 skipped, 96 deselected; make lint clean.
…ctories
Adds the two missing waypoint-interpolation behaviours: Akima (C1,
overshoot-resistant, local) and the natural cubic spline (C2, global, zero
boundary acceleration). Both are cubic Hermite curves reusing
CubicPolynomial per hop — only the knot-derivative rule differs:
- akima: Akima's curvature-weighted average of the adjacent secants,
d_i = (|s_{i+1}-s_i| s_{i-1} + |s_{i-1}-s_{i-2}| s_i) / (|s_{i+1}-s_i| +
|s_{i-1}-s_{i-2}|), with linearly extrapolated end slopes. No solve.
- cubic_spline: interior second derivatives from a tridiagonal bk.solve
(M_0 = M_n = 0), Hermite velocities d_i = s_i - h_i(2M_i+M_{i+1})/6, which
reproduce the spline segment exactly.
A shared _WaypointSpline base holds the validation / segment-building /
position_at / from_config plumbing; subclasses implement _tangents only.
Config shape mirrors catmull_rom (start + [[waypoints]]{duration, position}),
plus the derivatives opt-in. CatmullRom was left as-is (not folded onto the
base) to keep the diff additive.
Verified: on a flat-then-step dataset Akima stays in [0, 1] while the natural
spline overshoots to [-0.109, 1.109]; pytest test_trajectories+test_factories
278 passed; make test 1701 passed, 5 skipped, 96 deselected; make lint clean.
Two motion primitives with no equivalent in the package: - min_snap: MinSnapPolynomial, 7th order (8 coefficients) with position, velocity, acceleration, and jerk pinned at both ends via one 8x8 bk.solve. Zero boundaries give the rest-to-rest minimum snap p0 + dp(35 s^4 - 84 s^5 + 70 s^6 - 20 s^7), the 7th-order sibling of the min-jerk quintic. Exposed as a single config (start/end/duration plus optional boundary v/a/j); jerk is input-only since position_at stays a 3-tuple. - circular_arc: CircularArc, analytic circle/helix p = c + r cos(theta) + (n x r) sin(theta) + n (pitch/2pi) theta with theta = omega t - constant speed omega r and centripetal acceleration. center/start/sweep_angle/duration + optional normal/pitch; start - center must be perpendicular to normal. Both inherit the derivatives opt-in via sample_schedule. Verified: min-snap matches the closed form to 3.9e-14; arc radius, speed and centripetal acceleration exact; pytest test_trajectories+test_factories 312 passed; make test 1735 passed, 5 skipped, 96 deselected; make lint clean.
Store the arc radius R and unit in-plane basis vectors u, v explicitly and evaluate p = center + R(u cos θ + v sin θ) + axial, instead of the implicit R-scaled r_vec / (n_hat × r_vec). Same math (verified identical output), clearer to read. Docstring now writes R, u, v explicitly. Verified: pytest -k CircularArc 16 passed; make lint clean.
…ng samples
Both QuinticPolynomial and QuinticPolynomialConfigAdapter carried
@register_trajectory("quintic_segments"), so the adapter silently shadowed the
class. Drop the decorator from QuinticPolynomial (the adapter is the registered
entry point and owns the segment-list from_config) with a comment; behaviour is
unchanged.
Add the three sample TOMLs the Trajectories docs referenced but that never
existed: samples/trajectories/arm_quintic.toml (quintic_segments), arm_lift.toml
and base_triangle.toml (waypoints). All 13 sample refs in that section now
resolve.
Verified: registry resolves quintic_segments -> QuinticPolynomialConfigAdapter;
the restored samples load via TrajectoryFactory; make test 1735 passed, 5
skipped, 96 deselected; make lint clean.
New src/shinro/trajectories/time_scaling.py (renamed from an uncommitted limits.py — "time scaling" is the standard term) re-times a generator so it respects per-component motion limits, same geometry, different clock: - uniform (constant): limit_trajectory / time_scale_factor / TimeScaled traverse the path k times slower (velocity / k, acceleration / k^2). - min-jerk (S-curve): s_curve_limit / s_curve_horizon / SCurveScaled re-time the path parameter with the rest-to-rest min-jerk profile sigma(s) = 10 s^3 - 15 s^4 + 6 s^5, choosing the horizon so velocity, acceleration, and jerk fit. Both are drop-ins for sample_schedule. Jerk is finite-differenced (generators expose no p'''). Wired programmatically: the registry's from_config returns sampled arrays, not generator objects, so there is no TOML-level wrapper. Known limitation (to be reworked): the S-curve re-times the trajectory's native parameter, not arc length, so it over-slows straight-line rest-to-rest moves (e.g. cubic 6.25 -> 9.375 s) instead of matching the min-jerk horizon formula. Verified: pytest test_trajectories+test_factories 344 passed (312 before time scaling); make test 1767 passed, 5 skipped, 96 deselected; make lint clean.
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
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
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.
Branch
trajectory-work(renamed fromfeat/component-additions).Trajectory generators
New registered trajectories (all backend-agnostic — numpy + torch):
bspline— Cox–de Boor B-spline with an analytic k-th derivative (degree reduction)catmull_rom— C¹ cubic Hermite through an ordered waypoint listakima— overshoot-resistant C¹ waypoint splinecubic_spline— natural C² waypoint splinemin_snap— 7th-order point-to-point (the 7th-order sibling of the min-jerk quintic)circular_arc— analytic circular arc / helixlissajous,bezier— previously drafted, now completed as framework componentsPlus a reference-derivative opt-in:
derivatives = trueon the curve/segment Configs makesfrom_configreturn a{position, velocity, acceleration}dict of(steps, N)arrays, built by the sharedshinro.trajectories.sample_schedule/sample_segmentshelpers. A velocity-tracking controller that consumes it is deliberately follow-up work (no controller reads a reference derivative today).Quadrotor
12-state dynamics with a configurable rotor mixer, controller integration tests (LQR / MPC / MPPI / SMC), and a demo that stacks a bank of built-in SMCs.
Verification
make test: 1735 passed, 5 skipped, 96 deselectedmake lint(ruff + pyrefly): cleanscipy.interpolate.BSplineto 3.6e-12 over 300 random curves); min-snap matches its closed form to 3.9e-14; arc radius / speed / centripetal acceleration exact.Follow-ups (not in this PR)
CatmullRomonto the shared_WaypointSplinebase(pos, vel, acc, jerk)schedule output