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

python_motion_planning/utils/environment/env.py

@@ -0,0 +1,134 @@
+"""
+@file: env.py
+@breif: 2-dimension environment
+@author: Winter
+@update: 2023.1.13
+"""
+from math import sqrt
+from abc import ABC, abstractmethod
+from scipy.spatial import cKDTree
+import numpy as np
+
+from .node import Node
+
+class Env(ABC):
+    """
+    Class for building 2-d workspace of robots.
+
+    Parameters:
+        x_range (int): x-axis range of enviroment
+        y_range (int): y-axis range of environmet
+        eps (float): tolerance for float comparison
+
+    Examples:
+        >>> from python_motion_planning.utils import Env
+        >>> env = Env(30, 40)
+    """
+    def __init__(self, x_range: int, y_range: int, eps: float = 1e-6) -> None:
+        # size of environment
+        self.x_range = x_range  
+        self.y_range = y_range
+        self.eps = eps
+
+    @property
+    def grid_map(self) -> set:
+        return {(i, j) for i in range(self.x_range) for j in range(self.y_range)}
+
+    @abstractmethod
+    def init(self) -> None:
+        pass
+
+class Grid(Env):
+    """
+    Class for discrete 2-d grid map.
+    """
+    def __init__(self, x_range: int, y_range: int) -> None:
+        super().__init__(x_range, y_range)
+        # allowed motions
+        self.motions = [Node((-1, 0), None, 1, None), Node((-1, 1),  None, sqrt(2), None),
+                        Node((0, 1),  None, 1, None), Node((1, 1),   None, sqrt(2), None),
+                        Node((1, 0),  None, 1, None), Node((1, -1),  None, sqrt(2), None),
+                        Node((0, -1), None, 1, None), Node((-1, -1), None, sqrt(2), None)]
+        # obstacles
+        self.obstacles = None
+        self.obstacles_tree = None
+        self.init()
+    
+    def init(self) -> None:
+        """
+        Initialize grid map.
+        """
+        x, y = self.x_range, self.y_range
+        obstacles = set()
+
+        # boundary of environment
+        for i in range(x):
+            obstacles.add((i, 0))
+            obstacles.add((i, y - 1))
+        for i in range(y):
+            obstacles.add((0, i))
+            obstacles.add((x - 1, i))
+
+        # user-defined obstacles        
+        for i in range(10, 21):
+            obstacles.add((i, 15))
+        for i in range(15):
+            obstacles.add((20, i))
+        for i in range(15, 30):
+            obstacles.add((30, i))
+        for i in range(16):
+            obstacles.add((40, i))
+
+        self.obstacles = obstacles
+        self.obstacles_tree = cKDTree(np.array(list(obstacles)))
+
+    def update(self, obstacles):
+        self.obstacles = obstacles 
+        self.obstacles_tree = cKDTree(np.array(list(obstacles)))
+
+
+class Map(Env):
+    """
+    Class for continuous 2-d map.
+    """
+    def __init__(self, x_range: int, y_range: int) -> None:
+        super().__init__(x_range, y_range)
+        self.boundary = None
+        self.obs_circ = None
+        self.obs_rect = None
+        self.init()
+
+    def init(self):
+        """
+        Initialize map.
+        """
+        x, y = self.x_range, self.y_range
+
+        # boundary of environment
+        self.boundary = [
+            [0, 0, 1, y],
+            [0, y, x, 1],
+            [1, 0, x, 1],
+            [x, 1, 1, y]
+        ]
+
+        # user-defined obstacles
+        self.obs_rect = [
+            [14, 12, 8, 2],
+            [18, 22, 8, 3],
+            [26, 7, 2, 12],
+            [32, 14, 10, 2]
+        ]
+
+        self.obs_circ = [
+            [7, 12, 3],
+            [46, 20, 2],
+            [15, 5, 2],
+            [37, 7, 3],
+            [37, 23, 3]
+        ]
+
+    def update(self, boundary, obs_circ, obs_rect):
+        self.boundary = boundary if boundary else self.boundary
+        self.obs_circ = obs_circ if obs_circ else self.obs_circ
+        self.obs_rect = obs_rect if obs_rect else self.obs_rect

