Tutorial 3.2: Solving Mixed Discrete-Continuous Planning with Optimistic Search#
[1]:
from typing import List
import torch
import jacinle
import concepts.dm.pdsketch as pds
from concepts.dsl.tensor_value import v
[2]:
domain_string = r"""
(define (domain shapesetting)
(:types
tile - object
container - object
pose - vector[float32, 2]
cshape - vector[float32, 4]
identifier - int64
)
(:predicates
;; part 1: boolean predicates
(placed ?t - tile)
;; part 2: continuous-valued predicates
(pose ?t - tile -> pose)
(tile-identifier ?t - tile -> identifier)
(container-shape ?c - container -> cshape)
;; functions
(in-container ?ti - identifier ?tp - pose ?cshape - cshape)
(left-of ?t1 - identifier ?t2 - identifier ?tp1 - pose ?tp2 - pose)
(right-of ?t1 - identifier ?t2 - identifier ?tp1 - pose ?tp2 - pose)
(collision-free ?t1 - identifier ?t2 - identifier ?tp1 - pose ?tp2 - pose)
)
(:derived (left-of-t ?t1 - tile ?t2 - tile)
(left-of (tile-identifier ?t1) (tile-identifier ?t2) (pose ?t1) (pose ?t2))
)
(:derived (right-of-t ?t1 - tile ?t2 - tile)
(right-of (tile-identifier ?t1) (tile-identifier ?t2) (pose ?t1) (pose ?t2))
)
(:derived (collision-free-t ?t1 - tile ?t2 - tile)
(collision-free (tile-identifier ?t1) (tile-identifier ?t2) (pose ?t1) (pose ?t2))
)
(:derived (in-container-t ?t - tile ?c - container)
(in-container (tile-identifier ?t) (pose ?t) (container-shape ?c))
)
(:action place
:parameters (?t - tile ?p - pose)
:precondition (and
(not (placed ?t))
)
:effect (and
(placed ?t)
(pose::assign ?t ?p) ;; equivalent to (assign (pose ?t) ?p)
)
)
; (:generator gen-in-container-pose
; :parameters (?t - tile ?c - container)
; :certifies (in-container-t ?t ?c)
; :context (and
; (tile-identifier ?t)
; (container-shape ?c)
; )
; :generates (and
; (pose ?t)
; )
; )
;; the upper generate definition is equivalent to:
(:generator gen-in-container-pose
:parameters (?ti - identifier ?tp - pose ?c - cshape)
:certifies (in-container ?ti ?tp ?c)
:context (and ?ti ?c)
:generates (and ?tp)
)
) ;; end of domain definition
"""
[3]:
domain = pds.load_domain_string(domain_string)
domain.print_summary()
Domain shapesetting
Types: dict{
container: container
cshape: cshape
identifier: identifier
pose: pose
tile: tile
}
Functions: dict{
collision-free: collision-free(?t1: identifier, ?t2: identifier, ?tp1: pose, ?tp2: pose) -> bool
collision-free-t: collision-free-t[cacheable](?t1: tile, ?t2: tile) -> bool {
collision-free(tile-identifier(V::?t1), tile-identifier(V::?t2), pose(V::?t1), pose(V::?t2))
}
container-shape: container-shape[observation, state, cacheable, static](?c: container) -> cshape
in-container: in-container(?ti: identifier, ?tp: pose, ?cshape: cshape) -> bool
in-container-t: in-container-t[cacheable](?t: tile, ?c: container) -> bool {
in-container(tile-identifier(V::?t), pose(V::?t), container-shape(V::?c))
}
left-of: left-of(?t1: identifier, ?t2: identifier, ?tp1: pose, ?tp2: pose) -> bool
left-of-t: left-of-t[cacheable](?t1: tile, ?t2: tile) -> bool {
left-of(tile-identifier(V::?t1), tile-identifier(V::?t2), pose(V::?t1), pose(V::?t2))
}
placed: placed[observation, state, cacheable](?t: tile) -> bool
pose: pose[observation, state, cacheable](?t: tile) -> pose
right-of: right-of(?t1: identifier, ?t2: identifier, ?tp1: pose, ?tp2: pose) -> bool
right-of-t: right-of-t[cacheable](?t1: tile, ?t2: tile) -> bool {
right-of(tile-identifier(V::?t1), tile-identifier(V::?t2), pose(V::?t1), pose(V::?t2))
}
tile-identifier: tile-identifier[observation, state, cacheable, static](?t: tile) -> identifier
}
External Functions: dict{
generator::gen-in-container-pose: generator::gen-in-container-pose(?c0: identifier, ?c1: cshape) -> pose
predicate::collision-free: collision-free(?t1: identifier, ?t2: identifier, ?tp1: pose, ?tp2: pose) -> bool
predicate::in-container: in-container(?ti: identifier, ?tp: pose, ?cshape: cshape) -> bool
predicate::left-of: left-of(?t1: identifier, ?t2: identifier, ?tp1: pose, ?tp2: pose) -> bool
predicate::right-of: right-of(?t1: identifier, ?t2: identifier, ?tp1: pose, ?tp2: pose) -> bool
type::cshape::equal: type::cshape::equal(#0: cshape, #1: cshape) -> bool
type::identifier::equal: type::identifier::equal(#0: identifier, #1: identifier) -> bool
type::pose::equal: type::pose::equal(#0: pose, #1: pose) -> bool
}
Generators: dict{
gen-in-container-pose: gen-in-container-pose(V::?ti, V::?c) -> V::?tp {
generator::gen-in-container-pose(?c0: identifier, ?c1: cshape) -> pose
parameters: (Variable<?ti: identifier>, Variable<?tp: pose>, Variable<?c: cshape>)
certifies: in-container(V::?c0, V::?g0, V::?c1)
context: (VariableExpression<V::?ti>, VariableExpression<V::?c>)
generates: (VariableExpression<V::?tp>,)
}
}
Fancy Generators: dict{
}
Operators:
(:action place
:parameters (?t: tile ?p: pose)
:precondition (and
not(placed(V::?t))
)
:effect (and
assign(placed(V::?t): Const::1)
assign(pose(V::?t): V::?p)
)
)
Axioms:
<Empty>
Regression Rules:
<Empty>
[4]:
@pds.config_function_implementation(include_executor_args=True)
def left_of(executor, t1, t2, p1, p2):
env: MyEnvironment = executor.environment
assert env.tile_shapes[t1.item()] == 'box'
assert env.tile_shapes[t2.item()] == 'box'
# print('calling left_of', t1, t2, p1, p2)
return p1[0] < p2[0]
def right_of(t1, t2, p1, p2):
return p1[0] > p2[0]
def collision_free(t1, t2, p1, p2):
return torch.ones_like(t1)
@pds.config_function_implementation(include_executor_args=True)
def in_container(executor, ti, tp, cshape):
env: MyEnvironment = executor.environment
assert env.tile_shapes[ti.item()] == 'box'
x1, y1, w, h = cshape[..., 0], cshape[..., 1], cshape[..., 2], cshape[..., 3]
x2, y2 = x1 + w, y1 + h
x, y = tp[..., 0], tp[..., 1]
return (x1 <= x) & (x <= x2) & (y1 <= y) & (y <= y2)
# if you don't want to use the "unwrap_values" helper function
# you can also use the following code:
# because all arguments are wrapped in a "Value" object.
# def left_of(t1, t2, p1, p2):
# return p1.tensor[0] < p2.tensor[0]
def gen_in_container_pose(ti, cshape):
return torch.rand(2) * cshape[..., 2:] + cshape[..., :2]
class MyEnvironment(object):
def __init__(self, tile_shapes: List[str]):
self.tile_shapes = tile_shapes
[5]:
executor = pds.PDSketchExecutor(domain)
executor.register_function_implementation('predicate::left-of', left_of)
executor.register_function_implementation('predicate::right-of', right_of)
executor.register_function_implementation('predicate::collision-free', collision_free)
executor.register_function_implementation('predicate::in-container', in_container)
executor.register_function_implementation('generator::gen-in-container-pose', gen_in_container_pose)
env = MyEnvironment(['box', 'box'])
executor.environment = env
state, ctx = executor.new_state({
'container': domain.types['container'],
'tile1': domain.types['tile'],
'tile2': domain.types['tile']
}, create_context=True)
# for all boolean predicates, write "true-valued" propositions as a list:
# ctx.define_predicates([ctx.placed('tile1')])
ctx.define_predicates([])
# for all continuous-valued predicates, use the "define_feature" function:
ctx.define_feature('pose', torch.zeros((2, 2), dtype=torch.float32))
ctx.define_feature('tile-identifier', torch.tensor([0, 1], dtype=torch.int64))
ctx.define_feature('container-shape', torch.tensor([
[0, 0, 10, 10]
], dtype=torch.float32)) # tensor shape is [1, 4]
print('Initial state:', state)
goal = r'''(and
(placed tile1)
(placed tile2)
(left-of-t tile1 tile2)
(collision-free-t tile1 tile2)
(in-container-t tile1 container)
(in-container-t tile2 container)
)'''
print('Goal:', repr(executor.parse(goal)))
Initial state: State{
objects: container: [container]; tile: [tile1, tile2]
states:
- container-shape: Value[cshape, axes=[?c], tdtype=torch.float32, tdshape=(1, 4)]{
tensor([[ 0., 0., 10., 10.]])
}
- pose: Value[pose, axes=[?t], tdtype=torch.float32, tdshape=(2, 2)]{
tensor([[0., 0.],
[0., 0.]])
}
- placed: Value[bool, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([0, 0])}
- tile-identifier: Value[identifier, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([0, 1])}
}
Goal: AndExpression<and(placed(OBJ::tile1), placed(OBJ::tile2), left-of-t(OBJ::tile1, OBJ::tile2), collision-free-t(OBJ::tile1, OBJ::tile2), in-container-t(OBJ::tile1, OBJ::container), in-container-t(OBJ::tile2, OBJ::container))>
[6]:
action_list = [
# v stands for "vector"
domain.operators['place']('tile1', v(1, 1, dtype=domain.types['pose'])),
domain.operators['place']('tile2', v(2, 2, dtype=domain.types['pose'])),
]
s = state.clone()
for action in action_list:
succ, ns = executor.apply(action, s)
assert succ
s = ns
print('Landing state:', s)
print('Evaluating goal:', executor.execute(goal, state))
Landing state: State{
objects: container: [container]; tile: [tile1, tile2]
states:
- container-shape: Value[cshape, axes=[?c], tdtype=torch.float32, tdshape=(1, 4)]{
tensor([[ 0., 0., 10., 10.]])
}
- pose: Value[pose, axes=[?t], tdtype=torch.float32, tdshape=(2, 2)]{
tensor([[1., 1.],
[2., 2.]])
}
- placed: Value[bool, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([1, 1])}
- tile-identifier: Value[identifier, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([0, 1])}
}
Evaluating goal: Value[bool, axes=[], tdtype=torch.int64, tdshape=(), quantized]{tensor(0)}
[7]:
from concepts.dm.pdsketch.planners.optimistic_search import construct_csp_from_optimistic_plan, ground_actions
from concepts.dm.pdsketch.csp_solvers.dpll_sampling import csp_dpll_sampling_solve
from concepts.dsl.constraint import print_assignment_dict
action_list = [
domain.operators['place']('tile1', '??'),
domain.operators['place']('tile2', '??'),
]
optimistic_action_list, csp = construct_csp_from_optimistic_plan(executor, state, goal, action_list)
print('Optimistic action list:')
for action in optimistic_action_list:
print(jacinle.indent_text(action))
print('-' * 40)
print('CSP:', csp)
print('-' * 40)
solution = csp_dpll_sampling_solve(executor, csp)
print('Solution:')
print_assignment_dict(solution)
print('-' * 40)
print('Insert back into the action list:')
solution_action_list = ground_actions(executor, optimistic_action_list, solution)
for action in solution_action_list:
print(jacinle.indent_text(action))
Optimistic action list:
action::place(?t=tile1, ?p=@0)
action::place(?t=tile2, ?p=@1)
----------------------------------------
CSP: ConstraintSatisfactionProblem{
Variables:
@0 - pose (actionable=True)
@1 - pose (actionable=True)
@2 - bool (actionable=False)
@3 - bool (actionable=False)
@4 - bool (actionable=False)
@5 - bool (actionable=False)
@6 - bool (actionable=False)
Constraints:
left-of(0, 1, O[pose]{@0}, O[pose]{@1}) == O[bool]{@2} # left-of(tile-identifier(V::?t1), tile-identifier(V::?t2), pose(V::?t1), pose(V::?t2))
collision-free(0, 1, O[pose]{@0}, O[pose]{@1}) == O[bool]{@3} # collision-free(tile-identifier(V::?t1), tile-identifier(V::?t2), pose(V::?t1), pose(V::?t2))
in-container(0, O[pose]{@0}, [0.0, 0.0, 10.0, 10.0]) == O[bool]{@4} # in-container(tile-identifier(V::?t), pose(V::?t), container-shape(V::?c))
in-container(1, O[pose]{@1}, [0.0, 0.0, 10.0, 10.0]) == O[bool]{@5} # in-container(tile-identifier(V::?t), pose(V::?t), container-shape(V::?c))
BoolOpType.AND(O[bool]{@2}, O[bool]{@3}, O[bool]{@4}, O[bool]{@5}) == O[bool]{@6} # and(placed(OBJ::tile1), placed(OBJ::tile2), left-of-t(OBJ::tile1, OBJ::tile2), collision-free-t(OBJ::tile1, OBJ::tile2), in-container-t(OBJ::tile1, OBJ::container), in-container-t(OBJ::tile2, OBJ::container))
__EQ__(O[bool]{@6}, 1) # goal_test
}
----------------------------------------
Solution:
AssignmentDict{
@6: True
@2: True
@3: True
@4: True
@5: True
@0: Value[pose, axes=[], tdtype=torch.float32, tdshape=(2,)]{tensor([3.5291, 0.9304])}
@1: Value[pose, axes=[], tdtype=torch.float32, tdshape=(2,)]{tensor([6.7986, 9.7743])}
}
----------------------------------------
Insert back into the action list:
action::place(?t=tile1, ?p=[3.5291428565979004, 0.9304475784301758])
action::place(?t=tile2, ?p=[6.798553466796875, 9.774337768554688])
[8]:
from concepts.dm.pdsketch.planners.optimistic_search import optimistic_search
solution = optimistic_search(executor, state, goal, max_depth=3, verbose=True)
print('Solution:')
for action in solution:
print(jacinle.indent_text(action))
opt::initial_state State{
objects: container: [container]; tile: [tile1, tile2]
states:
- container-shape: Value[cshape, axes=[?c], tdtype=torch.float32, tdshape=(1, 4)]{
tensor([[ 0., 0., 10., 10.]])
}
- pose: Value[pose, axes=[?t], tdtype=torch.float32, tdshape=(2, 2)]{
tensor([[0., 0.],
[0., 0.]])
}
- placed: Value[bool, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([0, 0])}
- tile-identifier: Value[identifier, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([0, 1])}
}
opt::actions nr 2
action::place(?t=tile1, ?p=??)
action::place(?t=tile2, ?p=??)
opt::goal_expr and(placed(OBJ::tile1), placed(OBJ::tile2), left-of-t(OBJ::tile1, OBJ::tile2), collision-free-t(OBJ::tile1, OBJ::tile2), in-container-t(OBJ::tile1, OBJ::container), in-container-t(OBJ::tile2, OBJ::container))
opt::depth=0, this_layer_states=2
opt::finished: depth=1 nr_expanded_states=2 nr_tested_actions=4.
Solution:
action::place(?t=tile1, ?p=[5.170578479766846, 0.5964750051498413])
action::place(?t=tile2, ?p=[5.586812496185303, 3.286633014678955])
[9]:
from concepts.dm.pdsketch.planners.optimistic_search import optimistic_search_strips
# there is another "fancier" function called
# pds.optimistic_search_strips
# which does heurisitic computation (hFF-like) to prune the search tree.
solution = optimistic_search_strips(executor, state, goal, max_depth=3, verbose=True)
print('Solution:')
for action in solution:
print(jacinle.indent_text(action))
optsstrips::initial_state State{
objects: container: [container]; tile: [tile1, tile2]
states:
- container-shape: Value[cshape, axes=[?c], tdtype=torch.float32, tdshape=(1, 4)]{
tensor([[ 0., 0., 10., 10.]])
}
- pose: Value[pose, axes=[?t], tdtype=torch.float32, tdshape=(2, 2)]{
tensor([[0., 0.],
[0., 0.]])
}
- placed: Value[bool, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([0, 0])}
- tile-identifier: Value[identifier, axes=[?t], tdtype=torch.int64, tdshape=(2,), quantized]{tensor([0, 1])}
}
optsstrips::actions nr 2
action::place(?t=tile1, ?p=??)
action::place(?t=tile2, ?p=??)
optsstrips::goal_expr and(placed(OBJ::tile1), placed(OBJ::tile2), left-of-t(OBJ::tile1, OBJ::tile2), collision-free-t(OBJ::tile1, OBJ::tile2), in-container-t(OBJ::tile1, OBJ::container), in-container-t(OBJ::tile2, OBJ::container))
optsstrips::search succeeded.
optsstrips::total_expansions: 3
Solution:
action::place(?t=tile2, ?p=[9.791505813598633, 4.838109970092773])
action::place(?t=tile1, ?p=[6.626519203186035, 3.665976047515869])
/Users/jiayuanm/Projects/Concepts/concepts/pdsketch/strips/strips_grounding.py:354: UserWarning: Regression rules are disabled in STRIPS compilation.
warnings.warn('Regression rules are disabled in STRIPS compilation.')