python-motion-planning 0.1__py3-none-any.whl

python_motion_planning/__init__.py

@@ -0,0 +1,4 @@
+from .utils import *
+from .global_planner import *
+from .local_planner import *
+from .curve_generation import *

python_motion_planning/curve_generation/__init__.py

@@ -0,0 +1,9 @@
+from .polynomial_curve import Polynomial
+from .bezier_curve import Bezier
+from .bspline_curve import BSpline
+from .dubins_curve import Dubins
+from .reeds_shepp import ReedsShepp
+from .cubic_spline import CubicSpline
+from .fem_pos_smooth import FemPosSmoother
+
+__all__ = ["Polynomial", "Dubins", "ReedsShepp", "Bezier", "CubicSpline", "BSpline", "FemPosSmoother"]

python_motion_planning/curve_generation/bezier_curve.py

@@ -0,0 +1,131 @@
+"""
+@file: bezier_curve.py
+@breif: Bezier curve generation
+@author: Winter
+@update: 2023.7.25
+"""
+import numpy as np
+
+from scipy.special import comb
+from python_motion_planning.utils import Plot
+from .curve import Curve
+
+class Bezier(Curve):
+	"""
+	Class for Bezier curve generation.
+
+	Parameters:
+		step (float): Simulation or interpolation size
+		offset (float): The offset of control points
+
+	Examples:
+		>>> from python_motion_planning.curve_generation import Bezier
+		>>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
+		>>> generator = Bezier(step, offset)
+		>>> generator.run(points)
+	"""
+	def __init__(self, step: float, offset: float) -> None:
+		super().__init__(step)
+		self.offset = offset
+
+	def __str__(self) -> str:
+		return "Bezier Curve"
+
+	def generation(self, start_pose: tuple, goal_pose: tuple):
+		"""
+		Generate the Bezier Curve.
+
+		Parameters:
+			start_pose (tuple): Initial pose (x, y, yaw)
+			goal_pose (tuple): Target pose (x, y, yaw)
+
+		Returns:
+			x_list (list): x of the trajectory
+			y_list (list): y of the trajectory
+			yaw_list (list): yaw of the trajectory
+		"""
+		sx, sy, _ = start_pose
+		gx, gy, _ = goal_pose
+		n_points = int(np.hypot(sx - gx, sy - gy) / self.step)
+		control_points = self.getControlPoints(start_pose, goal_pose)
+
+		return [self.bezier(t, control_points) for t in np.linspace(0, 1, n_points)], \
+			   control_points
+
+	def bezier(self, t: float, control_points: list) ->np.ndarray:
+		"""
+		Calculate the Bezier curve point.
+
+		Parameters:
+			t (float): scale factor
+			control_points (list[tuple]): control points
+
+		Returns:
+			point (np.array): point in Bezier curve with t
+		"""
+		n = len(control_points) - 1
+		control_points = np.array(control_points)
+		return np.sum([comb(n, i) * t ** i * (1 - t) ** (n - i) *
+			control_points[i] for i in range(n + 1)], axis=0)
+
+	def getControlPoints(self, start_pose: tuple, goal_pose: tuple):
+		"""
+		Calculate control points heuristically.
+
+		Parameters:
+			start_pose (tuple): Initial pose (x, y, yaw)
+			goal_pose (tuple): Target pose (x, y, yaw)
+
+		Returns:
+			control_points (list[tuple]): Control points
+		"""
+		sx, sy, syaw = start_pose
+		gx, gy, gyaw = goal_pose
+
+		dist = np.hypot(sx - gx, sy - gy) / self.offset
+		return [(sx, sy),
+				(sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)),
+				(gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)),
+				(gx, gy)]
+
+	def run(self, points: list):
+		"""
+        Running both generation and animation.
+
+		Parameters:
+			points (list[tuple]): path points
+        """
+		assert len(points) >= 2, "Number of points should be at least 2."
+		import matplotlib.pyplot as plt
+
+		# generation
+		path_x, path_y = [], []
+		path_control_x, path_control_y = [], []
+		for i in range(len(points) - 1):
+			path, control_points = self.generation(
+				(points[i][0], points[i][1], np.deg2rad(points[i][2])),
+				(points[i + 1][0], points[i + 1][1], np.deg2rad(points[i + 1][2])))
+
+			for pt in path:
+				path_x.append(pt[0])
+				path_y.append(pt[1])
+
+			path_control_x.append(points[i][0])
+			path_control_y.append(points[i][1])
+
+			for pt in control_points:
+				path_control_x.append(pt[0])
+				path_control_y.append(pt[1])
+
+		# animation
+		plt.figure("curve generation")
+		plt.plot(path_x, path_y, linewidth=2, c="#1f77b4")
+		plt.plot(path_control_x, path_control_y, '--o', c='#dddddd', label="Control Points")
+		for x, y, theta in points:
+			Plot.plotArrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
+
+		plt.axis("equal")
+		plt.legend()
+		plt.title(str(self))
+		plt.show()
+

