def sugar_drawer(world, fixed=False, **kwargs):
add_kinect(world)
if fixed:
set_fixed_base(world)
open_drawer = random.choice(ZED_DRAWERS)
world.open_door(
joint_from_name(world.kitchen, JOINT_TEMPLATE.format(open_drawer)))
# open_all_doors(world)
prior = {}
block_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
prior[block_name] = DeltaDist('indigo_tmp')
sugar_name = add_sugar_box(world, idx=0)
prior[sugar_name] = DeltaDist(open_drawer)
sample_placement(world, sugar_name, open_drawer, learned=True)
set_all_static()
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
goal_on={block_name: open_drawer},
return_init_bq=True,
return_init_aq=True,
#goal_open=[JOINT_TEMPLATE.format(open_drawer)],
goal_closed=ALL_JOINTS,
)
def get_item_belief():
return {
'room0': DeltaDist(WORLD),
'table0': DeltaDist('room0'),
'table1': DeltaDist('room0'),
'soup0': DeltaDist('table0'),
'green0': DeltaDist('table0'),
}
def fn(l):
# P(s2 | s1=loc1, a=(control_loc1, control_loc2))
perfect_d = DeltaDist(loc2)
fail_d = DeltaDist(l)
# fail_d = UniformDist(locations)
# return MixtureDD(perfect_d, fail_d, p_move_s)
if loc1 == l:
return MixtureDD(perfect_d, fail_d, p_move_s)
return fail_d
def get_room_belief(uniform_rooms, uniform_tables, uncertainty):
mix = 1 - uncertainty
return {
'room0': DeltaDist(WORLD),
'table0': MixtureDist(DeltaDist('room0'), uniform_rooms, mix),
'table1': MixtureDist(DeltaDist('room0'), uniform_rooms, mix),
'soup0': MixtureDist(DeltaDist('table0'), uniform_tables, mix),
'green0': MixtureDist(DeltaDist('table0'), uniform_tables, mix),
}
def set_delta_belief(task, b_on, body):
supports = task.get_supports(body)
if supports is None:
b_on[body] = DeltaDist(supports)
return
for bottom in task.get_supports(body):
if is_center_stable(body, bottom):
b_on[body] = DeltaDist(bottom)
return
raise RuntimeError('No support for body {}'.format(body))
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 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 create_observable_pose_dist(world, obj_name):
body = world.get_body(obj_name)
surface_name = world.get_supporting(obj_name)
if surface_name is None:
pose = RelPose(body, init=True) # Treats as obstacle
else:
pose = create_relative_pose(world, obj_name, surface_name, init=True)
return PoseDist(world, obj_name, DeltaDist(pose))
def transition_belief_update(belief, plan):
if plan is None:
return False
success = True
for action, params in plan:
if action in ['move_base', 'calibrate', 'detect']:
pass
elif action == 'press-on':
s, k, o, bq, aq, gq, at = params
belief.pressed.add(k)
belief.cooked.add(o)
for bowl, liquid in belief.liquid:
if bowl == o:
belief.cooked.add(liquid)
elif action == 'press-off':
s, k, o, bq, aq, gq, at = params
belief.pressed.remove(k)
elif action == 'move_arm':
bq, aq1, aq2, at = params
belief.arm_conf = aq2
elif action == 'move_gripper':
gq1, gq2, gt = params
belief.gripper_conf = gq2
elif action == 'pull':
j, a1, a2, o, wp1, wp2, bq, aq1, aq2, gq, at = params
belief.door_confs[j] = a2
belief.arm_conf = aq2
elif action == 'pour':
bowl, wp, cup, g, liquid, bq, aq, at = params
belief.liquid.discard((cup, liquid))
belief.liquid.add((bowl, liquid))
elif action == 'pick':
o, p, g, rp = params[:4]
obj_type = type_from_name(o)
if (obj_type not in TIN_OBJECTS) or not belief.is_gripper_closed():
del belief.pose_dists[o]
belief.grasped = g
# TODO: open gripper afterwards to ensure not in hand
else:
delocalize_belief(belief, o, rp)
print('Failed to grasp! Delocalizing belief')
success = False
break
elif action == 'place':
o, p, g, rp = params[:4]
belief.grasped = None
if STOCHASTIC_PLACE and belief.world.is_real():
delocalize_belief(belief, o, rp)
else:
belief.pose_dists[o] = PoseDist(belief.world, o, DeltaDist(rp))
elif action == 'cook':
pass
else:
raise NotImplementedError(action)
# TODO: replan after every action
return success
def cook_meal(world, fixed=False, **kwargs):
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
prior = {}
soup_name = add_ycb(world,
'tomato_soup_can',
pose2d=[0.1, 0.9, +np.pi / 8])
prior[soup_name] = DeltaDist('indigo_tmp')
if not fixed:
sample_placement(world, soup_name, 'indigo_tmp', learned=True)
mustard_name = add_ycb(world,
'mustard_bottle',
pose2d=[0.25, 1.2, -np.pi / 8])
prior[mustard_name] = DeltaDist('indigo_tmp')
if not fixed:
sample_placement(world, mustard_name, 'indigo_tmp', learned=True)
stove = STOVES[-1]
bowl_name = add_ycb(world, 'bowl')
prior[bowl_name] = DeltaDist(stove)
sample_placement(world, bowl_name, stove, learned=True)
#print(get_pose(world.get_body(bowl_name)))
set_all_static()
return Task(
world,
prior=prior,
movable_base=not fixed,
#grasp_types=[TOP_GRASP],
init_liquid=[(soup_name, 'tomato'), (mustard_name, 'mustard')],
#goal_liquid=[(bowl_name, 'tomato'), (bowl_name, 'mustard')],
#goal_holding=soup_name,
goal_hand_empty=True,
#goal_cooked=[bowl_name],
goal_cooked=['tomato', 'mustard'],
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
goal_closed=ALL_JOINTS)
def hold_block(world, num=5, fixed=False, **kwargs):
add_kinect(world)
if fixed:
set_fixed_base(world)
# TODO: compare with the NN grasp prediction in clutter
# TODO: consider a task where most directions are blocked except for one
initial_surface = 'indigo_tmp'
# initial_surface = 'dagger_door_left'
# joint_name = JOINT_TEMPLATE.format(initial_surface)
#world.open_door(joint_from_name(world.kitchen, joint_name))
#open_all_doors(world)
prior = {}
# green_name = add_block(world, idx=0, pose2d=BOX_POSE2D)
green_name = add_box(world, 'green', idx=0)
prior[green_name] = DeltaDist(initial_surface)
sample_placement(world, green_name, initial_surface, learned=True)
for idx in range(num):
red_name = add_box(world, 'red', idx=idx)
prior[red_name] = DeltaDist(initial_surface)
sample_placement(world, red_name, initial_surface, learned=True)
set_all_static()
return Task(
world,
prior=prior,
movable_base=not fixed,
# grasp_types=GRASP_TYPES,
grasp_types=[SIDE_GRASP],
return_init_bq=True,
return_init_aq=True,
goal_holding=green_name,
#goal_closed=ALL_JOINTS,
)
def stow_block(world, num=1, fixed=False, **kwargs):
add_kinect(world) # previously needed to be after set_all_static?
if fixed:
set_fixed_base(world)
# initial_surface = random.choice(DRAWERS) # COUNTERS | DRAWERS | SURFACES | CABINETS
initial_surface = 'indigo_tmp' # hitman_tmp | indigo_tmp | range | front_right_stove
# initial_surface = 'indigo_drawer_top'
goal_surface = 'indigo_drawer_top' # baker | hitman_drawer_top | indigo_drawer_top | hitman_tmp | indigo_tmp
print('Initial surface: | Goal surface: ', initial_surface,
initial_surface)
prior = {}
goal_on = {}
for idx in range(num):
entity_name = add_block(world, idx=idx, pose2d=SPAM_POSE2D)
prior[entity_name] = DeltaDist(initial_surface)
goal_on[entity_name] = goal_surface
if not fixed:
sample_placement(world, entity_name, initial_surface, learned=True)
#obstruction_name = add_box(world, idx=0)
#sample_placement(world, obstruction_name, 'hitman_tmp')
set_all_static()
# dump_world()
# open_names = [
# 'chewie_door_right_joint',
# 'dagger_door_right_joint',
# 'hitman_drawer_bottom_joint',
# # 'indigo_drawer_top_joint',
# ]
# for joint_name in open_names:
# world.open_door(joint_from_name(world.kitchen, joint_name))
return Task(
world,
prior=prior,
movable_base=not fixed,
grasp_types=[TOP_GRASP],
#goal_holding=list(prior)[0],
goal_hand_empty=True,
goal_on=goal_on,
#goal_cooked=list(prior),
return_init_bq=True,
return_init_aq=True,
#goal_open=[joint_name],
goal_closed=ALL_JOINTS)
def fn(pose, surface):
# P(obs point | state detect)
if surface is None:
return DeltaDist(None)
# Weight can be proportional weight in the event that the distribution can't be normalized
x, y, yaw = base_values_from_pose(pose.get_reference_from_body())
if DIM == 2:
return ProductDistribution([
GaussianDistribution(gmean=x, stdev=MODEL_POS_STD),
GaussianDistribution(gmean=y, stdev=MODEL_POS_STD),
#CUniformDist(-np.pi, +np.pi),
])
else:
return SE2Distribution(x,
y,
yaw,
pos_std=MODEL_POS_STD,
ori_std=MODEL_ORI_STD)
def update_dist(self, observation, obstacles=[], verbose=False):
# cfree_dist.conditionOnVar(index=1, has_detection=True)
if not BAYESIAN and (self.name in observation):
# TODO: convert into a Multivariate Gaussian
[detected_pose] = observation[self.name]
return DeltaDist(detected_pose)
if not self.world.cameras:
return self.dist.copy()
body = self.world.get_body(self.name)
all_poses = self.dist.support()
cfree_poses = all_poses
#cfree_poses = compute_cfree(body, all_poses, obstacles)
#cfree_dist = self.cfree_dist
cfree_dist = DDist(
{pose: self.dist.prob(pose)
for pose in cfree_poses})
# TODO: do these updates simultaneously for each object
# TODO: check all camera poses
[camera] = self.world.cameras.keys()
info = self.world.cameras[camera]
camera_pose = get_pose(info.body)
detectable_poses = compute_detectable(cfree_poses, camera_pose)
visible_poses = compute_visible(body,
detectable_poses,
camera_pose,
draw=False)
if verbose:
print(
'Total: {} | CFree: {} | Detectable: {} | Visible: {}'.format(
len(all_poses), len(cfree_poses), len(detectable_poses),
len(visible_poses)))
assert set(visible_poses) <= set(detectable_poses)
# obs_fn = get_observation_fn(surface)
#wait_for_user()
return self.bayesian_belief_update(cfree_dist,
visible_poses,
observation,
verbose=verbose)
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)
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 fn(pose):
# P(detect | s in visible)
# This could depend on the position as well
if pose in visible:
return DDist({pose.support: 1. - p_fn, None: p_fn})
return DeltaDist(None)
