pathplanning 0.1.0__py3-none-any.whl

CurvesGenerator/dubins_path.py

@@ -0,0 +1,351 @@
+"""
+Dubins Path
+"""
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.spatial.transform import Rotation as Rot
+import CurvesGenerator.draw as draw
+
+
+# class for PATH element
+class PATH:
+    def __init__(self, L, mode, x, y, yaw):
+        self.L = L  # total path length [float]
+        self.mode = mode  # type of each part of the path [string]
+        self.x = x  # final s positions [m]
+        self.y = y  # final y positions [m]
+        self.yaw = yaw  # final yaw angles [rad]
+
+
+# utils
+def pi_2_pi(theta):
+    while theta > math.pi:
+        theta -= 2.0 * math.pi
+
+    while theta < -math.pi:
+        theta += 2.0 * math.pi
+
+    return theta
+
+
+def mod2pi(theta):
+    return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0)
+
+
+def LSL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b)
+
+    if p_lsl < 0:
+        return None, None, None, ["L", "S", "L"]
+    else:
+        p_lsl = math.sqrt(p_lsl)
+
+    denominate = dist + sin_a - sin_b
+    t_lsl = mod2pi(-alpha + math.atan2(cos_b - cos_a, denominate))
+    q_lsl = mod2pi(beta - math.atan2(cos_b - cos_a, denominate))
+
+    return t_lsl, p_lsl, q_lsl, ["L", "S", "L"]
+
+
+def RSR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a)
+
+    if p_rsr < 0:
+        return None, None, None, ["R", "S", "R"]
+    else:
+        p_rsr = math.sqrt(p_rsr)
+
+    denominate = dist - sin_a + sin_b
+    t_rsr = mod2pi(alpha - math.atan2(cos_a - cos_b, denominate))
+    q_rsr = mod2pi(-beta + math.atan2(cos_a - cos_b, denominate))
+
+    return t_rsr, p_rsr, q_rsr, ["R", "S", "R"]
+
+
+def LSR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b)
+
+    if p_lsr < 0:
+        return None, None, None, ["L", "S", "R"]
+    else:
+        p_lsr = math.sqrt(p_lsr)
+
+    rec = math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr)
+    t_lsr = mod2pi(-alpha + rec)
+    q_lsr = mod2pi(-mod2pi(beta) + rec)
+
+    return t_lsr, p_lsr, q_lsr, ["L", "S", "R"]
+
+
+def RSL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b)
+
+    if p_rsl < 0:
+        return None, None, None, ["R", "S", "L"]
+    else:
+        p_rsl = math.sqrt(p_rsl)
+
+    rec = math.atan2(cos_a + cos_b, dist - sin_a - sin_b) - math.atan2(2.0, p_rsl)
+    t_rsl = mod2pi(alpha - rec)
+    q_rsl = mod2pi(beta - rec)
+
+    return t_rsl, p_rsl, q_rsl, ["R", "S", "L"]
+
+
+def RLR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0
+
+    if abs(rec) > 1.0:
+        return None, None, None, ["R", "L", "R"]
+
+    p_rlr = mod2pi(2 * math.pi - math.acos(rec))
+    t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0))
+    q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr))
+
+    return t_rlr, p_rlr, q_rlr, ["R", "L", "R"]
+
+
+def LRL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0
+
+    if abs(rec) > 1.0:
+        return None, None, None, ["L", "R", "L"]
+
+    p_lrl = mod2pi(2 * math.pi - math.acos(rec))
+    t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)
+    q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl))
+
+    return t_lrl, p_lrl, q_lrl, ["L", "R", "L"]
+
+
+def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
+    if m == "S":
+        px[ind] = ox + l / maxc * math.cos(oyaw)
+        py[ind] = oy + l / maxc * math.sin(oyaw)
+        pyaw[ind] = oyaw
+    else:
+        ldx = math.sin(l) / maxc
+        if m == "L":
+            ldy = (1.0 - math.cos(l)) / maxc
+        elif m == "R":
+            ldy = (1.0 - math.cos(l)) / (-maxc)
+
+        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
+        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
+        px[ind] = ox + gdx
+        py[ind] = oy + gdy
+
+    if m == "L":
+        pyaw[ind] = oyaw + l
+    elif m == "R":
+        pyaw[ind] = oyaw - l
+
+    if l > 0.0:
+        directions[ind] = 1
+    else:
+        directions[ind] = -1
+
+    return px, py, pyaw, directions
+
+
+def generate_local_course(L, lengths, mode, maxc, step_size):
+    point_num = int(L / step_size) + len(lengths) + 3
+
+    px = [0.0 for _ in range(point_num)]
+    py = [0.0 for _ in range(point_num)]
+    pyaw = [0.0 for _ in range(point_num)]
+    directions = [0 for _ in range(point_num)]
+    ind = 1
+
+    if lengths[0] > 0.0:
+        directions[0] = 1
+    else:
+        directions[0] = -1
+
+    if lengths[0] > 0.0:
+        d = step_size
+    else:
+        d = -step_size
+
+    ll = 0.0
+
+    for m, l, i in zip(mode, lengths, range(len(mode))):
+        if l > 0.0:
+            d = step_size
+        else:
+            d = -step_size
+
+        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
+
+        ind -= 1
+        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
+            pd = -d - ll
+        else:
+            pd = d - ll
+
+        while abs(pd) <= abs(l):
+            ind += 1
+            px, py, pyaw, directions = \
+                interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+            pd += d
+
+        ll = l - pd - d  # calc remain length
+
+        ind += 1
+        px, py, pyaw, directions = \
+            interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+
+    if len(px) <= 1:
+        return [], [], [], []
+
+    # remove unused data
+    while len(px) >= 1 and px[-1] == 0.0:
+        px.pop()
+        py.pop()
+        pyaw.pop()
+        directions.pop()
+
+    return px, py, pyaw, directions
+
+
+def planning_from_origin(gx, gy, gyaw, curv, step_size):
+    D = math.hypot(gx, gy)
+    d = D * curv
+
+    theta = mod2pi(math.atan2(gy, gx))
+    alpha = mod2pi(-theta)
+    beta = mod2pi(gyaw - theta)
+
+    planners = [LSL, RSR, LSR, RSL, RLR, LRL]
+
+    best_cost = float("inf")
+    bt, bp, bq, best_mode = None, None, None, None
+
+    for planner in planners:
+        t, p, q, mode = planner(alpha, beta, d)
+
+        if t is None:
+            continue
+
+        cost = (abs(t) + abs(p) + abs(q))
+        if best_cost > cost:
+            bt, bp, bq, best_mode = t, p, q, mode
+            best_cost = cost
+    lengths = [bt, bp, bq]
+
+    x_list, y_list, yaw_list, directions = generate_local_course(
+        sum(lengths), lengths, best_mode, curv, step_size)
+
+    return x_list, y_list, yaw_list, best_mode, best_cost
+
+
+def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1):
+    gx = gx - sx
+    gy = gy - sy
+
+    l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2]
+    le_xy = np.stack([gx, gy]).T @ l_rot
+    le_yaw = gyaw - syaw
+
+    lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin(
+        le_xy[0], le_xy[1], le_yaw, curv, step_size)
+
+    rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2]
+    converted_xy = np.stack([lp_x, lp_y]).T @ rot
+    x_list = converted_xy[:, 0] + sx
+    y_list = converted_xy[:, 1] + sy
+    yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw]
+
+    return PATH(lengths, mode, x_list, y_list, yaw_list)
+
+
+def main():
+    # choose states pairs: (s, y, yaw)
+    # simulation-1
+    states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
+              (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
+
+    # simulation-2
+    # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
+    #           (35, 10, 180), (32, -10, 180), (5, -12, 90)]
+
+    max_c = 0.25  # max curvature
+    path_x, path_y, yaw = [], [], []
+
+    for i in range(len(states) - 1):
+        s_x = states[i][0]
+        s_y = states[i][1]
+        s_yaw = np.deg2rad(states[i][2])
+        g_x = states[i + 1][0]
+        g_y = states[i + 1][1]
+        g_yaw = np.deg2rad(states[i + 1][2])
+
+        path_i = calc_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c)
+
+        for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw):
+            path_x.append(x)
+            path_y.append(y)
+            yaw.append(iyaw)
+
+    # animation
+    plt.ion()
+    plt.figure(1)
+
+    for i in range(len(path_x)):
+        plt.clf()
+        plt.plot(path_x, path_y, linewidth=1, color='gray')
+
+        for x, y, theta in states:
+            draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
+
+        draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)
+
+        plt.axis("equal")
+        plt.title("Simulation of Dubins Path")
+        plt.axis([-10, 42, -20, 20])
+        plt.draw()
+        plt.pause(0.001)
+
+    plt.pause(1)
+
+
+if __name__ == '__main__':
+    main()

