diff --git a/PathPlanning/BezierPath/bezier_path.py b/PathPlanning/BezierPath/bezier_path.py index c12229d7..5188ad81 100644 --- a/PathPlanning/BezierPath/bezier_path.py +++ b/PathPlanning/BezierPath/bezier_path.py @@ -1,109 +1,203 @@ """ -Path Planning with 4 point Beizer curve +Path Planning with Bezier curve. author: Atsushi Sakai(@Atsushi_twi) """ +from __future__ import division, print_function import scipy.special import numpy as np import matplotlib.pyplot as plt -import math show_animation = True -def calc_4point_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset): - D = math.sqrt((sx - ex)**2 + (sy - ey)**2) / offset - cp = np.array( +def calc_4points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset): + """ + Compute control points and path given start and end position. + + :param sx: (float) x-coordinate of the starting point + :param sy: (float) y-coordinate of the starting point + :param syaw: (float) yaw angle at start + :param ex: (float) x-coordinate of the ending point + :param ey: (float) y-coordinate of the ending point + :param eyaw: (float) yaw angle at the end + :param offset: (float) + :return: (numpy array, numpy array) + """ + dist = np.sqrt((sx - ex) ** 2 + (sy - ey) ** 2) / offset + control_points = np.array( [[sx, sy], - [sx + D * math.cos(syaw), sy + D * math.sin(syaw)], - [ex - D * math.cos(eyaw), ey - D * math.sin(eyaw)], + [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)], + [ex - dist * np.cos(eyaw), ey - dist * np.sin(eyaw)], [ex, ey]]) + path = calc_bezier_path(control_points, n_points=100) + + return path, control_points + + +def calc_bezier_path(control_points, n_points=100): + """ + Compute bezier path (trajectory) given control points. + + :param control_points: (numpy array) + :param n_points: (int) number of points in the trajectory + :return: (numpy array) + """ traj = [] - for t in np.linspace(0, 1, 100): - traj.append(bezier(3, t, cp)) - P = np.array(traj) + for t in np.linspace(0, 1, n_points): + traj.append(bezier(t, control_points)) - return P, cp + return np.array(traj) -def bernstein(n, i, t): - return scipy.special.comb(n, i) * t**i * (1 - t)**(n - i) +def bernstein_poly(n, i, t): + """ + Bernstein polynom. + + :param n: (int) polynom degree + :param i: (int) + :param t: (float) + :return: (float) + """ + return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i) -def bezier(n, t, q): - p = np.zeros(2) - for i in range(n + 1): - p += bernstein(n, i, t) * q[i] - return p +def bezier(t, control_points): + """ + Return one point on the bezier curve. + + :param t: (float) number in [0, 1] + :param control_points: (numpy array) + :return: (numpy array) Coordinates of the point + """ + n = len(control_points) - 1 + return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0) + + +def bezier_derivatives_control_points(control_points, n_derivatives): + """ + Compute control points of the successive derivatives of a given bezier curve. + + A derivative of a bezier curve is a bezier curve. + See https://pomax.github.io/bezierinfo/#derivatives + for detailed explanations + + :param control_points: (numpy array) + :param n_derivatives: (int) + e.g., n_derivatives=2 -> compute control points for first and second derivatives + :return: ([numpy array]) + """ + w = {0: control_points} + for i in range(n_derivatives): + n = len(w[i]) + w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) for j in range(n - 1)]) + return w + + +def curvature(dx, dy, ddx, ddy): + """ + Compute curvature at one point given first and second derivatives. + + :param dx: (float) First derivative along x axis + :param dy: (float) + :param ddx: (float) Second derivative along x axis + :param ddy: (float) + :return: (float) + """ + return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2) def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): - u""" - Plot arrow - """ - + """Plot arrow.""" 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), + plt.arrow(x, y, length * np.cos(yaw), length * np.sin(yaw), fc=fc, ec=ec, head_width=width, head_length=width) plt.plot(x, y) def main(): + """Plot an example bezier curve.""" start_x = 10.0 # [m] start_y = 1.0 # [m] - start_yaw = math.radians(180.0) # [rad] + start_yaw = np.radians(180.0) # [rad] end_x = -0.0 # [m] end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] + end_yaw = np.radians(-45.0) # [rad] offset = 3.0 - P, cp = calc_4point_bezier_path( + path, control_points = calc_4points_bezier_path( start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset) - assert P.T[0][0] == start_x, "path is invalid" - assert P.T[1][0] == start_y, "path is invalid" - assert P.T[0][-1] == end_x, "path is invalid" - assert P.T[1][-1] == end_y, "path is invalid" + # Note: alternatively, instead of specifying start and end position + # you can directly define n control points and compute the path: + # control_points = np.array([[5., 1.], [-2.78, 1.], [-11.5, -4.5], [-6., -8.]]) + # path = calc_bezier_path(control_points, n_points=100) + + # Display the tangent, normal and radius of cruvature at a given point + t = 0.86 # Number in [0, 1] + x_target, y_target = bezier(t, control_points) + derivatives_cp = bezier_derivatives_control_points(control_points, 2) + point = bezier(t, control_points) + dt = bezier(t, derivatives_cp[1]) + ddt = bezier(t, derivatives_cp[2]) + # Radius of curvature + radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1]) + # Normalize derivative + dt /= np.linalg.norm(dt, 2) + tangent = np.array([point, point + dt]) + normal = np.array([point, point + [- dt[1], dt[0]]]) + curvature_center = point + np.array([- dt[1], dt[0]]) * radius + circle = plt.Circle(tuple(curvature_center), radius, color=(0, 0.8, 0.8), fill=False, linewidth=1) + + assert path.T[0][0] == start_x, "path is invalid" + assert path.T[1][0] == start_y, "path is invalid" + assert path.T[0][-1] == end_x, "path is invalid" + assert path.T[1][-1] == end_y, "path is invalid" if show_animation: - plt.plot(P.T[0], P.T[1], label="Bezier Path") - plt.plot(cp.T[0], cp.T[1], '--o', label="Control Points") + fig, ax = plt.subplots() + ax.plot(path.T[0], path.T[1], label="Bezier Path") + ax.plot(control_points.T[0], control_points.T[1], '--o', label="Control Points") + ax.plot(x_target, y_target) + ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent") + ax.plot(normal[:, 0], normal[:, 1], label="Normal") + ax.add_artist(circle) plot_arrow(start_x, start_y, start_yaw) plot_arrow(end_x, end_y, end_yaw) - plt.legend() - plt.axis("equal") - plt.grid(True) + ax.legend() + ax.axis("equal") + ax.grid(True) plt.show() def main2(): + """Show the effect of the offset.""" start_x = 10.0 # [m] start_y = 1.0 # [m] - start_yaw = math.radians(180.0) # [rad] + start_yaw = np.radians(180.0) # [rad] end_x = -0.0 # [m] end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] - offset = 3.0 + end_yaw = np.radians(-45.0) # [rad] for offset in np.arange(1.0, 5.0, 1.0): - P, cp = calc_4point_bezier_path( + path, control_points = calc_4points_bezier_path( start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset) - assert P.T[0][0] == start_x, "path is invalid" - assert P.T[1][0] == start_y, "path is invalid" - assert P.T[0][-1] == end_x, "path is invalid" - assert P.T[1][-1] == end_y, "path is invalid" + assert path.T[0][0] == start_x, "path is invalid" + assert path.T[1][0] == start_y, "path is invalid" + assert path.T[0][-1] == end_x, "path is invalid" + assert path.T[1][-1] == end_y, "path is invalid" if show_animation: - plt.plot(P.T[0], P.T[1], label="Offset=" + str(offset)) + plt.plot(path.T[0], path.T[1], label="Offset=" + str(offset)) if show_animation: plot_arrow(start_x, start_y, start_yaw)