Skip to content

Add projections and tests! - #3

Open
zeerekahmad wants to merge 15 commits into
mainfrom
zeerek/add_projection_mapping
Open

Add projections and tests!#3
zeerekahmad wants to merge 15 commits into
mainfrom
zeerek/add_projection_mapping

Conversation

@zeerekahmad

@zeerekahmad zeerekahmad commented May 11, 2026

Copy link
Copy Markdown
Contributor

Description

Integrates the shared polymath_kinematics::ArticulatedModel into the articulated steering controller,
replacing the controller's hand-rolled kinematic formulas with the library used across the fleet. The
two previously-separate command paths (front_two_wd and four_wd, each with its own
get_velocity_command* / calculate_wheel_speed* pair) collapse into a single
get_velocity_command,
which derives the steering target from bodyVelocityToVehicleState and the per-wheel speeds from
articulationToAxleVelocities, with drive_mode deciding whether the rear wheels are driven or zeroed.

Beyond the kinematics swap, the controller gains proper signal conditioning and real odometry. Linear
velocity now passes through a first-order lag (time constant τ) that models drivetrain inertia instead
of the old ad-hoc filtering, and the articulation angle is rate-limited toward its target to model
hydraulic cylinder speed. The update() loop integrates pose (x, y, θ) from the measured wheel speeds
and articulation angle and publishes real odometry (pose + twist) via a tf2 quaternion, and parameters
are re-read every tick so they can be retuned at runtime.

  • src/articulated_steering_controller.cpp:
    constructs the ArticulatedModel on configure and validates the wheel-joint count against
    drive_mode; unifies the command path into get_velocity_command (general/straight-line via
    bodyVelocityToVehicleState, explicit pure-rotation and stop cases); first-order velocity lag,
    rate-limited steering, wheel speeds from articulationToAxleVelocities; odometry integrated in
    update() and published with real pose/twist; dynamic parameter reload each tick.
  • include/…/articulated_steering_controller.hpp:
    adds a std::unique_ptr<polymath::kinematics::ArticulatedModel> member, odometry state
    (odom_x_/odom_y_/odom_theta_, cached measured twist, last_odom_time_), and the
    velocity_time_constant_ / steering_rate_ tuning members; removes the now-dead
    get_velocity_command_front_2wd, calculate_wheel_speed, and calculate_wheel_speed_front_2wd
    declarations — unified into the single get_velocity_command.
  • src/articulated_steering_controller_parameters.yaml:
    replaces publish_rate with velocity_time_constant (first-order lag τ, default 0.5 s) and
    steering_rate (max articulation rate, default 1.0 rad/s), both validated > 0.
  • CMakeLists.txt
    and package.xml:
    add polymath_kinematics, tf2, and tf2_geometry_msgs as link/build dependencies.
  • repos-humble.yaml / repos-jazzy.yaml: add the
    polymath_kinematics git repository so vcs import pulls it into the workspace.

Migration Guide

Two migration steps for anyone deploying or building against this controller:

  1. New dependency polymath_kinematics. It is now a build/runtime dependency of
    articulated_steering_controller. Re-run vcs import with the updated repos-humble.yaml /
    repos-jazzy.yaml to fetch it, then rebuild. Downstream packages that depend on the controller
    rebuild transitively.
  2. publish_rate parameter removed. Any articulated_steering_controller config that sets
    publish_rate must drop it. It is superseded by velocity_time_constant (τ for the linear-velocity
    lag, default 0.5 s) and steering_rate (max articulation rate, default 1.0 rad/s). Defaults preserve
    sensible behavior, so setting them is optional.

Related issues

closes #6

Completeness checklist

  • Is there test coverage? If it was a bugfix, is there a regression test?
  • Is user-facing API/behavior documented?
  • Is core documentation updated?
  • Are all checks green?
  • Assign reviewers to the PR and post in #code_reviews

@zeerekahmad
zeerekahmad force-pushed the zeerek/add_projection_mapping branch from 10b377f to d3f9406 Compare June 26, 2026 19:48
zeerekahmad and others added 4 commits June 26, 2026 19:50
Footprint is now computed in the C++ projectors and exposed via pybind so
projecting a trajectory yields the vehicle outline at each step (single body
for bicycle/differential, front + rear for articulated, folding about the
articulation angle). Dimensions live on the projectors; models stay pure
kinematics. Unset/invalid dims -> empty footprint, never throws.

- Articulated base_link/pose is now the articulation joint (integration still
  runs at the rear axle); footprints gain front/rear overhang beyond the axles
- Rename k-style constants to ALL_CAPS; normalizeAngle via std::remainder
- Explorer: consume projected footprints; show only the longest (highest-vel)
  lattice subplot; 16:9 landscape plots constrained to 2/3-width columns
- Conservative test trim (bindings tests -> surface checks; C++ owns geometry)
  and small app cleanup (hoist vel_options, shared single-trajectory render)

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
@zeerekahmad zeerekahmad self-assigned this Aug 4, 2026
@zeerekahmad zeerekahmad linked an issue Aug 4, 2026 that may be closed by this pull request
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Add Ego Projection Framework

1 participant