python_motion_planning/curve_generation/bspline_curve.py

@@ -0,0 +1,271 @@
+"""
+@file: bspline_curve.py
+@breif: B-Spline curve generation
+@author: Winter
+@update: 2023.7.29
+"""
+import math
+import numpy as np
+
+from .curve import Curve
+
+class BSpline(Curve):
+    """
+    Class for B-Spline curve generation.
+
+    Parameters:
+        step (float): Simulation or interpolation size
+        k (int): Degree of curve
+
+    Examples:
+        >>> from python_motion_planning.curve_generation import BSpline
+        >>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
+        >>> generator = BSpline(step, k)
+        >>> generator.run(points)
+    """
+    def __init__(self, step: float, k: int, param_mode: str="centripetal", 
+                 spline_mode: str="interpolation") -> None:
+        super().__init__(step)
+        self.k = k
+
+        assert param_mode == "centripetal" or param_mode == "chord_length" \
+               or param_mode == "uniform_spaced", "Parameter selection mode error!"
+        self.param_mode = param_mode
+
+        assert spline_mode == "interpolation" or spline_mode == "approximation", \
+               "Spline mode selection error!"
+        self.spline_mode = spline_mode
+    
+    def __str__(self) -> str:
+        return "B-Spline Curve"
+    
+    def baseFunction(self, i: int, k: int, t: float, knot: list):
+        """
+        Calculate base function using Cox-deBoor function.
+
+        Parameters:
+            i (int): The index of base function
+            k (int): The degree of curve
+            t (float): parameter
+            knot (list[float]): knot vector
+
+        Returns:
+            Nik_t (float): The value of base function Nik(t)
+        """
+        Nik_t = 0
+        if k == 0:
+            Nik_t = 1.0 if t >= knot[i] and t < knot[i + 1] else 0.0
+        else:
+            length1 = knot[i + k] - knot[i]
+            length2 = knot[i + k + 1] - knot[i + 1]
+            if not length1 and not length2:
+                Nik_t = 0
+            elif not length1:
+                Nik_t = (knot[i + k + 1] - t) / length2 * self.baseFunction(i + 1, k - 1, t, knot)
+            elif not length2:
+                Nik_t = (t - knot[i]) / length1 * self.baseFunction(i, k - 1, t, knot)
+            else:
+                Nik_t = (t - knot[i]) / length1 * self.baseFunction(i, k - 1, t, knot) + \
+                        (knot[i + k + 1] - t) / length2 * self.baseFunction(i + 1, k - 1, t, knot)
+        return Nik_t
+
+    def paramSelection(self, points: list):
+        """
+        Calculate parameters using the `uniform spaced` or `chrod length`
+        or `centripetal` method.
+
+        Parameters:
+            points (list[tuple]): path points
+
+		Returns:
+		    Parameters (list[float]): The parameters of given points
+        """
+        n = len(points)
+        x_list = [pt[0] for pt in points]
+        y_list = [pt[1] for pt in points]
+        dx, dy = np.diff(x_list), np.diff(y_list)
+
+        if self.param_mode == "uniform_spaced":
+            return np.linspace(0, 1, n).tolist()
+
+        elif self.param_mode == "chord_length":
+            parameters = np.zeros(n)
+            s = np.cumsum([math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)])
+            for i in range(1, n):
+                parameters[i] = s[i - 1] / s[-1]
+            return parameters.tolist()
+        
+        elif self.param_mode == "centripetal":
+            alpha = 0.5
+            s = np.cumsum([math.pow(math.hypot(idx, idy), alpha) for (idx, idy) in zip(dx, dy)])
+            parameters = np.zeros(n)
+            for i in range(1, n):
+                parameters[i] = s[i - 1] / s[-1]
+            return parameters.tolist()
+
+    def knotGeneration(self, param: list, n: int):
+        """
+        Generate knot vector.
+
+        Parameters:
+            param (list[float]): The parameters of given points
+            n (int): The number of data points
+		
+		Returns:
+		    knot (list[float]): The knot vector
+        """
+        m = n + self.k + 1
+        knot = np.zeros(m)
+        for i in range(self.k + 1):
+            knot[i] = 0
+        for i in range(n, m):
+            knot[i] = 1
+        for i in range(self.k + 1, n):
+            for j in range(i - self.k, i):
+                knot[i] = knot[i] + param[j]
+            knot[i] = knot[i] / self.k
+        return knot.tolist()
+    
+    def interpolation(self, points: list, param: list, knot: list):
+        """
+        Given a set of N data points, D0, D1, ..., Dn and a degree k,
+        find a B-spline curve of degree k defined by N control points
+        that passes all data points in the given order.
+
+        Parameters:
+            points (list[tuple]): path points
+            param (list[float]): The parameters of given points
+            knot (list[float]): The knot vector
+        
+        Returns:
+            control_points (np.ndarray): The control points
+        """
+        n = len(points)
+        N = np.zeros((n, n))
+
+        for i in range(n):
+            for j in range(n):
+                N[i][j] = self.baseFunction(j, self.k, param[i], knot)
+        N[n-1][n-1] = 1
+        N_inv = np.linalg.inv(N)
+
+        D = np.array(points)
+
+        return N_inv @ D
+
+    def approximation(self, points: list, param: list, knot: list):
+        """
+        Given a set of N data points, D0, D1, ..., Dn, a degree k,
+        and a number H, where N > H > k >= 1, find a B-spline curve
+        of degree k defined by H control points that satisfies the
+        following conditions:
+            1. this curve contains the first and last data points;
+            2. this curve approximates the data polygon in the sense
+            of least square
+        
+        Parameters:
+            points (list[tuple]): path points
+            param (list[float]): The parameters of given points
+            knot (list[float]): The knot vector
+        
+		Returns:
+		    control_points (np.ndarray): The control points
+        """
+        n = len(points)
+        D = np.array(points)
+
+        # heuristically setting the number of control points
+        h = n - 1
+
+        N = np.zeros((n, h))
+        for i in range(n):
+            for j in range(h):
+                N[i][j] = self.baseFunction(j, self.k, param[i], knot)
+        N_ = N[1 : n - 1, 1 : h - 1]
+
+        qk = np.zeros((n - 2, 2))
+        for i in range(1, n - 1):
+            qk[i - 1] = D[i, :] - N[i][0] * D[0, :] - N[i][h - 1] * D[-1, :]
+        Q = N_.T @ qk
+
+        P = np.linalg.inv(N_.T @ N_) @ Q
+        P = np.insert(P, 0, D[0, :], axis=0)
+        P = np.insert(P, len(P), D[-1, :], axis=0)
+
+        return P
+
+    def generation(self, t, k, knot, control_pts):
+        """
+        Generate the B-spline curve.
+
+        Parameters:
+            t (np.ndarray): The parameter values
+            k (int): The degree of the B-spline curve
+            knot (list[float]): The knot vector
+            control_pts (np.ndarray): The control points
+
+        Returns:
+            curve (np.ndarray): The B-spline curve
+        """
+        N = np.zeros((len(t), len(control_pts)))
+
+        for i in range(len(t)):
+            for j in range(len(control_pts)):
+                N[i][j] = self.baseFunction(j, k, t[i], knot)
+        N[len(t) - 1][len(control_pts) - 1] = 1
+
+        return N @ control_pts
+
+    def run(self, points: list, display: bool = True):
+        """
+        Running both generation and animation.
+
+        Parameters:
+            points (list[tuple]): path points
+        """
+        assert len(points) >= 2, "Number of points should be at least 2."
+        import matplotlib.pyplot as plt
+
+        if len(points[0]) > 2:
+            points = [(points[i][0], points[i][1]) for i in range(len(points))]
+
+        t = np.linspace(0, 1, int(1 / self.step))
+        params = self.paramSelection(points)
+        knot = self.knotGeneration(params, len(points))
+
+        if self.spline_mode == "interpolation":
+            control_pts = self.interpolation(points, params, knot)
+        elif self.spline_mode == "approximation":
+            control_pts = self.approximation(points, params, knot)
+            h = len(control_pts)
+            new_points = [(control_pts[i][0], control_pts[i][1])
+                for i in range(h)]
+            params = self.paramSelection(new_points)
+            knot = self.knotGeneration(params, h)
+        else:
+            raise NotImplementedError
+        
+        control_x = control_pts[:, 0].tolist()
+        control_y = control_pts[:, 1].tolist()
+
+        path = self.generation(t, self.k, knot, control_pts)
+        path_x = path[:, 0].tolist()
+        path_y = path[:, 1].tolist()
+
+        if display:
+            # animation
+            plt.figure("curve generation")
+
+            # static
+            plt.figure("curve generation")
+            plt.plot(path_x, path_y, linewidth=2, c="#1f77b4")
+            plt.plot(control_x, control_y, '--o', c='#dddddd', label="Control Points")
+            for x, y in points:
+                plt.plot(x, y, "xr", linewidth=2)
+            plt.axis("equal")
+            plt.legend()
+            plt.title(str(self))
+
+            plt.show()
+        
+        return [(ix, iy) for (ix, iy) in zip(path_x, path_y)]

