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

curve_generation/dubins_curve.py

@@ -0,0 +1,348 @@
+"""
+@file: dubins_curve.py
+@breif: Dubins curve generation
+@author: Winter
+@update: 2023.5.31
+"""
+import math
+import numpy as np
+
+from scipy.spatial.transform import Rotation as Rot
+from python_motion_planning.utils import Plot
+from .curve import Curve
+
+class Dubins(Curve):
+	"""
+	Class for Dubins 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 Dubins
+		>>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
+		>>> generator = Dubins(step, max_curv)
+		>>> generator.run(points)
+
+	References:
+		[1] On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents
+	"""
+	def __init__(self, step: float, max_curv: float) -> None:
+		super().__init__(step)
+		self.max_curv = max_curv
+	
+	def __str__(self) -> str:
+		return "Dubins Curve"
+
+	def LSL(self, alpha: float, beta: float, dist: float):
+		"""
+		Left-Straight-Left generation mode.
+
+		Parameters:
+			alpha (float): Initial pose of (0, 0, alpha)
+			beta (float): Goal pose of (dist, 0, beta)
+			dist (float): The distance between the initial and goal pose
+
+		Returns:
+			t (float): Moving lenght of segments
+			p (float): Moving lenght of segments
+			q (float): Moving lenght of segments
+			mode (list): Motion mode
+		"""
+		sin_a, sin_b, cos_a, cos_b, _, cos_a_b  = self.trigonometric(alpha, beta)
+
+		p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b)
+		if p_lsl < 0:
+			return None, None, None, ["L", "S", "L"]
+		else:
+			p_lsl = math.sqrt(p_lsl)
+
+		t_lsl = self.mod2pi(-alpha + math.atan2(cos_b - cos_a, dist + sin_a - sin_b))
+		q_lsl = self.mod2pi(beta - math.atan2(cos_b - cos_a, dist + sin_a - sin_b))
+
+		return t_lsl, p_lsl, q_lsl, ["L", "S", "L"]
+
+	def RSR(self, alpha: float, beta: float, dist: float):
+		"""
+		Right-Straight-Right generation mode.
+
+		Parameters:
+			alpha (float): Initial pose of (0, 0, alpha)
+			beta (float): Goal pose of (dist, 0, beta)
+
+		Returns:
+			t (float): Moving lenght of segments
+			p (float): Moving lenght of segments
+			q (float): Moving lenght of segments
+			mode (list): Motion mode
+		"""
+		sin_a, sin_b, cos_a, cos_b, _, cos_a_b  = self.trigonometric(alpha, beta)
+
+		p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a)
+		if p_rsr < 0:
+			return None, None, None, ["R", "S", "R"]
+		else:
+			p_rsr = math.sqrt(p_rsr)
+
+		t_rsr = self.mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b))
+		q_rsr = self.mod2pi(-beta + math.atan2(cos_a - cos_b, dist - sin_a + sin_b))
+
+		return t_rsr, p_rsr, q_rsr, ["R", "S", "R"]
+
+	def LSR(self, alpha: float, beta: float, dist: float):
+		"""
+		Left-Straight-Right generation mode.
+
+		Parameters:
+			alpha (float): Initial pose of (0, 0, alpha)
+			beta (float): Goal pose of (dist, 0, beta)
+			dist (float): The distance between the initial and goal pose
+
+		Returns:
+			t (float): Moving lenght of segments
+			p (float): Moving lenght of segments
+			q (float): Moving lenght of segments
+			mode (list): Motion mode
+		"""
+		sin_a, sin_b, cos_a, cos_b, _, cos_a_b  = self.trigonometric(alpha, beta)
+
+		p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b)
+		if p_lsr < 0:
+			return None, None, None, ["L", "S", "R"]
+		else:
+			p_lsr = math.sqrt(p_lsr)
+
+		t_lsr = self.mod2pi(-alpha + math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr))
+		q_lsr = self.mod2pi(-beta + math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr))
+
+		return t_lsr, p_lsr, q_lsr, ["L", "S", "R"]
+
+
+	def RSL(self, alpha: float, beta: float, dist: float):
+		"""
+		Right-Straight-Left generation mode.
+
+		Parameters:
+			alpha (float): Initial pose of (0, 0, alpha)
+			beta (float): Goal pose of (dist, 0, beta)
+			dist (float): The distance between the initial and goal pose
+
+		Returns:
+			t (float): Moving lenght of segments
+			p (float): Moving lenght of segments
+			q (float): Moving lenght of segments
+			mode (list): Motion mode
+		"""
+		sin_a, sin_b, cos_a, cos_b, _, cos_a_b  = self.trigonometric(alpha, beta)
+
+		p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b)
+		if p_rsl < 0:
+			return None, None, None, ["R", "S", "L"]
+		else:
+			p_rsl = math.sqrt(p_rsl)
+
+		t_rsl = self.mod2pi(alpha - math.atan2(cos_a + cos_b, dist - sin_a - sin_b) + math.atan2(2.0, p_rsl))
+		q_rsl = self.mod2pi(beta - math.atan2(cos_a + cos_b, dist - sin_a - sin_b) + math.atan2(2.0, p_rsl))
+
+		return t_rsl, p_rsl, q_rsl, ["R", "S", "L"]
+
+
+	def RLR(self, alpha: float, beta: float, dist: float):
+		"""
+		Right-Left-Right generation mode.
+
+		Parameters:
+			alpha (float): Initial pose of (0, 0, alpha)
+			beta (float): Goal pose of (dist, 0, beta)
+			dist (float): The distance between the initial and goal pose
+
+		Returns:
+			t (float): Moving lenght of segments
+			p (float): Moving lenght of segments
+			q (float): Moving lenght of segments
+			mode (list): Motion mode
+		"""
+		sin_a, sin_b, cos_a, cos_b, _, cos_a_b  = self.trigonometric(alpha, beta)
+
+		p_rlr = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0
+		if abs(p_rlr) > 1.0:
+			return None, None, None, ["R", "L", "R"]
+		else:
+			p_rlr = self.mod2pi(2 * math.pi - math.acos(p_rlr))
+		
+		t_rlr = self.mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + p_rlr / 2.0)
+		q_rlr = self.mod2pi(alpha - beta - t_rlr + p_rlr)
+
+		return t_rlr, p_rlr, q_rlr, ["R", "L", "R"]
+
+	def LRL(self, alpha: float, beta: float, dist: float):
+		"""
+		Left-Right-Left generation mode.
+
+		Parameters:
+			alpha (float): Initial pose of (0, 0, alpha)
+			beta (float): Goal pose of (dist, 0, beta)
+			dist (float): The distance between the initial and goal pose
+
+		Returns:
+			t (float): Moving lenght of segments
+			p (float): Moving lenght of segments
+			q (float): Moving lenght of segments
+			mode (list): Motion mode
+		"""
+		sin_a, sin_b, cos_a, cos_b, _, cos_a_b  = self.trigonometric(alpha, beta)
+
+		p_lrl = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0
+		if abs(p_lrl) > 1.0:
+			return None, None, None, ["L", "R", "L"]
+		else:
+			p_lrl = self.mod2pi(2 * math.pi - math.acos(p_lrl))
+
+		t_lrl = self.mod2pi(-alpha + math.atan2(-cos_a + cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)
+		q_lrl = self.mod2pi(beta - alpha - t_lrl + p_lrl)
+
+		return t_lrl, p_lrl, q_lrl, ["L", "R", "L"]    
+
+	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 Dubins 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 (list): 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
+		gx, gy = gx - sx, gy - sy
+		theta = self.mod2pi(math.atan2(gy, gx))
+		dist = math.hypot(gx, gy) * self.max_curv
+		alpha = self.mod2pi(syaw - theta)
+		beta = self.mod2pi(gyaw - theta)
+
+		# select the best motion
+		planners = [self.LSL, self.RSR, self.LSR, self.RSL, self.RLR, self.LRL]
+		best_t, best_p, best_q, best_mode, best_cost = None, None, None, None, float("inf")
+
+		for planner in planners:
+			t, p, q, mode = planner(alpha, beta, dist)
+			if t is None:
+				continue
+			cost = (abs(t) + abs(p) + abs(q))
+			if best_cost > cost:
+				best_t, best_p, best_q, best_mode, best_cost = t, p, q, mode, cost
+		
+		# interpolation
+		segments = [best_t, best_p, best_q]
+		points_num = int(sum(segments) / self.step) + len(segments) + 3
+		x_list = [0.0 for _ in range(points_num)]
+		y_list = [0.0 for _ in range(points_num)]
+		yaw_list = [alpha for _ in range(points_num)]
+
+		i = 0
+		for mode_, seg_length in zip(best_mode, segments):
+			# 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
+		rot = Rot.from_euler('z', theta).as_matrix()[0:2, 0:2]
+		converted_xy = rot @ np.stack([x_list, y_list])
+		x_list = converted_xy[0, :] + sx
+		y_list = converted_xy[1, :] + sy
+		yaw_list = [self.pi2pi(i_yaw + theta) for i_yaw in yaw_list]
+
+		return best_cost, best_mode, 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")
+		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')
+
+		plt.axis("equal")
+		plt.title(str(self))
+		plt.show()
+
+		
+

curve_generation/fem_pos_smooth.py

@@ -0,0 +1,114 @@
+"""
+@file: fem_pos_smooth.py
+@breif: Fem-Pos Smoother
+@author: Winter
+@update: 2024.3.23
+"""
+import osqp
+import numpy as np
+from scipy import sparse
+
+from .curve import Curve
+
+class FemPosSmoother(Curve):
+	"""
+	Class for Fem-pos smoother.
+
+	Parameters:
+
+	Examples:
+		>>> from python_motion_planning.curve_generation import FemPosSmoother
+		>>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
+		>>> generator = FemPosSmoother(w_smooth, w_length, w_ref, dx_l, dx_u, dy_l, dy_u)
+		>>> generator.run(points)
+	"""
+	def __init__(self, w_smooth: float, w_length: float, w_ref: float,
+		dx_l: float, dx_u: float, dy_l: float, dy_u: float) -> None:
+		super().__init__(0.1)
+		self.w_smooth = w_smooth
+		self.w_length = w_length
+		self.w_ref = w_ref
+		self.dx_l = dx_l
+		self.dx_u = dx_u
+		self.dy_l = dy_l
+		self.dy_u = dy_u
+
+	def __str__(self) -> str:
+		return "Fem-pos Smoother"
+
+	def generation(self, start_pose: tuple, goal_pose: tuple):
+		pass
+	
+	def run(self, points: list, display: bool = True):
+		"""
+		Running both generation and animation.
+
+		Parameters:
+			points (list[tuple]): path points
+		"""
+		assert len(points) >= 3, "Number of points should be at least 3."
+		if len(points[0]) == 3:
+			points = [(ix, iy) for ix, iy, _ in points]
+		import matplotlib.pyplot as plt
+
+		n = len(points)
+		P = np.zeros((2 * n, 2 * n))
+
+		X = np.eye(2) * self.w_smooth
+		Y = np.eye(2) * self.w_length
+		Z = np.eye(2) * self.w_ref
+
+		P[0:2, 0:2] = X + Y + Z
+		P[0:2, 2:4] = -2 * X - Y
+		P[2:4, 2:4] = 5 * X + 2 * Y + Z
+		P[2 * n - 2:2 * n, 2 * n - 2:2 * n] =  X + Y + Z
+		P[2 * n - 4:2 * n - 2, 2 * n - 4:2 * n - 2] =  5 * X + 2 * Y + Z
+		P[2 * n - 4:2 * n - 2, 2 * n - 2:2 * n] =  -2 * X - Y
+
+		for i in range(2, n - 2):
+			P[2 * i:2 * i + 2, 2 * i:2 * i + 2] = 6 * X + 2 * Y + Z
+		for i in range(2, n - 1):
+			P[2 * i - 2:2 * i, 2 * i:2 * i + 2] = -4 * X - Y
+		for i in range(2, n):
+			P[2 * i - 4:2 * i - 2, 2 * i:2 * i + 2] = X
+		
+		A_I = np.eye(2 * n)
+		g = np.zeros((2 * n, 1))
+		lower = np.zeros((2 * n, 1))
+		upper = np.zeros((2 * n, 1))
+		for i, p in enumerate(points):
+			g[2 * i], g[2 * i + 1] = -2 * self.w_ref * p[0], -2 * self.w_ref * p[1]
+			lower[2 * i], lower[2 * i + 1] = p[0] - self.dx_l, p[1] - self.dy_l
+			upper[2 * i], upper[2 * i + 1] = p[0] + self.dx_u, p[1] + self.dy_u
+
+		# solve
+		solver = osqp.OSQP()
+		solver.setup(sparse.csc_matrix(P), g, sparse.csc_matrix(A_I), lower, upper, verbose=False)
+		res = solver.solve()
+		opt = res.x
+
+		path_x, path_y = [], []
+		for i in range(n):
+			path_x.append(opt[2 * i])
+			path_y.append(opt[2 * i + 1])
+
+		if display:
+			plt.figure("curve generation")
+			plt.plot(path_x, path_y, linewidth=2, c="#ff0000", marker="o", label="smooth path")
+			raw_x, raw_y = [], []
+			for i, (x, y) in enumerate(points):
+				# label = "bounding box" if i == 0 else None
+				# plt.gca().add_patch(
+				# 	plt.Rectangle(xy=(x - self.dx_l, y - self.dy_l), width=self.dx_u + self.dx_l,
+				# 	height=self.dy_u + self.dy_l, color='red', linestyle="--", fill=False, label=label)
+				# )
+				raw_x.append(x)
+				raw_y.append(y)
+			plt.plot(raw_x, raw_y, linewidth=2, c="#1f77b4", marker="x", label="raw path")
+			# plt.axis("equal")
+			plt.legend()
+			plt.title(str(self))
+
+			plt.show()
+		
+		return [(ix, iy) for (ix, iy) in zip(path_x, path_y)]

curve_generation/polynomial_curve.py

@@ -0,0 +1,226 @@
+"""
+@file: polynomial_curve.py
+@breif: Polynomial curve generation
+@author: Winter
+@update: 2023.7.25
+"""
+import math
+import numpy as np
+
+from python_motion_planning.utils import Plot
+from .curve import Curve
+
+class Polynomial(Curve):
+    """
+    Class for polynomial curve generation(Quintic).
+
+    Parameters:
+        step (float): Simulation or interpolation size
+        max_acc (float): Maximum acceleration
+        max_jerk (float): Maximum jerk
+
+    Examples:
+        >>> from python_motion_planning.curve_generation import Polynomial
+        >>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
+        >>> generator = Polynomial(step, max_acc, max_jerk)
+        >>> generator.run(points)
+    """
+    def __init__(self, step: float, max_acc: float, max_jerk: float) -> None:
+        super().__init__(step)
+        self.max_acc = max_acc
+        self.max_jerk = max_jerk
+        self.dt = 0.1
+        self.t_min = 1
+        self.t_max = 30
+    
+    def __str__(self) -> str:
+        return "Quintic Polynomial Curve"
+    
+    class Poly:
+        """
+        Polynomial interpolation solver
+        """
+        def __init__(self, state0: tuple, state1: tuple, t: float) -> None:
+            x0, v0, a0 = state0
+            xt, vt, at = state1
+
+            A = np.array([[t ** 3, t ** 4, t ** 5],
+                          [3 * t ** 2, 4 * t ** 3, 5 * t ** 4],
+                          [6 * t, 12 * t ** 2, 20 * t ** 3]])
+            b = np.array([xt - x0 - v0 * t - a0 * t ** 2 / 2,
+                          vt - v0 - a0 * t,
+                          at - a0])
+            X = np.linalg.solve(A, b)
+
+            # Quintic polynomial coefficient
+            self.p0 = x0
+            self.p1 = v0
+            self.p2 = a0 / 2.0
+            self.p3 = X[0]
+            self.p4 = X[1]
+            self.p5 = X[2]
+
+        def x(self, t):
+            return self.p0 + self.p1 * t + self.p2 * t ** 2 + \
+                self.p3 * t ** 3 + self.p4 * t ** 4 + self.p5 * t ** 5
+
+        def dx(self, t):
+            return self.p1 + 2 * self.p2 * t + 3 * self.p3 * t ** 2 + \
+                4 * self.p4 * t ** 3 + 5 * self.p5 * t ** 4
+
+        def ddx(self, t):
+            return 2 * self.p2 + 6 * self.p3 * t + 12 * self.p4 * t ** 2 + 20 * self.p5 * t ** 3
+
+        def dddx(self, t):
+            return 6 * self.p3 + 24 * self.p4 * t + 60 * self.p5 * t ** 2
+    
+    class Trajectory:
+        """
+        Polynomial interpolation solver
+        """
+        def __init__(self):
+            self.clear()
+        
+        def clear(self):
+            self.time = []
+            self.x = []
+            self.y = []
+            self.yaw = []
+            self.v = []
+            self.a = []
+            self.jerk = []
+        
+        @property
+        def size(self):
+            assert len(self.time) == len(self.x) and \
+                   len(self.x) == len(self.y) and    \
+                   len(self.y) == len(self.yaw) and  \
+                   len(self.yaw) == len(self.v) and  \
+                   len(self.v) == len(self.a) and    \
+                   len(self.a) == len(self.jerk),    \
+                   "Unequal dimensions of each attribute, this should not happen."
+            return len(self.time)
+
+    def generation(self, start_pose: tuple, goal_pose: tuple):
+        """
+        Generate the polynomial Curve.
+
+        Parameters:
+            start_pose (tuple): Initial pose (x, y, yaw)
+            goal_pose (tuple): Target pose (x, y, yaw)
+
+        Returns:
+            traj (Traj): The first trajectory that satisfies the acceleration and jerk constraint
+        """
+        sx, sy, syaw, sv, sa = start_pose
+        gx, gy, gyaw, gv, ga = goal_pose
+        
+        sv_x = sv * math.cos(syaw)
+        sv_y = sv * math.sin(syaw)
+        gv_x = gv * math.cos(gyaw)
+        gv_y = gv * math.sin(gyaw)
+
+        sa_x = sa * math.cos(syaw)
+        sa_y = sa * math.sin(syaw)
+        ga_x = ga * math.cos(gyaw)
+        ga_y = ga * math.sin(gyaw)
+        
+        traj = self.Trajectory()
+
+        for T in np.arange(self.t_min, self.t_max, self.step):
+            x_psolver = self.Poly((sx, sv_x, sa_x), (gx, gv_x, ga_x), T)
+            y_psolver = self.Poly((sy, sv_y, sa_y), (gy, gv_y, ga_y), T)
+
+            for t in np.arange(0.0, T + self.dt, self.dt):
+                traj.time.append(t)
+                traj.x.append(x_psolver.x(t))
+                traj.y.append(y_psolver.x(t))
+
+                vx = x_psolver.dx(t)
+                vy = y_psolver.dx(t)
+                traj.v.append(math.hypot(vx, vy))
+                traj.yaw.append(math.atan2(vy, vx))
+
+                ax = x_psolver.ddx(t)
+                ay = y_psolver.ddx(t)
+                a = math.hypot(ax, ay)
+                if len(traj.v) >= 2 and traj.v[-1] - traj.v[-2] < 0.0:
+                    a *= -1
+                traj.a.append(a)
+
+                jx = x_psolver.dddx(t)
+                jy = y_psolver.dddx(t)
+                j = math.hypot(jx, jy)
+                if len(traj.a) >= 2 and traj.a[-1] - traj.a[-2] < 0.0:
+                    j *= -1
+                traj.jerk.append(j)
+
+            if max(np.abs(traj.a)) <= self.max_acc and \
+               max(np.abs(traj.jerk)) <= self.max_jerk:
+                return traj
+            else:
+                traj.clear()
+
+        return traj
+
+    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
+
+        # generate velocity and acceleration constraints heuristically
+        v = [0]
+        for i in range(len(points) - 1):
+            v.append(1.0)
+
+        a = [(v[i + 1] - v[i]) / 5 for i in range(len(points) - 1)]
+        a.append(0)
+
+        # generate curve
+        path_x, path_y, path_yaw = [], [], []
+        for i in range(len(points) - 1):
+            path = self.generation(
+				(points[i][0], points[i][1], np.deg2rad(points[i][2]), v[i], a[i]),
+				(points[i + 1][0], points[i + 1][1], np.deg2rad(points[i + 1][2]), v[i + 1], a[i + 1])
+            )
+
+            for j in range(path.size):
+                path_x.append(path.x[j])
+                path_y.append(path.y[j])
+                path_yaw.append(path.yaw[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')
+
+        # plt.axis("equal")
+        # plt.title(str(self))
+
+        # 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.show()
+
+
+