python_motion_planning/utils/environment/node.py

@@ -0,0 +1,85 @@
+"""
+@file: node.py
+@breif: 2-dimension node data stucture
+@author: Yang Haodong, Wu Maojia
+@update: 2024.3.15
+"""
+
+class Node(object):
+    """
+    Class for searching nodes.
+
+    Parameters:
+        current (tuple): current coordinate
+        parent (tuple): coordinate of parent node
+        g (float): path cost
+        h (float): heuristic cost
+
+    Examples:
+        >>> from env import Node
+        >>> node1 = Node((1, 0), (2, 3), 1, 2)
+        >>> node2 = Node((1, 0), (2, 5), 2, 8)
+        >>> node3 = Node((2, 0), (1, 6), 3, 1)
+        ...
+        >>> node1 + node2
+        >>> Node((2, 0), (2, 3), 3, 2)
+        ...
+        >>> node1 == node2
+        >>> True
+        ...
+        >>> node1 != node3
+        >>> True
+    """
+    def __init__(self, current: tuple, parent: tuple = None, g: float = 0, h: float = 0) -> None:
+        self.current = current
+        self.parent = parent
+        self.g = g
+        self.h = h
+    
+    def __add__(self, node):
+        assert isinstance(node, Node)
+        return Node((self.x + node.x, self.y + node.y), self.parent, self.g + node.g, self.h)
+
+    def __eq__(self, node) -> bool:
+        if not isinstance(node, Node):
+            return False
+        return self.current == node.current
+    
+    def __ne__(self, node) -> bool:
+        return not self.__eq__(node)
+
+    def __lt__(self, node) -> bool:
+        assert isinstance(node, Node)
+        return self.g + self.h < node.g + node.h or \
+                (self.g + self.h == node.g + node.h and self.h < node.h)
+
+    def __hash__(self) -> int:
+        return hash(self.current)
+
+    def __str__(self) -> str:
+        return "Node({}, {}, {}, {})".format(self.current, self.parent, self.g, self.h)
+
+    def __repr__(self) -> str:
+        return self.__str__()
+    
+    @property
+    def x(self) -> float:
+        return self.current[0]
+    
+    @property
+    def y(self) -> float:
+        return self.current[1]
+
+    @property
+    def px(self) -> float:
+        if self.parent:
+            return self.parent[0]
+        else:
+            return None
+
+    @property
+    def py(self) -> float:
+        if self.parent:
+            return self.parent[1]
+        else:
+            return None

python_motion_planning/utils/environment/point2d.py

@@ -0,0 +1,96 @@
+"""
+@file: point2d.py
+@breif: 2-dimension point data stucture
+@author: Wu Maojia
+@update: 2024.3.15
+"""
+import math
+
+
+class Point2D(object):
+    """
+    Class for searching and manipulating 2-dimensional points.
+
+    Parameters:
+        x: x-coordinate of the 2d point
+        y: y-coordinate of the 2d point
+        eps: tolerance for float comparison
+
+    Examples:
+        >>> from python_motion_planning import Point2D
+        >>> p1 = Point2D(1, 2)
+        >>> p2 = Point2D(3, 4)
+        ...
+        >>> p1
+        >>> Point2D(1, 2)
+        ...
+        >>> p1 + p2
+        >>> Point2D(4, 6)
+        ...
+        >>> p1 - p2
+        >>> Point2D(-2, -2)
+        ...
+        >>> p1 == p2
+        >>> False
+        ...
+        >>> p1!= p2
+        >>> True
+        ...
+        >>> p1.dist(p2)
+        >>> 2.8284271247461903
+        ...
+        >>> p1.angle(p2)
+        >>> 0.7853981633974483
+    """
+
+    def __init__(self, x: float, y: float, eps: float = 1e-6) -> None:
+        self.x = x
+        self.y = y
+        self.eps = eps
+
+        if abs(self.x - round(self.x)) < self.eps:
+            self.x = round(self.x)
+
+        if abs(self.y - round(self.y)) < self.eps:
+            self.y = round(self.y)
+
+    def __add__(self, point):
+        assert isinstance(point, Point2D)
+        return Point2D(self.x + point.x, self.y + point.y)
+
+    def __sub__(self, point):
+        assert isinstance(point, Point2D)
+        return Point2D(self.x - point.x, self.y - point.y)
+
+    def __eq__(self, point) -> bool:
+        if not isinstance(point, Point2D):
+            return False
+        return abs(self.x - point.x) < self.eps and abs(self.y - point.y) < self.eps
+
+    def __ne__(self, point) -> bool:
+        return not self.__eq__(point)
+
+    def __hash__(self) -> int:
+        return hash((self.x, self.y))
+
+    def __str__(self) -> str:
+        return "Point2D({}, {})".format(self.x, self.y)
+
+    def __repr__(self) -> str:
+        return self.__str__()
+
+    @staticmethod
+    def from_tuple(point: tuple):
+        return Point2D(point[0], point[1])
+
+    @property
+    def to_tuple(self) -> tuple:
+        return int(self.x), int(self.y)
+
+    def dist(self, point) -> float:
+        assert isinstance(point, Point2D)
+        return math.hypot(self.x - point.x, self.y - point.y)
+
+    def angle(self, point) -> float:
+        assert isinstance(point, Point2D)
+        return math.atan2(point.y - self.y, point.x - self.x)

