Add changes for 31178c8e37

_modules/PathPlanning/DubinsPath/dubins_path_planner.html

@@ -6,6 +6,7 @@
   <title>PathPlanning.DubinsPath.dubins_path_planner &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>
@@ -101,21 +102,20 @@
 <span class="sd">&quot;&quot;&quot;</span>
 <span class="kn">import</span> <span class="nn">sys</span>
 <span class="kn">import</span> <span class="nn">os</span>
+
 <span class="n">sys</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">dirname</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">abspath</span><span class="p">(</span><span class="vm">__file__</span><span class="p">))</span> <span class="o">+</span> <span class="s2">&quot;/../utils/&quot;</span><span class="p">)</span>
 
-<span class="kn">import</span> <span class="nn">math</span>
+<span class="kn">from</span> <span class="nn">math</span> <span class="kn">import</span> <span class="n">sin</span><span class="p">,</span> <span class="n">cos</span><span class="p">,</span> <span class="n">atan2</span><span class="p">,</span> <span class="n">sqrt</span><span class="p">,</span> <span class="n">acos</span><span class="p">,</span> <span class="n">pi</span><span class="p">,</span> <span class="n">hypot</span>
 <span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
-<span class="kn">from</span> <span class="nn">utils.angle</span> <span class="kn">import</span> <span class="n">angle_mod</span><span class="p">,</span> <span class="n">create_2d_rotation_matrix</span>
+<span class="kn">from</span> <span class="nn">utils.angle</span> <span class="kn">import</span> <span class="n">angle_mod</span><span class="p">,</span> <span class="n">rot_mat_2d</span>
 
 <span class="n">show_animation</span> <span class="o">=</span> <span class="kc">True</span>
 
 
-<div class="viewcode-block" id="plan_dubins_path"><a class="viewcode-back" href="../../../modules/path_planning/dubins_path/dubins_path.html#PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path">[docs]</a><span class="k">def</span> <span class="nf">plan_dubins_path</span><span class="p">(</span><span class="n">s_x</span><span class="p">,</span> <span class="n">s_y</span><span class="p">,</span> <span class="n">s_yaw</span><span class="p">,</span>
-                     <span class="n">g_x</span><span class="p">,</span> <span class="n">g_y</span><span class="p">,</span> <span class="n">g_yaw</span><span class="p">,</span>
-                     <span class="n">curvature</span><span class="p">,</span>
-                     <span class="n">step_size</span><span class="o">=</span><span class="mf">0.1</span><span class="p">):</span>
+<div class="viewcode-block" id="plan_dubins_path"><a class="viewcode-back" href="../../../modules/path_planning/dubins_path/dubins_path.html#PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path">[docs]</a><span class="k">def</span> <span class="nf">plan_dubins_path</span><span class="p">(</span><span class="n">s_x</span><span class="p">,</span> <span class="n">s_y</span><span class="p">,</span> <span class="n">s_yaw</span><span class="p">,</span> <span class="n">g_x</span><span class="p">,</span> <span class="n">g_y</span><span class="p">,</span> <span class="n">g_yaw</span><span class="p">,</span> <span class="n">curvature</span><span class="p">,</span>
+                     <span class="n">step_size</span><span class="o">=</span><span class="mf">0.1</span><span class="p">,</span> <span class="n">selected_types</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
     <span class="sd">&quot;&quot;&quot;</span>
-<span class="sd">    Path dubins path</span>
+<span class="sd">    Plan dubins path</span>
 
 <span class="sd">    Parameters</span>
 <span class="sd">    ----------</span>
@@ -135,6 +135,11 @@
 <span class="sd">        curvature for curve [1/m]</span>
 <span class="sd">    step_size : float (optional)</span>
 <span class="sd">        step size between two path points [m]. Default is 0.1</span>
+<span class="sd">    selected_types : a list of string or None</span>
+<span class="sd">        selected path planning types. If None, all types are used for</span>
+<span class="sd">        path planning, and minimum path length result is returned.</span>
+<span class="sd">        You can select used path plannings types by a string list.</span>
+<span class="sd">        e.g.: [&quot;RSL&quot;, &quot;RSR&quot;]</span>
 
 <span class="sd">    Returns</span>
 <span class="sd">    -------</span>
@@ -147,21 +152,49 @@
 <span class="sd">    modes: array</span>
 <span class="sd">        mode list of the path</span>
 <span class="sd">    lengths: array</span>
-<span class="sd">        length list of the path segments.</span>
+<span class="sd">        arrow_length list of the path segments.</span>
 
+<span class="sd">    Examples</span>
+<span class="sd">    --------</span>
+<span class="sd">    You can generate a dubins path.</span>
+
+<span class="sd">    &gt;&gt;&gt; start_x = 1.0  # [m]</span>
+<span class="sd">    &gt;&gt;&gt; start_y = 1.0  # [m]</span>
+<span class="sd">    &gt;&gt;&gt; start_yaw = np.deg2rad(45.0)  # [rad]</span>
+<span class="sd">    &gt;&gt;&gt; end_x = -3.0  # [m]</span>
+<span class="sd">    &gt;&gt;&gt; end_y = -3.0  # [m]</span>
+<span class="sd">    &gt;&gt;&gt; end_yaw = np.deg2rad(-45.0)  # [rad]</span>
+<span class="sd">    &gt;&gt;&gt; curvature = 1.0</span>
+<span class="sd">    &gt;&gt;&gt; path_x, path_y, path_yaw, mode, _ = plan_dubins_path(</span>
+<span class="sd">                start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)</span>
+<span class="sd">    &gt;&gt;&gt; plt.plot(path_x, path_y, label=&quot;final course &quot; + &quot;&quot;.join(mode))</span>
+<span class="sd">    &gt;&gt;&gt; plot_arrow(start_x, start_y, start_yaw)</span>
+<span class="sd">    &gt;&gt;&gt; plot_arrow(end_x, end_y, end_yaw)</span>
+<span class="sd">    &gt;&gt;&gt; plt.legend()</span>
+<span class="sd">    &gt;&gt;&gt; plt.grid(True)</span>
+<span class="sd">    &gt;&gt;&gt; plt.axis(&quot;equal&quot;)</span>
+<span class="sd">    &gt;&gt;&gt; plt.show()</span>
+
+<span class="sd">    .. image:: dubins_path.jpg</span>
 <span class="sd">    &quot;&quot;&quot;</span>
+    <span class="k">if</span> <span class="n">selected_types</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
+        <span class="n">planning_funcs</span> <span class="o">=</span> <span class="n">_PATH_TYPE_MAP</span><span class="o">.</span><span class="n">values</span><span class="p">()</span>
+    <span class="k">else</span><span class="p">:</span>
+        <span class="n">planning_funcs</span> <span class="o">=</span> <span class="p">[</span><span class="n">_PATH_TYPE_MAP</span><span class="p">[</span><span class="n">ptype</span><span class="p">]</span> <span class="k">for</span> <span class="n">ptype</span> <span class="ow">in</span> <span class="n">selected_types</span><span class="p">]</span>
+
     <span class="c1"># calculate local goal x, y, yaw</span>
-    <span class="n">l_rot</span> <span class="o">=</span> <span class="n">create_2d_rotation_matrix</span><span class="p">(</span><span class="n">s_yaw</span><span class="p">)</span>
+    <span class="n">l_rot</span> <span class="o">=</span> <span class="n">rot_mat_2d</span><span class="p">(</span><span class="n">s_yaw</span><span class="p">)</span>
     <span class="n">le_xy</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">stack</span><span class="p">([</span><span class="n">g_x</span> <span class="o">-</span> <span class="n">s_x</span><span class="p">,</span> <span class="n">g_y</span> <span class="o">-</span> <span class="n">s_y</span><span class="p">])</span><span class="o">.</span><span class="n">T</span> <span class="o">@</span> <span class="n">l_rot</span>
     <span class="n">local_goal_x</span> <span class="o">=</span> <span class="n">le_xy</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
     <span class="n">local_goal_y</span> <span class="o">=</span> <span class="n">le_xy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span>
     <span class="n">local_goal_yaw</span> <span class="o">=</span> <span class="n">g_yaw</span> <span class="o">-</span> <span class="n">s_yaw</span>
 
-    <span class="n">lp_x</span><span class="p">,</span> <span class="n">lp_y</span><span class="p">,</span> <span class="n">lp_yaw</span><span class="p">,</span> <span class="n">modes</span><span class="p">,</span> <span class="n">lengths</span> <span class="o">=</span> <span class="n">dubins_path_planning_from_origin</span><span class="p">(</span>
-        <span class="n">local_goal_x</span><span class="p">,</span> <span class="n">local_goal_y</span><span class="p">,</span> <span class="n">local_goal_yaw</span><span class="p">,</span> <span class="n">curvature</span><span class="p">,</span> <span class="n">step_size</span><span class="p">)</span>
+    <span class="n">lp_x</span><span class="p">,</span> <span class="n">lp_y</span><span class="p">,</span> <span class="n">lp_yaw</span><span class="p">,</span> <span class="n">modes</span><span class="p">,</span> <span class="n">lengths</span> <span class="o">=</span> <span class="n">_dubins_path_planning_from_origin</span><span class="p">(</span>
+        <span class="n">local_goal_x</span><span class="p">,</span> <span class="n">local_goal_y</span><span class="p">,</span> <span class="n">local_goal_yaw</span><span class="p">,</span> <span class="n">curvature</span><span class="p">,</span> <span class="n">step_size</span><span class="p">,</span>
+        <span class="n">planning_funcs</span><span class="p">)</span>
 
     <span class="c1"># Convert a local coordinate path to the global coordinate</span>
-    <span class="n">rot</span> <span class="o">=</span> <span class="n">create_2d_rotation_matrix</span><span class="p">(</span><span class="o">-</span><span class="n">s_yaw</span><span class="p">)</span>
+    <span class="n">rot</span> <span class="o">=</span> <span class="n">rot_mat_2d</span><span class="p">(</span><span class="o">-</span><span class="n">s_yaw</span><span class="p">)</span>
     <span class="n">converted_xy</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">stack</span><span class="p">([</span><span class="n">lp_x</span><span class="p">,</span> <span class="n">lp_y</span><span class="p">])</span><span class="o">.</span><span class="n">T</span> <span class="o">@</span> <span class="n">rot</span>
     <span class="n">x_list</span> <span class="o">=</span> <span class="n">converted_xy</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">s_x</span>
     <span class="n">y_list</span> <span class="o">=</span> <span class="n">converted_xy</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">s_y</span>
@@ -174,276 +207,179 @@
     <span class="k">return</span> <span class="n">angle_mod</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">zero_2_2pi</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>
 
 
+<span class="k">def</span> <span class="nf">_calc_trig_funcs</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">):</span>
+    <span class="n">sin_a</span> <span class="o">=</span> <span class="n">sin</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
+    <span class="n">sin_b</span> <span class="o">=</span> <span class="n">sin</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
+    <span class="n">cos_a</span> <span class="o">=</span> <span class="n">cos</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
+    <span class="n">cos_b</span> <span class="o">=</span> <span class="n">cos</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
+    <span class="n">cos_ab</span> <span class="o">=</span> <span class="n">cos</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span><span class="p">)</span>
+    <span class="k">return</span> <span class="n">sin_a</span><span class="p">,</span> <span class="n">sin_b</span><span class="p">,</span> <span class="n">cos_a</span><span class="p">,</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">cos_ab</span>
+
+
 <span class="k">def</span> <span class="nf">_LSL</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