CurvesGenerator/quartic_polynomial.py

@@ -0,0 +1,43 @@
+"""
+Quartic Polynomial
+"""
+
+import numpy as np
+
+
+class QuarticPolynomial:
+    def __init__(self, x0, v0, a0, v1, a1, T):
+        A = np.array([[3 * T ** 2, 4 * T ** 3],
+                      [6 * T, 12 * T ** 2]])
+        b = np.array([v1 - v0 - a0 * T,
+                      a1 - a0])
+        X = np.linalg.solve(A, b)
+
+        self.a0 = x0
+        self.a1 = v0
+        self.a2 = a0 / 2.0
+        self.a3 = X[0]
+        self.a4 = X[1]
+
+    def calc_xt(self, t):
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
+             self.a3 * t ** 3 + self.a4 * t ** 4
+
+        return xt
+
+    def calc_dxt(self, t):
+        xt = self.a1 + 2 * self.a2 * t + \
+             3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
+
+        return xt
+
+    def calc_ddxt(self, t):
+        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
+
+        return xt
+
+    def calc_dddxt(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t
+
+        return xt
+

CurvesGenerator/quintic_polynomial.py

@@ -0,0 +1,143 @@
+"""
+Quintic Polynomial
+"""
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+
+import draw
+
+
+class QuinticPolynomial:
+    def __init__(self, x0, v0, a0, x1, v1, a1, T):
+        A = np.array([[T ** 3, T ** 4, T ** 5],
+                      [3 * T ** 2, 4 * T ** 3, 5 * T ** 4],
+                      [6 * T, 12 * T ** 2, 20 * T ** 3]])
+        b = np.array([x1 - x0 - v0 * T - a0 * T ** 2 / 2,
+                      v1 - v0 - a0 * T,
+                      a1 - a0])
+        X = np.linalg.solve(A, b)
+
+        self.a0 = x0
+        self.a1 = v0
+        self.a2 = a0 / 2.0
+        self.a3 = X[0]
+        self.a4 = X[1]
+        self.a5 = X[2]
+
+    def calc_xt(self, t):
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
+                self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5
+
+        return xt
+
+    def calc_dxt(self, t):
+        dxt = self.a1 + 2 * self.a2 * t + \
+            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4
+
+        return dxt
+
+    def calc_ddxt(self, t):
+        ddxt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3
+
+        return ddxt
+
+    def calc_dddxt(self, t):
+        dddxt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2
+
+        return dddxt
+
+
+class Trajectory:
+    def __init__(self):
+        self.t = []
+        self.x = []
+        self.y = []
+        self.yaw = []
+        self.v = []
+        self.a = []
+        self.jerk = []
+
+
+def simulation():
+    sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1
+    gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1
+
+    MAX_ACCEL = 1.0  # max accel [m/s2]
+    MAX_JERK = 0.5  # max jerk [m/s3]
+    dt = 0.1  # T tick [s]
+
+    MIN_T = 5
+    MAX_T = 100
+    T_STEP = 5
+
+    sv_x = sv * math.cos(syaw)
+    sv_y = sv * math.sin(syaw)
+    gv_x = gv * math.cos(gyaw)
+    gv_y = gv * math.sin(gyaw)
+
+    sa_x = sa * math.cos(syaw)
+    sa_y = sa * math.sin(syaw)
+    ga_x = ga * math.cos(gyaw)
+    ga_y = ga * math.sin(gyaw)
+
+    path = Trajectory()
+
+    for T in np.arange(MIN_T, MAX_T, T_STEP):
+        path = Trajectory()
+        xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T)
+        yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T)
+
+        for t in np.arange(0.0, T + dt, dt):
+            path.t.append(t)
+            path.x.append(xqp.calc_xt(t))
+            path.y.append(yqp.calc_xt(t))
+
+            vx = xqp.calc_dxt(t)
+            vy = yqp.calc_dxt(t)
+            path.v.append(np.hypot(vx, vy))
+            path.yaw.append(math.atan2(vy, vx))
+
+            ax = xqp.calc_ddxt(t)
+            ay = yqp.calc_ddxt(t)
+            a = np.hypot(ax, ay)
+
+            if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0:
+                a *= -1
+            path.a.append(a)
+
+            jx = xqp.calc_dddxt(t)
+            jy = yqp.calc_dddxt(t)
+            j = np.hypot(jx, jy)
+
+            if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0:
+                j *= -1
+            path.jerk.append(j)
+
+        if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(path.jerk)) <= MAX_JERK:
+            break
+
+    print("t_len: ", path.t, "s")
+    print("max_v: ", max(path.v), "m/s")
+    print("max_a: ", max(np.abs(path.a)), "m/s2")
+    print("max_jerk: ", max(np.abs(path.jerk)), "m/s3")
+
+    for i in range(len(path.t)):
+        plt.cla()
+        plt.gcf().canvas.mpl_connect('key_release_event',
+                                     lambda event: [exit(0) if event.key == 'escape' else None])
+        plt.axis("equal")
+        plt.plot(path.x, path.y, linewidth=2, color='gray')
+        draw.Car(sx, sy, syaw, 1.5, 3)
+        draw.Car(gx, gy, gyaw, 1.5, 3)
+        draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3)
+        plt.title("Quintic Polynomial Curves")
+        plt.grid(True)
+        plt.pause(0.001)
+
+    plt.show()
+
+
+if __name__ == '__main__':
+    simulation()