holonomic trajectory controller#2697
Open
bogwi wants to merge 12 commits into
Open
Conversation
added 7 commits
June 26, 2026 12:05
Integrate holonomic path tracking into LocalPlanner with speed profiling, trajectory types and metrics, JSONL telemetry, run profiles, Go2 blueprint wiring, hot replan support, and dannav split-stack experiments.
Resolve go2/connection.py and pyproject.toml from main; combine all_blueprints registry entries from both sides.
- Create DanLocalPlanner: module, placement, wiring - Create lock_replan commit window - Add path smoothing and uniform resampling - Tune configs for testing - Remove progress markers - Remove the monolith: delete unitree_go2_dannav.py
Move holonomic_tc under dannav, relocate shared geometry helpers, strip stale trajectory logging and run-profile code, and tighten controller types, configs, tests, and unitree_go2_mls_htc blueprint wiring.
…trajectory_controller
bogwi
requested review from
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
Contributor
|
Preview deployment for your docs. Learn more about Mintlify Previews.
💡 Tip: Enable Workflows to automatically generate PRs for you. |
Contributor
Greptile SummaryThis PR adds two new navigation modules —
Confidence Score: 4/5Safe to merge after fixing The dimos/navigation/dannav/local_planner/module.py — the Important Files Changed
Sequence Diagram%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
participant ML as MLSPlannerNative
participant LP as DanLocalPlanner (_ReplanGate)
participant HTC as DanHolonomicTC (_HolonomicPathFollower)
participant MM as MovementManager
participant GO2 as GO2Connection
GO2->>LP: odom (PoseStamped)
GO2->>HTC: odom (PoseStamped)
MM->>LP: goal (PointStamped)
Note over LP: arm fresh-click commit
ML->>LP: planner_path (Path)
alt cold start / lock released / fresh-click match
LP->>LP: _smooth + resample path
LP->>HTC: path (committed Path)
HTC->>HTC: start_planning / update_path
loop "control tick @ 10 Hz"
HTC->>HTC: _compute_path_following (speed profile + holonomic law)
HTC->>GO2: nav_cmd_vel (Twist)
end
HTC->>MM: goal_reached (Bool)
else within lock window
LP-->>ML: suppress
end
MM->>HTC: stop_movement
HTC->>GO2: cmd_vel zero (Twist)
%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
sequenceDiagram
participant ML as MLSPlannerNative
participant LP as DanLocalPlanner (_ReplanGate)
participant HTC as DanHolonomicTC (_HolonomicPathFollower)
participant MM as MovementManager
participant GO2 as GO2Connection
GO2->>LP: odom (PoseStamped)
GO2->>HTC: odom (PoseStamped)
MM->>LP: goal (PointStamped)
Note over LP: arm fresh-click commit
ML->>LP: planner_path (Path)
alt cold start / lock released / fresh-click match
LP->>LP: _smooth + resample path
LP->>HTC: path (committed Path)
HTC->>HTC: start_planning / update_path
loop "control tick @ 10 Hz"
HTC->>HTC: _compute_path_following (speed profile + holonomic law)
HTC->>GO2: nav_cmd_vel (Twist)
end
HTC->>MM: goal_reached (Bool)
else within lock window
LP-->>ML: suppress
end
MM->>HTC: stop_movement
HTC->>GO2: cmd_vel zero (Twist)
Reviews (3): Last reviewed commit: "Merge branch 'main' into danvi/feat/holo..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
added 2 commits
July 2, 2026 22:47
github-actions
Bot
added
the
ready-to-merge
bogwi
changed the title
github-actions
Bot
removed
the
ready-to-merge
github-actions
Bot
added
the
ready-to-merge
bogwi
commented
Jul 4, 2026
bogwi
changed the title
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
2 participants
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.
Add Holonomic Trajectory Controller
This PR ships two modules:
Holonomic Trajectory Controller, htc
Why write HTC when differential already exists?
PControllerworks for a classic diff-drive mental model: point at lookahead, drive forward, rotate in place when badly misaligned. That is simple and robust, which is why it remains the default inunitree_go2.htcextends to:HolonomicTrackingControllermaps lateral error to bodyvy, so the robot slides back onto the polyline while still moving - on bends and right after a replan nudge.TrajectoryReferenceSample: lookahead pose on the path plus body-frame feedforward twist from the path-speed profile.PControlleronly steers toward a lookahead XY point at fixed cruise speed.profile_speed_along_polylineslows for curvature and goal decel;clamp_holonomic_cmd_velslews planar and yaw limits from run profiles. Odometry-derivedkv/kwdamp overshoot when feedforward changes. Differential has none of this - scalar speed, no lateral DOF, no geometry caps.Htc also borrows some functionlity from the excellent "differential" ReplanningAStar and adapts it to its own workflow.
DanLocalPlanner
Is a local planner module, as it says, and has two features to support MLSPlanner performance coming from kronk-nav.
a) Lock replan, meters - to stabilize the heavy drifting path the global planner supplies. Useful in controlled, constrained environments requiring stable paths over a certain distance. By default it is set to
0.0but you can experiment locking the path asDanLocalPlanner.blueprint(lock_replan=meters)(seeunitree_go2_mls_htc.py)b) Path smoothing (borrows directly from the existing stack) - to smooth out the path's jagged edges. As with (a), very useful in a micro environment with many turns.
Eventually both features might migrate to MLSPlanner.
How to run on bare Go2 native lidar
Very simple:
dimos --robot-ip <IP> run unitree-go2-mls-htc -o danholonomictc.run_profile=<profile>There are three run profiles at the moment, think presets with nominal engineering envelopes . But all you need to know is they cap max speed.
walk - 0.55 m/s
trot - 1.0 m/s
run_conservative - 1.5 m/s
walk is default so you can just run like this:
dimos --robot-ip <IP> run unitree-go2-mls-htcHow to run on Go2 + mod360 + Jetson
Blueprint that works with full kronk-nav #2570 will be shared soon. But if you assembled own setup you can just swap DanLocalPlanner and DanHolonomicTC modules into unitree_go2_nav_3d.py instead of BasicPathFollower.
Next
Must know:
Run profiles are in m/s but the Go2 driver is not.
DanHolonomicTC(andMovementManagerbefore it) publishescmd_velin real body-frame m/s and rad/s - walk 0.55, trot 1.0, run_conservative 1.5, corner caps,vystrafe, all of it.UnitreeWebRTCConnection.move()indimos/robot/unitree/connection.pydoes not send velocities. It mapstwist.linear.x/yandtwist.angular.zstraight ontoWIRELESS_CONTROLLERstick fieldslx/ly/rx.Next PR will be: metric
cmd_velbehind a toggle.Sport
MoveonGO2Connectiongated by a config flag (joystick API vs velocity API). Default staysWIRELESS_CONTROLLERsounitree_go2and current controller keep today's behavior.unitree-go2-mls-htc(and anything else that outputs metric twist) opts into the velocity API. Until that lands, treat on-robot HTC speed as uncalibrated - the control law is in this PR and the proper speed execution is the follow-up.