### Complete Runnable Example for VoronoiPlanner

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/hybrid_search.md

A full example demonstrating the setup of a 3D grid map with obstacles, defining start and goal points, planning a path using VoronoiPlanner, and visualizing the results. Includes necessary imports and random seeding for reproducibility.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 31], [0, 31], [0, 31]], resolution=1.0)
for i in range(75):
    rd_p = tuple(np.random.randint(0, 30, size=3))
    map_.type_map[rd_p[0], rd_p[1], :rd_p[2]] = TYPES.OBSTACLE
map_.inflate_obstacles(radius=3)

start = (25, 5, 5)
goal = (5, 25, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = VoronoiPlanner(map_=map_, start=start, goal=goal, base_planner=AStar)
path, path_info = planner.plan()
print(path)
print(path_info)
# if "voronoi_candidates" in path_info:
#     map_.type_map[path_info["voronoi_candidates"]] = TYPES.CUSTOM

vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.plot_path(path)
vis.show()
vis.close()
```

--------------------------------

### Complete Runnable Example: VoronoiPlanner

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/path_planner/hybrid_search.md

This complete example sets up a Grid map with obstacles, defines start and goal points, initializes the VoronoiPlanner with AStar as the base planner, plans a path, and visualizes the result. It includes necessary imports and random seed settings for reproducibility.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = VoronoiPlanner(map_=map_, start=start, goal=goal, base_planner=AStar)
path, path_info = planner.plan()
print(path)
print(path_info)
if "voronoi_candidates" in path_info:
    map_.type_map[path_info["voronoi_candidates"]] = TYPES.CUSTOM
    map_.type_map[start] = TYPES.START
    map_.type_map[goal] = TYPES.GOAL

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.show()
vis.close()
```

--------------------------------

### Runnable Complete Code Example

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/traj_optimizer/curve_generator.md

A complete, runnable example demonstrating path planning with Theta* and smoothing with B-Spline, including map setup, visualization, and output printing.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *
from python_motion_planning.traj_optimizer import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = ThetaStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
map_.fill_expands(path_info["expand"])

path_world = map_.path_map_to_world(path)

generator = BSpline(step=0.01, k=3, param_mode="centripetal", spline_mode="interpolation")
smooth_path, curve_info = generator.generate(path_world)
print(curve_info)

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_path(smooth_path, style="-", color="C1", map_frame=False)
vis.show()
vis.close()
```

--------------------------------

### Complete Runnable Example for RRT Path Planning

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/path_planner/sample_search.md

A complete runnable example demonstrating RRT path planning, including map setup, obstacle definition, planner initialization, path planning, and visualization.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = RRT(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_expand_tree(path_info["expand"])
vis.show()
vis.close()
```

--------------------------------

### PurePursuit Controller Example

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/controller/path_tracker.md

This snippet demonstrates the setup and execution of a PurePursuit controller for a robot in a 2D environment. It includes map generation, path planning with A*, robot initialization, controller instantiation, and simulation visualization.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = AStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
map_.fill_expands(path_info["expand"])  # for visualizing the expanded nodes

path_world = map_.path_map_to_world(path)
print(path_world)

dim = 2
env = ToySimulator(dim=dim, obstacle_grid=map_, robot_collisions=False)

robots = {
    "1": CircularRobot(dim=dim, radius=1, pose=np.array([5.5, 5.5, 0]), vel=np.zeros(3),
                action_min=np.array([-2, -2, -3.14]), action_max=np.array([2, 2, 3.14]), color="C0", text="1"),
    "2": DiffDriveRobot(dim=dim, radius=1, pose=np.array([5.5, 5.5, 0]), vel=np.zeros(3),
                action_min=np.array([-2.82, 0, -6.28]), action_max=np.array([2.82, 0, 6.28]), color="C1", text="2")
}

controllers = {}
for rid, robot in robots.items():
    obs_space, act_space = env.build_robot_spaces(robot)
    controllers[rid] = PurePursuit(obs_space, act_space, env.dt, path_world, robot_model=robot, obstacle_grid=map_, max_lin_speed=3, max_ang_speed=3.14)
    env.add_robot(rid, robot)

obs, _ = env.reset()

vis = Visualizer2D()
vis.render_toy_simulator(env, controllers, steps=300, show_traj=True, show_env_info=True, grid_kwargs={"show_esdf": False})
vis.plot_path(path, style="--", color="C4")
vis.show()

for rid in robots:
    ctrl = controllers[rid]
    print(rid, ":", vis.get_traj_info(rid, path_world, ctrl.goal, ctrl.goal_dist_tol, ctrl.goal_orient_tol))
vis.close()

```

--------------------------------

### Initialize and Plan Path with VoronoiPlanner

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/hybrid_search.md

Instantiate the VoronoiPlanner with a grid map, start, and goal points. Optionally specify a base planner and its arguments. Then, call the plan() method to get the path and path information.

```python
planner = VoronoiPlanner(map_=map_, start=start, goal=goal, base_planner=AStar)
path, path_info = planner.plan()
print(path)
print(path_info)
```

--------------------------------

### Runnable Complete Code Example

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/path_planner/graph_search.md

A complete, runnable example demonstrating A* path planning. It includes setting up the environment, defining obstacles, planning a path, and visualizing the result.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = AStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)
map_.fill_expands(path_info["expand"])  # for visualizing the expanded nodes

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.show()
vis.close()
```

--------------------------------

### ToySimulator Environment Setup

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Initializes a ToySimulator with a grid, obstacles, and two robots. Demonstrates stepping the environment with zero actions and printing robot positions.

```python
import numpy as np
from python_motion_planning import Grid, TYPES, CircularRobot, ToySimulator
from python_motion_planning.common.env.robot.diff_drive_robot import DiffDriveRobot

map_ = Grid(bounds=[[0, 20], [0, 20]])
map_.type_map[8:12, 8:12] = TYPES.OBSTACLE

env = ToySimulator(
    dim=2,
    dt=0.1,
    obstacle_grid=map_,
    friction=0.015,
    restitution=0.3,
    noise=0.01,
    max_episode_steps=500,
    robot_collisions=True,
    boundary_collisions=True,
)

# Add two robots
r1 = CircularRobot(dim=2, pos=np.array([2.0, 2.0]),  radius=0.5, color="C0", text="1")
r2 = CircularRobot(dim=2, pos=np.array([18.0, 2.0]), radius=0.5, color="C1", text="2")
env.add_robot(0, r1)
env.add_robot(1, r2)

# Step with zero action
for _ in range(5):
    obs, rewards, dones, info = env.step({
        0: np.zeros(r1.action_space.shape),
        1: np.zeros(r2.action_space.shape),
    })
    print(f"t={env.time:.1f}s | r1={r1.pos.round(2)} | r2={r2.pos.round(2)}")

print(f"Truncated: {info['truncated']}")
```

--------------------------------

### Initialize and Run A* Path Planner

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/graph_search.md

Instantiate the A* planner with the map, start, and goal points. The plan() method returns the path and detailed planning information.

```python
planner = AStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)
```

--------------------------------

### Install Python Motion Planning Package

Source: https://github.com/ai-winter/python_motion_planning/blob/master/README.md

Install the Python Motion Planning library using pip after setting up the environment.

```shell
pip install python-motion-planning
```

--------------------------------

### Runnable Example: Reeds-Shepp Path Generation

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/traj_optimizer/curve_generator.md

A complete example demonstrating path planning with Theta*, converting the path to world frame with orientations, generating a Reeds-Shepp curve, and visualizing the results. This code sets up a grid map, defines start and goal points, plans a path, optimizes it with Reeds-Shepp, and displays the original and optimized paths.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *
from python_motion_planning.traj_optimizer import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = ThetaStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
map_.fill_expands(path_info["expand"])

path_world = map_.path_map_to_world(path)
path_with_orient = Geometry.add_orient_to_2d_path(path_world)

generator = ReedsShepp(step=0.01, max_curv=1.0)
smooth_path, curve_info = generator.generate(path_with_orient)
print(curve_info)

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_path(smooth_path, style="-", color="C1", map_frame=False)
vis.show()
vis.close()
```

--------------------------------

### Complete Runnable Example for 3D Path Planning

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/graph_search.md

This comprehensive script sets up a 3D grid map with random obstacles, defines start and goal points, plans a path using A*, and visualizes the results. It includes necessary imports and seed settings for reproducibility.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 31], [0, 31], [0, 31]], resolution=1.0)
for i in range(75):
    rd_p = tuple(np.random.randint(0, 30, size=3))
    map_.type_map[rd_p[0], rd_p[1], :rd_p[2]] = TYPES.OBSTACLE
map_.inflate_obstacles(radius=3)

start = (25, 5, 5)
goal = (5, 25, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = AStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)
map_.fill_expands(path_info["expand"])

vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.plot_path(path)
vis.show()
vis.close()
```

--------------------------------

### Initialize Toy Simulator

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/controller/path_tracker.md

Creates a toy simulator instance for rapid verification. The simulator requires the dimension and the obstacle grid. Robot collisions are disabled for this setup.

```python
dim = 2
env = ToySimulator(dim=dim, obstacle_grid=map_, robot_collisions=False)
```

--------------------------------

### Complete Runnable Example: Grid Map Creation and Visualization

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/map/grid.md

A self-contained script that sets up the environment, creates a grid map with obstacles, inflates them, and visualizes the final map.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.show()
vis.close()
```

--------------------------------

### 3-D A* Path Planning

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Demonstrates the A* path planning algorithm in a 3-dimensional grid. Obstacles are defined in the map, and a path is planned from start to goal.

```python
from python_motion_planning import Grid, TYPES, AStar

map_3d = Grid(bounds=[[0, 10], [0, 10], [0, 10]])
map_3d.type_map[3:7, 5, :] = TYPES.OBSTACLE
planner_3d = AStar(map_=map_3d, start=(0, 0, 0), goal=(9, 9, 9))
path_3d, info_3d = planner_3d.plan()
print(info_3d["success"])   # True
```

--------------------------------

### Create and Plan Path with RRT

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/sample_search.md

Instantiate the RRT planner with the map, start, and goal, then call the plan method. Prints the resulting path and planning information.

```python
planner = RRT(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)
```

--------------------------------

### DWA Controller Setup and Execution

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Sets up the DWA controller with a robot model and obstacle grid, then executes it for a fixed number of steps. Requires a Grid, Robot model, and ToySimulator.

```python
import numpy as np
from python_motion_planning import Grid, TYPES, CircularRobot, ToySimulator, DWA

map_ = Grid(bounds=[[0, 20], [0, 20]])
map_.type_map[8:12, 5:15] = TYPES.OBSTACLE
map_.inflate_obstacles(1.0)

env   = ToySimulator(dim=2, dt=0.1, obstacle_grid=map_)
robot = CircularRobot(dim=2, pos=np.array([1.0, 1.0]), radius=0.5)
env.add_robot(0, robot)

path = [(1.0, 1.0), (5.0, 5.0), (19.0, 19.0)]

controller = DWA(
    observation_space=robot.observation_space,
    action_space=robot.action_space,
    dt=env.dt,
    path=path,
    max_lin_speed=2.0,
    max_ang_speed=np.pi,
    robot_model=robot,
    obstacle_grid=map_,
    vel_reso=np.array([0.2, 0.2, np.deg2rad(15)]),
    predict_time=1.0,
    heading_weight=0.5,
    velocity_weight=0.2,
    clearance_weight=0.3,
)

for step in range(20):
    obs = robot.get_observation(env)
    action, target = controller.get_action(obs)
    env.step({0: action})

print(f"Final position: {robot.pos.round(3)}")
# Predicted trajectory for visualization: controller.pred_traj
```

--------------------------------

### Install Dependencies with Conda

Source: https://github.com/ai-winter/python_motion_planning/blob/master/README.md

Use this command to create and activate a new conda environment for the project. Tested with Python 3.10.

```shell
conda create -n pmp python=3.10
conda activate pmp
```

--------------------------------

### Create Path Planner and Plan Path

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/path_planner/graph_search.md

Instantiate the A* path planner with the map, start, and goal points, then call the plan() method. The function returns the planned path and detailed information about the planning process.

```python
planner = AStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)
map_.fill_expands(path_info["expand"])  # for visualizing the expanded nodes
```

--------------------------------

### Define Start and Goal Points

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/path_planner/graph_search.md

Define the start and goal coordinates for path planning. These are represented as tuples, with integer values for discrete grid maps and floating-point numbers for world frames.

```python
start = (5, 5)
goal = (45, 25)
```

--------------------------------

### Add Start and Goal to Map

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/path_planner/graph_search.md

Add the start and goal points to the map's type map. This aids visualization and ensures obstacles are cleared at these locations to prevent planning failures.

```python
map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL
```

--------------------------------

### Visualize Controller Simulation

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Visualizes a controller simulation on a grid map using a toy simulator and a robot model. Requires environment setup, robot definition, a path, and a controller (e.g., PurePursuit).

```python
import numpy as np
from python_motion_planning import (
    Grid,
    TYPES,
    AStar,
    Visualizer2D,
    CircularRobot,
    ToySimulator,
    PurePursuit,
)

