def main():
uniform_rooms = UniformDist(['room0', OTHER])
#uniform_tables = UniformDist(['table0', 'table1'])
#uniform_tables = UniformDist(['table0', OTHER])
uniform_tables = UniformDist(['table0', 'table1', OTHER])
#initial_belief = get_room_belief(uniform_rooms, uniform_tables, 1.0)
initial_belief = get_room_belief(uniform_rooms, uniform_tables, 0.2)
#initial_belief = get_table_belief(uniform_tables, 1.0)
#initial_belief = get_table_belief(uniform_tables, 0.2)
#initial_belief = get_item_belief()
pddlstream_problem = pddlstream_from_belief(initial_belief)
_, _, _, _, init, goal = pddlstream_problem
print(sorted(init))
print(goal)
pr = cProfile.Profile()
pr.enable()
planner = 'max-astar'
#solution = solve_incremental(pddlstream_problem, planner=planner, unit_costs=False)
solution = solve_focused(pddlstream_problem,
planner=planner,
unit_costs=False)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def main():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
uniform_rooms = UniformDist(['room0', OTHER])
#uniform_tables = UniformDist(['table0', 'table1'])
#uniform_tables = UniformDist(['table0', OTHER])
uniform_tables = UniformDist(['table0', 'table1', OTHER])
#initial_belief = get_room_belief(uniform_rooms, uniform_tables, 1.0)
initial_belief = get_room_belief(uniform_rooms, uniform_tables, 0.2)
#initial_belief = get_table_belief(uniform_tables, 1.0)
#initial_belief = get_table_belief(uniform_tables, 0.2)
#initial_belief = get_item_belief()
pddlstream_problem = pddlstream_from_belief(initial_belief)
_, _, _, _, init, goal = pddlstream_problem
print('Init:', sorted(init))
print('Goal:', goal)
planner = 'max-astar'
with Profiler(field='tottime', num=10):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
unit_costs=args.unit,
planner=planner)
print_solution(solution)
def to_pddlstream(belief_problem, collisions=True):
locations = {l for (_, l, _) in belief_problem.initial + belief_problem.goal} | \
set(belief_problem.locations)
observations = [True, False]
uniform = UniformDist(locations)
initial_bel = {o: MixtureDD(DeltaDist(l), uniform, p) for o, l, p in belief_problem.initial}
max_p_collision = 0.25 if collisions else 1.0
# TODO: separate pick and place for move
init = [('Obs', obs) for obs in observations] + \
[('Location', l) for l in locations]
for o, d in initial_bel.items():
init += [('Dist', o, d), ('BLoc', o, d)]
for (o, l, p) in belief_problem.goal:
init += [('Location', l), ('GoalProb', l, p)]
goal_literals = [('BLocGE', o, l, p) for (o, l, p) in belief_problem.goal]
goal = And(*goal_literals)
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain.pddl'))
stream_pddl = read(os.path.join(directory, 'stream.pddl'))
constant_map = {}
stream_map = {
'BCollision': get_collision_test(max_p_collision),
'GE': from_test(ge_fn),
'prob-after-move': from_fn(get_move_fn(belief_problem.p_move_s)),
'MoveCost': move_cost_fn,
'prob-after-look': from_fn(get_look_fn(belief_problem.p_look_fp, belief_problem.p_look_fn)),
'LookCost': get_look_cost_fn(belief_problem.p_look_fp, belief_problem.p_look_fn),
#'PCollision': from_fn(prob_occupied), # Then can use GE
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def cook_block(world, fixed=False, **kwargs):
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
set_all_static()
initial_surface = 'indigo_tmp'
sample_placement(world, entity_name, initial_surface, learned=True)
prior = {
entity_name: UniformDist([initial_surface]),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
#goal_detected=[entity_name],
#goal_holding=entity_name,
goal_cooked=[entity_name],
#goal_on={entity_name: goal_surface},
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
#goal_closed=ALL_JOINTS,
)
def regrasp_block(world, fixed=False, **kwargs):
add_kinect(world)
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0)
set_all_static()
#world.open_door(joint_from_name(world.kitchen, JOINT_TEMPLATE.format(LEFT_DOOR)))
#drawer = random.choice(ZED_DRAWERS)
drawer = 'indigo_tmp'
sample_placement(world, entity_name, drawer, learned=True)
prior = {
entity_name: UniformDist(drawer),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
#grasp_types=[SIDE_GRASP], #, TOP_GRASP],
#goal_holding=entity_name,
goal_on={entity_name: LEFT_DOOR},
#return_init_bq=True, return_init_aq=True,
#goal_open=[JOINT_TEMPLATE.format(LEFT_DOOR)]
#goal_closed=ALL_JOINTS,
)
def resample(self, n=NUM_PARTICLES):
if len(self.dist.support()) <= 1:
return self
with LockRenderer():
poses = [self.sample() for _ in range(n)]
new_dist = UniformDist(poses)
return self.__class__(self.world, self.name, new_dist)
def detect_block(world, fixed=False, **kwargs):
add_kinect(world)
#for side in CAMERAS[:1]:
# add_kinect(world, side)
#add_kinects(world)
if fixed:
set_fixed_base(world)
#plane = create_plane([1, 0, 0])
#set_point(plane, [MIN_PLACEMENT_X, 0, 0])
#wait_for_user()
block_poses = [(0.1, 1.05, 0.), (0.1, 1.3, 0.)]
entity_name = add_block(world, idx=0, pose2d=random.choice(block_poses))
sugar_name = add_sugar_box(world, idx=0, pose2d=(0.2, 1.35, np.pi / 4))
cracker_name = add_cracker_box(world, idx=1, pose2d=(0.2, 1.1, np.pi / 4))
#other_name = add_box(world, idx=1)
set_all_static()
#goal_surface = 'indigo_drawer_top'
#initial_distribution = UniformDist([goal_surface]) # indigo_tmp
#initial_surface = initial_distribution.sample()
#if random.random() < 0.0:
# # TODO: sometimes base/arm failure causes the planner to freeze
# # Freezing is because the planner is struggling to find new samples
# sample_placement(world, entity_name, initial_surface, learned=True)
#sample_placement(world, other_name, 'hitman_tmp', learned=True)
prior = {
entity_name:
UniformDist(['indigo_tmp']), # 'indigo_tmp', 'indigo_drawer_top'
sugar_name: DeltaDist('indigo_tmp'),
cracker_name: DeltaDist('indigo_tmp'),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
return_init_bq=True,
return_init_aq=True,
#goal_detected=[entity_name],
goal_holding=cracker_name,
#goal_on={entity_name: random.choice(ZED_DRAWERS)},
goal_cooked=[entity_name],
goal_closed=ALL_JOINTS,
)
def swap_drawers(world, fixed=False, **kwargs):
# Starts in the incorrect drawer
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
set_all_static()
#open_all_doors(world)
#initial_surface, goal_surface = 'indigo_tmp', 'indigo_drawer_top'
#initial_surface, goal_surface = 'indigo_drawer_top', 'indigo_drawer_top'
#initial_surface, goal_surface = 'indigo_drawer_bottom', 'indigo_drawer_bottom'
#initial_surface, goal_surface = ZED_DRAWERS
#initial_surface, goal_surface = reversed(ZED_DRAWERS)
initial_surface, goal_surface = randomize(ZED_DRAWERS)
if initial_surface != 'indigo_tmp':
sample_placement(world, entity_name, initial_surface, learned=True)
#joint_name = JOINT_TEMPLATE.format(goal_surface)
#world.open_door(joint_from_name(world.kitchen, JOINT_TEMPLATE.format(goal_surface)))
# TODO: declare success if already believe it's in the drawer or require detection?
prior = {
#entity_name: UniformDist([initial_surface]),
entity_name: UniformDist(ZED_DRAWERS),
#entity_name: UniformDist(['indigo_tmp', 'indigo_drawer_top', 'indigo_drawer_bottom']),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
#goal_detected=[entity_name],
#goal_holding=entity_name,
#goal_cooked=[entity_name],
goal_on={entity_name: goal_surface},
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
#goal_closed=[JOINT_TEMPLATE.format(initial_surface)],
#goal_closed=[JOINT_TEMPLATE.format(goal_surface)], # TODO: this caused non-fixed base planning to fail due to cost
goal_closed=ALL_JOINTS,
)
def create_surface_pose_dist(world, obj_name, surface_dist, n=NUM_PARTICLES):
# TODO: likely easier to just make a null surface below ground
placement_gen = get_stable_gen(world,
max_attempts=100,
learned=True,
pos_scale=1e-3,
rot_scale=1e-2)
poses = []
with LockRenderer():
with BodySaver(world.get_body(obj_name)):
while len(poses) < n:
surface_name = surface_dist.sample()
assert surface_name is not ELSEWHERE
result = next(placement_gen(obj_name, surface_name), None)
if result is None:
surface_dist = surface_dist.condition(
lambda s: s != surface_name)
else:
(rel_pose, ) = result
rel_pose.init = True
poses.append(rel_pose)
return PoseDist(world, obj_name, UniformDist(poses))
def inspect_drawer(world, fixed=False, **kwargs):
# Starts in the correct drawer
add_kinect(world)
if fixed:
set_fixed_base(world)
entity_name = add_block(world, idx=0)
set_all_static()
drawer = random.choice(ZED_DRAWERS)
sample_placement(world, entity_name, drawer, learned=True)
prior = {
entity_name: UniformDist(ZED_DRAWERS),
}
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
goal_on={entity_name: drawer},
return_init_bq=True,
return_init_aq=True,
goal_closed=ALL_JOINTS,
)
def set_uniform_belief(task, b_on, body, p_other=0.5):
# TODO: option to bias towards particular bottom
other = DeltaDist(OTHER)
uniform = UniformDist(task.get_supports(body))
b_on[body] = MixtureDD(other, uniform, p_other)
def delocalize_belief(belief, o, rp):
dist = UniformDist([rp, copy.copy(rp)])
belief.pose_dists[o] = PoseDist(belief.world, o, dist)
return dist
def set_uniform_belief(task, b_on, body, p_other=0.):
# p_other is the probability that it doesn't actually exist
# TODO: option to bias towards particular bottom
other = DeltaDist(OTHER)
uniform = UniformDist(task.get_supports(body))
b_on[body] = MixtureDD(other, uniform, p_other)
