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? Image courtesy of JPL. Mars 98: Climate Orbiter Mars Polar Lander 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. Create Embedded Languages That Reason on the Fly from Commonsense Models 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 Model-based Embedded Program Obs Cntrl S Plant S Plant Programmer must map between state and sensors/actuators. Model-based executive maps between sensors, actuators to states.
Example: The model-based program sets the state to thrusting, and the deductive controller.... Oxidizer tank Fuel tank Deduces that thrust is off, and the engine is healthy Plans actions to open six valves Deduces that a valve failed - stuck closed Determines that valves on the backup engine will achieve thrust, and plans needed actions. RMPL Model-based Program Titan Model-based Executive Control Program Executes concurrently Preempts Queries (hidden) states Asserts (hidden) state Generates target goal states conditioned Control on Sequencer state estimates System Model State estimates State goals Open Open Closed Valve Close 0.01 0. 01 0. 01 Stuck open Stuck 0.01 closed inflow = outflow = 0 Tracks Mode Mode Deductive likely Controller Estimation plant states Observations Commands Plant Tracks least cost Reconfiguration goal states
Modeling Complex Behaviors through Probabilistic Concurrent Constraint Automata Open Open Closed Valve Close 0.01 0. 01 0. 01 Stuck open Stuck 0.01 closed inflow = outflow = 0 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?
Example Scenario TWO ONE HOME Landing Site: ABC COLLECTION POINTEnroute RENDEZVOUS Landing Site: XYZ SCIENCE AREA 1 Diverge SCIENCE AREA 1 Properties: SCIENCE AREA 3 Teams exploit a hierarchy of complex strategies. Maneuvers are temporally coordinated. Novel events occur during critical phases. Quick responses draw upon a library of contingencies. 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)))))
Self-Adaptive Languages: RAPs [Firby PhD] 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 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
RMPL Model-based Program Control Program Executes concurrently Preempts non-deterministic choice A[l,u] timing A at l location Environment Model Kirk Model-based Executive Selects consistent threads of activity Control from redundant Sequencer methods Schedules and Dispatches Activities Dynamically location estimates location goals HOME Landing Site: ABC Landing Site: XYZ T W O O N E COLLECTION POINT Enroute RENDEZVOUS SCIENCE AREA 1 Diverge SCIENCE AREA 1 Tracks location Mode Mode Deductive Controller Estimation Finds least cost paths Reconfiguration SCIENCE AREA 3 Executive Observations pre-plans activities pre-plans paths Plant dynamically schedules [Tsmardinos et al.] Commands 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): c If c next A Unless c next A A, B Always A Sensing/actuation activities Conditional execution Preemption Full concurrency Iteration Add temporal constraints: A [l,u] Timing Add choice (non-deterministic or decision-theoretic): Choose {A, B} Contingency Example Enroute Activity: Enroute Corridor 2 Rendezvous Rescue Area Corridor 1
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 Activities: 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 Conditionality Group-Enroute()[l,u] = { choose { and Preemption: 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 Sequentiality: Group-Enroute()[l,u] = { choose { Concurrency: 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 Temporal Constraints: 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 } } RMPL for Group-Enroute Non-deterministic Group-Enroute()[l,u] = { choose { choice: 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 } }
Model-Predictive Dispatch for RMPL 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 Reactive Model-based Programming Language RMPL Compiler Temporal Plan Network (TPN) with STN Represents all RMPL executions Reactive Temporal Planner Selects schedulable execution threads of TPN Concurrent Plan Plan = Execution threads related by Simple Temporal Net
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: Start with flexible plan representation 1 Enroute 2 Group Fly Traverse 4 5 Group Wait 9 10 Science Target 8 Group Transmit 11 12 13 Activity (or sub-activity)
Enroute Activity: Start with flexible plan representation 1 Enroute [450,540] 2 Group Traverse Group Wait 4 [405, 486] 5 [0, 54] Science Target 8 Group Transmit [0, ] [0, 2] Activity (or sub-activity) Duration (temporal constraint) Enroute Activity: TPN representation of Enroute activity and sub-activities 1 Enroute 2 Group Fly Path Group Wait 4 5 9 10 Science Target 3 Group Fly Path 8 Group Transmit 13 6 7 11 12 Activity (or sub-activity)
Enroute Activity: Add conditional nodes 1 Enroute [450,540] 2 Group Traverse Group Wait [405, 486] [0, 54] 4 5 Science Target 3 Group Traverse 8 Group Transmit [0, ] [405, 486] [0, 2] Activity (or sub-activity) Duration (temporal constraint) Conditional node Enroute Activity: Add temporally extended, symbolic constraints 1 Enroute [450,540] 2 Group Traverse Group Wait [405, 486] [0, 54] 4 5 9 10 Ask( PATH1 = OK) Science Target Ask( EXPLORE = OK) 3 Group Traverse 8 Group Transmit [0, ] 13 6 [405, 486] 7 11 [0, 2] 12 Ask( PATH2 = OK) Activity (or sub-activity) Duration (temporal constraint) Conditional node Symbolic constraint (Ask,Tell)
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 Group-Enroute 1 2 Group Traverse Group Wait 3 4 5 [405,486] 6 Ask(PATH1=OK) Group Traverse Ask(PATH2=OK) [405,486] 7 [450,540] Science Target 8 11 Ask(PROCEED) 9 10 [0,54] Group Transmit [0,2] 12 [0, ] 1 3 To Plan: Instantiate Group-Enroute
s [0, ] 3 Planning Group-Enroute 1 2 Group Traverse Group Wait 4 5 [405,486] 6 Ask(PATH1=OK) Group Traverse Ask(PATH2=OK) [405,486] Group-Enroute 7 [500,800] [450,540] Science Target 8 11 Ask(PROCEED) 9 10 [0,54] Group Transmit [0,2] 12 [0, ] 1 3 [0, ] e Tell(PATH1=OK) Tell(PROCEED) [10,10] [0, ] 14 15 16 17 [450,450] [200,200] To Plan: Instantiate Group-Enroute Add External Constraints (Tells) s [0, ] 3 Generates Schedulable Plan 1 2 Group Traverse Group Wait 4 5 [405,486] 6 Ask(PATH1=OK) Group Traverse Ask(PATH2=OK) [405,486] Group-Enroute 7 [500,800] [450,540] Science Target 8 11 Ask(PROCEED) 9 10 [0,54] Group Transmit [0,2] 12 [0, ] 13 [0, ] e Tell(PATH1=OK) Tell(PROCEED) [10,10] [0, ] 14 15 16 17 [450,450] [200,200] To Plan: Instantiate Group-Enroute Add External Constraints Trace consistent trajectories Check Schedulability Satisfy and Protect Asks
Planning Example Find paths from start-node to end-node Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18 Planning Example Not a decision-node: Follow all outarcs Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18
Planning Example Not a decision-node: Follow all outarcs Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18 Planning Example Not a decision-node: Follow all outarcs Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18
Planning Example Decision-node: Select a single outarc Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18 Planning Example Not a decision-node: Follow all outarcs Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18
Planning Example Continue Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18 Planning Example Not a decision-node: Follow all outarcs Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18
Planning Example Continue Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18 Planning Example Start End 1 2 7 8 9 3 4 5 6 13 14 10 11 12 15 16 17 18
Temporal Constraint Consistency Don t test consistency at each step. Only when a path induces a cycle, check for negative cycle in the STN distance graph [18,20] 1 2 [4,6] 7 8 9 [15,16] 3 4 5 6 [2,3] 13 14 10 11 12 15 16 17 18 [3,8] [0, ] [5,5] Temporal Constraint Consistency Example: Inconsistent [18,20] 1 2 [4,6] 7 8 9 [15,16] 3 4 5 6 [2,3] 13 14 10 11 12 15 16 17 18 [3,8] [0, ] [5,5]
Temporal Constraint Consistency Backtrack to choice [18,20] 1 2 [4,6] 7 8 9 [15,16] 3 4 5 6 [2,3] 13 14 10 11 12 [12,13] 15 16 17 18 [3,8] [0, ] [5,5] Temporal Constraint Consistency Complete paths [18,20] 1 2 [4,6] 7 8 9 [15,16] 3 4 5 6 [2,3] 13 14 10 11 12 [12,13] 15 16 17 18 [3,8] [0, ] [5,5]
s [0, ] 3 How Do We Handle Asks? 1 2 Group Traverse Group Wait 4 5 [405,486] 6 Ask(PATH1=OK) Group Traverse Ask(PATH2=OK) [405,486] Group-Enroute 7 [500,800] [450,540] Science Target 8 11 Ask(PROCEED) 9 10 [0,54] Group Transmit [0,2] 12 [0, ] 1 3 [0, ] e Tell(PATH1=OK) Tell(PROCEED) [10,10] [0, ] 14 15 16 17 [450,450] [200,200] Unconditional Planning: Guarantee satisfaction at compile time. Treatment similar to causal-link planning Satisfying Asks Compute bounds on activities. Link ask to equivalent, overlapping tell. Constrain tell to contain ask. [4,6] [5,8] [7,11] ask(c) 7 8 9 [4,6} 5 6 [8,11] 10 11 12 [4,6] [6,9] tell(c) [7,10]
Avoiding Threats Identify overlapping Inconsistent activities. [4,6] [5,8] [7,11] tell(c) 7 8 9 [4,6] 5 6 [8,11] 10 11 12 [4,6] [6,9] tell( c) [7,10] Symbolic Constraint Consistency Promote or demote [4,6] [5,8] [7,9] tell(c) 7 8 9 [4,6] 5 [0,inf] 6 [8,11] 10 11 12 [4,6] [7,9] tell( c) [7,10]
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: Closer look at Group Traverse sub-activity 1 Enroute [450,540] 2 Group Traverse Group Wait 4 [405, 486] 5 9 [0, 54] 10 Ask( PATH1 = OK) Science Target Ask( PROCEED) 3 Group Traverse 8 Group Transmit [0, ] 13 6 [405, 486] 7 11 [0, 2] 12 Ask( PATH2 = OK) Traverse to Science Target Group Traverse sub-activity: Traverse through way points to science target Group Traverse [405, 486] 4 5 Ask( PATH2 = OK) 3 8 Group Traverse [405, 486] 6 7 Ask( PATH2 = OK)
Group Traverse sub-activity: One obstacle between nodes 4 and 5 Two Obstacles between nodes 6 and 7 Obstacle 4 5 3 8 Obstacle Obstacle 6 7 Group Traverse sub-activity: Non-explicit representations of obstacles obtained from an incremental collision detection algorithm 4 5 3 8 6 7
RRT: Example Path 1 4 5 3 8 Path 2 6 7 RRT: Example Planner considers rovers taking Path 1: x init 4 Path 1 5 x goal 3 8 Path 2 6 7
RRT: Example Path 1 x init x goal 4 X obs 5 RRT: Example Path 1 x init x goal 4 X obs 5
RRT: Example Path 1 x init x goal 4 X obs 5 RRT: Example Path 1 x init x goal 4 X obs 5
RRT: Example Path 1 x init x goal 4 X obs 5 RRT: Example Path 1 x init x goal 4 X obs 5
RRT: Example Path 1 x init x goal 4 X obs 5 Common Node RRT: Example Path 1 x init x goal 4 X obs 5
RRT: Example Path 1 x init x goal 4 X obs 5 RRT: Example Path 1 x init x goal 4 X obs 5
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, graphbased planning paradigm built upon Simple Temporal Nets. Global optimality achieved by unifying activity planning and global kino-dynamic path planning.