# --- Controller simulation visualization ---
env   = ToySimulator(dim=2, dt=0.1, obstacle_grid=map_)
robot = CircularRobot(dim=2, pos=np.array([1.5, 1.5]), radius=0.5, color="C0", text="R")
env.add_robot(0, robot)

world_path = map_.path_map_to_world(path)
ctrl = PurePursuit(
    observation_space=robot.observation_space,
    action_space=robot.action_space,
    dt=env.dt,
    path=world_path,
    max_lin_speed=2.0,
    max_ang_speed=np.pi,
    lookahead_distance=2.0,
)

vis2 = Visualizer2D(figname="Pure Pursuit", figsize=(8, 6))
vis2.plot_grid_map(map_)
vis2.plot_path(path, label="Planned path")
vis2.render_toy_simulator(
    env=env,
    controllers={0: ctrl},
    steps=200,
    show_traj=True,
    show_env_info=True,
    rtf_limit=1.0,
)
vis2.show()
```

--------------------------------

### Define Start and Goal Points in 3D

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/graph_search.md

Coordinate points are represented as tuples. Use integers for discrete grid maps and floating-point numbers for world frames.

```python
start = (25, 5, 5)
goal = (5, 25, 25)
```

--------------------------------

### Plan Path in Discrete 3D Grid Map

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/sample_search.md

To plan a path in a discrete grid map, set the `discrete` argument to `True` when initializing the RRT planner. This example shows the planner instantiation for discrete planning.

```python
planner = RRT(map_=map_, start=start, goal=goal, discrete=True)
```

--------------------------------

### RRTConnect Bidirectional Path Planning

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Implements RRTConnect, a bidirectional RRT algorithm that grows two trees from the start and goal simultaneously. This approach typically finds solutions faster by connecting the two trees when they meet.

```python
from python_motion_planning import Grid, TYPES
from python_motion_planning.path_planner.sample_search.rrt_connect import RRTConnect

