majiayu000 Motion Planning Skill
Version: 1.0.0 | Alignment: Constitution v6.0.0, Pyodide-Compatible Code | Purpose: Generate educational motion planning code for browser-based learning
Purpose
Generate production-quality Python code for motion planning that:
- Runs in Pyodide (browser environment)
- Implements path planning algorithms (A*, RRT, etc.)
- Demonstrates trajectory optimization
- Shows control algorithms (PID, MPC)
- Provides educational value for robot motion
- Supports InteractivePython components in Docusaurus
Key Focus: Teaching motion planning algorithms and control theory, not just using planning libraries.
When to Activate
Activate when:
- Creating code examples for path planning (A*, RRT, Dijkstra)
- Demonstrating trajectory optimization
- Teaching PID control for robots
- Creating model predictive control (MPC) examples
- Building whole-body control systems
Trigger phrases:
- "Create path planning code"
- "Generate A* algorithm example"
- "Build PID controller for robot"
- "Create trajectory optimization code"
- "Generate MPC control example"
Technical Capabilities
Supported Algorithms
Path Planning
- A* algorithm
- RRT (Rapidly-exploring Random Tree)
- Dijkstra's algorithm
- Potential fields
Trajectory Optimization
- Minimum jerk trajectories
- Time-optimal trajectories
- Constrained optimization
- Smooth path generation
Control Algorithms
- PID control
- Feedforward control
- Model Predictive Control (MPC)
- LQR (Linear Quadratic Regulator)
Whole-Body Control
- Task-space control
- Null-space control
- Prioritized control
Pyodide-Compatible Libraries
✅ Supported:
- NumPy (arrays, linear algebra, optimization)
- Matplotlib (plotting paths, trajectories, control responses)
- SciPy (optimization, signal processing, interpolation)
- Math (basic math functions)
❌ NOT Supported:
- OMPL (Open Motion Planning Library)
- ROS navigation stack
- Real-time control libraries
- Hardware-specific controllers
Code Generation Standards
Code Structure
"""
[Motion Planning Algorithm] Implementation
Educational Purpose: [What students will learn]
Physical AI Context: [How real robots use this algorithm]
"""
import numpy as np
import matplotlib.pyplot as plt
from typing import Tuple, List, Optional
from scipy.optimize import minimize
# Type hints for all functions
# Comprehensive docstrings (Google style)
# Error handling (try-except where appropriate)
# Clear algorithm implementation
# Visualization of results
Example Template: A* Path Planning
"""
A* Path Planning Algorithm
Educational Purpose: Students learn how A* finds optimal paths
Physical AI Context: Real robots use A* for navigation and motion planning
"""
import numpy as np
import matplotlib.pyplot as plt
from typing import List, Tuple, Optional
from heapq import heappush, heappop
class Node:
"""Node in A* search graph."""
def __init__(self, x: int, y: int, g: float = 0, h: float = 0, parent=None):
self.x = x
self.y = y
self.g = g # Cost from start
self.h = h # Heuristic to goal
self.f = g + h # Total cost
self.parent = parent
def __lt__(self, other):
return self.f < other.f
def heuristic(node: Node, goal: Tuple[int, int]) -> float:
"""
Euclidean distance heuristic for A*.
Args:
node: Current node
goal: Goal position (x, y)
Returns:
Estimated cost to goal
"""
return np.sqrt((node.x - goal[0])**2 + (node.y - goal[1])**2)
def astar_path_planning(
start: Tuple[int, int],
goal: Tuple[int, int],
obstacles: List[Tuple[int, int]],
grid_size: Tuple[int, int] = (20, 20)
) -> Optional[List[Tuple[int, int]]]:
"""
A* path planning algorithm.
Args:
start: Start position (x, y)
goal: Goal position (x, y)
obstacles: List of obstacle positions
grid_size: Grid dimensions (width, height)
Returns:
Path as list of (x, y) positions, or None if no path found
"""
open_set = []
closed_set = set()
obstacle_set = set(obstacles)
start_node = Node(start[0], start[1], 0, heuristic(Node(start[0], start[1]), goal))
heappush(open_set, start_node)
while open_set:
current = heappop(open_set)
if (current.x, current.y) in closed_set:
continue
closed_set.add((current.x, current.y))
# Check if goal reached
if current.x == goal[0] and current.y == goal[1]:
# Reconstruct path
path = []
node = current
while node:
path.append((node.x, node.y))
node = node.parent
return path[::-1]
# Explore neighbors (8-connected)
for dx, dy in [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]:
nx, ny = current.x + dx, current.y + dy
# Check bounds
if nx < 0 or nx >= grid_size[0] or ny < 0 or ny >= grid_size[1]:
continue
# Check obstacle
if (nx, ny) in obstacle_set:
continue
# Check if already explored
if (nx, ny) in closed_set:
continue
# Calculate costs
move_cost = np.sqrt(dx**2 + dy**2) # Diagonal moves cost more
g_new = current.g + move_cost
h_new = heuristic(Node(nx, ny), goal)
neighbor = Node(nx, ny, g_new, h_new, current)
heappush(open_set, neighbor)
return None # No path found
def visualize_path(
start: Tuple[int, int] = (0, 0),
goal: Tuple[int, int] = (19, 19),
obstacles: List[Tuple[int, int]] = [(5, 5), (10, 10), (15, 15)]
):
"""Visualize A* path planning."""
path = astar_path_planning(start, goal, obstacles)
if path is None:
print("No path found!")
return
fig, ax = plt.subplots(figsize=(10, 10))
# Draw grid
grid = np.zeros((20, 20))
for ox, oy in obstacles:
grid[oy, ox] = 1
ax.imshow(grid, cmap='gray', origin='lower', alpha=0.3)
# Draw path
path_x = [p[0] for p in path]
path_y = [p[1] for p in path]
ax.plot(path_x, path_y, 'b-', linewidth=2, label='Planned Path')
ax.scatter(path_x, path_y, c='blue', s=50, zorder=5)
# Draw start and goal
ax.plot(start[0], start[1], 'go', markersize=15, label='Start', zorder=6)
ax.plot(goal[0], goal[1], 'ro', markersize=15, label='Goal', zorder=6)
# Draw obstacles
for ox, oy in obstacles:
ax.add_patch(plt.Rectangle((ox-0.5, oy-0.5), 1, 1,
facecolor='black', edgecolor='black'))
ax.set_xlim(-0.5, 19.5)
ax.set_ylim(-0.5, 19.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_title('A* Path Planning')
ax.legend()
plt.tight_layout()
plt.show()
# Example usage
if __name__ == "__main__":
visualize_path()
Example Template: PID Control
"""
PID Control for Robot Motion
Educational Purpose: Students learn how PID controllers work for robot control
Physical AI Context: Real robots use PID control for position, velocity, and force control
"""
import numpy as np
import matplotlib.pyplot as plt
from typing import Tuple
class PIDController:
"""PID controller implementation."""
def __init__(self, kp: float, ki: float, kd: float, dt: float = 0.01):
"""
Initialize PID controller.
Args:
kp: Proportional gain
ki: Integral gain
kd: Derivative gain
dt: Time step (seconds)
"""
self.kp = kp
self.ki = ki
self.kd = kd
self.dt = dt
self.integral = 0.0
self.prev_error = 0.0
def compute(self, setpoint: float, current_value: float) -> float:
"""
Compute PID control output.
Args:
setpoint: Desired value
current_value: Current value
Returns:
Control output
"""
error = setpoint - current_value
# Proportional term
p_term = self.kp * error
# Integral term
self.integral += error * self.dt
i_term = self.ki * self.integral
# Derivative term
derivative = (error - self.prev_error) / self.dt
d_term = self.kd * derivative
# Update for next iteration
self.prev_error = error
# Total output
output = p_term + i_term + d_term
return output
def reset(self):
"""Reset controller state."""
self.integral = 0.0
self.prev_error = 0.0
def simulate_pid_control(
kp: float = 1.0,
ki: float = 0.1,
kd: float = 0.5,
duration: float = 10.0,
dt: float = 0.01
):
"""
Simulate PID control system.
Args:
kp: Proportional gain
ki: Integral gain
kd: Derivative gain
duration: Simulation duration (seconds)
dt: Time step (seconds)
"""
pid = PIDController(kp, ki, kd, dt)
time = np.arange(0, duration, dt)
setpoint = np.ones_like(time) * 1.0 # Step input
position = np.zeros_like(time)
velocity = np.zeros_like(time)
# Simple dynamics: mass-spring-damper
mass = 1.0
damping = 0.1
for i in range(1, len(time)):
# Compute control
control = pid.compute(setpoint[i], position[i-1])
# Update dynamics
acceleration = (control - damping * velocity[i-1]) / mass
velocity[i] = velocity[i-1] + acceleration * dt
position[i] = position[i-1] + velocity[i] * dt
# Visualize
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))
ax1.plot(time, setpoint, 'r--', label='Setpoint', linewidth=2)
ax1.plot(time, position, 'b-', label='Position', linewidth=2)
ax1.set_xlabel('Time (seconds)')
ax1.set_ylabel('Position')
ax1.set_title('PID Control: Position Response')
ax1.legend()
ax1.grid(True, alpha=0.3)
error = setpoint - position
ax2.plot(time, error, 'g-', label='Error', linewidth=2)
ax2.set_xlabel('Time (seconds)')
ax2.set_ylabel('Error')
ax2.set_title('Control Error')
ax2.legend()
ax2.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
# Example usage
if __name__ == "__main__":
simulate_pid_control()
Best Practices
1. Algorithm Clarity
- Step-by-step: Show algorithm execution step-by-step
- Visualization: Plot paths, trajectories, control responses
- Comments: Explain algorithm logic in comments
- Pseudocode: Include algorithm structure in docstrings
2. Educational Focus
- Start Simple: Begin with 2D, single-robot examples
- Build Complexity: Progress to 3D, multi-robot scenarios
- Compare Algorithms: Show different planning/control approaches
- Real-World Context: Connect to actual robot applications
3. Code Quality
- Type Hints: All functions should have type hints
- Docstrings: Google-style docstrings explaining algorithms
- Error Handling: Validate inputs, handle edge cases
- Modularity: Separate algorithm from visualization
4. Performance
- Efficient Implementation: Use vectorized NumPy operations
- Reasonable Complexity: Keep algorithms simple enough for browser
- Optimization: Use SciPy optimization where appropriate
Common Patterns
Pattern 1: Path Planning
def plan_path(start, goal, obstacles):
# Initialize search
# Explore graph
# Find optimal path
# Return path
pass
Pattern 2: Trajectory Generation
def generate_trajectory(waypoints, constraints):
# Interpolate between waypoints
# Optimize for smoothness/time
# Return trajectory
pass
Pattern 3: Control Loop
def control_loop(setpoint, current_state, controller):
# Compute error
# Compute control output
# Apply to system
# Return new state
pass
Pattern 4: Optimization-Based Planning
def optimize_trajectory(objective, constraints):
# Define optimization problem
# Solve using scipy.optimize
# Return optimal trajectory
pass
Integration with InteractivePython
Usage in MDX Files
<InteractivePython>
{`
import numpy as np
import matplotlib.pyplot as plt
# [Generated motion planning code here]
`}
</InteractivePython>
Student Modifications
Encourage students to:
- Modify algorithm parameters (gains, weights, etc.)
- Change start/goal positions
- Add/remove obstacles
- Experiment with different algorithms
- Tune control parameters
Troubleshooting
Issue: Path planning too slow
Solution: Reduce grid resolution, limit search space, use simpler heuristics.
Issue: Control unstable
Solution: Check gains, ensure proper initialization, validate system dynamics.
Issue: Visualization unclear
Solution: Add labels, legends, use appropriate colors, show multiple views.
Output Format
When generating code:
- Header Comment: Clear description and educational purpose
- Algorithm Class/Function: Clear implementation with comments
- Visualization: Clear plots showing paths, trajectories, or control responses
- Example Usage: Working example demonstrating functionality
- Parameter Tuning: Show how parameters affect results
Remember: You're teaching motion planning and control algorithms. Focus on understanding how algorithms work, not just using planning libraries.