ROBOSYNTH: SMT-Based Synthesis of Integrated Task and Motion Plans
R
O
B
O
S
Y
N
T
H
:
S
M
T
-
B
a
s
e
d
S
y
n
t
h
e
s
i
s
o
f
I
n
t
e
g
r
a
t
e
d
T
a
s
k
a
n
d
M
o
t
i
o
n
P
l
a
n
s
f
r
o
m
P
l
a
n
O
u
t
l
i
n
e
s
S
r
i
n
i
v
a
s
N
e
d
u
n
u
r
i
,
S
a
i
l
e
s
h
P
r
a
b
h
u
,
M
a
r
k
M
o
l
l
,
S
w
a
r
a
t
C
h
a
u
d
h
u
r
i
,
a
n
d
L
y
d
i
a
E
.
K
a
v
r
a
k
i
Department of Computer Science
Rice University
June 2, 2014
O
b
j
e
c
t
i
v
e
o
f
R
O
B
O
S
Y
N
T
H
:
M
a
k
e
i
t
e
a
s
i
e
r
t
o
w
r
i
t
e
t
a
s
k
p
l
a
n
s
t
h
a
t
a
r
e
m
o
t
i
o
n
l
e
v
e
l
f
e
a
s
i
b
l
e
•
Example: Want all dirty dishes in the kitchen cleaned and stored away
T
h
e
r
e
m
a
y
b
e
c
o
n
s
t
r
a
i
n
t
s
o
n
t
h
e
s
o
l
u
t
i
o
n
•
E.g. Constraints on paths followed by the robot
Dishwasher
Countertop
Island
Storage
Food Prep
T
h
e
d
i
f
f
i
c
u
l
t
y
?
T
w
o
d
i
s
t
i
n
c
t
l
a
y
e
r
s
!
•
Task Layer
•
Logical representation of the task
•
D
i
s
c
r
e
t
e
a
b
s
t
r
a
c
t
i
o
n
o
f
t
h
e
r
o
b
o
t
/
e
n
v
i
r
o
n
m
e
n
t
•
Motion Layer
•
Collision-free path generation
•
C
o
n
t
i
n
u
o
u
s
-
c
a
n
n
o
t
i
n
d
i
s
c
r
i
m
i
n
a
t
e
l
y
d
i
s
c
r
e
t
i
z
e
Motion
Layer
Task
Layer
E
.
g
.
,
M
o
v
e
d
i
s
h
e
s
t
o
D
i
s
h
w
a
s
h
e
r
:
C
p
r
o
g
r
a
m
w
i
t
h
s
u
i
t
a
b
l
e
a
b
s
t
r
a
c
t
i
o
n
s
#
define
paths_ok(path1,path2) \
|path1| + |path2| < 10 && !crosses(path1, FOODPREP)
for
(dish=0; dish < NUM_DISHES; dish++)
{ loc1 = findPlace(Dishwasher);while
(path1 = get_path(CURR(), dish.loc))
while
(path2 = get_path(DIRTY[dish].loc, tgt_loc))
if
(paths_ok(path1,path2)) break;
if
(!path1 || !path2) error(…);
pickup(DIRTY[dish],DIRTY[dish].loc,path1);
place(DIRTY[dish],tgt_loc,path2);
}
{ for dish in
DIRTY do
{ findPlace
(?loc1,Dishwasher);
pickup
(dish,?somewhere,?path1);
place
(dish,?loc1,?path2);
}
}
@goal
: contains(Dishwasher,DIRTY)
@invariant
: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
eg. DIRTY = {Cup1,Plate3,Glass5}Program unknown (
hole
)
Set-theoretic constraint
Linear arithmetic constraint
R
O
B
O
S
Y
N
T
H
p
l
a
n
o
u
t
l
i
n
e
actions
Assume a library
of actions and
suitable
abstractions of
underlying
frameworks
R
e
l
a
t
e
d
A
p
p
r
o
a
c
h
e
s
•
Integrated Task and Motion Planning (ITMP)
o
Answer Set solver based [Havir et al. '13, Erdem et al. ’12, Caldiran ‘09]
o
OTS heuristic planner based [Dornhege et al.’13, Srivastava et al. ‘13]
o
Hierarchical Planning in the Now [Kaelbling & Lozano-Perez '11]
o
HTN-based [Marthi et al. ‘08, Wolfe et al. '10]
o
Multiple graphs [Cambon et al.'03]
•
Template driven program synthesis:
o
Solar-Lezama et al. '06, Srivastava et al. '10
•
SAT Planning:
o
Kautz and Selman ‘92, Rintanen '12
I
T
M
P
•
A
n
s
w
e
r
S
e
t
s
o
l
v
e
r
b
a
s
e
d
[
H
a
v
i
r
e
t
a
l
.
,
'
1
3
]
:
U
s
e
a
u
t
o
m
a
t
e
d
s
o
l
v
e
r
,
b
u
t
c
a
u
s
e
o
f
m
o
t
i
o
n
l
e
v
e
l
f
a
i
l
u
r
e
s
n
o
t
p
r
o
p
a
g
a
t
e
d
u
p
t
o
m
o
t
i
o
n
l
e
v
e
l
•
OTS heuristic planner based [Srivastava et al., ’13, Dornhege et al.,’13]:
•
S
r
i
v
a
s
t
a
v
a
e
t
a
l
.
,
’
1
3
:
S
k
o
l
e
m
i
z
e
c
o
n
t
i
n
u
o
u
s
v
a
r
i
a
b
l
e
s
,
i
m
p
l
e
m
e
n
t
c
h
o
i
c
e
f
u
n
c
t
i
o
n
,
D
o
r
n
e
g
e
e
t
a
l
.
,
‘
1
3
:
E
x
p
o
s
e
m
o
t
i
o
n
l
e
v
e
l
p
r
i
m
i
t
i
v
e
s
a
t
t
a
s
k
p
l
a
n
l
e
v
e
l
;
c
o
m
b
i
n
a
t
o
r
i
a
l
l
y
e
x
p
l
o
s
i
v
e
•
H
i
e
r
a
r
c
h
i
c
a
l
P
l
a
n
n
i
n
g
i
n
t
h
e
N
o
w
[
K
a
e
l
b
l
i
n
g
&
L
o
z
a
n
o
-
P
e
r
e
z
,
'
1
1
]
:
S
o
l
v
e
p
l
a
n
n
i
n
g
p
r
o
b
l
e
m
a
t
a
b
s
t
r
a
c
t
l
e
v
e
l
,
t
h
e
n
r
e
f
i
n
e
.
R
e
l
i
e
s
o
n
c
o
m
p
o
s
i
t
i
o
n
a
l
i
t
y
(
?
)
•
H
T
N
-
b
a
s
e
d
[
W
o
l
f
e
e
t
a
l
.
,
'
1
0
]
:
D
o
m
a
i
n
k
n
o
w
l
e
d
g
e
i
n
f
o
r
m
o
f
H
T
N
.
N
e
e
d
s
d
o
m
a
i
n
e
x
p
e
r
t
i
s
e
K
e
y
I
d
e
a
#
1
:
U
s
e
p
r
o
g
r
a
m
m
e
r
k
n
o
w
l
e
d
g
e
!
{ findPlace
(?loc1,Dishwasher);
pickup
(dish,?somewhere,?path1);
place
(dish,?loc1,?path2);
}
@goal
: contains(Dishwasher,DIRTY)
@invariant
: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
For dish
in
DIRTY do
in!
Dish can be moved only once!
K
e
y
I
d
e
a
#
1
:
U
s
e
p
r
o
g
r
a
m
m
e
r
k
n
o
w
l
e
d
g
e
!
{ findPlace
(?loc1,Dishwasher);
pickup
(dish,?somewhere,?path1);
place
(dish,?loc1,?path2);
}
@goal
: contains(Dishwasher,DIRTY)
@invariant
: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
For dish
@pre
: pickup(obj,rgn,_):
while (obstructs(?obst,obj))
{ pickup(?obst,rgn,?pathR);place(?obst,?safeR,?pathR);
}
Event handler
in
DIRTY do
in!
Dish can be moved only once!
Placement
Graph
Generator
I
n
p
u
t
s
a
n
d
O
u
t
p
u
t
o
f
R
O
B
O
S
Y
N
T
H
Placement
Graph
Generator
Solveable?
Formula
Generator
SMT Solver
Plan
Extractor
Plan Outline
Robot Model
Scene Description
Executable Plan
Model
N
Y
Report Problem
Environment
Scene
Key Idea #1
Key Idea #2
Key Idea #3
Placement Graph
ROBOSYNTH
ROBOSYNTH
Formula
A
r
c
h
i
t
e
c
t
u
r
e
o
f
R
O
B
O
S
Y
N
T
H
W
h
a
t
h
a
p
p
e
n
s
w
h
e
n
t
h
e
D
o
m
a
i
n
a
n
d
S
c
e
n
e
a
r
e
c
o
n
f
l
a
t
e
d
Workspace
Only a small part of the
configuration changes
between each move,
These paths need not be planned
out in the entire configuration space
Observations
K
e
y
I
d
e
a
#
2
:
R
e
p
r
e
s
e
n
t
m
o
t
i
o
n
l
e
v
e
l
i
n
f
o
r
m
a
t
i
o
n
t
h
a
t
d
o
e
s
n
’
t
c
h
a
n
g
e
o
v
e
r
p
l
a
n
s
Placement Graph
Robot base movement
Reachability
edge
Blocking
edge
Base
node
location
ROBOSYNTH
tracks actual
object locations
as plan evolves
e2
e3
S
-
p
o
i
n
t
&
B
-
p
o
i
n
t
g
e
n
e
r
a
t
i
o
n
a
n
d
u
s
a
g
e
•
Manually identify place for robot base close to Countertop, Dishwasher, etc. →
B-point
•
Manually identify stable location for object and required end-effector position
→ S-point
•
Collision-aware Inverse Kinematics (IK) solver returns corresponding arm
configuration
•
Motion planner (KPIECE) returns a collision-free path from B-point to S-point
A
r
c
h
i
t
e
c
t
u
r
e
o
f
R
O
B
O
S
Y
N
T
H
Placement
Graph
Generator
Satisfiable?
Formula
Generator
SMT
Solver
Plan
Extractor
Plan Outline
Robot Model
Scene Description
Executable Plan
Model
N
Y
Report Problem
Environment
Scene
A
I
P
l
a
n
n
i
n
g
v
s
.
R
O
B
O
S
Y
N
T
H
A
I
P
l
a
n
n
i
n
g
R
O
B
O
S
Y
N
T
H
Start State
Goal State
Goal State
Given initial & goal states,
determine plan
Given goal state & plan outline, constraint
generator produces
weakest precond.
formula
loc(Cup1)ϵ rchOf(B2) ˄
¬blocks(loc(x),loc(Cup1))
˄
...
holes
I
d
e
a
b
e
h
i
n
d
t
h
e
F
o
r
m
u
l
a
G
e
n
e
r
a
t
o
r
Goal State
Given goal state & plan outline, formula
generator produces
weakest precond.
formula
loc(Cup1)ϵ rchOf(B2) ˄
¬blocks(loc(x),loc(Cup1))
˄
...
holes
W
h
a
t
d
o
e
s
t
h
e
f
o
r
m
u
l
a
r
e
p
r
e
s
e
n
t
?
•
States! Examples:
•
Robot@B2, Cup1@s3, Cup2@d1
•
Robot@B2, Cup1@s2, Cup2@d2
•
Robot@B2, Cup1@s3, Cup2@s2
•
What do these have in common?
•
They are characterized by the formula
loc(Cup1)ϵ rchOf(B2) ˄ ¬
∃
x·blocks(loc(x),loc(Cup1))
C
a
l
c
u
l
a
t
i
n
g
W
e
a
k
e
s
t
P
r
e
c
o
n
d
i
t
i
o
n
E.g.: simple one-action plan outline body
pickup(Cup1,Countertop,?p)
Suppose goal and constraint is
holding(Cup1)
˄
energy(?p) <= BATT_CAP
Then (using defn. of
pickup
action) weakest precondition is
Fluent tracked by ROBOSYNTH
˄ loc(Cup1) ϵ rchOf(
b
)
˄
¬
blocks(loc(
o’
), loc(Cup1))
˄ path(
p
,CURR,
b
)˄
energy(
p
) <= BATT_CAP
Defined using pre and post conditions
Interpreted
over
Placement
graph
loc(Cup1) ϵ Countertop
F
i
l
l
i
n
g
i
n
t
h
e
h
o
l
e
s
i
n
W
e
a
k
e
s
t
P
r
e
c
o
n
d
i
t
i
o
n
K
e
y
I
d
e
a
#
3
Formula
Generator
SMT
Solver
loc(Cup1)
ϵ Countertop
˄ loc(Cup1) ϵ rchOf(
b
))
˄ ¬blocks(loc(
o'
),loc(Cup1))
˄ path(
p
,CURR,
b
)
˄ energy
(
p
) <= BATT_CAP
loc(Cup1)
ϵ Countertop
˄ loc(Cup1) ϵ rchOf(
b
))
˄ ¬blocks(loc(
o'
),loc(Cup1))
˄ path(
p
,CURR,
b
)
˄ energy
(
p
)
<= BATT_CAP
b = B__Ctr
p
= [e5,e7,e1]
model
W
e
a
k
e
s
t
P
r
e
c
o
n
d
i
t
i
o
n
f
o
r
E
n
t
i
r
e
P
l
a
n
O
u
t
l
i
n
e
•
Can do other actions similar to pick (
place, moveTo, findLoc
)
•
Weakest precondition for compound statements (sequence, conditional, for,
while) can be automatically determined using Dijkstra's rules
•
Result: weakest precondition for body of entire plan outline
A
r
c
h
i
t
e
c
t
u
r
e
o
f
R
O
B
O
S
Y
N
T
H
Placement
Graph
Generator
Satisfiable?
Formula
Generator
SMT
Solver
Plan
Extractor
Plan Outline
Robot Model
Scene Description
Executable Plan
Model
N
Y
Report Problem
Environment
Scene
O
u
t
p
u
t
:
c
o
n
c
r
e
t
e
p
l
a
n
e
x
e
c
u
t
e
d
b
y
i
n
t
e
p
r
e
t
e
r
moveTo(B_tmp, e25)
moveTo(B_Ctr, e27)
pick(B_Ctr, s1106)
moveTo(B_DW, e39)
place(B_DW, s1174)
moveTo(B_Stg, e51)
...
Robot base node on placement graph
Edge
on placement graph
Location node on placement graph
R
e
s
u
l
t
s
Locations
Goal: Move DIRTY dishes from arbitrary (but
known) locations to the Dishwasher and then from
Dishwasher to Storage
R
e
s
u
l
t
s
I
:
T
i
m
e
v
s
.
N
u
m
b
e
r
o
f
o
b
j
e
c
t
s
R
e
s
u
l
t
s
I
:
T
i
m
e
v
s
.
N
u
m
b
e
r
o
f
o
b
j
e
c
t
s
R
e
s
u
l
t
s
I
I
:
T
i
m
e
v
s
.
N
u
m
b
e
r
o
f
C
o
n
s
t
r
a
i
n
t
s
for o in! DIRTY_DISHES
...
@goal
: contains(Dishwasher,DIRTY)
@invariant
: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
S
u
m
m
a
r
y
•
Integrated Task and Motion plans can be effectively synthesized by using
programmer knowledge
and
automated solvers
•
3 Main Contributions
•
Plan outline language for programmer to express broad knowledge of plan
•
Compactly represent pre-computed motion level info
•
Handle both plan outlines and (linear) constraints -> automated solvers
•
Future Work
•
Reactivity – allow environment to change from underneath robot
•
Dynamic and lazy construction of placement graph
B
a
s
i
c
P
l
a
n
O
u
t
l
i
n
e
t
o
m
o
v
e
D
I
R
T
Y
d
i
s
h
e
s
{ findPlace
(?loc1,Dishwasher);
pickup
(o,?somewhere,?path1);
place
(o,?loc1,?path2);
}
@goal
: contains(Dishwasher,DIRTY)
@invariant
: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
for o
in
DIRTY do
in
V
i
d
e
o
s
1
a
,
1
b
Locations
Videos show DIRTY dishes can be located anywhere in kitchen and robot path
avoids Food Prep
W
h
e
n
D
I
R
T
Y
d
i
s
h
e
s
o
b
s
t
r
u
c
t
e
a
c
h
o
t
h
e
r
{ findPlace
(?loc1,Dishwasher);
pickup
(o,?somewhere,?path1);
place
(o,?loc1,?path2);
}
@goal
: contains(Dishwasher,DIRTY)
@invariant
: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
for o
in
DIRTY do
In!
V
i
d
e
o
2
a
,
2
b
Locations
Videos show DIRTY dishes can be obstructing each other
W
i
t
h
o
t
h
e
r
o
b
j
e
c
t
s
o
b
s
t
r
u
c
t
i
n
g
D
I
R
T
Y
d
i
s
h
e
s
{ findPlace
(?loc1,Dishwasher);
pickup
(o,?somewhere,?path1);
place
(o,?loc1,?path2);
}
@goal
: contains(Dishwasher,DIRTY)
@invariant
: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
for o
@pre
: pickup(obj,rgn,_):
while (obstructs(?obst,obj))
{ pickup(?obst,rgn,?pathR);place(?obst,?tempR,?pathR);
}
in
DIRTY do
in!
V
i
d
e
o
3
Locations
Video shows DIRTY dishes can be obstructed by some other item
The ROBOSYNTH system aims to facilitate the creation of task plans that are feasible at the motion level by integrating task and motion planning. It provides a structured approach to generating plans, considering constraints on robot paths. The system employs a C program with defined actions and constraints to achieve tasks like moving dishes to the dishwasher. Related approaches include Integrated Task and Motion Planning, OTS heuristic planners, and template-driven program synthesis. The key ideas of ROBOSYNTH involve utilizing programmer knowledge and maintaining invariants during plan generation.
Download Presentation
Please find below an Image/Link to download the presentation.
The content on the website is provided AS IS for your information and personal use only. It may not be sold, licensed, or shared on other websites without obtaining consent from the author. Download presentation by click this link. If you encounter any issues during the download, it is possible that the publisher has removed the file from their server.
E N D
Presentation Transcript
ROBOSYNTH: SMT-Based Synthesis of Integrated Task and Motion Plans from Plan Outlines Srinivas Nedunuri, Sailesh Prabhu, Mark Moll, Swarat Chaudhuri, and Lydia E. Kavraki Department of Computer Science Rice University June 2, 2014
Objective of ROBOSYNTH: Make it easier to write task plans that are motion level feasible Example: Want all dirty dishes in the kitchen cleaned and stored away
There may be constraints on the solution E.g. Constraints on paths followed by the robot Dishwasher Countertop Island Food Prep Storage
E.g., Move dishes to Dishwasher: C program with suitable abstractions actions ROBOSYNTH plan outline #define define paths_ok(path1,path2) \ { for dish in DIRTY do eg. DIRTY = {Cup1,Plate3,Glass5} |path1| + |path2| < 10 && !crosses(path1, FOODPREP) { findPlace(?loc1,Dishwasher); for for (dish=0; dish < NUM_DISHES; dish++) pickup(dish,?somewhere,?path1); Program unknown (hole) { loc1 = findPlace(Dishwasher); place(dish,?loc1,?path2); while while (path1 = get_path(CURR(), dish.loc)) } while while (path2 = get_path(DIRTY[dish].loc, tgt_loc)) } if @goal @goal: contains(Dishwasher,DIRTY) if (paths_ok(path1,path2)) break; Set-theoretic constraint Assume a library of actions and suitable abstractions of underlying frameworks if if (!path1 || !path2) error( ); @invariant @invariant: (||?path|| <= 10) & ~crosses(?path,FoodPrep) pickup(DIRTY[dish],DIRTY[dish].loc,path1); Linear arithmetic constraint place(DIRTY[dish],tgt_loc,path2); }
Related Approaches Integrated Task and Motion Planning (ITMP) o Answer Set solver based [Havir et al. '13, Erdem et al. 12, Caldiran 09] o OTS heuristic planner based [Dornhege et al. 13, Srivastava et al. 13] o Hierarchical Planning in the Now [Kaelbling & Lozano-Perez '11] o HTN-based [Marthi et al. 08, Wolfe et al. '10] o Multiple graphs [Cambon et al.'03] Template driven program synthesis: o Solar-Lezama et al. '06, Srivastava et al. '10 SAT Planning: o Kautz and Selman 92, Rintanen '12
Key Idea #1: Use programmer knowledge! For dish in DIRTY do in! { findPlace(?loc1,Dishwasher); Dish can be moved only once! pickup(dish,?somewhere,?path1); place(dish,?loc1,?path2); } @goal @goal: contains(Dishwasher,DIRTY) @invariant @invariant: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
Inputs and Output of ROBOSYNTH Architecture of ROBOSYNTH Key Idea #1 Key Idea #2 Executable Plan Plan Outline Robot Model Scene Description Environment Scene Placement Graph Placement Graph Generator Generator ROBOSYNTH Plan Extractor Placement Graph Key Idea #3 Formula Generator Model Y Formula SMT Solver Solveable? Report Problem N
Key Idea #2: Represent motion level information that doesn t change over plans Base node ROBOSYNTH tracks actual object locations as plan evolves B_ctr Robot base movement e2 Blocking edge e3 B_stg B_dw Reachability edge s1 s2 s3 d1 d2 d3 location Placement Graph
Architecture of ROBOSYNTH Executable Plan Plan Outline Robot Model Scene Description Environment Scene Placement Graph Generator Plan Extractor Formula Generator Model Y SMT Solver Satisfiable? Report Problem N
Idea behind the Formula Generator loc(Cup1) rchOf(B2) blocks(loc(x),loc(Cup1)) ... Statement 1 holes Statement 2 Statement n Goal State Given goal state & plan outline, formula generator produces weakest precond. formula
Calculating Weakest Precondition E.g.: simple one-action plan outline body pickup(Cup1,Countertop,?p) Defined using pre and post conditions Suppose goal and constraint is holding(Cup1) energy(?p) <= BATT_CAP Then (using defn. of pickup action) weakest precondition is Fluent tracked by ROBOSYNTH loc(Cup1) Countertop loc(Cup1) rchOf(b) Interpreted over Placement graph blocks(loc(o ), loc(Cup1)) path(p,CURR,b) energy(p) <= BATT_CAP
Filling in the holes in Weakest Precondition Key Idea #3 Formula Generator loc(Cup1) Countertop loc(Cup1) rchOf(b)) blocks(loc(o'),loc(Cup1)) path(p,CURR,b) energy(p) <= BATT_CAP path(p,CURR,b) energy(p) <= BATT_CAP loc(Cup1) Countertop loc(Cup1) rchOf(b)) blocks(loc(o'),loc(Cup1)) b = B__Ctr p = [e5,e7,e1] SMT Solver model
Weakest Precondition for Entire Plan Outline Can do other actions similar to pick (place, moveTo, findLoc) Weakest precondition for compound statements (sequence, conditional, for, while) can be automatically determined using Dijkstra's rules Result: weakest precondition for body of entire plan outline
Architecture of ROBOSYNTH Executable Plan Plan Outline Robot Model Scene Description Scene Environment Placement Graph Generator Plan Extractor Formula Generator Model Y SMT Solver Satisfiable? Report Problem N
Output: concrete plan executed by intepreter moveTo(B_tmp, e25) Edge on placement graph moveTo(B_Ctr, e27) pick(B_Ctr, s1106) Robot base node on placement graph moveTo(B_DW, e39) place(B_DW, s1174) Location node on placement graph moveTo(B_Stg, e51) ...
Results Locations Goal: Move DIRTY dishes from arbitrary (but known) locations to the Dishwasher and then from Dishwasher to Storage
Results II: Time vs. Number of Constraints for o in! DIRTY_DISHES ... @goal @goal: contains(Dishwasher,DIRTY) @invariant @invariant: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
Summary Integrated Task and Motion plans can be effectively synthesized by using programmer knowledge and automated solvers 3 Main Contributions Plan outline language for programmer to express broad knowledge of plan Compactly represent pre-computed motion level info Handle both plan outlines and (linear) constraints -> automated solvers Future Work Reactivity allow environment to change from underneath robot Dynamic and lazy construction of placement graph
Basic Plan Outline to move DIRTY dishes for o in DIRTY do in { findPlace(?loc1,Dishwasher); pickup(o,?somewhere,?path1); place(o,?loc1,?path2); } @goal @goal: contains(Dishwasher,DIRTY) @invariant @invariant: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
Videos 1a, 1b Locations Videos show DIRTY dishes can be located anywhere in kitchen and robot path avoids Food Prep
When DIRTY dishes obstruct each other for o in DIRTY do In! { findPlace(?loc1,Dishwasher); pickup(o,?somewhere,?path1); place(o,?loc1,?path2); } @goal @goal: contains(Dishwasher,DIRTY) @invariant @invariant: (||?path|| <= 10) & ~crosses(?path,FoodPrep)
Video 2a, 2b Locations Videos show DIRTY dishes can be obstructing each other
With other objects obstructing DIRTY dishes in DIRTY do in! for o { findPlace(?loc1,Dishwasher); pickup(o,?somewhere,?path1); place(o,?loc1,?path2); } @goal @goal: contains(Dishwasher,DIRTY) @invariant @invariant: (||?path|| <= 10) & ~crosses(?path,FoodPrep) @pre @pre: pickup(obj,rgn,_): while (obstructs(?obst,obj)) { pickup(?obst,rgn,?pathR); place(?obst,?tempR,?pathR); }
Video 3 Locations Video shows DIRTY dishes can be obstructed by some other item