map_ = Grid(bounds=[[0, 30], [0, 30]])
map_.type_map[10:25, 15] = TYPES.OBSTACLE

planner = RRTConnect(map_=map_, start=(1, 1), goal=(28, 28), max_dist=3.0)
path, info = planner.plan()
print(info["success"])   # True
```

--------------------------------

### Simulate and Render Toy Simulator

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/controller/path_tracker.md

Resets the simulator, initializes a 2D visualizer, and renders the simulation. The visualization can be customized with parameters like the number of steps, trajectory display, environment information, and ESDF map visibility.

```python
obs, _ = env.reset()

vis = Visualizer2D()
vis.render_toy_simulator(env, controllers, steps=300, show_traj=True, show_env_info=True, grid_kwargs={"show_esdf": False})
vis.plot_path(path, style="--", color="C4")
vis.show()
```

--------------------------------

### VoronoiPlanner: Compute Path Using Voronoi Diagram

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Uses VoronoiPlanner to compute a path by maximizing clearance from obstacles. Falls back to a base planner like AStar if needed. Prints planning success and path details.

```python
from python_motion_planning import Grid, TYPES, VoronoiPlanner, AStar

map_ = Grid(bounds=[[0, 20], [0, 20]])
map_.type_map[5:10, 5:10] = TYPES.OBSTACLE
map_.type_map[12:17, 12:17] = TYPES.OBSTACLE

