def update_state(self): # TODO: apply this directly from observations # No use applying this to base confs self.base_conf = FConf(self.world.robot, self.world.base_joints, init=True) arm_conf = FConf(self.world.robot, self.world.arm_joints, init=True) if (self.arm_conf is None) or not are_confs_close( arm_conf, self.arm_conf, tol=ARM_TOLERANCE): self.arm_conf = arm_conf else: print('At anticipated arm conf') gripper_conf = FConf(self.world.robot, self.world.gripper_joints, init=True) if (self.gripper_conf is None) or not are_confs_close( gripper_conf, self.gripper_conf, tol=GRIPPER_TOLERANCE): self.gripper_conf = gripper_conf else: print('At anticipated gripper conf') # TODO: do I still need to test if the current values are equal to the last ones? for joint in self.world.kitchen_joints: name = get_joint_name(self.world.kitchen, joint) position = get_joint_position(self.world.kitchen, joint) self.update_door_conf(name, position) self.update_door_conf(name, position) #wait_for_user() return self.check_consistent()
def _update_initial(self): # TODO: store initial poses as well? self.initial_saver = WorldSaver() self.goal_bq = FConf(self.robot, self.base_joints) self.goal_aq = FConf(self.robot, self.arm_joints) if are_confs_close(self.goal_aq, self.carry_conf): self.goal_aq = self.carry_conf self.goal_gq = FConf(self.robot, self.gripper_joints) self.initial_confs = [self.goal_bq, self.goal_aq, self.goal_gq] set_all_static()
def update_door_conf(self, name, position): joint = joint_from_name(self.world.kitchen, name) conf = FConf(self.world.kitchen, [joint], [position], init=True) if (name not in self.door_confs) or not are_confs_close( conf, self.door_confs[name], tol=1e-3): # TODO: different threshold for drawers and doors self.door_confs[name] = conf else: print('At anticipated conf for door {}'.format(name)) return self.door_confs[name]
def pdddlstream_from_problem(belief, additional_init=[], fixed_base=True, **kwargs): world = belief.world # One world per state task = world.task # One task per world print(task) domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl')) # TODO: repackage stream outputs to avoid recomputation # Despite the base not moving, it could be re-estimated init_bq = belief.base_conf init_aq = belief.arm_conf init_gq = belief.gripper_conf carry_aq = world.carry_conf init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq # TODO: the following doesn't work. Maybe because carry_conf is used elsewhere #carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf #calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf # Don't need this now that returning to old confs #open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq #closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq open_gq = world.open_gq closed_gq = world.closed_gq constant_map = { '@world': 'world', '@gripper': 'gripper', '@stove': 'stove', '@none': None, '@rest_aq': carry_aq, #'@calibrate_aq': calibrate_aq, '@open_gq': open_gq, '@closed_gq': closed_gq, '@open': OPEN, '@closed': CLOSED, '@top': TOP_GRASP, '@side': SIDE_GRASP, '@bq0': init_bq, } top_joint = JOINT_TEMPLATE.format(TOP_DRAWER) bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER) init = [ ('BConf', init_bq), ('AtBConf', init_bq), ('AConf', init_bq, carry_aq), #('RestAConf', carry_aq), #('AConf', init_bq, calibrate_aq), ( 'Stationary', ), ('AConf', init_bq, init_aq), ('AtAConf', init_aq), ('GConf', open_gq), ('GConf', closed_gq), ('Grasp', None, None), ('AtGrasp', None, None), ('Above', top_joint, bottom_joint), ('Adjacent', top_joint, bottom_joint), ('Adjacent', bottom_joint, top_joint), ('Calibrated', ), ('CanMoveBase', ), ('CanMoveArm', ), ('CanMoveGripper', ), ] + list(task.init) + list(additional_init) for action_name, cost in ACTION_COSTS.items(): function_name = '{}Cost'.format(title_from_snake(action_name)) function = (function_name, ) init.append(Equal(function, cost)) # TODO: stove state init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \ [('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \ [('Pressed', name) for name in belief.pressed] + \ [('Cookable', name) for name in task.goal_cooked] + \ [('Cooked', name) for name in belief.cooked] + \ [('Status', status) for status in DOOR_STATUSES] + \ [('Knob', knob) for knob in KNOBS] + \ [('Joint', knob) for knob in KNOBS] + \ [('Liquid', liquid) for _, liquid in task.init_liquid] + \ [('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \ [('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \ [('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object #[('Type', obj_name, 'stove') for obj_name in STOVES] + \ #[('Camera', name) for name in world.cameras] if task.movable_base: init.append(('MovableBase', )) if fixed_base: init.append(('InitBConf', init_bq)) if task.noisy_base: init.append(('NoisyBase', )) compute_pose_kin = get_compute_pose_kin(world) compute_angle_kin = get_compute_angle_kin(world) initial_poses = {} for joint_name, init_conf in belief.door_confs.items(): if joint_name in DRAWER_JOINTS: init.append(('Drawer', joint_name)) if joint_name in CABINET_JOINTS: init.append(('Cabinet', joint_name)) joint = joint_from_name(world.kitchen, joint_name) surface_name = surface_from_joint(joint_name) init.append(('SurfaceJoint', surface_name, joint_name)) # Relies on the fact that drawers have identical surface and link names link_name = get_link_name(world.kitchen, child_link_from_joint(joint)) #link_name = str(link_name.decode('UTF-8')) #link_name = str(link_name.encode('ascii','ignore')) for conf in { init_conf, world.open_kitchen_confs[joint], world.closed_kitchen_confs[joint] }: # TODO: return to initial poses? world_pose, = compute_angle_kin(link_name, joint_name, conf) init.extend([ ('Joint', joint_name), ('Angle', joint_name, conf), ('Obstacle', link_name), ('AngleKin', link_name, world_pose, joint_name, conf), ('WorldPose', link_name, world_pose), ]) if joint in world.kitchen_joints: init.extend([ ('Sample', world_pose), #('Value', world_pose), # comment out? ]) if conf == init_conf: initial_poses[link_name] = world_pose init.extend([ ('AtAngle', joint_name, conf), ('AtWorldPose', link_name, world_pose), ]) for surface_name in ALL_SURFACES: if surface_name in OPEN_SURFACES: init.append(('Counter', surface_name)) # Fixed surface if surface_name in DRAWERS: init.append(('Drawer', surface_name)) surface = surface_from_name(surface_name) surface_link = link_from_name(world.kitchen, surface.link) parent_joint = parent_joint_from_link(surface_link) if parent_joint not in world.kitchen_joints: # TODO: attach to world frame? world_pose = RelPose(world.kitchen, surface_link, init=True) initial_poses[surface_name] = world_pose init += [ #('RelPose', surface_name, world_pose, 'world'), ('WorldPose', surface_name, world_pose), #('AtRelPose', surface_name, world_pose, 'world'), ('AtWorldPose', surface_name, world_pose), ('Sample', world_pose), #('Value', world_pose), ] init.extend([ ('CheckNearby', surface_name), #('InitPose', world_pose), ('Localized', surface_name), ]) for grasp_type in task.grasp_types: if (surface_name in OPEN_SURFACES) or has_place_database( world.robot_name, surface_name, grasp_type): init.append(('AdmitsGraspType', surface_name, grasp_type)) if belief.grasped is None: init.extend([ ('HandEmpty', ), ('GConf', init_gq), ('AtGConf', init_gq), ]) else: obj_name = belief.grasped.body_name assert obj_name not in belief.pose_dists grasp = belief.grasped init += [ # Static #('Graspable', obj_name), ('Grasp', obj_name, grasp), ('IsGraspType', obj_name, grasp, grasp.grasp_type), # Fluent ('AtGrasp', obj_name, grasp), ('Holding', obj_name), ('Localized', obj_name), ] init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) for obj_name in world.movable: obj_type = type_from_name(obj_name) if obj_type in BOWLS: init.append(('Bowl', obj_name)) else: init.append( ('Obstacle', obj_name)) # TODO: hack to place within bowls if obj_type in COOKABLE: init.append(('Cookable', obj_name)) if obj_type in POURABLE: init.append(('Pourable', obj_name)) init += [ ('Entity', obj_name), ('CheckNearby', obj_name), ] + [('Stackable', obj_name, counter) for counter in set(ALL_SURFACES) & set(COUNTERS)] # TODO: track poses over time to produce estimates for obj_name, pose_dist in belief.pose_dists.items(): dist_support = pose_dist.dist.support() localized = pose_dist.is_localized() graspable = True if localized: init.append(('Localized', obj_name)) [rel_pose] = dist_support roll, pitch, yaw = euler_from_quat( quat_from_pose(rel_pose.get_reference_from_body())) if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)): graspable = False print( '{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'. format(obj_name, roll, pitch)) if graspable: #init.append(('Graspable', obj_name)) init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) # Could also fully decompose into points (but many samples) # Could immediately add likely points for collision checking for rel_pose in (dist_support if localized else pose_dist.decompose()): surface_name = rel_pose.support if surface_name is None: # Treats as obstacle # TODO: could temporarily add to fixed world_pose = rel_pose init += [ ('WorldPose', obj_name, world_pose), ('AtWorldPose', obj_name, world_pose), ] poses = [world_pose] #raise RuntimeError(obj_name, supporting) else: surface_pose = initial_poses[surface_name] world_pose, = compute_pose_kin(obj_name, rel_pose, surface_name, surface_pose) init += [ # Static ('RelPose', obj_name, rel_pose, surface_name), ('WorldPose', obj_name, world_pose), ('PoseKin', obj_name, world_pose, rel_pose, surface_name, surface_pose), # Fluent ('AtRelPose', obj_name, rel_pose, surface_name), ('AtWorldPose', obj_name, world_pose), ] if localized: init.append(('On', obj_name, surface_name)) poses = [rel_pose, world_pose] for pose in poses: if isinstance(pose, PoseDist): init.append(('Dist', pose)) else: init.extend([('Sample', pose)]) #, ('Value', pose)]) #for body, ty in problem.body_types: # init += [('Type', body, ty)] #bodies_from_type = get_bodies_from_type(problem) #bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty] goal_formula = get_goal(belief, init) stream_pddl, stream_map = get_streams(world, teleport_base=task.teleport_base, **kwargs) print('Constants:', constant_map) print('Init:', sorted(init, key=lambda f: f[0])) print('Goal:', goal_formula) #print('Streams:', stream_map.keys()) # DEBUG return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def get_goal(belief, init, base_threshold=(0.05, 0.05, math.radians(10)), arm_threshold=math.radians(10)): # TODO: order goals for serialization # TODO: make independent of belief and world world = belief.world # One world per state task = world.task # One task per world init_bq = belief.base_conf init_aq = belief.arm_conf carry_aq = world.carry_conf goal_literals = [Not(('Unsafe', ))] if task.goal_hand_empty: goal_literals.append(('HandEmpty', )) if task.goal_holding is not None: goal_literals.append(('Holding', task.goal_holding)) goal_literals += [('On', name, surface) for name, surface in task.goal_on.items()] + \ [Not(('Pressed', name)) for name in KNOBS] + \ [('HasLiquid', cup, liquid) for cup, liquid in task.goal_liquid] + \ [('Cooked', name) for name in task.goal_cooked] + \ [('Localized', name) for name in task.goal_detected] + \ [door_closed_formula(joint_name) for joint_name in task.goal_closed] + \ [door_open_formula(joint_name) for joint_name in task.goal_open] + \ list(task.goal) if not task.movable_base or task.return_init_bq: # fixed_base? goal_bq = world.goal_bq if task.movable_base else init_bq init.extend([ ('BConf', goal_bq), ('AConf', goal_bq, carry_aq), ('CloseTo', goal_bq, goal_bq), ]) base_difference_fn = get_difference_fn(world.robot, world.base_joints) if np.less_equal( np.abs(base_difference_fn(init_bq.values, goal_bq.values)), base_threshold).all(): print('Close to goal base configuration') init.append(('CloseTo', init_bq, goal_bq)) goal_literals.append( Exists(['?bq'], And(('CloseTo', '?bq', goal_bq), ('AtBConf', '?bq')))) goal_aq = task.goal_aq if task.return_init_aq: goal_aq = init_aq if are_confs_close( init_aq, world.goal_aq) else world.goal_aq if goal_aq is not None: arm_difference_fn = get_difference_fn(world.robot, world.arm_joints) if np.less_equal( np.abs(arm_difference_fn(init_aq.values, goal_aq.values)), arm_threshold * np.ones(len(world.arm_joints))).all(): print('Close to goal arm configuration') init.append(('CloseTo', init_aq, goal_aq)) init.extend([ ('AConf', goal_bq, goal_aq), ('CloseTo', goal_aq, goal_aq), ]) goal_literals.append( Exists(['?aq'], And(('CloseTo', '?aq', goal_aq), ('AtAConf', '?aq')))) return existential_quantification(goal_literals)