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

curve_generation/reeds_shepp.py

@@ -0,0 +1,736 @@
+"""
+@file: reeds_shepp.py
+@breif: Reeds shepp curve generation
+@author: Winter
+@update: 2023.7.26
+"""
+import math
+import numpy as np
+
+from python_motion_planning.utils import Plot
+from .curve import Curve
+
+class ReedsShepp(Curve):
+	"""
+	Class for Reeds shepp curve generation.
+
+	Parameters:
+		step (float): Simulation or interpolation size
+		max_curv (float): The maximum curvature of the curve
+
+	Examples:
+		>>> from python_motion_planning.curve_generation import ReedsShepp
+		>>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
+		>>> generator = ReedsShepp(step, max_curv)
+		>>> generator.run(points)
+
+	References:
+		[1] Optimal paths for a car that goes both forwards and backwards
+	"""
+	def __init__(self, step: float, max_curv: float) -> None:
+		super().__init__(step)
+		self.max_curv = max_curv
+	
+	def __str__(self) -> str:
+		return "Reeds Shepp Curve"
+
+	def R(self, x, y):
+		"""
+		Return the polar coordinates (r, theta) of the point (x, y)
+		i.e. rcos(theta) = x; rsin(theta) = y
+
+		Parameters:
+			x (float): x-coordinate value
+			y (float): y-coordinate value
+
+		Returns:
+			r, theta (float): Polar coordinates
+
+		"""
+		r = math.hypot(x, y)
+		theta = math.atan2(y, x)
+
+		return r, theta
+
+	def M(self, theta):
+		"""
+		Truncate the angle to the interval of -π to π.
+
+		Parameters:
+			theta (float): Angle value
+
+		Returns:
+			theta (float): Truncated angle value
+		"""
+		return self.pi2pi(theta)
+
+	class Path:
+		"""
+		class for Path element
+		"""
+		def __init__(self, lengths: list = [], ctypes: list = [], x: list = [],
+			y: list = [], yaw: list = [], dirs: list = []):
+			self.lengths = lengths  	# lengths of each part of path (+: forward, -: backward)
+			self.ctypes = ctypes  		# type of each part of the path
+			self.path_length = sum([abs(i) for i in lengths])  # total path length
+			self.x = x  				# x-coordinate value of curve
+			self.y = y  				# y-coordinate value of curve
+			self.yaw = yaw  			# yaw value of curve
+			self.dirs = dirs 			# direction value of curve (1: forward, -1: backward)
+
+	def SLS(self, x: float, y: float, phi: float):
+		"""
+		Straight-Left-Straight generation mode.
+		"""
+		phi = self.M(phi)
+
+		if y > 0.0 and 0.0 < phi < math.pi * 0.99:
+			xd = -y / math.tan(phi) + x
+			t = xd - math.tan(phi / 2.0)
+			u = phi
+			v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+			return True, t, u, v
+		elif y < 0.0 and 0.0 < phi < math.pi * 0.99:
+			xd = -y / math.tan(phi) + x
+			t = xd - math.tan(phi / 2.0)
+			u = phi
+			v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+			return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LRL(self, x: float, y: float, phi: float):
+		"""
+		Left-Right-Left generation mode. (L+R-L-)
+		"""
+		r, theta = self.R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+		if r <= 4.0:
+			u = -2.0 * math.asin(0.25 * r)
+			t = self.M(theta + 0.5 * u + math.pi)
+			v = self.M(phi - t + u)
+
+			if t >= 0.0 and u <= 0.0:
+				return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LSL(self, x: float, y: float, phi: float):
+		"""
+		Left-Straight-Left generation mode. (L+S+L+)
+		"""
+		u, t = self.R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+		if t >= 0.0:
+			v = self.M(phi - t)
+			if v >= 0.0:
+				return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LSR(self, x: float, y: float, phi: float):
+		"""
+		Left-Straight-Right generation mode. (L+S+R+)
+		"""
+		r, theta = self.R(x + math.sin(phi), y - 1.0 - math.cos(phi))
+		r = r ** 2
+
+		if r >= 4.0:
+			u = math.sqrt(r - 4.0)
+			t = self.M(theta + math.atan2(2.0, u))
+			v = self.M(t - phi)
+
+			if t >= 0.0 and v >= 0.0:
+				return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LRLRn(self, x: float, y: float, phi: float):
+		"""
+		Left-Right(beta)-Left(beta)-Right generation mode. (L+R+L-R-)
+		"""
+		xi = x + math.sin(phi)
+		eta = y - 1.0 - math.cos(phi)
+		rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))
+
+		if rho <= 1.0:
+			u = math.acos(rho)
+			t, v = self._calTauOmega(u, -u, xi, eta, phi)
+			if t >= 0.0 and v <= 0.0:
+				return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LRLRp(self, x: float, y: float, phi: float):
+		"""
+		Left-Right(beta)-Left(beta)-Right generation mode. (L+R-L-R+)
+		"""
+		xi = x + math.sin(phi)
+		eta = y - 1.0 - math.cos(phi)
+		rho = (20.0 - xi * xi - eta * eta) / 16.0
+
+		if 0.0 <= rho <= 1.0:
+			u = -math.acos(rho)
+			if u >= -0.5 * math.pi:
+				t, v = self._calTauOmega(u, u, xi, eta, phi)
+				if t >= 0.0 and v >= 0.0:
+					return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LRSR(self, x: float, y: float, phi: float):
+		"""
+		Left-Right(pi/2)-Straight-Right generation mode. (L+R-S-R-)
+		"""
+		xi = x + math.sin(phi)
+		eta = y - 1.0 - math.cos(phi)
+		rho, theta = self.R(-eta, xi)
+
+		if rho >= 2.0:
+			t = theta
+			u = 2.0 - rho
+			v = self.M(t + 0.5 * math.pi - phi)
+			if t >= 0.0 and u <= 0.0 and v <= 0.0:
+				return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LRSL(self, x: float, y: float, phi: float):
+		"""
+		Left-Right(pi/2)-Straight-Left generation mode. (L+R-S-L-)
+		"""
+		xi = x - math.sin(phi)
+		eta = y - 1.0 + math.cos(phi)
+		rho, theta = self.R(xi, eta)
+
+		if rho >= 2.0:
+			r = math.sqrt(rho * rho - 4.0)
+			u = 2.0 - r
+			t = self.M(theta + math.atan2(r, -2.0))
+			v = self.M(phi - 0.5 * math.pi - t)
+			if t >= 0.0 and u <= 0.0 and v <= 0.0:
+				return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def LRSLR(self, x: float, y: float, phi: float):
+		"""
+		Left-Right(pi/2)-Straight-Left(pi/2)-Right generation mode. (L+R-S-L-R+)
+		"""
+		xi = x + math.sin(phi)
+		eta = y - 1.0 - math.cos(phi)
+		r, _ = self.R(xi, eta)
+
+		if r >= 2.0:
+			u = 4.0 - math.sqrt(r * r - 4.0)
+			if u <= 0.0:
+				t = self.M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))
+				v = self.M(t - phi)
+
+				if t >= 0.0 and v >= 0.0:
+					return True, t, u, v
+
+		return False, 0.0, 0.0, 0.0
+
+	def SCS(self, x: float, y: float, phi: float):
+		"""
+		# 2
+		Straight-Circle-Straight generation mode(using reflect).
+
+		Parameters:
+			x (float): x of goal position
+			y (float): y of goal position
+			phi (float): goal orientation
+
+		Returns:
+			paths (list): Available paths
+		"""
+		paths = []
+
+		flag, t, u, v = self.SLS(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["S", "L", "S"]))
+
+		flag, t, u, v = self.SLS(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["S", "R", "S"]))
+
+		return paths
+
+	def CCC(self, x: float, y: float, phi: float):
+		"""
+		# 8
+		Circle-Circle-Circle generation mode(using reflect, timeflip and backwards).
+
+		Parameters:
+			x (float): x of goal position
+			y (float): y of goal position
+			phi (float): goal orientation
+
+		Returns:
+			paths (list): Available paths
+		"""
+		paths = []
+
+		# L+R-L-
+		flag, t, u, v = self.LRL(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["L", "R", "L"]))
+
+		# timefilp: L-R+L+
+		flag, t, u, v = self.LRL(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -v], ctypes=["L", "R", "L"]))
+
+		# reflect: R+L-R-
+		flag, t, u, v = self.LRL(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["R", "L", "R"]))
+
+		# timeflip + reflect: R-L+R+
+		flag, t, u, v = self.LRL(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -v], ctypes=["R", "L", "R"]))
+
+		# backwards
+		xb = x * math.cos(phi) + y * math.sin(phi)
+		yb = x * math.sin(phi) - y * math.cos(phi)
+
+		# backwards: L-R-L+
+		flag, t, u, v = self.LRL(xb, yb, phi)
+		if flag:
+			paths.append(self.Path(lengths=[v, u, t], ctypes=["L", "R", "L"]))
+
+		# backwards + timefilp: L+R+L-
+		flag, t, u, v = self.LRL(-xb, yb, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-v, -u, -t], ctypes=["L", "R", "L"]))
+
+		# backwards + reflect: R-L-R+
+		flag, t, u, v = self.LRL(xb, -yb, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[v, u, t], ctypes=["R", "L", "R"]))
+
+		# backwards + timeflip + reflect: R+L+R-
+		flag, t, u, v = self.LRL(-xb, -yb, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-v, -u, -t], ctypes=["R", "L", "R"]))
+
+		return paths
+
+	def CSC(self, x: float, y: float, phi: float):
+		"""
+		# 8
+		Circle-Straight-Circle generation mode(using reflect, timeflip and backwards).
+
+		Parameters:
+			x (float): x of goal position
+			y (float): y of goal position
+			phi (float): goal orientation
+
+		Returns:
+			paths (list): Available paths
+		"""
+		paths = []
+
+		# L+S+L+
+		flag, t, u, v = self.LSL(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["L", "S", "L"]))
+
+		# timefilp: L-S-L-
+		flag, t, u, v = self.LSL(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -v], ctypes=["L", "S", "L"]))
+
+		# reflect: R+S+R+
+		flag, t, u, v = self.LSL(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["R", "S", "R"]))
+
+		# timeflip + reflect: R-S-R-
+		flag, t, u, v = self.LSL(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -v], ctypes=["R", "S", "R"]))
+
+		# L+S+R+
+		flag, t, u, v = self.LSR(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["L", "S", "R"]))
+
+		# timefilp: L-S-R-
+		flag, t, u, v = self.LSR(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -v], ctypes=["L", "S", "R"]))
+
+		# reflect: R+S+L+
+		flag, t, u, v = self.LSR(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, v], ctypes=["R", "S", "L"]))
+
+		# timeflip + reflect: R+S+l-
+		flag, t, u, v = self.LSR(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -v], ctypes=["R", "S", "L"]))
+
+		return paths
+
+	def CCCC(self, x: float, y: float, phi: float):
+		"""
+		# 8
+		Circle-Circle(beta)-Circle(beta)-Circle generation mode
+		(using reflect, timeflip and backwards).
+
+		Parameters:
+			x (float): x of goal position
+			y (float): y of goal position
+			phi (float): goal orientation
+
+		Returns:
+			paths (list): Available paths
+		"""
+		paths = []
+
+		# L+R+L-R-
+		flag, t, u, v = self.LRLRn(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, -u, v], ctypes=["L", "R", "L", "R"]))
+
+		# timefilp: L-R-L+R+
+		flag, t, u, v = self.LRLRn(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, u, -v], ctypes=["L", "R", "L", "R"]))
+		
+		# reflect: R+L+R-L-
+		flag, t, u, v = self.LRLRn(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, -u, v], ctypes=["R", "L", "R", "L"]))
+
+		# timeflip + reflect: R-L-R+L+
+		flag, t, u, v = self.LRLRn(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, u, -v], ctypes=["R", "L", "R", "L"]))
+
+		# L+R-L-R+
+		flag, t, u, v = self.LRLRp(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, u, v], ctypes=["L", "R", "L", "R"]))
+
+		# timefilp: L-R+L+R-
+		flag, t, u, v = self.LRLRp(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -u, -v], ctypes=["L", "R", "L", "R"]))
+
+		# reflect: R+L-R-L+
+		flag, t, u, v = self.LRLRp(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, u, u, v], ctypes=["R", "L", "R", "L"]))
+
+		# timeflip + reflect: R-L+R+L-
+		flag, t, u, v = self.LRLRp(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, -u, -u, -v], ctypes=["R", "L", "R", "L"]))
+
+		return paths
+
+	def CCSC(self, x: float, y: float, phi: float):
+		"""
+		# 16
+		Circle-Circle(pi/2)-Straight-Circle and Circle-Straight-Circle(pi/2)-Circle
+		generation mode (using reflect, timeflip and backwards).
+
+		Parameters:
+			x (float): x of goal position
+			y (float): y of goal position
+			phi (float): goal orientation
+
+		Returns:
+			paths (list): Available paths
+		"""
+		paths = []
+
+		# L+R-(pi/2)S-L-
+		flag, t, u, v = self.LRSL(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, -0.5 * math.pi, u, v], ctypes=["L", "R", "S", "L"]))
+
+		# timefilp: L-R+(pi/2)S+L+
+		flag, t, u, v = self.LRSL(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, 0.5 * math.pi, -u, -v], ctypes=["L", "R", "S", "L"]))
+
+		# reflect: R+L-(pi/2)S-R-
+		flag, t, u, v = self.LRSL(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, -0.5 * math.pi, u, v], ctypes=["R", "L", "S", "R"]))
+
+		# timeflip + reflect: R-L+(pi/2)S+R+
+		flag, t, u, v = self.LRSL(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, 0.5 * math.pi, -u, -v], ctypes=["R", "L", "S", "R"]))
+
+		# L+R-(pi/2)S-R-
+		flag, t, u, v = self.LRSR(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, -0.5 * math.pi, u, v], ctypes=["L", "R", "S", "R"]))
+
+		# timefilp: L-R+(pi/2)S+R+
+		flag, t, u, v = self.LRSR(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, 0.5 * math.pi, -u, -v], ctypes=["L", "R", "S", "R"]))
+
+		# reflect: R+L-(pi/2)S-L-
+		flag, t, u, v = self.LRSR(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, -0.5 * math.pi, u, v], ctypes=["R", "L", "S", "L"]))
+
+		# timeflip + reflect: R-L+(pi/2)S+L+
+		flag, t, u, v = self.LRSR(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, 0.5 * math.pi, -u, -v], ctypes=["R", "L", "S", "L"]))
+
+		# backwards
+		xb = x * math.cos(phi) + y * math.sin(phi)
+		yb = x * math.sin(phi) - y * math.cos(phi)
+
+		# backwards: L-S-R-(pi/2)L+
+		flag, t, u, v = self.LRSL(xb, yb, phi)
+		if flag:
+			paths.append(self.Path(lengths=[v, u, -0.5 * math.pi, t], ctypes=["L", "S", "R", "L"]))
+
+		# backwards + timefilp: L+S+R+(pi/2)L-
+		flag, t, u, v = self.LRSL(-xb, yb, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-v, -u, 0.5 * math.pi, -t], ctypes=["L", "S", "R", "L"]))
+
+		# backwards + reflect: R-S-L-(pi/2)R+
+		flag, t, u, v = self.LRSL(xb, -yb, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[v, u, -0.5 * math.pi, t], ctypes=["R", "S", "L", "R"]))
+
+		# backwards + timefilp + reflect: R+S+L+(pi/2)R-
+		flag, t, u, v = self.LRSL(-xb, -yb, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-v, -u, 0.5 * math.pi, -t], ctypes=["R", "S", "L", "R"]))
+
+		# backwards: R-S-R-(pi/2)L+
+		flag, t, u, v = self.LRSR(xb, yb, phi)
+		if flag:
+			paths.append(self.Path(lengths=[v, u, -0.5 * math.pi, t], ctypes=["R", "S", "R", "L"]))
+
+		# backwards + timefilp: R+S+R+(pi/2)L-
+		flag, t, u, v = self.LRSR(-xb, yb, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-v, -u, 0.5 * math.pi, -t], ctypes=["R", "S", "R", "L"]))
+
+		# backwards + reflect: L-S-L-(pi/2)R+
+		flag, t, u, v = self.LRSR(xb, -yb, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[v, u, -0.5 * math.pi, t], ctypes=["L", "S", "L", "R"]))
+
+		# backwards + timefilp + reflect: L+S+L+(pi/2)R-
+		flag, t, u, v = self.LRSR(-xb, -yb, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-v, -u, 0.5 * math.pi, -t], ctypes=["L", "S", "L", "R"]))
+
+		return paths
+
+	def CCSCC(self, x: float, y: float, phi: float):
+		"""
+		# 4
+		Circle-Circle(pi/2)-Straight--Circle(pi/2)-Circle generation mode (using reflect, timeflip and backwards).
+
+		Parameters:
+			x (float): x of goal position
+			y (float): y of goal position
+			phi (float): goal orientation
+
+		Returns:
+			paths (list): Available paths
+		"""
+		paths = []
+
+		# L+R-(pi/2)S-L-(pi/2)R+
+		flag, t, u, v = self.LRSLR(x, y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, -0.5 * math.pi, u, -0.5 * math.pi, v], ctypes=["L", "R", "S", "L", "R"]))
+
+		# timefilp: L-R+(pi/2)S+L+(pi/2)R-
+		flag, t, u, v = self.LRSLR(-x, y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, 0.5 * math.pi, -u, 0.5 * math.pi, -v], ctypes=["L", "R", "S", "L", "R"]))
+
+		# reflect: R+L-(pi/2)S-R-(pi/2)L+
+		flag, t, u, v = self.LRSLR(x, -y, -phi)
+		if flag:
+			paths.append(self.Path(lengths=[t, -0.5 * math.pi, u, -0.5 * math.pi, v], ctypes=["R", "L", "S", "R", "L"]))
+
+		# timefilp + reflect: R-L+(pi/2)S+R+(pi/2)L-
+		flag, t, u, v = self.LRSLR(-x, -y, phi)
+		if flag:
+			paths.append(self.Path(lengths=[-t, 0.5 * math.pi, -u, 0.5 * math.pi, -v], ctypes=["R", "L", "S", "R", "L"]))
+
+		return paths
+
+	def interpolate(self, mode: str, length: float, init_pose: tuple):
+		"""
+		Planning path interpolation.
+
+		Parameters:
+			mode (str): motion, e.g., L, S, R
+			length (float): Single step motion path length
+			init_pose (tuple): Initial pose (x, y, yaw)
+
+		Returns:
+			new_pose (tuple): New pose (new_x, new_y, new_yaw) after moving
+		"""
+		x, y, yaw = init_pose
+
+		if mode == "S":
+			new_x = x + length / self.max_curv * math.cos(yaw)
+			new_y = y + length / self.max_curv * math.sin(yaw)
+			new_yaw = yaw
+		elif mode == "L":
+			new_x = x + (math.sin(yaw + length) - math.sin(yaw)) / self.max_curv
+			new_y = y - (math.cos(yaw + length) - math.cos(yaw)) / self.max_curv
+			new_yaw = yaw + length
+		elif mode == "R":
+			new_x = x - (math.sin(yaw - length) - math.sin(yaw)) / self.max_curv
+			new_y = y + (math.cos(yaw - length) - math.cos(yaw)) / self.max_curv
+			new_yaw = yaw - length
+		else:
+			raise NotImplementedError
+
+		return new_x, new_y, new_yaw
+
+	def generation(self, start_pose: tuple, goal_pose: tuple):
+		"""
+		Generate the Reeds Shepp Curve.
+
+		Parameters:
+			start_pose (tuple): Initial pose (x, y, yaw)
+			goal_pose (tuple): Target pose (x, y, yaw)
+
+		Returns:
+			best_cost (float): Best planning path length
+			best_mode: Best motion modes
+			x_list (list): Trajectory of x
+			y_list (list): Trajectory of y
+			yaw_list (list): Trajectory of yaw
+		"""
+		sx, sy, syaw = start_pose
+		gx, gy, gyaw = goal_pose
+
+		# coordinate transformation
+		dx, dy, dyaw = gx - sx, gy - sy, gyaw - syaw
+		x = (math.cos(syaw) * dx + math.sin(syaw) * dy) * self.max_curv
+		y = (-math.sin(syaw) * dx + math.cos(syaw) * dy) * self.max_curv
+
+		# select the best motion
+		planners = [self.SCS, self.CCC, self.CSC, self.CCCC, self.CCSC, self.CCSCC]
+		best_path, best_cost = None, float("inf")
+
+		for planner in planners:
+			paths = planner(x, y, dyaw)
+			for path in paths:
+				if path.path_length < best_cost:
+					best_path, best_cost = path, path.path_length
+
+		# interpolation
+		points_num = int(best_cost / self.step) + len(best_path.lengths) + 3
+		x_list = [0.0 for _ in range(points_num)]
+		y_list = [0.0 for _ in range(points_num)]
+		yaw_list = [0.0 for _ in range(points_num)]
+
+		i = 0
+		for mode_, seg_length in zip(best_path.ctypes, best_path.lengths):
+			# path increment
+			d_length = self.step if seg_length > 0.0 else -self.step
+			x, y, yaw = x_list[i], y_list[i], yaw_list[i]
+			# current path length
+			length = d_length
+			while abs(length) <= abs(seg_length):
+				i += 1
+				x_list[i], y_list[i], yaw_list[i] = self.interpolate(mode_, length, (x, y, yaw))
+				length += d_length
+			i += 1
+			x_list[i], y_list[i], yaw_list[i] = self.interpolate(mode_, seg_length, (x, y, yaw))
+		
+		# failed
+		if len(x_list) <= 1:
+			return None, None, [], [], []
+
+		# remove unused data
+		while len(x_list) >= 1 and x_list[-1] == 0.0:
+			x_list.pop()
+			y_list.pop()
+			yaw_list.pop()
+
+		# coordinate transformation
+		x_list_ = [math.cos(-syaw) * ix + math.sin(-syaw) * iy + sx for (ix, iy) in zip(x_list, y_list)]
+		y_list_ = [-math.sin(-syaw) * ix + math.cos(-syaw) * iy + sy for (ix, iy) in zip(x_list, y_list)]
+		yaw_list_ = [self.pi2pi(iyaw + syaw) for iyaw in yaw_list]
+
+		return best_cost / self.max_curv, best_path.ctypes, x_list_, y_list_, yaw_list_
+
+	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_yaw = [], [], []
+		for i in range(len(points) - 1):
+			_, _, x_list, y_list, yaw_list = 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 j in range(len(x_list)):
+				path_x.append(x_list[j])
+				path_y.append(y_list[j])
+				path_yaw.append(yaw_list[j])
+
+		# animation
+		plt.figure("curve generation")
+		# # static
+		# plt.plot(path_x, path_y, linewidth=2, c="#1f77b4")
+		# for x, y, theta in points:
+		# 	Plot.plotArrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
+
+		# dynamic
+		plt.ion()
+		for i in range(len(path_x)):
+			plt.clf()
+			plt.gcf().canvas.mpl_connect('key_release_event',
+											lambda event: [exit(0) if event.key == 'escape' else None])
+			plt.plot(path_x, path_y, linewidth=2, c="#1f77b4")
+			for x, y, theta in points:
+				Plot.plotArrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
+			Plot.plotCar(path_x[i], path_y[i], path_yaw[i], 1.5, 3, "black")
+			plt.axis("equal")
+			plt.title(str(self))
+			plt.draw()
+			plt.pause(0.001)
+
+		plt.axis("equal")
+		plt.title(str(self))
+		plt.show()
+
+
+	def _calTauOmega(self, u, v, xi, eta, phi):
+		delta = self.M(u - v)
+		A = math.sin(u) - math.sin(delta)
+		B = math.cos(u) - math.cos(delta) - 1.0
+
+		t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
+		t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
+
+		tau = self.M(t1 + math.pi) if t2 < 0 else self.M(t1)
+		omega = self.M(tau - u + v - phi)
+
+		return tau, omega