planner = VoronoiPlanner(
    map_=map_,
    start=(1, 1),
    goal=(19, 19),
    base_planner=AStar,      # planner used on Voronoi graph
    base_planner_kwargs={},
    cover_inflation=False,
)
path, info = planner.plan()

print(info["success"])
print(f"Voronoi start: {info.get('voronoi_start')}")
print(f"Voronoi goal : {info.get('voronoi_goal')}")
print(f"Total length : {info['length']:.2f}")
```

--------------------------------

### RRT* Asymptotically Optimal Path Planning

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Demonstrates RRT*, an extension of RRT that achieves asymptotic optimality by selecting the best parent and rewiring the tree. A `stop_func` allows for continued refinement after the first solution is found.

```python
from python_motion_planning import Grid, TYPES, RRTStar

map_ = Grid(bounds=[[0, 30], [0, 30]])
map_.type_map[10:25, 15] = TYPES.OBSTACLE

# Stop after sampling 10× the first-success step (cost refinement)
stop_fn = lambda cur, fss, mss: (cur >= fss * 10 if fss else False) or (cur >= mss)

planner = RRTStar(
    map_=map_,
    start=(1, 1),
    goal=(28, 28),
    max_dist=3.0,
    max_sample_step=100000,
    rewire_radius=None,    # adaptive radius via gamma factor
    gamma=50.0,
    stop_func=stop_fn,
    propagate_cost_to_children=True,
)
path, info = planner.plan()

