def add_body(self, name, **kwargs): obj_type = type_from_name(name) self.names_from_type.setdefault(obj_type, []).append(name) path = get_obj_path(obj_type) #self.path_from_name[name] = path print('Loading', path) body = load_pybullet(path, **kwargs) assert body is not None self.add(name, body)
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 pour_path_from_parameter(world, bowl_name, cup_name): bowl_body = world.get_body(bowl_name) bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body) cup_body = world.get_body(cup_name) cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body) ##### obj_type = type_from_name(cup_name) if obj_type in [MUSTARD]: initial_pitch = final_pitch = -np.pi radius = 0 else: initial_pitch = 0 # different if mustard final_pitch = -3 * np.pi / 4 radius = bowl_d / 2 #axis_in_cup_center_x = -0.05 axis_in_cup_center_x = 0 # meters #axis_in_cup_center_z = -cup_h/2. axis_in_cup_center_z = 0. # meters #axis_in_cup_center_z = +cup_h/2. # tl := top left | tr := top right cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2]) cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z) cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0]) cup_tl_pour_pitch = final_pitch - cup_tl_angle cup_radius2d = np.linalg.norm([cup_tl_in_axis]) pivot_in_bowl_tr = Point( x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01), z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET)) pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr base_from_pivot = Pose( Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z)) ##### assert -np.pi <= final_pitch <= initial_pitch pitches = [initial_pitch] if final_pitch != initial_pitch: pitches = list(np.arange(final_pitch, initial_pitch, np.pi / 16)) + pitches cup_path_in_bowl = [] for pitch in pitches: rotate_pivot = Pose( euler=Euler(pitch=pitch) ) # Can also interpolate directly between start and end quat cup_path_in_bowl.append( multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center), rotate_pivot, invert(base_from_pivot), invert(Pose(point=cup_center)))) return cup_path_in_bowl
def is_gripper_closed(self): # TODO: base this on the object type if self.holding is not None: obj_type = type_from_name(self.holding) if obj_type not in TIN_OBJECTS: return False # each joint in [0.00, 0.04] (units coincide with meters on the physical gripper) current_gq = get_joint_positions(self.world.robot, self.world.gripper_joints) gripper_width = sum(current_gq) return gripper_width <= MIN_GRASP_WIDTH
def gen(bowl_name, wp, cup_name, grasp, bq): # https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/plan_tools/samplers/pour.py if bowl_name == cup_name: return #attachment = get_grasp_attachment(world, arm, grasp) bowl_body = world.get_body(bowl_name) #cup_body = world.get_body(cup_name) obstacles = (world.static_obstacles | {bowl_body}) if collisions else set() cup_path_bowl = pour_path_from_parameter(world, bowl_name, cup_name) while True: for _ in range(max_attempts): bowl_pose = wp.get_world_from_body() rotate_bowl = Pose(euler=Euler( yaw=random.uniform(-np.pi, np.pi))) rotate_cup = Pose(euler=Euler( yaw=random.uniform(-np.pi, np.pi))) cup_path = [ multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl, rotate_cup) for cup_pose_bowl in cup_path_bowl ] #visualize_cartesian_path(cup_body, cup_path) #if cartesian_path_collision(cup_body, cup_path, obstacles + [bowl_body]): # continue tool_path = [ multiply(p, invert(grasp.grasp_pose)) for p in cup_path ] # TODO: extra collision test for visibility # TODO: orientation constraint while moving bq.assign() grasp.set_gripper() world.carry_conf.assign() arm_path = plan_workspace(world, tool_path, obstacles, randomize=True) # tilt to upright if arm_path is None: continue assert MOVE_ARM aq = FConf(world.robot, world.arm_joints, arm_path[-1]) robot_saver = BodySaver(world.robot) obj_type = type_from_name(cup_name) duration = 5.0 if obj_type in [MUSTARD] else 1.0 objects = [bowl_name, cup_name] cmd = Sequence(State(world, savers=[robot_saver]), commands=[ ApproachTrajectory(objects, world, world.robot, world.arm_joints, arm_path[::-1]), Wait(world, duration=duration), ApproachTrajectory(objects, world, world.robot, world.arm_joints, arm_path), ], name='pour') yield ( aq, cmd, ) break else: yield None
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)