-    <span class="n">sa</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">sb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">ca</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">cb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">c_ab</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span><span class="p">)</span>
-
-    <span class="n">tmp0</span> <span class="o">=</span> <span class="n">d</span> <span class="o">+</span> <span class="n">sa</span> <span class="o">-</span> <span class="n">sb</span>
-
+    <span class="n">sin_a</span><span class="p">,</span> <span class="n">sin_b</span><span class="p">,</span> <span class="n">cos_a</span><span class="p">,</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">cos_ab</span> <span class="o">=</span> <span class="n">_calc_trig_funcs</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">)</span>
     <span class="n">mode</span> <span class="o">=</span> <span class="p">[</span><span class="s2">&quot;L&quot;</span><span class="p">,</span> <span class="s2">&quot;S&quot;</span><span class="p">,</span> <span class="s2">&quot;L&quot;</span><span class="p">]</span>
-    <span class="n">p_squared</span> <span class="o">=</span> <span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="n">d</span> <span class="o">*</span> <span class="n">d</span><span class="p">)</span> <span class="o">-</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">c_ab</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sa</span> <span class="o">-</span> <span class="n">sb</span><span class="p">))</span>
-    <span class="k">if</span> <span class="n">p_squared</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
+    <span class="n">p_squared</span> <span class="o">=</span> <span class="mi">2</span> <span class="o">+</span> <span class="n">d</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">-</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">cos_ab</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sin_a</span> <span class="o">-</span> <span class="n">sin_b</span><span class="p">))</span>
+    <span class="k">if</span> <span class="n">p_squared</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>  <span class="c1"># invalid configuration</span>
         <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="n">mode</span>
-    <span class="n">tmp1</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">((</span><span class="n">cb</span> <span class="o">-</span> <span class="n">ca</span><span class="p">),</span> <span class="n">tmp0</span><span class="p">)</span>
-    <span class="n">t</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">alpha</span> <span class="o">+</span> <span class="n">tmp1</span><span class="p">)</span>
-    <span class="n">p</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
-    <span class="n">q</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span> <span class="o">-</span> <span class="n">tmp1</span><span class="p">)</span>
-
-    <span class="k">return</span> <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span>
+    <span class="n">tmp</span> <span class="o">=</span> <span class="n">atan2</span><span class="p">((</span><span class="n">cos_b</span> <span class="o">-</span> <span class="n">cos_a</span><span class="p">),</span> <span class="n">d</span> <span class="o">+</span> <span class="n">sin_a</span> <span class="o">-</span> <span class="n">sin_b</span><span class="p">)</span>
+    <span class="n">d1</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">alpha</span> <span class="o">+</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="n">d2</span> <span class="o">=</span> <span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
+    <span class="n">d3</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span> <span class="o">-</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="k">return</span> <span class="n">d1</span><span class="p">,</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span>
 
 
-<span class="k">def</span> <span class="nf">right_straight_right</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
-    <span class="n">sa</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">sb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">ca</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">cb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">c_ab</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span><span class="p">)</span>
-
-    <span class="n">tmp0</span> <span class="o">=</span> <span class="n">d</span> <span class="o">-</span> <span class="n">sa</span> <span class="o">+</span> <span class="n">sb</span>
+<span class="k">def</span> <span class="nf">_RSR</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
+    <span class="n">sin_a</span><span class="p">,</span> <span class="n">sin_b</span><span class="p">,</span> <span class="n">cos_a</span><span class="p">,</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">cos_ab</span> <span class="o">=</span> <span class="n">_calc_trig_funcs</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">)</span>
     <span class="n">mode</span> <span class="o">=</span> <span class="p">[</span><span class="s2">&quot;R&quot;</span><span class="p">,</span> <span class="s2">&quot;S&quot;</span><span class="p">,</span> <span class="s2">&quot;R&quot;</span><span class="p">]</span>
-    <span class="n">p_squared</span> <span class="o">=</span> <span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="n">d</span> <span class="o">*</span> <span class="n">d</span><span class="p">)</span> <span class="o">-</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">c_ab</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sb</span> <span class="o">-</span> <span class="n">sa</span><span class="p">))</span>
+    <span class="n">p_squared</span> <span class="o">=</span> <span class="mi">2</span> <span class="o">+</span> <span class="n">d</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">-</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">cos_ab</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sin_b</span> <span class="o">-</span> <span class="n">sin_a</span><span class="p">))</span>
     <span class="k">if</span> <span class="n">p_squared</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
         <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="n">mode</span>
-    <span class="n">tmp1</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">((</span><span class="n">ca</span> <span class="o">-</span> <span class="n">cb</span><span class="p">),</span> <span class="n">tmp0</span><span class="p">)</span>
-    <span class="n">t</span> <span class="o">=</span> <span class="n">angle_mod</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">tmp1</span><span class="p">,</span> <span class="n">zero_2_2pi</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>
-    <span class="n">p</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
-    <span class="n">q</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">beta</span> <span class="o">+</span> <span class="n">tmp1</span><span class="p">)</span>
-
-    <span class="k">return</span> <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span>
+    <span class="n">tmp</span> <span class="o">=</span> <span class="n">atan2</span><span class="p">((</span><span class="n">cos_a</span> <span class="o">-</span> <span class="n">cos_b</span><span class="p">),</span> <span class="n">d</span> <span class="o">-</span> <span class="n">sin_a</span> <span class="o">+</span> <span class="n">sin_b</span><span class="p">)</span>
+    <span class="n">d1</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="n">d2</span> <span class="o">=</span> <span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
+    <span class="n">d3</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">beta</span> <span class="o">+</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="k">return</span> <span class="n">d1</span><span class="p">,</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span>
 
 
-<span class="k">def</span> <span class="nf">left_straight_right</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
-    <span class="n">sa</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">sb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">ca</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">cb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">c_ab</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span><span class="p">)</span>
-
-    <span class="n">p_squared</span> <span class="o">=</span> <span class="o">-</span><span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="n">d</span> <span class="o">*</span> <span class="n">d</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">c_ab</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sa</span> <span class="o">+</span> <span class="n">sb</span><span class="p">))</span>
+<span class="k">def</span> <span class="nf">_LSR</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
+    <span class="n">sin_a</span><span class="p">,</span> <span class="n">sin_b</span><span class="p">,</span> <span class="n">cos_a</span><span class="p">,</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">cos_ab</span> <span class="o">=</span> <span class="n">_calc_trig_funcs</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">)</span>
+    <span class="n">p_squared</span> <span class="o">=</span> <span class="o">-</span><span class="mi">2</span> <span class="o">+</span> <span class="n">d</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">cos_ab</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sin_a</span> <span class="o">+</span> <span class="n">sin_b</span><span class="p">))</span>
     <span class="n">mode</span> <span class="o">=</span> <span class="p">[</span><span class="s2">&quot;L&quot;</span><span class="p">,</span> <span class="s2">&quot;S&quot;</span><span class="p">,</span> <span class="s2">&quot;R&quot;</span><span class="p">]</span>
     <span class="k">if</span> <span class="n">p_squared</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
         <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="n">mode</span>
-    <span class="n">p</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
-    <span class="n">tmp2</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">((</span><span class="o">-</span><span class="n">ca</span> <span class="o">-</span> <span class="n">cb</span><span class="p">),</span> <span class="p">(</span><span class="n">d</span> <span class="o">+</span> <span class="n">sa</span> <span class="o">+</span> <span class="n">sb</span><span class="p">))</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="o">-</span><span class="mf">2.0</span><span class="p">,</span> <span class="n">p</span><span class="p">)</span>
-    <span class="n">t</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">alpha</span> <span class="o">+</span> <span class="n">tmp2</span><span class="p">)</span>
-    <span class="n">q</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span> <span class="o">+</span> <span class="n">tmp2</span><span class="p">)</span>
-
-    <span class="k">return</span> <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span>
+    <span class="n">d1</span> <span class="o">=</span> <span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
+    <span class="n">tmp</span> <span class="o">=</span> <span class="n">atan2</span><span class="p">((</span><span class="o">-</span><span class="n">cos_a</span> <span class="o">-</span> <span class="n">cos_b</span><span class="p">),</span> <span class="p">(</span><span class="n">d</span> <span class="o">+</span> <span class="n">sin_a</span> <span class="o">+</span> <span class="n">sin_b</span><span class="p">))</span> <span class="o">-</span> <span class="n">atan2</span><span class="p">(</span><span class="o">-</span><span class="mf">2.0</span><span class="p">,</span> <span class="n">d1</span><span class="p">)</span>
+    <span class="n">d2</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">alpha</span> <span class="o">+</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="n">d3</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span> <span class="o">+</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="k">return</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d1</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span>
 
 
-<span class="k">def</span> <span class="nf">right_straight_left</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
-    <span class="n">sa</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">sb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">ca</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">cb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">c_ab</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span><span class="p">)</span>
-
-    <span class="n">p_squared</span> <span class="o">=</span> <span class="p">(</span><span class="n">d</span> <span class="o">*</span> <span class="n">d</span><span class="p">)</span> <span class="o">-</span> <span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">c_ab</span><span class="p">)</span> <span class="o">-</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sa</span> <span class="o">+</span> <span class="n">sb</span><span class="p">))</span>
+<span class="k">def</span> <span class="nf">_RSL</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
+    <span class="n">sin_a</span><span class="p">,</span> <span class="n">sin_b</span><span class="p">,</span> <span class="n">cos_a</span><span class="p">,</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">cos_ab</span> <span class="o">=</span> <span class="n">_calc_trig_funcs</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">)</span>
+    <span class="n">p_squared</span> <span class="o">=</span> <span class="n">d</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">-</span> <span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">cos_ab</span><span class="p">)</span> <span class="o">-</span> <span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sin_a</span> <span class="o">+</span> <span class="n">sin_b</span><span class="p">))</span>
     <span class="n">mode</span> <span class="o">=</span> <span class="p">[</span><span class="s2">&quot;R&quot;</span><span class="p">,</span> <span class="s2">&quot;S&quot;</span><span class="p">,</span> <span class="s2">&quot;L&quot;</span><span class="p">]</span>
     <span class="k">if</span> <span class="n">p_squared</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
         <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="n">mode</span>
-    <span class="n">p</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
-    <span class="n">tmp2</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">((</span><span class="n">ca</span> <span class="o">+</span> <span class="n">cb</span><span class="p">),</span> <span class="p">(</span><span class="n">d</span> <span class="o">-</span> <span class="n">sa</span> <span class="o">-</span> <span class="n">sb</span><span class="p">))</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="mf">2.0</span><span class="p">,</span> <span class="n">p</span><span class="p">)</span>
-    <span class="n">t</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">tmp2</span><span class="p">)</span>
-    <span class="n">q</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span> <span class="o">-</span> <span class="n">tmp2</span><span class="p">)</span>
-
-    <span class="k">return</span> <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span>
+    <span class="n">d1</span> <span class="o">=</span> <span class="n">sqrt</span><span class="p">(</span><span class="n">p_squared</span><span class="p">)</span>
+    <span class="n">tmp</span> <span class="o">=</span> <span class="n">atan2</span><span class="p">((</span><span class="n">cos_a</span> <span class="o">+</span> <span class="n">cos_b</span><span class="p">),</span> <span class="p">(</span><span class="n">d</span> <span class="o">-</span> <span class="n">sin_a</span> <span class="o">-</span> <span class="n">sin_b</span><span class="p">))</span> <span class="o">-</span> <span class="n">atan2</span><span class="p">(</span><span class="mf">2.0</span><span class="p">,</span> <span class="n">d1</span><span class="p">)</span>
+    <span class="n">d2</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="n">d3</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span> <span class="o">-</span> <span class="n">tmp</span><span class="p">)</span>
+    <span class="k">return</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d1</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span>
 
 
-<span class="k">def</span> <span class="nf">right_left_right</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
-    <span class="n">sa</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">sb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">ca</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">cb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">c_ab</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span><span class="p">)</span>
-
+<span class="k">def</span> <span class="nf">_RLR</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
+    <span class="n">sin_a</span><span class="p">,</span> <span class="n">sin_b</span><span class="p">,</span> <span class="n">cos_a</span><span class="p">,</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">cos_ab</span> <span class="o">=</span> <span class="n">_calc_trig_funcs</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">)</span>
     <span class="n">mode</span> <span class="o">=</span> <span class="p">[</span><span class="s2">&quot;R&quot;</span><span class="p">,</span> <span class="s2">&quot;L&quot;</span><span class="p">,</span> <span class="s2">&quot;R&quot;</span><span class="p">]</span>