print(info["success"])                            # True
print(f"First success at step: {info['first_success_step']}")
print(f"Best cost at step    : {info['best_step']}")
print(f"Total steps          : {info['total_step']}")
print(f"Final path cost      : {info['cost']:.2f}")
```

--------------------------------

### PID: PID Path-Tracking Controller

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Utilizes a PID controller for path tracking, incorporating integral and derivative velocity error terms. Resets accumulators before getting the first action.

```python
import numpy as np
from python_motion_planning import Grid, CircularRobot, ToySimulator, PID

map_  = Grid(bounds=[[0, 20], [0, 20]])
env   = ToySimulator(dim=2, dt=0.1, obstacle_grid=map_)
robot = CircularRobot(dim=2, pos=np.array([1.0, 1.0]), radius=0.5)
env.add_robot(0, robot)

path = [(1.0, 1.0), (10.0, 10.0), (19.0, 19.0)]

controller = PID(
    observation_space=robot.observation_space,
    action_space=robot.action_space,
    dt=env.dt,
    path=path,
    max_lin_speed=2.0,
    max_ang_speed=np.pi,
    Kp=1.5,
    Ki=0.1,
    Kd=0.05,
)

controller.reset()  # zero integral / derivative accumulators

obs    = robot.get_observation(env)
action, _ = controller.get_action(obs)
print(f"PID action: {action}")
```

--------------------------------

### RRT Path Planning

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Shows how to use the Rapidly-exploring Random Tree (RRT) algorithm for finding feasible, though not necessarily optimal, paths. Supports continuous and discrete spaces and uses FAISS for efficient nearest-neighbor searches.

```python
from python_motion_planning import Grid, TYPES, RRT

map_ = Grid(bounds=[[0, 30], [0, 30]])
map_.type_map[10:25, 15] = TYPES.OBSTACLE

planner = RRT(
    map_=map_,
    start=(1, 1),
    goal=(28, 28),
    max_dist=3.0,          # max step length
    max_sample_step=50000, # iteration limit
    goal_sample_rate=0.05, # 5% chance to sample goal directly
    discrete=False,        # continuous space
    use_faiss=True,        # FAISS-accelerated nearest-neighbor
)
path, info = planner.plan()

if info["success"]:
    print(f"Found path with {len(path)} waypoints, cost={info['cost']:.2f}")
else:
    print("Increase max_sample_step or adjust map")
```

--------------------------------

### Create and Manipulate Grid Maps

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Demonstrates creating a 2D or 3D occupancy grid, placing obstacles, inflating them, updating the ESDF, and performing coordinate conversions and neighbor checks. Use for defining the environment for path planning.

```python
import numpy as np
from python_motion_planning import Grid, TYPES

