🔬 Show & Tell 25.3 — MotorPlanner Architecture

[web3guru888]

🔬 MotorPlanner Architecture — Hierarchical Motion Planning

Overview

Phase 25.3 introduces MotorPlanner, implementing a three-level hierarchical planning architecture inspired by robotics and motor neuroscience. The hierarchy decomposes high-level goals into executable motion trajectories:

1. Task level — What to do (symbolic plan from affordances)
2. Trajectory level — How to move (RRT* path + trajectory optimization)
3. Reactive level — Real-time adjustments (potential fields for obstacle avoidance)

Data Flow

┌───────────────────────────────────────────────────────────┐
│                  HierarchicalMotorPlanner                   │
│                                                            │
│  ┌──────────────┐                                          │
│  │ Affordance    │ (from AffordanceDetector 25.2)          │
│  │ {grasp, cup,  │                                         │
│  │  conf: 0.92}  │                                         │
│  └──────┬───────┘                                          │
│         ▼                                                  │
│  ┌──────────────────────────────────────────────┐          │
│  │ TASK DECOMPOSITION                            │          │
│  │ grasp(cup) → [approach, pre-grasp, close,     │          │
│  │               lift, verify_grip]               │          │
│  └──────────────────┬───────────────────────────┘          │
│                     ▼                                      │
│  ┌──────────────────────────────────────────────┐          │
│  │ RRT* PATH PLANNING                            │          │
│  │                                               │          │
│  │  start ──○──○──○              ○── goal        │          │
│  │              ╲   ╲          ╱                  │          │
│  │               ○────○──○──○                    │          │
│  │           [obstacle avoidance]                 │          │
│  │                                               │          │
│  │  Asymptotically optimal: cost → c* as n→∞    │          │
│  └──────────────────┬───────────────────────────┘          │
│                     ▼                                      │
│  ┌──────────────────────────────────────────────┐          │
│  │ TRAJECTORY OPTIMIZATION                       │          │
│  │                                               │          │
│  │ minimize: J = ∫₀ᵀ (τ² + λ·jerk²) dt          │          │
│  │ subject to: joint limits, velocity limits,    │          │
│  │             obstacle clearance ≥ safety_margin│          │
│  │                                               │          │
│  │ Smooth, energy-efficient trajectory           │          │
│  └──────────────────┬───────────────────────────┘          │
│                     ▼                                      │
│  ┌──────────────────────────────────────────────┐          │
│  │ REACTIVE LAYER (Potential Fields)             │          │
│  │                                               │          │
│  │ F_total = F_attractive + Σ F_repulsive        │          │
│  │                                               │          │
│  │ F_att = -k_att · (pos - goal)                 │          │
│  │ F_rep = k_rep · (1/d - 1/d₀)² · ∇d          │          │
│  │         if d < d₀, else 0                     │          │
│  │                                               │          │
│  │ Real-time obstacle avoidance overlay          │          │
│  └──────────────────┬───────────────────────────┘          │
│                     ▼                                      │
│              ┌──────────────┐                              │
│              │ MotorPlan     │                              │
│              │ .waypoints[]  │                              │
│              │ .timing[]     │                              │
│              │ .status       │                              │
│              │ .energy_cost  │                              │
│              └──────┬───────┘                              │
└─────────────────────┼──────────────────────────────────────┘
                      ▼
           EmbodiedOrchestrator 25.5

RRT* Algorithm Visualization

Iteration 1-10:         Iteration 100+:        Final path:
                        
  S · · · · · G         S─○─○ · · · G          S─○─○─○ · · G
  · · ■ ■ · ·           · · ■ ■ · ·            · · ■ ■ ╲ ·
  · · ■ ■ · ·           · ○ ■ ■ ○ ·            · · ■ ■  ○
  · · · · · ·           · ○─○─○─○ ·            · · · ○──○─G
  
  (sparse tree)         (dense, rewired)        (optimal path)
  
S=start, G=goal, ■=obstacle, ○=tree node, ─=edge

RRT key properties:*

• Probabilistically complete (finds a path if one exists)
• Asymptotically optimal (converges to optimal cost as samples → ∞)
• Rewiring step: when new node is added, check if nearby nodes benefit from routing through it

Dynamic Movement Primitives (DMPs)

DMPs encode learned movements as a forcing function on a spring-damper system:

τ·ẍ = α(β(g - x) - ẋ) + f(s)

where:
  τ = time scaling
  α, β = spring-damper constants (α = 4β for critical damping)
  g = goal position
  x = current position
  f(s) = learned forcing function (weighted Gaussian basis functions)
  s = phase variable (1 → 0 over movement duration)

Learning from demonstration:

1. Record demonstrated trajectory {x(t), ẋ(t), ẍ(t)}
2. Invert dynamics to extract f_demo(s) at each timestep
3. Fit Gaussian basis function weights via locally weighted regression
4. Reproduce: run DMP with learned f(s), optionally with new goal g'

Prometheus Metrics

Metric	Type	Description
asi_motor_planner_plans_total	Counter	Total motion plans generated
asi_motor_planner_planning_latency_seconds	Histogram	Time to generate a plan
asi_motor_planner_rrt_iterations	Histogram	RRT* iterations per plan
asi_motor_planner_collisions_detected_total	Counter	Collisions detected during execution
asi_motor_planner_energy_cost_joules	Histogram	Energy cost of executed plans

PromQL:

# Planning throughput
rate(asi_motor_planner_plans_total[5m])

# Collision rate (should be near zero)
rate(asi_motor_planner_collisions_detected_total[5m])

# Average energy efficiency
histogram_quantile(0.5, rate(asi_motor_planner_energy_cost_joules_bucket[5m]))

Integration Map

AffordanceDetector 25.2 ──→ MotorPlanner 25.3 (affordances → plan constraints)
SpatialReasoner 25.4 ──→ MotorPlanner 25.3 (spatial map → obstacle map)
MotorPlanner 25.3 ──→ EmbodiedOrchestrator 25.5 (MotorPlan → execution)
GoalDecomposer 10.2 ──→ MotorPlanner (sub-goals → task-level input)
PlanExecutor 10.3 ──→ MotorPlanner (execution feedback loop)

Open Questions

1. Should DMP weights be stored in MemoryConsolidator (18.2) for long-term skill memory?
2. How should the reactive layer handle local minima in potential fields?
3. What is the right trade-off between planning time and plan quality (RRT* iteration budget)?

📌 Spec: #564 | Q&A: See companion discussion