Upload qads/planner/hybrid.py

qads/planner/hybrid.py

@@ -0,0 +1,280 @@
+"""Hybrid Planner: combines classical planning with quantum optimization."""
+import numpy as np
+import time
+from typing import Dict, Any, Optional, Tuple, List
+from .graph import WorldGraph
+from .classical import AStarPlanner, DStarPlanner, RRTStarPlanner
+
+
+class HybridPlanner:
+    """
+    Hybrid classical + quantum planner with entropy-based activation.
+    
+    Simple environments: classical planner only
+    Complex uncertain environments: quantum planner activated
+    """
+    
+    def __init__(self, 
+                 config: Any,
+                 quantum_core: Optional[Any] = None):
+        self.config = config
+        self.quantum_core = quantum_core
+        
+        # Classical planners
+        self.classical_planners = {
+            'astar': AStarPlanner(),
+            'dstar': DStarPlanner(),
+            'rrt_star': RRTStarPlanner()
+        }
+        
+        # Metrics
+        self.stats = {
+            'classical_calls': 0,
+            'quantum_calls': 0,
+            'total_plans': 0,
+            'avg_classical_time': 0.0,
+            'avg_quantum_time': 0.0
+        }
+    
+    def plan(self,
+             start: Tuple[float, ...],
+             goal: Tuple[float, ...],
+             world_state: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
+        """
+        Generate plan with automatic quantum activation.
+        """
+        self.stats['total_plans'] += 1
+        
+        # Build or use world graph
+        graph = self._build_graph(world_state)
+        
+        # Check if quantum should be activated
+        use_quantum = self._should_activate_quantum(graph, world_state)
+        
+        if use_quantum and self.quantum_core is not None:
+            return self._quantum_plan(graph, start, goal, world_state)
+        else:
+            return self._classical_plan(graph, start, goal)
+    
+    def _build_graph(self, world_state: Optional[Dict[str, Any]]) -> WorldGraph:
+        """Build world graph from state or create default."""
+        if world_state is None:
+            # Create default grid
+            graph = WorldGraph(resolution=self.config.grid_resolution)
+            graph.build_grid(
+                bounds=((0, 20), (0, 20)),
+                obstacle_map=None,
+                uncertainty_map=None
+            )
+            return graph
+        
+        # Build from world state
+        bounds = world_state.get('bounds', ((0, 20), (0, 20)))
+        obstacle_map = world_state.get('obstacle_map')
+        uncertainty_map = world_state.get('uncertainty_map')
+        resolution = world_state.get('resolution', self.config.grid_resolution)
+        
+        graph = WorldGraph(resolution=resolution)
+        graph.build_grid(bounds=bounds, 
+                        obstacle_map=obstacle_map,
+                        uncertainty_map=uncertainty_map)
+        
+        # Add any additional nodes/edges from world state
+        obstacles = world_state.get('obstacles', [])
+        for obs in obstacles:
+            pos = obs['position']
+            node_id = graph.find_node_at(tuple(pos))
+            if node_id is not None:
+                graph.update_node(node_id, 
+                                 obstacle_prob=obs.get('probability', 1.0),
+                                 risk=obs.get('risk', 1.0))
+        
+        return graph
+    
+    def _should_activate_quantum(self, 
+                                  graph: WorldGraph,
+                                  world_state: Optional[Dict[str, Any]]) -> bool:
+        """Determine if quantum planner should be activated."""
+        # Direct check from world state
+        if world_state is not None:
+            entropy = world_state.get('entropy', 0.0)
+            uncertainty = world_state.get('uncertainty', 0.0)
+            obstacle_density = world_state.get('obstacle_density', 0.0)
+            
+            # Activation conditions
+            activate = (
+                entropy > self.config.activation_entropy or
+                uncertainty > 0.5 or
+                obstacle_density > 0.4
+            )
+            
+            if activate:
+                return True
+        
+        # Check graph metrics
+        graph_entropy = graph.get_entropy()
+        graph_uncertainty = graph.get_uncertainty()
+        graph_obstacle = graph.get_obstacle_density()
+        
+        return (
+            graph_entropy > self.config.activation_entropy or
+            graph_uncertainty > 0.5 or
+            graph_obstacle > 0.4
+        )
+    
+    def _classical_plan(self, 
+                       graph: WorldGraph,
+                       start: Tuple[float, ...],
+                       goal: Tuple[float, ...]) -> Dict[str, Any]:
+        """Classical planning path."""
+        start_time = time.time()
+        planner = self.classical_planners.get(
+            self.config.classical_planner, 
+            AStarPlanner()
+        )
+        
+        result = planner.plan(graph, start, goal)
+        elapsed = time.time() - start_time
+        
+        self.stats['classical_calls'] += 1
+        self.stats['avg_classical_time'] = (
+            (self.stats['avg_classical_time'] * (self.stats['classical_calls'] - 1) + elapsed) /
+            self.stats['classical_calls']
+        )
+        
+        result.update({
+            'quantum_activated': False,
+            'plan_time': elapsed,
+            'start': start,
+            'goal': goal
+        })
+        
+        # Convert path to actions
+        result['actions'] = self._path_to_actions(result.get('path_positions', []))
+        result['goal_reached'] = result.get('success', False)
+        
+        return result
+    
+    def _quantum_plan(self,
+                     graph: WorldGraph,
+                     start: Tuple[float, ...],
+                     goal: Tuple[float, ...],
+                     world_state: Optional[Dict[str, Any]]) -> Dict[str, Any]:
+        """Quantum-enhanced planning path."""
+        start_time = time.time()
+        
+        # Step 1: Generate classical candidates
+        classical_results = []
+        for planner_name, planner in self.classical_planners.items():
+            r = planner.plan(graph, start, goal)
+            if r['success']:
+                classical_results.append(r)
+        
+        if not classical_results:
+            # Fall back to classical only
+            return self._classical_plan(graph, start, goal)
+        
+        # Step 2: Generate trajectory candidates from classical plans
+        trajectories = []
+        for r in classical_results:
+            positions = np.array(r['path_positions'])
+            if len(positions) > 0:
+                trajectories.append(positions)
+        
+        # Step 3: Quantum evaluation of trajectories
+        if self.quantum_core is not None and trajectories:
+            # Compute world state for quantum evaluation
+            world_state_for_quantum = {
+                'entropy': graph.get_entropy(),
+                'uncertainty': graph.get_uncertainty(),
+                'obstacle_density': graph.get_obstacle_density(),
+                'risk_score': np.mean([m['risk'] for m in graph.node_metadata.values()])
+            }
+            
+            evaluated = self.quantum_core.evaluate_trajectories(
+                trajectories, world_state_for_quantum
+            )
+            
+            # Select best trajectory
+            if evaluated:
+                best = evaluated[0]
+                best_traj = best['trajectory']
+                
+                # Convert back to node IDs
+                path = []
+                path_positions = []
+                for pos in best_traj:
+                    path_positions.append(tuple(pos))
+                    # Find nearest node
+                    node_id = None
+                    for nid, npos in graph.node_positions.items():
+                        if np.allclose(npos, pos, atol=graph.resolution):
+                            node_id = nid
+                            break
+                    if node_id is not None:
+                        path.append(node_id)
+                
+                elapsed = time.time() - start_time
+                self.stats['quantum_calls'] += 1
+                self.stats['avg_quantum_time'] = (
+                    (self.stats['avg_quantum_time'] * (self.stats['quantum_calls'] - 1) + elapsed) /
+                    self.stats['quantum_calls']
+                )
+                
+                result = {
+                    'path': path,
+                    'path_positions': path_positions,
+                    'cost': best['quantum_score'],
+                    'success': True,
+                    'explored': len(classical_results),
+                    'planner': 'hybrid_quantum',
+                    'quantum_activated': True,
+                    'quantum_score': best['quantum_score'],
+                    'entropy': best['entropy'],
+                    'confidence': best['confidence'],
+                    'uncertainty': best['uncertainty'],
+                    'plan_time': elapsed,
+                    'start': start,
+                    'goal': goal,
+                    'goal_reached': True,
+                    'actions': self._path_to_actions(path_positions)
+                }
+                return result
+        
+        # Quantum failed or not available, use best classical
+        best_classical = min(classical_results, key=lambda x: x['cost'])
+        elapsed = time.time() - start_time
+        
+        self.stats['classical_calls'] += 1
+        best_classical.update({
+            'quantum_activated': False,
+            'quantum_score': 0.0,
+            'plan_time': elapsed,
+            'goal_reached': best_classical.get('success', False)
+        })
+        best_classical['actions'] = self._path_to_actions(
+            best_classical.get('path_positions', [])
+        )
+        return best_classical
+    
+    def _path_to_actions(self, path_positions: List[Tuple[float, ...]]) -> List[np.ndarray]:
+        """Convert path positions to action vectors."""
+        if len(path_positions) < 2:
+            return []
+        
+        actions = []
+        for i in range(len(path_positions) - 1):
+            current = np.array(path_positions[i])
+            next_pos = np.array(path_positions[i + 1])
+            action = next_pos - current
+            actions.append(action)
+        
+        return actions
+    
+    def update_world_state(self, world_state: Dict[str, Any]):
+        """Update planner with new world state."""
+        pass  # State is rebuilt each plan call
+    
+    def get_stats(self) -> Dict[str, Any]:
+        """Get planning statistics."""
+        return self.stats.copy()