Executing Model-based Programs Using Graph-based Temporal Planning Prof. Brian C. Williams February 23 rd , 2004 16.412/6.834 Cognitive Robotics based on [Kim,Williams & Abramson, IJCAI01] Outline ? Model-based Programming ? Cooperative Vehicle Missions ? The Reactive Model-based Programming Language (RMPL) ? Temporal Plan Networks (TPN) ? Activity Planning (Kirk) ? Unifying Activity and Path Planning Why Model-based Programming? Create Embedded Languages That Reason on the Fly from Commonsense Models Leading Diagnosis: ?Legs deployed during descent. ? Noise spike on leg sensors latched by monitors. ? Laser altimeter registers 50ft. ? Begins polling leg monitors to determine touch down. ? Latched noise spike read as touchdown. ? Engine shutdown at ~50ft. Mars 98: ? Climate Orbiter ? Mars Polar Lander Model-based Programs Interact Directly with State Embedded programs interact with plant sensors/actuators: ? Read sensors ? Set actuators Model-based programs interact with plant state: ? Read state ?Write state Embedded Program S Plant Obs Cntrl Model-based Embedded Program S Plant Programmer must map between state and sensors/actuators. Model-based executive maps between sensors, actuators to states. Image courtesy of JPL. Example: The model-based program sets the state to thrusting, and the deductive controller . . . . Determines that valves on the backup engine will achieve thrust, and plans needed actions. Deduces that a valve failed - stuck closed Plans actions to open six valves Fuel tankOxidizer tank Deduces that thrust is off, and the engine is healthy Control Sequencer Deductive Controller System Model CommandsObservations Control Program Plant Titan Model-based ExecutiveRMPL Model-based Program State goalsState estimates Generates target goal states conditioned on state estimates Mode Estimation Mode Reconfiguration Tracks likely plant states Tracks least cost goal states z Executes concurrently z Preempts z Queries (hidden) states z Asserts (hidden) state Closed Valve Open Stuck open Stuck closed Open Close 0. 01 0. 01 0.01 0.01 inflow = outflow = 0 Closed Valve Open Stuck open Stuck closed Open Close 0. 01 0. 01 0.01 0.01 inflow = outflow = 0 Modeling Complex Behaviors through Probabilistic Concurrent Constraint Automata ? Complex, discrete behaviors ? modeled through concurrency, hierarchy and non-determinism. ? Anomalies and uncertainty ? modeled by probabilistic transitions ? Physical interactions ? modeled by discrete and continuous constraints Outline ? Model-based Programming ? Cooperative Vehicle Missions ? The Reactive Model-based Programming Language (RMPL) ? Temporal Plan Networks (TPN) ? Activity Planning (Kirk) ? Unifying Activity and Path Planning Cooperative Search and Rescue ? High-level vehicle coordination ? Fast Agile Maneuvering Cooperative Mars Exploration How do we coordinate heterogeneous teams of orbiters, rovers and air vehicles to perform globally optimal science exploration? Properties: z Teams exploit a hierarchy of complex strategies. z Maneuvers are temporally coordinated. z Novel events occur during critical phases. z Quick responses draw upon a library of contingencies. Example Scenario HOME TWO Enroute COLLECTION POINT RENDEZVOUS Diverge SCIENCE AREA 1’ SCIENCE AREA 3 Landing Site: ABC Landing Site: XYZ ONE SCIENCE AREA 1 Self-Adaptive Languages: RAPs [Firby PhD] ? RAPS programs monitor goals and plan activities (define-rap (index (move-to ?thing ?place)) (succeed (LOCATION ?thing ?place)) (method (context (and (LOCATION ?thing ?loc) (not (= ?loc UNKNOWN)))) (task-net (t0 (goto ?loc) ((TRUCK-LOCATION ?loc) for t1)) (t1 (pickup ?thing)((TRUCK-HOLDING ?thing) for t2) ((TRUCK-HOLDING ?thing) for t3)) (t2 (goto ?place) ((TRUCK-LOCATION ?place) for t3)) (t3 (putdown ?thing)))) (method (context (LOCATION ?thing UNKNOWN)) (task-net (t0 (goto WAREHOUSE))))) ? RAPS Programs recover by selecting from functionally redundant methods (define-rap (index (move-to ?thing ?place)) (succeed (LOCATION ?thing ?place)) (method (context(and (LOCATION ?thing ?loc) (not (= ?loc UNKNOWN)))) (task-net (t0 (goto ?loc) ((TRUCK-LOCATION ?loc) for t1)) (t1 (pickup ?thing)((TRUCK-HOLDING ?thing) for t2) ((TRUCK-HOLDING ?thing) for t3)) (t2 (goto ?place) ((TRUCK-LOCATION ?place) for t3)) (t3 (putdown ?thing)))) (method (context (LOCATION ?thing UNKNOWN)) (task-net (t0 (goto WAREHOUSE))))) Self-Adaptive Languages: RAPs [Firby PhD] Self-Adaptive languages: RAPS ? RAPS Exploits contingencies by performing functionally redundant method selection – Methods are chosen based on the current situation. – If a method fails, another is tried instead. – Tasks do not complete until satisfied. – Methods can include monitoring subtasks that deal with contingencies and opportunities. ? Limitations ? Goals must be explicitly observable ? Methods selected reactively ?Method selection may dig itself into a hole. Create Languages with planner-like capabilities Control Sequencer Deductive Controller Environment Model CommandsObservations Control Program Kirk Model-based ExecutiveRMPL Model-based Program location goalslocation estimates Selects consistent threads of activity from redundant methods Mode Estimation Mode Reconfiguration Tracks location Finds least cost paths z Executes concurrently z Preempts z non-deterministic choice z A[l,u] timing z A at l location HOME T W O Enroute COLLECTION POINT RENDEZVOUS Diverge SCIENCE AREA 1’ SCIENCE AREA 3SCIENCE ARE Landing Site: ABC Landing Site: XYZ O N E SCIENCE AREA 1SCIENCE ARE Executive ? pre-plans activities ? pre-plans paths ? dynamically schedules [Tsmardinos et al.] Plant Schedules and Dispatches Activities Dynamically Outline ? Model-based Programming ? Cooperative Vehicle Missions ? Reactive Model-based Programming Language (RMPL) ? Temporal Plan Networks (TPN) ? Activity Planning (Kirk) ? Unifying Activity and Path Planning Reactive Model-based Programming Idea: Describe team behaviors by starting with a rich concurrent, embedded programming language (RMPL,TCC, Esterel): z c z If c next A z Unless c next A z A, B z Always A ? Sensing/actuation activities ? Conditional execution ? Preemption ? Full concurrency ? Iteration z A [l,u] ? Timing z Add temporal constraints: z Choose {A, B} ? Contingency z Add choice (non-deterministic or decision-theoretic): Example Enroute Activity: Rendezvous Rescue Area Corridor 2 Corridor 1 Enroute RMPL for Group-Enroute Group-Enroute()[l,u] = { choose { do { Group-Traverse- Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Traverse- Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)[l*90%,u*90%]; } maintaining PATH2_OK }; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED } } RMPL for Group-Enroute Group-Enroute()[l,u] = { choose { do { Group-Traverse- Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Traverse- Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)[l*90%,u*90%]; } maintaining PATH2_OK }; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED } } Activities: RMPL for Group-Enroute Group-Enroute()[l,u] = { choose { do { Group-Traverse- Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Traverse- Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)[l*90%,u*90%]; } maintaining PATH2_OK }; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED } } Conditionality and Preemption: RMPL for Group-Enroute Group-Enroute()[l,u] = { choose { do { Group-Traverse- Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Traverse- Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)[l*90%,u*90%]; } maintaining PATH2_OK } ; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED } } Sequentiality: Concurrency: RMPL for Group-Enroute Group-Enroute()[l,u] = { choose { do { Group-Fly- Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Fly- Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)[l*90%,u*90%]; } maintaining PATH2_OK }; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED } } Temporal Constraints: RMPL for Group-Enroute Group-Enroute()[l,u] = { choose { do { Group-Traverse- Path(PATH1_1,PATH1_2,PATH1_3,RE_POS)[l*90%,u*90%]; } maintaining PATH1_OK, do { Group-Traverse- Path(PATH2_1,PATH2_2,PATH2_3,RE_POS)[l*90%,u*90%]; } maintaining PATH2_OK }; { Group-Transmit(OPS,ARRIVED)[0,2], do { Group-Wait(HOLD1,HOLD2)[0,u*10%] } watching PROCEED } } Non-deterministic choice: ? How do we provide fast, temporally flexible planning? ? Graph-based planners support fast planning. ? … but plans are totally order. ? Desire flexible plans based on simple temporal networks (e.g., HSTS, Muscetola et al.). How do we create temporally flexible plan graphs? ? Generalize simple temporal networks (temporal plan network TPN). Model-Predictive Dispatch for RMPL RMPL Compiler Temporal Plan Network (TPN) with STN Reactive Temporal Planner z Selects schedulable execution threads of TPN Reactive Model-based Programming Language Concurrent Plan z Plan = Execution threads related by Simple Temporal Net z Represents all RMPL executions Model-Predictive Dispatch for RMPL Outline ? Model-based Programming ? Cooperative Vehicle Missions ? Reactive Model-based Programming Language (RMPL) ? Temporal Plan Networks (TPN) ? Activity Planning (Kirk) ? Unifying Activity and Path Planning Enroute Activity: 1 4 5 8 9 10 13 2 11 12 Enroute Group Fly Traverse Group Wait Group Transmit Activity (or sub-activity) Science Target ? Start with flexible plan representation Enroute Activity: 1 4 5 8 2 Enroute [450,540] [405, 486] Group Traverse Group Wait Group Transmit [0, 54] [0, 2] Activity (or sub-activity) Duration (temporal constraint) [0, ] [0, 0][0, 0] [0, 0] [0, 0] [0, 0] [0, 0] Science Target ? Start with flexible plan representation Enroute Activity: 3 1 4 5 8 9 10 13 2 6 7 11 12 Enroute Group Fly Path Group Fly Path Group Wait Group Transmit Activity (or sub-activity) Science Target ?TPN representation of Enroute activity and sub-activities Enroute Activity: 3 1 4 5 8 2 Enroute [450,540] Group Traverse [405, 486] [405, 486] Group Traverse Group Wait Group Transmit [0, 54] [0, 2] Activity (or sub-activity) Duration (temporal constraint) [0, ] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] Science Target ? Add conditional nodes Conditional node Enroute Activity: 3 1 4 5 8 9 10 13 2 6 7 11 12 Enroute [450,540] Group Traverse [405, 486] [405, 486] Group Traverse Group Wait Group Transmit [0, 54] [0, 2] Activity (or sub-activity) Duration (temporal constraint) [0, ] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] Ask( PATH1 = OK) Ask( PATH2 = OK) Ask( EXPLORE = OK) Science Target ?Add temporally extended, symbolic constraints Symbolic constraint (Ask,Tell) Conditional node Outline ? Cooperative Vehicle Missions ? Model-based Programming ? Reactive Model-based Programming Language (RMPL) ? Temporal Plan Networks (TPN) ? Activity Planning (Kirk) ? Unifying Activity and Path Planning Planning Group-Enroute 3 6 4 5 [405,486] Ask(PATH1=OK) 1 2 7 Ask(PATH2=OK) 8 [405,486] [450,540] Ask(PROCEED) 11 9 10 [0,54] 12 1 3 [0,2] [0,∞] To Plan: ? Instantiate Group-Enroute Group-Enroute Group Traverse Group Traverse Group Wait Group Transmit Science Target Planning Group-Enroute 3 6 4 5 [405,486] Ask(PATH1=OK) 1 2 7 Ask(PATH2=OK) 8 [405,486] [450,540] Ask(PROCEED) 11 9 10 [0,54] 12 1 3 [0,2] [0,∞] [0,∞] [0,∞] 14 15 Tell(PATH1=OK) [450,450] 16 17 Tell(PROCEED) [200,200] s e [500,800] [10,10] [0,∞] To Plan: ? Instantiate Group-Enroute ? Add External Constraints (Tells) Group-Enroute Group Traverse Group Traverse Group Wait Group Transmit Science Target Generates Schedulable Plan 3 6 4 5 [405,486] Ask(PATH1=OK) 1 2 7 Ask(PATH2=OK) 8 [405,486] [450,540] Ask(PROCEED) 11 9 10 [0,54] 12 13 [0,2] [0,∞] 14 15 Tell(PATH1=OK) [450,450] 16 17 Tell(PROCEED) [200,200] s e [500,800] [10,10] [0,∞] [0,∞] [0,∞] Group-Enroute Group Traverse Group Traverse Group Wait Group Transmit Science Target Trace consistent trajectories ? Check Schedulability ? Satisfy and Protect Asks To Plan: ? Instantiate Group-Enroute ? Add External Constraints Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Find paths from start-node to end-node Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Not a decision-node: Follow all outarcs Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Not a decision-node: Follow all outarcs Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Not a decision-node: Follow all outarcs Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Decision-node: Select a single outarc Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Not a decision-node: Follow all outarcs Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Continue Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Not a decision-node: Follow all outarcs Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Continue Start End 15 16 17 18 Planning Example 1 2 3 4 5 6 7 8 9 10 11 12 13 14 Start End 15 16 17 18 Temporal Constraint Consistency 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Don’t test consistency at each step. ? Only when a path induces a cycle, check for negative cycle in the STN distance graph 15 16 17 18 [18,20] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,∞] [2,3] [15,16] [4,6] [5,5][3,8] Temporal Constraint Consistency 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ?Example: Inconsistent 15 16 17 18 [18,20] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,∞] [2,3] [15,16] [4,6] [5,5][3,8] Temporal Constraint Consistency 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Backtrack to choice 15 16 17 18 [18,20] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,∞] [2,3] [15,16] [4,6] [5,5][3,8] [0,0] [0,0] [12,13] [0,0] Temporal Constraint Consistency 1 2 3 4 5 6 7 8 9 10 11 12 13 14 ? Complete paths 15 16 17 18 [18,20] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,0] [0,∞] [2,3] [15,16] [4,6] [5,5][3,8] [0,0] [0,0] [12,13] [0,0] How Do We Handle Asks? 3 6 4 5 [405,486] Ask(PATH1=OK) 1 2 7 Ask(PATH2=OK) 8 [405,486] [450,540] Ask(PROCEED) 11 9 10 [0,54] 12 1 3 [0,2] [0,∞] [0,∞] [0,∞] 14 15 Tell(PATH1=OK) [450,450] 16 17 Tell(PROCEED) [200,200] s e [500,800] [10,10] [0,∞] Unconditional Planning: ? Guarantee satisfaction at compile time. ?Treatment similar to causal-link planning Group-Enroute Group Traverse Group Traverse Group Wait Group Transmit Science Target Satisfying Asks ? Compute bounds on activities. ? Link ask to equivalent, overlapping tell. ? Constrain tell to contain ask. 5 7 8 9 10 11 12 6 [4,6} [4,6] [4,6] [6,9] [5,8] [7,11] [7,10] [8,11] ask(c) tell(c) Avoiding Threats ? Identify overlapping Inconsistent activities. 5 7 8 9 10 11 12 6 [4,6] [4,6] [4,6] [6,9] [5,8] [7,11] [7,10] [8,11] tell(c) tell(?c) Symbolic Constraint Consistency ? Promote or demote 5 7 8 9 10 11 12 6 [4,6] [4,6] [4,6] [7,9] [5,8] [7,9] [7,10] [8,11] tell(c) tell(?c) [0,inf] Outline ? Cooperative Vehicle Missions ? Model-based Programming ? Reactive Model-based Programming Language (RMPL) ? Temporal Plan Networks (TPN) ? Activity Planning (Kirk) ? Unifying Activity and Path Planning How do we optimally select activities and paths? Current Research: ? Perform global path planning using Rapidly-exploring Random Trees (RRTs) (la Valle). ? Search for globally optimal plan by unifying TPN & RRT graphs, and by searching hybrid graph best first. ? Refine plan using receding horizon control based on CLP (Krishnan, Williams), or MILP (How) or hybrid maneuver automata (Frazzoli, Dahleh, Feron). Enroute Activity: 3 1 4 5 8 9 10 13 2 6 7 11 12 Enroute [450,540] Group Traverse [405, 486] [405, 486] Group Traverse Group Wait Group Transmit [0, 54] [0, 2] [0, ] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] [0, 0] Ask( PATH1 = OK) Ask( PATH2 = OK) Ask( PROCEED) Traverse to Science Target Science Target ?Closer look at Group Traverse sub-activity Group Traverse sub-activity: 3 4 8 6 7 [0, 0] [0, 0] Ask( PATH2 = OK) Ask( PATH2 = OK) 5 ?Traverse through way points to science target Group Traverse [405, 486] Group Traverse [405, 486] [0, 0] [0, 0] 3 4 8 6 7 [0, 0] [0, 0] 5 ?One obstacle between nodes 4 and 5 ?Two Obstacles between nodes 6 and 7 [0, 0] [0, 0] Group Traverse sub-activity: Obstacle Obstacle Obstacle 3 4 8 6 7 [0, 0] [0, 0] 5 ?Non-explicit representations of obstacles obtained from an incremental collision detection algorithm [0, 0] [0, 0] Group Traverse sub-activity: RRT: Example 3 4 8 6 7 [0, 0] [0, 0] 5 [0, 0] [0, 0] Path 1 Path 2 3 4 8 6 7 [0, 0] [0, 0] 5 [0, 0] [0, 0] x init x goal Planner considers rovers taking Path 1: Path 1 Path 2 RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs Common Node RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example 4 5 x init Path 1 x goal X obs RRT: Example Model-based Cooperative Programming Goal: Fast, mission-directed coordination of teams of vehicles acting in an uncertain environments. Solution: New middle ground between embedded programming, task decomposition execution, and temporal planning. ? Rich embedded language,RMPL, for describing complex concurrent team strategies extended to time and contingency. ? Kirk Interpreter “looks” for schedulable threads of execution before “leaping” to execution. ? Temporal Plan Network provides a flexible, temporal, graph- based planning paradigm built upon Simple Temporal Nets. ? Global optimality achieved by unifying activity planning and global kino-dynamic path planning.