mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-02-02 06:04:56 -05:00
58 lines
1.0 KiB
Python
58 lines
1.0 KiB
Python
"""
|
|
|
|
Quinitc Polynomials Planner
|
|
|
|
author: Atsushi Sakai (@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
import matplotlib.pyplot as plt
|
|
import math
|
|
|
|
|
|
def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga):
|
|
|
|
return None
|
|
|
|
|
|
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
|
|
u"""
|
|
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),
|
|
fc=fc, ec=ec, head_width=width, head_length=width)
|
|
plt.plot(x, y)
|
|
|
|
|
|
def main():
|
|
print(__file__ + " start!!")
|
|
|
|
sx = 10.0
|
|
sy = 10.0
|
|
syaw = math.radians(10.0)
|
|
sv = 0.0 # [m/s]
|
|
sa = 0.0
|
|
gx = 30.0
|
|
gy = 20.0
|
|
gyaw = math.radians(90.0)
|
|
gv = 0.0 # [m/s]
|
|
ga = 0.0
|
|
|
|
quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga)
|
|
|
|
plot_arrow(sx, sy, syaw)
|
|
plot_arrow(gx, gy, gyaw)
|
|
plt.grid(True)
|
|
plt.axis("equal")
|
|
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|