def find_graph_path(spec, init_node):
"""
This method performs a breadth first search of the state graph and return a list of configs which form a path
through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step
requirement - you will need to interpolate between the configs in the returned list.
You may use this method in your solver if you wish, or can implement your own graph search algorithm to improve
performance.
:param spec: ProblemSpec object
:param init_node: GraphNode object for the initial configuration
:return: List of configs forming a path through the graph from initial to goal
"""
# search the graph
init_container = [init_node]
# here, each key is a graph node, each value is the list of configs visited on the path to the graph node
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
if test_config_equality(current.config, spec.goal, spec):
# found path to goal
return init_visited[current]
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
return None
def solve(spec, output):
"""
An example solve method containing code to perform a breadth first search of the state graph and return a list of
configs which form a path through the state graph between the initial and the goal. Note that this path will not
satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list.
If you wish to use this code, you may either copy this code into your own file or add your existing code to this
file.
:param spec: ProblemSpec object
:return: List of configs forming a path through the graph from initial to goal
"""
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
# TODO: Insert your code to build the state graph here
# *** example for adding neighbors ***
# if path between n1 and n2 is collision free:
# n1.neighbors.append(n2)
# n2.neighbors.append(n1)
vertices, edges = state_graph(350, 45, 5, init_node, goal_node, spec)
# search the graph
init_container = [init_node]
# here, each key is a graph node, each value is the list of configs visited on the path to the graph node
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
# print(current.__hash__())
if test_config_equality(current.config, spec.goal, spec):
# found path to goal
# print(init_visited[current])
print("found")
# for i in init_visited[current]:
# print(str(i))
write_robot_config_list_to_file(output, primitive_step(init_visited[current]))
print("finish")
return init_visited[current]
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
if test_self_collision(suc.config, spec) \
and test_environment_bounds(suc.config) \
and test_obstacle_collision(suc.config, spec, spec.obstacles):
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def path_between_points(initial, goal, spec, number_of_iterations,a_count):
init_node = GraphNode(spec, initial)
goal_node = GraphNode(spec, goal)
GraphNodes = [] # List of all nodes created
found_result = False
for i in range(number_of_iterations):
print("iteration number: ", i)
# Create Random Samples
random_graph_nodes = randomsample([init_node, goal_node], 100, spec, GraphNodes,a_count)
# Building state graph
init_container = [init_node]
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
if test_config_equality(current.config, goal, spec):
# found path to goal
print("Finally :)")
final_config = init_visited[current]
# Build small neighbours
print_configs = []
for i in range(len(final_config) - 1):
neigh = neighbour_check_configs(final_config[i], final_config[i + 1], spec)
if neigh:
for j in neigh:
print_configs.append(j)
else:
print("Something is just not right")
print_configs.append(final_config[len(final_config) - 1])
return print_configs
# Check each successor to check if previously visited node
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
return None
def solve(spec):
"""
An example solve method containing code to perform a breadth first search of the state graph and return a list of
configs which form a path through the state graph between the initial and the goal. Note that this path will not
satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list.
If you wish to use this code, you may either copy this code into your own file or add your existing code to this
file.
:param spec: ProblemSpec object
:return: List of configs forming a path through the graph from initial to goal
"""
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
# TODO: Insert your code to build the state graph here
# *** example for adding neighbors ***
# if path between n1 and n2 is collision free:
# n1.neighbors.append(n2)
# n2.neighbors.append(n1)
# search the graph
init_container = [init_node]
# here, each key is a graph node, each value is the list of configs visited on the path to the graph node
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
if test_config_equality(current.config, spec.goal, spec):
# found path to goal
return init_visited[current]
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def __eq__(self, other):
return test_config_equality(self.config, other.config, self.spec)
def create_state_graph(spec, obstacles):
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
nodes = [init_node, goal_node]
for stage in range(spec.num_grapple_points):
for i in range(500):
sample_point = uniformly_sampling(spec, obstacles, stage)
new_node = GraphNode(spec, sample_point)
nodes.append(new_node)
for node in nodes:
if check_distance(
spec, sample_point, node.config) and check_path(
sample_point, node.config, spec, obstacles):
node.neighbors.append(new_node)
new_node.neighbors.append(node)
init_container = [init_node]
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
if test_config_equality(current.config, spec.goal, spec):
result = init_visited[current]
# return result
new_list = []
for i in range(len(result) - 1):
angles1 = result[i].ee1_angles
angles2 = result[i + 1].ee1_angles
if result[i].ee2_grappled is True:
angles1 = result[i].ee2_angles
angles2 = result[i + 1].ee2_angles
angle_distance = []
lengths1 = result[i].lengths
lengths2 = result[i + 1].lengths
length_distance = []
for j in range(len(angles1)):
angle_distance.append(angles2[j].__sub__(angles1[j]))
length_distance.append(lengths2[j] - lengths1[j])
for j in range(501):
new_angle_list = []
new_length_list = []
for k in range(len(angles1)):
if result[i].ee1_grappled is True:
new_angle_list.append(
result[i].ee1_angles[k].__add__(
angle_distance[k].__mul__(0.002 * j)))
elif result[i].ee2_grappled is True:
new_angle_list.append(
result[i].ee2_angles[k].__add__(
angle_distance[k].__mul__(0.002 * j)))
new_length_list.append(result[i].lengths[k] +
length_distance[k] * 0.002 * j)
if result[i].ee1_grappled is True:
x, y = result[i].points[0]
new_config = make_robot_config_from_ee1(
x,
y,
new_angle_list,
new_length_list,
ee1_grappled=True,
ee2_grappled=False)
if test_obstacle_collision(
new_config, spec,
obstacles) and test_self_collision(
new_config, spec
) and test_environment_bounds(new_config):
new_list.append(new_config)
elif result[i].ee2_grappled is True:
x, y = result[i].points[-1]
new_config = make_robot_config_from_ee2(
x,
y,
new_angle_list,
list(reversed(new_length_list)),
ee1_grappled=False,
ee2_grappled=True)
if test_obstacle_collision(
new_config, spec,
obstacles) and test_self_collision(
new_config, spec
) and test_environment_bounds(new_config):
new_list.append(new_config)
return new_list
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def create_state_graph(spec, obstacles, stage, sub_init=None, sub_goals=None):
if sub_init is None:
init_node = GraphNode(spec, spec.initial)
else:
init_node = GraphNode(spec, sub_init)
nodes = [init_node]
if sub_goals is None:
goal_node = GraphNode(spec, spec.goal)
nodes.append(goal_node)
else:
for sub_goal in sub_goals:
nodes.append(GraphNode(spec, sub_goal))
for i in range(2000):
sample_point = uniformly_sampling(spec, obstacles, stage)
new_node = GraphNode(spec, sample_point)
nodes.append(new_node)
for node in nodes:
if check_distance(spec, sample_point, node.config, stage):
if check_path(sample_point, node.config, spec, obstacles, stage):
node.neighbors.append(new_node)
new_node.neighbors.append(node)
init_container = [init_node]
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
find_goal = False
if sub_goals is None:
if test_config_equality(current.config, spec.goal, spec):
find_goal = True
else:
for sub_goal in sub_goals:
if test_config_equality(current.config, sub_goal, spec):
find_goal = True
if find_goal is True:
result = init_visited[current]
# return result
new_list = []
for i in range(len(result) - 1):
angles1 = result[i].ee1_angles
angles2 = result[i + 1].ee1_angles
if stage % 2 == 1:
angles1 = result[i].ee2_angles
angles2 = result[i + 1].ee2_angles
angle_distance = []
lengths1 = result[i].lengths
lengths2 = result[i + 1].lengths
length_distance = []
for j in range(len(angles1)):
angle_distance.append(angles2[j].__sub__(angles1[j]))
length_distance.append(lengths2[j] - lengths1[j])
for j in range(501):
new_angle_list = []
new_length_list = []
for k in range(len(angles1)):
if stage % 2 == 0:
new_angle_list.append(result[i].ee1_angles[k].__add__(angle_distance[k].__mul__(0.002 * j)))
elif stage % 2 == 1:
new_angle_list.append(result[i].ee2_angles[k].__add__(angle_distance[k].__mul__(0.002 * j)))
new_length_list.append(result[i].lengths[k] + length_distance[k] * 0.002 * j)
if stage % 2 == 0:
x, y = result[i].points[0]
new_config = make_robot_config_from_ee1(x, y, new_angle_list, new_length_list,
ee1_grappled=True)
new_list.append(new_config)
elif stage % 2 == 1:
x, y = result[i].points[-1]
new_config = make_robot_config_from_ee2(x, y, new_angle_list, new_length_list,
ee2_grappled=True)
new_list.append(new_config)
return new_list
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def find_graph_path(spec,
obstacles,
phase,
partial_init=None,
bridge_configs=None):
"""
This method performs a breadth first search of the state graph and return a list of configs which form a path
through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step
requirement - you will need to interpolate between the configs in the returned list.
:param spec: ProblemSpec object
:param obstacles: Obstacles object
:return: List of configs forming a path through the graph from initial to goal
for i := range[1, N] do
configs = uniformly sampling();
for config in all configs do
if distance between c1 and c2 is below certain amount and path is valid do
add a neighbor relationship between nodes;
search graph
if goal reached:
break
"""
# set init and goal nodes, put into nodes list
init_node = GraphNode(spec,
spec.initial) if partial_init is None else GraphNode(
spec, partial_init)
nodes = [init_node]
if bridge_configs is None:
goal_node = GraphNode(spec, spec.goal)
nodes.append(goal_node)
else:
for partial_goal in bridge_configs:
nodes.append(GraphNode(spec, partial_goal))
# sample configs
sample_number = 1000 if spec.num_segments <= 3 else 2000
for i in range(sample_number):
sample_point = uniformly_sampling(spec, obstacles, phase)
new_node = GraphNode(spec, sample_point)
nodes.append(new_node)
for node in nodes:
if distance_checking(spec, phase, sample_point, node.config) \
and interpolation_checking(spec, phase, sample_point, node.config, obstacles):
GraphNode.add_connection(node, new_node)
# search the graph
init_container = [init_node]
# here, each key is a graph node, each value is the list of configs visited on the path to the graph node
init_visited = {init_node: [init_node.config]}
while len(init_container) > 0:
current = init_container.pop(0)
complete_find = False
if bridge_configs is None:
if test_config_equality(current.config, spec.goal, spec):
complete_find = True
else:
for partial_goal in bridge_configs:
if test_config_equality(current.config, partial_goal, spec):
complete_find = True
if complete_find is True:
# found path to goal
config_list = init_visited[current]
path_list = []
for n in range(len(config_list) - 1):
angles_first = config_list[
n].ee1_angles if phase % 2 == 0 else config_list[
n].ee2_angles
angles_sec = config_list[
n + 1].ee1_angles if phase % 2 == 0 else config_list[
n + 1].ee2_angles
between_angle = []
between_lengths = []
lengths_first = config_list[n].lengths
lengths_sec = config_list[n + 1].lengths
for i in range(len(angles_first)):
between_angle.append(angles_sec[i].__sub__(
angles_first[i]))
between_lengths.append(lengths_sec[i] - lengths_first[i])
# interpolated the path into steps less than 0,001
for j in range(851):
valid_angle_list = []
valid_length_list = []
for k in range(len(angles_first)):
if phase % 2 == 0:
valid_angle_list.append(
config_list[n].ee1_angles[k].__add__(
between_angle[k].__mul__(1 / 850 * j)))
else:
valid_angle_list.append(
config_list[n].ee2_angles[k].__add__(
between_angle[k].__mul__(1 / 850 * j)))
valid_length_list.append(config_list[n].lengths[k] +
between_lengths[k] * 1 / 850 *
j)
if phase % 2 == 0:
x, y = config_list[n].points[0]
new_config = make_robot_config_from_ee1(
x,
y,
valid_angle_list,
valid_length_list,
ee1_grappled=True)
path_list.append(new_config)
else:
x, y = config_list[n].points[-1]
new_config = make_robot_config_from_ee2(
x,
y,
valid_angle_list,
valid_length_list,
ee2_grappled=True)
path_list.append(new_config)
return path_list
successors = current.get_successors()
for suc in successors:
if suc not in init_visited:
init_container.append(suc)
init_visited[suc] = init_visited[current] + [suc.config]
def interpolate_path(RobotConfig1, RobotConfig2):
paths = [RobotConfig1]
lengths1 = RobotConfig1.lengths
lengths2 = RobotConfig2.lengths
kLengths = []
for i in range(len(lengths1)):
differences = lengths2[i] - lengths1[i]
if differences >= 0:
kLengths.append(math.floor(differences / 0.001))
else:
kLengths.append(math.ceil(differences / 0.001))
angles1 = RobotConfig1.ee1_angles
angles2 = RobotConfig2.ee1_angles
kAngles = []
for i in range(len(angles1)):
differences = angles2[i].radians - angles1[i].radians
if differences >= 0:
kAngles.append(math.floor(differences / 0.001))
else:
kAngles.append(math.ceil(differences / 0.001))
if RobotConfig1.ee1_grappled:
ee2Grappled = True if RobotConfig1.ee2_grappled else False
ee1Grappled = True
grappledX, grappledY = RobotConfig1.get_ee1()
else:
ee2Grappled = True
ee1Grappled = False
grappledX, grappledY = RobotConfig1.get_ee2()
# for each config,
newLengths = []
newAngles = []
counter = 0
# getConfig2 = False
while True:
if counter < 2:
counter += 1
for i in range(len(lengths1)):
if kLengths[i] > 0:
if counter == 1:
newLengths.append(lengths1[i] + 0.001)
else:
newLengths[i] += 0.001
kLengths[i] -= 1
elif kLengths[i] < 0:
if counter == 1:
newLengths.append(lengths1[i] - 0.001)
else:
newLengths[i] -= 0.001
kLengths[i] += 1
else:
if counter != 1:
if lengths2[i] - newLengths[i] > 0:
newLengths[i] += (lengths2[i] - newLengths[i])
elif lengths2[i] - newLengths[i] < 0:
newLengths[i] -= (lengths1[i] - lengths2[i])
else:
newLengths.append(lengths1[i])
if kAngles[i] > 0:
if counter == 1:
newAngles.append(angles1[i] + 0.001)
else:
newAngles[i] += 0.001
kAngles[i] -= 1
print(newAngles[i])
print(angles2[i])
elif kAngles[i] < 0:
if counter == 1:
newAngles.append(angles1[i] - 0.001)
else:
newAngles[i] -= 0.001
kAngles[i] += 1
else:
if counter != 1:
# if i == 0:
# print("This")
if angles2[i] > newAngles[i]:
newAngles[i] += (angles2[i] - newAngles[i])
elif angles2[i] < newAngles[i]:
newAngles[i] -= (newAngles[i] - angles2[i])
else:
newAngles.append(angles1[i])
if i == len(lengths1) - 1:
newConfig = make_robot_config_from_ee1(x=grappledX, y=grappledY,
angles=newAngles.copy(), lengths=newLengths.copy(),
ee1_grappled=ee1Grappled, ee2_grappled=ee2Grappled)
paths.append(newConfig)
if paths[-1]:
if test_config_equality(paths[-1], RobotConfig2, spec):
break
return paths
