forked from drwiner/PyDPOCL
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathgenerate_random_problems.py
More file actions
179 lines (163 loc) · 6.35 KB
/
Copy pathgenerate_random_problems.py
File metadata and controls
179 lines (163 loc) · 6.35 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
import sys
import random
import json
from shapely import box, intersects
def generate_problem(name):
n_robots = 2
max_nr_objects = 6
n_goals = random.randint(1, max_nr_objects+1)
n_objects = random.randint(n_goals, max_nr_objects+1)
min_obj_size = 0.1
max_obj_size = 0.4
buffer = 0.1 # goal areas should be this much larger than the object
max_goal_size = 1
goal_overlap_allowed = False
table_width = 2.5
table_length = 1.5
attempts = 0
max_attempts = 1000
# generate the reaches of the different robots
robot_reach = []
if n_robots == 2:
width_overlap = random.uniform(max_obj_size+buffer, 1.0)
x_overlap = (table_width-width_overlap)/2 # overlap in the middle of the table
robot_reach = []
# robot 1
robot_reach.append([[0, 0],
[0, table_length],
[x_overlap+width_overlap, table_length],
[x_overlap+width_overlap, 0]])
# robot 2
robot_reach.append([[x_overlap, 0],
[x_overlap, table_length],
[table_width, table_length],
[table_width, 0]])
else:
raise ValueError(f"unsupported number of robots {n_robots}")
# generate objects and their initial positions
obj_geometry = []
obj_list = []
goal_geometry = []
goal_list = []
for i in range(n_objects):
redo = True
while redo:
attempts+=1
if attempts > max_attempts:
return False
width = random.uniform(min_obj_size, max_obj_size)
length = random.uniform(min_obj_size, max_obj_size)
x_pos = random.uniform(0, table_width-width)
y_pos = random.uniform(0, table_length-length)
obj_box = box(x_pos, y_pos, x_pos+width, y_pos+length)
for other_obj in obj_geometry:
if intersects(obj_box, other_obj):
break
else: # object does not overlap with another
redo = False
obj_geometry.append(obj_box)
obj_list.append({
"name": f"box_{i}",
"initial_pose": [x_pos+0.5*width, y_pos+0.5*length],
"width": width,
"length": length
})
if i < n_goals:
redo = True
while redo:
attempts+=1
if attempts > max_attempts:
return False
goal_width = random.uniform(width+buffer, max_goal_size)
goal_length = random.uniform(length+buffer, max_goal_size)
x_goal = random.uniform(0, table_width-goal_width)
y_goal = random.uniform(0, table_length-goal_length)
if goal_overlap_allowed:
break # exit the while loop
# ensure the goal does not overlap with previous goals
goal_box = box(x_goal, y_goal, x_goal+goal_width, y_goal+goal_length)
for other_goal in goal_geometry:
if intersects(goal_box, other_goal):
break
# object does not overlap with another
else:
redo = False
goal_geometry.append(goal_box)
goal_list.append({"name": f"goal_{i}",
"coords": [
[x_goal, y_goal],
[x_goal, y_goal+goal_length],
[x_goal+goal_width, y_goal+goal_length],
[x_goal+goal_width, y_goal]
]
})
# create pddl file
robots_string = ""
reach_string = ""
for i in range(n_robots):
robots_string = robots_string + f"robot_{i} "
reach_string = reach_string + f"reach_robot_{i} "
box_string = ""
for i in range(n_objects):
box_string = box_string + f"box_{i} "
goal_string = ""
goal_area_string = ""
for i in range(n_goals):
goal_area_string = goal_area_string + f"goal_{i} "
goal_string = goal_string + f" (within box_{i} goal_{i})\n"
pddl_content = f"""
(define (problem test-{i}-problem)
(:domain manipulation)
(:objects {robots_string} - robot
{box_string} - physical_item
table - area
{goal_area_string} - area
{reach_string} - area
)
(:init )
(:goal (and
{goal_string} )))
"""
filepath = name + "_problem.pddl"
with open(filepath, "w") as f:
f.write(pddl_content)
# create worldmodel file
worldmodel_dict = {}
worldmodel_dict["domain"] = "manipulation-domain"
worldmodel_dict["robots"] = []
for i in range(n_robots):
worldmodel_dict["robots"].append({"name": f"robot_{i}",
"reach": f"reach_robot_{i}"})
worldmodel_dict["base_area"] = "table"
worldmodel_dict["areas"] = []
worldmodel_dict["areas"].append({"name": "table",
"coords": [
[0, 0],
[0, table_length],
[table_width, table_length],
[table_width, 0]
]
})
for i in range(n_robots):
worldmodel_dict["areas"].append({"name": f"reach_robot_{i}",
"coords": robot_reach[i]
})
for i in range(n_goals):
worldmodel_dict["areas"].append(goal_list[i])
worldmodel_dict["objects"] = obj_list
filepath = name+"_worldmodel.json"
with open(filepath, "w") as f:
json.dump(worldmodel_dict, f, indent=2)
print(f"made random problem: {name} with {n_objects} objects and {n_goals} goals")
return True
if __name__ == '__main__':
num_args = len(sys.argv)
if num_args >1:
n = int(sys.argv[1])
else:
n = 10
for i in range(n):
problem_path = "domains/manipulation-domain-batch/"
problem_name = problem_path + "test_" + str(i)
while not generate_problem(problem_name):
pass