diff --git a/PathPlanning/CRRRTStar/cr_rrt_star_car.py b/PathPlanning/CRRRTStar/cr_rrt_star_car.py new file mode 100644 index 00000000..f5bc4276 --- /dev/null +++ b/PathPlanning/CRRRTStar/cr_rrt_star_car.py @@ -0,0 +1,393 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +""" +@brief: Path Planning Sample Code with RRT for car like robot. + +@author: AtsushiSakai(@Atsushi_twi) + +@license: MIT + +""" + +import random +import math +import copy +import numpy as np +import dubins_path_planning +import pure_pursuit +import unicycle_model + + +class RRT(): + u""" + Class for RRT Planning + """ + + def __init__(self, start, goal, obstacleList, randArea, + goalSampleRate=10, maxIter=100): + u""" + Setting Parameter + + start:Start Position [x,y] + goal:Goal Position [x,y] + obstacleList:obstacle Positions [[x,y,size],...] + randArea:Ramdom Samping Area [min,max] + + """ + self.start = Node(start[0], start[1], start[2]) + self.end = Node(goal[0], goal[1], goal[2]) + self.minrand = randArea[0] + self.maxrand = randArea[1] + self.goalSampleRate = goalSampleRate + self.obstacleList = obstacleList + self.maxIter = maxIter + + def Planning(self, animation=True): + u""" + Pathplanning + + animation: flag for animation on or off + """ + + self.nodeList = [self.start] + for i in range(self.maxIter): + rnd = self.get_random_point() + nind = self.GetNearestListIndex(self.nodeList, rnd) + + newNode = self.steer(rnd, nind) + # print(newNode.cost) + + if self.CollisionCheck(newNode, obstacleList): + nearinds = self.find_near_nodes(newNode) + newNode = self.choose_parent(newNode, nearinds) + self.nodeList.append(newNode) + self.rewire(newNode, nearinds) + + if animation and i % 5 == 0: + self.DrawGraph(rnd=rnd) + matplotrecorder.save_frame() # save each frame + + # generate coruse + path_indexs = self.get_best_last_indexs() + + # pure pursuit tracking + for ind in path_indexs: + path = self.gen_final_course(ind) + + flag, x, y, yaw, v, t = self.check_tracking_path_is_feasible(path) + + if flag: + print("feasible path is found") + break + + return x, y, yaw, v, t + + def check_tracking_path_is_feasible(self, path): + print("check_tracking_path_is_feasible") + + init_speed = 0.0 + target_speed = 10.0 / 3.6 + + path = np.matrix(path[::-1]) + + state = unicycle_model.State( + x=self.start.x, y=self.start.y, yaw=self.start.yaw, v=init_speed) + + target_ind = pure_pursuit.calc_nearest_index( + state, path[:, 0], path[:, 1]) + + lastIndex = len(path[:, 0]) - 2 + + x = [state.x] + y = [state.y] + yaw = [state.yaw] + v = [state.v] + t = [0.0] + a = [0.0] + d = [0.0] + time = 0.0 + + while lastIndex > target_ind: + print(lastIndex, target_ind) + ai = pure_pursuit.PIDControl(target_speed, state.v) + di, target_ind = pure_pursuit.pure_pursuit_control( + state, path[:, 0], path[:, 1], target_ind) + state = unicycle_model.update(state, ai, di) + + time = time + unicycle_model.dt + + x.append(state.x) + y.append(state.y) + yaw.append(state.yaw) + v.append(state.v) + t.append(time) + a.append(ai) + d.append(di) + + if self.CollisionCheckWithXY(path[:, 0], path[:, 1], self.obstacleList): + # print("OK") + return True, x, y, yaw, v, t, a, d + else: + # print("NG") + return False, x, y, yaw, v, t, a, d + + # plt.plot(x, y, '-r') + # plt.plot(path[:, 0], path[:, 1], '-g') + # plt.show() + + # return True + + def choose_parent(self, newNode, nearinds): + if len(nearinds) == 0: + return newNode + + dlist = [] + for i in nearinds: + tNode = self.steer(newNode, i) + if self.CollisionCheck(tNode, obstacleList): + dlist.append(tNode.cost) + else: + dlist.append(float("inf")) + + mincost = min(dlist) + minind = nearinds[dlist.index(mincost)] + + if mincost == float("inf"): + print("mincost is inf") + return newNode + + newNode = self.steer(newNode, minind) + + return newNode + + def pi_2_pi(self, angle): + while(angle >= math.pi): + angle = angle - 2.0 * math.pi + + while(angle <= -math.pi): + angle = angle + 2.0 * math.pi + + return angle + + def steer(self, rnd, nind): + # print(rnd) + curvature = 1.0 + + nearestNode = self.nodeList[nind] + + px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning( + nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature) + + newNode = copy.deepcopy(nearestNode) + newNode.x = px[-1] + newNode.y = py[-1] + newNode.yaw = pyaw[-1] + + newNode.path_x = px + newNode.path_y = py + newNode.path_yaw = pyaw + newNode.cost += clen + newNode.parent = nind + + return newNode + + def get_random_point(self): + + if random.randint(0, 100) > self.goalSampleRate: + rnd = [random.uniform(self.minrand, self.maxrand), + random.uniform(self.minrand, self.maxrand), + random.uniform(-math.pi, math.pi) + ] + else: # goal point sampling + rnd = [self.end.x, self.end.y, self.end.yaw] + + node = Node(rnd[0], rnd[1], rnd[2]) + + return node + + def get_best_last_indexs(self): + # print("get_best_last_index") + + YAWTH = math.radians(1.0) + XYTH = 0.5 + + goalinds = [] + for (i, node) in enumerate(self.nodeList): + if self.calc_dist_to_goal(node.x, node.y) <= XYTH: + goalinds.append(i) + + # angle check + fgoalinds = [] + for i in goalinds: + if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH: + fgoalinds.append(i) + + return fgoalinds + + # mincost = min([self.nodeList[i].cost for i in fgoalinds]) + # for i in fgoalinds: + # if self.nodeList[i].cost == mincost: + # return i + + # return None + + def gen_final_course(self, goalind): + path = [[self.end.x, self.end.y]] + while self.nodeList[goalind].parent is not None: + node = self.nodeList[goalind] + for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)): + path.append([ix, iy]) + # path.append([node.x, node.y]) + goalind = node.parent + path.append([self.start.x, self.start.y]) + return path + + def calc_dist_to_goal(self, x, y): + return np.linalg.norm([x - self.end.x, y - self.end.y]) + + def find_near_nodes(self, newNode): + nnode = len(self.nodeList) + r = 50.0 * math.sqrt((math.log(nnode) / nnode)) + # r = self.expandDis * 5.0 + dlist = [(node.x - newNode.x) ** 2 + + (node.y - newNode.y) ** 2 + + (node.yaw - newNode.yaw) ** 2 + for node in self.nodeList] + nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] + return nearinds + + def rewire(self, newNode, nearinds): + + nnode = len(self.nodeList) + + for i in nearinds: + nearNode = self.nodeList[i] + tNode = self.steer(nearNode, nnode - 1) + + obstacleOK = self.CollisionCheck(tNode, obstacleList) + imporveCost = nearNode.cost > tNode.cost + + if obstacleOK and imporveCost: + # print("rewire") + self.nodeList[i] = tNode + + def DrawGraph(self, rnd=None): + u""" + Draw Graph + """ + import matplotlib.pyplot as plt + # plt.clf() + if rnd is not None: + plt.plot(rnd.x, rnd.y, "^k") + for node in self.nodeList: + if node.parent is not None: + plt.plot(node.path_x, node.path_y, "-g") + # plt.plot([node.x, self.nodeList[node.parent].x], [ + # node.y, self.nodeList[node.parent].y], "-g") + + for (ox, oy, size) in obstacleList: + plt.plot(ox, oy, "ok", ms=30 * size) + + dubins_path_planning.plot_arrow( + self.start.x, self.start.y, self.start.yaw) + dubins_path_planning.plot_arrow( + self.end.x, self.end.y, self.end.yaw) + + plt.axis([-2, 15, -2, 15]) + plt.grid(True) + plt.pause(0.01) + + # plt.show() + # input() + + def GetNearestListIndex(self, nodeList, rnd): + dlist = [(node.x - rnd.x) ** 2 + + (node.y - rnd.y) ** 2 + + (node.yaw - rnd.yaw) ** 2 for node in nodeList] + minind = dlist.index(min(dlist)) + + return minind + + def CollisionCheck(self, node, obstacleList): + + for (ox, oy, size) in obstacleList: + for (ix, iy) in zip(node.path_x, node.path_y): + dx = ox - ix + dy = oy - iy + d = dx * dx + dy * dy + if d <= size ** 2: + return False # collision + + return True # safe + + def CollisionCheckWithXY(self, x, y, obstacleList): + + for (ox, oy, size) in obstacleList: + for (ix, iy) in zip(x, y): + dx = ox - ix + dy = oy - iy + d = dx * dx + dy * dy + if d <= size ** 2: + return False # collision + + return True # safe + + +class Node(): + u""" + RRT Node + """ + + def __init__(self, x, y, yaw): + self.x = x + self.y = y + self.yaw = yaw + self.path_x = [] + self.path_y = [] + self.path_yaw = [] + self.cost = 0.0 + self.parent = None + + +if __name__ == '__main__': + print("Start rrt start planning") + import matplotlib.pyplot as plt + import matplotrecorder + matplotrecorder.donothing = True + + # ====Search Path with RRT==== + obstacleList = [ + (5, 5, 1), + (3, 6, 2), + (3, 8, 2), + (3, 10, 2), + (7, 5, 2), + (9, 5, 2) + ] # [x,y,size(radius)] + + # Set Initial parameters + start = [0.0, 0.0, math.radians(0.0)] + goal = [10.0, 10.0, math.radians(0.0)] + + rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList) + x, y, yaw, v, t = rrt.Planning(animation=False) + + flg, ax = plt.subplots(1) + # Draw final path + rrt.DrawGraph() + plt.plot(x, y, '-r') + plt.grid(True) + plt.pause(0.001) + + for i in range(10): + matplotrecorder.save_frame() # save each frame + + flg, ax = plt.subplots(1) + plt.plot(t, yaw, '-r') + + flg, ax = plt.subplots(1) + plt.plot(t, v, '-r') + + plt.show() + + matplotrecorder.save_movie("animation.gif", 0.1) diff --git a/PathPlanning/CRRRTStar/dubins_path_planning.py b/PathPlanning/CRRRTStar/dubins_path_planning.py new file mode 100644 index 00000000..a5bc8bbd --- /dev/null +++ b/PathPlanning/CRRRTStar/dubins_path_planning.py @@ -0,0 +1,313 @@ +#! /usr/bin/python +# -*- coding: utf-8 -*- +""" + +Dubins path planner sample code + +author Atsushi Sakai(@Atsushi_twi) + +License MIT + +""" +import math + + +def mod2pi(theta): + return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi) + + +def pi_2_pi(angle): + while(angle >= math.pi): + angle = angle - 2.0 * math.pi + + while(angle <= -math.pi): + angle = angle + 2.0 * math.pi + + return angle + + +def LSL(alpha, beta, d): + sa = math.sin(alpha) + sb = math.sin(beta) + ca = math.cos(alpha) + cb = math.cos(beta) + c_ab = math.cos(alpha - beta) + + tmp0 = d + sa - sb + + mode = ["L", "S", "L"] + p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb)) + if p_squared < 0: + return None, None, None, mode + tmp1 = math.atan2((cb - ca), tmp0) + t = mod2pi(-alpha + tmp1) + p = math.sqrt(p_squared) + q = mod2pi(beta - tmp1) + # print(math.degrees(t), p, math.degrees(q)) + + return t, p, q, mode + + +def RSR(alpha, beta, d): + sa = math.sin(alpha) + sb = math.sin(beta) + ca = math.cos(alpha) + cb = math.cos(beta) + c_ab = math.cos(alpha - beta) + + tmp0 = d - sa + sb + mode = ["R", "S", "R"] + p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa)) + if p_squared < 0: + return None, None, None, mode + tmp1 = math.atan2((ca - cb), tmp0) + t = mod2pi(alpha - tmp1) + p = math.sqrt(p_squared) + q = mod2pi(-beta + tmp1) + + return t, p, q, mode + + +def LSR(alpha, beta, d): + sa = math.sin(alpha) + sb = math.sin(beta) + ca = math.cos(alpha) + cb = math.cos(beta) + c_ab = math.cos(alpha - beta) + + p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb)) + mode = ["L", "S", "R"] + if p_squared < 0: + return None, None, None, mode + p = math.sqrt(p_squared) + tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p) + t = mod2pi(-alpha + tmp2) + q = mod2pi(-mod2pi(beta) + tmp2) + + return t, p, q, mode + + +def RSL(alpha, beta, d): + sa = math.sin(alpha) + sb = math.sin(beta) + ca = math.cos(alpha) + cb = math.cos(beta) + c_ab = math.cos(alpha - beta) + + p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb)) + mode = ["R", "S", "L"] + if p_squared < 0: + return None, None, None, mode + p = math.sqrt(p_squared) + tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p) + t = mod2pi(alpha - tmp2) + q = mod2pi(beta - tmp2) + + return t, p, q, mode + + +def RLR(alpha, beta, d): + sa = math.sin(alpha) + sb = math.sin(beta) + ca = math.cos(alpha) + cb = math.cos(beta) + c_ab = math.cos(alpha - beta) + + mode = ["R", "L", "R"] + tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0 + if abs(tmp_rlr) > 1.0: + return None, None, None, mode + + p = mod2pi(2 * math.pi - math.acos(tmp_rlr)) + t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0)) + q = mod2pi(alpha - beta - t + mod2pi(p)) + return t, p, q, mode + + +def LRL(alpha, beta, d): + sa = math.sin(alpha) + sb = math.sin(beta) + ca = math.cos(alpha) + cb = math.cos(beta) + c_ab = math.cos(alpha - beta) + + mode = ["L", "R", "L"] + tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8. + if abs(tmp_lrl) > 1: + return None, None, None, mode + p = mod2pi(2 * math.pi - math.acos(tmp_lrl)) + t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.) + q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p)) + + return t, p, q, mode + + +def dubins_path_planning_from_origin(ex, ey, eyaw, c): + # nomalize + dx = ex + dy = ey + D = math.sqrt(dx ** 2.0 + dy ** 2.0) + d = D / c + # print(dx, dy, D, d) + + theta = mod2pi(math.atan2(dy, dx)) + alpha = mod2pi(- theta) + beta = mod2pi(eyaw - theta) + # print(theta, alpha, beta, d) + + planners = [LSL, RSR, LSR, RSL, RLR, LRL] + + bcost = float("inf") + bt, bp, bq, bmode = None, None, None, None + + for planner in planners: + t, p, q, mode = planner(alpha, beta, d) + if t is None: + # print("".join(mode) + " cannot generate path") + continue + + cost = (abs(t) + abs(p) + abs(q)) + if bcost > cost: + bt, bp, bq, bmode = t, p, q, mode + bcost = cost + + # print(bmode) + px, py, pyaw = generate_course([bt, bp, bq], bmode, c) + + return px, py, pyaw, bmode, bcost + + +def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): + """ + Dubins path plannner + + input: + sx x position of start point [m] + sy y position of start point [m] + syaw yaw angle of start point [rad] + ex x position of end point [m] + ey y position of end point [m] + eyaw yaw angle of end point [rad] + c curvature [1/m] + + output: + px + py + pyaw + mode + + """ + + ex = ex - sx + ey = ey - sy + + lex = math.cos(syaw) * ex + math.sin(syaw) * ey + ley = - math.sin(syaw) * ex + math.cos(syaw) * ey + leyaw = eyaw - syaw + + lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( + lex, ley, leyaw, c) + + px = [math.cos(-syaw) * x + math.sin(-syaw) * + y + sx for x, y in zip(lpx, lpy)] + py = [- math.sin(-syaw) * x + math.cos(-syaw) * + y + sy for x, y in zip(lpx, lpy)] + pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] + # print(syaw) + # pyaw = lpyaw + + # plt.plot(pyaw, "-r") + # plt.plot(lpyaw, "-b") + # plt.plot(eyaw, "*r") + # plt.plot(syaw, "*b") + # plt.show() + + return px, py, pyaw, mode, clen + + +def generate_course(length, mode, c): + + px = [0.0] + py = [0.0] + pyaw = [0.0] + + for m, l in zip(mode, length): + pd = 0.0 + if m is "S": + d = 1.0 / c + else: # turning couse + d = math.radians(3.0) + + while pd < abs(l - d): + # print(pd, l) + px.append(px[-1] + d * c * math.cos(pyaw[-1])) + py.append(py[-1] + d * c * math.sin(pyaw[-1])) + + if m is "L": # left turn + pyaw.append(pyaw[-1] + d) + elif m is "S": # Straight + pyaw.append(pyaw[-1]) + elif m is "R": # right turn + pyaw.append(pyaw[-1] - d) + pd += d + else: + d = l - pd + px.append(px[-1] + d * c * math.cos(pyaw[-1])) + py.append(py[-1] + d * c * math.sin(pyaw[-1])) + + if m is "L": # left turn + pyaw.append(pyaw[-1] + d) + elif m is "S": # Straight + pyaw.append(pyaw[-1]) + elif m is "R": # right turn + pyaw.append(pyaw[-1] - d) + pd += d + + return px, py, pyaw + + +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): + u""" + Plot arrow + """ + import matplotlib.pyplot as plt + + if not isinstance(x, float): + for (ix, iy, iyaw) in zip(x, y, yaw): + plot_arrow(ix, iy, iyaw) + else: + plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), + fc=fc, ec=ec, head_width=width, head_length=width) + plt.plot(x, y) + + +if __name__ == '__main__': + print("Dubins path planner sample start!!") + import matplotlib.pyplot as plt + + start_x = 1.0 # [m] + start_y = 1.0 # [m] + start_yaw = math.radians(45.0) # [rad] + + end_x = -3.0 # [m] + end_y = -3.0 # [m] + end_yaw = math.radians(-45.0) # [rad] + + curvature = 1.0 + + px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw, + end_x, end_y, end_yaw, curvature) + + plt.plot(px, py, label="final course " + "".join(mode)) + + # plotting + plot_arrow(start_x, start_y, start_yaw) + plot_arrow(end_x, end_y, end_yaw) + + # for (ix, iy, iyaw) in zip(px, py, pyaw): + # plot_arrow(ix, iy, iyaw, fc="b") + + plt.legend() + plt.grid(True) + plt.axis("equal") + plt.show() diff --git a/PathPlanning/CRRRTStar/matplotrecorder.py b/PathPlanning/CRRRTStar/matplotrecorder.py new file mode 100644 index 00000000..7d5ae858 --- /dev/null +++ b/PathPlanning/CRRRTStar/matplotrecorder.py @@ -0,0 +1,59 @@ +""" + A simple Python module for recording matplotlib animation + + This tool use convert command of ImageMagick + + author: Atsushi Sakai +""" + +import matplotlib.pyplot as plt +import subprocess + +iframe = 0 +donothing = False + + +def save_frame(): + """ + Save a frame for movie + """ + + if not donothing: + global iframe + plt.savefig("recoder" + '{0:04d}'.format(iframe) + '.png') + iframe += 1 + + +def save_movie(fname, d_pause): + """ + Save movie as gif + """ + if not donothing: + cmd = "convert -delay " + str(int(d_pause * 100)) + \ + " recoder*.png " + fname + subprocess.call(cmd, shell=True) + cmd = "rm recoder*.png" + subprocess.call(cmd, shell=True) + + +if __name__ == '__main__': + print("A sample recording start") + import math + + time = range(50) + + x1 = [math.cos(t / 10.0) for t in time] + y1 = [math.sin(t / 10.0) for t in time] + x2 = [math.cos(t / 10.0) + 2 for t in time] + y2 = [math.sin(t / 10.0) + 2 for t in time] + + for ix1, iy1, ix2, iy2 in zip(x1, y1, x2, y2): + plt.plot(ix1, iy1, "xr") + plt.plot(ix2, iy2, "xb") + plt.axis("equal") + plt.pause(0.1) + + save_frame() # save each frame + + save_movie("animation.gif", 0.1) + # save_movie("animation.mp4", 0.1) diff --git a/PathPlanning/CRRRTStar/pure_pursuit.py b/PathPlanning/CRRRTStar/pure_pursuit.py new file mode 100644 index 00000000..30603e04 --- /dev/null +++ b/PathPlanning/CRRRTStar/pure_pursuit.py @@ -0,0 +1,132 @@ +#! /usr/bin/python +# -*- coding: utf-8 -*- +u""" + +Path tracking simulation with pure pursuit steering control and PID speed control. + +author: Atsushi Sakai + +""" +import numpy as np +import math +import matplotlib.pyplot as plt +import unicycle_model + +Kp = 1.0 # speed propotional gain +Lf = 1.0 # look-ahead distance + + +def PIDControl(target, current): + a = Kp * (target - current) + + return a + + +def pure_pursuit_control(state, cx, cy, pind): + + if state.v >= 0: + ind = calc_nearest_index(state, cx[pind:], cy[pind:]) + else: + ind = calc_nearest_index(state, cx[:pind + 1], cy[:pind + 1]) + + if state.v >= 0: + ind = ind + pind + + tx = cx[ind] + ty = cy[ind] + + alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw + + if state.v < 0: # back + if alpha > 0: + alpha = math.pi - alpha + else: + alpha = math.pi + alpha + + delta = math.atan2(2.0 * unicycle_model.L * math.sin(alpha) / Lf, 1.0) + + if state.v < 0: # back + delta = delta * -1.0 + + return delta, ind + + +def calc_nearest_index(state, cx, cy): + dx = [state.x - icx for icx in cx] + dy = [state.y - icy for icy in cy] + + d = [abs(math.sqrt(idx ** 2 + idy ** 2) - + Lf) for (idx, idy) in zip(dx, dy)] + + ind = d.index(min(d)) + + return ind + + +def main(): + # target course + cx = np.arange(0, 50, 0.1) + cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx] + target_speed = 30.0 / 3.6 + + T = 15.0 # max simulation time + + state = unicycle_model.State(x=-1.0, y=-5.0, yaw=0.0, v=0.0) + # state = unicycle_model.State(x=-1.0, y=-5.0, yaw=0.0, v=-30.0 / 3.6) + # state = unicycle_model.State(x=10.0, y=5.0, yaw=0.0, v=-30.0 / 3.6) + # state = unicycle_model.State( + # x=3.0, y=5.0, yaw=math.radians(-40.0), v=-10.0 / 3.6) + # state = unicycle_model.State( + # x=3.0, y=5.0, yaw=math.radians(40.0), v=50.0 / 3.6) + + lastIndex = len(cx) - 1 + time = 0.0 + x = [state.x] + y = [state.y] + yaw = [state.yaw] + v = [state.v] + t = [0.0] + target_ind = calc_nearest_index(state, cx, cy) + + while T >= time and lastIndex > target_ind: + ai = PIDControl(target_speed, state.v) + di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) + state = unicycle_model.update(state, ai, di) + + time = time + unicycle_model.dt + + x.append(state.x) + y.append(state.y) + yaw.append(state.yaw) + v.append(state.v) + t.append(time) + + # plt.cla() + # plt.plot(cx, cy, ".r", label="course") + # plt.plot(x, y, "-b", label="trajectory") + # plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") + # plt.axis("equal") + # plt.grid(True) + # plt.pause(0.1) + # input() + + flg, ax = plt.subplots(1) + plt.plot(cx, cy, ".r", label="course") + plt.plot(x, y, "-b", label="trajectory") + plt.legend() + plt.xlabel("x[m]") + plt.ylabel("y[m]") + plt.axis("equal") + plt.grid(True) + + flg, ax = plt.subplots(1) + plt.plot(t, [iv * 3.6 for iv in v], "-r") + plt.xlabel("Time[s]") + plt.ylabel("Speed[km/h]") + plt.grid(True) + plt.show() + + +if __name__ == '__main__': + print("Pure pursuit path tracking simulation start") + main() diff --git a/PathPlanning/CRRRTStar/unicycle_model.py b/PathPlanning/CRRRTStar/unicycle_model.py new file mode 100644 index 00000000..27925e3b --- /dev/null +++ b/PathPlanning/CRRRTStar/unicycle_model.py @@ -0,0 +1,68 @@ +#! /usr/bin/python +# -*- coding: utf-8 -*- +""" + + +author Atsushi Sakai +""" + +import math + +dt = 0.1 # [s] +L = 2.9 # [m] + + +class State: + + def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): + self.x = x + self.y = y + self.yaw = yaw + self.v = v + + +def update(state, a, delta): + + state.x = state.x + state.v * math.cos(state.yaw) * dt + state.y = state.y + state.v * math.sin(state.yaw) * dt + state.yaw = state.yaw + state.v / L * math.tan(delta) * dt + state.v = state.v + a * dt + + return state + + +if __name__ == '__main__': + print("start unicycle simulation") + import matplotlib.pyplot as plt + + T = 100 + a = [1.0] * T + delta = [math.radians(1.0)] * T + # print(delta) + # print(a, delta) + + state = State() + + x = [] + y = [] + yaw = [] + v = [] + + for (ai, di) in zip(a, delta): + state = update(state, ai, di) + + x.append(state.x) + y.append(state.y) + yaw.append(state.yaw) + v.append(state.v) + + flg, ax = plt.subplots(1) + plt.plot(x, y) + plt.axis("equal") + plt.grid(True) + + flg, ax = plt.subplots(1) + plt.plot(v) + plt.grid(True) + + plt.show()