Add projections and tests! - #3
Open
zeerekahmad wants to merge 15 commits into
Open
Conversation
zeerekahmad
force-pushed
the
zeerek/add_projection_mapping
branch
from
June 26, 2026 19:48
10b377f to
d3f9406
Compare
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>
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
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.
Description
Integrates the shared
polymath_kinematics::ArticulatedModelinto 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_wdandfour_wd, each with its ownget_velocity_command*/calculate_wheel_speed*pair) collapse into a singleget_velocity_command,which derives the steering target from
bodyVelocityToVehicleStateand the per-wheel speeds fromarticulationToAxleVelocities, withdrive_modedeciding 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 speedsand 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
ArticulatedModelon configure and validates the wheel-joint count againstdrive_mode; unifies the command path intoget_velocity_command(general/straight-line viabodyVelocityToVehicleState, explicit pure-rotation and stop cases); first-order velocity lag,rate-limited steering, wheel speeds from
articulationToAxleVelocities; odometry integrated inupdate()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 thevelocity_time_constant_/steering_rate_tuning members; removes the now-deadget_velocity_command_front_2wd,calculate_wheel_speed, andcalculate_wheel_speed_front_2wddeclarations — unified into the single
get_velocity_command.src/articulated_steering_controller_parameters.yaml:replaces
publish_ratewithvelocity_time_constant(first-order lag τ, default 0.5 s) andsteering_rate(max articulation rate, default 1.0 rad/s), both validated> 0.CMakeLists.txtand
package.xml:add
polymath_kinematics,tf2, andtf2_geometry_msgsas link/build dependencies.repos-humble.yaml/repos-jazzy.yaml: add thepolymath_kinematicsgit repository sovcs importpulls it into the workspace.Migration Guide
Two migration steps for anyone deploying or building against this controller:
polymath_kinematics. It is now a build/runtime dependency ofarticulated_steering_controller. Re-runvcs importwith the updatedrepos-humble.yaml/repos-jazzy.yamlto fetch it, then rebuild. Downstream packages that depend on the controllerrebuild transitively.
publish_rateparameter removed. Anyarticulated_steering_controllerconfig that setspublish_ratemust drop it. It is superseded byvelocity_time_constant(τ for the linear-velocitylag, default 0.5 s) and
steering_rate(max articulation rate, default 1.0 rad/s). Defaults preservesensible behavior, so setting them is optional.
Related issues
closes #6
Completeness checklist