# Create a 30×20 world-unit grid with resolution=1 (default)
map_ = Grid(bounds=[[0, 30], [0, 20]], resolution=1.0)

# Place obstacles
map_.type_map[5:15, 8] = TYPES.OBSTACLE   # vertical wall column 8, rows 5-14
map_.type_map[10, 5:15] = TYPES.OBSTACLE  # horizontal wall row 10, cols 5-14

# Inflate obstacles by 1 grid cell radius
map_.inflate_obstacles(radius=1.0)

# Update ESDF (called automatically when needed by planners)
map_.update_esdf()

# Coordinate conversions
world_pt = map_.map_to_world((5, 10))   # → (5.5, 10.5)
map_pt   = map_.world_to_map((5.5, 10.5))  # → (5, 10)

# Neighbor expansion
from python_motion_planning import Node
neighbors = map_.get_neighbors(Node((5, 5)), diagonal=True)
print(len(neighbors))   # up to 8 (diagonal) or 4 (orthogonal)

# Line-of-sight and collision check
los_pts = map_.line_of_sight((0, 0), (5, 5))  # Bresenham trace
collides = map_.in_collision((0, 0), (5, 5))   # True/False

# 3D grid (same API, just more dimensions)
map_3d = Grid(bounds=[[0, 10], [0, 10], [0, 5]])
map_3d.type_map[3:7, 3:7, 2] = TYPES.OBSTACLE
```

--------------------------------

### Visualize A* Path Planning

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Visualizes the A* path planning process on a grid map, including the expansion tree and the final path. Requires a Grid object, planner, and Visualizer2D instance. The map can be configured with obstacles.

```python
import numpy as np
from python_motion_planning import (
    Grid,
    TYPES,
    AStar,
    Visualizer2D,
    CircularRobot,
    ToySimulator,
    PurePursuit,
)

# --- Path planner visualization ---
map_ = Grid(bounds=[[0, 20], [0, 20]])
map_.type_map[5:15, 10] = TYPES.OBSTACLE

planner = AStar(map_=map_, start=(1, 1), goal=(19, 19))
path, info = planner.plan()

vis = Visualizer2D(figname="A* Demo", figsize=(8, 6))
vis.plot_grid_map(map_, show_esdf=True, alpha_esdf=0.3)
vis.plot_expand_tree(info["expand"], node_color="#8c564b", edge_color="#e377c2")
vis.plot_path(path, color="#13ae00", linewidth=2.5, label="A* path")
vis.set_title("A* Path Planning")
vis.savefig("a_star_result.png")
vis.show()
```

--------------------------------

### Visualize Path and Expansion Tree

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/path_planner/sample_search.md

Visualizes the planned path and the expansion tree using the Visualizer2D class. Plots the grid map, the path, and the expansion details.

```python
vis = Visualizer2D()
vis.plot_grid_map(map_)
vis.plot_path(path, style="--", color="C4")
vis.plot_expand_tree(path_info["expand"])   # sample-search-featured expand 
vis.show()
vis.close()
```

--------------------------------

### Configure RRT* Stopping Condition

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/sample_search.md

For RRT*, use the `stop_func` argument to define a custom stopping condition. This example uses a lambda function to stop sampling when the current iteration count is 10 times the first successful path's iteration count, or when the maximum sampling steps are reached.

```python
planner = RRTStar(map_=map_, start=start, goal=goal, stop_func=lambda cur, fss, mss: (cur >= fss * 10 if fss is not None else False) or (cur >= mss))
```

--------------------------------

### Import Trajectory Optimizer Modules

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/traj_optimizer/curve_generator.md

Import necessary modules for path planning, control, and trajectory optimization.

```python
from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *
from python_motion_planning.traj_optimizer import *
```

--------------------------------

### Add Controllers to the Environment

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/controller/path_tracker.md

Initializes and adds path-tracking controllers for each robot in the simulation. Controllers are configured with observation and action spaces, simulation time step, and path information. Optional parameters like max linear/angular speed and goal tolerances can be set.

```python
controllers = {}
for rid, robot in robots.items():
    obs_space, act_space = env.build_robot_spaces(robot)
    controllers[rid] = PurePursuit(obs_space, act_space, env.dt, path_world, robot_model=robot, obstacle_grid=map_, max_lin_speed=3, max_ang_speed=3.14)
    env.add_robot(rid, robot)
