def plot_path(start_angles, end_angles, iterations, obstacles):
"""Plots a path between two given angle configurations."""
start_node = Node(start_angles)
end_node = Node(end_angles)
if (not valid_path_configuration(start_node, obstacles)) or (
not valid_path_configuration(end_node, obstacles)):
raise Exception("Invalid configuration")
pos = end_node.end_effector_pos
path, _ = linear_rrt_to_point(start_node.angles, pos[0], pos[1], pos[2],
obstacles, iterations)
g = Graph(start_node.angles, end_node.angles)
if path is not None:
g.add_vex(path[0], g.start_node)
for i in range(len(path)):
try:
g.add_vex(path[i], path[i - 1])
except IndexError:
pass
arm_plot.plot_3d(g, path, obstacles)
else:
print("No Path Found :(")
def linear_rrt_to_point(start_angles,
end_x,
end_y,
end_z,
obstacles,
num_iter=15):
"""Generates a linear path to a desired end effector position, and maneuvers around obstacles with RRT if necessary.
Args:
start_angles: The initial angles of the arm.
end_x, end_y, end_z: The desired end effector position of the arm.
num_iter: The number of angle configurations to be generated on the path in between the start and end positions.
obstacles: An array of float arrays representing cube obstacles.
degrees: Whether to convert to and from degrees for motor output.
Returns:
An array of Node instances or float arrays representing a valid path between the start and end configurations
"""
end_angles = Node.from_point((end_x, end_y, end_z)).angles
g = generate_linear_path(start_angles, end_angles, num_iter)
linear_path = g[0].nodes
if path_is_colliding(linear_path, obstacles):
print("Finding OPC path")
g = opc.find_path((end_x, end_y, end_z), start_angles, obstacles)
if g.success:
linear_path = rrt.dijkstra(g)
else:
linear_path = None
if linear_path is None:
return linear_path, False
return linear_path, True
def compute_step_sizes(start_angles, end_angles, num_iter):
"""Computes each arm angle's step size based on how long it needs to travel to go from the start to end pose.
Depending on whether the anticipated path goes through the no-go zone, switches that path to go the other way
around.
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.
arm: An instance of precision_arm.PrecisionArm, containing the bounds of each arm angle.
Returns:
A array containing the step sizes of each angle of the arm.
"""
# Strategy for bounding angles: split into two quadrants: (1) less than the first bound,
# (2) greater than the second bound.
# If both are in the same quadrant, use the closest angle distance as usual.
# If start_angles[i] in (1) and end_angles[i] in (2) and angle distance > 0, we must go the other way.
# If start_angles[i] in (2) and end_angles[i] in (1) and angle distance < 0, we must go the other way.
step_sizes = []
true_angle_distances = true_angle_distances_arm(start_angles, end_angles)
bounds = Node(None).bounds
for i in range(len(start_angles)):
if i >= len(bounds) or bounds[i] is None:
angle_distance = end_angles[i] - start_angles[i]
step_size = angle_distance / num_iter
if angle_distance > math.pi:
new_angle_dist = (2 * math.pi - end_angles[i] +
start_angles[i])
step_size = -new_angle_dist / num_iter
elif angle_distance < -math.pi:
new_angle_dist = (2 * math.pi - start_angles[i] +
end_angles[i])
step_size = new_angle_dist / num_iter
else:
bound_1 = bounds[i][0]
bound_2 = bounds[i][1]
th_1 = start_angles[i]
th_2 = end_angles[i]
angle_dist = true_angle_distances[i]
if th_1 < bound_1 and th_2 > bound_2:
if true_angle_distances[i] > 0:
angle_dist = -(2 * math.pi - true_angle_distances[i])
if th_2 < bound_1 and th_1 > bound_2:
if true_angle_distances[i] < 0:
angle_dist = 2 * math.pi + true_angle_distances[i]
step_size = angle_dist / num_iter
step_sizes.append(step_size)
return step_sizes
def __init__(self, start_angles, end_angles, target_end_pos=None):
self.start_node = Node(start_angles)
if end_angles is not None:
self.end_node = Node(end_angles)
if target_end_pos is not None:
self.target_end_pos = target_end_pos
else:
self.target_end_pos = self.end_node.end_effector_pos
self.edges = []
self.success = False
self.node_to_index = {self.start_node: 0}
self.neighbors = {0: []}
self.distances = {0: 0.}
self.ranking = []
self.nodes = [self.start_node]
self.end_effectors = np.array([self.start_node.end_effector_pos])
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 linear_rrt_test(num_trials, obstacles, iter_per_path=10):
"""Runs num_trials of a linear rrt pathing approach.
Success is defined as converging to the desired end position, and any intermediate RRT converging to the desired end
position.
"""
start_time = time.time()
s_count = 0
for i in range(num_trials):
print("Trial", i)
start_node = Node(configuration=None)
end_node = Node(configuration=None)
while not valid_path_configuration(start_node, obstacles):
start_node = Node(None)
while not valid_path_configuration(end_node, obstacles):
end_node = Node(None)
if cd.arm_is_colliding_prisms(end_node, obstacles):
raise Exception("Approved a colliding node")
_, success = linear_rrt_to_point(start_node.angles,
end_node.end_effector_pos, obstacles,
iter_per_path)
if success:
s_count = s_count + 1
else:
print("start_pos =", format(start_node.angles))
print("end_pos =", format(end_node.angles))
print("success rate in {t} trials: {r}".format(t=num_trials,
r=(s_count / num_trials) *
100))
print("average time per trial: {}".format(
(time.time() - start_time) / num_trials))
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 random_start_environment(num_obstacles,
bounds,
obstacle_bounds,
obstacle_size=.2):
"""Generates a start environment for a run of RRT.
Returns:
A Node representing a valid start configuration.
A Node representing a valid end configuration.
A set of [num_obstacles] obstacles that do not collide with the start or end configurations.
"""
random_start_node = Node(configuration=None)
target_end_pos = [
random.uniform(bounds[0][0], bounds[0][1]),
random.uniform(bounds[1][0], bounds[1][1]),
random.uniform(bounds[2][0], bounds[2][1])
]
random_end_node = Node.from_point(target_end_pos, random_start_node.angles)
while (not random_end_node.valid_configuration()):
target_end_pos = [
random.uniform(bounds[0][0], bounds[0][1]),
random.uniform(bounds[1][0], bounds[1][1]),
random.uniform(bounds[2][0], bounds[2][1])
]
random_end_node = Node.from_point(target_end_pos, random_start_node.angles)\
current_obstacles = generate_random_obstacles(
num_obstacles, obstacle_bounds, max_side_length=obstacle_size)
while arm_is_colliding_prisms(random_end_node, current_obstacles):
current_obstacles = generate_random_obstacles(
num_obstacles, obstacle_bounds, max_side_length=obstacle_size)
while arm_is_colliding_prisms(
random_start_node,
current_obstacles) or not random_start_node.valid_configuration():
random_start_node = Node(None)
print("start angles:", random_start_node.angles)
print("end angles:", random_end_node.angles)
print("obstacles:", current_obstacles)
return random_start_node, random_end_node, current_obstacles, target_end_pos
def random_start_environment(num_obstacles, bounds, obstacle_size=.2):
"""Generates a start environment for a run of RRT.
Returns:
An Node representing a valid start configuration.
An Node representing a valid end configuration.
A set of [num_obstacles] obstacles that do not collide with the start or end configurations.
"""
random_start_node = Node(configuration=None)
random_end_node = Node.from_point([
random.uniform(bounds[0][0], bounds[0][1]),
random.uniform(bounds[1][0], bounds[1][1]),
random.uniform(bounds[2][0], bounds[2][1])
], random_start_node.angles)
max_tries = 10
tries = 1
while not random_end_node.valid_configuration():
random_end_node = Node.from_point([
random.uniform(bounds[0][0], bounds[0][1]),
random.uniform(bounds[1][0], bounds[1][1]),
random.uniform(bounds[2][0], bounds[2][1])
], random_start_node.angles)
tries += 1
if tries > max_tries:
return None, None, None
current_obstacles = obstacle_generation.generate_random_obstacles(
num_obstacles, bounds, max_side_length=obstacle_size)
while arm_is_colliding_prisms(random_end_node, current_obstacles):
current_obstacles = obstacle_generation.generate_random_obstacles(
num_obstacles, bounds, max_side_length=obstacle_size)
while arm_is_colliding_prisms(
random_start_node,
current_obstacles) or not random_start_node.valid_configuration():
random_start_node = Node(None)
#print("start angles:", random_start_node.angles)
#print("end angles:", random_end_node.angles)
#print("obstacles:", current_obstacles)
return random_start_node, random_end_node, current_obstacles
def steer(rand_angles, near_angles, step_size):
"""Generates a new node a certain distance along the path between the nearest node to the random node.
Args:
rand_angles: The angles of the randomly generated node.
near_angles: The angles of the node closest to the randomly generated node.
step_size: The distance from the nearest node for the new node to be generated.
Returns:
An instance of Node representing the new node of the tree.
"""
dirn = true_angle_distances_arm(np.array(near_angles),
np.array(rand_angles))
length = np.linalg.norm(dirn)
dirn = (dirn / length) * min(step_size, length)
new_angles = (near_angles[0] + dirn[0], near_angles[1] + dirn[1],
near_angles[2] + dirn[2], near_angles[3] + dirn[3],
near_angles[4] + dirn[4])
return Node(new_angles)
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 expand(node, directions, step_size, g):
""" Expands from a node in a certain amount of directions for length [step_size]. """
for i in range(directions):
new_angles_pos = list.copy(node.angles)
new_angles_neg = list.copy(node.angles)
new_angles_pos[i] += step_size
new_angles_neg[i] -= step_size
node_pos = Node(new_angles_pos)
node_neg = Node(new_angles_neg)
if not visited(node_pos, g) and node_pos.valid_configuration():
g.add_vex(node_pos, node)
if not visited(node_neg, g) and node_neg.valid_configuration():
g.add_vex(node_neg, node)
def inverse_test(threshold, num_trials, bounds):
error_count = 0
for i in range(num_trials):
print("Trial:", i)
point = [
random.uniform(bounds[0][0], bounds[0][1]),
random.uniform(bounds[1][0], bounds[1][1]),
random.uniform(bounds[2][0], bounds[2][1])
]
node = Node.from_point(point)
print("Target point:", point)
print("Obtained point:", node.end_effector_pos)
diff = np.linalg.norm(node.end_effector_pos, point)
print("Difference:", diff)
if diff > threshold:
print("BAD!!!")
error_count += 1
else:
print("GOOD!!!!")
print("Success rate", error_count / num_trials)
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import art3d
from arm_node import Node
import kinpy as kp
chain = kp.build_chain_from_urdf(
open("models/SimpleArmModelforURDF.URDF").read())
chain = kp.build_serial_chain_from_urdf(
open("models/SimpleArmModelforURDF.URDF").read(), "hand_1", "base_link")
angle_config = kp.ik.inverse_kinematics(chain,
kp.Transform(pos=[0.2, 0.2, 0.2]))
ret = chain.forward_kinematics(angle_config, end_only=False)
print(ret)
node = Node.from_point([.2, .2, .2])
print(node.joint_positions)
# # Generate locations of each joint. Input angles here.
# th = {'Rev2': 0, 'Rev3': 0, 'Rev4': 0, 'Rev5': 0, 'Rev6': math.pi/2}
# ret = chain.forward_kinematics(th)
#
# print(chain.get_joint_parameter_names())
# print(chain)
# print(ret)
# print(ret['base_link'])
#
#
ax = plt.axes(projection='3d')
# Draw lines in between each joint
if graph_list[i].success:
plt.scatter(i, times[i], c='green')
else:
plt.scatter(i, times[i], c='red')
plt.yticks(np.arange(0, 11, 1))
plt.ylim(0, 10)
plt.xlabel('Trial')
plt.ylabel('Time (s)')
plt.show()
def plot_ik_trial(target_point, obtained_config):
""" Visualizes the distance between a target point and a point obtained through inverse kinematics. """
ax = plt.axes(projection='3d')
plot_nodes(ax, [target_point])
plot_arm_configs(ax, [obtained_config], None)
if __name__ == "__main__":
configs = []
for i in range(1):
node = Node(None)
configs.append(node)
ax = plt.axes(projection='3d')
configure_graph(ax)
plot_arm_configs(ax, configs, [])
plt.show()
def main():
"""
Places a prism randomly in 3D space, and draws a larger prism around it that represents it's
bounding box. Then checks if a randomly generated arm collides the outer prism at any point.
"""
fig = plt.figure()
ax = plt.axes(projection="3d")
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
# Set the limits for the range of plotted values
lim = .6
plt.xlim(-lim, lim)
plt.ylim(-lim, lim)
ax.set_zlim(-lim, lim)
# create the prism to be avoided (hard coded)
lim = .3
location = random.uniform(-lim, lim)
prism = ([0, 0, 0, .2, .3, .4])
exoPrism = ([
prism[0] - .05, prism[1] - .05, prism[2] - .05, prism[3] + .1,
prism[4] + .1, prism[5] + .1
])
plot_linear_prism(ax, prism, 'blue')
plot_linear_prism(ax, exoPrism, 'red')
lim = .5
"""
This code was used to generate a single line, and has since been replaced
by code that generates a random N link arm.
randomX1 = random.uniform(-lim, lim)
randomY1 = random.uniform(-lim, lim)
randomZ1 = random.uniform(-lim, lim)
randomX2 = random.uniform(-lim, lim)
randomY2 = random.uniform(-lim, lim)
randomZ2 = random.uniform(-lim, lim)
line_seg = ([randomX1, randomY1, randomZ1,
randomX2, randomY2, randomZ2]) # ([-.1, .1, .1, .3, .1, .1])
# line_seg = ([-.3,.15,-.2,.6,.15,.2]) # intersects x faces
# line_seg = ([.16, -.3, .2, .16, .3, .2]) # intersects y faces
# line_seg = ([.2, -.2, -.3, .2, .15, .5]) # intersects z faces
xs = ([line_seg[0], line_seg[3]])
ys = ([line_seg[1], line_seg[4]])
zs = ([line_seg[2], line_seg[5]])
plot_lines(ax, xs, ys, zs, None)
"""
# Create a random arm
arm = Node(None)
plot_arm(ax, arm, None)
class Index:
def draw(self, event):
plt.close()
main()
axnext = plt.axes([0.81, 0.05, 0.1, 0.075])
run = Button(axnext, 'ReRun')
callback = Index()
run.on_clicked(callback.draw)
if checkCollision(exoPrism, arm):
plt.title("Collision Detected!")
else:
plt.title("Wow no Collision")
plt.show()
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