Created example python package

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,86 @@
+"""
+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)