From 65debdc332b23083a434c24fa5b73e917ee71aa2 Mon Sep 17 00:00:00 2001 From: Mahyar Abdeetedal Date: Tue, 29 Dec 2020 06:32:13 -0500 Subject: [PATCH] RRT* for seven joint arm control (#439) --- .../rrt_star_seven_joint_arm.py | 403 ++++++++++++++++++ tests/test_rrt_star_seven_joint_arm.py | 26 ++ 2 files changed, 429 insertions(+) create mode 100755 ArmNavigation/seven_joint_arm_to_point_control/rrt_star_seven_joint_arm.py create mode 100644 tests/test_rrt_star_seven_joint_arm.py diff --git a/ArmNavigation/seven_joint_arm_to_point_control/rrt_star_seven_joint_arm.py b/ArmNavigation/seven_joint_arm_to_point_control/rrt_star_seven_joint_arm.py new file mode 100755 index 00000000..d10733b4 --- /dev/null +++ b/ArmNavigation/seven_joint_arm_to_point_control/rrt_star_seven_joint_arm.py @@ -0,0 +1,403 @@ +""" +RRT* path planner for a seven joint arm +Author: Mahyar Abdeetedal (mahyaret) +""" +import math +import os +import sys +import random +import numpy as np +from mpl_toolkits import mplot3d +import matplotlib.pyplot as plt +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../n_joint_arm_3d/") +try: + from NLinkArm3d import NLinkArm +except ImportError: + raise + +show_animation = True +verbose = False + + +class RobotArm(NLinkArm): + def get_points(self, joint_angle_list): + self.set_joint_angles(joint_angle_list) + + x_list = [] + y_list = [] + z_list = [] + + trans = np.identity(4) + + x_list.append(trans[0, 3]) + y_list.append(trans[1, 3]) + z_list.append(trans[2, 3]) + for i in range(len(self.link_list)): + trans = np.dot(trans, self.link_list[i].transformation_matrix()) + x_list.append(trans[0, 3]) + y_list.append(trans[1, 3]) + z_list.append(trans[2, 3]) + + return x_list, y_list, z_list + + +class RRTStar: + """ + Class for RRT Star planning + """ + + class Node: + def __init__(self, x): + self.x = x + self.parent = None + self.cost = 0.0 + + def __init__(self, start, goal, robot, obstacle_list, rand_area, + expand_dis=.30, + path_resolution=.1, + goal_sample_rate=20, + max_iter=300, + connect_circle_dist=50.0 + ): + """ + Setting Parameter + + start:Start Position [q1,...,qn] + goal:Goal Position [q1,...,qn] + obstacleList:obstacle Positions [[x,y,z,size],...] + randArea:Random Sampling Area [min,max] + + """ + self.start = self.Node(start) + self.end = self.Node(goal) + self.dimension = len(start) + self.min_rand = rand_area[0] + self.max_rand = rand_area[1] + self.expand_dis = expand_dis + self.path_resolution = path_resolution + self.goal_sample_rate = goal_sample_rate + self.max_iter = max_iter + self.robot = robot + self.obstacle_list = obstacle_list + self.connect_circle_dist = connect_circle_dist + self.goal_node = self.Node(goal) + self.ax = plt.axes(projection='3d') + self.node_list = [] + + def planning(self, animation=False, search_until_max_iter=False): + """ + rrt star path planning + + animation: flag for animation on or off + search_until_max_iter: search until max iteration for path + improving or not + """ + + self.node_list = [self.start] + for i in range(self.max_iter): + if verbose: + print("Iter:", i, ", number of nodes:", len(self.node_list)) + rnd = self.get_random_node() + nearest_ind = self.get_nearest_node_index(self.node_list, rnd) + new_node = self.steer(self.node_list[nearest_ind], + rnd, + self.expand_dis) + + if self.check_collision(new_node, self.robot, self.obstacle_list): + near_inds = self.find_near_nodes(new_node) + new_node = self.choose_parent(new_node, near_inds) + if new_node: + self.node_list.append(new_node) + self.rewire(new_node, near_inds) + + if animation and i % 5 == 0 and self.dimension <= 3: + self.draw_graph(rnd) + + if (not search_until_max_iter) and new_node: + last_index = self.search_best_goal_node() + if last_index is not None: + return self.generate_final_course(last_index) + if verbose: + print("reached max iteration") + + last_index = self.search_best_goal_node() + if last_index is not None: + return self.generate_final_course(last_index) + + return None + + def choose_parent(self, new_node, near_inds): + if not near_inds: + return None + + # search nearest cost in near_inds + costs = [] + for i in near_inds: + near_node = self.node_list[i] + t_node = self.steer(near_node, new_node) + if t_node and self.check_collision(t_node, + self.robot, + self.obstacle_list): + costs.append(self.calc_new_cost(near_node, new_node)) + else: + costs.append(float("inf")) # the cost of collision node + min_cost = min(costs) + + if min_cost == float("inf"): + print("There is no good path.(min_cost is inf)") + return None + + min_ind = near_inds[costs.index(min_cost)] + new_node = self.steer(self.node_list[min_ind], new_node) + new_node.parent = self.node_list[min_ind] + new_node.cost = min_cost + + return new_node + + def search_best_goal_node(self): + dist_to_goal_list = [self.calc_dist_to_goal(n.x) + for n in self.node_list] + goal_inds = [dist_to_goal_list.index(i) + for i in dist_to_goal_list if i <= self.expand_dis] + + safe_goal_inds = [] + for goal_ind in goal_inds: + t_node = self.steer(self.node_list[goal_ind], self.goal_node) + if self.check_collision(t_node, self.robot, self.obstacle_list): + safe_goal_inds.append(goal_ind) + + if not safe_goal_inds: + return None + + min_cost = min([self.node_list[i].cost for i in safe_goal_inds]) + for i in safe_goal_inds: + if self.node_list[i].cost == min_cost: + return i + + return None + + def find_near_nodes(self, new_node): + nnode = len(self.node_list) + 1 + r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode)) + # if expand_dist exists, search vertices in + # a range no more than expand_dist + if hasattr(self, 'expand_dis'): + r = min(r, self.expand_dis) + dist_list = [np.sum((np.array(node.x) - np.array(new_node.x)) ** 2) + for node in self.node_list] + near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2] + return near_inds + + def rewire(self, new_node, near_inds): + for i in near_inds: + near_node = self.node_list[i] + edge_node = self.steer(new_node, near_node) + if not edge_node: + continue + edge_node.cost = self.calc_new_cost(new_node, near_node) + + no_collision = self.check_collision(edge_node, + self.robot, + self.obstacle_list) + improved_cost = near_node.cost > edge_node.cost + + if no_collision and improved_cost: + self.node_list[i] = edge_node + self.propagate_cost_to_leaves(new_node) + + def calc_new_cost(self, from_node, to_node): + d, _, _ = self.calc_distance_and_angle(from_node, to_node) + return from_node.cost + d + + def propagate_cost_to_leaves(self, parent_node): + + for node in self.node_list: + if node.parent == parent_node: + node.cost = self.calc_new_cost(parent_node, node) + self.propagate_cost_to_leaves(node) + + def generate_final_course(self, goal_ind): + path = [self.end.x] + node = self.node_list[goal_ind] + while node.parent is not None: + path.append(node.x) + node = node.parent + path.append(node.x) + + return path + + def calc_dist_to_goal(self, x): + distance = np.linalg.norm(np.array(x) - np.array(self.end.x)) + return distance + + def get_random_node(self): + if random.randint(0, 100) > self.goal_sample_rate: + rnd = self.Node(np.random.uniform(self.min_rand, + self.max_rand, + self.dimension)) + else: # goal point sampling + rnd = self.Node(self.end.x) + return rnd + + def steer(self, from_node, to_node, extend_length=float("inf")): + new_node = self.Node(list(from_node.x)) + d, phi, theta = self.calc_distance_and_angle(new_node, to_node) + + new_node.path_x = [list(new_node.x)] + + if extend_length > d: + extend_length = d + + n_expand = math.floor(extend_length / self.path_resolution) + + start, end = np.array(from_node.x), np.array(to_node.x) + v = end - start + u = v / (np.sqrt(np.sum(v ** 2))) + for _ in range(n_expand): + new_node.x += u * self.path_resolution + new_node.path_x.append(list(new_node.x)) + + d, _, _ = self.calc_distance_and_angle(new_node, to_node) + if d <= self.path_resolution: + new_node.path_x.append(list(to_node.x)) + + new_node.parent = from_node + + return new_node + + def draw_graph(self, rnd=None): + plt.cla() + self.ax.axis([-1, 1, -1, 1]) + self.ax.set_zlim(0, 1) + self.ax.grid(True) + for (ox, oy, oz, size) in self.obstacle_list: + self.plot_sphere(self.ax, ox, oy, oz, size=size) + if self.dimension > 3: + return self.ax + if rnd is not None: + self.ax.plot([rnd.x[0]], [rnd.x[1]], [rnd.x[2]], "^k") + for node in self.node_list: + if node.parent: + path = np.array(node.path_x) + plt.plot(path[:, 0], path[:, 1], path[:, 2], "-g") + self.ax.plot([self.start.x[0]], [self.start.x[1]], + [self.start.x[2]], "xr") + self.ax.plot([self.end.x[0]], [self.end.x[1]], [self.end.x[2]], "xr") + plt.pause(0.01) + return self.ax + + @staticmethod + def get_nearest_node_index(node_list, rnd_node): + dlist = [np.sum((np.array(node.x) - np.array(rnd_node.x))**2) + for node in node_list] + minind = dlist.index(min(dlist)) + + return minind + + @staticmethod + def plot_sphere(ax, x, y, z, size=1, color="k"): + u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j] + xl = x+size*np.cos(u)*np.sin(v) + yl = y+size*np.sin(u)*np.sin(v) + zl = z+size*np.cos(v) + ax.plot_wireframe(xl, yl, zl, color=color) + + @staticmethod + def calc_distance_and_angle(from_node, to_node): + dx = to_node.x[0] - from_node.x[0] + dy = to_node.x[1] - from_node.x[1] + dz = to_node.x[2] - from_node.x[2] + d = np.sqrt(np.sum((np.array(to_node.x) - np.array(from_node.x))**2)) + phi = math.atan2(dy, dx) + theta = math.atan2(math.hypot(dx, dy), dz) + return d, phi, theta + + @staticmethod + def calc_distance_and_angle2(from_node, to_node): + dx = to_node.x[0] - from_node.x[0] + dy = to_node.x[1] - from_node.x[1] + dz = to_node.x[2] - from_node.x[2] + d = math.sqrt(dx**2 + dy**2 + dz**2) + phi = math.atan2(dy, dx) + theta = math.atan2(math.hypot(dx, dy), dz) + return d, phi, theta + + @staticmethod + def check_collision(node, robot, obstacleList): + + if node is None: + return False + + for (ox, oy, oz, size) in obstacleList: + for x in node.path_x: + x_list, y_list, z_list = robot.get_points(x) + dx_list = [ox - x_point for x_point in x_list] + dy_list = [oy - y_point for y_point in y_list] + dz_list = [oz - z_point for z_point in z_list] + d_list = [dx * dx + dy * dy + dz * dz + for (dx, dy, dz) in zip(dx_list, + dy_list, + dz_list)] + + if min(d_list) <= size ** 2: + return False # collision + + return True # safe + + +def main(): + print("Start " + __file__) + + # init NLinkArm with Denavit-Hartenberg parameters of panda + # https://frankaemika.github.io/docs/control_parameters.html + # [theta, alpha, a, d] + seven_joint_arm = RobotArm([[0., math.pi/2., 0., .333], + [0., -math.pi/2., 0., 0.], + [0., math.pi/2., 0.0825, 0.3160], + [0., -math.pi/2., -0.0825, 0.], + [0., math.pi/2., 0., 0.3840], + [0., math.pi/2., 0.088, 0.], + [0., 0., 0., 0.107]]) + # ====Search Path with RRT==== + obstacle_list = [ + (-.3, -.3, .7, .1), + (.0, -.3, .7, .1), + (.2, -.1, .3, .15), + ] # [x,y,size(radius)] + start = [0 for _ in range(len(seven_joint_arm.link_list))] + end = [1.5 for _ in range(len(seven_joint_arm.link_list))] + # Set Initial parameters + rrt_star = RRTStar(start=start, + goal=end, + rand_area=[0, 2], + max_iter=200, + robot=seven_joint_arm, + obstacle_list=obstacle_list) + path = rrt_star.planning(animation=show_animation, + search_until_max_iter=False) + + if path is None: + print("Cannot find path") + else: + print("found path!!") + + # Draw final path + if show_animation: + ax = rrt_star.draw_graph() + for i, q in enumerate(reversed(path)): + x_points, y_points, z_points = seven_joint_arm.get_points(q) + if i == 0 or i == len(path) - 1: + color = None + else: + color = "grey" + ax.plot([x for x in x_points], + [y for y in y_points], + [z for z in z_points], + "o-", color=color, ms=4, mew=0.5) + plt.pause(0.01) + plt.show() + + +if __name__ == '__main__': + main() diff --git a/tests/test_rrt_star_seven_joint_arm.py b/tests/test_rrt_star_seven_joint_arm.py new file mode 100644 index 00000000..fe90801b --- /dev/null +++ b/tests/test_rrt_star_seven_joint_arm.py @@ -0,0 +1,26 @@ +import os +import sys +from unittest import TestCase + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../ArmNavigation/seven_joint_arm_to_point_control/") + +try: + import rrt_star_seven_joint_arm as m +except ImportError: + raise + + +print(__file__) + + +class Test(TestCase): + + def test1(self): + m.show_animation = False + m.main() + + +if __name__ == '__main__': # pragma: no cover + test = Test() + test.test1()