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

python_motion_planning-1.0.dist-info/RECORD

@@ -0,0 +1,65 @@
+curve_generation/__init__.py,sha256=6eeN0NBlAndBhjk-ol3Yaa-0om2lNG7n43BASRfeV9k,370
+curve_generation/bezier_curve.py,sha256=QVMsXceZ7KNJjFJL0zwagqRujO4Zx-FtwdinDzdyaVo,3637
+curve_generation/bspline_curve.py,sha256=2yHMYUbqg2aUQ3GXMN2lO44OLOfsqPH2Rk5sZT7mkCc,9332
+curve_generation/cubic_spline.py,sha256=gW4vkGCmtJzpC7aRhGcYPdJLAAi-UKCOFqmqnqBno4Q,3378
+curve_generation/curve.py,sha256=xf7G5lDGTCVUBoSK8Oa14fmdf_AANOZVqdSmAKlwu-A,1543
+curve_generation/dubins_curve.py,sha256=3U6jYRzuc9MrE9fjOCoLE3hSRvd65dZkksrgYH5rglI,10987
+curve_generation/fem_pos_smooth.py,sha256=WAHZym4_E2Mp0Uk4w8olQ6c_KigJo79nqAs-KscXyJ0,3388
+curve_generation/polynomial_curve.py,sha256=9eM55opnFuwCPY6ErJY3knH-MMhvT6g7PwhSiAGMP78,7504
+curve_generation/reeds_shepp.py,sha256=hWJDbBhRuHgtcHzGUJefqv_IuO7W8irrBMxCQOGeFkM,21427
+global_planner/__init__.py,sha256=cx2kwds-up49JOu1LGjJkviEOAlG1-mV3A7QvNDfRoI,93
+global_planner/evolutionary_search/__init__.py,sha256=2MIP1t74TmKwRVGbSxDSy38rMObDjjpqNn-tHB8fCEc,70
+global_planner/evolutionary_search/aco.py,sha256=fPvEEOLLo-W7k6jvgZxUtN4V59vnaHLi4_hqh9odRF4,6997
+global_planner/evolutionary_search/evolutionary_search.py,sha256=WbftOQQ--XivulEFYCT_CFX2FylIvSVxysxbqQkxnl4,2697
+global_planner/evolutionary_search/pso.py,sha256=i_QqU6_NUgiXI4rPZrtAskpjrE35IZdi0kyVJU1IQ-w,13673
+global_planner/graph_search/__init__.py,sha256=5wAp5GsaPfyWmfAPiXtdjaWK1VGQp1nBUZXn7MCCXdQ,745
+global_planner/graph_search/a_star.py,sha256=81g0syj09DrpmQcbVX3rKlinXMDzQB7_0Vq_s8s4UxY,3821
+global_planner/graph_search/d_star.py,sha256=amBzAXM895FqpeVA-J3PbYgKusZM0ujquzjLzPkHwzA,10445
+global_planner/graph_search/d_star_lite.py,sha256=iI8hA5GbrTtpIqr9pY6SVB55QyxPHe4F3vYcTJe2RyE,6770
+global_planner/graph_search/dijkstra.py,sha256=ssQg5eIyFBVVXH_MMAblCEJWvh6egEfGln_BMShDK7E,2451
+global_planner/graph_search/gbfs.py,sha256=2x43OjShaFv7nox1AxzARIWsxS0lv5zWWZy0uikxsLU,2531
+global_planner/graph_search/graph_search.py,sha256=QmIBScvOfYI_Ud4qQ3lh3Di3Tw-ZuiUV343w7zj6X_c,2675
+global_planner/graph_search/jps.py,sha256=fxYBClawoj-sk9dNsqKOwNA7X6jsRqEdi0Nvr_qD0uo,5291
+global_planner/graph_search/lazy_theta_star.py,sha256=S-1-eUAzhRsjhms1sAACFmh7EtsFcNGnl5T8f4Kwip8,3884
+global_planner/graph_search/lpa_star.py,sha256=TpD4jqQvdn9cFWphhpyIibQmqeAI8gDrtHw3T2Unc6E,7474
+global_planner/graph_search/s_theta_star.py,sha256=Q9mUgVwfIsd3vC0x1xQVtSabcjaUFunICvBRElJmDWs,4372
+global_planner/graph_search/theta_star.py,sha256=_kTcY1fTiKO5TxgNcBrsD9Z6Xh3e_OgyTuZMVWUJ0jg,5410
+global_planner/graph_search/voronoi.py,sha256=KcdnLz9RrqlB-MLkDxQBhZn2u3y6Jz3583yNVAj71Fo,6685
+global_planner/sample_search/__init__.py,sha256=0foVXexrJxGDiECtc8BWGADzjsgGS_BfvMfecMHFxAI,188
+global_planner/sample_search/informed_rrt.py,sha256=pTKXfovP_0H1hVcWpFp-p2upyOAVnKYNKYwbVJ2pei4,5588
+global_planner/sample_search/rrt.py,sha256=YXrHvR8NdlmrSoVFhohNZEDp0rBgidX6o2Q4RKZv93o,5280
+global_planner/sample_search/rrt_connect.py,sha256=qvBPNe-gaA0Xi_S9SraAvq0DzTdDzDcUCZDMRUBK514,5553
+global_planner/sample_search/rrt_star.py,sha256=0VLXlGuhndIxZT_FZMS-11avlJB4lDsjp5PbQgGVPvs,2892
+global_planner/sample_search/sample_search.py,sha256=rgeEfLe8kkbXx5pSa80BTG_nOb0l7VNrG3L2H718wnM,4243
+local_planner/__init__.py,sha256=ojbK5WMquubPDMADURk78qRIP4vGI_yaE7MX86gM-0A,304
+local_planner/apf.py,sha256=dc2h8PCyihlwdmyICQ22uKhbOvYA9t2BGqWuZK7o9Ss,5586
+local_planner/ddpg.py,sha256=R7jcve4bokTZzyl2qYbvtb3NWxRAbCzkgZ-22scpHvo,29726
+local_planner/dqn.py,sha256=Y2HH0GaKR1mG8JwLfDBRa_3oCttzmfid13osad5xl_0,28026
+local_planner/dwa.py,sha256=HWkVVDFmjWv_n4uT3vdQpe9_SJdF07lt-q6SWGFyr3E,8120
+local_planner/local_planner.py,sha256=AmaVt3STFIq4kQi6f-eEn_fwAdfno13vPdAYlRubZC8,10543
+local_planner/lqr.py,sha256=jxlTFEFIFlf5k9rdXWVhwiVKWSy4_EBSZAH1H9AWPes,5444
+local_planner/mpc.py,sha256=GZa7jtLr5Dhds1vmxn5TdxZVSO5KyO8yz1md3IaA5kQ,8209
+local_planner/pid.py,sha256=kfvXCGkNFcPnqehbIdByHTaa7ZN7DUDfefrScFTeCL8,5939
+local_planner/rpp.py,sha256=GLfjGoqjy0L4uTRVk7ZywM8xtBmfw7ZVuB5JA4kweUg,5809
+utils/__init__.py,sha256=ebv1k_btXnz6YbTqaokEma0Xr3J5WKkiCw3V1ESEQ4I,632
+utils/agent/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+utils/agent/agent.py,sha256=vv_cP6ZJ2sZSpot5txi5YdQmVQRRUURVyx2CX2uyn9s,3978
+utils/environment/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+utils/environment/env.py,sha256=_XVn6z6x46b-EEUHPrgY3UHPkN25fgv54TWU3ZXDkNg,3838
+utils/environment/node.py,sha256=1kLobhv7wxuG2KatSNcuaIRDKcokox9FG8dpCbjvH9s,2279
+utils/environment/point2d.py,sha256=fKwF3GctvVWsX_QcLX9W7LRma4G1QoNWnseAoqsleBM,2517
+utils/environment/pose2d.py,sha256=zDuwm4EDMO0-LAfnLp3RvvNl46TmP2Ap46ZgolwItNQ,2478
+utils/helper/__init__.py,sha256=1n8T8SlUeRKDWiZf3nsdEDA4h99xXyegvPLV08c_D2c,63
+utils/helper/math_helper.py,sha256=tFfeyKJYsD4e7LPtyie4nrTVv3QBLipDcSUHejNIl48,2160
+utils/planner/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+utils/planner/control_factory.py,sha256=eLiwsRxtRFi3JCPlMunRKZO1gIPN-Wm5X6Zn5AXby18,1035
+utils/planner/curve_factory.py,sha256=U-rm_sRMpxgLkXko6-VyzhXlOVG8UP86-05Mugf_lOM,956
+utils/planner/planner.py,sha256=4ysHWj8Ho8O8v9KqCHCBS5Nb_5vzm-hbiIvkHCtmi4s,1065
+utils/planner/search_factory.py,sha256=cD1znMcR8wMbdM1D23TpiplfZfQTS_Z01QnKYTMpO6E,1816
+utils/plot/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+utils/plot/plot.py,sha256=BMHGFL2n8P0bgxCrWiVEZly_DPXx-NMKZQh2_BNzEFc,10207
+python_motion_planning-1.0.dist-info/LICENSE,sha256=a4N8el8H0UrdAJzCeVgj9HklpT3VkZfsy4aL1SAifJE,35793
+python_motion_planning-1.0.dist-info/METADATA,sha256=oke5gV_-Rzuzl3ZkC7-s2eeE8RAM7KmSYUCMUlp8npc,57991
+python_motion_planning-1.0.dist-info/WHEEL,sha256=jB7zZ3N9hIM9adW7qlTAyycLYW9npaWKLRzaoVcLKcM,91
+python_motion_planning-1.0.dist-info/top_level.txt,sha256=FhASGG2D6Eo2pNwFLbE9t3mTAVX5Vd6GreG_uV4bN28,52
+python_motion_planning-1.0.dist-info/RECORD,,

