mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 18:28:06 -05:00
Merge pull request #72 from araffin/bezier-doc
Add comments + support for n point bezier curve
This commit is contained in:
Atsushi Sakai
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user