python_motion_planning/utils/environment/pose2d.py

@@ -0,0 +1,91 @@
+"""
+@file: pose2d.py
+@breif: 2-dimension pose data stucture
+@author: Wu Maojia
+@update: 2024.3.15
+"""
+import math
+
+
+class Pose2D(object):
+    """
+    Class for searching and manipulating 2-dimensional poses.
+
+    Parameters:
+        x: x-coordinate of the 2d pose
+        y: y-coordinate of the 2d pose
+        theta: orientation of the 2d pose in radians
+        eps: tolerance for float comparison
+
+    Examples:
+        >>> from python_motion_planning import Pose2D
+        >>> p1 = Pose2D(1, 2)
+        >>> p2 = Pose2D(3, 4, 1)
+        ...
+        >>> p1
+        >>> Pose2D(1, 2, 0)
+        ...
+        >>> p2
+        >>> Pose2D(3, 4, 1)
+        ...
+        >>> p1 + p2
+        >>> Pose2D(4, 6, 1)
+        ...
+        >>> p1 - p2
+        >>> Pose2D(-2, -2, -1)
+        ...
+        >>> p1 == p2
+        >>> False
+        ...
+        >>> p1!= p2
+        >>> True
+    """
+
+    def __init__(self, x: float, y: float, theta: float = 0, eps: float = 1e-6) -> None:
+        self.x = x
+        self.y = y
+        self.theta = theta
+        self.eps = eps
+
+        if abs(self.x - round(self.x)) < self.eps:
+            self.x = round(self.x)
+
+        if abs(self.y - round(self.y)) < self.eps:
+            self.y = round(self.y)
+
+        if abs(self.theta - round(self.theta)) < self.eps:
+            self.theta = round(self.theta)
+
+    def __add__(self, pose):
+        assert isinstance(pose, Pose2D)
+        return Pose2D(self.x + pose.x, self.y + pose.y, self.theta + pose.theta)
+
+    def __sub__(self, pose):
+        assert isinstance(pose, Pose2D)
+        return Pose2D(self.x - pose.x, self.y - pose.y, self.theta - pose.theta)
+
+    def __eq__(self, pose) -> bool:
+        if not isinstance(pose, Pose2D):
+            return False
+        return (abs(self.x - pose.x) < self.eps and abs(self.y - pose.y) < self.eps
+                and abs(self.theta - pose.theta) < self.eps)
+
+    def __ne__(self, pose) -> bool:
+        return not self.__eq__(pose)
+
+    def __hash__(self) -> int:
+        return hash((self.x, self.y, self.theta))
+
+    def __str__(self) -> str:
+        return "Pose2D({}, {}, {})".format(self.x, self.y, self.theta)
+
+    def __repr__(self) -> str:
+        return self.__str__()
+
+    @staticmethod
+    def from_tuple(pose: tuple):
+        return Pose2D(pose[0], pose[1], pose[2])
+
+    @property
+    def to_tuple(self) -> tuple:
+        return self.x, self.y, self.theta

