mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:41 -04:00
450 lines
58 KiB
HTML
450 lines
58 KiB
HTML
<!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>PathPlanning.DubinsPath.dubins_path_planner — 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/copybutton.css" type="text/css" />
|
|
<link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
|
|
<link rel="stylesheet" href="../../../_static/dark_mode_css/general.css" type="text/css" />
|
|
<link rel="stylesheet" href="../../../_static/dark_mode_css/dark.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/clipboard.min.js"></script>
|
|
<script src="../../../_static/copybutton.js"></script>
|
|
<script src="../../../_static/dark_mode_js/default_dark.js"></script>
|
|
<script src="../../../_static/dark_mode_js/theme_switcher.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/utils/utils.html">Utilities</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> »</li>
|
|
<li><a href="../../index.html">Module code</a> »</li>
|
|
<li>PathPlanning.DubinsPath.dubins_path_planner</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>Source code for PathPlanning.DubinsPath.dubins_path_planner</h1><div class="highlight"><pre>
|
|
<span></span><span class="sd">"""</span>
|
|
|
|
<span class="sd">Dubins path planner sample code</span>
|
|
|
|
<span class="sd">author Atsushi Sakai(@Atsushi_twi)</span>
|
|
|
|
<span class="sd">"""</span>
|
|
<span class="kn">import</span> <span class="nn">sys</span>
|
|
<span class="kn">import</span> <span class="nn">pathlib</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="nb">str</span><span class="p">(</span><span class="n">pathlib</span><span class="o">.</span><span class="n">Path</span><span class="p">(</span><span class="vm">__file__</span><span class="p">)</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">parent</span><span class="p">))</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">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> <span class="n">selected_types</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
|
|
<span class="w"> </span><span class="sd">"""</span>
|
|
<span class="sd"> Plan dubins path</span>
|
|
|
|
<span class="sd"> Parameters</span>
|
|
<span class="sd"> ----------</span>
|
|
<span class="sd"> s_x : float</span>
|
|
<span class="sd"> x position of the start point [m]</span>
|
|
<span class="sd"> s_y : float</span>
|
|
<span class="sd"> y position of the start point [m]</span>
|
|
<span class="sd"> s_yaw : float</span>
|
|
<span class="sd"> yaw angle of the start point [rad]</span>
|
|
<span class="sd"> g_x : float</span>
|
|
<span class="sd"> x position of the goal point [m]</span>
|
|
<span class="sd"> g_y : float</span>
|
|
<span class="sd"> y position of the end point [m]</span>
|
|
<span class="sd"> g_yaw : float</span>
|
|
<span class="sd"> yaw angle of the end point [rad]</span>
|
|
<span class="sd"> curvature : float</span>
|
|
<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.: ["RSL", "RSR"]</span>
|
|
|
|
<span class="sd"> Returns</span>
|
|
<span class="sd"> -------</span>
|
|
<span class="sd"> x_list: array</span>
|
|
<span class="sd"> x positions of the path</span>
|
|
<span class="sd"> y_list: array</span>
|
|
<span class="sd"> y positions of the path</span>
|
|
<span class="sd"> yaw_list: array</span>
|
|
<span class="sd"> yaw angles of the path</span>
|
|
<span class="sd"> modes: array</span>
|
|
<span class="sd"> mode list of the path</span>
|
|
<span class="sd"> lengths: array</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"> >>> start_x = 1.0 # [m]</span>
|
|
<span class="sd"> >>> start_y = 1.0 # [m]</span>
|
|
<span class="sd"> >>> start_yaw = np.deg2rad(45.0) # [rad]</span>
|
|
<span class="sd"> >>> end_x = -3.0 # [m]</span>
|
|
<span class="sd"> >>> end_y = -3.0 # [m]</span>
|
|
<span class="sd"> >>> end_yaw = np.deg2rad(-45.0) # [rad]</span>
|
|
<span class="sd"> >>> curvature = 1.0</span>
|
|
<span class="sd"> >>> 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"> >>> plt.plot(path_x, path_y, label="final course " + "".join(mode))</span>
|
|
<span class="sd"> >>> plot_arrow(start_x, start_y, start_yaw)</span>
|
|
<span class="sd"> >>> plot_arrow(end_x, end_y, end_yaw)</span>
|
|
<span class="sd"> >>> plt.legend()</span>
|
|
<span class="sd"> >>> plt.grid(True)</span>
|
|
<span class="sd"> >>> plt.axis("equal")</span>
|
|
<span class="sd"> >>> plt.show()</span>
|
|
|
|
<span class="sd"> .. image:: dubins_path.jpg</span>
|
|
<span class="sd"> """</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">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">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">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>
|
|
<span class="n">yaw_list</span> <span class="o">=</span> <span class="n">angle_mod</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">lp_yaw</span><span class="p">)</span> <span class="o">+</span> <span class="n">s_yaw</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">modes</span><span class="p">,</span> <span class="n">lengths</span></div>
|
|
|
|
|
|
<span class="k">def</span> <span class="nf">_mod2pi</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
|
|
<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">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">"L"</span><span class="p">,</span> <span class="s2">"S"</span><span class="p">,</span> <span class="s2">"L"</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"><</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">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">_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">"R"</span><span class="p">,</span> <span class="s2">"S"</span><span class="p">,</span> <span class="s2">"R"</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"><</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">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">_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">"L"</span><span class="p">,</span> <span class="s2">"S"</span><span class="p">,</span> <span class="s2">"R"</span><span class="p">]</span>
|
|
<span class="k">if</span> <span class="n">p_squared</span> <span class="o"><</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">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">_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">"R"</span><span class="p">,</span> <span class="s2">"S"</span><span class="p">,</span> <span class="s2">"L"</span><span class="p">]</span>
|
|
<span class="k">if</span> <span class="n">p_squared</span> <span class="o"><</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">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">_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">"R"</span><span class="p">,</span> <span class="s2">"L"</span><span class="p">,</span> <span class="s2">"R"</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">></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">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">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">"L"</span><span class="p">,</span> <span class="s2">"R"</span><span class="p">,</span> <span class="s2">"L"</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">></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">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="n">_PATH_TYPE_MAP</span> <span class="o">=</span> <span class="p">{</span><span class="s2">"LSL"</span><span class="p">:</span> <span class="n">_LSL</span><span class="p">,</span> <span class="s2">"RSR"</span><span class="p">:</span> <span class="n">_RSR</span><span class="p">,</span> <span class="s2">"LSR"</span><span class="p">:</span> <span class="n">_LSR</span><span class="p">,</span> <span class="s2">"RSL"</span><span class="p">:</span> <span class="n">_RSL</span><span class="p">,</span>
|
|
<span class="s2">"RLR"</span><span class="p">:</span> <span class="n">_RLR</span><span class="p">,</span> <span class="s2">"LRL"</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">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">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">best_cost</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="s2">"inf"</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">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">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">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">></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">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">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">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">"S"</span><span class="p">:</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">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">"L"</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">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">"R"</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">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">"L"</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">"R"</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">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">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="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="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="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">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"><=</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="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="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">"Dubins path planner sample start!!"</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>
|
|
<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="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="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="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="n">curvature</span> <span class="o">=</span> <span class="mf">1.0</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">lengths</span> <span class="o">=</span> <span class="n">plan_dubins_path</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">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="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">""</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">"equal"</span><span class="p">)</span>
|
|
<span class="n">plt</span><span class="o">.</span><span class="n">show</span><span class="p">()</span>
|
|
|
|
|
|
<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s1">'__main__'</span><span class="p">:</span>
|
|
<span class="n">main</span><span class="p">()</span>
|
|
</pre></div>
|
|
|
|
</div>
|
|
</div>
|
|
<footer>
|
|
|
|
<hr/>
|
|
|
|
<div role="contentinfo">
|
|
<p>© 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> |