def main(arglist):
#input_file = arglist[0]
#output_file = arglist[1]
#input_file = r'C:\Users\fyc19\Desktop\ai\assignment-2-support-code-master\assignment-2-support-code-master\testcases\3g1_m0.txt'
# 'C:/Users/fyc19/Desktop/ai/assignment-2-support-code-master/assignment-2-support-code-master/testcase/3g1_m0.txt'
# r'C:\Users\fyc19\Desktop\ai\assignment-2-support-code-master\assignment-2-support-code-master\testcase\3g1_m0.txt'
input_file = './testcases/3g1_m0.txt'
output_file = 'asdasdas'
spec = ProblemSpec(input_file)
n = 0
i = 0
j = 0
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
sampling = sample(spec, segements=spec.num_segments)
sampling = [init_node] + sampling + [goal_node]
for i in range(0, len(sampling)):
for j in range(i + 1, len(sampling)):
node_distance = config_distance_graphnode(sampling[i], sampling[j],
spec)
# print(node_distance)
if node_distance > spec.PRIMITIVE_STEP:
interpolation_check = interpolation(sampling[i].config,
sampling[j].config,
spec,
rate=20)
for n in range(len(interpolation_check)):
obstacle_collision = test_obstacle_collision(
interpolation_check[n], spec, spec.obstacles)
if obstacle_collision == False:
break
if obstacle_collision:
sampling[i].add_connection(sampling[i], sampling[j])
j += 1
i += 1
path = find_graph_path(spec, init_node)
steps = []
if path == None:
print('None')
return
steps.append(path[0])
for i in range(1, len(path)):
steps += interpolation(path[i - 1], path[i], spec)
steps += [path[i]]
#
#
# Code for your main method can go here.
#
# Your code should find a sequence of RobotConfig objects such that all configurations are collision free, the
# distance between 2 successive configurations is less than 1 primitive step, the first configuration is the initial
# state and the last configuration is the goal state.
#
#
write_robot_config_list_to_file(output_file, steps)
def main(arglist):
if len(arglist) == 0 or len(arglist) > 2:
print(
"Running this file launches a graphical program for visualising maps and solutions."
)
print("Usage: visualiser.py [input_file] [solution_file(optional)]")
return
spec = ProblemSpec(arglist[0])
if len(arglist) == 2:
soln = load_output(arglist[1])
else:
soln = []
vis = Visualiser(spec, soln)
def main(arglist):
input_file = arglist[0]
output_file = arglist[1]
spec = ProblemSpec(input_file)
obstacles = __get_lenient_obstacles(spec)
steps = []
# single grapple point no need bridge config
if spec.num_grapple_points == 1:
steps += find_graph_path(spec, obstacles, 0)
else: # for grapple points at least two
for phase in range(spec.num_grapple_points):
if phase == 0:
bridge_configs = generate_bridge_configs(
spec, obstacles, phase)
partial_steps_1 = find_graph_path(
spec, obstacles, phase, bridge_configs=bridge_configs)
if partial_steps_1 is None:
print("no bridge_configs in stage1")
else:
steps += partial_steps_1
elif 0 < phase < spec.num_grapple_points - 1:
bridge_configs = generate_bridge_configs(
spec, obstacles, phase)
partial_steps_2 = find_graph_path(
spec,
obstacles,
phase,
partial_init=steps[-1],
bridge_configs=bridge_configs)
if partial_steps_2 is None:
print("no bridge_configs in stage2")
else:
steps += partial_steps_2
else:
partial_steps_3 = find_graph_path(spec,
obstacles,
phase,
partial_init=steps[-1])
if partial_steps_3 is None:
print("no bridge_configs in stage3")
else:
steps += partial_steps_3
if len(arglist) > 1:
print("steps:", len(steps))
write_robot_config_list_to_file(output_file, steps)
def main():
input_file = "testcases/3g2_m1.txt"
output_file = "out.txt"
#input_file = "testcases/3g1_m2.txt"
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
steps = []
#
#
# Code for your main method can go here.
#
# Your code should find a sequence of RobotConfig objects such that all configurations are collision free, the
# distance between 2 successive configurations is less than 1 primitive step, the first configuration is the initial
# state and the last configuration is the goal state.
samples = sample(spec)
nodes = []
for s in samples:
nodes.append(GraphNode(spec, s))
nodes.append(init_node)
nodes.append(goal_node)
connect(nodes, spec)
steps = []
if find_graph_path(spec, init_node) is not None:
current = goal_node
while (current.parent is not None):
steps = current.parent.p_steps[current] + steps
current = current.parent
else:
print("Failed")
#
#
write_robot_config_list_to_file(output_file, steps)
def main(arglist):
input_file = arglist[0]
output_file = arglist[1]
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
steps = []
#
#
# Code for your main method can go here.
#
# Your code should find a sequence of RobotConfig objects such that all configurations are collision free, the
# distance between 2 successive configurations is less than 1 primitive step, the first configuration is the initial
# state and the last configuration is the goal state.
#
#
if len(arglist) > 1:
write_robot_config_list_to_file(output_file, steps)
def main(arglist):
# input_file = arglist[0]
# output_file = arglist[1]
input_file = "testcases/3g1_m2.txt"
output_file = "testcases/output.txt"
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
g = GraphNode(spec, spec.goal)
# path_plan = []
#
# for i in range(200):
# c = g.generate_sample()
# path_plan.append(c)
steps = []
path_plan = g.PRM(init_node, goal_node)
# write_robot_config_list_to_file(output_file, path_plan)
# Code for your main method can go here.
#
# Your code should find a sequence of RobotConfig objects such that all configurations are collision free, the
# distance between 2 successive configurations is less than 1 primitive step, the first configuration is the initial
# state and the last configuration is the goal state.
#
#
# if len(arglist) > 1:
#
# You may uncomment this line to launch visualiser once a solution has been found. This may be useful for debugging.
# *** Make sure this line is commented out when you submit to Gradescope ***
#
v = Visualiser(spec, path_plan)
def main(arglist):
input_file = arglist[0]
output_file = arglist[1]
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
nodes = [init_node, goal_node]
steps = []
timeout = time.time() + 45
while True:
robot_config = sample_config(spec)
if robot_config is None:
continue
current_node = GraphNode(spec, robot_config)
# Check for collisions between current_node and other nodes
collision_check(current_node, nodes, spec)
nodes.append(current_node)
route = find_graph_path(spec, init_node)
if route or time.time() > timeout:
# Start at init and get to end appending to list of steps
break
for i in range(len(route) - 1):
steps.extend(interpolate_path(route[i], route[i + 1], True, spec))
steps.append(goal_node.config)
if len(arglist) > 1:
write_robot_config_list_to_file(output_file, steps)
def main(arglist):
input_file = arglist[0]
output_file = arglist[1]
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
num_grapple_points = spec.num_grapple_points
if num_grapple_points == 1:
graph_nodes = [[init_node, goal_node]]
else:
graph_nodes = [[] for i in range(num_grapple_points)]
graph_nodes[0].append(init_node)
graph_nodes[-1].append(goal_node)
steps = []
# Solution
# Note on solution: if there are multiple grapple points, samples are collected separately for each grapple point
# Then, the closest node to a grapple point in the sample for an ADJACENT grapple point is taken as the goal node.
#
steps = generate_steps(spec, 5000 * spec.num_grapple_points, graph_nodes,
init_node, goal_node, spec.num_grapple_points)
if len(arglist) > 1:
write_robot_config_list_to_file(output_file, steps)
#
# You may uncomment this line to launch visualiser once a solution has been found. This may be useful for debugging.
# *** Make sure this line is commented out when you submit to Gradescope ***
#
v = Visualiser(spec, steps)
def main(arglist):
sys.setrecursionlimit(5000)
# print(sys.getrecursionlimit())
# t_start = time.time()
input_file = arglist[0]
output_file = arglist[1]
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
steps = []
#
#
# Code for your main method can go here.
#
# Your code should find a sequence of RobotConfig objects such that all configurations are collision free, the
# distance between 2 successive configurations is less than 1 primitive step, the first configuration is the initial
# state and the last configuration is the goal state.
#
#
sample_list = [init_node, goal_node]
grapple_points = spec.grapple_points[:]
num_grapple_points = spec.num_grapple_points
if spec.num_grapple_points > 1:
if (spec.num_grapple_points % 2
== 0) and (spec.goal.ee1_grappled is True):
spec.num_grapple_points -= 1
spec.grapple_points.pop(2)
for i in range(spec.num_grapple_points - 1):
for _ in range(NUM_BRIDGES):
sample_list.append(bridge_config(spec, spec.initial, i))
# print(sample_list)
while True:
temp_list = []
while len(temp_list) < SAMPLE_SIZE:
for i in range(spec.num_grapple_points):
sample = generate_sample(spec, spec.initial, i)
if (sample not in sample_list) and (sample not in temp_list):
temp_list.append(sample)
else:
continue
sample_list += temp_list
for i in range(len(sample_list) - 1):
for j in range(i + 1, len(sample_list)):
xi1, yi1 = sample_list[i].config.points[0]
xj1, yj1 = sample_list[j].config.points[0]
xi2, yi2 = sample_list[i].config.points[-1]
xj2, yj2 = sample_list[j].config.points[-1]
if point_is_close(xi1, yi1, xj1, yj1,
spec.TOLERANCE) or point_is_close(
xi2, yi2, xj2, yj2, spec.TOLERANCE):
if (dist(sample_list[i].config, sample_list[j].config, spec) <= DIST_THRESHOLD) and \
(sample_list[j] not in sample_list[i].neighbors):
if interpolation_path(sample_list[i].config,
sample_list[j].config, spec):
sample_list[i].add_connection(
sample_list[i], sample_list[j])
config_list = find_graph_path(spec, init_node)
if config_list is not None:
for i in range(len(config_list) - 1):
path = interpolation_path(config_list[i], config_list[i + 1],
spec)
if not path:
continue
else:
steps += path
break
spec.grapple_points = grapple_points[:]
spec.num_grapple_points = num_grapple_points
if len(arglist) > 1:
write_robot_config_list_to_file(output_file, steps)
def main(arglist):
input_file = arglist[0]
soln_file = arglist[1]
spec = ProblemSpec(input_file)
robot_configs = load_output(soln_file)
lenient_obstacles = __get_lenient_obstacles(spec)
violations = 0
# run validity tests for each config
if not test_config_equality(robot_configs[0], spec.initial, spec):
violations += 1
if violations <= 10:
print(
"!!! The first robot configuration does not match the initial position from the problem spec !!!"
)
for i in range(len(robot_configs)):
if not test_environment_bounds(robot_configs[i]):
violations += 1
if violations <= 10:
print(
"!!! Robot goes outside the bounds of the environment at step number "
+ str(i) + " !!!")
if not test_angle_constraints(robot_configs[i], spec):
violations += 1
if violations <= 10:
print(
"!!! One or more of the angles between robot segments is tighter than allowed at step number "
+ str(i) + " !!!")
if not test_length_constraints(robot_configs[i], spec):
violations += 1
if violations <= 10:
print(
"!!! One or more of the robot segments is shorter or longer than allowed at step number "
+ str(i) + " !!!")
if not test_grapple_point_constraint(robot_configs[i], spec):
violations += 1
if violations <= 10:
print(
"!!! Robot is not connected to at least 1 grapple point (or a grapple point is occupied by "
+ "more than one end effector) at step number " + str(i) +
" !!!")
if not test_self_collision(robot_configs[i], spec):
violations += 1
if violations <= 10:
print("!!! Robot is in collision with itself at step number " +
str(i) + " !!!")
if not test_obstacle_collision(robot_configs[i], spec,
lenient_obstacles):
violations += 1
if violations <= 10:
print(
"!!! Robot is in collision with an obstacle at step number "
+ str(i) + " !!!")
if i + 1 < len(robot_configs) and not test_config_distance(
robot_configs[i], robot_configs[i + 1], spec):
violations += 1
if violations <= 10:
print(
"!!! Step size is greater than primitive step limit between step number "
+ str(i) + " and " + str(i + 1) + " !!!")
if not test_config_equality(robot_configs[-1], spec.goal, spec):
violations += 1
if violations <= 10:
print(
"!!! The last robot configuration does not match the goal position from the problem spec !!!"
)
# print summary
if violations > 10:
print("!!! " + str(violations - 10) + " other rule violation(s) !!!")
if violations == 0:
print("Testcase solved successfully!")
return 0
else:
print("Invalid solution file - " + str(violations) +
" violations encountered.")
return 1
def main(arglist):
#def main():
input_file = arglist[0]
output_file = arglist[1]
#input_file = 'testcases/3g2_m1.txt'
#output_file = 'new_output.txt'
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
# Keep track of all samples generated
nodes_in_graph = []
nodes_in_graph.append(init_node)
nodes_in_graph.append(goal_node)
#bridge = []
#new_bridge = create_bridge_config(spec)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#bridge.append(new_bridge)
#write_robot_config_list_to_file(output_file, bridge)
#exit()
search_remaining = True
#connections_added = 0
while search_remaining:
# Generate X new Samples
for i in range(10):
sample = generate_sample(spec)
sample_node = GraphNode(spec, sample)
# Connect sample to all samples if d < 0.2 and interpolated_path != None
for node in nodes_in_graph:
distance_bw_nodes = abs(sample.points[-1][1] -
node.config.points[-1][1]) + abs(
sample.points[-1][0] -
node.config.points[-1][0])
#print(distance_bw_nodes)
if distance_bw_nodes < 0.2:
# Lazy Check
mid_angles = []
mid_angles.extend(node.config.ee1_angles)
for angle in range(len(sample.ee1_angles)):
mid_angles[angle] = (
mid_angles[angle].in_degrees() +
sample.ee1_angles[angle].in_degrees()) / 2
mid_lengths = []
mid_lengths.extend(node.config.lengths)
for length in range(len(sample.lengths)):
mid_lengths[length] = (mid_lengths[length] +
sample.lengths[length]) / 2
mid_new_config = make_robot_config_from_ee1(
sample.points[0][0], sample.points[0][1], mid_angles,
mid_lengths, True)
if individual_config_collision_checking(
spec,
mid_new_config) == False: # False shouldn't work
GraphNode.add_connection(node, sample_node)
#connections_added = connections_added + 1
# Full Check
#interpolated_path = interpolate_path(node.config, sample_node.config, spec)
#if interpolated_path != None:
#GraphNode.add_connection(node, sample_node)
#connections_added = connections_added + 1
nodes_in_graph.append(sample_node)
#print(connections_added)
# Search Graph
path_found = find_graph_path(spec, init_node)
if path_found != None:
final_path = []
searched_paths = 0
for path_no in range(len(path_found) - 1):
path_yay = interpolate_path(path_found[path_no],
path_found[path_no + 1], spec)
if path_yay == None:
print("Some Error")
for node in nodes_in_graph:
if node == GraphNode(spec, path_found[path_no]):
first_node = node
elif node == GraphNode(spec, path_found[path_no + 1]):
second_node = node
first_node.neighbors.remove(second_node)
second_node.neighbors.remove(first_node)
break
else:
print("yay")
final_path.extend(path_yay)
# Break when goal reached
searched_paths += 1
#search_completed = False
if searched_paths == len(path_found) - 1:
write_robot_config_list_to_file(output_file, final_path)
search_remaining = False
else:
print("no path found")
#GraphNode.add_connection(init_node, goal_node)
#interpolate_path(spec.initial, spec.goal)
steps = []
def main(arglist):
input_file = arglist[0]
output_file = arglist[1]
spec = ProblemSpec(input_file)
init_node = GraphNode(spec, spec.initial)
goal_node = GraphNode(spec, spec.goal)
steps = []
#
#
# Code for your main method can go here.
#
# Your code should find a sequence of RobotConfig objects such that all configurations are collision free, the
# distance between 2 successive configurations is less than 1 primitive step, the first configuration is the initial
# state and the last configuration is the goal state.
#
#
def get_config_distance(c1, c2, spec):
max_ee1_delta = 0
max_ee2_delta = 0
for i in range(spec.num_segments):
if abs((c2.ee1_angles[i] - c1.ee1_angles[i]).in_radians()) > max_ee1_delta:
max_ee1_delta = abs((c2.ee1_angles[i] - c1.ee1_angles[i]).in_radians())
if abs((c2.ee2_angles[i] - c1.ee2_angles[i]).in_radians()) > max_ee2_delta:
max_ee2_delta = abs((c2.ee2_angles[i] - c1.ee2_angles[i]).in_radians())
# measure leniently - allow compliance from EE1 or EE2
max_delta = min(max_ee1_delta, max_ee2_delta)
for i in range(spec.num_segments):
if abs(c2.lengths[i] - c1.lengths[i]) > max_delta:
max_delta = abs(c2.lengths[i] - c1.lengths[i])
return max_delta
def rand_angle():
minKAngle = -2879
maxKAngle = 2879
anglesRand = Angle(radians=0.001) * random.randint(minKAngle, maxKAngle)
return anglesRand
def rand_length(spec, initLength):
minKLength = 0
maxKLength = (spec.max_lengths[0] - spec.min_lengths[0]) / 0.001
maxKLength = float(f'{maxKLength:.3f}')
if not maxKLength == 0:
return float(f'{initLength + random.randint(minKLength, maxKLength) * 1e-3:.3f}')
else:
return initLength
def generate_sample():
initialConfig = spec.initial
# if spec.initial.ee1_grappled:
# initialAngles = initialConfig.ee1_angles
# else:
# initialAngles = initialConfig.ee2_angles
anglesRand = []
for i in range(spec.num_segments):
anglesRand.append(rand_angle())
# print([angle.in_degrees() for angle in anglesRand])
lengthsRand = [rand_length(spec, length) for length in spec.min_lengths]
randomIndexGrapplePoint = random.randint(0, len(spec.grapple_points) - 1)
grappleXRand, grappleYRand = spec.grapple_points[randomIndexGrapplePoint]
# check number of grapple
numGrapplePoints = spec.num_grapple_points
if numGrapplePoints == 1:
ee1_grappled = True if spec.initial.ee1_grappled else False
ee2_grappled = not ee1_grappled
else:
# Careful
ee1_grappled = bool(random.getrandbits(1))
ee2_grappled = bool(random.getrandbits(1))
if ee1_grappled:
robotConfig = make_robot_config_from_ee1(grappleXRand, grappleYRand, anglesRand, lengthsRand,
ee1_grappled=ee1_grappled, ee2_grappled=ee2_grappled)
else:
robotConfig = make_robot_config_from_ee2(grappleXRand, grappleYRand, anglesRand, lengthsRand,
ee1_grappled=ee1_grappled, ee2_grappled=ee2_grappled)
if test_obstacle_collision(robotConfig, spec, spec.obstacles) and test_self_collision(robotConfig, spec):
return robotConfig
return generate_sample()
def get_current_grapple_point(config):
if config.ee1_grappled:
return config.get_ee1()
return config.get_ee2()
def get_nearest_grapple_point(newConfig, lastX, lastY):
nearestGrappleDistance = float('inf')
nearestGrapplePoint = None
for grapplePoint in spec.grapple_points:
if grapplePoint == get_current_grapple_point(newConfig):
continue
grappleX, grappleY = grapplePoint
deltaX = abs(lastX - grappleX)
deltaY = abs(lastY - grappleY)
if deltaX + deltaY < nearestGrappleDistance:
nearestGrappleDistance = deltaX + deltaY
nearestGrapplePoint = grapplePoint
delta = (deltaX, deltaY)
return nearestGrapplePoint
def get_perpendicular_intersect(A, B, C):
x1, y1 = A
x2, y2 = B
x3, y3 = C
px = x2 - x1
py = y2 - y1
dAB = px * px + py * py
u = ((x3 - x1) * px + (y3 - y1) * py) / dAB
x = x1 + u * px
y = y1 + u * py;
return x, y
def get_bridge_config(ee1_grappled, ee2_grappled, grappledX, grappledY):
# randomly sample n-1 links
while True:
anglesRand = []
lengthsRand = []
for i in range(spec.num_segments - 1):
anglesRand.append(rand_angle())
lengthsRand.append(rand_length(spec, spec.min_lengths[i]))
# Connect last link to the endpoint
# Get coords of the second last joint
if ee1_grappled:
newConfig = make_robot_config_from_ee1(grappledX, grappledY, anglesRand, lengthsRand,
ee1_grappled, ee2_grappled)
else:
newConfig = make_robot_config_from_ee2(grappledX, grappledY, anglesRand, lengthsRand,
ee1_grappled, ee2_grappled)
# if not test_obstacle_collision(newConfig, spec, spec.obstacles) or not test_self_collision(newConfig, spec):
# continue
lastX, lastY = newConfig.points[-1]
grapplePoint = get_nearest_grapple_point(newConfig, lastX, lastY)
# xD, yD = get_perpendicular_intersect(newConfig.points[-1], newConfig.points[-2], grapplePoint)
# deltaY = math.sqrt((grapplePoint[0] - xD)**2 + (grapplePoint[1] - yD)**2)
# deltaX = math.sqrt((grapplePoint[0] - lastX) ** 2 + (grapplePoint[1] - lastY) ** 2)
# angleLastLink = - Angle.atan(deltaY / deltaX)
# lengthLastLink = math.sqrt(deltaX**2 + deltaY**2)
gX, gY = grapplePoint
deltaY = gY - lastY
deltaX = gX - lastX
angleLastLink = Angle.atan2(deltaY, deltaX)
lengthLastLink = math.sqrt(deltaY**2 + deltaX**2)
if not ((-11 * math.pi / 12) - spec.TOLERANCE < angleLastLink < (11 * math.pi / 12) + spec.TOLERANCE) \
or lengthLastLink < spec.min_lengths[-1] - spec.TOLERANCE \
or lengthLastLink > spec.max_lengths[-1] + spec.TOLERANCE:
continue
anglesRand.append(angleLastLink)
lengthsRand.append(lengthLastLink)
if ee1_grappled:
finalConfig = make_robot_config_from_ee1(grappledX, grappledY, anglesRand, lengthsRand,
ee1_grappled=ee1_grappled, ee2_grappled=True)
else:
finalConfig = make_robot_config_from_ee2(grappledX, grappledY, anglesRand, lengthsRand,
ee1_grappled=True, ee2_grappled=ee2_grappled)
gX, gY = finalConfig.points[-1]
grappledX, grappledY = grapplePoint
if not point_is_close(gX, gY, grappledX, grappledY, spec.TOLERANCE):
continue
if test_obstacle_collision(finalConfig, spec, spec.obstacles) and test_self_collision(finalConfig, spec):
return finalConfig
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
def check_path_collision(RobotConfig1, RobotConfig2):
"""
return: true if pass, false otherwise
"""
configList = interpolate_path(RobotConfig1, RobotConfig2)
for config in configList:
if not test_obstacle_collision(config, spec, spec.obstacles):
return False
return True
def connect_node(Node1, Node2):
if check_path_collision(Node1.config, Node2.config):
GraphNode.add_connection(Node1, Node2)
def build_graph():
graph = []
paths = []
# graph.add(init_node)
# graph.add(goal_node)
minInitDistance = float('inf')
minGoalDistance = float('inf')
while True:
numberOfSamples = 10
for i in range(numberOfSamples):
randomConfig = generate_sample()
# initDistance = get_config_distance(init_node.config, randomConfig, spec)
# if initDistance < minInitDistance:
# minInitDistance = initDistance
# configNearInit = randomConfig
#
# goalDistance = get_config_distance(goal_node.config, randomConfig, spec)
# if goalDistance < minGoalDistance:
# minGoalDistance = goalDistance
# configNearGoal = randomConfig
newNode = GraphNode(spec, randomConfig)
if graph:
for node in graph:
# Should bias the node with low distance
connect_node(newNode, node)
# careful
connect_node(newNode, goal_node)
connect_node(newNode, init_node)
graph.append(newNode)
else:
graph.append(newNode)
# connect init and goal to the graph
# nodeNearInit = GraphNode(spec, configNearInit)
# nodeNearGoal = GraphNode(spec, configNearGoal)
# connect_node(init_node, nodeNearInit)
# connect_node(goal_node, nodeNearGoal)
initPaths = find_graph_path(spec, init_node)
# if initPaths:
# return initPaths
# else:
# continue
foundGoal = True
if initPaths:
for i in range(len(initPaths) - 1):
if check_path_collision(initPaths[i], initPaths[i + 1]):
paths += (interpolate_path(initPaths[i], initPaths[i + 1]))
else:
foundGoal = False
paths = []
break
if foundGoal:
return paths
if len(arglist) > 1:
counter = 0
while counter < 10:
counter += 1
print(counter)
grappleX, grappleY = spec.initial.get_ee1()
config = get_bridge_config(spec.initial.ee1_grappled, spec.initial.ee2_grappled, grappleX, grappleY)
steps.append(config)
# steps = build_graph()
# grappleX, grappleY = spec.initial.get_ee1()
# steps = get_bridge_config(spec.initial.ee1_grappled, spec.initial.ee2_grappled, grappleX, grappleY)
# print("Bridge", bridge)
# steps = interpolate_path(spec.initial, )
write_robot_config_list_to_file(output_file, steps)
#
# You may uncomment this line to launch visualiser once a solution has been found. This may be useful for debugging.
# *** Make sure this line is commented out when you submit to Gradescope ***
#
v = Visualiser(spec, steps)