diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/LQRplanner.py new file mode 100644 index 00000000..bca4f291 --- /dev/null +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -0,0 +1,39 @@ +""" + +LQR local path planning module + +author: Atsushi Sakai (@Atsushi_twi) + +""" + +import matplotlib.pyplot as plt + +show_animation = True + + +def LQRplanning(sx, sy, gx, gy): + + rx, ry = [], [] + + return rx, ry + + +def main(): + print(__file__ + " start!!") + + sx = 0.0 + sy = 0.0 + gx = 10.0 + gy = 5.0 + + rx, ry = LQRplanning(sx, sy, gx, gy) + + plt.plot(sx, sy, "xb") + plt.plot(gx, gy, "xb") + plt.plot(rx, ry) + plt.axis("equal") + plt.show() + + +if __name__ == '__main__': + main()