```

--------------------------------

### Import Necessary Modules

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/map/grid.md

Import all necessary modules from the `python_motion_planning` library, including common utilities, path planners, and controllers.

```python
from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *
```

--------------------------------

### PathTracker: Proportional Lookahead Path-Tracking Controller

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Demonstrates the PathTracker controller for steering a robot towards a lookahead point on a planned path. Requires a simulator, robot, and a predefined path.

```python
import numpy as np
from gymnasium import spaces
from python_motion_planning import Grid, CircularRobot, ToySimulator, PathTracker

# Build map and simulator
map_ = Grid(bounds=[[0, 20], [0, 20]])
env  = ToySimulator(dim=2, dt=0.1, obstacle_grid=map_)
robot = CircularRobot(dim=2, pos=np.array([1.0, 1.0]),
                      radius=0.5, color="C0", text="R1")
env.add_robot(0, robot)

# Planned path (from any path planner, converted to world frame)
path_world = [(1.0, 1.0), (5.0, 5.0), (10.0, 10.0), (15.0, 15.0), (19.0, 19.0)]

obs_space = robot.observation_space
act_space = robot.action_space

controller = PathTracker(
    observation_space=obs_space,
    action_space=act_space,
    dt=env.dt,
    path=path_world,
    max_lin_speed=2.0,
    max_ang_speed=np.pi,
    goal_dist_tol=0.5,
    lookahead_distance=2.0,
    k_theta=0.8,
)

# One step
obs = robot.get_observation(env)
action, target_pose = controller.get_action(obs)
print(f"Action      : {action}")
print(f"Target pose : {target_pose}")
```

--------------------------------

### Runnable Complete Code for 3D Path Planning

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/sample_search.md

A complete script demonstrating 3D path planning using RRT. It includes map generation, obstacle inflation, planner instantiation, path planning, and visualization. Set random seeds for reproducibility.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 31], [0, 31], [0, 31]], resolution=1.0)
for i in range(75):
    rd_p = tuple(np.random.randint(0, 30, size=3))
    map_.type_map[rd_p[0], rd_p[1], :rd_p[2]] = TYPES.OBSTACLE
map_.inflate_obstacles(radius=3)

start = (25, 5, 5)
goal = (5, 25, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = RRT(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)

vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.plot_path(path)
vis.plot_expand_tree(path_info["expand"])
vis.show()
vis.close()
```

--------------------------------

### Visualize Path and Expanded Nodes

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/graph_search.md

Fill the map with expanded nodes from planning information and use the Visualizer3D to plot the grid map and the planned path. Ensure to display and close the visualizer.

```python
map_.fill_expands(path_info["expand"])
vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.plot_path(path)
vis.show()
vis.close()
```

--------------------------------

### Visualize Voronoi Path and Grid Map

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/hybrid_search.md

Use the Visualizer3D class to plot the grid map and the planned path. This is useful for understanding the planner's output and the environment.

```python
# if "voronoi_candidates" in path_info:
#     map_.type_map[path_info["voronoi_candidates"]] = TYPES.CUSTOM
vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.plot_path(path)
vis.show()
vis.close()
```

--------------------------------

### Visualize 3D Grid Map

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/map/grid.md

Use the `Visualizer3D` class to plot and display the created 3D grid map. Ensure to close the visualizer after use.

```python
vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.show()
vis.close()
```

--------------------------------

### Visualize 3D Path Planning Results

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/3d/path_planner/sample_search.md

Uses the Visualizer3D class to plot the grid map, the planned path, and the expansion tree from the planning information. Ensure the map, path, and expand tree data are available.

```python
vis = Visualizer3D()
vis.plot_grid_map(map_)
vis.plot_path(path)
vis.plot_expand_tree(path_info["expand"])   # sample-search-featured expand tree
vis.show()
vis.close()
```

--------------------------------

### DWA Controller with ESDF Visualization

