python-motion-planning 2.0__tar.gz → 2.0.dev2__tar.gz

{python_motion_planning-2.0 → python_motion_planning-2.0.dev2}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: python-motion-planning
-Version: 2.0
+Version: 2.0.dev2
 Summary: Motion planning algorithms for Python
 Maintainer-email: Wu Maojia <omige@mail.nwpu.edu.cn>, Yang Haodong <913982779@qq.com>
 License: GNU GENERAL PUBLIC LICENSE
@@ -697,8 +697,6 @@ Dynamic: license-file
 
 # Introduction
 
-`Python Motion Planning` repository provides the implementations of common `Motion planning` algorithms, including path planners on N-D grid, controllers for path-tracking, curve generators, a visualizer based on matplotlib and a toy physical simulator to test controllers.
-
 `Motion planning` plans the state sequence of the robot without conflict between the start and goal. 
 
 `Motion planning` mainly includes `Path planning` and `Trajectory planning`.
@@ -706,6 +704,8 @@ Dynamic: license-file
 * `Path Planning`: It's based on path constraints (such as obstacles), planning the optimal path sequence for the robot to travel without conflict between the start and goal.
 * `Trajectory planning`: It plans the motion state to approach the global path based on kinematics, dynamics constraints and path sequence.
 
+This repository provides the implementations of common `Motion planning` algorithms, including path planners on N-D grid and controllers for path-tracking, a visualizer based on matplotlib and a toy physical simulator to test controllers.
+
 The theory analysis can be found at [motion-planning](https://blog.csdn.net/frigidwinter/category_11410243.html).
 
 We also provide [ROS C++](https://github.com/ai-winter/ros_motion_planning) version and [Matlab](https://github.com/ai-winter/matlab_motion_planning) version.
@@ -732,8 +732,7 @@ python_motion_planning
 |   ├─graph_search
 |   ├─sample_search
 |   └─hybrid_search
-└─traj_optimizer
-|   └─curve_generator
+└─curve_generator
 ```
 
 ## Install
@@ -747,7 +746,7 @@ conda activate pmp
 To install the repository, please run the following command in shell.
 
 ```shell
-pip install python-motion-planning
+pip install python-motion-planning==2.0.dev2
 ```
 
 ## Run
@@ -804,7 +803,7 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 | **PID**     |![pid_2d.gif](assets/pid_2d.gif)|Not implemented
 | **APF**     |![apf_2d.gif](assets/apf_2d.gif)|Not implemented
 | **DWA**     |![dwa_2d.gif](assets/dwa_2d.gif)|Not implemented
-| **RPP**     |![rpp_2d.gif](assets/rpp_2d.gif)|Not implemented
+| **RPP**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **LQR**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **MPC**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **MPPI**    |Not implemented|Not implemented
@@ -815,14 +814,17 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 
 ## Curve Generator
 
+The visualization of the curve generators has not been implemented in current version. They can be visualized in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1).
+
 |Generator|2D|3D|
 | ------- | -------------------------------------------------------- | -------------------------------------------------------- 
-| **Polynomia** | ![polynomial_2d.svg](assets/polynomial_2d.svg)|Not implemented
-| **Bezier** |![bezier_2d.svg](assets/bezier_2d.svg)|Not implemented
-| **Cubic Spline** |![cubic_spline_2d.svg](assets/cubic_spline_2d.svg)|Not implemented
-| **BSpline** |![bspline_2d.svg](assets/bspline_2d.svg)|Not implemented
-| **Dubins** |![dubins_2d.svg](assets/dubins_2d.svg)|Not implemented
-| **Reeds-Shepp** |![reeds_shepp_2d.svg](assets/reeds_shepp_2d.svg)|Not implemented
+| **Polynomia** | ![polynomial_curve_python.gif](assets/polynomial_curve_python.gif)|Not implemented
+| **Bezier** |![bezier_curve_python.png](assets/bezier_curve_python.png)|Not implemented
+| **Cubic Spline** |![cubic_spline_python.png](assets/cubic_spline_python.png)|Not implemented
+| **BSpline** |![bspline_curve_python.png](assets/bspline_curve_python.png)|Not implemented
+| **Dubins** |![dubins_curve_python.png](assets/dubins_curve_python.png)|Not implemented
+| **Reeds-Shepp** |![reeds_shepp_python.png](assets/reeds_shepp_python.gif)|Not implemented
+| **Fem-Pos Smoother** |![fem_pos_smoother_python.png](assets/fem_pos_smoother_python.png)|Not implemented
 
 # Future Works
 
@@ -832,8 +834,6 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 
 * Path planning on topological map.
 
-* Sample search with Dubins or Reeds-Shepp curves.
-
 * Application on ROS2.
 
 * Application in mainstream robot simulation environments (e.g. Gazebo, Carla, Airsim, PyBullet, MuJoCo, Issac Sim).

{python_motion_planning-2.0 → python_motion_planning-2.0.dev2}/README.md

@@ -1,8 +1,6 @@
 
 # Introduction
 
-`Python Motion Planning` repository provides the implementations of common `Motion planning` algorithms, including path planners on N-D grid, controllers for path-tracking, curve generators, a visualizer based on matplotlib and a toy physical simulator to test controllers.
-
 `Motion planning` plans the state sequence of the robot without conflict between the start and goal. 
 
 `Motion planning` mainly includes `Path planning` and `Trajectory planning`.
@@ -10,6 +8,8 @@
 * `Path Planning`: It's based on path constraints (such as obstacles), planning the optimal path sequence for the robot to travel without conflict between the start and goal.
 * `Trajectory planning`: It plans the motion state to approach the global path based on kinematics, dynamics constraints and path sequence.
 
+This repository provides the implementations of common `Motion planning` algorithms, including path planners on N-D grid and controllers for path-tracking, a visualizer based on matplotlib and a toy physical simulator to test controllers.
+
 The theory analysis can be found at [motion-planning](https://blog.csdn.net/frigidwinter/category_11410243.html).
 
 We also provide [ROS C++](https://github.com/ai-winter/ros_motion_planning) version and [Matlab](https://github.com/ai-winter/matlab_motion_planning) version.
@@ -36,8 +36,7 @@ python_motion_planning
 |   ├─graph_search
 |   ├─sample_search
 |   └─hybrid_search
-└─traj_optimizer
-|   └─curve_generator
+└─curve_generator
 ```
 
 ## Install
@@ -51,7 +50,7 @@ conda activate pmp
 To install the repository, please run the following command in shell.
 
 ```shell
-pip install python-motion-planning
+pip install python-motion-planning==2.0.dev2
 ```
 
 ## Run
@@ -108,7 +107,7 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 | **PID**     |![pid_2d.gif](assets/pid_2d.gif)|Not implemented
 | **APF**     |![apf_2d.gif](assets/apf_2d.gif)|Not implemented
 | **DWA**     |![dwa_2d.gif](assets/dwa_2d.gif)|Not implemented
-| **RPP**     |![rpp_2d.gif](assets/rpp_2d.gif)|Not implemented
+| **RPP**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **LQR**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **MPC**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **MPPI**    |Not implemented|Not implemented
@@ -119,14 +118,17 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 
 ## Curve Generator
 
+The visualization of the curve generators has not been implemented in current version. They can be visualized in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1).
+
 |Generator|2D|3D|
 | ------- | -------------------------------------------------------- | -------------------------------------------------------- 
-| **Polynomia** | ![polynomial_2d.svg](assets/polynomial_2d.svg)|Not implemented
-| **Bezier** |![bezier_2d.svg](assets/bezier_2d.svg)|Not implemented
-| **Cubic Spline** |![cubic_spline_2d.svg](assets/cubic_spline_2d.svg)|Not implemented
-| **BSpline** |![bspline_2d.svg](assets/bspline_2d.svg)|Not implemented
-| **Dubins** |![dubins_2d.svg](assets/dubins_2d.svg)|Not implemented
-| **Reeds-Shepp** |![reeds_shepp_2d.svg](assets/reeds_shepp_2d.svg)|Not implemented
+| **Polynomia** | ![polynomial_curve_python.gif](assets/polynomial_curve_python.gif)|Not implemented
+| **Bezier** |![bezier_curve_python.png](assets/bezier_curve_python.png)|Not implemented
+| **Cubic Spline** |![cubic_spline_python.png](assets/cubic_spline_python.png)|Not implemented
+| **BSpline** |![bspline_curve_python.png](assets/bspline_curve_python.png)|Not implemented
+| **Dubins** |![dubins_curve_python.png](assets/dubins_curve_python.png)|Not implemented
+| **Reeds-Shepp** |![reeds_shepp_python.png](assets/reeds_shepp_python.gif)|Not implemented
+| **Fem-Pos Smoother** |![fem_pos_smoother_python.png](assets/fem_pos_smoother_python.png)|Not implemented
 
 # Future Works
 
@@ -136,8 +138,6 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 
 * Path planning on topological map.
 
-* Sample search with Dubins or Reeds-Shepp curves.
-
 * Application on ROS2.
 
 * Application in mainstream robot simulation environments (e.g. Gazebo, Carla, Airsim, PyBullet, MuJoCo, Issac Sim).

{python_motion_planning-2.0 → python_motion_planning-2.0.dev2}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "python-motion-planning"
-version = "2.0"
+version = "2.0.dev2"
 description = "Motion planning algorithms for Python"
 maintainers = [
     {name = "Wu Maojia", email = "omige@mail.nwpu.edu.cn"},

{python_motion_planning-2.0 → python_motion_planning-2.0.dev2}/src/python_motion_planning/__init__.py

@@ -1,4 +1,4 @@
 from .common import *
 from .path_planner import *
 from .controller import *
-from .traj_optimizer import *
+from .curve_generator import *

{python_motion_planning-2.0 → python_motion_planning-2.0.dev2}/src/python_motion_planning/common/utils/geometry.py

@@ -35,31 +35,42 @@ class Geometry:
         else:
             raise ValueError("Invalid distance type")
 
-    @staticmethod
-    def mod_to_2pi(orient: np.ndarray) -> np.ndarray:
-        """
-        Regularize orientation to be within [0, 2*pi)
+    # @staticmethod
+    # def angle(v1: tuple, v2: tuple) -> float:
+    #     """
+    #     Calculate the angle between two vectors
 
-        Args:
-            orient: the orientation angle
+    #     Args:
+    #         v1: First vector
+    #         v2: Second vector
 
-        Returns:
-            new_orient: modded orientation
-        """
-        return np.mod(orient, 2 * np.pi)
+    #     Returns:
+    #         angle_rad: Angle in rad between the two vectors
+    #     """
+    #     if len(v1) != len(v2):
+    #         raise ValueError("Dimension mismatch")
+        
+    #     dot_product = sum(a * b for a, b in zip(v1, v2))
+    #     v1_norm = math.sqrt(sum(a**2 for a in v1))
+    #     v2_norm = math.sqrt(sum(b**2 for b in v2))
+        
+    #     if  v1_norm == 0 or v2_norm == 0:
+    #         raise ValueError("Zero vector cannot calculate angle")
+
+    #     cos_theta = dot_product / (v1_norm * v2_norm)
+
+    #     cos_theta = min(1.0, max(-1.0, cos_theta))
+
+    #     angle_rad = math.acos(cos_theta)
+        
+    #     return angle_rad
 
     @staticmethod
     def regularize_orient(orient: np.ndarray) -> np.ndarray:
         """
         Regularize orientation to be within (-pi, pi]
-
-        Args:
-            orient: the orientation angle
-
-        Returns:
-            new_orient: regularized orientation
         """
-        return -np.mod(-orient + np.pi, 2 * np.pi) + np.pi
+        return np.mod(orient + np.pi, 2 * np.pi) - np.pi
         
     @staticmethod
     def add_orient_to_2d_path(path: List[Tuple[float, float]]) -> List[Tuple[float, float, float]]:
@@ -91,4 +102,4 @@ class Geometry:
         last_x, last_y = path[-1]
         path_with_orient.append((last_x, last_y, path_with_orient[-1][2]))
         
-        return path_with_orient
+        return path_with_orient

python_motion_planning-2.0.dev2/src/python_motion_planning/curve_generator/__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-2.0.dev2/src/python_motion_planning/curve_generator/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-2.0.dev2/src/python_motion_planning/curve_generator/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)]