python_motion_planning/utils/helper/__init__.py

@@ -0,0 +1,3 @@
+from .math_helper import MathHelper
+
+__all__ = ["MathHelper"]

python_motion_planning/utils/helper/math_helper.py

@@ -0,0 +1,65 @@
+"""
+@file: math_helper.py
+@breif: Contains common/commonly used math function
+@author: Yang Haodong, Wu Maojia
+@update: 2024.5.20
+"""
+import math 
+
+class MathHelper:
+    @staticmethod
+    def circleSegmentIntersection(p1: tuple, p2: tuple, r: float) -> list:
+        x1, x2 = p1[0], p2[0]
+        y1, y2 = p1[1], p2[1]
+
+        dx, dy = x2 - x1, y2 - y1
+        dr2 = dx * dx + dy * dy
+        D = x1 * y2 - x2 * y1
+
+        # the first element is the point within segment
+        d1 = x1 * x1 + y1 * y1
+        d2 = x2 * x2 + y2 * y2
+        dd = d2 - d1
+
+        delta_2 = r * r * dr2 - D * D
+        if delta_2 < 0:  # no intersection
+            return []
+
+        delta = math.sqrt(delta_2)
+        if (delta == 0):
+            return [(D * dy / dr2, -D * dx / dr2)]
+        else:   # delta > 0
+            return [
+                ((D * dy + math.copysign(1.0, dd) * dx * delta) / dr2,
+                (-D * dx + math.copysign(1.0, dd) * dy * delta) / dr2),
+                ((D * dy - math.copysign(1.0, dd) * dx * delta) / dr2,
+                (-D * dx - math.copysign(1.0, dd) * dy * delta) / dr2)
+            ]
+
+    @staticmethod
+    def closestPointOnLine(a: tuple, b: tuple, p: tuple = (0.0, 0.0)) -> tuple:
+        """
+        Find the closest intersection point (foot of a perpendicular) between point p and the line ab.
+
+        Parameters:
+            a (tuple): point a of the line
+            b (tuple): point b of the line
+            p (tuple): point p to find the closest intersection point
+
+        References:
+            [1] method 2 of https://www.youtube.com/watch?v=TPDgB6136ZE
+        """
+        ap = (p[0] - a[0], p[1] - a[1])
+        ab = (b[0] - a[0], b[1] - a[1])
+        af_coef = (ap[0] * ab[0] + ap[1] * ab[1]) / (ab[0] ** 2 + ab[1] ** 2)
+        af = (af_coef * ab[0], af_coef * ab[1])
+        f = (a[0] + af[0], a[1] + af[1])
+        return f
+    
+    @staticmethod
+    def clamp(val: float, min_val: float, max_val: float) -> float:
+        if val < min_val:
+            val = min_val
+        if val > max_val :
+            val = max_val
+        return val

python_motion_planning/utils/planner/control_factory.py

@@ -0,0 +1,31 @@
+"""
+@file: control_factory.py
+@breif: Facotry class for local planner.
+@author: Winter
+@update: 2023.10.24
+"""
+from python_motion_planning.local_planner import *
+
+class ControlFactory(object):
+    def __init__(self) -> None:
+        pass
+
+    def __call__(self, planner_name, **config):
+        if planner_name == "dwa":
+            return DWA(**config)
+        elif planner_name == "pid":
+            return PID(**config)
+        elif planner_name == "apf":
+            return APF(**config)
+        elif planner_name == "rpp":
+            return RPP(**config)
+        elif planner_name == "lqr":
+            return LQR(**config)
+        elif planner_name == "mpc":
+            return MPC(**config)
+        elif planner_name == "ddpg":
+            return DDPG(actor_load_path="models/actor_best_example.pth",
+                        critic_load_path="models/critic_best_example.pth",
+                        **config)
+        else:
+            raise ValueError("The `planner_name` must be set correctly.")

