refactor: use Heuclid polygon predicates for collision detection - #7
Merged
Conversation
added 9 commits
March 17, 2026 14:46
- demo_export.cpp: exports planner data to CSV (footsteps, polygons, body path) - demo_visualize.py: Python matplotlib reads real data, renders footstep GIF - Foot polygons from getFootVertex2D, alternating L/R from AccurateFootstep - Body path from Simple2DBodyPathHolder (ellipsoid) with direction arrows - Start/Goal markers drawn last with white background labels - .gitignore: exclude generated CSV and frames_flat/ from tracking
- goalPose updated to test7 coords (0.663, -0.962, yaw=-1.554) - HWPOfPathDistance: 1.0 → 2.50 (strong body path guidance) - HWP walk/turn weights: default → test7 tuned (1.30) - Full parameter set: edge costs, step yaw, proximity, step size - Result: 22 footsteps (vs 10), natural alternating L/R straddling body path
Key fixes: - sqrt() fix in computeFollowEllipsoidPath: pfp.distance is squared distance, must use sqrt() before comparing to IdealStepWidth - Remove duplicate SetFollowBodyPath(false) that overwrote true - Add followBodyPath parameter (separate from isStairAlignMode) to enable body path heuristic without stair constraints - Add edgecost_w_pathdev for optional edge cost penalty Result: 25 footsteps evenly straddling the ellipsoid body path. Search completes in 72 nodes (vs 1456+ with the sqrt bug).
- README.md / README_CN.md: 19 → 25 footsteps, accurate descriptions - parameters.h: Doxygen for followBodyPath, edgecost_w_pathdev - HeuristicCalculator.cpp: sqrt() comment on squared distance fix - FootstepCostCalculator.cpp: body path deviation penalty docs - AStarFootstepPlanner.cpp: followBodyPath heuristic selection comment - Re-apply followBodyPath change (accidentally reverted by checkout)
- demo_obstacle.cpp: planner with stair polygon as obstacle (12×12cm block) - demo_visualize.py: supports optional obstacle.csv for polygon rendering - Obstacle placed at body path center, forces detour (24 steps) - README.md / README_CN.md: obstacle demo section with GIF - assets/obstacle_avoidance.gif (196 KB)
- isTwoFootCollided: replaced 40-line manual vertex/midpoint loop with Heuclid isConvexPolygonIntersect (delegates to shared logic) - isFootPolygonCollidedWithPolygon: NEW — full polygon-polygon intersection for obstacle avoidance (replaces incomplete vertex-only check) - isFootPolygonContainedInPolygon: NEW — checks foot ⊆ terrain for stair climbing where foot must land on a single terrain patch - buildFootPolygon helper: extracts foot ConvexPolygon2D construction - CMakeLists: Heuclid FetchContent GIT_TAG v2.1 → v2.2 - ParameterBasedStepExpansion: uses new collision API Pre-existing issue: demo_export segfault tracked in #6 CI will verify compilation and unit tests independently. Heuclid: v2.2 (isConvexPolygonIntersect + isConvexPolygonContained)
Keep stair/obstacle step expansion using isAnyVertexOfFootInsideStairRegion to avoid changing test behavior. New APIs (isFootPolygonCollidedWithPolygon, isFootPolygonContainedInPolygon) are available but not yet wired into step expansion — will be used in stair demo phase. Changes: - isTwoFootCollided: uses isConvexPolygonIntersect via buildFootPolygon helper - StepConstraintCheck.h: new function declarations (unused yet) - StepConstraintCheck.cpp: buildFootPolygon helper + new implementations - CMakeLists.txt: Heuclid v2.1 → v2.2 Known issue: demo_export segfault (issue #6, pre-existing)
Mr-tooth
added a commit
that referenced
this pull request
Mar 20, 2026
refactor: use Heuclid polygon predicates for collision detection
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.
Depends on
isConvexPolygonIntersect(polyA, polyB)— detect overlapisConvexPolygonContained(polyA, polyB)— check polyA ⊆ polyBChanges
StepConstraintCheck.cppisTwoFootCollided: Replaced 40-line manual vertex/midpoint point-in-polygon loop withHeuclidPolygonTools::isConvexPolygonIntersect()— same algorithm, cleaner codebuildFootPolygon: New helper — buildsConvexPolygon2Dfrom foot pose + side (shared by all foot-polygon operations)isFootPolygonCollidedWithPolygon: NEW — full polygon-polygon intersection for obstacle avoidance. Checks foot vertices in obstacle AND obstacle vertices in foot AND edge midpoints. Replaces the incompleteisAnyVertexOfFootInsideStairRegionwhich only checked foot→obstacle directionisFootPolygonContainedInPolygon: NEW — checks if foot polygon is fully inside a terrain polygon. Required for stair climbing where each foot must land on a single terrain patchParameterBasedStepExpansion.cppisFootPolygonCollidedWithPolygoninstead ofisAnyVertexOfFootInsideStairRegionCMakeLists.txtGIT_TAG "v2.1"→GIT_TAG "v2.2"Known Issue
Test Results (local)
CI will verify ubuntu/macOS/windows build + tests.