-    <span class="n">tmp_rlr</span> <span class="o">=</span> <span class="p">(</span><span class="mf">6.0</span> <span class="o">-</span> <span class="n">d</span> <span class="o">*</span> <span class="n">d</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">c_ab</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sa</span> <span class="o">-</span> <span class="n">sb</span><span class="p">))</span> <span class="o">/</span> <span class="mf">8.0</span>
-    <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">tmp_rlr</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mf">1.0</span><span class="p">:</span>
+    <span class="n">tmp</span> <span class="o">=</span> <span class="p">(</span><span class="mf">6.0</span> <span class="o">-</span> <span class="n">d</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">cos_ab</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="n">sin_a</span> <span class="o">-</span> <span class="n">sin_b</span><span class="p">))</span> <span class="o">/</span> <span class="mf">8.0</span>
+    <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">tmp</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mf">1.0</span><span class="p">:</span>
         <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="n">mode</span>
-
-    <span class="n">p</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">acos</span><span class="p">(</span><span class="n">tmp_rlr</span><span class="p">))</span>
-    <span class="n">t</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">ca</span> <span class="o">-</span> <span class="n">cb</span><span class="p">,</span> <span class="n">d</span> <span class="o">-</span> <span class="n">sa</span> <span class="o">+</span> <span class="n">sb</span><span class="p">)</span> <span class="o">+</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">p</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">))</span>
-    <span class="n">q</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span> <span class="o">-</span> <span class="n">t</span> <span class="o">+</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">p</span><span class="p">))</span>
-    <span class="k">return</span> <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span>
+    <span class="n">d2</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">pi</span> <span class="o">-</span> <span class="n">acos</span><span class="p">(</span><span class="n">tmp</span><span class="p">))</span>
+    <span class="n">d1</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">atan2</span><span class="p">(</span><span class="n">cos_a</span> <span class="o">-</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">d</span> <span class="o">-</span> <span class="n">sin_a</span> <span class="o">+</span> <span class="n">sin_b</span><span class="p">)</span> <span class="o">+</span> <span class="n">d2</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">)</span>
+    <span class="n">d3</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span> <span class="o">-</span> <span class="n">d1</span> <span class="o">+</span> <span class="n">d2</span><span class="p">)</span>
+    <span class="k">return</span> <span class="n">d1</span><span class="p">,</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span>
 
 
 <span class="k">def</span> <span class="nf">_LRL</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">):</span>
-    <span class="n">sa</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">sb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">ca</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span><span class="p">)</span>
-    <span class="n">cb</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span>
-    <span class="n">c_ab</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">beta</span><span class="p">)</span>
-
+    <span class="n">sin_a</span><span class="p">,</span> <span class="n">sin_b</span><span class="p">,</span> <span class="n">cos_a</span><span class="p">,</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">cos_ab</span> <span class="o">=</span> <span class="n">_calc_trig_funcs</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">)</span>
     <span class="n">mode</span> <span class="o">=</span> <span class="p">[</span><span class="s2">&quot;L&quot;</span><span class="p">,</span> <span class="s2">&quot;R&quot;</span><span class="p">,</span> <span class="s2">&quot;L&quot;</span><span class="p">]</span>
-    <span class="n">tmp_lrl</span> <span class="o">=</span> <span class="p">(</span><span class="mf">6.0</span> <span class="o">-</span> <span class="n">d</span> <span class="o">*</span> <span class="n">d</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">c_ab</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="o">-</span> <span class="n">sa</span> <span class="o">+</span> <span class="n">sb</span><span class="p">))</span> <span class="o">/</span> <span class="mf">8.0</span>
-    <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">tmp_lrl</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">1</span><span class="p">:</span>
+    <span class="n">tmp</span> <span class="o">=</span> <span class="p">(</span><span class="mf">6.0</span> <span class="o">-</span> <span class="n">d</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">cos_ab</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">d</span> <span class="o">*</span> <span class="p">(</span><span class="o">-</span> <span class="n">sin_a</span> <span class="o">+</span> <span class="n">sin_b</span><span class="p">))</span> <span class="o">/</span> <span class="mf">8.0</span>
+    <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">tmp</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mf">1.0</span><span class="p">:</span>
         <span class="k">return</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="n">mode</span>
-    <span class="n">p</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">acos</span><span class="p">(</span><span class="n">tmp_lrl</span><span class="p">))</span>
-    <span class="n">t</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">ca</span> <span class="o">-</span> <span class="n">cb</span><span class="p">,</span> <span class="n">d</span> <span class="o">+</span> <span class="n">sa</span> <span class="o">-</span> <span class="n">sb</span><span class="p">)</span> <span class="o">+</span> <span class="n">p</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">)</span>
-    <span class="n">q</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span> <span class="o">-</span> <span class="n">alpha</span> <span class="o">-</span> <span class="n">t</span> <span class="o">+</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">p</span><span class="p">))</span>
-
-    <span class="k">return</span> <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span>
+    <span class="n">d2</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">pi</span> <span class="o">-</span> <span class="n">acos</span><span class="p">(</span><span class="n">tmp</span><span class="p">))</span>
+    <span class="n">d1</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">alpha</span> <span class="o">-</span> <span class="n">atan2</span><span class="p">(</span><span class="n">cos_a</span> <span class="o">-</span> <span class="n">cos_b</span><span class="p">,</span> <span class="n">d</span> <span class="o">+</span> <span class="n">sin_a</span> <span class="o">-</span> <span class="n">sin_b</span><span class="p">)</span> <span class="o">+</span> <span class="n">d2</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">)</span>
+    <span class="n">d3</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">_mod2pi</span><span class="p">(</span><span class="n">beta</span><span class="p">)</span> <span class="o">-</span> <span class="n">alpha</span> <span class="o">-</span> <span class="n">d1</span> <span class="o">+</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">d2</span><span class="p">))</span>
+    <span class="k">return</span> <span class="n">d1</span><span class="p">,</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span>
 
 
-<span class="k">def</span> <span class="nf">dubins_path_planning_from_origin</span><span class="p">(</span><span class="n">end_x</span><span class="p">,</span> <span class="n">end_y</span><span class="p">,</span> <span class="n">end_yaw</span><span class="p">,</span> <span class="n">curvature</span><span class="p">,</span>
-                                     <span class="n">step_size</span><span class="p">):</span>
+<span class="n">_PATH_TYPE_MAP</span> <span class="o">=</span> <span class="p">{</span><span class="s2">&quot;LSL&quot;</span><span class="p">:</span> <span class="n">_LSL</span><span class="p">,</span> <span class="s2">&quot;RSR&quot;</span><span class="p">:</span> <span class="n">_RSR</span><span class="p">,</span> <span class="s2">&quot;LSR&quot;</span><span class="p">:</span> <span class="n">_LSR</span><span class="p">,</span> <span class="s2">&quot;RSL&quot;</span><span class="p">:</span> <span class="n">_RSL</span><span class="p">,</span>
+                  <span class="s2">&quot;RLR&quot;</span><span class="p">:</span> <span class="n">_RLR</span><span class="p">,</span> <span class="s2">&quot;LRL&quot;</span><span class="p">:</span> <span class="n">_LRL</span><span class="p">,</span> <span class="p">}</span>
+
+
+<span class="k">def</span> <span class="nf">_dubins_path_planning_from_origin</span><span class="p">(</span><span class="n">end_x</span><span class="p">,</span> <span class="n">end_y</span><span class="p">,</span> <span class="n">end_yaw</span><span class="p">,</span> <span class="n">curvature</span><span class="p">,</span>
+                                      <span class="n">step_size</span><span class="p">,</span> <span class="n">planning_funcs</span><span class="p">):</span>
     <span class="n">dx</span> <span class="o">=</span> <span class="n">end_x</span>
     <span class="n">dy</span> <span class="o">=</span> <span class="n">end_y</span>
-    <span class="n">D</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">hypot</span><span class="p">(</span><span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">)</span>
-    <span class="n">d</span> <span class="o">=</span> <span class="n">D</span> <span class="o">*</span> <span class="n">curvature</span>
+    <span class="n">d</span> <span class="o">=</span> <span class="n">hypot</span><span class="p">(</span><span class="n">dx</span><span class="p">,</span> <span class="n">dy</span><span class="p">)</span> <span class="o">*</span> <span class="n">curvature</span>
 
-    <span class="n">theta</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">dy</span><span class="p">,</span> <span class="n">dx</span><span class="p">))</span>
-    <span class="n">alpha</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span> <span class="n">theta</span><span class="p">)</span>
+    <span class="n">theta</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">atan2</span><span class="p">(</span><span class="n">dy</span><span class="p">,</span> <span class="n">dx</span><span class="p">))</span>
+    <span class="n">alpha</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="o">-</span><span class="n">theta</span><span class="p">)</span>
     <span class="n">beta</span> <span class="o">=</span> <span class="n">_mod2pi</span><span class="p">(</span><span class="n">end_yaw</span> <span class="o">-</span> <span class="n">theta</span><span class="p">)</span>
 
-    <span class="n">planning_funcs</span> <span class="o">=</span> <span class="p">[</span><span class="n">_LSL</span><span class="p">,</span> <span class="n">right_straight_right</span><span class="p">,</span>
-                      <span class="n">left_straight_right</span><span class="p">,</span> <span class="n">right_straight_left</span><span class="p">,</span>
-                      <span class="n">right_left_right</span><span class="p">,</span> <span class="n">_LRL</span><span class="p">]</span>
-
     <span class="n">best_cost</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="s2">&quot;inf&quot;</span><span class="p">)</span>
-    <span class="n">bt</span><span class="p">,</span> <span class="n">bp</span><span class="p">,</span> <span class="n">bq</span><span class="p">,</span> <span class="n">best_mode</span> <span class="o">=</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span>
+    <span class="n">b_d1</span><span class="p">,</span> <span class="n">b_d2</span><span class="p">,</span> <span class="n">b_d3</span><span class="p">,</span> <span class="n">b_mode</span> <span class="o">=</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span><span class="p">,</span> <span class="kc">None</span>
 
     <span class="k">for</span> <span class="n">planner</span> <span class="ow">in</span> <span class="n">planning_funcs</span><span class="p">:</span>
-        <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span> <span class="o">=</span> <span class="n">planner</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">)</span>
-        <span class="k">if</span> <span class="n">t</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
+        <span class="n">d1</span><span class="p">,</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span> <span class="o">=</span> <span class="n">planner</span><span class="p">(</span><span class="n">alpha</span><span class="p">,</span> <span class="n">beta</span><span class="p">,</span> <span class="n">d</span><span class="p">)</span>
+        <span class="k">if</span> <span class="n">d1</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
             <span class="k">continue</span>
 
