def pddlstream_from_problem(robot, movable=[], teleport=False, grasp_name='top'): #assert (not are_colliding(tree, kin_cache)) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} print('Robot:', robot) conf = BodyConf(robot, get_configuration(robot)) init = [('CanMove',), ('Conf', conf), ('AtConf', conf), ('HandEmpty',)] fixed = get_fixed(robot, movable) print('Movable:', movable) print('Fixed:', fixed) for body in movable: pose = BodyPose(body, get_pose(body)) init += [('Graspable', body), ('Pose', body, pose), ('AtPose', body, pose)] for surface in fixed: init += [('Stackable', body, surface)] if is_placement(body, surface): init += [('Supported', body, pose, surface)] for body in fixed: name = get_body_name(body) if 'sink' in name: init += [('Sink', body)] if 'stove' in name: init += [('Stove', body)] body = movable[0] goal = ('and', ('AtConf', conf), #('Holding', body), #('On', body, fixed[1]), #('On', body, fixed[2]), #('Cleaned', body), ('Cooked', body), ) stream_map = { 'sample-pose': from_gen_fn(get_stable_gen(fixed)), 'sample-grasp': from_gen_fn(get_grasp_gen(robot, grasp_name)), 'inverse-kinematics': from_fn(get_ik_fn(robot, fixed, teleport)), 'plan-free-motion': from_fn(get_free_motion_gen(robot, fixed, teleport)), 'plan-holding-motion': from_fn(get_holding_motion_gen(robot, fixed, teleport)), 'TrajCollision': get_movable_collision_test(), } if USE_SYNTHESIZERS: stream_map.update({ 'plan-free-motion': empty_gen(), 'plan-holding-motion': empty_gen(), }) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def fn(outputs, certified): assert (len(outputs) == 1) q0, _, q1 = find_unique(lambda f: f[0] == 'freemotion', certified)[1:] obstacles = fixed + place_movable(certified) free_motion_fn = get_free_motion_gen(robot, obstacles, teleport) return free_motion_fn(q0, q1)