Created example python package AtsushiSakai#724

pyproject.toml

@@ -0,0 +1,23 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "atsushi_robotics"
+version = "0.0.1"
+authors = [
+  { name="Atsushi Sakai", email="[email protected]" },
+  { name="Abinash Satapathy", email="[email protected]"}
+]
+description = "Quick robotics algorithms"
+readme = "README.md"
+requires-python = ">=3.7"
+classifiers = [
+    "Programming Language :: Python :: 3",
+    "License :: OSI Approved :: MIT License",
+    "Operating System :: OS Independent",
+]
+
+[project.urls]
+"Homepage" = "https://github.com/AtsushiSakai/PythonRobotics"
+"Bug Tracker" = "https://github.com/AtsushiSakai/PythonRobotics/issues"

src/atsushi_robotics/angle.py

@@ -0,0 +1,83 @@
+import numpy as np
+from scipy.spatial.transform import Rotation as Rot
+
+
+def rot_mat_2d(angle):
+    """
+    Create 2D rotation matrix from an angle
+
+    Parameters
+    ----------
+    angle :
+
+    Returns
+    -------
+    A 2D rotation matrix
+
+    Examples
+    --------
+    >>> angle_mod(-4.0)
+
+
+    """
+    return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
+
+
+def angle_mod(x, zero_2_2pi=False, degree=False):
+    """
+    Angle modulo operation
+    Default angle modulo range is [-pi, pi)
+
+    Parameters
+    ----------
+    x : float or array_like
+        A angle or an array of angles. This array is flattened for
+        the calculation. When an angle is provided, a float angle is returned.
+    zero_2_2pi : bool, optional
+        Change angle modulo range to [0, 2pi)
+        Default is False.
+    degree : bool, optional
+        If True, then the given angles are assumed to be in degrees.
+        Default is False.
+
+    Returns
+    -------
+    ret : float or ndarray
+        an angle or an array of modulated angle.
+
+    Examples
+    --------
+    >>> angle_mod(-4.0)
+    2.28318531
+
+    >>> angle_mod([-4.0])
+    np.array(2.28318531)
+
+    >>> angle_mod([-150.0, 190.0, 350], degree=True)
+    array([-150., -170.,  -10.])
+
+    >>> angle_mod(-60.0, zero_2_2pi=True, degree=True)
+    array([300.])
+
+    """
+    if isinstance(x, float):
+        is_float = True
+    else:
+        is_float = False
+
+    x = np.asarray(x).flatten()
+    if degree:
+        x = np.deg2rad(x)
+
+    if zero_2_2pi:
+        mod_angle = x % (2 * np.pi)
+    else:
+        mod_angle = (x + np.pi) % (2 * np.pi) - np.pi
+
+    if degree:
+        mod_angle = np.rad2deg(mod_angle)
+
+    if is_float:
+        return mod_angle.item()
+    else:
+        return mod_angle

src/atsushi_robotics/plot.py

@@ -0,0 +1,87 @@
+"""
+Matplotlib based plotting utilities
+"""
+import math
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+def plot_arrow(x, y, yaw, arrow_length=1.0,
+               origin_point_plot_style="xr",
+               head_width=0.1, fc="r", ec="k", **kwargs):
+    """
+    Plot an arrow or arrows based on 2D state (x, y, yaw)
+
+    All optional settings of matplotlib.pyplot.arrow can be used.
+    - matplotlib.pyplot.arrow:
+    https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.arrow.html
+
+    Parameters
+    ----------
+    x : a float or array_like
+        a value or a list of arrow origin x position.
+    y : a float or array_like
+        a value or a list of arrow origin y position.
+    yaw : a float or array_like
+        a value or a list of arrow yaw angle (orientation).
+    arrow_length : a float (optional)
+        arrow length. default is 1.0
+    origin_point_plot_style : str (optional)
+        origin point plot style. If None, not plotting.
+    head_width : a float (optional)
+        arrow head width. default is 0.1
+    fc : string (optional)
+        face color
+    ec : string (optional)
+        edge color
+    """
+    if not isinstance(x, float):
+        for (i_x, i_y, i_yaw) in zip(x, y, yaw):
+            plot_arrow(i_x, i_y, i_yaw, head_width=head_width,
+                       fc=fc, ec=ec, **kwargs)
+    else:
+        plt.arrow(x, y,
+                  arrow_length * math.cos(yaw),
+                  arrow_length * math.sin(yaw),
+                  head_width=head_width,
+                  fc=fc, ec=ec,
+                  **kwargs)
+        if origin_point_plot_style is not None:
+            plt.plot(x, y, origin_point_plot_style)
+
+
+def plot_curvature(x_list, y_list, heading_list, curvature,
+                   k=0.01, c="-c", label="Curvature"):
+    """
+    Plot curvature on 2D path. This plot is a line from the original path,
+    the lateral distance from the original path shows curvature magnitude.
+    Left turning shows right side plot, right turning shows left side plot.
+    For straight path, the curvature plot will be on the path, because
+    curvature is 0 on the straight path.
+
+    Parameters
+    ----------
+    x_list : array_like
+        x position list of the path
+    y_list : array_like
+        y position list of the path
+    heading_list : array_like
+        heading list of the path
+    curvature : array_like
+        curvature list of the path
+    k : float
+        curvature scale factor to calculate distance from the original path
+    c : string
+        color of the plot
+    label : string
+        label of the plot
+    """
+    cx = [x + d * k * np.cos(yaw - np.pi / 2.0) for x, y, yaw, d in
+          zip(x_list, y_list, heading_list, curvature)]
+    cy = [y + d * k * np.sin(yaw - np.pi / 2.0) for x, y, yaw, d in
+          zip(x_list, y_list, heading_list, curvature)]
+
+    plt.plot(cx, cy, c, label=label)
+    for ix, iy, icx, icy in zip(x_list, y_list, cx, cy):
+        plt.plot([ix, icx], [iy, icy], c)
+