python_motion_planning/curve_generation/cubic_spline.py

@@ -0,0 +1,128 @@
+"""
+@file: cubic_spline.py
+@breif: Cubic spline generation
+@author: Winter
+@update: 2023.7.28
+"""
+import math
+import bisect
+import numpy as np
+
+from .curve import Curve
+
+class CubicSpline(Curve):
+	"""
+	Class for cubic spline generation.
+
+	Parameters:
+		step (float): Simulation or interpolation size
+
+	Examples:
+		>>> from python_motion_planning.curve_generation import CubicSpline
+		>>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
+		>>> generator = CubicSpline(step)
+		>>> generator.run(points)
+	"""
+	def __init__(self, step: float) -> None:
+		super().__init__(step)
+	
+	def __str__(self) -> str:
+		return "Cubic Spline"
+
+	def spline(self, x_list: list, y_list: list, t: list):
+		"""
+		Running both generation and animation.
+
+		Parameters:
+			x_list (list[tuple]): path points x-direction
+			y_list (list[tuple]): path points y-direction
+			t (list): parameter
+		
+		Returns:
+			p (list): The (x, y) of curve with given t
+			dp (list): The derivative (dx, dy) of curve with given t
+		"""
+		# cubic polynomial functions
+		a, b, c, d = y_list, [], [], []
+		h = np.diff(x_list)
+		num = len(x_list)
+
+		# calculate coefficient matrix
+		A = np.zeros((num, num))
+		for i in range(1, num - 1):
+			A[i, i - 1] = h[i - 1]
+			A[i, i] = 2.0 * (h[i - 1] + h[i])
+			A[i, i + 1] = h[i]
+		A[0, 0] = 1.0
+		A[num - 1, num - 1] = 1.0
+
+		B = np.zeros(num)
+		for i in range(1, num - 1):
+			B[i] = 3.0 * (a[i + 1] - a[i]) / h[i] - \
+					3.0 * (a[i] - a[i - 1]) / h[i - 1]
+
+		c = np.linalg.solve(A, B)
+		for i in range(num - 1):
+			d.append((c[i + 1] - c[i]) / (3.0 * h[i]))
+			b.append((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2.0 * c[i]) / 3.0)
+
+		# calculate spline value and its derivative
+		p, dp = [], []
+		for it in t:
+			if it < x_list[0] or it > x_list[-1]:
+				continue
+			i = bisect.bisect(x_list, it) - 1
+			dx = it - x_list[i]
+			p.append(a[i] + b[i] * dx + c[i] * dx**2 + d[i] * dx**3)
+			dp.append(b[i] + 2.0 * c[i] * dx + 3.0 * d[i] * dx**2)
+
+		return p, dp
+
+	def generation(self, start_pose: tuple, goal_pose: tuple):
+		pass
+
+	def run(self, points: list):
+		"""
+		Running both generation and animation.
+
+		Parameters:
+			points (list[tuple]): path points
+		"""
+		assert len(points) >= 2, "Number of points should be at least 2."
+		import matplotlib.pyplot as plt
+
+		if len(points[0]) == 2:
+			x_list = [ix for (ix, _) in points]
+			y_list = [iy for (_, iy) in points]
+		elif len(points[0]) == 3:
+			x_list = [ix for (ix, _, _) in points]
+			y_list = [iy for (_, iy, _) in points]
+		else:
+			raise NotImplementedError
+		
+		dx, dy = np.diff(x_list), np.diff(y_list)
+		ds = [math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)]
+		s = [0]
+		s.extend(np.cumsum(ds))
+		t = np.arange(0, s[-1], self.step)
+
+		path_x, d_path_x = self.spline(s, x_list, t)
+		path_y, d_path_y = self.spline(s, y_list, t)
+		path_yaw = [math.atan2(d_path_y[i], d_path_x[i]) for i in range(len(d_path_x))]
+
+		# animation
+		plt.figure("curve generation")
+
+		# static
+		plt.figure("curve generation")
+		plt.plot(path_x, path_y, linewidth=2, c="#1f77b4")
+		for x, y, _ in points:
+			plt.plot(x, y, "xr", linewidth=2)
+		plt.axis("equal")
+		plt.title(str(self))
+
+		plt.figure("yaw")
+		plt.plot(t, [math.degrees(iyaw) for iyaw in path_yaw], "-r")
+		plt.title("yaw curve")
+
+		plt.show()