Source: https://github.com/ai-winter/python_motion_planning/blob/master/tutorials/2d/controller/path_tracker.md

This snippet demonstrates the Dynamic Window Approach (DWA) controller, highlighting its ability to use ESDF for obstacle avoidance. The visualization includes an option to display the ESDF by setting `show_esdf=True`.

```python
import random
random.seed(0)

import numpy as np
np.random.seed(0)

from python_motion_planning.common import *
from python_motion_planning.path_planner import *
from python_motion_planning.controller import *

map_ = Grid(bounds=[[0, 51], [0, 31]])

map_.fill_boundary_with_obstacles()
map_.type_map[10:21, 15] = TYPES.OBSTACLE
map_.type_map[20, :15] = TYPES.OBSTACLE
map_.type_map[30, 15:] = TYPES.OBSTACLE
map_.type_map[40, :16] = TYPES.OBSTACLE

map_.inflate_obstacles(radius=3)

start = (5, 5)
goal = (45, 25)

map_.type_map[start] = TYPES.START
map_.type_map[goal] = TYPES.GOAL

planner = AStar(map_=map_, start=start, goal=goal)
path, path_info = planner.plan()
print(path)
print(path_info)
map_.fill_expands(path_info["expand"])  # for visualizing the expanded nodes

path_world = map_.path_map_to_world(path)
print(path_world)

dim = 2
env = ToySimulator(dim=dim, obstacle_grid=map_, robot_collisions=False)

robots = {
    "1": CircularRobot(dim=dim, radius=1, pose=np.array([5.5, 5.5, 0]), vel=np.zeros(3),
                action_min=np.array([-2, -2, -3.14]), action_max=np.array([2, 2, 3.14]), color="C0", text="1"),
    "2": DiffDriveRobot(dim=dim, radius=1, pose=np.array([5.5, 5.5, 0]), vel=np.zeros(3),
                action_min=np.array([-2.82, 0, -6.28]), action_max=np.array([2.82, 0, 6.28]), color="C1", text="2")
}

controllers = {}
for rid, robot in robots.items():
    obs_space, act_space = env.build_robot_spaces(robot)
    controllers[rid] = DWA(obs_space, act_space, env.dt, path_world, robot_model=robot, obstacle_grid=map_, max_lin_speed=3, max_ang_speed=3.14)
    env.add_robot(rid, robot)

obs, _ = env.reset()

vis = Visualizer2D()
vis.render_toy_simulator(env, controllers, steps=300, show_traj=True, show_env_info=True, grid_kwargs={"show_esdf": True})
vis.plot_path(path, style="--", color="C4")
vis.show()

for rid in robots:
    ctrl = controllers[rid]
    print(rid, ":", vis.get_traj_info(rid, path_world, ctrl.goal, ctrl.goal_dist_tol, ctrl.goal_orient_tol))
vis.close()

```

--------------------------------

### PurePursuit: Geometric Path-Tracking Controller

Source: https://context7.com/ai-winter/python_motion_planning/llms.txt

Implements the PurePursuit controller, calculating curvature to steer towards a lookahead point. Simulates robot movement over several steps.

```python
import numpy as np
from gymnasium import spaces
from python_motion_planning import Grid, CircularRobot, ToySimulator, PurePursuit

map_  = Grid(bounds=[[0, 20], [0, 20]])
env   = ToySimulator(dim=2, dt=0.05, obstacle_grid=map_)
robot = CircularRobot(dim=2, pos=np.array([1.0, 1.0]), radius=0.5)
env.add_robot(0, robot)

path = [(1.0, 1.0), (5.0, 3.0), (10.0, 10.0), (18.0, 18.0)]

controller = PurePursuit(
    observation_space=robot.observation_space,
    action_space=robot.action_space,
    dt=env.dt,
    path=path,
    max_lin_speed=3.0,
    max_ang_speed=np.pi,
    lookahead_distance=2.5,
)

# Run 5 simulation steps
for _ in range(5):
    obs = robot.get_observation(env)
    action, lookahead = controller.get_action(obs)
    env.step({0: action})
    print(f"Robot pos: {robot.pos.round(3)}, lookahead: {lookahead[:2].round(3)}")
```