-        <span class="n">cost</span> <span class="o">=</span> <span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">t</span><span class="p">)</span> <span class="o">+</span> <span class="nb">abs</span><span class="p">(</span><span class="n">p</span><span class="p">)</span> <span class="o">+</span> <span class="nb">abs</span><span class="p">(</span><span class="n">q</span><span class="p">))</span>
-        <span class="k">if</span> <span class="n">best_cost</span> <span class="o">&gt;</span> <span class="n">cost</span><span class="p">:</span>
-            <span class="n">bt</span><span class="p">,</span> <span class="n">bp</span><span class="p">,</span> <span class="n">bq</span><span class="p">,</span> <span class="n">best_mode</span> <span class="o">=</span> <span class="n">t</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">q</span><span class="p">,</span> <span class="n">mode</span>
-            <span class="n">best_cost</span> <span class="o">=</span> <span class="n">cost</span>
-    <span class="n">lengths</span> <span class="o">=</span> <span class="p">[</span><span class="n">bt</span><span class="p">,</span> <span class="n">bp</span><span class="p">,</span> <span class="n">bq</span><span class="p">]</span>
+        <span class="n">cost</span> <span class="o">=</span> <span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">d1</span><span class="p">)</span> <span class="o">+</span> <span class="nb">abs</span><span class="p">(</span><span class="n">d2</span><span class="p">)</span> <span class="o">+</span> <span class="nb">abs</span><span class="p">(</span><span class="n">d3</span><span class="p">))</span>
+        <span class="k">if</span> <span class="n">best_cost</span> <span class="o">&gt;</span> <span class="n">cost</span><span class="p">:</span>  <span class="c1"># Select minimum length one.</span>
+            <span class="n">b_d1</span><span class="p">,</span> <span class="n">b_d2</span><span class="p">,</span> <span class="n">b_d3</span><span class="p">,</span> <span class="n">b_mode</span><span class="p">,</span> <span class="n">best_cost</span> <span class="o">=</span> <span class="n">d1</span><span class="p">,</span> <span class="n">d2</span><span class="p">,</span> <span class="n">d3</span><span class="p">,</span> <span class="n">mode</span><span class="p">,</span> <span class="n">cost</span>
 
-    <span class="n">x_list</span><span class="p">,</span> <span class="n">y_list</span><span class="p">,</span> <span class="n">yaw_list</span><span class="p">,</span> <span class="n">directions</span> <span class="o">=</span> <span class="n">generate_local_course</span><span class="p">(</span><span class="nb">sum</span><span class="p">(</span><span class="n">lengths</span><span class="p">),</span>
-                                                                 <span class="n">lengths</span><span class="p">,</span>
-                                                                 <span class="n">best_mode</span><span class="p">,</span>
-                                                                 <span class="n">curvature</span><span class="p">,</span>
-                                                                 <span class="n">step_size</span><span class="p">)</span>
+    <span class="n">lengths</span> <span class="o">=</span> <span class="p">[</span><span class="n">b_d1</span><span class="p">,</span> <span class="n">b_d2</span><span class="p">,</span> <span class="n">b_d3</span><span class="p">]</span>
+    <span class="n">x_list</span><span class="p">,</span> <span class="n">y_list</span><span class="p">,</span> <span class="n">yaw_list</span> <span class="o">=</span> <span class="n">_generate_local_course</span><span class="p">(</span><span class="n">lengths</span><span class="p">,</span> <span class="n">b_mode</span><span class="p">,</span>
+                                                      <span class="n">curvature</span><span class="p">,</span> <span class="n">step_size</span><span class="p">)</span>
 
     <span class="n">lengths</span> <span class="o">=</span> <span class="p">[</span><span class="n">length</span> <span class="o">/</span> <span class="n">curvature</span> <span class="k">for</span> <span class="n">length</span> <span class="ow">in</span> <span class="n">lengths</span><span class="p">]</span>
 
