This repository contains a lightweight C++ waypoint planner designed to generate collision-free intermediate poses (waypoints) for a robot end-effector, in environments modelled using point clouds. The planner relies on an NMPC-style (Nonlinear Model Predictive Control) approach to ensure:
- Smooth convergence from a start pose to a target pose
- Avoidance of obstacles
- Sufficient visibility of the environment (for pose estimation)
-
NMPC-Style Optimization
Generates a sequence of intermediate waypoints by minimizing a nonlinear cost function.
The cost includes:- Pose tracking error
- Collision penalty
- Visibility constraint
-
Collision-Aware
Uses PCL (Point Cloud Library) for nearest-neighbor distance checks and box/frustum culling, ensuring the path remains safe in dense, cluttered scenes. -
Visibility Constraints
Incorporates field-of-view checks so the camera can maintain a clear view of critical parts of the environment while moving. -
Dynamic Receding Horizon
Each iteration optimizes over a short horizon, then shifts it forward (like standard NMPC). This makes the planner computationally lighter and more responsive.
- Eigen (for linear algebra)
- PCL (for point cloud processing, collision checks)
- nlopt (for nonlinear optimization, if using the NLopt-based solver)
- C++17 or above (recommended)
This project is licensed under MIT License. Feel free to modify and adapt the code.