AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 17:07:55 -05:00
Code Issues Packages Projects Releases Wiki Activity
Files
7341eeacf71e561cbe1288bdcd4d6a75eaa41c58
PythonRobotics/_modules/utils/plot.html
GitHub Action 7341eeacf7 Add changes for e40b4d9dec
2022-09-27 12:51:28 +00:00

212 lines
17 KiB
HTML
Raw Blame History

<!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>utils.plot &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>
<![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/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> &raquo;</li>
<li><a href="../index.html">Module code</a> &raquo;</li>
<li>utils.plot</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 utils.plot</h1><div class="highlight"><pre>
<span></span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">Matplotlib based plotting utilities</span>
<span class="sd">&quot;&quot;&quot;</span>
<span class="kn">import</span> <span class="nn">math</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">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</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">arrow_length</span><span class="o">=</span><span class="mf">1.0</span><span class="p">,</span>
<span class="n">origin_point_plot_style</span><span class="o">=</span><span class="s2">&quot;xr&quot;</span><span class="p">,</span>
<span class="n">head_width</span><span class="o">=</span><span class="mf">0.1</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="o">**</span><span class="n">kwargs</span><span class="p">):</span>
<span class="sd">&quot;&quot;&quot;</span>
<span class="sd"> Plot an arrow or arrows based on 2D state (x, y, yaw)</span>
<span class="sd"> All optional settings of matplotlib.pyplot.arrow can be used.</span>
<span class="sd"> - matplotlib.pyplot.arrow:</span>
<span class="sd"> https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.arrow.html</span>
<span class="sd"> Parameters</span>
<span class="sd"> ----------</span>
<span class="sd"> x : a float or array_like</span>
<span class="sd"> a value or a list of arrow origin x position.</span>
<span class="sd"> y : a float or array_like</span>
<span class="sd"> a value or a list of arrow origin y position.</span>
<span class="sd"> yaw : a float or array_like</span>
<span class="sd"> a value or a list of arrow yaw angle (orientation).</span>
<span class="sd"> arrow_length : a float (optional)</span>
<span class="sd"> arrow length. default is 1.0</span>
<span class="sd"> origin_point_plot_style : str (optional)</span>
<span class="sd"> origin point plot style. If None, not plotting.</span>
<span class="sd"> head_width : a float (optional)</span>
<span class="sd"> arrow head width. default is 0.1</span>
<span class="sd"> fc : string (optional)</span>
<span class="sd"> face color</span>
<span class="sd"> ec : string (optional)</span>
<span class="sd"> edge color</span>
<span class="sd"> &quot;&quot;&quot;</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="n">head_width</span><span class="o">=</span><span class="n">head_width</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="o">**</span><span class="n">kwargs</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">arrow_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">arrow_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">head_width</span><span class="o">=</span><span class="n">head_width</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="o">**</span><span class="n">kwargs</span><span class="p">)</span>
<span class="k">if</span> <span class="n">origin_point_plot_style</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</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="n">origin_point_plot_style</span><span class="p">)</span>
<div class="viewcode-block" id="plot_curvature"><a class="viewcode-back" href="../../modules/utils/plot/plot.html#utils.plot.plot_curvature">[docs]</a><span class="k">def</span> <span class="nf">plot_curvature</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">heading_list</span><span class="p">,</span> <span class="n">curvature</span><span class="p">,</span>
<span class="n">k</span><span class="o">=</span><span class="mf">0.01</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s2">&quot;-c&quot;</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="s2">&quot;Curvature&quot;</span><span class="p">):</span>
<span class="sd">&quot;&quot;&quot;</span>
<span class="sd"> Plot curvature on 2D path. This plot is a line from the original path,</span>
<span class="sd"> the lateral distance from the original path shows curvature magnitude.</span>
<span class="sd"> Left turning shows right side plot, right turning shows left side plot.</span>
<span class="sd"> For straight path, the curvature plot will be on the path, because</span>
<span class="sd"> curvature is 0 on the straight path.</span>
<span class="sd"> Parameters</span>
<span class="sd"> ----------</span>
<span class="sd"> x_list : array_like</span>
<span class="sd"> x position list of the path</span>
<span class="sd"> y_list : array_like</span>
<span class="sd"> y position list of the path</span>
<span class="sd"> heading_list : array_like</span>
<span class="sd"> heading list of the path</span>
<span class="sd"> curvature : array_like</span>
<span class="sd"> curvature list of the path</span>
<span class="sd"> k : float</span>
<span class="sd"> curvature scale factor to calculate distance from the original path</span>
<span class="sd"> c : string</span>
<span class="sd"> color of the plot</span>
<span class="sd"> label : string</span>
<span class="sd"> label of the plot</span>
<span class="sd"> &quot;&quot;&quot;</span>
<span class="n">cx</span> <span class="o">=</span> <span class="p">[</span><span class="n">x</span> <span class="o">+</span> <span class="n">d</span> <span class="o">*</span> <span class="n">k</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">yaw</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">)</span> <span class="k">for</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">d</span> <span class="ow">in</span>
<span class="nb">zip</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">heading_list</span><span class="p">,</span> <span class="n">curvature</span><span class="p">)]</span>
<span class="n">cy</span> <span class="o">=</span> <span class="p">[</span><span class="n">y</span> <span class="o">+</span> <span class="n">d</span> <span class="o">*</span> <span class="n">k</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">yaw</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">)</span> <span class="k">for</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">d</span> <span class="ow">in</span>
<span class="nb">zip</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">heading_list</span><span class="p">,</span> <span class="n">curvature</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">cx</span><span class="p">,</span> <span class="n">cy</span><span class="p">,</span> <span class="n">c</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="n">label</span><span class="p">)</span>
<span class="k">for</span> <span class="n">ix</span><span class="p">,</span> <span class="n">iy</span><span class="p">,</span> <span class="n">icx</span><span class="p">,</span> <span class="n">icy</span> <span class="ow">in</span> <span class="nb">zip</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">cx</span><span class="p">,</span> <span class="n">cy</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">ix</span><span class="p">,</span> <span class="n">icx</span><span class="p">],</span> <span class="p">[</span><span class="n">iy</span><span class="p">,</span> <span class="n">icy</span><span class="p">],</span> <span class="n">c</span><span class="p">)</span></div>
</pre></div>
</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>
Reference in New Issue View Git Blame Copy Permalink