-    <span class="k">return</span> <span class="n">x_list</span><span class="p">,</span> <span class="n">y_list</span><span class="p">,</span> <span class="n">yaw_list</span><span class="p">,</span> <span class="n">best_mode</span><span class="p">,</span> <span class="n">lengths</span>
+    <span class="k">return</span> <span class="n">x_list</span><span class="p">,</span> <span class="n">y_list</span><span class="p">,</span> <span class="n">yaw_list</span><span class="p">,</span> <span class="n">b_mode</span><span class="p">,</span> <span class="n">lengths</span>
 
 
-<span class="k">def</span> <span class="nf">interpolate</span><span class="p">(</span><span class="n">ind</span><span class="p">,</span> <span class="n">length</span><span class="p">,</span> <span class="n">mode</span><span class="p">,</span> <span class="n">max_curvature</span><span class="p">,</span> <span class="n">origin_x</span><span class="p">,</span> <span class="n">origin_y</span><span class="p">,</span>
-                <span class="n">origin_yaw</span><span class="p">,</span> <span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">path_yaw</span><span class="p">,</span> <span class="n">directions</span><span class="p">):</span>
+<span class="k">def</span> <span class="nf">_interpolate</span><span class="p">(</span><span class="n">length</span><span class="p">,</span> <span class="n">mode</span><span class="p">,</span> <span class="n">max_curvature</span><span class="p">,</span> <span class="n">origin_x</span><span class="p">,</span> <span class="n">origin_y</span><span class="p">,</span>
+                 <span class="n">origin_yaw</span><span class="p">,</span> <span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">path_yaw</span><span class="p">):</span>
     <span class="k">if</span> <span class="n">mode</span> <span class="o">==</span> <span class="s2">&quot;S&quot;</span><span class="p">:</span>
-        <span class="n">path_x</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="n">origin_x</span> <span class="o">+</span> <span class="n">length</span> <span class="o">/</span> <span class="n">max_curvature</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">origin_yaw</span><span class="p">)</span>
-        <span class="n">path_y</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="n">origin_y</span> <span class="o">+</span> <span class="n">length</span> <span class="o">/</span> <span class="n">max_curvature</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">origin_yaw</span><span class="p">)</span>
-        <span class="n">path_yaw</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="n">origin_yaw</span>
+        <span class="n">path_x</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">origin_x</span> <span class="o">+</span> <span class="n">length</span> <span class="o">/</span> <span class="n">max_curvature</span> <span class="o">*</span> <span class="n">cos</span><span class="p">(</span><span class="n">origin_yaw</span><span class="p">))</span>
+        <span class="n">path_y</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">origin_y</span> <span class="o">+</span> <span class="n">length</span> <span class="o">/</span> <span class="n">max_curvature</span> <span class="o">*</span> <span class="n">sin</span><span class="p">(</span><span class="n">origin_yaw</span><span class="p">))</span>
+        <span class="n">path_yaw</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">origin_yaw</span><span class="p">)</span>
     <span class="k">else</span><span class="p">:</span>  <span class="c1"># curve</span>
-        <span class="n">ldx</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">length</span><span class="p">)</span> <span class="o">/</span> <span class="n">max_curvature</span>
+        <span class="n">ldx</span> <span class="o">=</span> <span class="n">sin</span><span class="p">(</span><span class="n">length</span><span class="p">)</span> <span class="o">/</span> <span class="n">max_curvature</span>
         <span class="n">ldy</span> <span class="o">=</span> <span class="mf">0.0</span>
         <span class="k">if</span> <span class="n">mode</span> <span class="o">==</span> <span class="s2">&quot;L&quot;</span><span class="p">:</span>  <span class="c1"># left turn</span>
-            <span class="n">ldy</span> <span class="o">=</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">length</span><span class="p">))</span> <span class="o">/</span> <span class="n">max_curvature</span>
+            <span class="n">ldy</span> <span class="o">=</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">cos</span><span class="p">(</span><span class="n">length</span><span class="p">))</span> <span class="o">/</span> <span class="n">max_curvature</span>
         <span class="k">elif</span> <span class="n">mode</span> <span class="o">==</span> <span class="s2">&quot;R&quot;</span><span class="p">:</span>  <span class="c1"># right turn</span>
-            <span class="n">ldy</span> <span class="o">=</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">length</span><span class="p">))</span> <span class="o">/</span> <span class="o">-</span><span class="n">max_curvature</span>
-        <span class="n">gdx</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldx</span> <span class="o">+</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldy</span>
-        <span class="n">gdy</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldx</span> <span class="o">+</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldy</span>
-        <span class="n">path_x</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="n">origin_x</span> <span class="o">+</span> <span class="n">gdx</span>
-        <span class="n">path_y</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="n">origin_y</span> <span class="o">+</span> <span class="n">gdy</span>
+            <span class="n">ldy</span> <span class="o">=</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">cos</span><span class="p">(</span><span class="n">length</span><span class="p">))</span> <span class="o">/</span> <span class="o">-</span><span class="n">max_curvature</span>
+        <span class="n">gdx</span> <span class="o">=</span> <span class="n">cos</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldx</span> <span class="o">+</span> <span class="n">sin</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldy</span>
+        <span class="n">gdy</span> <span class="o">=</span> <span class="o">-</span><span class="n">sin</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldx</span> <span class="o">+</span> <span class="n">cos</span><span class="p">(</span><span class="o">-</span><span class="n">origin_yaw</span><span class="p">)</span> <span class="o">*</span> <span class="n">ldy</span>
+        <span class="n">path_x</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">origin_x</span> <span class="o">+</span> <span class="n">gdx</span><span class="p">)</span>
+        <span class="n">path_y</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">origin_y</span> <span class="o">+</span> <span class="n">gdy</span><span class="p">)</span>
 
-    <span class="k">if</span> <span class="n">mode</span> <span class="o">==</span> <span class="s2">&quot;L&quot;</span><span class="p">:</span>  <span class="c1"># left turn</span>
-        <span class="n">path_yaw</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="n">origin_yaw</span> <span class="o">+</span> <span class="n">length</span>
-    <span class="k">elif</span> <span class="n">mode</span> <span class="o">==</span> <span class="s2">&quot;R&quot;</span><span class="p">:</span>  <span class="c1"># right turn</span>
-        <span class="n">path_yaw</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="n">origin_yaw</span> <span class="o">-</span> <span class="n">length</span>
+        <span class="k">if</span> <span class="n">mode</span> <span class="o">==</span> <span class="s2">&quot;L&quot;</span><span class="p">:</span>  <span class="c1"># left turn</span>
+            <span class="n">path_yaw</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">origin_yaw</span> <span class="o">+</span> <span class="n">length</span><span class="p">)</span>
+        <span class="k">elif</span> <span class="n">mode</span> <span class="o">==</span> <span class="s2">&quot;R&quot;</span><span class="p">:</span>  <span class="c1"># right turn</span>
+            <span class="n">path_yaw</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">origin_yaw</span> <span class="o">-</span> <span class="n">length</span><span class="p">)</span>
 
-    <span class="k">if</span> <span class="n">length</span> <span class="o">&gt;</span> <span class="mf">0.0</span><span class="p">:</span>
-        <span class="n">directions</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="mi">1</span>
-    <span class="k">else</span><span class="p">:</span>
-        <span class="n">directions</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="mi">1</span>
-
-    <span class="k">return</span> <span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">path_yaw</span><span class="p">,</span> <span class="n">directions</span>
+    <span class="k">return</span> <span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">path_yaw</span>
 
 
-<span class="k">def</span> <span class="nf">generate_local_course</span><span class="p">(</span><span class="n">total_length</span><span class="p">,</span> <span class="n">lengths</span><span class="p">,</span> <span class="n">modes</span><span class="p">,</span> <span class="n">max_curvature</span><span class="p">,</span>
-                          <span class="n">step_size</span><span class="p">):</span>
-    <span class="n">n_point</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">trunc</span><span class="p">(</span><span class="n">total_length</span> <span class="o">/</span> <span class="n">step_size</span><span class="p">)</span> <span class="o">+</span> <span class="nb">len</span><span class="p">(</span><span class="n">lengths</span><span class="p">)</span> <span class="o">+</span> <span class="mi">4</span>
+<span class="k">def</span> <span class="nf">_generate_local_course</span><span class="p">(</span><span class="n">lengths</span><span class="p">,</span> <span class="n">modes</span><span class="p">,</span> <span class="n">max_curvature</span><span class="p">,</span> <span class="n">step_size</span><span class="p">):</span>
+    <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.0</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.0</span><span class="p">],</span> <span class="p">[</span><span class="mf">0.0</span><span class="p">]</span>
 
-    <span class="n">p_x</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.0</span> <span class="k">for</span> <span class="n">_</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n_point</span><span class="p">)]</span>
-    <span class="n">p_y</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.0</span> <span class="k">for</span> <span class="n">_</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n_point</span><span class="p">)]</span>
-    <span class="n">p_yaw</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.0</span> <span class="k">for</span> <span class="n">_</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n_point</span><span class="p">)]</span>
-    <span class="n">directions</span> <span class="o">=</span> <span class="p">[</span><span class="mf">0.0</span> <span class="k">for</span> <span class="n">_</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">n_point</span><span class="p">)]</span>
-    <span class="n">ind</span> <span class="o">=</span> <span class="mi">1</span>
-
-    <span class="k">if</span> <span class="n">lengths</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">&gt;</span> <span class="mf">0.0</span><span class="p">:</span>
-        <span class="n">directions</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mi">1</span>
-    <span class="k">else</span><span class="p">:</span>
-        <span class="n">directions</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="mi">1</span>
-
-    <span class="n">ll</span> <span class="o">=</span> <span class="mf">0.0</span>
-
-    <span class="k">for</span> <span class="p">(</span><span class="n">m</span><span class="p">,</span> <span class="n">length</span><span class="p">,</span> <span class="n">i</span><span class="p">)</span> <span class="ow">in</span> <span class="nb">zip</span><span class="p">(</span><span class="n">modes</span><span class="p">,</span> <span class="n">lengths</span><span class="p">,</span> <span class="nb">range</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">modes</span><span class="p">))):</span>
+    <span class="k">for</span> <span class="p">(</span><span class="n">mode</span><span class="p">,</span> <span class="n">length</span><span class="p">)</span> <span class="ow">in</span> <span class="nb">zip</span><span class="p">(</span><span class="n">modes</span><span class="p">,</span> <span class="n">lengths</span><span class="p">):</span>
         <span class="k">if</span> <span class="n">length</span> <span class="o">==</span> <span class="mf">0.0</span><span class="p">:</span>
             <span class="k">continue</span>
-        <span class="k">elif</span> <span class="n">length</span> <span class="o">&gt;</span> <span class="mf">0.0</span><span class="p">:</span>
-            <span class="n">dist</span> <span class="o">=</span> <span class="n">step_size</span>
-        <span class="k">else</span><span class="p">:</span>
-            <span class="n">dist</span> <span class="o">=</span> <span class="o">-</span><span class="n">step_size</span>
 
         <span class="c1"># set origin state</span>
-        <span class="n">origin_x</span><span class="p">,</span> <span class="n">origin_y</span><span class="p">,</span> <span class="n">origin_yaw</span> <span class="o">=</span> <span class="n">p_x</span><span class="p">[</span><span class="n">ind</span><span class="p">],</span> <span class="n">p_y</span><span class="p">[</span><span class="n">ind</span><span class="p">],</span> <span class="n">p_yaw</span><span class="p">[</span><span class="n">ind</span><span class="p">]</span>
+        <span class="n">origin_x</span><span class="p">,</span> <span class="n">origin_y</span><span class="p">,</span> <span class="n">origin_yaw</span> <span class="o">=</span> <span class="n">p_x</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">p_y</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">p_yaw</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span>
 
-        <span class="n">ind</span> <span class="o">-=</span> <span class="mi">1</span>
-        <span class="k">if</span> <span class="n">i</span> <span class="o">&gt;=</span> <span class="mi">1</span> <span class="ow">and</span> <span class="p">(</span><span class="n">lengths</span><span class="p">[</span><span class="n">i</span> <span class="o">-</span> <span class="mi">1</span><span class="p">]</span> <span class="o">*</span> <span class="n">lengths</span><span class="p">[</span><span class="n">i</span><span class="p">])</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">:</span>
-            <span class="n">pd</span> <span class="o">=</span> <span class="o">-</span> <span class="n">dist</span> <span class="o">-</span> <span class="n">ll</span>
-        <span class="k">else</span><span class="p">:</span>
-            <span class="n">pd</span> <span class="o">=</span> <span class="n">dist</span> <span class="o">-</span> <span class="n">ll</span>
+        <span class="n">current_length</span> <span class="o">=</span> <span class="n">step_size</span>
+        <span class="k">while</span> <span class="nb">abs</span><span class="p">(</span><span class="n">current_length</span> <span class="o">+</span> <span class="n">step_size</span><span class="p">)</span> <span class="o">&lt;=</span> <span class="nb">abs</span><span class="p">(</span><span class="n">length</span><span class="p">):</span>
+            <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span> <span class="o">=</span> <span class="n">_interpolate</span><span class="p">(</span><span class="n">current_length</span><span class="p">,</span> <span class="n">mode</span><span class="p">,</span> <span class="n">max_curvature</span><span class="p">,</span>
+                                           <span class="n">origin_x</span><span class="p">,</span> <span class="n">origin_y</span><span class="p">,</span> <span class="n">origin_yaw</span><span class="p">,</span>
+                                           <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span><span class="p">)</span>
+            <span class="n">current_length</span> <span class="o">+=</span> <span class="n">step_size</span>
 
-        <span class="k">while</span> <span class="nb">abs</span><span class="p">(</span><span class="n">pd</span><span class="p">)</span> <span class="o">&lt;=</span> <span class="nb">abs</span><span class="p">(</span><span class="n">length</span><span class="p">):</span>
-            <span class="n">ind</span> <span class="o">+=</span> <span class="mi">1</span>
-            <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span><span class="p">,</span> <span class="n">directions</span> <span class="o">=</span> <span class="n">interpolate</span><span class="p">(</span><span class="n">ind</span><span class="p">,</span> <span class="n">pd</span><span class="p">,</span> <span class="n">m</span><span class="p">,</span>
-                                                      <span class="n">max_curvature</span><span class="p">,</span>
-                                                      <span class="n">origin_x</span><span class="p">,</span>
-                                                      <span class="n">origin_y</span><span class="p">,</span>
-                                                      <span class="n">origin_yaw</span><span class="p">,</span>
-                                                      <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span>
-                                                      <span class="n">p_yaw</span><span class="p">,</span>
-                                                      <span class="n">directions</span><span class="p">)</span>
-            <span class="n">pd</span> <span class="o">+=</span> <span class="n">dist</span>
+        <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span> <span class="o">=</span> <span class="n">_interpolate</span><span class="p">(</span><span class="n">length</span><span class="p">,</span> <span class="n">mode</span><span class="p">,</span> <span class="n">max_curvature</span><span class="p">,</span> <span class="n">origin_x</span><span class="p">,</span>
+                                       <span class="n">origin_y</span><span class="p">,</span> <span class="n">origin_yaw</span><span class="p">,</span> <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span><span class="p">)</span>
 
-        <span class="n">ll</span> <span class="o">=</span> <span class="n">length</span> <span class="o">-</span> <span class="n">pd</span> <span class="o">-</span> <span class="n">dist</span>  <span class="c1"># calc remain length</span>
-
-        <span class="n">ind</span> <span class="o">+=</span> <span class="mi">1</span>
-        <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span><span class="p">,</span> <span class="n">directions</span> <span class="o">=</span> <span class="n">interpolate</span><span class="p">(</span><span class="n">ind</span><span class="p">,</span> <span class="n">length</span><span class="p">,</span> <span class="n">m</span><span class="p">,</span>
-                                                  <span class="n">max_curvature</span><span class="p">,</span>
-                                                  <span class="n">origin_x</span><span class="p">,</span> <span class="n">origin_y</span><span class="p">,</span>
-                                                  <span class="n">origin_yaw</span><span class="p">,</span>
-                                                  <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span><span class="p">,</span>
-                                                  <span class="n">directions</span><span class="p">)</span>
-
-    <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">p_x</span><span class="p">)</span> <span class="o">&lt;=</span> <span class="mi">1</span><span class="p">:</span>
-        <span class="k">return</span> <span class="p">[],</span> <span class="p">[],</span> <span class="p">[],</span> <span class="p">[]</span>
-
-    <span class="c1"># remove unused data</span>
-    <span class="k">while</span> <span class="nb">len</span><span class="p">(</span><span class="n">p_x</span><span class="p">)</span> <span class="o">&gt;=</span> <span class="mi">1</span> <span class="ow">and</span> <span class="n">p_x</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span> <span class="o">==</span> <span class="mf">0.0</span><span class="p">:</span>
-        <span class="n">p_x</span><span class="o">.</span><span class="n">pop</span><span class="p">()</span>
-        <span class="n">p_y</span><span class="o">.</span><span class="n">pop</span><span class="p">()</span>
-        <span class="n">p_yaw</span><span class="o">.</span><span class="n">pop</span><span class="p">()</span>
-        <span class="n">directions</span><span class="o">.</span><span class="n">pop</span><span class="p">()</span>
-
-    <span class="k">return</span> <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span><span class="p">,</span> <span class="n">directions</span>
-
-
-<span class="k">def</span> <span class="nf">plot_arrow</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">yaw</span><span class="p">,</span> <span class="n">length</span><span class="o">=</span><span class="mf">1.0</span><span class="p">,</span> <span class="n">width</span><span class="o">=</span><span class="mf">0.5</span><span class="p">,</span> <span class="n">fc</span><span class="o">=</span><span class="s2">&quot;r&quot;</span><span class="p">,</span>
-               <span class="n">ec</span><span class="o">=</span><span class="s2">&quot;k&quot;</span><span class="p">):</span>  <span class="c1"># pragma: no cover</span>
-    <span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
-    <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="nb">float</span><span class="p">):</span>
-        <span class="k">for</span> <span class="p">(</span><span class="n">i_x</span><span class="p">,</span> <span class="n">i_y</span><span class="p">,</span> <span class="n">i_yaw</span><span class="p">)</span> <span class="ow">in</span> <span class="nb">zip</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">yaw</span><span class="p">):</span>
-            <span class="n">plot_arrow</span><span class="p">(</span><span class="n">i_x</span><span class="p">,</span> <span class="n">i_y</span><span class="p">,</span> <span class="n">i_yaw</span><span class="p">)</span>
-    <span class="k">else</span><span class="p">:</span>
-        <span class="n">plt</span><span class="o">.</span><span class="n">arrow</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">length</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">yaw</span><span class="p">),</span> <span class="n">length</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">yaw</span><span class="p">),</span> <span class="n">fc</span><span class="o">=</span><span class="n">fc</span><span class="p">,</span>
-                  <span class="n">ec</span><span class="o">=</span><span class="n">ec</span><span class="p">,</span> <span class="n">head_width</span><span class="o">=</span><span class="n">width</span><span class="p">,</span> <span class="n">head_length</span><span class="o">=</span><span class="n">width</span><span class="p">)</span>
-        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
+    <span class="k">return</span> <span class="n">p_x</span><span class="p">,</span> <span class="n">p_y</span><span class="p">,</span> <span class="n">p_yaw</span>
 
 
 <span class="k">def</span> <span class="nf">main</span><span class="p">():</span>
     <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;Dubins path planner sample start!!&quot;</span><span class="p">)</span>
     <span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
