def avg_distance_traveled_test(paths):
total_dist = 0
for path in paths:
path_dist = 0
for i in range(1, len(path)):
path_dist += line.distance(path[i].end_effector_pos,
path[i - 1].end_effector_pos)
total_dist += path_dist
return total_dist / len(paths)
def find_path(target_end_point,
start_angles,
obs,
n_iter=150,
radius=.01,
step_size=.1):
"""Executes the Optimistic Predictive Cost algorithm to find a collision-free path between two configurations.
Arguments:
end_node: A Node representing the target arm configuration.
start_angles: A float array representing the initial angles of the arm.
obs: An array of float arrays representing obstacles.
n_iter: The maximum amount of iterations to find a path.
radius: The maximum distance from the target end effector position needed to converge.
step_size: The distance, in radians, between each edge in the graph.
Returns:
A graph g containing a collision-free path if success = True, or no path if success = False.
"""
g = Graph(start_angles, None, target_end_point)
g.ranking.append(g.start_node)
g.end_node = Node.from_point(target_end_point)
close_to_end = False
for i in range(n_iter):
try:
best_node = g.ranking.pop(0)
except IndexError:
global stack_empty_error
stack_empty_error += 1
return g
if arm_is_colliding_prisms(best_node, obs):
continue
dist_to_goal = line.distance(best_node.end_effector_pos,
target_end_point)
if dist_to_goal < radius:
g.end_node = best_node
g.success = True
return g
# if dist_to_goal < 10 * radius and not close_to_end:
# step_size /= 10
# close_to_end = True
expand(best_node, 4, step_size, g)
return g
def generate_linear_path(start_angles, end_angles, num_iter):
"""Generates a linear path of arm configurations that moves an arm configuration from start_angles to end_angles.
Args:
start_angles: The initial angles of the arm.
end_angles: The desired angles of the arm.
num_iter: The number of angle configurations to be generated on the path in between the start and end positions.
Returns:
a path of nodes from start_pos to end_pos, with [num_iter] iterations of equal length.
"""
step_sizes = compute_step_sizes(start_angles, end_angles, num_iter)
g = Graph(start_angles, end_angles)
current_angles = start_angles
current_idx = 0
current_node = g.nodes[0]
for i in range(num_iter):
new_angles = np.add(current_angles, step_sizes)
new_angles = np.mod(new_angles, math.pi * 2)
new_node = Node(new_angles)
new_idx = g.add_vex(new_node, current_node)
dist = line.distance(new_node.end_effector_pos,
current_node.end_effector_pos)
current_angles = new_angles
current_idx = new_idx
current_node = new_node
if i == num_iter - 1:
rounded_current = np.around(current_angles, decimals=4)
rounded_end = np.around(end_angles, decimals=4)
if not np.array_equal(rounded_current, rounded_end):
return g, False
return g, True
def add_vex(self, node, parent):
try:
idx = self.node_to_index[node]
except KeyError:
parent_idx = self.node_to_index[parent]
idx = len(self.nodes)
dist = parent.optimistic_cost + Node.distance(parent, node)
node.optimistic_cost = dist
self.neighbors[idx] = []
self.add_edge(idx, parent_idx, Node.distance(parent, node))
self.nodes.append(node)
self.node_to_index[node] = idx
self.ranking.append(node)
self.ranking.sort(key=lambda n: dist + line.distance(
n.end_effector_pos, self.target_end_pos))
self.end_effectors = np.vstack(
[self.end_effectors, node.end_effector_pos])
return idx
def distance(cls, node1, node2):
return line.distance(node1.end_effector_pos, node2.end_effector_pos)
def rrt(start_angles,
end_angles,
obstacles,
n_iter=300,
radius=0.02,
angle_threshold=1,
stepSize=.05,
heuristic_threshold=10):
"""Uses the RRT algorithm to determine a collision-free path from start_angles to end_angles.
Args:
start_angles: An array of length 5 representing the initial angle configuration of the arm.
end_angles: An array of length 5 representing the desired angle configuration.
obstacles: An array of float arrays representing cube obstacles.
n_iter: Maximum number of iterations to find a path.
radius: Maximum distance between the final end effector position and the second-to-last position in a path.
angle_threshold: Maximum distance between the final angles and the second-to-last angles in a path.
stepSize: Distance between nodes in the graph.
heuristic_threshold: Maximum number of times a new node can fail to expand from any given node in the graph.
Returns:
An instance of rrtgraph.Graph containing a list of instances of Node, and a path of Node instances between
the start and end nodes, if successful.
A boolean indicator representing whether a path was found.
"""
G = Graph(start_angles, end_angles)
for i in range(n_iter):
rand_node = Node(None)
if arm_is_colliding_prisms(rand_node, obstacles):
continue
if i % 2 == 0 or not G.ranking:
nearest_node, nearest_node_index = nearest(
G, rand_node.end_effector_pos)
if nearest_node is None:
continue
new_node = steer(rand_node.angles, nearest_node.angles, stepSize)
if arm_is_colliding_prisms(
new_node, obstacles) or not new_node.valid_configuration():
continue
nearest_to_new, _ = nearest(G, new_node.end_effector_pos)
G.add_vex(new_node, nearest_to_new)
# dist = line.distance(new_node.end_effector_pos, nearest_to_new.end_effector_pos)
# G.add_edge(newidx, nearest_to_new_idx, dist)
else:
new_node, newidx = extend_heuristic(G, rand_node, stepSize,
heuristic_threshold, obstacles)
if arm_is_colliding_prisms(new_node, obstacles):
continue
end_eff_dist_to_goal = line.distance(new_node.end_effector_pos,
G.end_node.end_effector_pos)
angle_dist_to_goal = np.linalg.norm(
true_angle_distances_arm(new_node.angles, G.end_node.angles))
if end_eff_dist_to_goal < radius and not G.success:
if arm_is_colliding_prisms(G.end_node, obstacles):
raise Exception("Adding a colliding node")
desired_position = G.end_node.end_effector_pos
if angle_dist_to_goal > angle_threshold:
# tries to get a closer arm configuration to the second-to-last arm configration with inverse kinematics
G.end_node = Node.from_point(desired_position,
start_config=new_node.angles)
endidx = G.add_vex(G.end_node, new_node)
# G.add_edge(newidx, endidx, end_eff_dist_to_goal)
G.success = True
print("Iterations:", i)
break
return G
def dist_to_end(self, node):
return line.distance(node.end_effector_pos,
self.end_node.end_effector_pos)