python_motion_planning/curve_generation/curve.py

@@ -0,0 +1,64 @@
+"""
+@file: curve.py
+@breif: Trajectory generation
+@author: Winter
+@update: 2023.5.31
+"""
+import math
+from abc import ABC, abstractmethod
+
+class Curve(ABC):
+	def __init__(self, step: float) -> None:
+		"""
+		Base class for curve generation.
+
+		Parameters:
+			step (float): Simulation or interpolation size
+		"""
+		self.step = step
+
+	@abstractmethod
+	def run(self, points: list):
+		"""
+        Running both generation and animation.
+        """
+		pass
+
+	@abstractmethod
+	def generation(self, start_pose: tuple, goal_pose: tuple):
+		"""
+		Generate the curve.
+		"""
+		pass
+
+	def trigonometric(self, alpha: float, beta: float):
+		"""
+		Calculate some useful trigonometric value with angles.
+		"""
+		return math.sin(alpha), math.sin(beta), math.cos(alpha), math.cos(beta), \
+			math.sin(alpha - beta), math.cos(alpha - beta)
+
+	def pi2pi(self, theta: float) -> float:
+		"""
+		Truncate the angle to the interval of -π to π.
+		"""
+		while theta > math.pi:
+			theta -= 2.0 * math.pi
+		while theta < -math.pi:
+			theta += 2.0 * math.pi
+		return theta
+
+	def mod2pi(self, theta: float) -> float:
+		"""
+		Perform modulus operation on 2π.
+		"""
+		return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0)
+	
+	def length(self, path: list) -> float:
+		"""
+		Calculate path or trajectory length with `path` format [(ix, iy)] (i from 0 to N)
+		"""
+		dist = 0
+		for i in range(len(path) - 1):
+			dist = dist + math.hypot(path[i + 1][0] - path[i][0], path[i + 1][1] - path[i][1])
+		return dist