+    <span class="kn">from</span> <span class="nn">utils.plot</span> <span class="kn">import</span> <span class="n">plot_arrow</span>
 
     <span class="n">start_x</span> <span class="o">=</span> <span class="mf">1.0</span>  <span class="c1"># [m]</span>
     <span class="n">start_y</span> <span class="o">=</span> <span class="mf">1.0</span>  <span class="c1"># [m]</span>
@@ -464,12 +400,9 @@
                                                                <span class="n">curvature</span><span class="p">)</span>
 
     <span class="k">if</span> <span class="n">show_animation</span><span class="p">:</span>
-        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="s2">&quot;final course &quot;</span> <span class="o">+</span> <span class="s2">&quot;&quot;</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">mode</span><span class="p">))</span>
-
-        <span class="c1"># plotting</span>
+        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="s2">&quot;&quot;</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">mode</span><span class="p">))</span>
         <span class="n">plot_arrow</span><span class="p">(</span><span class="n">start_x</span><span class="p">,</span> <span class="n">start_y</span><span class="p">,</span> <span class="n">start_yaw</span><span class="p">)</span>
         <span class="n">plot_arrow</span><span class="p">(</span><span class="n">end_x</span><span class="p">,</span> <span class="n">end_y</span><span class="p">,</span> <span class="n">end_yaw</span><span class="p">)</span>
-
         <span class="n">plt</span><span class="o">.</span><span class="n">legend</span><span class="p">()</span>
         <span class="n">plt</span><span class="o">.</span><span class="n">grid</span><span class="p">(</span><span class="kc">True</span><span class="p">)</span>
         <span class="n">plt</span><span class="o">.</span><span class="n">axis</span><span class="p">(</span><span class="s2">&quot;equal&quot;</span><span class="p">)</span>

_modules/index.html

@@ -6,6 +6,7 @@
   <title>Overview: module code &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../_static/js/html5shiv.min.js"></script>

_sources/modules/path_planning/dubins_path/dubins_path_main.rst.txt

@@ -9,40 +9,68 @@ Dubins path
 ~~~~~~~~~~~~
 Dubins path is a analytical path planning algorithm for a simple car model.
 
-It can generates a shortest path between 2D poses (x, y, yaw) with maximum curvture comstraint and tangent(yaw angle) constraint.
+It can generates a shortest path between two 2D poses (x, y, yaw) with maximum curvature constraint and tangent(yaw angle) constraint.
 
-The path consist of 3 segments of maximum curvature curves or a straight line segment.
+Generated paths consist of 3 segments of maximum curvature curves or a straight line segment.
 
 Each segment type can is categorized by 3 type: 'Right turn (R)' , 'Left turn (L)', and 'Straight (S).' 
 
-Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL. 
+Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL.
 
-For example, one of RSR Dubins paths is:
+Dubins path planner can output each segment type and distance of each course segment.
+
+For example, a RSR Dubins path is:
 
 .. image:: RSR.jpg
    :width: 400px
 
-one of RLR Dubins paths is:
+Each segment distance can be calculated by:
+
+:math:`\alpha = mod(-\theta)`
+
+:math:`\beta = mod(x_{e, yaw} - \theta)`
+
+:math:`p^2 = 2 + d ^ 2 - 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)`
+
+:math:`t = atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta)`
+
+:math:`d_1 = mod(-\alpha + t)`
+
+:math:`d_2 = p`
+
+:math:`d_3 = mod(\beta - t)`
+
+where :math:`\theta` is tangent and d is distance from :math:`x_s` to :math:`x_e`
+
+A RLR Dubins path is:
 
 .. image:: RLR.jpg
    :width: 200px
 
-Dubins path planner can output three types and distances of each course segment.
+Each segment distance can be calculated by:
+
+:math:`t = (6.0 - d^2 + 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)) / 8.0`
+
+:math:`d_2 = mod(2\pi - acos(t))`
+
+:math:`d_1 = mod(\alpha - atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta) + d_2 / 2.0)`
+
+:math:`d_3 = mod(\alpha - \beta - d_1 + d_2)`
 
 You can generate a path from these information and the maximum curvature information.
 
-In the example code, a path which is minimum course length one among 6 course type is selected and then a path is constructed.
+A path type which has minimum course length among 6 types is selected,
+and then a path is constructed based on the selected type and its distances.
 
-Code
+API
 ~~~~~~~~~~~~~~~~~~~~
 
-.. automodule:: PathPlanning.DubinsPath.dubins_path_planner
-    :members:
+.. autofunction:: PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path
 
 
 Reference
 ~~~~~~~~~~~~~~~~~~~~
-
+-  `On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents <https://www.jstor.org/stable/2372560?origin=crossref>`__
 -  `Dubins path - Wikipedia <https://en.wikipedia.org/wiki/Dubins_path>`__
 -  `15.3.1 Dubins Curves <http://planning.cs.uiuc.edu/node821.html>`__
 -  `A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths <https://gieseanw.wordpress.com/2012/10/21/a-comprehensive-step-by-step-tutorial-to-computing-dubins-paths/>`__

_static/plot_directive.css

@@ -0,0 +1,16 @@
+/*
+ * plot_directive.css
+ * ~~~~~~~~~~~~
+ *
+ * Stylesheet controlling images created using the `plot` directive within
+ * Sphinx.
+ *
+ * :copyright: Copyright 2020-* by the Matplotlib development team.
+ * :license: Matplotlib, see LICENSE for details.
+ *
+ */
+
+img.plot-directive {
+    border: 0;
+    max-width: 100%;
+}

genindex.html

@@ -6,6 +6,7 @@
   <title>Index &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="_static/js/html5shiv.min.js"></script>
@@ -95,34 +96,11 @@
 <h1 id="index">Index</h1>
 
 <div class="genindex-jumpbox">
- <a href="#M"><strong>M</strong></a>
- | <a href="#P"><strong>P</strong></a>
+ <a href="#P"><strong>P</strong></a>
  
 </div>
-<h2 id="M">M</h2>
-<table style="width: 100%" class="indextable genindextable"><tr>
-  <td style="width: 33%; vertical-align: top;"><ul>
-      <li>
-    module
-
-      <ul>
-        <li><a href="modules/path_planning/dubins_path/dubins_path.html#module-PathPlanning.DubinsPath.dubins_path_planner">PathPlanning.DubinsPath.dubins_path_planner</a>
-</li>
-      </ul></li>
-  </ul></td>
-</tr></table>
-
 <h2 id="P">P</h2>
 <table style="width: 100%" class="indextable genindextable"><tr>
-  <td style="width: 33%; vertical-align: top;"><ul>
-      <li>
-    PathPlanning.DubinsPath.dubins_path_planner
-
-      <ul>
-        <li><a href="modules/path_planning/dubins_path/dubins_path.html#module-PathPlanning.DubinsPath.dubins_path_planner">module</a>
-</li>
-      </ul></li>
-  </ul></td>
   <td style="width: 33%; vertical-align: top;"><ul>
       <li><a href="modules/path_planning/dubins_path/dubins_path.html#PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path">plan_dubins_path() (in module PathPlanning.DubinsPath.dubins_path_planner)</a>
 </li>

getting_started.html

@@ -7,6 +7,7 @@
   <title>Getting Started &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="_static/js/html5shiv.min.js"></script>

how_to_contribute.html

@@ -7,6 +7,7 @@
   <title>How To Contribute &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="_static/js/html5shiv.min.js"></script>

index.html

@@ -7,6 +7,7 @@
   <title>Welcome to PythonRobotics’s documentation! &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="_static/js/html5shiv.min.js"></script>

modules/aerial_navigation/aerial_navigation.html

@@ -7,6 +7,7 @@
   <title>Aerial Navigation &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following.html

@@ -7,6 +7,7 @@
   <title>Drone 3d trajectory following &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing.html

@@ -7,6 +7,7 @@
   <title>Rocket powered landing &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/appendix/Kalmanfilter_basics.html

@@ -7,6 +7,7 @@
   <title>KF Basics - Part I &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/appendix/Kalmanfilter_basics_2.html

@@ -7,6 +7,7 @@
   <title>KF Basics - Part 2 &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/appendix/appendix.html

@@ -7,6 +7,7 @@
   <title>Appendix &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/arm_navigation/arm_navigation.html

@@ -7,6 +7,7 @@
   <title>Arm Navigation &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/arm_navigation/n_joint_arm_to_point_control.html

@@ -7,6 +7,7 @@
   <title>N joint arm to point control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/arm_navigation/obstacle_avoidance_arm_navigation.html

@@ -7,6 +7,7 @@
   <title>Arm navigation with obstacle avoidance &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/arm_navigation/planar_two_link_ik.html

@@ -7,6 +7,7 @@
   <title>Two joint arm to point control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/bipedal/bipedal.html

@@ -7,6 +7,7 @@
   <title>Bipedal &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/bipedal/bipedal_planner/bipedal_planner.html

@@ -7,6 +7,7 @@
   <title>Bipedal Planner &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/control/control.html

@@ -7,6 +7,7 @@
   <title>Control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/control/inverted_pendulum_control/inverted_pendulum_control.html

@@ -7,6 +7,7 @@
   <title>Inverted Pendulum Control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/control/move_to_a_pose_control/move_to_a_pose_control.html

@@ -7,6 +7,7 @@
   <title>Move to a Pose Control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/introduction.html

@@ -7,6 +7,7 @@
   <title>Introduction &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../_static/js/html5shiv.min.js"></script>

modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization.html

@@ -7,6 +7,7 @@
   <title>Ensamble Kalman Filter Localization &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html

@@ -7,6 +7,7 @@
   <title>Extended Kalman Filter Localization &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/localization/histogram_filter_localization/histogram_filter_localization.html

@@ -7,6 +7,7 @@
   <title>Histogram filter localization &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/localization/localization.html

@@ -7,6 +7,7 @@
   <title>Localization &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/localization/particle_filter_localization/particle_filter_localization.html

@@ -7,6 +7,7 @@
   <title>Particle filter localization &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization.html

@@ -7,6 +7,7 @@
   <title>Unscented Kalman Filter localization &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/mapping/circle_fitting/circle_fitting.html

@@ -7,6 +7,7 @@
   <title>Object shape recognition using circle fitting &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/mapping/gaussian_grid_map/gaussian_grid_map.html

@@ -7,6 +7,7 @@
   <title>Gaussian grid map &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/mapping/k_means_object_clustering/k_means_object_clustering.html

@@ -7,6 +7,7 @@
   <title>k-means object clustering &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial.html

@@ -7,6 +7,7 @@
   <title>Lidar to grid map &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/mapping/mapping.html

@@ -7,6 +7,7 @@
   <title>Mapping &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/mapping/ray_casting_grid_map/ray_casting_grid_map.html

@@ -7,6 +7,7 @@
   <title>Ray casting grid map &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/mapping/rectangle_fitting/rectangle_fitting.html

@@ -7,6 +7,7 @@
   <title>Object shape recognition using rectangle fitting &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/bezier_path/bezier_path.html

