Merge remote-tracking branch 'origin/master'

Author: gbmxdwmssj

PathPlanning/BezierPath/Figure_1.png

@@ -0,0 +1,103 @@
+"""
+
+Path Planning with 4 point Beizer curve
+
+author: Atsushi Sakai(@Atsushi_twi)
+
+"""
+
+import scipy.misc as scm
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+
+
+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(
+        [[sx, sy],
+         [sx + D * math.cos(syaw), sy + D * math.sin(syaw)],
+         [ex - D * math.cos(eyaw), ey - D * math.sin(eyaw)],
+         [ex, ey]])
+
+    traj = []
+    for t in np.linspace(0, 1, 100):
+        traj.append(bezier(3, t, cp))
+    P = np.array(traj)
+
+    return P, cp
+
+
+def bernstein(n, i, t):
+    return scm.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 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():
+    start_x = 10.0  # [m]
+    start_y = 1.0  # [m]
+    start_yaw = math.radians(180.0)  # [rad]
+
+    end_x = -0.0  # [m]
+    end_y = -3.0  # [m]
+    end_yaw = math.radians(-45.0)  # [rad]
+    offset = 3.0
+
+    P, cp = calc_4point_bezier_path(
+        start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)
+
+    plt.plot(P.T[0], P.T[1], label="Bezier Path")
+    plt.plot(cp.T[0], cp.T[1], '--o', label="Control Points")
+    plot_arrow(start_x, start_y, start_yaw)
+    plot_arrow(end_x, end_y, end_yaw)
+    plt.legend()
+    plt.axis("equal")
+    plt.grid(True)
+    plt.show()
+
+
+def main2():
+    start_x = 10.0  # [m]
+    start_y = 1.0  # [m]
+    start_yaw = math.radians(180.0)  # [rad]
+
+    end_x = -0.0  # [m]
+    end_y = -3.0  # [m]
+    end_yaw = math.radians(-45.0)  # [rad]
+    offset = 3.0
+
+    for offset in np.arange(1.0, 5.0, 1.0):
+        P, cp = calc_4point_bezier_path(
+            start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)
+        plt.plot(P.T[0], P.T[1], label="Offset=" + str(offset))
+    plot_arrow(start_x, start_y, start_yaw)
+    plot_arrow(end_x, end_y, end_yaw)
+    plt.legend()
+    plt.axis("equal")
+    plt.grid(True)
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()
+    #  main2()

PathPlanning/BezierPath/Figure_2.png

@@ -31,6 +31,7 @@ Python codes for robotics algorithm.
          * [RRT* with reeds-sheep path](#rrt-with-reeds-sheep-path)
          * [Closed Loop RRT*](#closed-loop-rrt)
       * [Cubic spline planning](#cubic-spline-planning)
+      * [Bezier path planning](#bezier-path-planning)
       * [Dubins path planning](#dubins-path-planning)
       * [Reeds Shepp planning](#reeds-shepp-planning)
       * [Mix Integer Optimization based model predictive planning and control](#mix-integer-optimization-based-model-predictive-planning-and-control)
@@ -118,7 +119,8 @@ This script is a path planning code with model predictive trajectory generator.
 
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png?raw=True)
 
-see: 
+Ref: 
+
 - [Optimal rough terrain trajectory generation for wheeled mobile robots](http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
 
 
@@ -198,8 +200,12 @@ This script is a simple path planning code with Rapidly-Exploring Random Trees (
 
 This script is a  path planning code with RRT \*
 
+Ref:
+
 - [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416)
 
+- [Sampling-based Algorithms for Optimal Motion Planningj](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf)
+
 
 ### RRT with dubins path 
 
@@ -229,7 +235,7 @@ A sample code with closed loop RRT\*.
 ![PythonRobotics/figure_1-5.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/CRRRTStar/Figure_4.png?raw=True)
 ![PythonRobotics/figure_1-5.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/CRRRTStar/Figure_5.png?raw=True)
 
-see:
+Ref:
 
 - [Motion Planning in Complex Environments
 using Closed-loop Prediction](http://acl.mit.edu/papers/KuwataGNC08.pdf)
@@ -251,17 +257,40 @@ Heading angle of each point can be also calculated analytically.
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/CubicSpline/Figure_2.png?raw=True)
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/CubicSpline/Figure_3.png?raw=True)
 
+## Bezier path planning
+
+A sample code of Bezier path planning.
+
+It is based on 4 control points Beier path.
+
+![Bezier1](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/BezierPath/Figure_1.png?raw=True)
+
+If you change the offset distance from start and end point,
+
+You can get different Beizer course:
+
+![Bezier2](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/BezierPath/Figure_2.png?raw=True)
+
+ 
+
+Ref:
+
+- [Continuous Curvature Path Generation Based on B ́ezier Curves for Autonomous Vehicles](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf)
+
 
 ## Dubins path planning
 
 A sample code for Dubins path planning.
 
-[Dubins path - Wikipedia](https://en.wikipedia.org/wiki/Dubins_path)
 
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DubinsPath/figures/figure_1.png?raw=True)
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DubinsPath/figures/figure_13.png?raw=True)
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DubinsPath/figures/figure_15.png?raw=True)
 
+Ref:
+
+- [Dubins path - Wikipedia](https://en.wikipedia.org/wiki/Dubins_path)
+
 ## Reeds Shepp planning
 
 A sample code with Reeds Shepp path planning.
@@ -270,6 +299,16 @@ A sample code with Reeds Shepp path planning.
 ![PythonRobotics/figure_1-5.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/ReedsSheppPath/figure_1-5.png?raw=true)
 ![PythonRobotics/figure_1-5.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/ReedsSheppPath/figure_1-7.png?raw=true)
 
+Ref:
+
+- [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html) 
+
+- [optimal paths for a car that goes both forwards and backwards](https://pdfs.semanticscholar.org/932e/c495b1d0018fd59dee12a0bf74434fac7af4.pdf)
+
+- [ghliu/pyReedsShepp: Implementation of Reeds Shepp curve\.](https://github.com/ghliu/pyReedsShepp)
+
+　
+
 ## Mix Integer Optimization based model predictive planning and control
 
 ![2](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/MixIntegerPathPlanning/animation.gif)