From 879071d8adaa01bb673f846d6ceb00ac33498fd4 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 13 Aug 2026 13:41:32 -0700 Subject: [PATCH] document installation, the projectors and the explorer The README gained three installation paths that did not exist before - wheel via uv/pip, ROS2 via colcon, and standalone CMake - plus how to run the explorer and how to run each test suite, and worked Python and C++ examples for the projectors covering footprints, the axle reference and the ramp semantics. The derivations pick up the forward-projection integration for each model: the steering and articulation ramps, the gamma-dot term now that it is a real input rather than a hardcoded zero, and how the axle-reference conversion is applied without changing where motion is integrated. Co-Authored-By: Claude Opus 5 (1M context) --- README.md | 406 ++++++++++++++++++++++++++---- derivations/articulated_model.md | 70 +++++- derivations/bicycle_model.md | 53 ++++ derivations/differential_drive.md | 43 ++++ 4 files changed, 519 insertions(+), 53 deletions(-) diff --git a/README.md b/README.md index baeb07a..b9291e9 100644 --- a/README.md +++ b/README.md @@ -1,79 +1,140 @@ # polymath_kinematics -Kinematic models for differential-drive, bicycle (Ackermann), and articulated vehicles. +Kinematic models and forward-projection helpers for differential-drive, bicycle (Ackermann), and articulated vehicles. C++ library with Python bindings via pybind11, packaged as a single Python wheel built by [scikit-build-core]. -## Derivations +Includes an interactive **Kinematic Explorer** (Streamlit) for visualising trajectory lattices, kinematic relationships, and vehicle footprints. -Math and equations behind each model: +[scikit-build-core]: https://scikit-build-core.readthedocs.io/ -- [Differential drive](derivations/differential_drive.md) — forward/inverse kinematics for two independently driven wheels -- [Bicycle model (Ackermann)](derivations/bicycle_model.md) — steering-angle kinematics with four-wheel ICR geometry -- [Articulated model](derivations/articulated_model.md) — pivot-joint kinematics for front/rear section vehicles +## Installation -## C++ usage +**Prerequisites:** Python 3.10+, a C++17 compiler, CMake 3.15+, and [uv](https://github.com/astral-sh/uv) for the wheel path. -```cmake -find_package(polymath_kinematics REQUIRED) -target_link_libraries(my_target PRIVATE polymath_kinematics::polymath_kinematics) -``` +The package supports three build paths. They share one `CMakeLists.txt`; `find_package(ament_cmake QUIET)` picks between the ROS2 and standalone branches at configure time. -```cpp -#include -#include -#include +### 1. Python wheel (uv / pip) + +For Python-only consumers and the Streamlit explorer. Builds a static `polymath_kinematics` library and links it into the `polymath_kinematics_cpp` pybind11 module; ships the result as a single wheel. + +```bash +cd src/polymath_kinematics + +# Bindings only +uv pip install -e . + +# Bindings + Kinematic Explorer (streamlit, matplotlib, pandas) +uv pip install -e ".[explorer]" + +# Bindings + Explorer + pytest +uv pip install -e ".[dev]" + +# Or, without activating a venv first: +uv run python -c "import polymath_kinematics; print(polymath_kinematics.BicycleModel(2.5, 1.5, 0.3))" ``` -### DifferentialDriveModel +The wheel build sets `-DCMAKE_DISABLE_FIND_PACKAGE_ament_cmake=ON` via `pyproject.toml`, so a host-installed `/opt/ros/humble` does not pull the build down the ROS2 branch. -```cpp -polymath::kinematics::DifferentialDriveModel model(0.15, 0.5); // wheel_radius_m, track_width_m +### 2. ROS2 / colcon -auto wheels = model.bodyVelocityToWheelVelocities(1.0, 0.3); // linear m/s, angular rad/s -// wheels.left_wheel_velocity_rad_s, wheels.right_wheel_velocity_rad_s +For consumers inside a ROS2 workspace. Produces a shared `libpolymath_kinematics.so` exported through ament (`polymath_kinematics::polymath_kinematics`) plus the pybind11 module installed alongside the Python package. + +```bash +# From the colcon workspace root +colcon build --packages-select polymath_kinematics -auto body = model.wheelVelocitiesToBodyVelocity(6.0, 7.0); // left rad/s, right rad/s -// body.linear_velocity_m_s, body.angular_velocity_rad_s +# Build + run C++ Catch2 tests and Python pytest suites +colcon build --packages-select polymath_kinematics --cmake-args -DBUILD_TESTING=ON +colcon test --packages-select polymath_kinematics +colcon test-result --verbose ``` -### BicycleModel +### 3. Standalone CMake -```cpp -polymath::kinematics::BicycleModel model(2.7, 1.6, 0.35); // wheelbase_m, track_width_m, wheel_radius_m +For working on the C++ code or running the Catch2 tests outside of ROS2 and outside of the wheel build (e.g. local IDE / ctest workflows). -auto state = model.bodyVelocityToSteering(2.0, 0.2); // linear m/s, angular rad/s -// state.steering_angle_rad, state.turning_radius_m -// state.front_right_wheel_rad_s, state.front_left_wheel_rad_s -// state.rear_right_wheel_rad_s, state.rear_left_wheel_rad_s +```bash +cd src/polymath_kinematics +cmake -S . -B build -DBUILD_TESTING=ON +cmake --build build -j +ctest --test-dir build --output-on-failure +``` -auto body = model.steeringToBodyVelocity(2.0, 0.15); // velocity m/s, steering_angle rad -// body.linear_velocity_m_s, body.angular_velocity_rad_s +If `/opt/ros/humble` (or any other ament-providing install) is on the host, force the standalone path with: -double radius = model.turningRadius(0.15); // steering_angle rad -> meters -double angle = model.steeringAngleFromRadius(10.0); // radius m -> radians +```bash +cmake -S . -B build -DBUILD_TESTING=ON -DCMAKE_DISABLE_FIND_PACKAGE_ament_cmake=ON ``` -### ArticulatedModel +## Running the Kinematic Explorer -```cpp -polymath::kinematics::ArticulatedModel model( - 1.8, 1.5, // articulation_to_front_axle_m, articulation_to_rear_axle_m - 2.0, 2.0, // front_track_width_m, rear_track_width_m - 0.6, 0.6); // front_wheel_radius_m, rear_wheel_radius_m +After `uv pip install -e ".[explorer]"`: -auto state = model.bodyVelocityToVehicleState(1.5, 0.1); // linear m/s, angular rad/s -// state.articulation_angle_rad -// state.front_right_wheel_speed_rad_s, state.front_left_wheel_speed_rad_s -// state.rear_right_wheel_speed_rad_s, state.rear_left_wheel_speed_rad_s -// state.front_axle_turning_radius_m, state.rear_axle_turning_radius_m +```bash +# Console script (recommended). Extra arguments are forwarded to streamlit. +kinematic-explorer +kinematic-explorer --server.port=8600 -auto axles = model.articulationToAxleVelocities(1.5, 0.3); // linear m/s, articulation_angle rad -// axles.front_axle_turning_velocity_rad_s, axles.rear_axle_turning_velocity_rad_s +# Or invoke streamlit directly +uv run python -m streamlit run polymath_kinematics/kinematic_explorer_app.py ``` +The app opens at `http://localhost:8501` and exposes: + +- Model selection (Differential Drive / Bicycle / Articulated) +- Geometry sliders (wheelbase, track width, wheel radii, body overhangs) and a front/rear axle reference selector +- Trajectory lattice visualisation across steering / articulation angles +- Kinematic analysis plots (angle → angular velocity, turning radius vs angle) +- Vehicle-footprint overlays along trajectories, drawn from the projector-computed polygons +- Single projected trajectory with initial → target ramp controls +- CSV / JSON / PNG / SVG / PDF download + +Every trajectory the explorer draws comes from the C++ projectors, so it exercises the same +forward-simulation code as production. Integration is Euler at the configured time step. + +The console script binds to `localhost` by default; pass `--server.address=0.0.0.0` to serve a +demo over the network. + +## Running tests + +```bash +# Python only (from the wheel install) +uv run pytest + +# C++ Catch2 + Python pytest under ROS2 +colcon build --packages-select polymath_kinematics --cmake-args -DBUILD_TESTING=ON +colcon test --packages-select polymath_kinematics +colcon test-result --verbose + +# C++ Catch2 standalone (no ROS2) +cmake -S . -B build -DBUILD_TESTING=ON -DCMAKE_DISABLE_FIND_PACKAGE_ament_cmake=ON +cmake --build build -j +ctest --test-dir build --output-on-failure +``` + +## Derivations + +Math and equations behind each model: + +- [Differential drive](derivations/differential_drive.md) — forward/inverse kinematics for two independently driven wheels +- [Bicycle model (Ackermann)](derivations/bicycle_model.md) — steering-angle kinematics with four-wheel ICR geometry +- [Articulated model](derivations/articulated_model.md) — pivot-joint kinematics for front/rear section vehicles + ## Python usage ```python -from polymath_kinematics import DifferentialDriveModel, BicycleModel, ArticulatedModel +from polymath_kinematics import ( + DifferentialDriveModel, + BicycleModel, + ArticulatedModel, + DifferentialDriveProjector, + BicycleProjector, + ArticulatedProjector, + AxleReference, + Pose2D, + Point2D, + rectangle_footprint, + transform_footprint, +) ``` ### DifferentialDriveModel @@ -115,17 +176,250 @@ model = ArticulatedModel( rear_wheel_radius_m=0.6, ) +# Steady articulation (gamma-dot = 0) state = model.body_velocity_to_vehicle_state( linear_velocity_m_s=1.5, angular_velocity_rad_s=0.1 ) print(state.articulation_angle_rad, state.front_axle_turning_radius_m) +# With explicit articulation rate (gamma-dot, rad/s) +state_with_rate = model.body_velocity_to_vehicle_state( + linear_velocity_m_s=1.5, + angular_velocity_rad_s=0.1, + articulation_turning_velocity_rad_s=0.2, +) + axles = model.articulation_to_axle_velocities( - linear_velocity_m_s=1.5, articulation_angle_rad=0.3 + linear_velocity_m_s=1.5, + articulation_angle_rad=0.3, + articulation_turning_velocity_rad_s=0.2, # optional, defaults to 0 ) print(axles.front_axle_turning_velocity_rad_s, axles.rear_axle_turning_velocity_rad_s) ``` +### Projectors — forward simulation with actuator limits + +Each projector wraps its model and integrates pose forward in time under realistic actuator +constraints. For `BicycleProjector` and `ArticulatedProjector` the steering / articulation angle +ramps toward a target at a bounded rate, clamped to `[min, max]`. `DifferentialDriveProjector` +has no steered joint, so it instead ramps the body command `(v, ω)` under separate linear and +angular acceleration limits. + +Every projector optionally computes a per-sample vehicle footprint. Footprints are **arbitrary +polygons** given in the body frame, and they live on the projector rather than the model — the +kinematic models stay dimension-free. A polygon is counter-clockwise and not closed; an empty +polygon (the default) means "unset", so the footprint comes back empty and projection proceeds +normally. `rectangle_footprint(front_m, rear_m, width_m)` builds the boxy common case, and +`transform_footprint(polygon, pose)` maps a body-frame polygon into another frame if you need to +transform one yourself. + +For the bicycle and articulated models, poses and the body-frame footprint are both measured from +an axle you choose with `AxleReference.FRONT` or `AxleReference.REAR`. A differential drive has one +axle, so there is nothing to select. + +| Projector | Pose reference | Footprint | +|---|---|---| +| `DifferentialDriveProjector` | Body centre | One polygon in the body-centre frame | +| `BicycleProjector` | Selected axle (`AxleReference`) | One polygon in the selected axle's frame | +| `ArticulatedProjector` | Selected axle (`AxleReference`); θ is that axle's body heading | One polygon per body, **each in its own axle's frame** — the only anchoring that stays rigid as the joint articulates | + +`ArticulatedProjector` also reports `joint_pose` on every sample, so the articulation joint +(`base_link` for a ROS articulated vehicle) is available regardless of which axle you reference. +See the [derivations](#derivations) for the per-model geometry and integration details. + +```python +projector = BicycleProjector( + model=BicycleModel(wheelbase_m=2.7, track_width_m=1.6, wheel_radius_m=0.35), + min_steering_angle_rad=-0.6, + max_steering_angle_rad=0.6, + axle_reference=AxleReference.REAR, + # Measured from the rear axle: front bumper 0.9 m past the 2.7 m front axle, 0.8 m of tail. + footprint=rectangle_footprint(2.7 + 0.9, 0.8, 1.8), +) + +# Or describe the same body from the front axle — the polygon moves with the reference: +front_referenced = BicycleProjector( + model=BicycleModel(wheelbase_m=2.7, track_width_m=1.6, wheel_radius_m=0.35), + min_steering_angle_rad=-0.6, + max_steering_angle_rad=0.6, + axle_reference=AxleReference.FRONT, + footprint=rectangle_footprint(0.9, 2.7 + 0.8, 1.8), +) + +# Any polygon works, not just rectangles — a tapered nose, for instance: +tapered = [Point2D(-0.8, -0.9), Point2D(3.2, -0.9), Point2D(3.6, 0.0), Point2D(3.2, 0.9), Point2D(-0.8, 0.9)] + +# One-step advance +result = projector.step( + dt_s=0.1, + current_pose=Pose2D(x=0.0, y=0.0, theta=0.0), + current_steering_angle_rad=0.0, + target_steering_angle_rad=0.3, + steering_rate_rad_s=0.5, + linear_velocity_m_s=1.0, +) +print(result.pose.x, result.steering_angle_rad) +print([(p.x, p.y) for p in result.footprint]) # world-frame body polygon; empty if no footprint was given + +# Full trajectory +trajectory = projector.project( + horizon_s=5.0, + dt_s=0.05, + initial_pose=Pose2D(), + initial_steering_angle_rad=0.0, + target_steering_angle_rad=0.4, + steering_rate_rad_s=0.3, + linear_velocity_m_s=1.5, +) +# trajectory[0] is the initial state; trajectory[-1] is the end-of-horizon state. +``` + +`ArticulatedProjector` has the same shape — the angle is the articulation angle γ and the rate is γ̇: + +```python +projector = ArticulatedProjector( + model=ArticulatedModel(1.66, 1.44, 2.0, 2.0, 0.723, 0.723), + min_articulation_angle_rad=-0.785, + max_articulation_angle_rad=0.785, + axle_reference=AxleReference.REAR, + # Front body about the FRONT axle: 1.0 m of bucket ahead, back to the joint 1.66 m behind. + front_footprint=rectangle_footprint(1.0, 1.66, 2.0), + # Rear body about the REAR axle: forward to the joint 1.44 m ahead, 0.8 m of counterweight. + rear_footprint=rectangle_footprint(1.44, 0.8, 2.0), +) +trajectory = projector.project( + horizon_s=5.0, dt_s=0.05, + initial_pose=Pose2D(), + initial_articulation_angle_rad=0.0, + target_articulation_angle_rad=0.5, + articulation_rate_rad_s=0.2, + linear_velocity_m_s=1.0, +) +``` + +`DifferentialDriveProjector` ramps the body command instead of an angle: + +```python +projector = DifferentialDriveProjector( + model=DifferentialDriveModel(wheel_radius_m=0.15, track_width_m=0.5), + min_linear_velocity_m_s=-2.0, + max_linear_velocity_m_s=2.0, + min_angular_velocity_rad_s=-1.5, + max_angular_velocity_rad_s=1.5, +) +trajectory = projector.project( + horizon_s=5.0, dt_s=0.05, + initial_pose=Pose2D(), + initial_linear_velocity_m_s=0.0, + initial_angular_velocity_rad_s=0.0, + target_linear_velocity_m_s=1.0, + target_angular_velocity_rad_s=0.5, + linear_acceleration_m_s2=1.0, + angular_acceleration_rad_s2=1.0, +) +# Setting initial == target makes the ramp a no-op, giving a constant-command trajectory. +``` + +## C++ usage + +The C++ headers and library are installed under the package's CMake export when built via colcon: + +```cmake +find_package(polymath_kinematics REQUIRED) +target_link_libraries(my_target PRIVATE polymath_kinematics::polymath_kinematics) +``` + +```cpp +#include +#include +#include +#include +#include +#include +#include +``` + +### DifferentialDriveModel + +```cpp +polymath::kinematics::DifferentialDriveModel model(0.15, 0.5); // wheel_radius_m, track_width_m + +auto wheels = model.bodyVelocityToWheelVelocities(1.0, 0.3); // linear m/s, angular rad/s +auto body = model.wheelVelocitiesToBodyVelocity(6.0, 7.0); // left rad/s, right rad/s +``` + +### BicycleModel + +```cpp +polymath::kinematics::BicycleModel model(2.7, 1.6, 0.35); // wheelbase, track, wheel_radius (m) + +auto state = model.bodyVelocityToSteering(2.0, 0.2); // linear m/s, angular rad/s +auto body = model.steeringToBodyVelocity(2.0, 0.15); // velocity m/s, steering rad + +double radius = model.turningRadius(0.15); // steering rad → meters +double angle = model.steeringAngleFromRadius(10.0); // radius m → radians +``` + +### ArticulatedModel + +```cpp +polymath::kinematics::ArticulatedModel model( + 1.8, 1.5, // articulation_to_front_axle_m, articulation_to_rear_axle_m + 2.0, 2.0, // front_track_width_m, rear_track_width_m + 0.6, 0.6); // front_wheel_radius_m, rear_wheel_radius_m + +// 2-arg form: assumes a zero articulation turning velocity +auto state = model.bodyVelocityToVehicleState(1.5, 0.1); + +// 3-arg form: feeds the actual gamma-dot into the kinematics +auto state_with_rate = model.bodyVelocityToVehicleState(1.5, 0.1, 0.2); + +auto axles = model.articulationToAxleVelocities(1.5, 0.3); // gamma-dot = 0 +auto axles_with_rate = model.articulationToAxleVelocities(1.5, 0.3, 0.2); +``` + +### Projectors + +```cpp +polymath::kinematics::BicycleProjector projector( + polymath::kinematics::BicycleModel(2.7, 1.6, 0.35), + -0.6, 0.6, // min/max steering angle (rad) + polymath::kinematics::AxleReference::REAR, // poses + footprint about the rear axle + polymath::kinematics::rectangleFootprint(3.6, 0.8, 1.8)); // front_m, rear_m, width_m + +polymath::kinematics::Pose2D pose{0.0, 0.0, 0.0}; + +auto step = projector.step( + /*dt_s=*/0.1, pose, + /*current_steering=*/0.0, + /*target_steering=*/0.3, + /*steering_rate=*/0.5, + /*linear_velocity=*/1.0); + +auto trajectory = projector.project( + /*horizon_s=*/5.0, /*dt_s=*/0.05, pose, + /*initial_steering=*/0.0, + /*target_steering=*/0.4, + /*steering_rate=*/0.3, + /*linear_velocity=*/1.5); +// trajectory.front() is the initial state, trajectory.back() is the end of the horizon. +``` + +`ArticulatedProjector` is shaped identically — the angle is the articulation angle (γ) and the rate is γ̇. `DifferentialDriveProjector` takes a body command and acceleration limits instead: + +```cpp +polymath::kinematics::DifferentialDriveProjector diff_projector( + polymath::kinematics::DifferentialDriveModel(0.15, 0.5), + -2.0, 2.0, // min/max linear velocity (m/s) + -1.5, 1.5); // min/max angular velocity (rad/s) + +auto diff_trajectory = diff_projector.project( + /*horizon_s=*/5.0, /*dt_s=*/0.05, pose, + /*initial_v=*/0.0, /*initial_omega=*/0.0, + /*target_v=*/1.0, /*target_omega=*/0.5, + /*linear_accel=*/1.0, /*angular_accel=*/1.0); +``` + ## Models reference ### DifferentialDriveModel @@ -168,5 +462,17 @@ print(axles.front_axle_turning_velocity_rad_s, axles.rear_axle_turning_velocity_ | Method (C++ / Python) | Parameters | Returns | |---|---|---| -| `bodyVelocityToVehicleState` / `body_velocity_to_vehicle_state` | linear vel (m/s), angular vel (rad/s) | `ArticulatedVehicleState` | -| `articulationToAxleVelocities` / `articulation_to_axle_velocities` | linear vel (m/s), articulation angle (rad) | `ArticulatedAxleVelocities` | +| `bodyVelocityToVehicleState` / `body_velocity_to_vehicle_state` | linear vel (m/s), angular vel (rad/s), [articulation rate (rad/s) = 0] | `ArticulatedVehicleState` | +| `articulationToAxleVelocities` / `articulation_to_axle_velocities` | linear vel (m/s), articulation angle (rad), [articulation rate (rad/s) = 0] | `ArticulatedAxleVelocities` | + +### Projectors + +| Class | Constructor | Notable methods | +|---|---|---| +| `BicycleProjector` | `(BicycleModel, min_steering, max_steering, [axle_reference, footprint])` | `step(dt, pose, current, target, rate, v)`, `project(horizon, dt, pose, initial, target, rate, v)` | +| `ArticulatedProjector` | `(ArticulatedModel, min_articulation, max_articulation, [axle_reference, front_footprint, rear_footprint])` | `step(dt, pose, current, target, rate, v)`, `project(horizon, dt, pose, initial, target, rate, v)` | +| `DifferentialDriveProjector` | `(DifferentialDriveModel, min_v, max_v, min_omega, max_omega, [footprint])` | `step(dt, pose, current_v, current_omega, target_v, target_omega, accel, angular_accel)`, `project(horizon, dt, pose, initial_v, initial_omega, target_v, target_omega, accel, angular_accel)` | + +All three clamp the target to its `[min, max]` bounds before ramping, then advance toward the clamped target at the given rate or acceleration (never overshooting), and integrate pose with Euler. Each step advances position using the heading `θ` at the *start* of the step, while the angular rate `ω` comes from the *post-ramp* steering/articulation angle — the model headers phrase this as integrating with the "post-ramp angle", referring to that rate, not the heading used for position. `project()` returns `ceil(horizon / dt) + 1` samples, with the initial state as element 0; it returns an empty sequence for degenerate inputs (`dt_s <= 0` or `horizon_s < 0`). + +`BicycleProjector` and `ArticulatedProjector` ramp an angle; `DifferentialDriveProjector` ramps the body command `(v, ω)` under separate linear and angular acceleration limits, since a differential drive has no steered joint. diff --git a/derivations/articulated_model.md b/derivations/articulated_model.md index 3c58382..af00d7c 100644 --- a/derivations/articulated_model.md +++ b/derivations/articulated_model.md @@ -18,7 +18,7 @@ A vehicle with two rigid sections connected by a central pivot (the articulation - $v$ — forward linear velocity of the vehicle (positive = forward) - $\omega$ — yaw rate of the body (positive = counter-clockwise / left turn) - $\gamma$ — articulation angle between front and rear sections (positive = left turn, counterclockwise) -- $\dot{\gamma}$ — articulation angle rate of change (currently fixed at $0$ in the implementation; placeholder for future estimation) +- $\dot{\gamma}$ — articulation angle rate of change. Supplied by the caller; the two-argument overloads of `bodyVelocityToVehicleState` / `articulationToAxleVelocities` assume steady articulation ($\dot{\gamma} = 0$) - $\theta_f$ — heading angle of the front body - $\theta_r$ — heading angle of the rear body @@ -206,9 +206,73 @@ Computing `bodyVelocityToVehicleState(v, omega)` to get $\gamma$, then feeding t --- -## Future work +## Forward projection (`ArticulatedProjector`) + +`ArticulatedProjector` wraps this model to roll a pose forward in time under a rate-limited +articulation joint. + +**Pose reference.** Poses are measured at whichever axle the caller selects +(`AxleReference::FRONT` or `REAR`), and $\theta$ is the heading of the body that axle belongs to — +$\theta_r$ for a REAR reference, $\theta_f = \theta_r + \gamma$ for a FRONT one. Motion is always +integrated at the rear axle. The conversions chain through the joint: + +$$\mathbf{p}^{\text{joint}} = \mathbf{p}^{\text{rear}} + L_r\begin{bmatrix}\cos\theta_r\\\sin\theta_r\end{bmatrix}, \qquad \mathbf{p}^{\text{front}} = \mathbf{p}^{\text{joint}} + L_f\begin{bmatrix}\cos\theta_f\\\sin\theta_f\end{bmatrix}$$ + +Every sample also reports `joint_pose` (position of the joint, $\theta = \theta_r$), so the +articulation joint — `base_link` for a ROS articulated vehicle — is available whichever axle is +referenced. + +**Per step**, given $\Delta t$, the current angle $\gamma_k$, a target $\gamma^\*$, a rate +limit $\dot{\gamma}_{\max}$, and a commanded $v$: + +1. **Clamp then ramp.** The target is first clamped to the joint's mechanical limits, then the + angle advances toward it without overshooting: + $$\gamma_{k+1} = \gamma_k + \operatorname{clamp}\!\left(\operatorname{clamp}(\gamma^\*,\, \gamma_{\min},\, \gamma_{\max}) - \gamma_k,\; -\dot{\gamma}_{\max}\Delta t,\; +\dot{\gamma}_{\max}\Delta t\right)$$ + Clamping before ramping means an out-of-range command saturates at the limit rather than + oscillating around it. +2. **Realized rate.** $\dot{\gamma}_k = (\gamma_{k+1} - \gamma_k)/\Delta t$, which falls to $0$ + once the angle pins at the target. This value — not the rate limit — is fed into the + kinematics above, so the $\dot{\gamma}$ term is exercised during the ramp and vanishes at + steady state. +3. **Integrate at the rear axle.** Because $\omega$ here is the rear-axle turning velocity, the + joint pose is converted back to the rear axle, Euler-stepped, and converted forward again: + $$\mathbf{p}^{\text{rear}}_k = \mathbf{p}_k - L_r\begin{bmatrix}\cos\theta_k\\\sin\theta_k\end{bmatrix}, \qquad \mathbf{p}^{\text{rear}}_{k+1} = \mathbf{p}^{\text{rear}}_k + v\begin{bmatrix}\cos\theta_k\\\sin\theta_k\end{bmatrix}\Delta t$$ + $$\theta_{k+1} = \operatorname{wrap}(\theta_k + \omega_k \Delta t), \qquad \mathbf{p}_{k+1} = \mathbf{p}^{\text{rear}}_{k+1} + L_r\begin{bmatrix}\cos\theta_{k+1}\\\sin\theta_{k+1}\end{bmatrix}$$ + The heading is taken at the *start* of the step (explicit Euler). Integrating at the joint + directly would trace the wrong arc, since the joint is not the point that moves along the + body's velocity vector. + +`project(horizon, dt, ...)` repeats this for $\lceil \text{horizon}/\Delta t \rceil$ steps and +returns $\lceil \text{horizon}/\Delta t \rceil + 1$ samples, with the initial state seeded as +element 0 so plots have a clean $t = 0$ anchor. + +### Footprints + +The projector optionally emits one polygon per body, keeping the kinematic model itself +dimension-free. Each is an **arbitrary polygon** supplied in the frame of **its own axle**: the front +polygon about the front axle with $+x$ along $\theta_f$, the rear polygon about the rear axle with +$+x$ along $\theta_r$. + +Anchoring each body at its own axle is the only choice that stays rigid as the joint articulates. A +polygon measured from the *other* body's axle would have to be re-measured for every value of +$\gamma$, since the two bodies rotate relative to one another about the joint — so a fixed polygon +in that frame would only be correct at one articulation angle. + +The joint sits $L_f$ behind the front axle and $L_r$ ahead of the rear axle, so a loader with $f$ of +bucket ahead of the front axle and $r$ of counterweight behind the rear axle is described as: + +| Body | Forward extent | Rearward extent | +|---|---|---| +| Front (about front axle) | $f$ | $L_f$ (back to the joint) | +| Rear (about rear axle) | $L_r$ (forward to the joint) | $r$ | + +`rectangleFootprint(front_m, rear_m, width_m)` builds the boxy case, ordered counter-clockwise and +open. An empty polygon means "unset": that body's footprint comes back empty and projection proceeds +normally rather than throwing. -The $\dot{\gamma}$ term is currently hardcoded to $0$. Once articulation angle rate estimation is available (e.g., from an IMU or joint encoder derivative), it will be incorporated to improve accuracy during transient steering maneuvers. +--- + +## Future work Add diagrams to the readme for visualization diff --git a/derivations/bicycle_model.md b/derivations/bicycle_model.md index 87fc16c..64d75f0 100644 --- a/derivations/bicycle_model.md +++ b/derivations/bicycle_model.md @@ -89,5 +89,58 @@ $$\omega_{\text{fr}} = \frac{\omega \cdot \operatorname{copysign}\!\left(\sqrt{( | $\omega \approx 0,\; v \neq 0$ | Straight line. All wheels spin at $v / r$ | | $\|R\| < W/2$ | ICR between rear wheels. Inner wheel reverses direction (handled by $\operatorname{copysign}$) | +## Forward projection (`BicycleProjector`) + +`BicycleProjector` wraps this model to roll a pose forward in time under a rate-limited steering +actuator. + +**Pose reference.** Poses are measured at whichever axle the caller selects +(`AxleReference::FRONT` or `REAR`). Motion is always integrated at the **rear axle**, the classic +bicycle reference and the point that travels along the body velocity vector. Because the chassis is +one rigid body, both axles share the heading $\theta$, so a FRONT reference is a pure longitudinal +offset applied on input and output: + +$$\mathbf{p}^{\text{front}} = \mathbf{p}^{\text{rear}} + L\begin{bmatrix}\cos\theta\\\sin\theta\end{bmatrix}$$ + +**Per step**, given $\Delta t$, the current angle $\delta_k$, a target $\delta^\*$, a rate limit +$\dot{\delta}_{\max}$, and a commanded $v$: + +1. **Clamp then ramp.** The target is clamped to the actuator's limits first, then the angle + advances toward it without overshooting: + $$\delta_{k+1} = \delta_k + \operatorname{clamp}\!\left(\operatorname{clamp}(\delta^\*,\, \delta_{\min},\, \delta_{\max}) - \delta_k,\; -\dot{\delta}_{\max}\Delta t,\; +\dot{\delta}_{\max}\Delta t\right)$$ + Clamping before ramping means an out-of-range command saturates at the limit rather than + oscillating around it. +2. **Forward kinematics** with the post-ramp angle gives the body yaw rate: + $\omega_k = v \tan(\delta_{k+1}) / L$. +3. **Euler pose update**, heading taken at the *start* of the step: + $$x_{k+1} = x_k + v\cos\theta_k\,\Delta t, \qquad y_{k+1} = y_k + v\sin\theta_k\,\Delta t, \qquad \theta_{k+1} = \operatorname{wrap}(\theta_k + \omega_k \Delta t)$$ + +Each sample also carries the full `BicycleSteeringState` (four wheel speeds and turning radius) +from running the inverse kinematics on $\omega_k$. + +`project(horizon, dt, ...)` repeats this for $\lceil \text{horizon}/\Delta t \rceil$ steps and +returns $\lceil \text{horizon}/\Delta t \rceil + 1$ samples, with the initial state seeded as +element 0 so plots have a clean $t = 0$ anchor. + +### Footprints + +The projector optionally emits a body polygon per sample, keeping the kinematic model itself +dimension-free. The footprint is an **arbitrary polygon** supplied in the body frame of the selected +axle: $+x$ along the chassis heading, $+y$ to the left, origin at that axle. Each sample carries the +polygon transformed into the world frame by that sample's pose. + +Because the reference axle sets the polygon's origin, the same physical body is described +differently depending on the choice. For a vehicle with $f$ of overhang past the front axle and $r$ +of tail behind the rear axle: + +| Reference | Forward extent | Rearward extent | +|---|---|---| +| `REAR` | $L + f$ | $r$ | +| `FRONT` | $f$ | $L + r$ | + +`rectangleFootprint(front_m, rear_m, width_m)` builds the boxy case, ordered counter-clockwise and +open: rear-right, front-right, front-left, rear-left. An empty polygon means "unset": the footprint +comes back empty and projection proceeds normally rather than throwing. + ## Future Work Add diagrams to the readme for visualization diff --git a/derivations/differential_drive.md b/derivations/differential_drive.md index 20172f3..521d69b 100644 --- a/derivations/differential_drive.md +++ b/derivations/differential_drive.md @@ -57,6 +57,49 @@ $$d_{\text{ICR}} = \frac{v}{\omega} = \frac{W}{2} \cdot \frac{\omega_R + \omega_ When the robot drives straight, the ICR is at infinity. When spinning in place, it is at the origin. +## Forward projection (`DifferentialDriveProjector`) + +`DifferentialDriveProjector` wraps this model to roll a pose forward in time under acceleration +limits. Unlike the bicycle and articulated projectors — which rate-limit a steering *angle* — a +differential drive has no steered joint, so what is limited here is the **body command itself**: +$v$ and $\omega$ each ramp toward their target under their own acceleration limit. + +**Pose reference.** The pose is the **body centre** (midway between the two wheels), the point +the ICR distance is measured from above. + +**Per step**, given $\Delta t$, current $(v_k, \omega_k)$, targets $(v^\*, \omega^\*)$, and +limits $(a_{\max}, \alpha_{\max})$: + +1. **Clamp then ramp**, independently per channel, neither overshooting: + $$v_{k+1} = v_k + \operatorname{clamp}\!\left(\operatorname{clamp}(v^\*,\, v_{\min},\, v_{\max}) - v_k,\; -a_{\max}\Delta t,\; +a_{\max}\Delta t\right)$$ + $$\omega_{k+1} = \omega_k + \operatorname{clamp}\!\left(\operatorname{clamp}(\omega^\*,\, \omega_{\min},\, \omega_{\max}) - \omega_k,\; -\alpha_{\max}\Delta t,\; +\alpha_{\max}\Delta t\right)$$ + Clamping before ramping means an out-of-range command saturates at the limit rather than + oscillating around it. +2. **Inverse kinematics** on the post-ramp command gives the wheel speeds recorded on the sample. +3. **Euler pose update**, using the post-ramp velocities with the heading taken at the *start* of + the step: + $$x_{k+1} = x_k + v_{k+1}\cos\theta_k\,\Delta t, \qquad y_{k+1} = y_k + v_{k+1}\sin\theta_k\,\Delta t, \qquad \theta_{k+1} = \operatorname{wrap}(\theta_k + \omega_{k+1} \Delta t)$$ + +`project(horizon, dt, ...)` repeats this for $\lceil \text{horizon}/\Delta t \rceil$ steps and +returns $\lceil \text{horizon}/\Delta t \rceil + 1$ samples, with the initial state seeded as +element 0 so plots have a clean $t = 0$ anchor. Setting initial $=$ target makes the ramp a +no-op, which is how a constant-command trajectory (e.g. one lattice cell) is generated. + +### Footprints + +The projector optionally emits a body polygon per sample, keeping the kinematic model itself +dimension-free. The footprint is an **arbitrary polygon** supplied in the body frame: $+x$ along the +heading, $+y$ to the left, origin at the body centre. Each sample carries it transformed into the +world frame by that sample's pose. + +A differential drive has a single axle, so unlike the bicycle and articulated projectors there is no +axle reference to select — the body centre is the only sensible origin, and polygon extents are the +bumper distances directly. + +`rectangleFootprint(front_m, rear_m, width_m)` builds the boxy case, ordered counter-clockwise and +open: rear-right, front-right, front-left, rear-left. An empty polygon means "unset": the footprint +comes back empty and projection proceeds normally rather than throwing. + ## Future Work Add diagrams to the readme for visualization