(goal, obstacles=(), regions={}, max_distance=.5)
| 18 | #ARRAY = tuple # Hashing |
| 19 | |
| 20 | def create_problem(goal, obstacles=(), regions={}, max_distance=.5): |
| 21 | directory = os.path.dirname(os.path.abspath(__file__)) |
| 22 | domain_pddl = read(os.path.join(directory, 'domain.pddl')) |
| 23 | stream_pddl = read(os.path.join(directory, 'stream.pddl')) |
| 24 | constant_map = {} |
| 25 | |
| 26 | q0 = ARRAY([0, 0]) |
| 27 | init = [ |
| 28 | ('Conf', q0), |
| 29 | ('AtConf', q0), |
| 30 | ] + [('Region', r) for r in regions] |
| 31 | |
| 32 | if isinstance(goal, str): |
| 33 | goal = ('In', goal) |
| 34 | else: |
| 35 | init += [('Conf', goal)] |
| 36 | goal = ('AtConf', goal) |
| 37 | |
| 38 | np.set_printoptions(precision=3) |
| 39 | samples = [] |
| 40 | def region_gen(region): |
| 41 | lower, upper = regions[region] |
| 42 | area = np.product(upper - lower) |
| 43 | # TODO: sample proportional to area |
| 44 | while True: |
| 45 | q = ARRAY(sample_box(regions[region])) |
| 46 | samples.append(q) |
| 47 | yield (q,) |
| 48 | |
| 49 | # http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf |
| 50 | #d = 2 |
| 51 | #vol_free = (1 - 0) * (1 - 0) |
| 52 | #vol_ball = math.pi * (1 ** 2) |
| 53 | #gamma = 2 * ((1 + 1. / d) * (vol_free / vol_ball)) ** (1. / d) |
| 54 | |
| 55 | roadmap = [] |
| 56 | def connected_test(q1, q2): |
| 57 | #n = len(samples) |
| 58 | #threshold = gamma * (math.log(n) / n) ** (1. / d) |
| 59 | threshold = max_distance |
| 60 | are_connected = (get_distance(q1, q2) <= threshold) and \ |
| 61 | is_collision_free((q1, q2), obstacles) |
| 62 | if are_connected: |
| 63 | roadmap.append((q1, q2)) |
| 64 | return are_connected |
| 65 | |
| 66 | stream_map = { |
| 67 | 'sample-region': from_gen_fn(region_gen), |
| 68 | 'connect': from_test(connected_test), |
| 69 | 'distance': get_distance, |
| 70 | } |
| 71 | |
| 72 | problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal) |
| 73 | |
| 74 | return problem, samples, roadmap |
| 75 | |
| 76 | |
| 77 | ################################################## |
no test coverage detected