python_motion_planning-1.0.dist-info/WHEEL

@@ -0,0 +1,5 @@
+Wheel-Version: 1.0
+Generator: setuptools (75.8.2)
+Root-Is-Purelib: true
+Tag: py3-none-any
+

python_motion_planning-1.0.dist-info/top_level.txt

@@ -0,0 +1,4 @@
+curve_generation
+global_planner
+local_planner
+utils

utils/__init__.py

@@ -0,0 +1,19 @@
+from .helper import MathHelper
+from .agent.agent import Robot
+from .environment.env import Env, Grid, Map
+from .environment.node import Node
+from .environment.point2d import Point2D
+from .environment.pose2d import Pose2D
+from .plot.plot import Plot
+from .planner.planner import Planner
+from .planner.search_factory import SearchFactory
+from .planner.curve_factory import CurveFactory
+from .planner.control_factory import ControlFactory
+
+__all__ = [
+    "MathHelper",
+    "Env", "Grid", "Map", "Node", "Point2D", "Pose2D",
+    "Plot", 
+    "Planner", "SearchFactory", "CurveFactory", "ControlFactory",
+    "Robot"
+]

utils/agent/agent.py

@@ -0,0 +1,135 @@
+"""
+@file: agent.py
+@breif: Class for agent
+@author: Yang Haodong, Wu Maojia
+@update: 2024.3.29
+"""
+import math
+import numpy as np
+from abc import abstractmethod, ABC
+
+class Agent(ABC):
+    """
+    Abstract class for agent.
+
+    Parameters:
+        px (float): initial x-position
+        py (float): initial y-position
+        theta (float): initial pose angle
+    """
+    def __init__(self, px, py, theta) -> None:
+        self.px = px
+        self.py = py
+        self.theta = theta
+        self.parameters = None
+
+    def setParameters(self, **parameters) -> None:
+        # other customer parameters
+        self.parameters = parameters
+        for param, val in parameters.items():
+            setattr(self, param, val)
+
+    @property
+    def position(self):
+        return (self.px, self.py)
+
+    @abstractmethod
+    def kinematic(self, u, dt):
+        pass
+
+    @property
+    @abstractmethod
+    def state(self):
+        pass
+
+
+class Robot(Agent):
+    """
+    Class for robot.
+
+    Parameters:
+        px (float): initial x-position
+        py (float): initial y-position
+        theta (float): initial pose angle
+        v (float): linear velocity
+        w (float): angular velocity
+    """
+    def __init__(self, px, py, theta, v, w) -> None:
+        super().__init__(px, py, theta)
+        # velocity
+        self.v = v
+        self.w = w
+        # history
+        self.history_pose = []
+    
+    def __str__(self) -> str:
+        return "Robot"
+    
+    def kinematic(self, u: np.ndarray, dt: float, replace: bool = True):
+        """
+        Run robot kinematic once.
+
+        Parameters:
+            u (np.ndarray): control command with [v, w]
+            dt (float): simulation time
+            replace (bool): update-self if true else return a new Robot object
+
+        Returns:
+            robot (Robot): a new robot object
+        """
+        new_state = self.lookforward(self.state, u, dt).squeeze().tolist()
+        if replace:
+            self.history_pose.append((self.px, self.py, self.theta))
+            self.px, self.py, self.theta = new_state[0], new_state[1], new_state[2]
+            self.v, self.w = new_state[3], new_state[4]
+        else:
+            new_robot = Robot(new_state[0], new_state[1], new_state[2], 
+                new_state[3], new_state[4])
+            new_robot.setParameters(self.parameters)
+            return new_robot
+    
+    def lookforward(self, state: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
+        """
+        Run robot kinematic once but do not update.
+
+        Parameters:
+            state (np.ndarray): robot state with [x, y, theta, v, w]
+            u (np.ndarray): control command with [v, w]
+            dt (float): simulation time
+            obstacles (set): set of obstacles with (x, y)
+
+        Returns:
+            new_state (np.ndarray (5x1)): new robot state with [x, y, theta, v, w]
+        """
+        F = np.array([[1, 0, 0, 0, 0],
+                      [0, 1, 0, 0, 0],
+                      [0, 0, 1, 0, 0],
+                      [0, 0, 0, 0, 0],
+                      [0, 0, 0, 0, 0]])
+        B = np.array([[dt * math.cos(state[2]),  0],
+                      [dt * math.sin(state[2]),  0],
+                      [                      0, dt],
+                      [                      1,  0],
+                      [                      0,  1]])
+        new_state = F @ state + B @ u
+
+        return new_state
+
+    def reset(self) -> None:
+        """
+        Reset the state.
+        """
+        self.v = 0
+        self.w = 0
+        self.history_pose = []
+
+    @property
+    def state(self) -> None:
+        """
+        Get the state.
+
+        Returns:
+            state (np.ndarray (5x1)): robot state with [x, y, theta, v, w]
+        """
+        state = np.array([[self.px], [self.py], [self.theta], [self.v], [self.w]]) 
+        return state

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

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

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)

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

utils/helper/__init__.py

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