@@ -7,6 +7,7 @@
   <title>Bezier path planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/bspline_path/bspline_path.html

@@ -7,6 +7,7 @@
   <title>B-Spline planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/bugplanner/bugplanner.html

@@ -7,6 +7,7 @@
   <title>Bug planner &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/clothoid_path/clothoid_path.html

@@ -7,6 +7,7 @@
   <title>Clothoid path planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/coverage_path/coverage_path.html

@@ -7,6 +7,7 @@
   <title>Coverage path planner &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/cubic_spline/cubic_spline.html

@@ -7,6 +7,7 @@
   <title>Cubic spline planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/dubins_path/dubins_path.html

@@ -7,6 +7,7 @@
   <title>Dubins path planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>
@@ -16,6 +17,7 @@
         <script src="../../../_static/jquery.js"></script>
         <script src="../../../_static/underscore.js"></script>
         <script src="../../../_static/doctools.js"></script>
+        <script async="async" src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
     <script src="../../../_static/js/theme.js"></script>
     <link rel="index" title="Index" href="../../../genindex.html" />
     <link rel="search" title="Search" href="../../../search.html" />
@@ -78,7 +80,7 @@
 <li class="toctree-l2"><a class="reference internal" href="../quintic_polynomials_planner/quintic_polynomials_planner.html">Quintic polynomials planning</a></li>
 <li class="toctree-l2 current"><a class="current reference internal" href="#">Dubins path planning</a><ul>
 <li class="toctree-l3"><a class="reference internal" href="#dubins-path">Dubins path</a></li>
-<li class="toctree-l3"><a class="reference internal" href="#module-PathPlanning.DubinsPath.dubins_path_planner">Code</a></li>
+<li class="toctree-l3"><a class="reference internal" href="#api">API</a></li>
 <li class="toctree-l3"><a class="reference internal" href="#reference">Reference</a></li>
 </ul>
 </li>
@@ -130,26 +132,39 @@
 <section id="dubins-path">
 <h2>Dubins path<a class="headerlink" href="#dubins-path" title="Permalink to this headline"></a></h2>
 <p>Dubins path is a analytical path planning algorithm for a simple car model.</p>
-<p>It can generates a shortest path between 2D poses (x, y, yaw) with maximum curvture comstraint and tangent(yaw angle) constraint.</p>
-<p>The path consist of 3 segments of maximum curvature curves or a straight line segment.</p>
+<p>It can generates a shortest path between two 2D poses (x, y, yaw) with maximum curvature constraint and tangent(yaw angle) constraint.</p>
+<p>Generated paths consist of 3 segments of maximum curvature curves or a straight line segment.</p>
 <p>Each segment type can is categorized by 3 type: ‘Right turn (R)’ , ‘Left turn (L)’, and ‘Straight (S).’</p>
 <p>Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL.</p>
-<p>For example, one of RSR Dubins paths is:</p>
+<p>Dubins path planner can output each segment type and distance of each course segment.</p>
+<p>For example, a RSR Dubins path is:</p>
 <a class="reference internal image-reference" href="../../../_images/RSR.jpg"><img alt="../../../_images/RSR.jpg" src="../../../_images/RSR.jpg" style="width: 400px;" /></a>
-<p>one of RLR Dubins paths is:</p>
+<p>Each segment distance can be calculated by:</p>
+<p><span class="math notranslate nohighlight">\(\alpha = mod(-\theta)\)</span></p>
+<p><span class="math notranslate nohighlight">\(\beta = mod(x_{e, yaw} - \theta)\)</span></p>
+<p><span class="math notranslate nohighlight">\(p^2 = 2 + d ^ 2 - 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)\)</span></p>
+<p><span class="math notranslate nohighlight">\(t = atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta)\)</span></p>
+<p><span class="math notranslate nohighlight">\(d_1 = mod(-\alpha + t)\)</span></p>
+<p><span class="math notranslate nohighlight">\(d_2 = p\)</span></p>
+<p><span class="math notranslate nohighlight">\(d_3 = mod(\beta - t)\)</span></p>
+<p>where <span class="math notranslate nohighlight">\(\theta\)</span> is tangent and d is distance from <span class="math notranslate nohighlight">\(x_s\)</span> to <span class="math notranslate nohighlight">\(x_e\)</span></p>
+<p>A RLR Dubins path is:</p>
 <a class="reference internal image-reference" href="../../../_images/RLR.jpg"><img alt="../../../_images/RLR.jpg" src="../../../_images/RLR.jpg" style="width: 200px;" /></a>
-<p>Dubins path planner can output three types and distances of each course segment.</p>
+<p>Each segment distance can be calculated by:</p>
+<p><span class="math notranslate nohighlight">\(t = (6.0 - d^2 + 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)) / 8.0\)</span></p>
+<p><span class="math notranslate nohighlight">\(d_2 = mod(2\pi - acos(t))\)</span></p>
+<p><span class="math notranslate nohighlight">\(d_1 = mod(\alpha - atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta) + d_2 / 2.0)\)</span></p>
+<p><span class="math notranslate nohighlight">\(d_3 = mod(\alpha - \beta - d_1 + d_2)\)</span></p>
 <p>You can generate a path from these information and the maximum curvature information.</p>
-<p>In the example code, a path which is minimum course length one among 6 course type is selected and then a path is constructed.</p>
+<p>A path type which has minimum course length among 6 types is selected,
+and then a path is constructed based on the selected type and its distances.</p>
 </section>
-<section id="module-PathPlanning.DubinsPath.dubins_path_planner">
-<span id="code"></span><h2>Code<a class="headerlink" href="#module-PathPlanning.DubinsPath.dubins_path_planner" title="Permalink to this headline"></a></h2>
-<p>Dubins path planner sample code</p>
-<p>author Atsushi Sakai(&#64;Atsushi_twi)</p>
+<section id="api">
+<h2>API<a class="headerlink" href="#api" title="Permalink to this headline"></a></h2>
 <dl class="py function">
 <dt class="sig sig-object py" id="PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path">
-<span class="sig-prename descclassname"><span class="pre">PathPlanning.DubinsPath.dubins_path_planner.</span></span><span class="sig-name descname"><span class="pre">plan_dubins_path</span></span><span class="sig-paren">(</span><em class="sig-param"><span class="n"><span class="pre">s_x</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">s_y</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">s_yaw</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">g_x</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">g_y</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">g_yaw</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">curvature</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">step_size</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">0.1</span></span></em><span class="sig-paren">)</span><a class="reference internal" href="../../../_modules/PathPlanning/DubinsPath/dubins_path_planner.html#plan_dubins_path"><span class="viewcode-link"><span class="pre">[source]</span></span></a><a class="headerlink" href="#PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path" title="Permalink to this definition"></a></dt>
-<dd><p>Path dubins path</p>
+<span class="sig-prename descclassname"><span class="pre">PathPlanning.DubinsPath.dubins_path_planner.</span></span><span class="sig-name descname"><span class="pre">plan_dubins_path</span></span><span class="sig-paren">(</span><em class="sig-param"><span class="n"><span class="pre">s_x</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">s_y</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">s_yaw</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">g_x</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">g_y</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">g_yaw</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">curvature</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">step_size</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">0.1</span></span></em>, <em class="sig-param"><span class="n"><span class="pre">selected_types</span></span><span class="o"><span class="pre">=</span></span><span class="default_value"><span class="pre">None</span></span></em><span class="sig-paren">)</span><a class="reference internal" href="../../../_modules/PathPlanning/DubinsPath/dubins_path_planner.html#plan_dubins_path"><span class="viewcode-link"><span class="pre">[source]</span></span></a><a class="headerlink" href="#PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path" title="Permalink to this definition"></a></dt>
+<dd><p>Plan dubins path</p>
 <dl class="field-list simple">
 <dt class="field-odd">Parameters</dt>
 <dd class="field-odd"><ul class="simple">
@@ -161,6 +176,10 @@
 <li><p><strong>g_yaw</strong> (<em>float</em>) – yaw angle of the end point [rad]</p></li>
 <li><p><strong>curvature</strong> (<em>float</em>) – curvature for curve [1/m]</p></li>
 <li><p><strong>step_size</strong> (<em>float</em><em> (</em><em>optional</em><em>)</em>) – step size between two path points [m]. Default is 0.1</p></li>
+<li><p><strong>selected_types</strong> (<em>a list of string</em><em> or </em><em>None</em>) – selected path planning types. If None, all types are used for
+path planning, and minimum path length result is returned.
+You can select used path plannings types by a string list.
+e.g.: [“RSL”, “RSR”]</p></li>
 </ul>
 </dd>
 <dt class="field-even">Returns</dt>
@@ -169,17 +188,39 @@
 <li><p><strong>y_list</strong> (<em>array</em>) – y positions of the path</p></li>
 <li><p><strong>yaw_list</strong> (<em>array</em>) – yaw angles of the path</p></li>
 <li><p><strong>modes</strong> (<em>array</em>) – mode list of the path</p></li>
-<li><p><strong>lengths</strong> (<em>array</em>) – length list of the path segments.</p></li>
+<li><p><strong>lengths</strong> (<em>array</em>) – arrow_length list of the path segments.</p></li>
 </ul>
 </p>
 </dd>
 </dl>
+<p class="rubric">Examples</p>
+<p>You can generate a dubins path.</p>
+<div class="doctest highlight-default notranslate"><div class="highlight"><pre><span></span><span class="gp">&gt;&gt;&gt; </span><span class="n">start_x</span> <span class="o">=</span> <span class="mf">1.0</span>  <span class="c1"># [m]</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">start_y</span> <span class="o">=</span> <span class="mf">1.0</span>  <span class="c1"># [m]</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">start_yaw</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">deg2rad</span><span class="p">(</span><span class="mf">45.0</span><span class="p">)</span>  <span class="c1"># [rad]</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">end_x</span> <span class="o">=</span> <span class="o">-</span><span class="mf">3.0</span>  <span class="c1"># [m]</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">end_y</span> <span class="o">=</span> <span class="o">-</span><span class="mf">3.0</span>  <span class="c1"># [m]</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">end_yaw</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">deg2rad</span><span class="p">(</span><span class="o">-</span><span class="mf">45.0</span><span class="p">)</span>  <span class="c1"># [rad]</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">curvature</span> <span class="o">=</span> <span class="mf">1.0</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">path_yaw</span><span class="p">,</span> <span class="n">mode</span><span class="p">,</span> <span class="n">_</span> <span class="o">=</span> <span class="n">plan_dubins_path</span><span class="p">(</span>
+<span class="go">            start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">path_x</span><span class="p">,</span> <span class="n">path_y</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="s2">&quot;final course &quot;</span> <span class="o">+</span> <span class="s2">&quot;&quot;</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">mode</span><span class="p">))</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">plot_arrow</span><span class="p">(</span><span class="n">start_x</span><span class="p">,</span> <span class="n">start_y</span><span class="p">,</span> <span class="n">start_yaw</span><span class="p">)</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">plot_arrow</span><span class="p">(</span><span class="n">end_x</span><span class="p">,</span> <span class="n">end_y</span><span class="p">,</span> <span class="n">end_yaw</span><span class="p">)</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">plt</span><span class="o">.</span><span class="n">legend</span><span class="p">()</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">plt</span><span class="o">.</span><span class="n">grid</span><span class="p">(</span><span class="kc">True</span><span class="p">)</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">plt</span><span class="o">.</span><span class="n">axis</span><span class="p">(</span><span class="s2">&quot;equal&quot;</span><span class="p">)</span>
+<span class="gp">&gt;&gt;&gt; </span><span class="n">plt</span><span class="o">.</span><span class="n">show</span><span class="p">()</span>
+</pre></div>
+</div>
+<img alt="../../../_images/dubins_path.jpg" src="../../../_images/dubins_path.jpg" />
 </dd></dl>
 
 </section>
 <section id="reference">
 <h2>Reference<a class="headerlink" href="#reference" title="Permalink to this headline"></a></h2>
 <ul class="simple">
