Waypoint planning in clutter

The planner uses RRT-Connect to generate a collision-free path through a set of waypoints. Obstacles are represented as axis-aligned bounding boxes, which is crude but sufficient for the indoor environments the drone currently flies in.

The trajectory smoothing step takes the RRT output and fits a minimum-jerk curve through the waypoints. The tracking controller then follows this smooth trajectory. At the current loop rate the drone tracks within a few centimeters on straight segments, with larger error on tight turns.

One thing worth noting: the planner is fast enough to replan on the fly if an obstacle appears, but the smoothing step adds enough latency that a full replan takes longer than a single control cycle. For now, replanning mid-flight is not attempted.