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)