diff --git a/.gitignore b/.gitignore index c971b8f9c5..cae29a2fdd 100644 --- a/.gitignore +++ b/.gitignore @@ -71,3 +71,10 @@ target/ matplotrecorder/* .vscode/settings.json + +# scratchpad files +scratch/ + +# IDE files +.vscode/ +workspace.code-workspace diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 0000000000..c167893ce3 --- /dev/null +++ b/.pylintrc @@ -0,0 +1,584 @@ +[MASTER] + +# A comma-separated list of package or module names from where C extensions may +# be loaded. Extensions are loading into the active Python interpreter and may +# run arbitrary code. +extension-pkg-whitelist= + +# Add files or directories to the blacklist. They should be base names, not +# paths. +ignore=CVS + +# Add files or directories matching the regex patterns to the blacklist. The +# regex matches against base names, not paths. +ignore-patterns= + +# Python code to execute, usually for sys.path manipulation such as +# pygtk.require(). +#init-hook= + +# Use multiple processes to speed up Pylint. Specifying 0 will auto-detect the +# number of processors available to use. +jobs=1 + +# Control the amount of potential inferred values when inferring a single +# object. This can help the performance when dealing with large functions or +# complex, nested conditions. +limit-inference-results=100 + +# List of plugins (as comma separated values of python module names) to load, +# usually to register additional checkers. +load-plugins= + +# Pickle collected data for later comparisons. +persistent=yes + +# Specify a configuration file. +#rcfile= + +# When enabled, pylint would attempt to guess common misconfiguration and emit +# user-friendly hints instead of false-positive error messages. +suggestion-mode=yes + +# Allow loading of arbitrary C extensions. Extensions are imported into the +# active Python interpreter and may run arbitrary code. +unsafe-load-any-extension=no + + +[MESSAGES CONTROL] + +# Only show warnings with the listed confidence levels. Leave empty to show +# all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED. +confidence= + +# Disable the message, report, category or checker with the given id(s). You +# can either give multiple identifiers separated by comma (,) or put this +# option multiple times (only on the command line, not in the configuration +# file where it should appear only once). You can also use "--disable=all" to +# disable everything first and then reenable specific checks. For example, if +# you want to run only the similarities checker, you can use "--disable=all +# --enable=similarities". If you want to run only the classes checker, but have +# no Warning level messages displayed, use "--disable=all --enable=classes +# --disable=W". +disable=print-statement, + parameter-unpacking, + unpacking-in-except, + old-raise-syntax, + backtick, + long-suffix, + old-ne-operator, + old-octal-literal, + import-star-module-level, + non-ascii-bytes-literal, + raw-checker-failed, + bad-inline-option, + locally-disabled, + file-ignored, + suppressed-message, + useless-suppression, + deprecated-pragma, + use-symbolic-message-instead, + apply-builtin, + basestring-builtin, + buffer-builtin, + cmp-builtin, + coerce-builtin, + execfile-builtin, + file-builtin, + long-builtin, + raw_input-builtin, + reduce-builtin, + standarderror-builtin, + unicode-builtin, + xrange-builtin, + coerce-method, + delslice-method, + getslice-method, + setslice-method, + no-absolute-import, + old-division, + dict-iter-method, + dict-view-method, + next-method-called, + metaclass-assignment, + indexing-exception, + raising-string, + reload-builtin, + oct-method, + hex-method, + nonzero-method, + cmp-method, + input-builtin, + round-builtin, + intern-builtin, + unichr-builtin, + map-builtin-not-iterating, + zip-builtin-not-iterating, + range-builtin-not-iterating, + filter-builtin-not-iterating, + using-cmp-argument, + eq-without-hash, + div-method, + idiv-method, + rdiv-method, + exception-message-attribute, + invalid-str-codec, + sys-max-int, + bad-python3-import, + deprecated-string-function, + deprecated-str-translate-call, + deprecated-itertools-function, + deprecated-types-field, + next-method-defined, + dict-items-not-iterating, + dict-keys-not-iterating, + dict-values-not-iterating, + deprecated-operator-function, + deprecated-urllib-function, + xreadlines-attribute, + deprecated-sys-function, + exception-escape, + comprehension-escape, + invalid-name, + missing-function-docstring, + missing-class-docstring + +# Enable the message, report, category or checker with the given id(s). You can +# either give multiple identifier separated by comma (,) or put this option +# multiple time (only on the command line, not in the configuration file where +# it should appear only once). See also the "--disable" option for examples. +enable=c-extension-no-member + + +[REPORTS] + +# Python expression which should return a score less than or equal to 10. You +# have access to the variables 'error', 'warning', 'refactor', and 'convention' +# which contain the number of messages in each category, as well as 'statement' +# which is the total number of statements analyzed. This score is used by the +# global evaluation report (RP0004). +evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10) + +# Template used to display messages. This is a python new-style format string +# used to format the message information. See doc for all details. +#msg-template= + +# Set the output format. Available formats are text, parseable, colorized, json +# and msvs (visual studio). You can also give a reporter class, e.g. +# mypackage.mymodule.MyReporterClass. +output-format=text + +# Tells whether to display a full report or only the messages. +reports=no + +# Activate the evaluation score. +score=yes + + +[REFACTORING] + +# Maximum number of nested blocks for function / method body +max-nested-blocks=5 + +# Complete name of functions that never returns. When checking for +# inconsistent-return-statements if a never returning function is called then +# it will be considered as an explicit return statement and no message will be +# printed. +never-returning-functions=sys.exit + + +[TYPECHECK] + +# List of decorators that produce context managers, such as +# contextlib.contextmanager. Add to this list to register other decorators that +# produce valid context managers. +contextmanager-decorators=contextlib.contextmanager + +# List of members which are set dynamically and missed by pylint inference +# system, and so shouldn't trigger E1101 when accessed. Python regular +# expressions are accepted. +generated-members= + +# Tells whether missing members accessed in mixin class should be ignored. A +# mixin class is detected if its name ends with "mixin" (case insensitive). +ignore-mixin-members=yes + +# Tells whether to warn about missing members when the owner of the attribute +# is inferred to be None. +ignore-none=yes + +# This flag controls whether pylint should warn about no-member and similar +# checks whenever an opaque object is returned when inferring. The inference +# can return multiple potential results while evaluating a Python object, but +# some branches might not be evaluated, which results in partial inference. In +# that case, it might be useful to still emit no-member and other checks for +# the rest of the inferred objects. +ignore-on-opaque-inference=yes + +# List of class names for which member attributes should not be checked (useful +# for classes with dynamically set attributes). This supports the use of +# qualified names. +ignored-classes=optparse.Values,thread._local,_thread._local + +# List of module names for which member attributes should not be checked +# (useful for modules/projects where namespaces are manipulated during runtime +# and thus existing member attributes cannot be deduced by static analysis). It +# supports qualified module names, as well as Unix pattern matching. +ignored-modules= + +# Show a hint with possible names when a member name was not found. The aspect +# of finding the hint is based on edit distance. +missing-member-hint=yes + +# The minimum edit distance a name should have in order to be considered a +# similar match for a missing member name. +missing-member-hint-distance=1 + +# The total number of similar names that should be taken in consideration when +# showing a hint for a missing member. +missing-member-max-choices=1 + +# List of decorators that change the signature of a decorated function. +signature-mutators= + + +[STRING] + +# This flag controls whether the implicit-str-concat-in-sequence should +# generate a warning on implicit string concatenation in sequences defined over +# several lines. +check-str-concat-over-line-jumps=no + + +[BASIC] + +# Naming style matching correct argument names. +argument-naming-style=snake_case + +# Regular expression matching correct argument names. Overrides argument- +# naming-style. +#argument-rgx= + +# Naming style matching correct attribute names. +attr-naming-style=snake_case + +# Regular expression matching correct attribute names. Overrides attr-naming- +# style. +#attr-rgx= + +# Bad variable names which should always be refused, separated by a comma. +bad-names=foo, + bar, + baz, + toto, + tutu, + tata + +# Naming style matching correct class attribute names. +class-attribute-naming-style=any + +# Regular expression matching correct class attribute names. Overrides class- +# attribute-naming-style. +#class-attribute-rgx= + +# Naming style matching correct class names. +class-naming-style=PascalCase + +# Regular expression matching correct class names. Overrides class-naming- +# style. +#class-rgx= + +# Naming style matching correct constant names. +const-naming-style=UPPER_CASE + +# Regular expression matching correct constant names. Overrides const-naming- +# style. +#const-rgx= + +# Minimum line length for functions/classes that require docstrings, shorter +# ones are exempt. +docstring-min-length=-1 + +# Naming style matching correct function names. +function-naming-style=snake_case + +# Regular expression matching correct function names. Overrides function- +# naming-style. +#function-rgx= + +# Good variable names which should always be accepted, separated by a comma. +good-names=i, + j, + k, + ex, + Run, + _ + +# Include a hint for the correct naming format with invalid-name. +include-naming-hint=no + +# Naming style matching correct inline iteration names. +inlinevar-naming-style=any + +# Regular expression matching correct inline iteration names. Overrides +# inlinevar-naming-style. +#inlinevar-rgx= + +# Naming style matching correct method names. +method-naming-style=snake_case + +# Regular expression matching correct method names. Overrides method-naming- +# style. +#method-rgx= + +# Naming style matching correct module names. +module-naming-style=snake_case + +# Regular expression matching correct module names. Overrides module-naming- +# style. +#module-rgx= + +# Colon-delimited sets of names that determine each other's naming style when +# the name regexes allow several styles. +name-group= + +# Regular expression which should only match function or class names that do +# not require a docstring. +no-docstring-rgx=^_ + +# List of decorators that produce properties, such as abc.abstractproperty. Add +# to this list to register other decorators that produce valid properties. +# These decorators are taken in consideration only for invalid-name. +property-classes=abc.abstractproperty + +# Naming style matching correct variable names. +variable-naming-style=snake_case + +# Regular expression matching correct variable names. Overrides variable- +# naming-style. +#variable-rgx= + + +[VARIABLES] + +# List of additional names supposed to be defined in builtins. Remember that +# you should avoid defining new builtins when possible. +additional-builtins= + +# Tells whether unused global variables should be treated as a violation. +allow-global-unused-variables=yes + +# List of strings which can identify a callback function by name. A callback +# name must start or end with one of those strings. +callbacks=cb_, + _cb + +# A regular expression matching the name of dummy variables (i.e. expected to +# not be used). +dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_ + +# Argument names that match this expression will be ignored. Default to name +# with leading underscore. +ignored-argument-names=_.*|^ignored_|^unused_ + +# Tells whether we should check for unused import in __init__ files. +init-import=no + +# List of qualified module names which can have objects that can redefine +# builtins. +redefining-builtins-modules=six.moves,past.builtins,future.builtins,builtins,io + + +[LOGGING] + +# Format style used to check logging format string. `old` means using % +# formatting, `new` is for `{}` formatting,and `fstr` is for f-strings. +logging-format-style=old + +# Logging modules to check that the string format arguments are in logging +# function parameter format. +logging-modules=logging + + +[SPELLING] + +# Limits count of emitted suggestions for spelling mistakes. +max-spelling-suggestions=4 + +# Spelling dictionary name. Available dictionaries: none. To make it work, +# install the python-enchant package. +spelling-dict= + +# List of comma separated words that should not be checked. +spelling-ignore-words= + +# A path to a file that contains the private dictionary; one word per line. +spelling-private-dict-file= + +# Tells whether to store unknown words to the private dictionary (see the +# --spelling-private-dict-file option) instead of raising a message. +spelling-store-unknown-words=no + + +[MISCELLANEOUS] + +# List of note tags to take in consideration, separated by a comma. +notes=FIXME, + XXX, + TODO + + +[FORMAT] + +# Expected format of line ending, e.g. empty (any line ending), LF or CRLF. +expected-line-ending-format= + +# Regexp for a line that is allowed to be longer than the limit. +ignore-long-lines=^\s*(# )??$ + +# Number of spaces of indent required inside a hanging or continued line. +indent-after-paren=4 + +# String used as indentation unit. This is usually " " (4 spaces) or "\t" (1 +# tab). +indent-string=' ' + +# Maximum number of characters on a single line. +max-line-length=100 + +# Maximum number of lines in a module. +max-module-lines=1000 + +# List of optional constructs for which whitespace checking is disabled. `dict- +# separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}. +# `trailing-comma` allows a space between comma and closing bracket: (a, ). +# `empty-line` allows space-only lines. +no-space-check=trailing-comma, + dict-separator + +# Allow the body of a class to be on the same line as the declaration if body +# contains single statement. +single-line-class-stmt=no + +# Allow the body of an if to be on the same line as the test if there is no +# else. +single-line-if-stmt=no + + +[SIMILARITIES] + +# Ignore comments when computing similarities. +ignore-comments=yes + +# Ignore docstrings when computing similarities. +ignore-docstrings=yes + +# Ignore imports when computing similarities. +ignore-imports=no + +# Minimum lines number of a similarity. +min-similarity-lines=4 + + +[IMPORTS] + +# List of modules that can be imported at any level, not just the top level +# one. +allow-any-import-level= + +# Allow wildcard imports from modules that define __all__. +allow-wildcard-with-all=no + +# Analyse import fallback blocks. This can be used to support both Python 2 and +# 3 compatible code, which means that the block might have code that exists +# only in one or another interpreter, leading to false positives when analysed. +analyse-fallback-blocks=no + +# Deprecated modules which should not be used, separated by a comma. +deprecated-modules=optparse,tkinter.tix + +# Create a graph of external dependencies in the given file (report RP0402 must +# not be disabled). +ext-import-graph= + +# Create a graph of every (i.e. internal and external) dependencies in the +# given file (report RP0402 must not be disabled). +import-graph= + +# Create a graph of internal dependencies in the given file (report RP0402 must +# not be disabled). +int-import-graph= + +# Force import order to recognize a module as part of the standard +# compatibility libraries. +known-standard-library= + +# Force import order to recognize a module as part of a third party library. +known-third-party=enchant + +# Couples of modules and preferred modules, separated by a comma. +preferred-modules= + + +[CLASSES] + +# List of method names used to declare (i.e. assign) instance attributes. +defining-attr-methods=__init__, + __new__, + setUp, + __post_init__ + +# List of member names, which should be excluded from the protected access +# warning. +exclude-protected=_asdict, + _fields, + _replace, + _source, + _make + +# List of valid names for the first argument in a class method. +valid-classmethod-first-arg=cls + +# List of valid names for the first argument in a metaclass class method. +valid-metaclass-classmethod-first-arg=cls + + +[DESIGN] + +# Maximum number of arguments for function / method. +max-args=5 + +# Maximum number of attributes for a class (see R0902). +max-attributes=7 + +# Maximum number of boolean expressions in an if statement (see R0916). +max-bool-expr=5 + +# Maximum number of branch for function / method body. +max-branches=12 + +# Maximum number of locals for function / method body. +max-locals=15 + +# Maximum number of parents for a class (see R0901). +max-parents=7 + +# Maximum number of public methods for a class (see R0904). +max-public-methods=20 + +# Maximum number of return / yield for function / method body. +max-returns=6 + +# Maximum number of statements in function / method body. +max-statements=50 + +# Minimum number of public methods for a class (see R0903). +min-public-methods=2 + + +[EXCEPTIONS] + +# Exceptions that will emit a warning when being caught. Defaults to +# "BaseException, Exception". +overgeneral-exceptions=BaseException, + Exception diff --git a/PathTracking/pure_pursuit/img/pure_pursuit.png b/PathTracking/pure_pursuit/img/pure_pursuit.png new file mode 100644 index 0000000000..004230451c Binary files /dev/null and b/PathTracking/pure_pursuit/img/pure_pursuit.png differ diff --git a/PathTracking/pure_pursuit/img/pure_pursuit_2.gif b/PathTracking/pure_pursuit/img/pure_pursuit_2.gif new file mode 100644 index 0000000000..c96ead6a1a Binary files /dev/null and b/PathTracking/pure_pursuit/img/pure_pursuit_2.gif differ diff --git a/PathTracking/pure_pursuit/pure_pursuit.ipynb b/PathTracking/pure_pursuit/pure_pursuit.ipynb new file mode 100644 index 0000000000..c5fffd922c --- /dev/null +++ b/PathTracking/pure_pursuit/pure_pursuit.ipynb @@ -0,0 +1,127 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Pure pursuit controller\n", + "\n", + "This notebook shows basic geometrical calculations needed for a pure pursuit controller.\n", + "\n", + "\n", + "An excellent explanation can be found on [TheNatureOfCode](https://natureofcode.com/book/chapter-6-autonomous-agents/#chapter06_section8)\n", + "\n", + "\"Drawing\"\n", + "\n", + "An interactive geometric model below is avaliable on [GeoGebra](https://www.geogebra.org/calculator/vh5d7jvy)\n", + "\n", + "\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Heading error = 1.315 rad\n", + "Point E: (x=2.000, y=1.000)\n" + ] + }, + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "\n", + "from PythonRobotics.vectors import Vector \n", + "from PythonRobotics.plots import plot_point, plot_path, plot_vector\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "plt.style.use('ggplot')\n", + "plt.rcParams['figure.figsize'] = (8,8)\n", + "\n", + " \n", + "def point_on_line(a: Vector,b: Vector,p: Vector):\n", + " \"\"\"\n", + " project point p to line defined by a and b\n", + " \"\"\"\n", + " ap = p - a\n", + " ab = b - a\n", + " return a + ap.dot(ab) / ab.dot(ab) * ab\n", + "\n", + "# reproduce geogebra setup\n", + "\n", + "look_ahead = 0.5 # [m]\n", + "phi = np.pi/2# robot pose [rad]\n", + "\n", + "a = Vector(1,0)\n", + "b = Vector(3,2)\n", + "pos = Vector(1,1)\n", + "v = Vector.from_polar(1,phi)\n", + "\n", + "d = pos + v # future position\n", + "e = point_on_line(a,b,d) # project onto path\n", + "f = look_ahead*(b - a)/abs(b -a) + e\n", + "\n", + "# calculate heading error\n", + "alpha = v.angle(f-pos)\n", + "print('Heading error = %.3f rad' % alpha)\n", + "print('Point E:', e)\n", + "\n", + "plot_point(pos,'position')\n", + "plot_point(d,'D')\n", + "plot_point(e,'E')\n", + "plot_point(f,'F')\n", + "plot_path((a,b),style='o-')\n", + "plot_path((pos,d))\n", + "plot_vector(pos,d)\n", + "\n", + "plot_path((pos,f),style='-r')\n", + "plot_path((d,e),style='--')\n", + "plt.axis('equal');" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now let's continue to in to the `pure_pursuit_2.py` for full iplementation of the controller!\n", + "\n", + "" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.7.9" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index ff995033a9..d54e776084 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -6,8 +6,8 @@ Guillaume Jacquenot (@Gjacquenot) """ -import numpy as np import math +import numpy as np import matplotlib.pyplot as plt # Parameters diff --git a/PathTracking/pure_pursuit/pure_pursuit_2.py b/PathTracking/pure_pursuit/pure_pursuit_2.py new file mode 100644 index 0000000000..6d952f6af8 --- /dev/null +++ b/PathTracking/pure_pursuit/pure_pursuit_2.py @@ -0,0 +1,209 @@ +#!/usr/bin/env python +""" + Pure pursuit controller V2 + + Copyright (c) 2021 AIGRO B.V. - Jev Kuznetsov +""" + +from collections import UserList +import time +import math +import turtle +from typing import Tuple +import numpy as np +from PythonRobotics.vectors import Vector +from PythonRobotics.turtlebot import Robot +from PythonRobotics.graphics import World + +show_animation = True + +# sim parameters +WAYPOINTS_TYPE = 'square' +TARGET_SPEED = .1 # [m/s] +LOOK_AHEAD_1 = 0.1 # [m] lenth of velocity vector +LOOK_AHEAD_2 = 0.2 # [m] step forward along the path segment +Kp = 1.0 # controller gain +t_sim = 50.0 # simulation time [sec] +dt = 0.1 # timestep +SIM_DELAY = 0.01 + + +class Waypoints(UserList): + """ navigation waypoints, collection of xy Vectors """ + + def __init__(self, *args, **kwargs): + """ create waypoints. """ + super().__init__(*args, **kwargs) + self.data = [Vector(d) for d in self.data] + + self.next_idx = None + + def find_next_idx(self, xy: 'Vector') -> 'int': + """ get next waypoint """ + + if self.next_idx is None: # first time search + # distance between points + dst = [pt.distance(xy) for pt in self.data] + idx = np.argmin(dst) + self.next_idx = idx + 1 + else: + # calculate distances to n and n+1 waypoints + idx = self.next_idx + dst_a = xy.distance(self.data[idx-1]) + dst_b = xy.distance(self.data[idx]) + if dst_a > dst_b: # next point + self.next_idx += 1 + + return self.next_idx + + @property + def target_reached(self): + return not self.next_idx < len(self.data) + + +def project_on_line(a: Vector, b: Vector, p: Vector) -> Vector: + """ project point p to a line defined by points a and b """ + ap = p - a + ab = b - a + result = a + ap.dot(ab) / ab.dot(ab) * ab + return result + + +def target_position(xy: Vector, + phi: float, + a: Vector, b: Vector, + velocity_vector=LOOK_AHEAD_1, + look_ahead=LOOK_AHEAD_2 + ) -> Tuple[Vector, Vector]: + """calculate look-ahead and target positions + + Args: + xy (Vector): robot position + phi (float): robot pose [rad] + a (Vector): first waypoint + b (Vector): second waypoint + velocity_vector (float, optional): magnitude of velocity vector . + Defaults to 1.0. + look_ahead (float, optional): forward distance from projecton + point to path. Defaults to 1.0. + + Returns: + Tuple[Vector, Vector, float]: target & look-ahead positions, + and angle error + """ + # point names consistent with https://www.geogebra.org/calculator/vh5d7jvy + # future position + v = Vector.from_polar(velocity_vector, phi) + d = xy + v + + # project to line + e = project_on_line(a, b, d) + f = look_ahead*(b - a)/abs(b - a) + e + + # calculate angele error + alpha = v.angle(f-xy) + + return f, d, alpha + + +def proportional_control(target: float, current: float) -> float: + a = Kp * (target - current) + + return a + + +def generate_waypoints(key: str) -> Waypoints: + """generate different types of waypoints + + Args: + key (str): name of waypoint generator + + Returns: + Waypoints: waypoints to follow + """ + if key == 'simple': + coord = [Vector(xy) + for xy in [(0, 0), (0, 2), (2, 2), (2, 0), (2, -3)]] + waypoints = Waypoints(coord) + elif key == 'sine': + cx = np.arange(0, 5, 0.02) + cy = [np.sin(ix / 0.1) * ix / 2.0 for ix in cx] + waypoints = Waypoints(zip(cx, cy)) + elif key == 'square': + w_tops = np.linspace(0.1, 1, 5) # turn widhts + waypoints = Waypoints([Vector(0, 0)]) + + v_up = Vector(0, 1) + waypoints.append(waypoints[-1] + 0.5*v_up) + for w in reversed(w_tops): + waypoints.append(waypoints[-1] + Vector(w, 0)) + waypoints.append(waypoints[-1] - v_up) + waypoints.append(waypoints[-1] + Vector(0.4, 0)) + waypoints.append(waypoints[-1] + v_up) + + else: + raise KeyError(f"Don't know how to generate waypoints for {key}") + + return waypoints + + +def main(): + + # create visualisation + world = World((0, -3, 5, 3)) + world.screen.title('Pure pursuit 2') + world.add_marker('target') + world.add_marker('future_xy', color="magenta") + + # create waypoints + waypoints = generate_waypoints(WAYPOINTS_TYPE) + print('Waypoints: ', waypoints) + world.plot_path(waypoints) + + # create robot + bot = Robot(phi=math.pi/4) + + i_step = 0 + # simulate + while True: + + # advance simulation + bot.step(dt) + + # calculate target point + idx = waypoints.find_next_idx(bot.xy) + if waypoints.target_reached: + print('Target reached') + break + + target, future_xy, alpha = target_position( + bot.xy, bot.state.phi, waypoints[idx-1], waypoints[idx]) + + # control robot + omega = proportional_control(0, alpha) + bot.set_velocity(TARGET_SPEED, omega) + + # visualise + world.move_robot(bot.xy, bot.phi) + world.move_marker('future_xy', future_xy) + world.move_marker('target', target) + print( + f"step[{i_step}] next_wp:{waypoints.next_idx} \ + target:{target} omega:{omega:.2f}") + time.sleep(SIM_DELAY) + + i_step += 1 + + # check for exit + if world.click_xy: + print('Aborted by click') + break + + print('Simulation result:\n', bot.states_df()) + + print('close simulation window to exit...') + turtle.done() + + +if __name__ == "__main__": + main() diff --git a/PathTracking/pure_pursuit/readme.md b/PathTracking/pure_pursuit/readme.md new file mode 100644 index 0000000000..5bef32f664 --- /dev/null +++ b/PathTracking/pure_pursuit/readme.md @@ -0,0 +1,5 @@ +# Pure pusuit controller + +![animation](img/pure_pursuit_2.gif) + +See `pure_pursuit.ipynb` and `pure_pursuit_2.py` for explanation and a prototype diff --git a/__init__.py b/PythonRobotics/__init__.py similarity index 100% rename from __init__.py rename to PythonRobotics/__init__.py diff --git a/PythonRobotics/graphics.py b/PythonRobotics/graphics.py new file mode 100644 index 0000000000..7f141bcaad --- /dev/null +++ b/PythonRobotics/graphics.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python +""" + +Graphics for simulations, based on turtle graphics + +Author - Jev Kuznetsov + +""" +import math +from time import sleep +import turtle + +TESTING = True + + +class World: + """ wrapper class for visualisations """ + + def __init__(self, coord=(-10, -10, 10, 10), window_size=(800, 800)): + """ create world with worldcoordinates as coord """ + + self.screen = turtle.Screen() + self.screen.setup(width=window_size[0], height=window_size[1]) + self.screen.setworldcoordinates(*coord) + self.screen.tracer(0, 0) + self.screen.onclick(self.click_callback) + + self.turtle = turtle.Turtle() + self.turtle.color('blue') + + self._markers = {} # marker objects + self.click_xy = None # last clicked position + + def click_callback(self, x, y): + self.click_xy = (x, y) + + def add_marker(self, + name, + color='green', + shape="circle", + size=0.5, + trace=False): + """ add marker (markers are Turtle objects) """ + m = turtle.Turtle() + m.color(color) + m.shape(shape) + m.shapesize(size, size) + if not trace: + m.penup() + self._markers[name] = m + + def move_marker(self, name, xy): + """move marker to a position + + Args: + name (str): marker name + xy (Vector): (x,y) vector + """ + + self._markers[name].setpos(xy) + self.screen.update() + + def move_robot(self, xy, phi): + """ plot robot state """ + + self.turtle.setpos(xy) + self.turtle.setheading(math.degrees(phi)) + self.screen.update() + + def plot_path(self, points, color='red', dotsize=3): + """ plot conntected points """ + + t = turtle.Turtle() + t.color(color) + + t.hideturtle() + for x, y in points: + t.setpos(x, y) + t.dot(dotsize) + self.screen.update() + + +def main(): + """ demo function """ + + w = World() + + # plot path + points = [(-5, -5), (0, 0), (0, 2), + (2, 2), (2, 0), (2, -2), (5, 5)] + + w.plot_path(points) + + w.add_marker('m') + for x in range(10): + w.move_marker('m', (x/10, 0)) + w.move_robot((0, x), math.pi/2) + if not TESTING: + sleep(0.1) + + if not TESTING: + print('Please close graphics window...') + turtle.done() + + +if __name__ == "__main__": + main() diff --git a/PythonRobotics/plots.py b/PythonRobotics/plots.py new file mode 100644 index 0000000000..ebd09f3a3f --- /dev/null +++ b/PythonRobotics/plots.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +""" +plotting functions + +the functions are kept in a separate file to increase the +readability of main algorithm. +""" + +import math +import matplotlib.pyplot as plt + + +def plot_arrow(x, y, yaw, length=0.1, width=0.5, fc="r", ec="k"): + """ + Plot arrow + """ + + if not isinstance(x, float): + for ix, iy, iyaw in zip(x, y, yaw): + plot_arrow(ix, iy, iyaw) + else: + plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), + fc=fc, ec=ec, head_width=width, head_length=width) + plt.plot(x, y) + + +def plot_point(xy, txt=None, style='o'): + plt.plot(xy[0], xy[1], style) + if txt is not None: + plt.text(xy[0]+0.1, xy[1], txt) + + +def plot_path(points, style='o-'): + x = [pt[0] for pt in points] + y = [pt[1] for pt in points] + plt.plot(x, y, style) + + +def plot_vector(p1, p2): + plt.quiver(p1.x, p1.y, (p2-p1).x, (p2-p1).y, + angles='xy', scale_units='xy', scale=1) diff --git a/PythonRobotics/turtlebot.py b/PythonRobotics/turtlebot.py new file mode 100755 index 0000000000..d6bc046436 --- /dev/null +++ b/PythonRobotics/turtlebot.py @@ -0,0 +1,195 @@ +#!/usr/bin/env python +""" + +Simulator of a differantial drive robot, Turtlebot3 - like + +Author - Jev Kuznetsov + +""" + +from collections import namedtuple +import math +import numpy as np +import matplotlib.pyplot as plt +import pandas as pd +from PythonRobotics.vectors import Vector + +PLOT_RESULT = True + +# define state object +state_fields = ['x', 'y', 'phi', 'v', 'omega', 't'] +State = namedtuple('State', state_fields, defaults=(0.,) * len(state_fields)) + + +# robot settings are defined with profiles +profile_simple = {'v_max': 1., + 'omega_max': math.pi*2, + 'accel_angular': 1., + 'accel_linear': 1., + 'wheel_diameter': 100e-3, + 'wheel_distance': 200e-3} + + +profile_turtlebot3 = {'v_max': 0.22, + 'omega_max': 2.84, + 'accel_angular': 0.1, + 'accel_linear': 1., + 'wheel_diameter': 66e-3, + 'wheel_distance': 160e-3} + +profile_car = {'v_max': 200 / 3.6, # m/s + 'omega_max': 2.84, # rad/s + 'accel_angular': 0.5, # rad/s2 + 'accel_linear': 3., # m/s2 + 'wheel_diameter': 0.63, # m + 'wheel_distance': 2.6} + + +class Robot: + """ + Robot kinematics simulator + """ + + def __init__(self, + x=0., # x-position + y=0., + phi=0., + profile=None): + """ initiate robot at given location and orientation """ + if profile is not None: + self.profile = profile + else: + self.profile = profile_simple + + self.states = [State(x, y, phi)] + self._omega_target = 0. + self._v_target = 0. + + def __getattr__(self, attr): + """ get parameter from profile """ + if attr in self.profile: + return self.profile[attr] + else: + raise AttributeError(f"no attribute '{attr}' in self.profile") + + def set_velocity(self, v, omega): + """ set target velocities """ + self._omega_target = np.clip(omega, -self.omega_max, self.omega_max) + self._v_target = np.clip(v, 0, self.v_max) + + def step(self, dt=0.1): + """ perform simulation step with timestep dt """ + + s = self.state # start state + + # update linear speed + delta_v = self.accel_linear * dt # dv is an absolute number + v_error = self._v_target - s.v + if abs(v_error) > delta_v: + v_new = s.v + np.sign(v_error) * delta_v + else: + v_new = s.v + v_error + + # update angular speed + delta_omega = self.accel_angular * dt + omega_error = self._omega_target - s.omega + if abs(omega_error) > delta_omega: + omega_new = s.omega + np.sign(omega_error) * delta_omega + else: + omega_new = s.omega + omega_error + + # create new state + s_new = State(x=s.x + s.v*math.cos(s.phi)*dt, + y=s.y + s.v*math.sin(s.phi)*dt, + phi=s.phi + s.omega*dt, + t=s.t + dt, + v=v_new, + omega=omega_new) + + self.states.append(s_new) + + def states_df(self) -> pd.DataFrame: + """ states as DataFrame """ + cols = state_fields[:-1] + data = {} + + for var_name in cols: + data[var_name] = [getattr(s, var_name) for s in self.states] + + t = pd.Index(name='time', data=[s.t for s in self.states]) + df = pd.DataFrame(data, index=t) + + return df + + @property + def xy(self) -> Vector: + """ xy position as Vector """ + return Vector(self.state.x, self.state.y) + + @property + def phi(self) -> float: + return self.state.phi + + @property + def state(self) -> State: + """ last known state """ + return self.states[-1] + + def __repr__(self): + return f'Turtlebot {self.state})' + + +def main(): + """ demonstrate functionality """ + + sim = Robot() + + # calculate path + t_sim = 10 # simulation time [sec] + dt = 0.1 # timestep + n_steps = int(t_sim / dt) + + omega = 2*np.pi # rad/s + sim.set_velocity(1.0, omega) + + for _ in range(n_steps): + sim.step(dt) + + states = sim.states_df() + print('Simulation result:\n', states) + + # plot data + if PLOT_RESULT: + plt.style.use('seaborn-whitegrid') + + plt.cla() + plt.tight_layout() + + plt.subplot(2, 2, 1) + states.v.plot() + plt.title('v (linear velocity)') + + plt.subplot(2, 2, 2) + plt.plot(states.x, states.y, 'x-',) + plt.xlabel('x') + plt.ylabel('y') + plt.title('position') + + plt.subplot(2, 2, 3) + states.omega.plot() + plt.title('omega (angular velocity)') + plt.ylabel('[rad/s]') + + plt.subplot(2, 2, 4) + states.phi.plot() + plt.title('phi') + plt.ylabel('phi [rad]') + + plt.subplots_adjust(hspace=0.4) + + plt.show() + + +if __name__ == '__main__': + print('Turtlebot simulation demo starting...') + main() diff --git a/PythonRobotics/vectors.py b/PythonRobotics/vectors.py new file mode 100644 index 0000000000..3bd3c128bf --- /dev/null +++ b/PythonRobotics/vectors.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python +""" +Simple vector classes. (Python3) + +Why not use numpy instead? A couple of reasons: +1. performance, working with small numpy arrays introduces a lot of overhead +2. .x and .y class properties +3. clear implementation for readability and porting to other languages. + + Copyright (c) 2021 AIGRO B.V. - Jev Kuznetsov + License: BSD + +Special thanks for these information sources and references: + +* [vector.py](https://gist.github.com/mostley/3819375) +* [StackOverflow](https://stackoverflow.com/a/43542669) +* [vector.py](https://github.com/betados/vector_2d/blob/develop/vector_2d/vector.py) + +""" + +from math import atan2, hypot, sin, cos +import numpy as np + + +def rotation_matrix(theta): + """ create 2-d rotation matrix """ + return np.array([[cos(theta), sin(theta)], [-sin(theta), cos(theta)]]) + + +class Vector: + """ 2d vector, compatible with numpy """ + + def __init__(self, *args): + """ create from x,y or xy """ + try: + self.x, self.y = float(args[0]), float(args[1]) + except (IndexError, TypeError): + self.x = float(args[0][0]) + self.y = float(args[0][1]) + + def __repr__(self): + return f"(x={self.x:.3f}, y={self.y:.3f})" + + def __add__(self, other): + return Vector(self.x + other.x, self.y + other.y) + + def __sub__(self, other): + return Vector(self.x - other.x, self.y - other.y) + + def __eq__(self, other): + return (self.x == other.x) & (self.y == other.y) + + def cross(self, other): + return (self.x * other.y) - (self.y * other.x) + + def dot(self, other): + return self.x * other.x + self.y * other.y + + def inner(self, other): + return self.dot(other) + + def rotate(self, angle): + a = np.dot(self, rotation_matrix(angle)) + return Vector(a[0], a[1]) + + def round(self, precision=5): + r = np.round(self, precision) + return Vector(r) + + def angle(self, other): + """ angle between vector, *relative* to other, range (-pi..pi) """ + return -atan2(self.cross(other), self.inner(other)) + + def __mul__(self, scalar): + """ scalar multiplification """ + return Vector(self.x * scalar, self.y * scalar) + + def __rmul__(self, scalar): + return self.__mul__(scalar) + + def __truediv__(self, scalar): # note __div__ was removed in Python3 + return Vector(self.x / scalar, self.y / scalar) + + def __neg__(self): + return Vector(-self.x, -self.y) + + def __abs__(self): + return hypot(self.x, self.y) + + def distance(self, other): + return hypot(self.x-other.x, self.y-other.y) + + @classmethod + def from_polar(cls, r, theta): + return Vector(r * cos(theta), r * sin(theta)) + + def __getitem__(self, item): + """ make subscriptable """ + return [self.x, self.y][item] + + def __array__(self, dtype=None) -> np.array: + """ make this class compatable with numpy operations """ + if dtype: + return np.array([self.x, self.y], dtype=dtype) + else: + return np.array([self.x, self.y]) + + +def point_on_line(a: Vector, b: Vector, p: Vector): + """ + project point p to line defined by a and b + """ + ap = p - a + ab = b - a + return a + ap.dot(ab) / ab.dot(ab) * ab diff --git a/README.md b/README.md index bc0198f78e..dd889606cb 100644 --- a/README.md +++ b/README.md @@ -150,10 +150,13 @@ All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation pip install -r requirements/requirements.txt ``` +3. Add this library to the environment (in development mode) -3. Execute python script in each directory. +> pip install -e . -4. Add star to this repo if you like it :smiley:. +4. Execute python script in each directory. + +5. Add star to this repo if you like it :smiley:. # Localization diff --git a/setup.py b/setup.py new file mode 100644 index 0000000000..cfd77c1cfd --- /dev/null +++ b/setup.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + + +from setuptools import setup, find_packages + + +setup( + name='PythonRobotics', + version='1.0.1', + description="Python implementation of robotics algoritms", + author="Atsushi Sakai et al.", + packages=find_packages(include=['PythonRobotics']), + url="https://github.com/AtsushiSakai/PythonRobotics", + install_requires=[], + license='MIT' +) diff --git a/tests/__init__.py b/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/tests/test_graphics.py b/tests/test_graphics.py new file mode 100644 index 0000000000..976b007141 --- /dev/null +++ b/tests/test_graphics.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python +""" +Test turtle graphics + +author - Jev Kuznetsov +""" + +import conftest +from PythonRobotics import graphics as m + + +def test_1(): + m.TESTING = True + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_turtlebot.py b/tests/test_turtlebot.py new file mode 100644 index 0000000000..d148203498 --- /dev/null +++ b/tests/test_turtlebot.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python +""" +Test turtlebot simulator + +author - Jev Kuznetsov +""" + +import conftest +from PythonRobotics import turtlebot as m + + +def test_1(): + m.PLOT_RESULT = False + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_vectors.py b/tests/test_vectors.py new file mode 100644 index 0000000000..7d56f3c91a --- /dev/null +++ b/tests/test_vectors.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python +""" +Test vector module + +author - Jev Kuznetsov +""" +import numpy as np +import conftest +from PythonRobotics.vectors import Vector, point_on_line + + +# test tuples +ta = (1, 2) +tb = (3, 4) + +a = Vector(ta) +b = Vector(tb) +c = Vector(1, 0) +d = Vector(0, 1) +e = Vector(1, 1) + + +def test_creation(): + an = np.array(ta) + v = Vector(ta) + assert (an == v.__array__()).all() + + v = Vector(an) + assert (an == v.__array__()).all() + + +def test_add(): + assert a + b == Vector(4, 6) + + +def test_cross(): + assert np.cross(a, b) == a.cross(b) + assert np.cross(c, d) == c.cross(d) + + +def test_sub(): + assert a - b == Vector(-2, -2) + + +def test_inner(): + assert (a.inner(b) == np.inner(ta, tb)).all() + + +def test_mul(): + assert a * 3 == Vector(3, 6) + + +def test_div(): + assert Vector(1, 2) / 2 == Vector(0.5, 1.) + + +def test_neg(): + assert -a == Vector(-1, -2) + + +def test_length(): + assert abs(c) == 1 + assert abs(d) == 1 + assert abs(e) == np.sqrt(2) + + +def test_angle(): + assert c.angle(c) == 0 + assert d.angle(c) == np.pi/2 + assert d.angle(e) == np.pi/4 + assert -d.angle(c) == -np.pi/2 + + +def test_from_polar(): + assert Vector.from_polar(1, 0) == Vector(1, 0) + + +def test_subscriptable(): + + assert a[0] == 1 + assert a[1] == 2 + + +def test_projection(): + + pa = Vector(1, 1) + pb = Vector(3, 3) + pc = Vector(1, 3) + + assert point_on_line(pa, pb, pc) == Vector(2, 2) + + +def test_rotation(): + + v = Vector(1, 0) + assert v.rotate(np.pi/2).round() == Vector(0, 1) + assert v.rotate(-np.pi/4) == Vector.from_polar(1, -np.pi/4) + + +def test_distance(): + + assert Vector(1, 0).distance(Vector(0, 0)) == 1 + assert Vector(2, 2).distance(Vector(1, 1)) == np.sqrt(2) + + +if __name__ == '__main__': + conftest.run_this_test(__file__)