def rrt_connect_planning(map_info, display=False): rrt_start = RRT(map_info.start) rrt_end = RRT(map_info.end) rrt_a = rrt_start rrt_b = rrt_end okdtree = cKDTree(map_info.obstacle) while True: q_rand = (randint(1, map_info.width - 1), randint(1, map_info.height - 1)) if q_rand == map_info.start or q_rand in map_info.obstacle or rrt_a.is_contain(q_rand): continue q_new = rrt_a.extend(q_rand, okdtree) if not q_new: continue while True: q_new_ = rrt_b.extend(q_new, okdtree) if display: map_info.set_rand(q_rand) map_info.set_rrt_connect(rrt_a.get_rrt(), rrt_b.get_rrt()) # 2 rrts reached if q_new == q_new_: return reconstruct_path(rrt_a, rrt_b, q_new, map_info) elif q_new_ == None: break # swap 2 rrts rrt_a, rrt_b = rrt_b, rrt_a
def __init__(self): #Initialising messages and variables self.position = Point() self.path = Path() self.obstacles = [] self.start = Point() self.goal = Point() self.scan = LaserScan() self.goal_recieved = False #Publishers and Subscribers self.path_pub = rospy.Publisher('/path', Path, queue_size=10) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.__odom_update) self.scan_pub = rospy.Subscriber('/scan', LaserScan, self.__laser_sub) self.goal_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.__goal_sub) #Flag to check if final goal is reached self.not_reached = True #Initialising path planner self.path_planner = RRT(1, 1000) self.path_points = [] #Initialise transform listener self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.rate = rospy.Rate(10)
def main(): # ====Search Path with RRT==== # Parameter obstacleList = [ (5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2), (9, 5, 2) ] # [x,y,size] rrt = RRT(start=[0, 0], goal=[6, 10], rand_area=[-2, 15], obstacle_list=obstacleList) path = rrt.planning(animation=show_animation) # Path smoothing maxIter = 1000 smoothedPath = path_smoothing(path, maxIter, obstacleList) # Draw final path if show_animation: rrt.draw_graph() plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.plot([x for (x, y) in smoothedPath], [ y for (x, y) in smoothedPath], '-c') plt.grid(True) plt.pause(0.01) # Need for Mac plt.show()
def __init__(self, my_map): self.map = my_map self.pop_num = 20 # 种群数量 self.rrt = RRT(self.map) self.mutate_rate = 0.0001 # 突变概率 self.iterations = 100 self.cross_rate = 0.9
def test_rrt_init_args(self): rrt = RRT(start_state=(0., 0.), goal_state=(1, 1), dim_ranges=[(-2, 2), (-2, 2)], obstacles=None, step_size=0.1, max_iter=1000)
def test_dynamic_env(self): """ Tests that the RRT works in a dynamic environement """ env = DynamicEnvironment((100, 100), 5) rrt = RRT(env) start = (50, 1, 1.57) end = (50, 99, 1.57) # Initialisation of the tree, to have a first edge rrt.set_start(start) rrt.run(end, 200, metric='local') # Initialisation of the position of the vehicle position = start[:2] current_edge = rrt.select_best_edge() for i in range(60): # We check if we are on an edge or if we have to choose a new edge if not current_edge.path: current_edge = rrt.select_best_edge() # Update the position of the vehicle position = current_edge.path.popleft() # Update the environment # The frontiers of the sampling and the obstacles env.update(position) # The position of the goal end = (50, position[1]+90, 1.57) # Continue the growth of the tree rrt.run(end, 2, metric='local') env.plot(display=False) rrt.plot(file_name='static'+str(i)+'.png', close=True) print('Dynamic RRT OK')
def __init__(self): # self.command_sub_ = rospy.Subscriber('controller_commands_dev', Controller_Commands, self.cmdCallback, queue_size=5) # self.command_pub_ = rospy.Publisher('controller_commands', Controller_Commands, queue_size=1) # self.state_sub_ = rospy.Subscriber('state', State, self.stateCallback, queue_size=1) ref_lat = rospy.get_param("ref_lat") ref_lon = rospy.get_param("ref_lon") ref_h = rospy.get_param("ref_h") self.ref_pos = [ref_lat, ref_lon, ref_h] #Get the obstacles, boundaries, and drop location in order to initialize the RRT class mission_type, self.obstacles, self.boundary_list, self.boundary_poly, drop_location = tools.get_server_data( JudgeMission.MISSION_TYPE_DROP, self.ref_pos) self.RRT_planner = RRT( self.obstacles, self.boundary_list, animate=False ) #Other arguments are available but have been given default values in the RRT constructor # Advertise the path planning service call self._plan_server = rospy.Service('plan_path', PlanMissionPoints, self.waypoints_callback) #Set up a service call to get waypoints from the mission planner self.mission_data = rospy.ServiceProxy('plan_mission', PlanMissionPoints)
def main(): # ====Search Path with RRT==== # Parameter obstacleList = [(6.23, 1.9, 1.2), (4.48, 3.44, 1.02), (7.51, 7.14, 0.96), (0.59, 8.35, 0.75), (2.19, 7.31, 0.6), (1.43, 2.5, 0.48), (5.8, 6.53, 0.45)] # [x,y,size] rrt = RRT(start=[x_array[start], y_array[start]], goal=[x_array[end], y_array[end]], rand_area=[0, 9], obstacle_list=obstacleList) path = rrt.planning(animation=show_animation) # Path smoothing maxIter = 1000 smoothedPath = path_smoothing(path, maxIter, obstacleList) # Draw final path if show_animation: rrt.draw_graph() plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.plot([x for (x, y) in smoothedPath], [y for (x, y) in smoothedPath], '-c') plt.grid(True) plt.pause(0.01) # Need for Mac plt.show()
def main2(): map = Map("maps/rss_offset.json") planner = RRT(map) start = [.5, .5] goal = [3.5, 2.5] path = np.array(planner.getPath(start, goal)) r = Controller(start[0], start[1], 0, map, path) a = input("presse a key to start controller: ") x, y, v, w, d = r.followPath()
def test6(): obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]), Polygon([(4,4), (4,8), (7,1)]), Polygon([(2,2), (1,8), (2,4), (3,3)])] workspace = Workspace(dim=(12,12), obstacles=obstacles) car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=4, max_control_time=1, step_threshold=.05) results = car_rrt.build_rrt() print(results) ax = workspace.graph_results(results, car_rrt, plot_tree=False) plt.show()
def test22(): obstacles = [Polygon([(2,0), (8, 0), (8, 1), (2, 1)]), Polygon([(2,10), (8,10), (8,8), (2,8)]), Polygon([(2,1), (2,8), (3,8), (3,1)])] workspace = Workspace(dim=(12,12), obstacles=obstacles) car_rrt = RRT(workspace, (1, 6, pi), (6, 6, pi), 10000, num_rand_actions=6, max_control_time=1, step_threshold=.05) results = car_rrt.build_rrt() print(results) anim = workspace.animate_results(results, car_rrt, plot_tree=True) plt.show()
def generate_paths(self, algo="a-star"): if algo == "a-star": for robot in self.robots: astar = Astar(robot.pose, robot.goal, self.obstacles) robot.path = astar.generate_path() elif algo == "rrt": for robot in self.robots: rrt = RRT(robot.pose, robot.goal, self.obstacles) robot.path = rrt.generate_path() else: print "No such algo currently exists." return
def setUp(self): # RRT 1: RRT with 1 node, in 3 dimensions self.rrt1 = RRT(start_state=(0., 0., 0.), goal_state=(1, 1, 1), dim_ranges=[(-2, 2)] * 3, step_size=0.1) # RRT 2: RRT with 2 nodes, in 2 dimensions # (start) --> (node1) self.rrt2 = RRT(start_state=(0., 0.), goal_state=(1, 1), dim_ranges=[(-2, 2), (-2, 2)], step_size=0.05) self.rrt2_node1 = self.rrt2.start.add_child(state=(0.5, 0.25)) # RRT 3: RRT with 2 nodes, in 3 dimensions self.rrt3 = RRT(start_state=(0.25, 0.25, 0.25), goal_state=(0.75, 0.75, 0.75), dim_ranges=[(0, 1)] * 3, step_size=0.1) self.rrt3_node1 = self.rrt3.start.add_child(state=(0.5, 0.5, 0.5)) self.rrt3_node2 = self.rrt3_node1.add_child(state=(0.75, 0.75, 0.7))
def plan_cb(self, goal): # Wait for map to be created while not rospy.is_shutdown(): if self.map is not None: break rospy.loginfo_throttle(5, 'Planning: Waiting for map...') self.wait_rate.sleep() rospy.loginfo('Planning: map recieved!') # Create waypoints for the planner to pass through waypoints = self.create_waypoints(self.map.gates) start_wp = waypoints.pop(0) # Run path planning planner = RRT(self.map) traj = planner.plan_traj(start_wp, waypoints) while traj is None and not rospy.is_shutdown(): rospy.logwarn('RRT: Restarting RRT...') traj = planner.plan_traj(start_wp, waypoints) # Convert trajectory of Beziers to dd241909_msgs/Trajectory traj_pieces = [] for bezier in traj: piece = TrajectoryPolynomialPiece() for c in bezier.coeffs: piece.poly_x.append(c.x) piece.poly_y.append(c.y) piece.poly_z.append(c.z) piece.poly_yaw.append(0.0) piece.duration = rospy.Duration(bezier.T) traj_pieces.append(piece) traj_msg = Trajectory() traj_msg.header.frame_id = 'map' traj_msg.header.stamp = rospy.Time.now() traj_msg.pieces = traj_pieces # Set yaw values of traj_msg inplace self.set_yaw_gates(traj_msg, start_wp, traj) # self.set_yaw_rotating(traj_msg, start_wp, waypoints[-1]) # Publish trajectory self.traj_pub.publish(traj_msg) # Visualise trajectory in rviz if self.use_rviz: publish_traj_to_rviz(traj_msg) self.plan_server.set_succeeded(SimpleResult(message=''))
def test_build(self): num_retries = 3 for _ in range(num_retries): rrt1 = RRT(start_state=(0.2, 0.2), goal_state=(0.8, 0.8), dim_ranges=[(0, 1), (0, 1)], max_iter=1000) path = rrt1.build() if path is not None: break else: print("retrying...") self.assertIsNotNone(path) self.assertTrue(np.all(path[0] == [0.2, 0.2])) self.assertTrue(np.all(path[-1] == [0.8, 0.8]))
def main(): fig = plt.figure() armax = fig.add_subplot(121, autoscale_on=False, xlim=(-15, 15), ylim=(-15, 15)) confax = fig.add_subplot(122, autoscale_on=False, xlim=(-2 * math.pi, 2.5 * math.pi), ylim=(-2 * math.pi, 2.5 * math.pi)) rrt = RRT(confax, Arm2D()) confax.set_title("Configuration Space") armax.set_title("Arm") arm = Arm2DGraph(armax, confax, rrt) armax.set_aspect('equal') confax.set_aspect('equal') oldjoints = arm.arm.joints cx = [] cy = [] for theta1 in na.arange(-2 * math.pi, 2.5 * math.pi, 0.1): for theta2 in na.arange(-2 * math.pi, 2.5 * math.pi, 0.1): arm.arm.update(joints=[theta1, theta2]) if arm.arm.inCollision(): cx.append(theta1) cy.append(theta2) confax.scatter(cx, cy, c='r', marker='x') arm.arm.joints = oldjoints #ax.axis((-10, 10, -10, 10)) fig.canvas.mpl_connect('button_press_event', arm.on_press) fig.canvas.mpl_connect('button_release_event', arm.on_release) fig.canvas.mpl_connect('motion_notify_event', arm.on_motion) fig.canvas.mpl_connect('motion_notify_event', arm.on_motion) fig.canvas.mpl_connect('key_press_event', arm.on_key_press) fig.canvas.mpl_connect('key_release_event', arm.on_key_release) armax.grid() confax.grid() ani = animation.FuncAnimation(fig, arm.animate, interval=10, blit=False) plt.show()
def rrt_local(self, pe, start, end): dims = len(start.vertex) lims = [] for i in range(dims): if start.vertex[i] < end.vertex[i]: min = start.vertex[i] - 20 max = end.vertex[i] + 20 else: max = start.vertex[i] + 20 min = end.vertex[i] - 20 lims.append([min,max]) rrt = RRT(50, dims, self.epsilon, lims=np.array(lims), connect_prob=.15, collision_func=pe.test_collisions) plan = rrt.build_rrt(start.vertex, end.vertex, pe.robot) return plan
def test_rrt(self): """ Tests that the RRT class works """ env = StaticEnvironment((100, 100), 100) rrt = RRT(env) # Selection of random starting and ending points start = env.random_free_space() end = env.random_free_space() # Trying first the euclidian distance rrt.set_start(start) rrt.run(end, 200, metric='euclidian') # Trying first the distance defined by the local planner rrt.set_start(start) rrt.run(end, 200, metric='local') print('Static RRT OK')
def solution(car): #boundary, start, goal, obstacles, space_resolution, stepFunction, # primitives=5, primitive_bounds=[-math.pi/4.0, math.pi/4.0], rounding=4 # Assumption: state = (id,prim,x,y,r,F,g) boundary = [[car.xlb, car.ylb], [car.xub, car.yub]] # start = [0,0,car.x0,car.y0,0,0,0] # gt = math.atan2(car.yt-car.y0, car.xt-car.x0) # goal = [-1,-1,car.xt,car.yt,gt,0,0] start = (car.x0, car.y0, 0.0, 0.0, 0, 0) #(x,y,theta,phi,id,parent) goal = (car.xt, car.yt, 0.0, 0.0, 0, 0) obstacles = car.obs #space_resolution = min(obstacles, key = lambda t: t[2])[2]/4.0 #smallest obstacle space_resolution = 0.01 stepFunction = car.step print("Resolution: {}".format(space_resolution)) print("Start: {}".format(start)) print("Goal: {}".format(goal)) print("Boundary {}".format(boundary)) # astar = AStar(boundary, start, goal, obstacles, space_resolution, stepFunction) rrt = RRT(boundary, start, goal, obstacles, space_resolution, stepFunction) controls = [0, 0, 0] print("Started Planning") # controls = astar.run() controls = rrt.run() print("End Planning") assert (controls != None) times = [0] for _ in controls: times.append(times[-1] + car.dt) return controls, times
def is_collision(self, state): #-- Casts as a shapely point state_point = Point(state[0], state[1]) if self.obstacles is None: return False else: #-- For each obstacle checks if the point is contained in the obstacle. for obstacle in self.obstacles: if obstacle.contains(state_point): return True return False def graph_results(self, results, car_rrt, plot_tree=False): return graph_results(self, results, car_rrt, plot_tree) def animate_results(self, results, car_rrt, plot_tree=False): return animate_results(self, results, car_rrt, plot_tree) if __name__ == '__main__': random.seed(446) obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]), Polygon([(4,4), (4,8), (7,1)]), Polygon([(2,2), (1,8), (2,4), (3,3)])] workspace = Workspace(dim=(12,12), obstacles=obstacles) car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=6, max_control_time=2, step_threshold=.05) results = car_rrt.build_rrt() print(results) ax = workspace.graph_results(results, car_rrt, plot_tree=False) plt.show()
def test_rrt_init_bad_dims(self): with self.assertRaises(AssertionError): rrt = RRT(start_state=(0., 0.), goal_state=(1, 1, 1), dim_ranges=[(-2, 2), (-2, 2)])
MAX_TREE_NODES = 1000 QGOAL_BIAS = 0.33 my_car = Car() space = ConfigSpace(g) Qinit_3d = (0, 0, 0) # x = g.objects[g.reverseLookup[goal]].pos.x # y = g.objects[g.reverseLookup[goal]].pos.y # z = g.objects[g.reverseLookup[goal]].pos.z # Qgoal_3d = (x, y, z) Qgoal_3d = g.objects[g.reverseLookup[goal]].pos #print 'GOAL',Qgoal_3d # RUN t1 = time.time() rrt = RRT(space, my_car, Qinit_3d, Qgoal_3d, QGOAL_BIAS, MAX_TREE_NODES) tree = rrt.build_rrt_3d() t2 = time.time() if rrt.path: # Order: run#, number of nodes, size, number of obstructions, time, path length, nodes expanded print "R,%d,%d,%d,%d,%f,%d,%d" % ( run, len(g.locations), size, obstructions, (100 * (t2 - t1)), len(rrt.path), len(rrt.attempts)) else: print "R,%d,%d,%d,%d,%f,%d,%d" % ( run, len(g.locations), size, obstructions, (100 * (t2 - t1)), -1, len(rrt.attempts)) # Draw the resulting path if draw and rrt.path:
import json from apf import APF from rrt import RRT def load_config(config_filename: str) -> dict: with open(config_filename) as f: config = json.load(f) return config def pretty_print_config(config: dict): print(json.dumps(config, indent=2, sort_keys=False)) if __name__ == '__main__': config = load_config('config.json') apf_paraboloid = APF(config, paraboloid=True) apf_paraboloid.apf() apf_conical = APF(config, paraboloid=False) apf_conical.apf() rrt = RRT(config) rrt.bidirectional_rrt()
def main(): rospy.init_node('rrt') rrt = RRT() rospy.spin()
def main(self): """ dims : graph's dimensions (numpy array of tuples) obstacles : obstacles to be placed in the graph (list of shapely.Polygons) home : home location (tuple) init_state : start location (tuple) goal_state : goal location (tuple) delta : distance between nodes (int) k : shrinking ball facotr (int) n : number of waypoints to push (int) path : list of waypoints (list of tuples) case : case number to set behavior (int) Units: meters """ #rospy.init_node('RRTnode') #rospy.wait_for_service('/control/waypoints') wp_push = rospy.ServiceProxy('/control/waypoints', WaypointPush) #rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_cb) #rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, self.global_heading) #rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_position) rate = rospy.Rate(1) # send msgs at 1 Hz start_time = rospy.get_time() rate.sleep() # while loop for gps_check: while self.gps_status == -1 or self.gps_status is None: rospy.loginfo("BAD GPS") rate.sleep() while not self.home_set: rospy.loginfo("Home not set!") rate.sleep() # E-x N-y D-z dims = np.array([(-500, 2500), (-100, 2900), (-50, -50)]) obstacle = [(Point(1300, 1300).buffer(500))] init = self.pos goal = (2300.0, 2600.0, -50.0) delta = 100 k = 2 graph = Graph(dims, obstacle) path = None trail = [] position = [] start_index = 0 rospy.loginfo("Calculating Trajectory...") #print('Calculating Trajectory...\n') while not rospy.is_shutdown(): if graph.num_nodes() == 0: #t0 = time.time() rrt = RRT(graph, init, goal, delta, k, path, self.heading) if graph.num_nodes() <= 250: path, leaves = rrt.search() else: init = path[path.index(rrt.brute_force(self.pos, path)) + 1] trail.append(path) position.append(self.pos) rate.sleep() graph.clear() pathNED = np.asarray(path) wp_msg = [] pathLLA = self.frame.ConvNED2LLA(pathNED.T) for i in range(len(pathNED)): wp_point = Waypoint() wp_point.frame = 3 wp_point.x_lat = pathLLA[0][i] wp_point.y_long = pathLLA[1][i] wp_point.z_alt = pathLLA[2][i] wp_msg += [wp_point] print(wp_msg, '\n') resp = wp_push(start_index, wp_msg) print(resp, '\n') start_index += path.index(rrt.brute_force(self.pos, path)) + 1 #print('Search Time: {} secs\n'.format(time.time() - t0)) rate.sleep() if path is not None: if dist(self.pos, goal) <= delta * 3 or goal in path: rospy.loginfo("Goal reached") #print('Goal Reached.\n') print('trail : ', trail, '\n') print('position: ', position, '\n') break print('EOL')
def test_rrt_init_basic(self): rrt = RRT(start_state=(0.0, 0.0), goal_state=(1, 1), dim_ranges=[(-2, 2), (-2, 2)])
def main(argv=None): if (argv == None): argv = sys.argv[1:] width = 100.0 height = 100.0 pp = PathPlanningProblem(width, height, 60, 40, 40) initial, goals = pp.CreateProblemInstance() fig = plt.figure() ax = fig.add_subplot(1, 2, 1, aspect='equal') ax.set_xlim(0.0, width) ax.set_ylim(0.0, height) for o in pp.obstacles: ax.add_patch(copy.copy(o.patch)) ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000') ax.add_patch(ip) for g in goals: g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00') ax.add_patch(g) qtd = QuadTreeDecomposition(pp, 0.2, initial, goals) qtd.Draw(ax) n = qtd.CountCells() start = timeit.default_timer() astar = AStarSearch( qtd.domain, qtd.root, Rectangle(qtd.initialCell[0], qtd.initialCell[1], 0.1, 0.1), Rectangle(qtd.goalsCell[0][0], qtd.goalsCell[0][1], 0.1, 0.1)) stop = timeit.default_timer() print("A* Running Time: ", stop - start) print("A* Path Length : ", astar.path_length) plt.plot([x for (x, y) in astar.path], [y for (x, y) in astar.path], '-') ax.set_title('Quadtree Decomposition\n{0} cells'.format(n)) ax = fig.add_subplot(1, 2, 2, aspect='equal') ax.set_xlim(0.0, width) ax.set_ylim(0.0, height) for o in pp.obstacles: ax.add_patch(copy.copy(o.patch)) ip = plt.Rectangle((initial[0], initial[1]), 1, 1, facecolor='#ff0000') ax.add_patch(ip) goal = None for g in goals: goal = g g = plt.Rectangle((g[0], g[1]), 1, 1, facecolor='#00ff00') ax.add_patch(g) start = timeit.default_timer() spath = RRT.ExploreDomain(RRT(8), pp, initial, goal, 5000) path = spath[0] final = spath[1] if len(final) == 0: print("No path found") RRT.draw(RRT(0), plt, path, final) stop = timeit.default_timer() print("RRT Running Time: ", stop - start) if len(final) > 0: print("RRT Path Length : ", RRT.pathLen(RRT(0), final)) ax.set_title('RRT') plt.show()
return 0 def goal_test(node): goal_region_radius = .01 n = 4 return np.sum(np.abs(node['state'][0:n] - goal[0:n])) < goal_region_radius #disregards time return distance( node, goal ) < goal_region_radius #need to think more carefully about this one start = np.array([0, 0, 0, 0, 0]) lqr_rrt = LQR_RRT(A, B, Q, R, max_time_horizon) rrt = RRT(state_ndim=5, control_ndim=2) lqr_rrt.action_state_valid = action_state_valid lqr_rrt.max_nodes_per_extension = 5 rrt.sample_goal = lambda: goal rrt.set_distance(lqr_rrt.distance_cache) rrt.set_same_state(lqr_rrt.same_state) rrt.set_cost(lqr_rrt.cost) rrt.set_steer(lqr_rrt.steer_cache) rrt.set_goal_test(goal_test) rrt.set_sample(sample) rrt.set_collision_free(lqr_rrt.collision_free)
def rrt_planning(self): #initialize array X = [] Y = [] Z_euler=[] #set start point for orientation angle_s= self.angle_orientation[0] #set start point for path x_s = self.x_array[0] y_s = self.x_array[0] l = len(self.order_pic) maxIter = 1000 obstacleList=self.obstacleList #Generate Path & Orientation based on given order for i in range(l): #get order number n = self.order_pic[i] print(n) #set goal point x_g = self.x_array[n] y_g = self.y_array[n] angle_g = self.angle_orientation[n] angle_g = angle_g+np.pi #call RRT from rrt.py rrt = RRT(start=[x_s, y_s], goal=[x_g, y_g], rand_area=[0, 9], obstacle_list=self.obstacleList) path = rrt.planning(animation=False) #print(path) smoothedPath = rrt.path_smoothing(path, maxIter, obstacleList) #print(smoothedPath) X_p,Y_p = map(list,zip(*smoothedPath)) #Reverse X,Y coordinate for each segment X_p = np.flipud(X_p) Y_p = np.flipud(Y_p) m = len(X_p)-1 num_point = 800 #Store the path for j in range(m): #linaer interpolation of path n_x = np.linspace(X_p[j], X_p[j+1], num=num_point) n_y = np.linspace(Y_p[j], Y_p[j+1], num=num_point) X = np.concatenate([X,n_x]) Y = np.concatenate([Y,n_y]) #linear interpolation of orientation n_z_euler=np.linspace(angle_s , angle_g, m*num_point) Z_euler=np.concatenate([Z_euler,n_z_euler]) #update start point x_s = x_g y_s = y_g angle_s = angle_g Z = np.ones(len(X))*1.3 X_euler = np.linspace(0, 0 , len(X)) Y_euler = np.linspace(0, 0 , len(X)) #print(X) self.number_of_points=len(X) return X, Y, Z, X_euler, Y_euler, Z_euler
[0, 0, 0, -np.pi / 4, 0, np.pi / 4, np.pi / 4]) joints_target[0] = -np.deg2rad(45) elif args.map2: joints_start = np.array( [0, np.pi / 6, 0, -2 * np.pi / 3, 0, 5 * np.pi / 6, np.pi / 4]) joints_target = np.array( [0, 0, 0, -np.pi / 4, 0, np.pi / 4, np.pi / 4]) else: joints_start = fr.home_joints.copy() joints_start[0] = -np.deg2rad(45) joints_target = joints_start.copy() joints_target[0] = np.deg2rad(45) if args.rrt: print("RRT: RRT planner is selected!") planner = RRT(fr, is_in_collision) elif args.rrtc: print("RRTC: RRT Connect planner is selected!") planner = RRTConnect(fr, is_in_collision) elif args.prm: print("PRM: PRM planner is selected!") planner = PRM(fr, is_in_collision) elif args.obprm: print("OB_PRM: OB_PRM planner is selected!") planner = OBPRM(fr, is_in_collision) constraint = ee_upright_constraint if args.prm or args.obprm: plan = planner.plan(joints_start, joints_target, constraint, args) else: plan = planner.plan(joints_start, joints_target, constraint)