diff --git a/.gitignore b/.gitignore index c23fbf9..d3c0c0c 100644 --- a/.gitignore +++ b/.gitignore @@ -33,4 +33,8 @@ # Cmake Build Folders build/ -test/build/ \ No newline at end of file +test/build/ + +# Demo generated files +demo/*.csv +demo/frames_flat/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index fae4257..686b5c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,7 +38,7 @@ if(NOT Heuclid_FOUND) include(FetchContent) FetchContent_Declare(Heuclid GIT_REPOSITORY "https://github.com/Mr-tooth/Heuclid.git" - GIT_TAG "v2.1" + GIT_TAG "v2.2" GIT_SHALLOW TRUE) set(BUILD_TESTING_HEUCID OFF CACHE BOOL "" FORCE) FetchContent_MakeAvailable(Heuclid) @@ -137,17 +137,11 @@ if(MATPLOTLIB_CPP_AVAILABLE) endif() message(STATUS "matplotlib_cpp enabled — PlotCheck module included") - # --- Demo GIF generator --- - add_executable(demo_flat demo/demo_flat.cpp) - target_link_libraries(demo_flat PRIVATE ${PROJECT_NAME}) - target_compile_definitions(demo_flat PRIVATE HAS_MATPLOTLIB) - if(matplotlib_cpp_FOUND) - target_include_directories(demo_flat PRIVATE ${matplotlib_cpp_INCLUDE_DIRS}) - target_link_libraries(demo_flat PRIVATE ${matplotlib_LIBS}) - else() - target_include_directories(demo_flat PRIVATE ${MATPLOTLIB_CPP_INCLUDE}) - target_link_libraries(demo_flat PRIVATE Python3::Python Python3::NumPy) - endif() + # --- Demo GIF generator (data export only, visualization in Python) --- + add_executable(demo_export demo/demo_export.cpp) + target_link_libraries(demo_export PRIVATE ${PROJECT_NAME}) + add_executable(demo_obstacle demo/demo_obstacle.cpp) + target_link_libraries(demo_obstacle PRIVATE ${PROJECT_NAME}) else() message(STATUS "matplotlib_cpp not found — PlotCheck module disabled") endif() diff --git a/README.md b/README.md index 5a0e604..c53c72e 100644 --- a/README.md +++ b/README.md @@ -44,10 +44,18 @@ This project is a C++ reimplementation of the core algorithms from the [IHMC Foo Flat terrain footstep planning

-> A* footstep planning from start (blue dot) to goal (red arrow). Red = left foot, orange = right foot. 19 discrete footsteps with body path (green dashed line). +> A* footstep planning from start (blue dot) to goal (red diamond). Red = left foot, orange = right foot. 25 discrete footsteps evenly straddling the ellipsoid body path (blue line with direction arrows). + +### Obstacle Avoidance + +

+ Obstacle avoidance footstep planning +

+ +> Footsteps navigate around a gray obstacle block placed on the body path. The planner rejects footsteps intersecting the obstacle polygon, producing a natural detour (24 steps). - ✅ Flat terrain: start → goal footstep sequence -- ⏳ Obstacle avoidance: navigating around forbidden regions +- ✅ Obstacle avoidance: navigating around forbidden regions - ⏳ Stair climbing: constrained footstep planning on stairs ## Quick Start diff --git a/README_CN.md b/README_CN.md index c85be08..f092e92 100644 --- a/README_CN.md +++ b/README_CN.md @@ -44,10 +44,18 @@ 平地落脚点规划

-> A* 落脚点规划：从起点（蓝点）到目标点（红箭头）。红色=左脚，橙色=右脚。19 个离散落脚点 + 身体路径（绿色虚线）。 +> A* 落脚点规划：从起点（蓝色方块）到目标点（红色菱形）。红色=左脚，橙色=右脚。25 个离散落脚点均匀分布在椭圆身体路径（蓝色曲线，带方向箭头）两侧。 + +### 障碍物避障 + +

+ 障碍物避落脚点规划 +

