def buildup(state): child = self.CircleNode(state[0], state[1], state[2], state[3]) # build the child child.set_parent(circle) # child.h = self.distance(child, self.goal) child.h = reeds_shepp.path_length( (child.x, child.y, child.a), (self.goal.x, self.goal.y, self.goal.a), 1. / self.maximum_curvature) child.g = circle.g + reeds_shepp.path_length( (circle.x, circle.y, circle.a), (child.x, child.y, child.a), 1. / self.maximum_curvature) child.f = child.g + child.h # add the child to expansion set return child
def reeds_shepp_path_planning(start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature): q0 = [start_x, start_y, start_yaw] q1 = [end_x, end_y, end_yaw] step_size = 0.1 qs = reeds_shepp.path_sample(q0, q1, curvature, step_size) xs = [q[0] for q in qs] ys = [q[1] for q in qs] yaw = [q[2] for q in qs] xs.append(end_x) ys.append(end_y) yaw.append(end_yaw) clen = reeds_shepp.path_length(q0, q1, curvature) pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature) ptype = "" for t in pathtypeTuple: if t == 1: ptype += "L" elif t == 2: ptype += "S" elif t == 3: ptype += "R" return xs, ys, yaw, ptype, clen
def step(self, action): # Need to add path planner truck_trgt, excvtr_trgt = action truck_trgt = np.clip(truck_trgt, self.action_space[0].low, self.action_space[0].high) excvtr_trgt = np.clip(excvtr_trgt, self.action_space[1].low, self.action_space[1].high) print('truck_trgt is {}, excvtr_trgt is {}'.format(truck_trgt, excvtr_trgt)) truck_current = [self.state[0], self.state[1], self.state[2]] excvtr_current = [self.state[3], self.state[4], self.state[5]] self.truck_path = self.truck_planner.get_path(truck_current, truck_trgt) self.excvtr_path = self.excvtr_planner.get_path(excvtr_current, excvtr_trgt) self.step_counter = 0 self.cntr += 1 observation = [self.state, self.state] reward = -reeds_shepp.path_length(tuple(truck_current), tuple( excvtr_current), self.truck_planner.rho) done = False if self.cntr >= 20: done = True # reaches time limit elif self._can_load_on_truck(): done = True reward = 8000 info = None return observation, [reward, reward], [done, done], info
def plan_motions(index_sec): motion = [] v0, v1 = 0, 0 seg = discontinuities2[index_sec[0]:index_sec[1] + 1] seg = list(zip(seg[:-1], seg[1:])) extent = 0 for sec in seg: s0, s1 = sec[0].state, sec[1].state extent += np.abs( reeds_shepp.path_length(s0, s1, 1. / self.maximum_curvature)) samples = [] for sec in seg: s0, s1 = sec[0].state, sec[1].state samples.extend( reeds_shepp.path_sample(s0, s1, 1. / self.maximum_curvature, res)) acc = min([(v_max**2 - v1**2) / extent, a_cc]) vcc = np.sqrt(v1**2 + acc * extent) for i, sample in enumerate(samples): if i * res < extent / 2.: vt = min([np.sqrt(v0**2 + 2 * acc * (i * res)), vcc]) else: vt = min([ np.sqrt(v1**2 + 2 * acc * np.abs(extent - i * res)), vcc ]) motion.append( self.Configuration(sample[:3], k=sample[3], v=np.sign(sample[4]) * vt)) return motion
def main(): # points to be followed pts = [(-6, -7), (-6, 0), (-4, 6), (0, 5), (0, -2), (-2, -6), (3, -5), (3, 6), (6, 4)] # generate PATH so the vectors are pointing at each other PATH = [] for i in range(len(pts) - 1): dx = pts[i + 1][0] - pts[i][0] dy = pts[i + 1][1] - pts[i][1] theta = math.atan2(dy, dx) PATH.append((pts[i][0], pts[i][1], utils.rad2deg(theta))) PATH.append((pts[-1][0], pts[-1][1], 0)) # or you can also manually set the angles: # PATH = [(-5,5,90),(-5,5,-90),(1,4,180), (5,4,0), (6,-3,90), (4,-4,-40),(-2,0,240), \ # (-6, -7, 160), (-7,-1,80)] # or just generate a random route: # PATH = [] # for _ in range(10): # PATH.append((rd.randint(-7,7), rd.randint(-7,7), rd.randint(0,359))) # init turtle tesla = turtle.Turtle() tesla.speed(0) # 0: fast; 1: slow, 8.4: cool tesla.shape('arrow') tesla.resizemode('user') tesla.shapesize(1, 1) # draw vectors representing points in PATH for pt in PATH: draw.goto(tesla, pt) draw.vec(tesla) # draw all routes found tesla.speed(0) for i in range(len(PATH) - 1): paths = rs.get_all_paths(PATH[i], PATH[i + 1]) for path in paths: draw.set_random_pencolor(tesla) draw.goto(tesla, PATH[i]) draw.draw_path(tesla, path) # draw shortest route tesla.pencolor(1, 0, 0) tesla.pensize(3) tesla.speed(10) draw.goto(tesla, PATH[0]) path_length = 0 for i in range(len(PATH) - 1): path = rs.get_optimal_path(PATH[i], PATH[i + 1]) path_length += rs.path_length(path) draw.draw_path(tesla, path) print("Shortest path length: {} px.".format(int(draw.scale(path_length)))) turtle.done()
def calculate_path_length(path, rho): sequence = map(deepcopy, path) sequence = map(list, sequence) sequence = zip(sequence[:-1], sequence[1:]) length = 0 for x_from, x_to in sequence: length += reeds_shepp.path_length(x_from, x_to, rho) return length
def get_heuristic(self, grid, goal_grid): #dx = goal_grid[0] - grid[0] #dy = goal_grid[1] - grid[1] #h_state = math.sqrt(dx ** 2 + dy ** 2) #dist = 0.0 #if self.iterations % 1000 == 0: # dist = reeds_shepp.path_length(grid, goal_grid, self.rho) dist = reeds_shepp.path_length(grid, goal_grid, self.rho) return dist
def plot_table(cols): rows = ((len(qs)) / cols) for i, (q0, q1) in enumerate(qs): plt.subplot(rows, cols, i + 1) plot_path(q0, q1) dist = reeds_shepp.path_length(q0, q1, rho) plt.title('length: {:.2f}'.format(dist)) plt.savefig('fig/demo.png') plt.show()
def plot_table(cols): rows = ((len(qs)) / cols) for i,(q0, q1) in enumerate(qs): plt.subplot(rows, cols, i+1) plot_path(q0, q1) dist = reeds_shepp.path_length(q0, q1, rho) plt.title('length: {:.2f}'.format(dist)) plt.savefig('fig/demo.png') plt.show()
def steer(self, p, q, eps): delta = 0.2 length = rp.path_length(p, q, self.turn_radius) points = rp.path_sample(p, q, self.turn_radius, delta) points.append(q) path = np.array(points) if length > eps: num_points = math.floor(eps / delta) path = path[0:num_points + 1] return Curve(path)
def plan_motions(sector): q0, v0, q1, v1 = sector[0].state, sector[0].v, sector[ 1].state, sector[1].v extent = reeds_shepp.path_length(q0, q1, 1. / self.maximum_curvature) acc = min([(v_max**2 - v1**2) / extent, a_cc]) vcc = np.sqrt(v1**2 + acc * extent) samples = reeds_shepp.path_sample(q0, q1, 1. / self.maximum_curvature, res) for i, sample in enumerate(samples): if i * res < extent / 2.: vt = min([np.sqrt(v0**2 + 2 * acc * (i * res)), vcc]) else: vt = min( [np.sqrt(v1**2 + 2 * acc * (extent - i * res)), vcc]) motions.append( self.Configuration(sample[:3], k=sample[3], v=np.sign(sample[4]) * vt))
def trajectory_generation(self): # DEFINIZIONE DELLA COORDINATA INIZIALE E DI QUELLA FINALE coordinate = [(0.0,0.0,0.0),(3.0,-4.0,np.pi)] # PARAMETRI UTILI ALLA GENERAZIONE DELLE CURVE DI REEDSSHEPP step_size = 0.01823 rho = 5.8 # GENERAZIONE DELLE CURVE DI REEDS SHEPP qs = reeds_shepp.path_sample(coordinate[0], coordinate[1], rho, step_size) tmax = np.round(reeds_shepp.path_length(coordinate[0], coordinate[1], rho),2) # ESTRAZIONE DEI PARAMETRI LUNGO I DUE ASSI E ARROTONDAMENTO ALLA TERZA DECIMALE xs = np.round([coordinate[0] for coordinate in qs],3) ys = np.round([coordinate[1] for coordinate in qs],3) theta = np.round([coordinate[2] for coordinate in qs],3) self.t = np.linspace(0,tmax,len(xs)) # Richiamo il metodo "trajectory" per l'estrazione dei segmenti rappresentanti le curve di Reeds Shepp a partire dalle circonferenze calcolate v_d_val = 0.5 # m/s element = 0 u1 = np.zeros(len(xs)) while element < len(theta)-1: if theta[element] < np.pi/2 and theta[element] > -np.pi/2: if xs[element] < xs[element+1]: u1[element] = 1 else: u1[element] = -1 else: if xs[element] > xs[element+1]: u1[element] = 1 else: u1[element] = -1 element = element+1 self.dotx_d = u1*np.cos(theta) self.doty_d = u1*np.sin(theta) self.v_d = np.sqrt(self.dotx_d**2 + self.doty_d**2) self.theta_d = np.arctan2(self.doty_d, self.dotx_d) self.x_d = xs self.y_d = ys
def initialize(self, start, goal, grid_map, grid_res, grid_ori, obstacle=255): # type: (CircleNode, CircleNode, np.ndarray, float, CircleNode, int) -> OrientationSpaceExplorer """ :param start: start circle-node :param goal: goal circle-node :param grid_map: occupancy map(0-1), 2d-square, with a certain resolution: gird_res :param grid_res: resolution of occupancy may (m/pixel) :param obstacle: value of the pixels of obstacles region on occupancy map. """ self.start, self.goal = start, goal self.grid_map, self.grid_res, self.grid_ori, self.obstacle = grid_map, grid_res, grid_ori, obstacle # padding grid map for clearance calculation s = int(np.ceil((self.maximum_radius + self.minimum_clearance) / self.grid_res)) self.grid_pad = np.pad(self.grid_map, ((s, s), (s, s)), 'constant', constant_values=((self.obstacle, self.obstacle), (self.obstacle, self.obstacle))) # complete the start and goal self.start.r, self.start.g = self.clearance(self.start) - self.minimum_clearance, 0 self.start.h = reeds_shepp.path_length( (start.x, start.y, start.a), (self.goal.x, self.goal.y, self.goal.a), 1. / self.maximum_curvature) self.goal.r, self.goal.h, self.goal.g = self.clearance(self.goal) - self.minimum_clearance, 0, np.inf self.start.f, self.goal.f = self.start.g + self.start.h, self.goal.g + self.goal.h return self
import reeds_shepp as rs import utils import math # a list of "vectors" (x, y, angle in degrees) ROUTE = [(-2, 4, 180), (2, 4, 0), (2, -3, 90), (-5, -6, 240), (-6, -7, 160), (-7, -1, 80)] full_path = [] total_length = 0 for i in range(len(ROUTE) - 1): path = rs.get_optimal_path(ROUTE[i], ROUTE[i + 1]) full_path += path total_length += rs.path_length(path) print("Shortest path length: {}".format(round(total_length, 2))) for e in full_path: print(e)
qd = [xs, ys, thetas] return qd def plot_table(cols): rows = ((len(qs)) / cols) for i, (q0, q1) in enumerate(qs): plt.subplot(rows, cols, i + 1) plot_path(q0, q1) dist = reeds_shepp.path_length(q0, q1, rho) plt.title('length: {:.2f}'.format(dist)) plt.savefig('fig/demo.png') plt.show() if __name__ == "__main__": q = [(0.0, 0.0, 0.0), (3.0, -4.0, np.pi)] q0 = q[0] q1 = q[1] qd = plot_path(q0, q1) print("asse X:") x = np.round(qd[0], 3) print(x) # PLOT tmax = reeds_shepp.path_length(q[0], q[1], rho) print(tmax) dist = reeds_shepp.path_length(q0, q1, rho) plt.title('length: {:.2f}'.format(dist)) plt.savefig('fig/demo.png') plt.show()
qs = reeds_shepp.path_sample(q0, q1, rho, step_size) xs = [q[0] for q in qs] ys = [q[1] for q in qs] plt.plot(xs, ys, 'b-') plt.plot(xs, ys, 'r.') plot_car(q0) plot_car(q1) plt.axis('equal') def plot_table(cols): rows = ((len(qs)) / cols) for i, (q0, q1) in enumerate(qs): plt.subplot(rows, cols, i + 1) plot_path(q0, q1) dist = reeds_shepp.path_length(q0, q1, rho) plt.title('length: {:.2f}'.format(dist)) plt.savefig('fig/demo.png') plt.show() if __name__ == "__main__": q = [(4.0, 4.0, 0.0), (3.0, 4.0, np.pi / 2)] q0 = q[0] q1 = q[1] plot_path(q0, q1) dist = reeds_shepp.path_length(q0, q1, rho) plt.title('length: {:.2f}'.format(dist)) plt.savefig('fig/demo.png') plt.show() #plot_table(3)
def cost(self, x_from, x_to): # type: (StateNode, StateNode) -> float """calculate the cost from one state to another state""" return reeds_shepp.path_length(x_from.state, x_to.state, 1. / self.maximum_curvature)
def step(self, action): """ Input: list(np.array([1x2]), np.array([1x2])) Output: None Use: Perform a step in the simulation environment. """ # scaled target state scl_truck_trgt, scl_excvtr_trgt = action # unscaled target state truck_trgt = self.truck_low + np.multiply(scl_truck_trgt, self.truck_range) excvtr_trgt = self.excvtr_low + np.multiply(scl_excvtr_trgt, self.excvtr_range) # radian state rd_state = self._get_radian_state() truck_state = rd_state[:3].tolist() truck_trgt = truck_trgt.tolist() excvtr_state = rd_state[3:-1].tolist() excvtr_trgt = excvtr_trgt.tolist() # get reeds-shepp path self.truck_path = self.truck_planner.get_path(truck_state, truck_trgt) self.excvtr_path = self.excvtr_planner.get_path( excvtr_state, excvtr_trgt) # set_trace() self.step_counter = 0 # self.counter += 1 info = [None, None] # execute first 60 points on the path if collides terminate the episode for _ in range(60): if self.if_render: self.render() obsv, flag = self._update_state() if flag: # set_trace() # print('collision occurred') return obsv, [-1000, -1000], [True, True], info # get current scaled state scaled_state = self._get_scaled_state() observation = [scaled_state, scaled_state] # get current radian state rd_state = self._get_radian_state() truck_state = rd_state[:3].tolist() excvtr_pos = rd_state[3:5] arm_ori = rd_state[-1] arm_pos = excvtr_pos + np.array( [130 * math.cos(arm_ori), 130 * math.sin(arm_ori)]) arm_state = np.concatenate((arm_pos, np.pi / 2), axis=None) # calculate reward reward = -reeds_shepp.path_length(tuple(truck_state), tuple(arm_state), self.truck_planner.rho) # check for termination done = False if self._can_load_on_truck(): done = True reward = 8000 # self.success_counter += 1 # print('loads on truck, count: {}'.format(self.success_counter)) return observation, [reward, reward], [done, done], info
def getDistance(self, p1, p2): length = rp.path_length(p1, p2, self.turn_radius) return length