+<li><p><a class="reference external" href="https://www.jstor.org/stable/2372560?origin=crossref">On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents</a></p></li>
 <li><p><a class="reference external" href="https://en.wikipedia.org/wiki/Dubins_path">Dubins path - Wikipedia</a></p></li>
 <li><p><a class="reference external" href="http://planning.cs.uiuc.edu/node821.html">15.3.1 Dubins Curves</a></p></li>
 <li><p><a class="reference external" href="https://gieseanw.wordpress.com/2012/10/21/a-comprehensive-step-by-step-tutorial-to-computing-dubins-paths/">A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths</a></p></li>

modules/path_planning/dynamic_window_approach/dynamic_window_approach.html

@@ -7,6 +7,7 @@
   <title>Dynamic Window Approach &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/eta3_spline/eta3_spline.html

@@ -7,6 +7,7 @@
   <title>Eta^3 Spline path planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/frenet_frame_path/frenet_frame_path.html

@@ -7,6 +7,7 @@
   <title>Optimal Trajectory in a Frenet Frame &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/grid_base_search/grid_base_search.html

@@ -7,6 +7,7 @@
   <title>Grid based search &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/hybridastar/hybridastar.html

@@ -7,6 +7,7 @@
   <title>Hybrid a star &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/lqr_path/lqr_path.html

@@ -7,6 +7,7 @@
   <title>LQR based path planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator.html

@@ -7,6 +7,7 @@
   <title>Model Predictive Trajectory Generator &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/path_planning.html

@@ -7,6 +7,7 @@
   <title>Path Planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>
@@ -183,7 +184,7 @@
 </li>
 <li class="toctree-l1"><a class="reference internal" href="dubins_path/dubins_path.html">Dubins path planning</a><ul>
 <li class="toctree-l2"><a class="reference internal" href="dubins_path/dubins_path.html#dubins-path">Dubins path</a></li>
-<li class="toctree-l2"><a class="reference internal" href="dubins_path/dubins_path.html#module-PathPlanning.DubinsPath.dubins_path_planner">Code</a></li>
+<li class="toctree-l2"><a class="reference internal" href="dubins_path/dubins_path.html#api">API</a></li>
 <li class="toctree-l2"><a class="reference internal" href="dubins_path/dubins_path.html#reference">Reference</a></li>
 </ul>
 </li>

modules/path_planning/prm_planner/prm_planner.html

@@ -7,6 +7,7 @@
   <title>Probabilistic Road-Map (PRM) planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner.html

@@ -7,6 +7,7 @@
   <title>Quintic polynomials planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/reeds_shepp_path/reeds_shepp_path.html

@@ -7,6 +7,7 @@
   <title>Reeds Shepp planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/rrt/rrt.html

@@ -7,6 +7,7 @@
   <title>Rapidly-Exploring Random Trees (RRT) &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/state_lattice_planner/state_lattice_planner.html

@@ -7,6 +7,7 @@
   <title>State Lattice Planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/visibility_road_map_planner/visibility_road_map_planner.html

@@ -7,6 +7,7 @@
   <title>Visibility Road-Map planner &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_planning/vrm_planner/vrm_planner.html

@@ -7,6 +7,7 @@
   <title>Voronoi Road-Map planning &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/cgmres_nmpc/cgmres_nmpc.html

@@ -7,6 +7,7 @@
   <title>Nonlinear Model Predictive Control with C-GMRES &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control.html

@@ -7,6 +7,7 @@
   <title>Linear–quadratic regulator (LQR) speed and steering control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/lqr_steering_control/lqr_steering_control.html

@@ -7,6 +7,7 @@
   <title>Linear–quadratic regulator (LQR) steering control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html

@@ -7,6 +7,7 @@
   <title>Model predictive speed and steering control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/path_tracking.html

@@ -7,6 +7,7 @@
   <title>Path Tracking &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking.html

@@ -7,6 +7,7 @@
   <title>Pure pursuit tracking &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control.html

@@ -7,6 +7,7 @@
   <title>Rear wheel feedback control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/path_tracking/stanley_control/stanley_control.html

@@ -7,6 +7,7 @@
   <title>Stanley control &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/slam/FastSLAM1/FastSLAM1.html

@@ -7,6 +7,7 @@
   <title>FastSLAM1.0 &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/slam/FastSLAM2/FastSLAM2.html

@@ -7,6 +7,7 @@
   <title>FastSLAM 2.0 &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/slam/ekf_slam/ekf_slam.html

@@ -7,6 +7,7 @@
   <title>EKF SLAM &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/slam/graph_slam/graph_slam.html

@@ -7,6 +7,7 @@
   <title>Graph based SLAM &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/slam/iterative_closest_point_matching/iterative_closest_point_matching.html

@@ -7,6 +7,7 @@
   <title>Iterative Closest Point (ICP) Matching &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../../_static/js/html5shiv.min.js"></script>

modules/slam/slam.html

@@ -7,6 +7,7 @@
   <title>SLAM &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="../../_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
   <!--[if lt IE 9]>
     <script src="../../_static/js/html5shiv.min.js"></script>

py-modindex.html

@@ -1,148 +0,0 @@
-<!DOCTYPE html>
-<html class="writer-html5" lang="en" >
-<head>
-  <meta charset="utf-8" />
-  <meta name="viewport" content="width=device-width, initial-scale=1.0" />
-  <title>Python Module Index &mdash; PythonRobotics  documentation</title>
-      <link rel="stylesheet" href="_static/pygments.css" type="text/css" />
-      <link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
-      <link rel="stylesheet" href="_static/custom.css" type="text/css" />
-  <!--[if lt IE 9]>
-    <script src="_static/js/html5shiv.min.js"></script>
-  <![endif]-->
-  
-        <script data-url_root="./" id="documentation_options" src="_static/documentation_options.js"></script>
-        <script src="_static/jquery.js"></script>
-        <script src="_static/underscore.js"></script>
-        <script src="_static/doctools.js"></script>
-    <script src="_static/js/theme.js"></script>
-    <link rel="index" title="Index" href="genindex.html" />
-    <link rel="search" title="Search" href="search.html" />
- 
-
-
-</head>
-
-<body class="wy-body-for-nav"> 
-  <div class="wy-grid-for-nav">
-    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
-      <div class="wy-side-scroll">
-        <div class="wy-side-nav-search" >
-
-            <a href="index.html" class="icon icon-home"> PythonRobotics
-            <img src="_static/icon.png" class="logo" alt="Logo"/>
-          </a>
-<div role="search">
-  <form id="rtd-search-form" class="wy-form" action="search.html" method="get">
-    <input type="text" name="q" placeholder="Search docs" />
-    <input type="hidden" name="check_keywords" value="yes" />
-    <input type="hidden" name="area" value="default" />
-  </form>
-</div>
-<script async src="https://pagead2.googlesyndication.com/pagead/js/adsbygoogle.js?client=ca-pub-9612347954373886"
-     crossorigin="anonymous"></script>
-<!-- PythonRoboticsDoc -->
-<ins class="adsbygoogle"
-     style="display:block"
-     data-ad-client="ca-pub-9612347954373886"
-     data-ad-slot="1579532132"
-     data-ad-format="auto"
-     data-full-width-responsive="true"></ins>
-<script>
-     (adsbygoogle = window.adsbygoogle || []).push({});
-</script>
-
-        </div><div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="Navigation menu">
-              <p class="caption" role="heading"><span class="caption-text">Contents</span></p>
-<ul>
-<li class="toctree-l1"><a class="reference internal" href="getting_started.html">Getting Started</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/introduction.html">Introduction</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/localization/localization.html">Localization</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/mapping/mapping.html">Mapping</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/slam/slam.html">SLAM</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/path_planning/path_planning.html">Path Planning</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/path_tracking/path_tracking.html">Path Tracking</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/arm_navigation/arm_navigation.html">Arm Navigation</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/aerial_navigation/aerial_navigation.html">Aerial Navigation</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/bipedal/bipedal.html">Bipedal</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/control/control.html">Control</a></li>
-<li class="toctree-l1"><a class="reference internal" href="modules/appendix/appendix.html">Appendix</a></li>
-<li class="toctree-l1"><a class="reference internal" href="how_to_contribute.html">How To Contribute</a></li>
-</ul>
-
-        </div>
-      </div>
-    </nav>
-
-    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap"><nav class="wy-nav-top" aria-label="Mobile navigation menu" >
-          <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
-          <a href="index.html">PythonRobotics</a>
-      </nav>
-
-      <div class="wy-nav-content">
-        <div class="rst-content">
-          <div role="navigation" aria-label="Page navigation">
-  <ul class="wy-breadcrumbs">
-      <li><a href="index.html" class="icon icon-home"></a> &raquo;</li>
-      <li>Python Module Index</li>
-      <li class="wy-breadcrumbs-aside">
-      </li>
-  </ul>
-  <hr/>
-</div>
-          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
-           <div itemprop="articleBody">
-             
-
-   <h1>Python Module Index</h1>
-
-   <div class="modindex-jumpbox">
-   <a href="#cap-p"><strong>p</strong></a>
-   </div>
-
-   <table class="indextable modindextable">
-     <tr class="pcap"><td></td><td>&#160;</td><td></td></tr>
-     <tr class="cap" id="cap-p"><td></td><td>
-       <strong>p</strong></td><td></td></tr>
-     <tr>
-       <td><img src="_static/minus.png" class="toggler"
-              id="toggle-1" style="display: none" alt="-" /></td>
-       <td>
-       <code class="xref">PathPlanning</code></td><td>
-       <em></em></td></tr>
-     <tr class="cg-1">
-       <td></td>
-       <td>&#160;&#160;&#160;
-       <a href="modules/path_planning/dubins_path/dubins_path.html#module-PathPlanning.DubinsPath.dubins_path_planner"><code class="xref">PathPlanning.DubinsPath.dubins_path_planner</code></a></td><td>
-       <em></em></td></tr>
-   </table>
-
-
-           </div>
-          </div>
-          <footer>
-
-  <hr/>
-
-  <div role="contentinfo">
-    <p>&#169; Copyright 2018-2021, Atsushi Sakai.</p>
-  </div>
-
-  Built with <a href="https://www.sphinx-doc.org/">Sphinx</a> using a
-    <a href="https://github.com/readthedocs/sphinx_rtd_theme">theme</a>
-    provided by <a href="https://readthedocs.org">Read the Docs</a>.
-   
-
-</footer>
-        </div>
-      </div>
-    </section>
-  </div>
-  <script>
-      jQuery(function () {
-          SphinxRtdTheme.Navigation.enable(true);
-      });
-  </script> 
-
-</body>
-</html>

search.html

@@ -6,6 +6,7 @@
   <title>Search &mdash; PythonRobotics  documentation</title>
       <link rel="stylesheet" href="_static/pygments.css" type="text/css" />
       <link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
+      <link rel="stylesheet" href="_static/plot_directive.css" type="text/css" />
       <link rel="stylesheet" href="_static/custom.css" type="text/css" />
     
   <!--[if lt IE 9]>