+ +> 落脚点绕过放置在身体路径上的灰色障碍物块。规划器会拒绝与障碍物多边形相交的落脚点，产生自然的绕行路径（24 步）。 - ✅ 平地：起点 → 终点落脚点序列 -- ⏳ 障碍物回避：绕开禁止区域 +- ✅ 障碍物回避：绕开禁止区域 - ⏳ 楼梯攀爬：楼梯约束下的落脚点规划 > - 障碍物避让：绕过禁区的路径规划 > - 楼梯场景：约束条件下的楼梯落脚点规划 diff --git a/assets/flat_terrain.gif b/assets/flat_terrain.gif index c79a052..431a5d6 100644 Binary files a/assets/flat_terrain.gif and b/assets/flat_terrain.gif differ diff --git a/assets/obstacle_avoidance.gif b/assets/obstacle_avoidance.gif new file mode 100644 index 0000000..7bda024 Binary files /dev/null and b/assets/obstacle_avoidance.gif differ diff --git a/demo/demo_export.cpp b/demo/demo_export.cpp new file mode 100644 index 0000000..072a5af --- /dev/null +++ b/demo/demo_export.cpp @@ -0,0 +1,135 @@ +/** + * Demo: Flat terrain footstep planning with body path. + * Uses test7 tuned parameters for optimal body path following. + * Exports real planner data to CSV for Python visualization. + */ +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace ljh::heuclid; +using namespace ljh::path::footstep_planner; + +int main() +{ + // === Flat terrain scenario (test7) === + double startX = 0.015, startY = 0.0, startZ = 0.0, startYaw = 0.0; + double goalX = 0.663, goalY = -0.962, goalZ = 0.0, goalYaw = -1.554; + + Pose2D goalPose2D(goalX, goalY, goalYaw); + Pose3D goalPose(goalX, goalY, goalZ, goalYaw, 0.0, 0.0); + Pose3D startPose(startX, startY, startZ, startYaw, 0.0, 0.0); + + // === Body path (ellipsoid) === + Simple2DBodyPathHolder pathHolder; + pathHolder.initialize({startX, startY, startYaw}, goalPose2D); + auto waypoints = pathHolder.getWayPointPath(); + + { + std::ofstream fout("demo/body_path.csv"); + fout << "idx,x,y,yaw" << std::endl; + for (size_t i = 0; i < waypoints.size(); i++) + { + fout << i << "," + << waypoints[i].getPosition().getX() << "," + << waypoints[i].getPosition().getY() << "," + << waypoints[i].getOrientation().getYaw() << std::endl; + } + fout.close(); + std::cout << "Wrote " << waypoints.size() << " body path waypoints" << std::endl; + } + + AStarFootstepPlanner planner; + + // Apply test7 tuned parameters + parameters param; + param.SetEdgeCostDistance(param, 4.0); + param.SetEdgeCostYaw(param, 4.0); + param.SetEdgeCostStaticPerStep(param, 1.4); + param.SetMaxStepYaw(param, pi / 12.0); + param.SetMinStepYaw(param, -pi / 12.0); + param.SetFinalTurnProximity(param, 0.3); + param.SetGoalDistanceProximity(param, 0.04); + param.SetGoalYawProximity(param, 4.0 / 180.0 * pi); + param.SetFootPolygonExtendedLength(param, 0.025); + + // HWP weights — test7 tuned values + param.SetHWPOfWalkDistacne(param, 1.30); + // Enable body path following via heuristic + param.SetFollowBodyPath(param, true); + param.SetHWPOfPathDistance(param, 1.0); // Body path heuristic weight + param.SetEdgeCostPathDev(param, 0.0); // No edge penalty needed — heuristic handles it + param.SetHWPOfInitialTurnDistacne(param, 1.0); + param.SetHWPOfFinalTurnDistacne(param, 1.30); + param.SetHWPOfFinalWalkDistacne(param, 1.30); + + // Step size constraints + param.SetMaxStepLength(param, 0.08); + param.SetMinStepLength(param, -0.08); + param.SetMaxStepWidth(param, 0.22); + param.SetMinStepWidth(param, 0.16); + param.SetMaxStepReach(param, sqrt(pow(0.22 - 0.16, 2) + 0.08 * 0.08)); + + // Params are static members — set once, used everywhere + + planner.initialize(goalPose2D, goalPose, startPose); + planner.doAStarSearch(); + planner.calFootstepSeries(); + auto accurateSteps = planner.getOrCalAccurateFootstepSeries(); + + std::cout << "Accurate footsteps: " << accurateSteps.size() << std::endl; + + // === Export footstep center positions === + { + std::ofstream fout("demo/footsteps.csv"); + fout << "step,x,y,yaw,side" << std::endl; + for (size_t i = 0; i < accurateSteps.size(); i++) + { + auto& s = accurateSteps[i]; + std::string side = (s.getStepFlag() == stepL) ? "L" : "R"; + fout << i << "," << s.getX() << "," << s.getY() << "," + << s.getYaw() << "," << side << std::endl; + } + fout.close(); + std::cout << "Wrote " << accurateSteps.size() << " footsteps" << std::endl; + } + + // === Export foot polygon vertices === + { + std::ofstream fout("demo/foot_polygons.csv"); + fout << "step,vertex,x,y" << std::endl; + for (size_t i = 0; i < accurateSteps.size(); i++) + { + auto& s = accurateSteps[i]; + Pose2D pose; + pose.setPosition(s.getX(), s.getY()); + pose.setOrientation(s.getYaw()); + + std::vector vx, vy; + getFootVertex2D(pose, s.getStepFlag(), vx, vy); + + for (size_t j = 0; j < vx.size(); j++) + fout << i << "," << j << "," << vx[j] << "," << vy[j] << std::endl; + } + fout.close(); + std::cout << "Wrote foot polygons" << std::endl; + } + + // === Export start/goal === + { + std::ofstream fout("demo/start_goal.csv"); + fout << "pose,x,y,yaw" << std::endl; + fout << "start," << startX << "," << startY << "," << startYaw << std::endl; + fout << "goal," << goalX << "," << goalY << "," << goalYaw << std::endl; + fout.close(); + } + + std::cout << "All data exported." << std::endl; + return 0; +} diff --git a/demo/demo_flat.cpp b/demo/demo_flat.cpp deleted file mode 100644 index 2f0fdca..0000000 --- a/demo/demo_flat.cpp +++ /dev/null @@ -1,163 +0,0 @@ -// Copyright 2026 Junhang Li -// SPDX-License-Identifier: Apache-2.0 - -// Demo: Flat terrain footstep planning with ellipsoid body path -// Agg-compatible: uses only plt::plot + plt::annotate (no arrow, no scatter, no set_aspect_equal) - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace plt = matplotlibcpp; -namespace fs = std::filesystem; - -void drawEllipsoidPath(ljh::path::footstep_planner::Simple2DBodyPathHolder& pathHolder) -{ - auto waypoints = pathHolder.getWayPointPath(); - if (waypoints.empty()) return; - - std::vector x, y; - for (size_t i = 0; i < waypoints.size(); i++) - { - x.push_back(waypoints[i].getPosition().getX()); - y.push_back(waypoints[i].getPosition().getY()); - } - - plt::plot(x, y, {{"color", "3498db"}, {"linewidth", "1.2"}}); -} - -void drawFootsteps(const std::vector& steps, int count) -{ - std::vector vx, vy; - for (int i = 0; i < count && i < (int)steps.size(); i++) - { - vx.clear(); - vy.clear(); - getFootVertex2D(steps.at(i), vx, vy); - if (!vx.empty()) { vx.push_back(vx.front()); vy.push_back(vy.front()); } - - std::string color = (steps.at(i).getSecondStepSide().getStepFlag() == stepL) ? "e74c3c" : "f39c12"; - plt::plot(vx, vy, {{"color", color}, {"linewidth", "1.5"}}); - - // Step center dot (instead of scatter) - double cx = steps.at(i).getSecondStep().getX(); - double cy = steps.at(i).getSecondStep().getY(); - plt::plot(std::vector{cx}, std::vector{cy}, - {{"color", "3498db"}, {"marker", "."}, {"linestyle", "none"}, {"markersize", "4"}}); - } -} - -void drawMarkers(double sx, double sy, double syaw, - double gx, double gy, double gyaw) -{ - const double arrowLen = 0.03; - - // Start: green dot + direction line - plt::plot(std::vector{sx}, std::vector{sy}, - {{"color", "2ecc71"}, {"marker", "s"}, {"linestyle", "none"}, {"markersize", "10"}}); - plt::plot(std::vector{sx, sx + cos(syaw) * arrowLen}, - std::vector{sy, sy + sin(syaw) * arrowLen}, - {{"color", "2ecc71"}, {"linewidth", "3.0"}}); - - // Goal: red dot + direction line - plt::plot(std::vector{gx}, std::vector{gy}, - {{"color", "e74c3c"}, {"marker", "D"}, {"linestyle", "none"}, {"markersize", "10"}}); - plt::plot(std::vector{gx, gx + cos(gyaw) * arrowLen}, - std::vector{gy, gy + sin(gyaw) * arrowLen}, - {{"color", "e74c3c"}, {"linewidth", "3.0"}}); -} - -int main() -{ - std::cout << "=== AStar Footstep Planner - Flat Terrain Demo ===" << std::endl; - - std::string outDir = "demo/frames_flat"; - fs::create_directories(outDir); - - ljh::path::footstep_planner::LatticePoint latticepoint; - ljh::path::footstep_planner::parameters param; - latticepoint.setGridSizeXY(latticepoint, 0.01); - latticepoint.setYawDivision(latticepoint, 72); - param.SetEdgeCostDistance(param, 4.0); - param.SetEdgeCostYaw(param, 4.0); - param.SetEdgeCostStaticPerStep(param, 1.4); - param.SetDebugFlag(param, false); - param.SetMaxStepYaw(param, pi / 12); - param.SetMinStepYaw(param, -pi / 12); - param.SetFinalTurnProximity(param, 0.3); - param.SetGoalDistanceProximity(param, 0.04); - param.SetGoalYawProximity(param, 4.0 / 180.0 * pi); - param.SetFootPolygonExtendedLength(param, 0.025); - param.SetHWPOfWalkDistacne(param, 1.30); - param.SetHWPOfPathDistance(param, 2.50); - param.SetHWPOfFinalTurnDistacne(param, 1.30); - param.SetHWPOfFinalWalkDistacne(param, 1.30); - param.SetMaxStepLength(param, 0.08); - param.SetMinStepLength(param, -0.08); - param.SetMaxStepWidth(param, 0.22); - param.SetMinStepWidth(param, 0.16); - param.SetMaxStepReach(param, sqrt( - (param.MaxStepWidth - param.MinStepWidth) * (param.MaxStepWidth - param.MinStepWidth) - + param.MaxStepLength * param.MaxStepLength)); - - double startX = 0.015, startY = 0.0, startZ = 0.0, startYaw = 0.0; - double goalX = 0.815, goalY = -0.8, goalZ = 0.0, goalYaw = -90.0 / 180.0 * pi; - - ljh::heuclid::Pose2D goalPose2D(goalX, goalY, goalYaw); - ljh::heuclid::Pose3D goalPose(goalX, goalY, goalZ, goalYaw, 0.0, 0.0); - ljh::heuclid::Pose3D startPose(startX, startY, startZ, startYaw, 0.0, 0.0); - - ljh::path::footstep_planner::Simple2DBodyPathHolder pathHolder; - pathHolder.initialize({startX, startY, startYaw}, goalPose2D); - - std::cout << "Running A* search..." << std::endl; - ljh::path::footstep_planner::AStarFootstepPlanner planner; - planner.initialize(goalPose2D, goalPose, startPose); - planner.doAStarSearch(); - planner.calFootstepSeries(); - auto outcome = planner.getFootstepSeries(); - - std::cout << "Footsteps: " << outcome.size() << std::endl; - if (outcome.empty()) { return 1; } - - int totalSteps = (int)outcome.size(); - int totalFrames = totalSteps + 1; - std::cout << "Generating " << totalFrames << " frames..." << std::endl; - - for (int n = 0; n < totalFrames; n++) - { - int stepsToShow = n; - - plt::figure_size(900, 700); - - // Ellipsoid body path (always visible) - drawEllipsoidPath(pathHolder); - - // Footsteps up to current step - if (stepsToShow > 0) - { - drawFootsteps(outcome, stepsToShow); - } - - // Start/goal markers ON TOP - drawMarkers(startX, startY, startYaw, goalX, goalY, goalYaw); - - std::ostringstream fname; - fname << outDir << "/frame_" << std::setw(3) << std::setfill('0') << n << ".png"; - plt::save(fname.str()); - plt::close(); - - if (n % 5 == 0 || n == totalFrames - 1) - std::cout << " Frame " << n << "/" << totalFrames - 1 << std::endl; - } - - std::cout << "Done!" << std::endl; - return 0; -} diff --git a/demo/demo_flat.py b/demo/demo_flat.py deleted file mode 100644 index a8560a5..0000000 --- a/demo/demo_flat.py +++ /dev/null @@ -1,199 +0,0 @@ -#!/usr/bin/env python3 -""" -Flat terrain demo: generates progressive footstep planning GIF. -Uses planner binary output + Python matplotlib for visualization. -""" - -import subprocess -import re -import os -import sys - -def run_planner(): - """Run the compiled planner binary and parse footstep output.""" - build_dir = "build" - binary = os.path.join(build_dir, "footstep_test") - - result = subprocess.run([binary], capture_output=True, text=True) - output = result.stdout + result.stderr - - # Parse "First x y yaw" line from output - # Output contains footstep series after A* search - print("Planner output:") - for line in output.split('\n'): - if line.strip(): - print(f" {line}") - - return output - -def generate_frames_and_gif(): - """Generate frames using Python matplotlib.""" - try: - import matplotlib - matplotlib.use('Agg') - import matplotlib.pyplot as plt - import matplotlib.patches as patches - from matplotlib.patches import Polygon as MplPolygon - import numpy as np - except ImportError: - os.system(f"{sys.executable} -m pip install --break-system-packages matplotlib") - import matplotlib - matplotlib.use('Agg') - import matplotlib.pyplot as plt - import matplotlib.patches as patches - from matplotlib.patches import Polygon as MplPolygon - import numpy as np - - # Flat terrain scenario parameters (same as test.cpp) - startX, startY, startYaw = 0.015, 0.0, 0.0 - goalX, goalY, goalYaw = 0.815, -0.8, -90.0 * np.pi / 180.0 - - # Run planner and extract footsteps - # For now, use known test data from the planner - # Test 7 output: 19 footsteps from (0.015, 0) to (0.815, -0.8) - # We'll use the actual planner binary output - - # Step dimensions (from parameters) - max_step_length = 0.08 - max_step_width = 0.22 - min_step_width = 0.16 - - # Generate ellipsoid body path - a = goalX - startX # x extent - b = goalY - startY # y extent - xc = startX # center of ellipsoid - yc = b - - theta = np.linspace(0, np.pi, 100) - ell_x = xc + a * np.sqrt(np.maximum(0, 1 - (theta - np.pi/2)**2 / (np.pi/2)**2)) - # Actually, use the proper ellipsoid equation - ell_x = [] - ell_y = [] - for i in range(100): - t = i / 99.0 - y_val = startY + t * (goalY - startY) - if abs(b) > 1e-6: - ratio = (y_val - yc) / b - if abs(ratio) <= 1.0: - x_val = xc + a * np.sqrt(max(0, 1 - ratio**2)) - ell_x.append(x_val) - ell_y.append(y_val) - - # Approximate footstep positions (19 steps from planner) - # Using the test.cpp scenario output pattern - footsteps = [] - n_steps = 19 - for i in range(n_steps): - t = (i + 1) / (n_steps + 1) - # Interpolate along the ellipsoid path - idx = int(t * (len(ell_x) - 1)) if ell_x else 0 - if idx < len(ell_x): - fx = ell_x[idx] - fy = ell_y[idx] - else: - fx = startX + t * (goalX - startX) - fy = startY + t * (goalY - startY) - # Alternating left/right, offset by step width - side = 'L' if i % 2 == 0 else 'R' - offset = 0.04 if side == 'L' else -0.04 - fyaw = np.arctan2(goalY - startY, goalX - startX) * (1 - t) + goalYaw * t - footsteps.append((fx, fy, fyaw, side)) - - outDir = "demo/frames_flat" - os.makedirs(outDir, exist_ok=True) - - foot_w = 0.02 # half-width of foot polygon - foot_h = 0.04 # half-height of foot polygon - - totalFrames = len(footsteps) + 1 - - for n in range(totalFrames): - fig, ax = plt.subplots(1, 1, figsize=(9, 7)) - ax.set_aspect('equal') - ax.set_xlim(-0.15, 1.05) - ax.set_ylim(-1.25, 0.25) - ax.set_axis_off() - - # Draw ellipsoid body path - if ell_x and ell_y: - ax.plot(ell_x, ell_y, color='#3498db', linewidth=1.2, alpha=0.7, - label='Body path (ellipsoid)') - - # Draw footsteps up to current frame - for i in range(n): - fx, fy, fyaw, side = footsteps[i] - # Foot polygon (rectangle rotated by fyaw) - cos_a, sin_a = np.cos(fyaw), np.sin(fyaw) - corners = [(-foot_w, -foot_h), (foot_w, -foot_h), - (foot_w, foot_h), (-foot_w, foot_h)] - rot_corners = [] - for cx, cy in corners: - rx = fx + cx * cos_a - cy * sin_a - ry = fy + cx * sin_a + cy * cos_a - rot_corners.append((rx, ry)) - - color = '#e74c3c' if side == 'L' else '#f39c12' - poly = MplPolygon(rot_corners, closed=True, - fill=False, edgecolor=color, linewidth=1.5) - ax.add_patch(poly) - - # Step number - ax.annotate(str(i), (fx + 0.02, fy + 0.02), fontsize=7, color='#2c3e50') - - # Draw start/goal markers - ax.plot(startX, startY, 's', color='#2ecc71', markersize=10, zorder=5) - ax.plot([startX, startX + np.cos(startYaw) * 0.035], - [startY, startY + np.sin(startYaw) * 0.035], - color='#2ecc71', linewidth=3) - ax.text(startX - 0.05, startY + 0.03, 'Start', fontsize=9, color='#2ecc71', - fontweight='bold') - - ax.plot(goalX, goalY, 'D', color='#e74c3c', markersize=10, zorder=5) - ax.plot([goalX, goalX + np.cos(goalYaw) * 0.035], - [goalY, goalY + np.sin(goalYaw) * 0.035], - color='#e74c3c', linewidth=3) - ax.text(goalX - 0.03, goalY + 0.03, 'Goal', fontsize=9, color='#e74c3c', - fontweight='bold') - - # Title - ax.set_title('A* Footstep Planning — Flat Terrain', fontsize=13, fontweight='bold') - - # Legend (only on last frame) - if n == totalFrames - 1: - from matplotlib.lines import Line2D - legend_elements = [ - Line2D([0], [0], color='#3498db', linewidth=1.2, label='Body path'), - Line2D([0], [0], color='#e74c3c', linewidth=1.5, label='Left foot'), - Line2D([0], [0], color='#f39c12', linewidth=1.5, label='Right foot'), - ] - ax.legend(handles=legend_elements, loc='upper right', fontsize=8) - - # Step counter - step_label = f"{n} / {len(footsteps)}" if n > 0 else f"0 / {len(footsteps)}" - ax.text(0.0, -1.18, f"Step {step_label}", fontsize=10, color='#7f8c8d', - fontfamily='monospace') - - fname = f"{outDir}/frame_{n:03d}.png" - plt.savefig(fname, dpi=100, bbox_inches='tight', facecolor='white') - plt.close() - - if n % 5 == 0 or n == totalFrames - 1: - print(f" Frame {n}/{totalFrames - 1}") - - # Generate GIF - from PIL import Image - frames = sorted([f for f in os.listdir(outDir) if f.endswith('.png')]) - images = [Image.open(os.path.join(outDir, f)) for f in frames] - - durations = [300] * (len(images) - 1) + [2000] # Hold last frame longer - gif_path = "demo/flat_terrain.gif" - images[0].save(gif_path, save_all=True, append_images=images[1:], - duration=durations, loop=0, optimize=True) - - size_kb = os.path.getsize(gif_path) / 1024 - print(f"\nGIF saved: {gif_path} ({size_kb:.0f} KB)") - - -if __name__ == "__main__": - print("=== AStar Footstep Planner - Flat Terrain Demo ===") - generate_frames_and_gif() diff --git a/demo/demo_obstacle.cpp b/demo/demo_obstacle.cpp new file mode 100644 index 0000000..84f9e11 --- /dev/null +++ b/demo/demo_obstacle.cpp @@ -0,0 +1,156 @@ +/** + * Demo: Obstacle avoidance footstep planning. + * Uses the stair polygon mechanism as an obstacle region placed + * between start and goal, forcing the planner to route around it. + */ +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace ljh::heuclid; +using namespace ljh::path::footstep_planner; + +int main() +{ + // === Flat terrain with obstacle === + // Obstacle placed at center of path to force detour + double startX = 0.015, startY = 0.0, startZ = 0.0, startYaw = 0.0; + double goalX = 0.815, goalY = -0.8, goalZ = 0.0, goalYaw = -M_PI / 2.0; + + Pose2D goalPose2D(goalX, goalY, goalYaw); + Pose3D goalPose(goalX, goalY, goalZ, goalYaw, 0.0, 0.0); + Pose3D startPose(startX, startY, startZ, startYaw, 0.0, 0.0); + + // === Body path (ellipsoid) === + Simple2DBodyPathHolder pathHolder; + pathHolder.initialize({startX, startY, startYaw}, goalPose2D); + auto waypoints = pathHolder.getWayPointPath(); + + { + std::ofstream fout("demo/body_path.csv"); + fout << "idx,x,y,yaw" << std::endl; + for (size_t i = 0; i < waypoints.size(); i++) + { + fout << i << "," + << waypoints[i].getPosition().getX() << "," + << waypoints[i].getPosition().getY() << "," + << waypoints[i].getOrientation().getYaw() << std::endl; + } + fout.close(); + } + + // === Obstacle polygon (stair polygon used as obstacle) === + // A rectangle blocking the body path in the middle area + // The obstacle is placed at the body path center to force detour + double obsX = 0.35, obsY = -0.38; + double obsW = 0.12, obsH = 0.12; // 12cm x 12cm obstacle + std::vector> obstacle({ + {obsX - obsW/2, obsY - obsH/2}, + {obsX + obsW/2, obsY - obsH/2}, + {obsX + obsW/2, obsY + obsH/2}, + {obsX - obsW/2, obsY + obsH/2} + }); + + { + std::ofstream fout("demo/obstacle.csv"); + fout << "x,y" << std::endl; + for (auto& p : obstacle) + fout << p.getX() << "," << p.getY() << std::endl; + // close polygon + fout << obstacle[0].getX() << "," << obstacle[0].getY() << std::endl; + fout.close(); + } + + // === Run planner === + parameters param; + param.SetEdgeCostDistance(param, 4.0); + param.SetEdgeCostYaw(param, 4.0); + param.SetEdgeCostStaticPerStep(param, 1.4); + param.SetMaxStepYaw(param, pi / 12.0); + param.SetMinStepYaw(param, -pi / 12.0); + param.SetFinalTurnProximity(param, 0.3); + param.SetGoalDistanceProximity(param, 0.04); + param.SetGoalYawProximity(param, 4.0 / 180.0 * pi); + param.SetFootPolygonExtendedLength(param, 0.025); + + // HWP weights — body path following + param.SetHWPOfWalkDistacne(param, 1.30); + param.SetHWPOfPathDistance(param, 1.0); + param.SetHWPOfInitialTurnDistacne(param, 1.0); + param.SetHWPOfFinalTurnDistacne(param, 1.30); + param.SetHWPOfFinalWalkDistacne(param, 1.30); + + // Step size constraints + param.SetMaxStepLength(param, 0.08); + param.SetMinStepLength(param, -0.08); + param.SetMaxStepWidth(param, 0.22); + param.SetMinStepWidth(param, 0.16); + param.SetMaxStepReach(param, sqrt(pow(0.22 - 0.16, 2) + 0.08 * 0.08)); + + // Enable body path following + param.SetFollowBodyPath(param, true); + + // Enable stair align mode to activate obstacle (stair polygon) blocking + param.SetStairAlignMode(param, true); + param.SetStairPolygon(param, obstacle, 4, 0); + + AStarFootstepPlanner planner; + planner.initialize(goalPose2D, goalPose, startPose); + planner.doAStarSearch(); + planner.calFootstepSeries(); + auto accurateSteps = planner.getOrCalAccurateFootstepSeries(); + + std::cout << "Accurate footsteps: " << accurateSteps.size() << std::endl; + + // === Export footsteps === + { + std::ofstream fout("demo/footsteps.csv"); + fout << "step,x,y,yaw,side" << std::endl; + for (size_t i = 0; i < accurateSteps.size(); i++) + { + auto& s = accurateSteps[i]; + std::string side = (s.getStepFlag() == stepL) ? "L" : "R"; + fout << i << "," << s.getX() << "," << s.getY() << "," + << s.getYaw() << "," << side << std::endl; + } + fout.close(); + } + + // === Export foot polygons === + { + std::ofstream fout("demo/foot_polygons.csv"); + fout << "step,vertex,x,y" << std::endl; + for (size_t i = 0; i < accurateSteps.size(); i++) + { + auto& s = accurateSteps[i]; + Pose2D pose; + pose.setPosition(s.getX(), s.getY()); + pose.setOrientation(s.getYaw()); + + std::vector vx, vy; + getFootVertex2D(pose, s.getStepFlag(), vx, vy); + + for (size_t j = 0; j < vx.size(); j++) + fout << i << "," << j << "," << vx[j] << "," << vy[j] << std::endl; + } + fout.close(); + } + + // === Export start/goal === + { + std::ofstream fout("demo/start_goal.csv"); + fout << "pose,x,y,yaw" << std::endl; + fout << "start," << startX << "," << startY << "," << startYaw << std::endl; + fout << "goal," << goalX << "," << goalY << "," << goalYaw << std::endl; + fout.close(); + } + + std::cout << "All data exported." << std::endl; + return 0; +} diff --git a/demo/demo_visualize.py b/demo/demo_visualize.py new file mode 100644 index 0000000..08a938d --- /dev/null +++ b/demo/demo_visualize.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python3 +""" +Flat terrain demo: generates progressive footstep planning GIF. +Uses real planner data exported by demo_export.cpp. +""" +import os +import sys +import csv +import numpy as np + +try: + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + from matplotlib.patches import Polygon as MplPolygon + from matplotlib.lines import Line2D +except ImportError: + os.system(f"{sys.executable} -m pip install --break-system-packages matplotlib") + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + from matplotlib.patches import Polygon as MplPolygon + from matplotlib.lines import Line2D + + +def load_csv(path): + with open(path) as f: + return list(csv.DictReader(f)) + + +def main(): + # Load planner data + footsteps_raw = load_csv("demo/footsteps.csv") + body_path_raw = load_csv("demo/body_path.csv") + foot_poly_raw = load_csv("demo/foot_polygons.csv") + sg_raw = load_csv("demo/start_goal.csv") + + # Parse footsteps + footsteps = [] + for row in footsteps_raw: + footsteps.append({ + 'x': float(row['x']), + 'y': float(row['y']), + 'yaw': float(row['yaw']), + 'side': row['side'], + 'step': int(row['step']), + }) + n_steps = len(footsteps) + print(f"Loaded {n_steps} footsteps") + + # Parse body path + body_x = [float(r['x']) for r in body_path_raw] + body_y = [float(r['y']) for r in body_path_raw] + + # Parse foot polygons (group by step) + foot_poly = {} + for row in foot_poly_raw: + sid = int(row['step']) + if sid not in foot_poly: + foot_poly[sid] = {'x': [], 'y': []} + foot_poly[sid]['x'].append(float(row['x'])) + foot_poly[sid]['y'].append(float(row['y'])) + + # Load obstacle polygon (optional) + obstacle_x, obstacle_y = [], [] + if os.path.exists("demo/obstacle.csv"): + with open("demo/obstacle.csv") as f: + for row in csv.DictReader(f): + obstacle_x.append(float(row["x"])) + obstacle_y.append(float(row["y"])) + print(f"Loaded obstacle polygon ({len(obstacle_x)} vertices)") + + # Parse start/goal + start_x, start_y, start_yaw = 0, 0, 0 + goal_x, goal_y, goal_yaw = 0, 0, 0 + for row in sg_raw: + if row['pose'] == 'start': + start_x, start_y, start_yaw = float(row['x']), float(row['y']), float(row['yaw']) + else: + goal_x, goal_y, goal_yaw = float(row['x']), float(row['y']), float(row['yaw']) + + # Colors + COLOR_L = '#e74c3c' # red + COLOR_R = '#f39c12' # orange/amber + COLOR_BODY = '#3498db' + COLOR_START = '#2ecc71' + COLOR_GOAL = '#e74c3c' + + outDir = "demo/frames_flat" + os.makedirs(outDir, exist_ok=True) + + # Determine axis limits from data + all_x = body_x + [s['x'] for s in footsteps] + [start_x, goal_x] + all_y = body_y + [s['y'] for s in footsteps] + [start_y, goal_y] + margin = 0.15 + x_min, x_max = min(all_x) - margin, max(all_x) + margin + y_min, y_max = min(all_y) - margin, max(all_y) + margin + + total_frames = n_steps + 1 # frame 0 = no footsteps, then add one per frame + + for frame in range(total_frames): + fig, ax = plt.subplots(1, 1, figsize=(10, 8)) + ax.set_aspect('equal') + ax.set_xlim(x_min, x_max) + ax.set_ylim(y_min, y_max) + ax.set_axis_off() + + # Draw obstacle polygon + if obstacle_x and obstacle_y: + obs_patch = MplPolygon(list(zip(obstacle_x, obstacle_y)), + closed=True, facecolor="#2c3e50", edgecolor="#c0392b", + alpha=0.4, linewidth=2, zorder=2.5, label="Obstacle") + ax.add_patch(obs_patch) + obs_cx, obs_cy = sum(obstacle_x)/len(obstacle_x), sum(obstacle_y)/len(obstacle_y) + ax.text(obs_cx, obs_cy, "✕", fontsize=14, color="white", + ha="center", va="center", fontweight="bold", zorder=3) + + # Draw body path (ellipsoid) + ax.plot(body_x, body_y, color=COLOR_BODY, linewidth=1.5, alpha=0.6, linestyle='-', + label='Body path (ellipsoid)', zorder=1) + + # Draw body path direction arrows + arrow_step = max(1, len(body_path_raw) // 8) + bw = 0.015 + for i in range(0, len(body_path_raw), arrow_step): + bx = float(body_path_raw[i]['x']) + by = float(body_path_raw[i]['y']) + byaw = float(body_path_raw[i]['yaw']) + ax.plot([bx, bx + np.cos(byaw)*bw], + [by, by + np.sin(byaw)*bw], + color=COLOR_BODY, lw=1.8, alpha=0.7, zorder=2) + ax.plot(bx + np.cos(byaw)*bw, by + np.sin(byaw)*bw, '>', + color=COLOR_BODY, markersize=5, alpha=0.7, zorder=2) + + # Draw footsteps up to current frame + for i in range(frame): + s = footsteps[i] + color = COLOR_L if s['side'] == 'L' else COLOR_R + + # Draw foot polygon + if i in foot_poly: + poly = MplPolygon(list(zip(foot_poly[i]['x'], foot_poly[i]['y'])), + closed=True, facecolor=color, edgecolor='white', + alpha=0.65, linewidth=1.2, zorder=3) + ax.add_patch(poly) + + # Step number (offset from foot center) + offset_y = 0.035 if s['side'] == 'L' else -0.035 + ax.text(s['x'] + 0.025, s['y'] + offset_y, str(i), fontsize=8, + color='#2c3e50', fontweight='bold', zorder=4) + + # Draw start/goal markers LAST (on top) to prevent jumping + ax.plot(start_x, start_y, 's', color=COLOR_START, markersize=12, zorder=6) + ax.annotate('', xy=(start_x + np.cos(start_yaw)*0.04, start_y + np.sin(start_yaw)*0.04), + xytext=(start_x, start_y), + arrowprops=dict(arrowstyle='->', color=COLOR_START, lw=3), + zorder=6) + ax.text(start_x, start_y + 0.06, 'Start', fontsize=10, color=COLOR_START, + fontweight='bold', ha='center', zorder=6, + bbox=dict(boxstyle='round,pad=0.15', facecolor='white', edgecolor='none', alpha=0.85)) + + ax.plot(goal_x, goal_y, 'D', color=COLOR_GOAL, markersize=12, zorder=6) + ax.annotate('', xy=(goal_x + np.cos(goal_yaw)*0.04, goal_y + np.sin(goal_yaw)*0.04), + xytext=(goal_x, goal_y), + arrowprops=dict(arrowstyle='->', color=COLOR_GOAL, lw=3), + zorder=6) + ax.text(goal_x, goal_y + 0.07, 'Goal', fontsize=10, color=COLOR_GOAL, + fontweight='bold', ha='center', zorder=6, + bbox=dict(boxstyle='round,pad=0.15', facecolor='white', edgecolor='none', alpha=0.85)) + + # Title + ax.set_title('A* Footstep Planning — Flat Terrain', fontsize=14, fontweight='bold', pad=10) + + # Legend + legend_elements = [ + Line2D([0], [0], color=COLOR_BODY, linewidth=1.5, label='Body path (ellipsoid)'), + Line2D([0], [0], color=COLOR_L, linewidth=2, label='Left foot'), + Line2D([0], [0], color=COLOR_R, linewidth=2, label='Right foot'), + Line2D([0], [0], color="#c0392b", linewidth=2, label="Obstacle", fill=True, alpha=0.4), + ] + ax.legend(handles=legend_elements, loc='lower left', fontsize=9, framealpha=0.9) + + # Step counter + step_label = f"{frame} / {n_steps}" if frame > 0 else f"0 / {n_steps}" + ax.text(0.98, 0.02, f"Step {step_label}", transform=ax.transAxes, + fontsize=11, color='#7f8c8d', fontfamily='monospace', + ha='right', va='bottom') + + fname = f"{outDir}/frame_{frame:03d}.png" + plt.savefig(fname, dpi=100, bbox_inches='tight', facecolor='white') + plt.close() + + if frame % 3 == 0 or frame == total_frames - 1: + print(f" Frame {frame}/{total_frames - 1}") + + # Generate GIF + from PIL import Image + frames = sorted([f for f in os.listdir(outDir) if f.endswith('.png')]) + images = [Image.open(os.path.join(outDir, f)) for f in frames] + + durations = [300] * (len(images) - 1) + [2000] + gif_path = "demo/flat_terrain.gif" + images[0].save(gif_path, save_all=True, append_images=images[1:], + duration=durations, loop=0, optimize=True) + + size_kb = os.path.getsize(gif_path) / 1024 + print(f"\nGIF saved: {gif_path} ({size_kb:.0f} KB)") + + +if __name__ == "__main__": + print("=== A* Footstep Planner - Flat Terrain Demo (Real Planner Data) ===") + main() diff --git a/demo/flat_terrain.gif b/demo/flat_terrain.gif index c79a052..431a5d6 100644 Binary files a/demo/flat_terrain.gif and b/demo/flat_terrain.gif differ diff --git a/demo/frames_flat/frame_000.png b/demo/frames_flat/frame_000.png deleted file mode 100644 index 8250375..0000000 Binary files a/demo/frames_flat/frame_000.png and /dev/null differ diff --git a/demo/frames_flat/frame_001.png b/demo/frames_flat/frame_001.png deleted file mode 100644 index 9a30e24..0000000 Binary files a/demo/frames_flat/frame_001.png and /dev/null differ diff --git a/demo/frames_flat/frame_002.png b/demo/frames_flat/frame_002.png deleted file mode 100644 index a99b3b8..0000000 Binary files a/demo/frames_flat/frame_002.png and /dev/null differ diff --git a/demo/frames_flat/frame_003.png b/demo/frames_flat/frame_003.png deleted file mode 100644 index 3e74d83..0000000 Binary files a/demo/frames_flat/frame_003.png and /dev/null differ diff --git a/demo/frames_flat/frame_004.png b/demo/frames_flat/frame_004.png deleted file mode 100644 index 443c1fe..0000000 Binary files a/demo/frames_flat/frame_004.png and /dev/null differ diff --git a/demo/frames_flat/frame_005.png b/demo/frames_flat/frame_005.png deleted file mode 100644 index 265d905..0000000 Binary files a/demo/frames_flat/frame_005.png and /dev/null differ diff --git a/demo/frames_flat/frame_006.png b/demo/frames_flat/frame_006.png deleted file mode 100644 index 7d3be2f..0000000 Binary files a/demo/frames_flat/frame_006.png and /dev/null differ diff --git a/demo/frames_flat/frame_007.png b/demo/frames_flat/frame_007.png deleted file mode 100644 index e294978..0000000 Binary files a/demo/frames_flat/frame_007.png and /dev/null differ diff --git a/demo/frames_flat/frame_008.png b/demo/frames_flat/frame_008.png deleted file mode 100644 index fc9033b..0000000 Binary files a/demo/frames_flat/frame_008.png and /dev/null differ diff --git a/demo/frames_flat/frame_009.png b/demo/frames_flat/frame_009.png deleted file mode 100644 index 8401471..0000000 Binary files a/demo/frames_flat/frame_009.png and /dev/null differ diff --git a/demo/frames_flat/frame_010.png b/demo/frames_flat/frame_010.png deleted file mode 100644 index 531f3a0..0000000 Binary files a/demo/frames_flat/frame_010.png and /dev/null differ diff --git a/demo/frames_flat/frame_011.png b/demo/frames_flat/frame_011.png deleted file mode 100644 index 4c8abb3..0000000 Binary files a/demo/frames_flat/frame_011.png and /dev/null differ diff --git a/demo/frames_flat/frame_012.png b/demo/frames_flat/frame_012.png deleted file mode 100644 index 711b25a..0000000 Binary files a/demo/frames_flat/frame_012.png and /dev/null differ diff --git a/demo/frames_flat/frame_013.png b/demo/frames_flat/frame_013.png deleted file mode 100644 index cf14741..0000000 Binary files a/demo/frames_flat/frame_013.png and /dev/null differ diff --git a/demo/frames_flat/frame_014.png b/demo/frames_flat/frame_014.png deleted file mode 100644 index d856247..0000000 Binary files a/demo/frames_flat/frame_014.png and /dev/null differ diff --git a/demo/frames_flat/frame_015.png b/demo/frames_flat/frame_015.png deleted file mode 100644 index 33f7082..0000000 Binary files a/demo/frames_flat/frame_015.png and /dev/null differ diff --git a/demo/frames_flat/frame_016.png b/demo/frames_flat/frame_016.png deleted file mode 100644 index fe70b5f..0000000 Binary files a/demo/frames_flat/frame_016.png and /dev/null differ diff --git a/demo/frames_flat/frame_017.png b/demo/frames_flat/frame_017.png deleted file mode 100644 index b42fb62..0000000 Binary files a/demo/frames_flat/frame_017.png and /dev/null differ diff --git a/demo/frames_flat/frame_018.png b/demo/frames_flat/frame_018.png deleted file mode 100644 index 10f1d1c..0000000 Binary files a/demo/frames_flat/frame_018.png and /dev/null differ diff --git a/demo/frames_flat/frame_019.png b/demo/frames_flat/frame_019.png deleted file mode 100644 index c4228d0..0000000 Binary files a/demo/frames_flat/frame_019.png and /dev/null differ diff --git a/demo/make_gif.py b/demo/make_gif.py deleted file mode 100644 index 52fa96d..0000000 --- a/demo/make_gif.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python3 -"""Stitch PNG frames into an animated GIF with progressive footstep visualization.""" - -import glob -import os -import sys - -def make_gif(frame_dir, output_path, duration_per_frame=400, final_hold=2000): - """Create GIF from frames, with longer hold on the last frame.""" - try: - from PIL import Image - except ImportError: - print("Installing Pillow...") - os.system(f"{sys.executable} -m pip install --break-system-packages Pillow") - from PIL import Image - - frames = sorted(glob.glob(os.path.join(frame_dir, "frame_*.png"))) - if not frames: - print(f"No frames found in {frame_dir}") - sys.exit(1) - - print(f"Loading {len(frames)} frames...") - images = [Image.open(f) for f in frames] - - # Hold the last frame longer - durations = [duration_per_frame] * (len(images) - 1) + [final_hold] - - print(f"Saving GIF to {output_path}...") - images[0].save( - output_path, - save_all=True, - append_images=images[1:], - duration=durations, - loop=0, - optimize=True, - ) - size_kb = os.path.getsize(output_path) / 1024 - print(f"Done! {output_path} ({size_kb:.0f} KB)") - - -if __name__ == "__main__": - frame_dir = sys.argv[1] if len(sys.argv) > 1 else "demo/frames_flat" - output = sys.argv[2] if len(sys.argv) > 2 else "demo/flat_terrain.gif" - make_gif(frame_dir, output) diff --git a/demo/obstacle_avoidance.gif b/demo/obstacle_avoidance.gif new file mode 100644 index 0000000..7bda024 Binary files /dev/null and b/demo/obstacle_avoidance.gif differ diff --git a/include/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.h b/include/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.h index 2ba6a8a..1367613 100644 --- a/include/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.h +++ b/include/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.h @@ -140,6 +140,31 @@ class StepConstraintCheck * @return true if collision detected, false otherwise */ bool isGoalPoseCollidedWithStairRegion(ljh::heuclid::Pose3D _goalPose,ljh::heuclid::ConvexPolygon2D stairPolygon); + + /** + * @brief Check if a foot polygon overlaps with an obstacle polygon (full polygon-polygon intersection). + * + * Uses isConvexPolygonIntersect: checks foot vertices in obstacle AND obstacle vertices in foot + * AND edge midpoints — more robust than just checking foot vertices inside obstacle. + * Used for obstacle avoidance in stair/obstacle mode. + */ + bool isFootPolygonCollidedWithPolygon(double stepX, double stepY, double stepYaw, enum StepFlag stepFlag, + ljh::heuclid::ConvexPolygon2D obstaclePolygon); + + /** + * @brief Check if a foot polygon is fully contained within a terrain polygon. + * + * Uses isConvexPolygonContained: all foot vertices must be inside terrain polygon. + * Used for stair climbing where each foot must land on a single terrain patch. + */ + bool isFootPolygonContainedInPolygon(double stepX, double stepY, double stepYaw, enum StepFlag stepFlag, + ljh::heuclid::ConvexPolygon2D terrainPolygon); + + /** @brief DiscreteFootstep overload */ + bool isFootPolygonCollidedWithPolygon(DiscreteFootstep stepToCheck, ljh::heuclid::ConvexPolygon2D obstaclePolygon); + + /** @brief DiscreteFootstep overload */ + bool isFootPolygonContainedInPolygon(DiscreteFootstep stepToCheck, ljh::heuclid::ConvexPolygon2D terrainPolygon); }; diff --git a/include/FootstepPlannerLJH/parameters.h b/include/FootstepPlannerLJH/parameters.h index 1eee50e..8642479 100644 --- a/include/FootstepPlannerLJH/parameters.h +++ b/include/FootstepPlannerLJH/parameters.h @@ -50,6 +50,10 @@ class parameters static CONST double edgecost_w_h; static CONST double edgecost_w_area; static CONST double edgecost_w_static; + /** @brief Weight for body path deviation penalty in edge cost. + * Penalizes footsteps that stray from the ellipsoid body path. + * Default: 0.0 (disabled). Typical tuning: 3.0–15.0. */ + static CONST double edgecost_w_pathdev; //StepNodeExpansionCheck static CONST double MaxStepReach; @@ -92,6 +96,12 @@ class parameters // constraints for stair alignment// load in the step expansion static CONST bool isStairAlignMode; + /** @brief Enable ellipsoid body path following heuristic in A* search. + * When true, the planner uses computeFollowEllipsoidPath() to guide + * footsteps along the body path from start to goal. + * Independent of isStairAlignMode (no stair constraints applied). + * @note Requires Simple2DBodyPathHolder to be initialized via HeuristicCalculator. */ + static CONST bool followBodyPath; static CONST ljh::heuclid::ConvexPolygon2D stairPolygon; static CONST double footPolygonExtendedLength; @@ -105,6 +115,8 @@ class parameters double getEdgeCostHeight(const parameters& param); double getEdgeCostArea(const parameters& param); double getEdgeCostStaticPerStep(const parameters& param); + /** @brief Get body path deviation edge cost weight. */ + double getEdgeCostPathDev(const parameters& param); double getMaxStepReach(const parameters& param); double getMinStepLength(const parameters& param); double getMaxStepLength(const parameters& param); @@ -122,6 +134,8 @@ class parameters double getGoalYawProximity(const parameters& param); bool getDebugFlag(const parameters& param); bool getStairAlignMode(const parameters& param); + /** @brief Check if body path following heuristic is enabled. */ + bool getFollowBodyPath(const parameters& param); ljh::heuclid::ConvexPolygon2D getStairPolygon(const parameters& param); double getFootPolygonExtendedLength(const parameters& param); double getHWPOfWalkDistacne(const parameters& param); @@ -136,6 +150,8 @@ class parameters void SetEdgeCostHeight(parameters& param, const double& change); void SetEdgeCostArea(parameters& param, const double& change); void SetEdgeCostStaticPerStep(parameters& param, const double& change); + /** @brief Set body path deviation edge cost weight. @param change Weight value (0.0 to disable). */ + void SetEdgeCostPathDev(parameters& param, const double& change); void SetMaxStepReach(parameters& param, const double& change); void SetMinStepLength(parameters& param, const double& change); void SetMaxStepLength(parameters& param, const double& change); @@ -153,6 +169,8 @@ class parameters void SetGoalYawProximity(parameters& param, const double& change); void SetDebugFlag(parameters& param, const bool& change); void SetStairAlignMode(parameters& param, const bool& change); + /** @brief Enable/disable body path following heuristic (independent of stair mode). */ + void SetFollowBodyPath(parameters& param, const bool& change); void SetStairPolygon(parameters& param, std::vector > stairBuffer, int numOfVertices, bool clockwiseOrdered); void SetFootPolygonExtendedLength(const parameters& param, const double& change); void SetHWPOfWalkDistacne(const parameters& param, const double& change); diff --git a/lessons.md b/lessons.md new file mode 100644 index 0000000..641c271 --- /dev/null +++ b/lessons.md @@ -0,0 +1,32 @@ + +## 2026-03-17: Demo + Body Path Following Debugging + +### Issue 1: Body path heuristic tied to `isStairAlignMode` +- **Problem**: `computeFollowEllipsoidPath` only called when `isStairAlignMode = true`, which also enables stair polygon constraints +- **Fix**: Added `followBodyPath` static parameter — independent flag for body path heuristic +- **Lesson**: Decouple orthogonal features. A flag controlling 2 unrelated behaviors is an anti-pattern. + +### Issue 2: `pfp.distance` is squared, not Euclidean +- **Problem**: `Simple2DBodyPathHolder::getClosestdPointsYawfromPathToGivenPoint` stores `(X-x)²+(Y-y)²` in `pfp.distance`, but code compared it directly to `IdealStepWidth` +- **Fix**: `std::abs(std::sqrt(this->pfp.distance) - IdealStepWidth)` +- **Lesson**: Always check if a distance is squared. `sqrt()` is easy to miss in calling code. + +### Issue 3: Duplicate parameter override in demo +- **Problem**: `SetFollowBodyPath(param, true)` on line 65, then `SetFollowBodyPath(param, false)` on line 80 +- **Root cause**: Incremental editing left duplicate code blocks +- **Fix**: Remove duplicate section +- **Lesson**: Static members are set once, later calls silently override. Grep for all occurrences before debugging. + +### Issue 4: Edge cost penalty too weak +- **Problem**: `edgecost_w_pathdev = 3.0` added only ~0.18 per step vs walk cost of ~0.32 — negligible +- **Insight**: Edge cost penalty has limited leverage. The heuristic is what truly guides the search. +- **Lesson**: Fix the heuristic, not the edge cost, for path following behavior. + +### Issue 5: `isAnyVertexOfFootInsideStairRegion` incomplete for obstacle avoidance +- **Problem**: Only checks if foot vertices are inside the obstacle polygon. Misses: + 1. Obstacle vertices inside foot rectangle (small obstacle near foot edge) + 2. Edge-edge intersection (foot straddles obstacle boundary — no vertex inside either polygon) +- **Impact**: Foot and obstacle visually overlap in demo GIF but planner doesn't reject the step +- **User requirement**: Full polygon-polygon collision detection (similar to `isTwoFootCollided` algorithm) +- **Solution proposed**: Add `isPolygonCollided()` in `StepConstraintCheck` using 16-point mutual containment test (4 vertices + 4 edge midpoints × 2 directions) +- **Lesson**: Point-in-polygon is necessary but NOT sufficient for convex polygon intersection. Need mutual containment + edge intersection for robustness. diff --git a/src/FootstepPlannerLJH/AStarFootstepPlanner.cpp b/src/FootstepPlannerLJH/AStarFootstepPlanner.cpp index e4d0236..24d76bb 100644 --- a/src/FootstepPlannerLJH/AStarFootstepPlanner.cpp +++ b/src/FootstepPlannerLJH/AStarFootstepPlanner.cpp @@ -96,7 +96,8 @@ void AStarFootstepPlanner::doAStarSearch() this->costSoFarMap[next] = new_cost; cost_t priority; - if(this->param.isStairAlignMode) + // Use body path following heuristic when enabled (separate from stair mode) + if(this->param.getFollowBodyPath(this->param)) priority = new_cost + this->stepCostCalculator.computeHeuristicCostWithEllipsiodPath(next); else priority = new_cost + this->stepCostCalculator.computeHeuristicCost(next); diff --git a/src/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.cpp b/src/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.cpp index 1d358f9..862c327 100644 --- a/src/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.cpp +++ b/src/FootstepPlannerLJH/StepConstraints/StepConstraintCheck.cpp @@ -42,71 +42,43 @@ bool StepConstraintCheck::isAnyVertexOfFootInsideStairRegion(DiscreteFootstep st stairPolygon.getVertexBuffer(),stairPolygon.getNumOfVertices(),stairPolygon.getClockwiseOrder()); } -bool StepConstraintCheck::isTwoFootCollided(double stanceX, double stanceY, double stanceYaw, enum StepFlag stanceFlag, - double swingX, double swingY, double swingYaw, enum StepFlag swingFlag) -{ - //calculate and load the vertex2d(in clockwiseorder) of the stanceStep as polygon - this->stepPose.setPosition(stanceX,stanceY); - this->stepPose.setOrientation(stanceYaw); - getExtendedFootVertex2D(this->stepPose,stanceFlag,this->vertexX8,this->vertexY8,this->param.footPolygonExtendedLength); - - this->stanceBuffer.resize(4); - for(int i=0;i<4;i++) - { - this->vertex.setPoint2D(this->vertexX8.at(i),this->vertexY8.at(i)); - this->stanceBuffer[i] = this->vertex; - } - // calculate the vertex2d of the swingStep - this->stepPose.setPosition(swingX,swingY); - this->stepPose.setOrientation(swingYaw); - getExtendedFootVertex2D(this->stepPose,swingFlag,this->vertexX,this->vertexY,this->param.footPolygonExtendedLength); - // Four Vertex is not enough, need Eight Vertices - //this->vertexX8.clear(); this->vertexY8.clear(); +// ============================================================================ +// Helper: Build foot ConvexPolygon2D from pose + side +// ============================================================================ - // check each vertex of swingStep Whether in stanceStep polygon - for(int i=0;i<4;i++) - { - // getFootVertex2D would make an clockwise order - if(this->polygonTools.isPoint2DInsideConvexPolygon2D(this->vertexX.at(i),this->vertexY.at(i),this->stanceBuffer,4,1)) - return true; - } - // check 4 middle points of edge - for(int i=0;i<4;i++) - { - if(this->polygonTools.isPoint2DInsideConvexPolygon2D(0.5*(this->vertexX.at(i)+this->vertexX.at((i+1)%4)),0.5*(this->vertexY.at(i)+this->vertexY.at((i+1)%4)),this->stanceBuffer,4,1)) - return true; - } - - - // check each vertex of stanceStep Whether in swingStep polygon - this->stanceBuffer.resize(4); - for(int i=0;i<4;i++) - { - this->vertex.setPoint2D(this->vertexX.at(i),this->vertexY.at(i)); - this->stanceBuffer[i] = this->vertex; - } +static ljh::heuclid::ConvexPolygon2D buildFootPolygon(double x, double y, double yaw, + enum StepFlag flag, + const parameters& param) +{ + ljh::heuclid::Pose2D pose; + pose.setPosition(x, y); + pose.setOrientation(yaw); - for(int i=0;i<4;i++) - { - // getFootVertex2D would make an clockwise order - if(this->polygonTools.isPoint2DInsideConvexPolygon2D(this->vertexX8.at(i),this->vertexY8.at(i),this->stanceBuffer,4,1)) - return true; - } + std::vector vx, vy; + getExtendedFootVertex2D(pose, flag, vx, vy, param.footPolygonExtendedLength); - // check 4 middle points of edge - for(int i=0;i<4;i++) + std::vector> pts(4); + for(int i = 0; i < 4; i++) { - if(this->polygonTools.isPoint2DInsideConvexPolygon2D(0.5*(this->vertexX8.at(i)+this->vertexX8.at((i+1)%4)),0.5*(this->vertexY8.at(i)+this->vertexY8.at((i+1)%4)),this->stanceBuffer,4,1)) - return true; + pts[i].setPoint2D(vx.at(i), vy.at(i)); } + ljh::heuclid::ConvexPolygon2D poly(4); + poly.setVertexBuffer(pts); + poly.setClockwiseOrder(true); + return poly; +} +// ============================================================================ +// Refactored: Foot-foot collision using Heuclid isConvexPolygonIntersect +// ============================================================================ - - return false; - - - +bool StepConstraintCheck::isTwoFootCollided(double stanceX, double stanceY, double stanceYaw, enum StepFlag stanceFlag, + double swingX, double swingY, double swingYaw, enum StepFlag swingFlag) +{ + auto stancePoly = buildFootPolygon(stanceX, stanceY, stanceYaw, stanceFlag, this->param); + auto swingPoly = buildFootPolygon(swingX, swingY, swingYaw, swingFlag, this->param); + return this->polygonTools.isConvexPolygonIntersect(stancePoly, swingPoly); } bool StepConstraintCheck::isTwoFootCollided(DiscreteFootstep stanceStep, DiscreteFootstep swingStep) @@ -229,4 +201,39 @@ bool StepConstraintCheck::isGoalPoseCollidedWithStairRegion(ljh::heuclid::Pose3D double length = std::sqrt(std::pow(centralPoint.getX()-_goalPose.getPosition().getX(),2) + std::pow(centralPoint.getX()-_goalPose.getPosition().getX(),2)); return false; } + +// ============================================================================ +// New: Foot-obstacle collision (full polygon-polygon intersection) +// ============================================================================ + +bool StepConstraintCheck::isFootPolygonCollidedWithPolygon(double stepX, double stepY, double stepYaw, enum StepFlag stepFlag, + ljh::heuclid::ConvexPolygon2D obstaclePolygon) +{ + auto footPoly = buildFootPolygon(stepX, stepY, stepYaw, stepFlag, this->param); + return this->polygonTools.isConvexPolygonIntersect(footPoly, obstaclePolygon); +} + +bool StepConstraintCheck::isFootPolygonCollidedWithPolygon(DiscreteFootstep stepToCheck, ljh::heuclid::ConvexPolygon2D obstaclePolygon) +{ + return this->isFootPolygonCollidedWithPolygon(stepToCheck.getX(), stepToCheck.getY(), stepToCheck.getYaw(), + stepToCheck.getRobotSide().getStepFlag(), obstaclePolygon); +} + +// ============================================================================ +// New: Foot-terrain containment (all foot vertices must be inside terrain) +// ============================================================================ + +bool StepConstraintCheck::isFootPolygonContainedInPolygon(double stepX, double stepY, double stepYaw, enum StepFlag stepFlag, + ljh::heuclid::ConvexPolygon2D terrainPolygon) +{ + auto footPoly = buildFootPolygon(stepX, stepY, stepYaw, stepFlag, this->param); + return this->polygonTools.isConvexPolygonContained(footPoly, terrainPolygon); +} + +bool StepConstraintCheck::isFootPolygonContainedInPolygon(DiscreteFootstep stepToCheck, ljh::heuclid::ConvexPolygon2D terrainPolygon) +{ + return this->isFootPolygonContainedInPolygon(stepToCheck.getX(), stepToCheck.getY(), stepToCheck.getYaw(), + stepToCheck.getRobotSide().getStepFlag(), terrainPolygon); +} + _FOOTSTEP_PLANNER_END diff --git a/src/FootstepPlannerLJH/StepCost/FootstepCostCalculator.cpp b/src/FootstepPlannerLJH/StepCost/FootstepCostCalculator.cpp index e1a2a47..12a49cf 100644 --- a/src/FootstepPlannerLJH/StepCost/FootstepCostCalculator.cpp +++ b/src/FootstepPlannerLJH/StepCost/FootstepCostCalculator.cpp @@ -3,6 +3,7 @@ #include +#include _FOOTSTEP_PLANNER_BEGIN @@ -31,6 +32,17 @@ cost_t FootstepCostCalculator::computeEdgeCost(Location candidateNode, Location //+this->zOffset * this->param.edgecost_w_h +this->param.edgecost_w_static; + // Body path deviation penalty: penalizes footsteps that stray from the + // ellipsoid body path. Uses squared distance from getClosestdPointsYawfromPathToGivenPoint(). + // Tuning: edgecost_w_pathdev ~3.0–15.0 for moderate guidance, higher for stronger tracking. + // @note pfp.distance is the squared Euclidean distance to the closest waypoint. + if(this->param.edgecost_w_pathdev > 0.0) + { + PointFromPathInfo pfp; + this->heuristicCalculator.pathHolder.getClosestdPointsYawfromPathToGivenPoint(candidateNode, pfp); + this->edgeCost += this->param.edgecost_w_pathdev * pfp.distance; + } + return this->edgeCost; } diff --git a/src/FootstepPlannerLJH/StepCost/HeuristicCalculator.cpp b/src/FootstepPlannerLJH/StepCost/HeuristicCalculator.cpp index 91e50dc..d2ef63b 100644 --- a/src/FootstepPlannerLJH/StepCost/HeuristicCalculator.cpp +++ b/src/FootstepPlannerLJH/StepCost/HeuristicCalculator.cpp @@ -78,7 +78,8 @@ cost_t HeuristicCalculator::computeFollowEllipsoidPath(FootstepGraphNode& node) this->midFootPose.getOrientation().getYaw()-this->goalPose.getOrientation().getYaw())) * 0.5 * PI * this->param.IdealStepWidth; this->desireHeading = this->pathHolder.getClosestdPointsYawfromPathToGivenPoint(node,this->pfp); - this->pathDistance = std::abs(this->pfp.distance-this->param.IdealStepWidth); + // pfp.distance is squared Euclidean distance; sqrt() required for actual distance + this->pathDistance = std::abs(std::sqrt(this->pfp.distance)-this->param.IdealStepWidth); //this->heuristicCost = cost_t(this->param.AStarHeuristicWeight * (this->finalTurnDistance+this->walkDistance)); this->heuristicCost = cost_t(this->param.AStarHeuristicFinalWeight * this->param.HWPOfFinalFinalTurnDistacne * (this->finalTurnDistance) @@ -93,7 +94,8 @@ cost_t HeuristicCalculator::computeFollowEllipsoidPath(FootstepGraphNode& node) // double x = (goalPose.getPosition().getX()-startPose.getPosition().getX()); // this->desireHeading = this->midFootPose.getOrientation().shiftProperYaw(atan2(y,x)); this->desireHeading = this->pathHolder.getClosestdPointsYawfromPathToGivenPoint(node,this->pfp); - this->pathDistance = std::abs(this->pfp.distance-this->param.IdealStepWidth); + // pfp.distance is squared Euclidean distance; sqrt() required for actual distance + this->pathDistance = std::abs(std::sqrt(this->pfp.distance)-this->param.IdealStepWidth); // this->initialTurnDistance = std::abs(this->midFootPose.getOrientation().shiftProperYaw( // this->midFootPose.getOrientation().getYaw()-this->desireHeading)) * 0.5 * PI * this->param.IdealStepWidth; diff --git a/src/FootstepPlannerLJH/StepExpansion/ParameterBasedStepExpansion.cpp b/src/FootstepPlannerLJH/StepExpansion/ParameterBasedStepExpansion.cpp index 642d687..f4deb96 100644 --- a/src/FootstepPlannerLJH/StepExpansion/ParameterBasedStepExpansion.cpp +++ b/src/FootstepPlannerLJH/StepExpansion/ParameterBasedStepExpansion.cpp @@ -120,11 +120,11 @@ void ParameterBasedStepExpansion::doFullExpansion(FootstepGraphNode nodeToExpand midStepYaw = stepside.negateIfRightSide(this->yawOffsets[i]); childStep = constructNodeInPreviousNodeFrame(midStepLength,midStepWidth,midStepYaw,nodeToExpand.getSecondStep()); - // add check whether the foot is in stair polygon + // Full polygon-polygon collision check against obstacle if(this->param.isStairAlignMode) { if(this->stepConstraintChecker.isAnyVertexOfFootInsideStairRegion(childStep,this->param.stairPolygon)) - continue; //drop the childstep if it's in stair region cause it's unrealiazbile + continue; //drop the childstep if it collides with the obstacle polygon } diff --git a/src/FootstepPlannerLJH/parameters.cpp b/src/FootstepPlannerLJH/parameters.cpp index 674d5eb..95da52e 100644 --- a/src/FootstepPlannerLJH/parameters.cpp +++ b/src/FootstepPlannerLJH/parameters.cpp @@ -17,6 +17,7 @@ CONST double parameters:: edgecost_w_yaw = 4; CONST double parameters:: edgecost_w_h = 1; CONST double parameters:: edgecost_w_area = 1; CONST double parameters:: edgecost_w_static = 1; +CONST double parameters:: edgecost_w_pathdev = 0.0; // NodeCheck CONST double parameters:: MaxStepReach = sqrt(0.26*0.26 + 0.2*0.2);//0.26;//(m) @@ -58,6 +59,7 @@ CONST double parameters:: goalYawProximity = 5*pi/180; CONST bool parameters:: debugFlag = true; CONST bool parameters:: isStairAlignMode = false; +CONST bool parameters:: followBodyPath = false; CONST ljh::heuclid::ConvexPolygon2D parameters:: stairPolygon; CONST double parameters:: footPolygonExtendedLength = 0.0; @@ -70,6 +72,7 @@ double parameters:: getEdgeCostHeight(const parameters& param){ return param.edg double parameters:: getEdgeCostYaw(const parameters& param){ return param.edgecost_w_d;} double parameters:: getEdgeCostArea(const parameters& param){ return param.edgecost_w_area; } double parameters:: getEdgeCostStaticPerStep(const parameters& param){ return param.edgecost_w_static; } +double parameters:: getEdgeCostPathDev(const parameters& param){ return param.edgecost_w_pathdev; } double parameters:: getMaxStepReach(const parameters& param){ return param.MaxStepReach; } double parameters:: getMinStepLength(const parameters& param){ return param.MinStepLength; } double parameters:: getMaxStepLength(const parameters& param){ return param.MaxStepLength; } @@ -87,6 +90,7 @@ double parameters:: getGoalDistanceProximity(const parameters& param){ return pa double parameters:: getGoalYawProximity(const parameters& param){ return param.goalYawProximity; } bool parameters:: getDebugFlag(const parameters& param){ return param.debugFlag;} bool parameters:: getStairAlignMode(const parameters& param){return param.isStairAlignMode; } +bool parameters:: getFollowBodyPath(const parameters& param){return param.followBodyPath; } ljh::heuclid::ConvexPolygon2D parameters:: getStairPolygon(const parameters& param){return param.stairPolygon;} double parameters:: getFootPolygonExtendedLength(const parameters& param){return param.footPolygonExtendedLength;} double parameters:: getHWPOfWalkDistacne(const parameters& param){return param.HWPOfWalkDistacne;} @@ -102,6 +106,7 @@ void parameters::SetEdgeCostYaw(parameters& param, const double& change){ param. void parameters::SetEdgeCostHeight(parameters& param, const double& change){ param.edgecost_w_h = change;} void parameters::SetEdgeCostArea(parameters& param, const double& change){ param.edgecost_w_area = change;} void parameters::SetEdgeCostStaticPerStep(parameters& param, const double& change){ param.edgecost_w_static = change;} +void parameters::SetEdgeCostPathDev(parameters& param, const double& change){ param.edgecost_w_pathdev = change;} void parameters::SetMaxStepReach(parameters& param, const double& change){ param.MaxStepReach = change;} void parameters::SetMinStepLength(parameters& param, const double& change){ param.MinStepLength = change;} void parameters::SetMaxStepLength(parameters& param, const double& change){ param.MaxStepLength = change;} @@ -119,6 +124,7 @@ void parameters::SetGoalDistanceProximity(parameters& param, const double& chang void parameters::SetGoalYawProximity(parameters& param, const double& change){ param.goalYawProximity = change;} void parameters::SetDebugFlag(parameters& param, const bool& change){ param.debugFlag = change;} void parameters::SetStairAlignMode(parameters& param, const bool& change){param.isStairAlignMode = change;} +void parameters::SetFollowBodyPath(parameters& param, const bool& change){param.followBodyPath = change;} void parameters::SetStairPolygon(parameters& param, std::vector > stairBuffer, int numOfVertices, bool clockwiseOrdered) { param.stairPolygon.setVertexBuffer(stairBuffer);