def step_to_block(self, cfg_0, cfg_m):
steps = [cfg_0, cfg_m]
p0 = cfg_0.positions[-1]
pm = cfg_m.positions[-1]
d = gm.distance(p0, pm)
# print(d)
# print(p0, pm)
theta = math.atan2(float(pm[1] - p0[1]), (pm[0] - p0[0]))
# print(theta)
num_steps = int(math.ceil(d * 1.0 / self.MAX_STEP))
# print(num_steps)
while len(steps) - 1 < num_steps:
for i in range(num_steps - 1):
cfg_1 = steps[i]
cfg_nxt = self.config_from_point(gm.point_from_polar(
cfg_1.positions[-1], self.MAX_STEP, theta))
steps.insert(i + 1, cfg_nxt)
if (not self.validator.fits_bounds(cfg_nxt)) \
or self.validator.has_collision(
cfg_nxt, self.obstacles):
print(i)
print("hit block or out of bounds")
print(cfg_nxt.positions[-1])
return cfg_nxt.positions[-1]
# print(steps)
# print(len(steps))
return None
def make_steps(self, cfg_0, cfg_m, check=True):
"""
Interpolate a straight-line path between configurations in C-space.
:param cfg_0: the initial configuration
:param cfg_m: the final configuration
:param check: check if the path is collision free if True
:return: a tuple; the first element is 1 if a connection is
made and 0 otherwise; the second element is the interpolated
configurations from the initial to the final state inclusive.
"""
connection = 0 # 1 if valid connection is made
steps = [cfg_0, cfg_m]
end_loop = False
max_step = 1000.0 # max of all iterations
while True:
old_steps = list(steps)
current_max_step = 0 # max of current iteration
end_index = len(steps) - 1
for i in range(end_index):
# Interpolate the midpoint configuration
cfg_1 = old_steps[i]
cfg_2 = old_steps[i + 1]
cfg_nxt = self.midpoint(cfg_1, cfg_2)
steps.insert(2 * i + 1, cfg_nxt)
i_max_step = gm.distance(cfg_1.positions[-1],
cfg_nxt.positions[-1])
if i_max_step > current_max_step:
current_max_step = i_max_step
if check and (self.validator.has_collision(cfg_nxt,
self.obstacles)
or not self.validator.fits_bounds(cfg_nxt)):
end_loop = True
break
if current_max_step < max_step:
max_step = current_max_step
if max_step < self.MAX_STEP:
# No collision and primitive step size has been reached
if check:
# Connect vertices in state graph
jump = PathJump(cfg_0, cfg_m)
cfg_0.add_connector(cfg_m, jump)
cfg_m.add_connector(cfg_0, jump)
connection = 1
break
if end_loop:
break
return connection, steps
def max_distance(self, cfg_1):
"""
Compute the maximum euclidean distance between corresponding
positions of this configuration and the given configuration
:param cfg_1:
:return:
"""
max_distance = 0.0
for i in range(len(self.positions)):
p0 = self.positions[i]
p1 = cfg_1.positions[i]
d = gm.distance(p0, p1)
if d > max_distance:
max_distance = d
return max_distance
def total_distance(self, cfg_1):
total_distance = 0.0
for i in range(len(self.positions)):
total_distance += gm.distance(self.get_position(i),
cfg_1.get_position(i))
return total_distance
def move_block(self):
push_behind = 50
push_distance = 50
max_push_distance = 150
# Plan path with movable block removed until path found
for block in self.scene.moveableBlocks:
self.set_obstacles(self.scene, pucks=True)
if block.y < 0:
continue
print("Attempting to remove block at ", block.x, block.y)
print("Length before removal = ", len(self.obstacles))
self.obstacles.remove(block.shape)
print("Length after removal = ", len(self.obstacles))
if len(self.scene.moveableBlocks) == 1:
all_paths, costs = self.find_all_paths(impatient=False)
else:
all_paths, costs = self.find_all_paths(impatient=True)
if self.all_paths_found:
self.configs = []
block_to_move = block
x = block.x
y = block.y
if block.blockType == BlockSettings.CYLINDER:
push_behind = 40
if block.blockType == BlockSettings.CUBE:
push_behind = 45
elif block.blockType == BlockSettings.PRISM:
push_behind = 65
break
self.configs = []
if not self.all_paths_found:
print("Error: Couldn't find a path for block pushing")
exit()
# Goals are obstacles when it comes to pushing
self.set_obstacles(self.scene, pucks=True, goals=True)
self.obstacles.remove(block_to_move.shape)
min_d1 = 1000000
min_d2 = 1000000
nearest_block1 = None
nearest_block2 = None
for block in self.scene.blocks:
if block.y < 0:
continue
d = gm.distance((block.x, block.y), (x, y))
if d < 0.5:
# THIS IS THE BLOCK THATS MOVING. 0.5 IS ARBITRARY
continue
if d < min_d1:
min_d2 = min_d1
min_d1 = d
nearest_block2 = nearest_block1
nearest_block1 = (block.x, block.y)
elif d < min_d2:
min_d2 = d
nearest_block2 = (block.x, block.y)
# Check if block is closer to boundary than one of the nearest
ww = WorkspaceSettings.WIDTH
wh = WorkspaceSettings.HEIGHT
bottom = gm.Line2D((-ww / 2, 0.0), (ww / 2, 0.0))
right = gm.Line2D((ww / 2, 0.0), (ww / 2, wh))
top = gm.Line2D((ww / 2, wh), (-ww / 2, wh))
left = gm.Line2D((-ww / 2, wh), (-ww / 2, 0.0))
boundaries = [bottom, right, top, left]
# Make the boundary point one of the nearest if so
min_db = 1000000
for boundary in boundaries:
p = boundary.p_point((x, y))
d = boundary.p_distance((x, y))
if d < min_d1:
min_d2 = min_d1
min_d1 = d
nearest_block2 = nearest_block1
nearest_block1 = p
elif d < min_d2:
min_d2 = d
nearest_block2 = p
if nearest_block1 is None or nearest_block2 is None:
# Maybe there were not enought blocks or something?
# This shouldn't happen, but if it does it will cause problems
print("Planner: Error finding nearest blocks in moving blocks")
return
print("Found Nearest Blocks: ", nearest_block1, nearest_block2)
nearest_block_line = gm.Line2D(nearest_block1, nearest_block2)
m1, c1 = nearest_block_line.find_normal_at_point((x, y))
print("Nearest block line: m,c: ", m1, c1)
# Direction of push
theta = math.atan2(m1, 1.0)
print("Theta is ", theta)
print("x,y is :", x, y)
start_point = gm.point_from_polar((x, y), push_behind, theta + np.pi)
start_point = (start_point[0], start_point[1] - 5.0)
print("Start point is: ", start_point)
start_cfg = self.config_from_point(start_point)
max_point = gm.point_from_polar((x, y), max_push_distance, theta)
max_cfg = self.config_from_point(max_point)
# Move forward (~ along path)
# Check each direction until not in collision
# TODO: intersect functions for prism and cube
nearest_collision = self.step_to_block(start_cfg, max_cfg)
if nearest_collision is None:
push_distance = max_push_distance
else:
push_distance = gm.distance((x, y), nearest_collision)
print("Nearest collision for first direction: ", nearest_collision)
# See if opposite direction is better
print("Checking opposite push direction")
start_point2 = gm.point_from_polar((x, y), push_behind, theta)
start_point2 = (start_point2[0], start_point2[1] - 5.0)
start_cfg2 = self.config_from_point(start_point2)
max_point2 = gm.point_from_polar((x, y), max_push_distance, theta + np.pi)
max_cfg2 = self.config_from_point(max_point)
nearest_collision2 = self.step_to_block(start_cfg2, max_cfg2)
print("Nearest collision for second direction: ", nearest_collision2)
if nearest_collision2 is None:
if nearest_collision is not None:
# Opposite direction does not collide -> better
theta += np.pi
start_point = start_point2
start_cfg = start_cfg2
push_distance = max_push_distance
# Both None -> go with first option
elif nearest_collision is not None:
# Both have collision -> choose longest available push
push_distance2 = gm.distance((x, y), nearest_collision2)
if push_distance2 > push_distance:
print("Chose opposite direction")
# Direction 2 is better
theta += np.pi
start_point = start_point2
start_cfg = start_cfg2
push_distance = push_distance2
# Else no change, go with direction 1
end_point = gm.point_from_polar((x, y), push_distance - 20, theta)
end_point = (end_point[0], end_point[1] - 5.0)
print("End point is: ", end_point)
end_cfg = self.config_from_point(end_point)
path = Path()
path.path.append(start_cfg)
path.path.append(end_cfg)
path.movingPath = True
self.super_path.paths.append(path)
# Update block position after push (inaccurate)
self.set_obstacles(self.scene, pucks=True)
self.obstacles.remove(block_to_move.shape)
block_to_move.x = end_point[0]
block_to_move.y = end_point[1]
block_to_move.find_shape()
print("Adding back the moved block: ", block_to_move.shape)
# Remove goals for path planning
self.obstacles.append(block_to_move.shape)
def do_query(self, root_index, goal_index):
"""
Search for a path between the specified root and goal via the roadmap
:return: True if a path was found
"""
self.validator.set_enlarge_factor(0.5)
print("Planner: finding path from puck {0} to goal {1}"
.format(root_index, goal_index))
root = self.get_end_state(0, index=root_index)
goal = self.get_end_state(1, index=goal_index)
if not self.validator.is_valid_sample(root, self.obstacles, debug=False) or \
not self.validator.is_valid_sample(goal, self.obstacles, debug=False):
print("Planner: ERROR: invalid end states")
print("Valid root: ", self.validator.is_valid_sample(root, self.obstacles))
print("Valid goal: ", self.validator.is_valid_sample(goal, self.obstacles))
exit()
if self.safety:
self.validator.set_enlarge_factor(self.SAFE_FACTOR)
else:
self.validator.set_enlarge_factor(self.RISKY_FACTOR)
# Try direct path
direct_connection = self.make_steps(root, goal, check=True)[0]
if direct_connection:
self.path.set_path([root, goal])
self.path_found = True
self.path.cost = gm.distance(root.positions[-1],
goal.positions[-1])
print("Planner: goal found directly")
return True
# Find vertices nearest initial and goal configs
connected_root = self.connect_configs(root)
connected_goal = self.connect_configs(goal)
# Check if root and goal were connected
if not (connected_root and connected_goal):
# Solution not possible
return False
# Initial and goal states are now in the roadmap
# Search for a path
ass = AStarSearch(root, goal)
print("Planner: searching")
ass.search()
if ass.goal_found:
# print("Planner: visualising")
# self.visualise_graph()
# Record solution
path = ass.path
full_path = []
while len(path) > 1:
cfg_0 = path.pop(0)
# Uncomment below 4 lines to display/calculate curved paths
# cfg_m = path[0]
# steps = self.make_steps(cfg_0, cfg_m, check=False)[1]
# steps.pop()
# full_path += steps
full_path.append(cfg_0)
full_path.append(goal)
self.path.set_path(full_path)
self.path.cost = ass.total_cost
self.path_found = True
# Remove this root and goal from the roadmap, ready for the next
for cfg in root.connectors:
cfg.connectors.pop(root, False)
for cfg in goal.connectors:
cfg.connectors.pop(goal, False)
return True
return False