def pddlstream_from_problem(problem, teleport=False, movable_collisions=False): robot = problem.robot domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} #initial_bq = Pose(robot, get_pose(robot)) initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) init = [ ('CanMove',), ('BConf', initial_bq), ('AtBConf', initial_bq), Equal(('PickCost',), scale_cost(1)), Equal(('PlaceCost',), scale_cost(1)), ] + [('Sink', s) for s in problem.sinks] + \ [('Stove', s) for s in problem.stoves] + \ [('Connected', b, d) for b, d in problem.buttons] + \ [('Button', b) for b, _ in problem.buttons] for arm in ARM_NAMES: #for arm in problem.arms: joints = get_arm_joints(robot, arm) conf = Conf(robot, joints, get_joint_positions(robot, joints)) init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf)] if arm in problem.arms: init += [('Controllable', arm)] for body in problem.movable: pose = Pose(body, get_pose(body)) init += [('Graspable', body), ('Pose', body, pose), ('AtPose', body, pose)] for surface in problem.surfaces: init += [('Stackable', body, surface)] if is_placement(body, surface): init += [('Supported', body, pose, surface)] goal = [AND] if problem.goal_conf is not None: goal_conf = Pose(robot, problem.goal_conf) init += [('BConf', goal_conf)] goal += [('AtBConf', goal_conf)] goal += [('Holding', a, b) for a, b in problem.goal_holding] + \ [('On', b, s) for b, s in problem.goal_on] + \ [('Cleaned', b) for b in problem.goal_cleaned] + \ [('Cooked', b) for b in problem.goal_cooked] stream_map = { 'sample-pose': get_stable_gen(problem), 'sample-grasp': from_list_fn(get_grasp_gen(problem)), 'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)), 'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)), 'MoveCost': move_cost_fn, 'TrajPoseCollision': fn_from_constant(False), 'TrajArmCollision': fn_from_constant(False), 'TrajGraspCollision': fn_from_constant(False), } if USE_SYNTHESIZERS: stream_map['plan-base-motion'] = empty_gen(), # get_press_gen(problem, teleport=teleport) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def gen(o, p): # default_conf = arm_conf(a, g.carry) # joints = get_arm_joints(robot, a) # TODO: check collisions with fixed links target_point = point_from_pose(p.value) base_generator = visible_base_generator(robot, target_point, base_range) while True: for _ in range(max_attempts): set_pose(o, p.value) base_conf = next(base_generator) #set_base_values(robot, base_conf) set_joint_positions(robot, base_joints, base_conf) if any(pairwise_collision(robot, b) for b in fixed): continue head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible continue #bq = Pose(robot, get_pose(robot)) bq = Conf(robot, base_joints, base_conf) hq = Conf(robot, head_joints, head_conf) yield (bq, hq) break else: yield None
def fn(robot, body, pose, grasp): joints = get_base_joints(robot) robot_pose = multiply(pose.value, invert(grasp.value)) base_values = base_values_from_pose(robot_pose) conf = Conf(robot, joints, base_values) conf.assign() if any(pairwise_collision(robot, obst) for obst in problem.obstacles): return None return (conf, )
def fn(robot, body, pose, grasp): # TODO: reverse into the pick or place for differential drive joints = get_base_joints(robot) robot_pose = multiply(pose.value, invert(grasp.value)) base_values = base_values_from_pose(robot_pose) conf = Conf(robot, joints, base_values) conf.assign() if any(pairwise_collision(robot, obst) for obst in problem.obstacles): return None return (conf, )
def gen(rover, objective): base_joints = get_base_joints(rover) target_point = get_point(objective) base_generator = visible_base_generator(rover, target_point, base_range) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) attempts = 0 while True: if max_attempts <= attempts: attempts = 0 yield None attempts += 1 base_conf = next(base_generator) if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in obstacles): continue link_pose = get_link_pose(rover, link_from_name(rover, KINECT_FRAME)) if use_cone: mesh, _ = get_detection_cone(rover, objective, camera_link=KINECT_FRAME, depth=max_range) if mesh is None: continue cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) local_pose = Pose() else: distance = get_distance(point_from_pose(link_pose), target_point) if max_range < distance: continue cone = create_cylinder(radius=0.01, height=distance, color=(0, 0, 1, 0.5)) local_pose = Pose(Point(z=distance / 2.)) set_pose(cone, multiply(link_pose, local_pose)) # TODO: colors corresponding to scanned object if any( pairwise_collision(cone, b) for b in obstacles if b != objective and not is_placement(objective, b)): # TODO: ensure that this works for the rover remove_body(cone) continue if not reachable_test(rover, bq): continue print('Visibility attempts:', attempts) y = Ray(cone, rover, objective) yield Output(bq, y)
def gen(rock): base_joints = get_group_joints(rover, 'base') x, y, _ = get_point(rock) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) while True: theta = np.random.uniform(-np.pi, np.pi) base_conf = [x, y, theta] if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in fixed): continue yield (bq, )
def __init__(self, robots, limits, movable=[], collisions=True, goal_holding={}, goal_confs={}): self.robots = tuple(robots) self.limits = limits self.movable = tuple(movable) self.collisions = collisions self.goal_holding = goal_holding self.goal_confs = goal_confs self.obstacles = tuple(body for body in get_bodies() if body not in self.robots + self.movable) self.custom_limits = {} if self.limits is not None: for robot in self.robots: self.custom_limits.update(get_custom_limits( robot, self.limits)) self.initial_confs = { robot: Conf(robot, get_base_joints(robot)) for robot in self.robots } self.initial_poses = {body: Pose(body) for body in self.movable}
def __init__(self, body_from_name, robots, limits, collisions=True, goal_confs={}): self.body_from_name = dict(body_from_name) self.robots = tuple(robots) self.limits = limits self.collisions = collisions self.goal_confs = dict(goal_confs) robot_bodies = set(map(self.get_body, self.robots)) self.obstacles = tuple(body for body in get_bodies() if body not in robot_bodies) self.custom_limits = {} if self.limits is not None: for body in robot_bodies: self.custom_limits.update( get_custom_limits(body, self.limits, yaw_limit=(0, 0))) self.initial_confs = { name: Conf(self.get_body(name), get_base_joints(self.get_body(name))) for name in self.robots }
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False): robot = problem.robot domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = { 'world': 'world', } world = 'world' initial_bq = Pose(robot, get_pose(robot)) init = [ ('CanMove',), ('BConf', initial_bq), # TODO: could make pose as well... ('AtBConf', initial_bq), ('AtAConf', world, None), ('AtPose', world, world, None), ] + [('Sink', s) for s in problem.sinks] + \ [('Stove', s) for s in problem.stoves] + \ [('Connected', b, d) for b, d in problem.buttons] + \ [('Button', b) for b, _ in problem.buttons] #for arm in ARM_JOINT_NAMES: for arm in problem.arms: joints = get_arm_joints(robot, arm) conf = Conf(robot, joints, get_joint_positions(robot, joints)) init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf), ('AtPose', arm, arm, None)] if arm in problem.arms: init += [('Controllable', arm)] for body in problem.movable: pose = Pose(body, get_pose(body)) init += [('Graspable', body), ('Pose', body, pose), ('AtPose', world, body, pose)] for surface in problem.surfaces: init += [('Stackable', body, surface)] if is_placement(body, surface): init += [('Supported', body, pose, surface)] goal = ['and'] if problem.goal_conf is not None: goal_conf = Pose(robot, problem.goal_conf) init += [('BConf', goal_conf)] goal += [('AtBConf', goal_conf)] goal += [('Holding', a, b) for a, b in problem.goal_holding] + \ [('On', b, s) for b, s in problem.goal_on] + \ [('Cleaned', b) for b in problem.goal_cleaned] + \ [('Cooked', b) for b in problem.goal_cooked] stream_map = { 'sample-pose': get_stable_gen(problem), 'sample-grasp': from_list_fn(get_grasp_gen(problem)), 'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)), 'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)), #'plan-base-motion': empty_gen(), 'BTrajCollision': fn_from_constant(False), } # get_press_gen(problem, teleport=teleport) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def gen(o, p): if o in task.rooms: # TODO: predicate instead return # default_conf = arm_conf(a, g.carry) # joints = get_arm_joints(robot, a) # TODO: check collisions with fixed links target_point = point_from_pose(p.value) base_generator = visible_base_generator(robot, target_point, base_range) while True: set_pose(o, p.value) # p.assign() bq = Conf(robot, base_joints, next(base_generator)) # bq = Pose(robot, get_pose(robot)) bq.assign() if any(pairwise_collision(robot, b) for b in fixed): yield None else: yield (bq, )
def pddlstream_from_problem(problem): # TODO: push and attach to movable objects domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} # TODO: action to generically connect to the roadmap # TODO: could check individual vertices first # TODO: dynamically generate the roadmap in interesting parts of the space # TODO: visibility graphs for sparse roadmaps # TODO: approximate robot with isotropic geometry # TODO: make the effort finite if applied to the roadmap vertex samples = [] init = [] for robot, conf in problem.initial_confs.items(): samples.append(conf) init += [('Robot', robot), ('Conf', robot, conf), ('AtConf', robot, conf), ('Free', robot)] for body, pose in problem.initial_poses.items(): init += [('Body', body), ('Pose', body, pose), ('AtPose', body, pose)] goal_literals = [] goal_literals += [('Holding', robot, body) for robot, body in problem.goal_holding.items()] for robot, base_values in problem.goal_confs.items(): q_goal = Conf(robot, get_base_joints(robot), base_values) samples.append(q_goal) init += [('Conf', robot, q_goal)] goal_literals += [('AtConf', robot, q_goal)] goal_formula = And(*goal_literals) # TODO: assuming holonomic for now [body] = problem.robots with LockRenderer(): init += create_vertices(problem, body, samples) #init += create_edges(problem, body, samples) stream_map = { 'test-cfree-conf-pose': from_test(get_test_cfree_conf_pose(problem)), 'test-cfree-traj-pose': from_test(get_test_cfree_traj_pose(problem)), # TODO: sample pushes rather than picks/places 'sample-grasp': from_gen_fn(get_grasp_generator(problem)), 'compute-ik': from_fn(get_ik_fn(problem)), 'compute-motion': from_fn(get_motion_fn(problem)), 'test-reachable': from_test(lambda *args: False), 'Cost': get_cost_fn(problem), } #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def gen(rover, rock): base_joints = get_base_joints(rover) x, y, _ = get_point(rock) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) while True: for _ in range(max_attempts): theta = np.random.uniform(-np.pi, np.pi) base_conf = [x, y, theta] if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in obstacles): continue if not reachable_test(rover, bq): continue yield Output(bq) break else: yield None
def gen(objective): base_joints = get_group_joints(robot, 'base') head_joints = get_group_joints(robot, 'head') target_point = get_point(objective) base_generator = visible_base_generator(robot, target_point, base_range) lower_limits, upper_limits = get_custom_limits(robot, base_joints, custom_limits) while True: base_conf = next(base_generator) if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(robot, base_joints, base_conf) bq.assign() if any(pairwise_collision(robot, b) for b in fixed): continue head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible continue hq = Conf(robot, head_joints, head_conf) y = None yield (bq, hq, y)
def gen_fn(robot): joints = get_base_joints(robot) sample_fn = get_halton_sample_fn(robot, joints, custom_limits=problem.custom_limits) collision_fn = get_collision_fn(robot, joints, problem.obstacles, attachments=[], self_collisions=False, disabled_collisions=set(), custom_limits=problem.custom_limits, max_distance=MAX_DISTANCE) handles = [] while True: sample = sample_fn() if not collision_fn(sample): q = Conf(robot, joints, sample) handles.extend(draw_point(point_from_conf(sample), size=0.05)) yield (q,)
def get_reachable_test(problem, iterations=10, **kwargs): initial_confs = { rover: Conf(rover, get_base_joints(rover)) for rover in problem.rovers } motion_fn = get_motion_fn(problem, restarts=0, iterations=iterations, smooth=0, **kwargs) def test(rover, bq): bq0 = initial_confs[rover] result = motion_fn(rover, bq0, bq) return result is not None return test
def gen(o, p): if o in task.rooms: #bq = Pose(robot, unit_pose()) #bq = state.poses[robot] bq = Conf(robot, base_joints, np.zeros(len(base_joints))) #hq = Conf(robot, head_joints, np.zeros(len(head_joints))) #ht = create_trajectory(robot, head_joints, plan_pause_scan_path(robot, tilt=tilt)) waypoints = plan_scan_path(task.robot, tilt=tilt) set_group_conf(robot, 'head', waypoints[0]) path = plan_waypoints_joint_motion(robot, head_joints, waypoints[1:], obstacles=None, self_collisions=False) ht = create_trajectory(robot, head_joints, path) yield bq, ht.path[0], ht else: for bq, hq in vis_gen(o, p): ht = Trajectory([hq]) yield bq, ht.path[0], ht
def fn(o, p, bq): set_pose(o, p.value) # p.assign() bq.assign() if o in task.rooms: waypoints = plan_scan_path(task.robot, tilt=ROOM_SCAN_TILT) set_group_conf(robot, 'head', waypoints[0]) path = plan_waypoints_joint_motion(robot, head_joints, waypoints[1:], obstacles=None, self_collisions=False) if path is None: return None ht = create_trajectory(robot, head_joints, path) hq = ht.path[0] else: target_point = point_from_pose(p.value) head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible return None hq = Conf(robot, head_joints, head_conf) ht = Trajectory([hq]) return (hq, ht)
def pddlstream_from_state(state, teleport=False): task = state.task robot = task.robot # TODO: infer open world from task domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = { 'base': 'base', 'left': 'left', 'right': 'right', 'head': 'head', } #base_conf = state.poses[robot] base_conf = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) scan_cost = 1 init = [ ('BConf', base_conf), ('AtBConf', base_conf), Equal(('MoveCost', ), 1), Equal(('PickCost', ), 1), Equal(('PlaceCost', ), 1), Equal(('ScanCost', ), scan_cost), Equal(('RegisterCost', ), 1), ] holding_arms = set() holding_bodies = set() for attach in state.attachments.values(): holding_arms.add(attach.arm) holding_bodies.add(attach.body) init += [('Grasp', attach.body, attach.grasp), ('AtGrasp', attach.arm, attach.body, attach.grasp)] for arm in ARM_NAMES: joints = get_arm_joints(robot, arm) conf = Conf(robot, joints, get_joint_positions(robot, joints)) init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)] if arm in task.arms: init += [('Controllable', arm)] if arm not in holding_arms: init += [('HandEmpty', arm)] for body in task.get_bodies(): if body in holding_bodies: continue # TODO: no notion whether observable actually corresponds to the correct thing pose = state.poses[body] init += [ ('Pose', body, pose), ('AtPose', body, pose), ('Observable', pose), ] init += [('Scannable', body) for body in task.rooms + task.surfaces] init += [('Registerable', body) for body in task.movable] init += [('Graspable', body) for body in task.movable] for body in task.get_bodies(): supports = task.get_supports(body) if supports is None: continue for surface in supports: p_obs = state.b_on[body].prob(surface) cost = revisit_mdp_cost(0, scan_cost, p_obs) # TODO: imperfect observation model init += [('Stackable', body, surface), Equal(('LocalizeCost', surface, body), clip_cost(cost))] #if is_placement(body, surface): if is_center_stable(body, surface): if body in holding_bodies: continue pose = state.poses[body] init += [('Supported', body, pose, surface)] for body in task.get_bodies(): if state.is_localized(body): init.append(('Localized', body)) else: init.append(('Uncertain', body)) if body in state.registered: init.append(('Registered', body)) goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \ [('On', b, s) for b, s in task.goal_on] + \ [('Localized', b) for b in task.goal_localized] + \ [('Registered', b) for b in task.goal_registered]) stream_map = { 'sample-pose': get_stable_gen(task), 'sample-grasp': from_list_fn(get_grasp_gen(task)), 'inverse-kinematics': from_gen_fn(get_ik_ir_gen(task, teleport=teleport)), 'plan-base-motion': from_fn(get_motion_gen(task, teleport=teleport)), 'test-vis-base': from_test(get_in_range_test(task, VIS_RANGE)), 'test-reg-base': from_test(get_in_range_test(task, REG_RANGE)), 'sample-vis-base': accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)), max_attempts=25), 'sample-reg-base': accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)), max_attempts=25), 'inverse-visibility': from_fn(get_inverse_visibility_fn(task)), } return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_problem(problem, collisions=True, **kwargs): # TODO: push and attach to movable objects domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} camera = 'RGBD' mode = 'color' # highres | lowres | color | depth init = [] goal_literals = [ #('Calibrated', camera, problem.rovers[0]) #('Analyzed', problem.rovers[0], problem.rocks[0]) #('ReceivedAnalysis', problem.rocks[0]) Exists(['?rock'], And(('Type', '?rock', 'stone'), ('ReceivedAnalysis', '?rock'))), Exists(['?soil'], And(('Type', '?soil', 'soil'), ('ReceivedAnalysis', '?soil'))), ] init += [('Type', b, ty) for b, ty in problem.body_types] init += [('Lander', l) for l in problem.landers] init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)] for rover in problem.rovers: base_joints = get_base_joints(rover) q0 = Conf(rover, base_joints) init += [('Rover', rover), ('OnBoard', camera, rover), ('Conf', rover, q0), ('AtConf', rover, q0)] goal_literals += [('AtConf', rover, q0)] for store in problem.stores: init += [('Store', store)] init += [('Free', rover, store) for rover in problem.rovers] goal_literals += [('Free', rover, store) for rover in problem.rovers] for name in problem.objectives: init += [('Objective', name)] goal_literals += [('ReceivedImage', name, mode)] for name in problem.rocks: init += [('Rock', name)] goal_formula = And(*goal_literals) custom_limits = {} if problem.limits is not None: for rover in problem.rovers: custom_limits.update(get_base_custom_limits(rover, problem.limits)) stream_map = { 'test-cfree-ray-conf': from_test(get_cfree_ray_test(problem, collisions=collisions)), 'test-reachable': from_test( get_reachable_test(problem, custom_limits=custom_limits, collisions=collisions, **kwargs)), 'obj-inv-visible': from_gen_fn( get_inv_vis_gen(problem, custom_limits=custom_limits, collisions=collisions, **kwargs)), 'com-inv-visible': from_gen_fn( get_inv_com_gen(problem, custom_limits=custom_limits, collisions=collisions, **kwargs)), 'sample-above': from_gen_fn( get_above_gen(problem, custom_limits=custom_limits, collisions=collisions, **kwargs)), 'sample-motion': from_fn( get_motion_fn(problem, custom_limits=custom_limits, collisions=collisions, **kwargs)), } #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def pddlstream_from_problem(problem, collisions=True, teleport=False): #rovers = problem.rovers domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} camera = 'RGBD' mode = 'color' # highres | lowres | color | depth init = [] goal_literals = [ #('Calibrated', camera, problem.rovers[0]) #('Analyzed', problem.rovers[0], problem.rocks[0]) #('ReceivedAnalysis', problem.rocks[0]) ] init += [('Lander', l) for l in problem.landers] init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)] for rover in problem.rovers: base_joints = get_group_joints(rover, 'base') bq0 = Conf(rover, base_joints) head_joints = get_group_joints(rover, 'head') hq0 = Conf(rover, head_joints) init += [('Rover', rover), ('OnBoard', camera, rover), ('BConf', bq0), ('AtBConf', rover, bq0), ('HConf', hq0), ('AtHConf', rover, hq0)] for name in problem.stores: init += [('Store', name)] init += [('Free', rover, name) for rover in problem.rovers] for name in problem.objectives: init += [('Objective', name)] #initial_atoms += [IsClass(name, cl) for cl in CLASSES if cl in name] goal_literals += [('ReceivedImage', name, mode)] for name in problem.rocks + problem.soils: # pose = Pose(name, get_pose(env.GetKinBody(name))) init += [('Rock', name)] # , IsPose(name, pose), AtPose(name, pose)] #init += [('IsClass', name, cl) for cl in CLASSES if cl in name] #if problem.rocks: # goal_literals.append(Exists(['?rock'], And(('Rock', '?rock'), # ('ReceivedAnalysis', '?rock')))) #if problem.soils: # goal_literals.append(Exists(['?soil'], And(('Soil', '?soil'), # ('ReceivedAnalysis', '?soil')))) # TODO: soil class goal_formula = And(*goal_literals) custom_limits = {} if problem.limits is not None: for rover in problem.rovers: custom_limits.update(get_base_custom_limits(rover, problem.limits)) stream_map = { 'test-reachable': from_test(get_reachable_test(problem)), 'obj-inv-visible': from_gen_fn(get_inv_vis_gen(problem, custom_limits=custom_limits)), 'com-inv-visible': from_gen_fn(get_inv_com_gen(problem, custom_limits=custom_limits)), 'sample-above': from_gen_fn(get_above_gen(problem, custom_limits=custom_limits)), 'sample-base-motion': from_fn(get_base_motion_fn(problem, custom_limits=custom_limits, teleport=teleport)), 'sample-head-motion': from_fn(get_head_motion_fn(problem, teleport=teleport)), } #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def pddlstream_from_problem(problem, base_limits=None, collisions=True, teleport=False): robot = problem.robot domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = { '@sink': 'sink', '@stove': 'stove', } #initial_bq = Pose(robot, get_pose(robot)) initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) init = [ ('CanMove',), ('BConf', initial_bq), ('AtBConf', initial_bq), Equal(('PickCost',), 1), Equal(('PlaceCost',), 1), ] + [('Sink', s) for s in problem.sinks] + \ [('Stove', s) for s in problem.stoves] + \ [('Connected', b, d) for b, d in problem.buttons] + \ [('Button', b) for b, _ in problem.buttons] for arm in ARM_NAMES: #for arm in problem.arms: joints = get_arm_joints(robot, arm) conf = Conf(robot, joints, get_joint_positions(robot, joints)) init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf)] if arm in problem.arms: init += [('Controllable', arm)] for body in problem.movable: pose = Pose(body, get_pose(body), init=True) # TODO: supported here init += [('Graspable', body), ('Pose', body, pose), ('AtPose', body, pose), ('Stackable', body, None)] for surface in problem.surfaces: if is_placement(body, surface): init += [('Supported', body, pose, surface)] for body, ty in problem.body_types: init += [('Type', body, ty)] bodies_from_type = get_bodies_from_type(problem) goal_literals = [] if problem.goal_conf is not None: goal_conf = Conf(robot, get_group_joints(robot, 'base'), problem.goal_conf) init += [('BConf', goal_conf)] goal_literals += [('AtBConf', goal_conf)] for ty, s in problem.goal_on: bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter( ty) else [ty] init += [('Stackable', b, s) for b in bodies] goal_literals += [('On', ty, s)] goal_literals += [('Holding', a, b) for a, b in problem.goal_holding] + \ [('Cleaned', b) for b in problem.goal_cleaned] + \ [('Cooked', b) for b in problem.goal_cooked] goal_formula = [] for literal in goal_literals: parameters = [a for a in get_args(literal) if is_parameter(a)] if parameters: type_literals = [('Type', p, get_parameter_name(p)) for p in parameters] goal_formula.append( Exists(parameters, And(literal, *type_literals))) else: goal_formula.append(literal) goal_formula = And(*goal_formula) custom_limits = {} if base_limits is not None: custom_limits.update(get_custom_limits(robot, problem.base_limits)) stream_map = { 'sample-pose': from_gen_fn(get_stable_gen(problem, collisions=collisions)), 'sample-grasp': from_list_fn(get_grasp_gen(problem, collisions=collisions)), #'sample-grasp': from_gen_fn(get_grasp_gen(problem, collisions=collisions)), 'inverse-kinematics': from_gen_fn( get_ik_ir_gen(problem, custom_limits=custom_limits, collisions=collisions, teleport=teleport)), 'plan-base-motion': from_fn( get_motion_gen(problem, custom_limits=custom_limits, collisions=collisions, teleport=teleport)), 'test-cfree-pose-pose': from_test(get_cfree_pose_pose_test(collisions=collisions)), 'test-cfree-approach-pose': from_test(get_cfree_approach_pose_test(problem, collisions=collisions)), 'test-cfree-traj-pose': from_test(get_cfree_traj_pose_test(robot, collisions=collisions)), #'test-cfree-traj-grasp-pose': from_test(get_cfree_traj_grasp_pose_test(problem, collisions=collisions)), #'MoveCost': move_cost_fn, 'Distance': distance_fn, } #stream_map = DEBUG return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def pddlstream_from_problem(problem, teleport=False): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {'{}'.format(name).lower(): name for name in problem.initial_confs.keys()} edges = set() init = [ ] for name, conf in problem.initial_confs.items(): init += [ ('Safe',), ('Robot', name), ('Conf', conf), ('AtConf', name, conf), Equal(('Speed', name), DIST_PER_TIME), Equal(('BatteryCapacity', name), MAX_ENERGY), Equal(('RechargeRate', name), CHARGE_PER_TIME), Equal(('ConsumptionRate', name), BURN_PER_TIME), Equal(('Energy', name), INITIAL_ENERGY), ] goal_literals = [ #('Safe',), #('Unachievable',), ] for name, base_values in problem.goal_confs.items(): body = problem.get_body(name) joints = get_base_joints(body) #extend_fn = get_extend_fn(body, joints, resolutions=5*BASE_RESOLUTIONS) #q_init = problem.initial_confs[name] #path = [q_init] + [Conf(body, joints, q) for q in extend_fn(q_init.values, base_values)] #edges.update(zip(path, path[1:])) #q_goal = path[-1] q_goal = Conf(body, joints, base_values) init += [('Conf', q_goal)] goal_literals += [('AtConf', name, q_goal)] goal_formula = And(*goal_literals) robot = list(map(problem.get_body, problem.initial_confs))[0] with LockRenderer(): init += [('Conf', q) for _, n, q in create_vertices(problem, robot, [], samples_per_ft2=6)] #vertices = {v for edge in edges for v in edge} #handles = [] #for vertex in vertices: # handles.extend(draw_point(point_from_conf(vertex.values), size=0.05)) #for conf1, conf2 in edges: # traj = linear_trajectory(conf1, conf2) # init += [ # ('Conf', conf1), # ('Traj', traj), # ('Conf', conf2), # ('Motion', conf1, traj, conf2), # ] #draw_edges(edges) cfree_traj_traj_test = get_test_cfree_traj_traj(problem) cfree_traj_traj_list_gen_fn = from_test(cfree_traj_traj_test) traj_traj_collision_test = negate_test(cfree_traj_traj_test) stream_map = { 'test-cfree-conf-conf': cfree_traj_traj_list_gen_fn, 'test-cfree-traj-conf': cfree_traj_traj_list_gen_fn, 'test-cfree-traj-traj': cfree_traj_traj_list_gen_fn, 'ConfConfCollision': traj_traj_collision_test, 'TrajConfCollision': traj_traj_collision_test, 'TrajTrajCollision': traj_traj_collision_test, 'compute-motion': from_fn(get_motion_fn2(problem)), 'TrajDistance': get_distance_fn(), } #stream_map = 'debug' problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula) return problem, edges