python_motion_planning/utils/planner/curve_factory.py

@@ -0,0 +1,29 @@
+"""
+@file: curve_factory.py
+@breif: Facotry class for curve generation.
+@author: Winter
+@update: 2023.7.25
+"""
+from python_motion_planning.curve_generation import *
+
+class CurveFactory(object):
+    def __init__(self) -> None:
+        pass
+
+    def __call__(self, curve_name, **config):
+        if curve_name == "dubins":
+            return Dubins(**config)
+        elif curve_name == "bezier":
+            return Bezier(**config)
+        elif curve_name == "polynomial":
+            return Polynomial(**config)
+        elif curve_name == "reeds_shepp":
+            return ReedsShepp(**config)
+        elif curve_name == "cubic_spline":
+            return CubicSpline(**config)
+        elif curve_name == "bspline":
+            return BSpline(**config)
+        elif curve_name == "fem_pos_smoother":
+            return FemPosSmoother(**config)
+        else:
+            raise ValueError("The `curve_name` must be set correctly.")

python_motion_planning/utils/planner/planner.py

@@ -0,0 +1,40 @@
+"""
+@file: planner.py
+@breif: Abstract class for planner
+@author: Winter
+@update: 2023.1.17
+"""
+import math
+from abc import abstractmethod, ABC
+from ..environment.env import Env, Node
+from ..plot.plot import Plot
+
+class Planner(ABC):
+    def __init__(self, start: tuple, goal: tuple, env: Env) -> None:
+        # plannig start and goal
+        self.start = Node(start, start, 0, 0)
+        self.goal = Node(goal, goal, 0, 0)
+        # environment
+        self.env = env
+        # graph handler
+        self.plot = Plot(start, goal, env)
+
+    def dist(self, node1: Node, node2: Node) -> float:
+        return math.hypot(node2.x - node1.x, node2.y - node1.y)
+    
+    def angle(self, node1: Node, node2: Node) -> float:
+        return math.atan2(node2.y - node1.y, node2.x - node1.x)
+
+    @abstractmethod
+    def plan(self):
+        '''
+        Interface for planning.
+        '''
+        pass
+
+    @abstractmethod
+    def run(self):
+        '''
+        Interface for running both plannig and animation.
+        '''
+        pass

python_motion_planning/utils/planner/search_factory.py

@@ -0,0 +1,51 @@
+"""
+@file: search_factory.py
+@breif: Factory class for global planner.
+@author: Winter
+@update: 2023.3.2
+"""
+from python_motion_planning.global_planner import *
+
+class SearchFactory(object):
+    def __init__(self) -> None:
+        pass
+
+    def __call__(self, planner_name, **config):
+        if planner_name == "a_star":
+            return AStar(**config)
+        elif planner_name == "dijkstra":
+            return Dijkstra(**config)
+        elif planner_name == "gbfs":
+            return GBFS(**config)
+        elif planner_name == "jps":
+            return JPS(**config)
+        elif planner_name == "d_star":
+            return DStar(**config)
+        elif planner_name == "lpa_star":
+            return LPAStar(**config)
+        elif planner_name == "d_star_lite":
+            return DStarLite(**config)
+        elif planner_name == "voronoi":
+            return VoronoiPlanner(**config)
+        elif planner_name == "theta_star":
+            return ThetaStar(**config)
+        elif planner_name == "lazy_theta_star":
+            return LazyThetaStar(**config)
+        elif planner_name == "s_theta_star":
+            return SThetaStar(**config)
+        elif planner_name == "anya":
+            return Anya(**config)
+        elif planner_name == "rrt":
+            return RRT(**config)
+        elif planner_name == "rrt_connect":
+            return RRTConnect(**config)
+        elif planner_name == "rrt_star":
+            return RRTStar(**config)
+        elif planner_name == "informed_rrt":
+            return InformedRRT(**config)
+        elif planner_name == "aco":
+            return ACO(**config)
+        elif planner_name == "pso":
+            return PSO(**config)
+        else:
+            raise ValueError("The `planner_name` must be set correctly.")