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

python_motion_planning/global_planner/sample_search/rrt.py

@@ -0,0 +1,151 @@
+"""
+@file: rrt.py
+@breif: RRT motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import math
+import numpy as np
+
+from .sample_search import SampleSearcher
+from python_motion_planning.utils import Env, Node
+
+
+class RRT(SampleSearcher):
+    """
+    Class for RRT motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        max_dist (float): Maximum expansion distance one step
+        sample_num (int): Maximum number of sample points
+        goal_sample_rate (float): heuristic sample
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.RRT((5, 5), (45, 25), pmp.Map(51, 31))
+        >>> cost, path, expand = planner.plan()     # planning results only
+        >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] Rapidly-Exploring Random Trees: A New Tool for Path Planning
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, max_dist: float = 0.5,
+        sample_num: int = 10000, goal_sample_rate: float = 0.05) -> None:
+        super().__init__(start, goal, env)
+        # Maximum expansion distance one step
+        self.max_dist = max_dist
+        # Maximum number of sample points
+        self.sample_num = sample_num
+        # heuristic sample
+        self.goal_sample_rate = goal_sample_rate
+
+    def __str__(self) -> str:
+        return "Rapidly-exploring Random Tree(RRT)"
+
+    def plan(self) -> tuple:
+        """
+        RRT motion plan function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            expand (list): expanded (sampled) nodes list
+        """
+        # Sampled list
+        sample_list = {self.start.current: self.start}
+
+        # main loop
+        for _ in range(self.sample_num):
+            # generate a random node in the map
+            node_rand = self.generateRandomNode()
+
+            # visited
+            if node_rand.current in sample_list:
+                continue
+            
+            # generate new node
+            node_new = self.getNearest(list(sample_list.values()), node_rand)
+            if node_new:
+                sample_list[node_new.current] = node_new
+                dist = self.dist(node_new, self.goal)
+                # goal found
+                if dist <= self.max_dist and not self.isCollision(node_new, self.goal):
+                    self.goal.parent = node_new.current
+                    self.goal.g = node_new.g + self.dist(self.goal, node_new)
+                    sample_list[self.goal.current] = self.goal
+                    cost, path = self.extractPath(sample_list)
+                    return cost, path, list(sample_list.values())
+
+        return 0, None, list(sample_list.values())
+
+    def run(self) -> None:
+        """
+        Running both plannig and animation.
+        """
+        cost, path, expand = self.plan()
+        self.plot.animation(path, str(self), cost, expand)
+
+    def generateRandomNode(self) -> Node:
+        """
+        Generate a random node to extend exploring tree.
+
+        Returns:
+            node (Node): a random node based on sampling
+        """
+        if np.random.random() > self.goal_sample_rate:
+            current = (np.random.uniform(self.delta, self.env.x_range - self.delta),
+                    np.random.uniform(self.delta, self.env.y_range - self.delta))
+            return Node(current, None, 0, 0)
+        return self.goal
+
+    def getNearest(self, node_list: list, node: Node) -> Node:
+        """
+        Get the node from `node_list` that is nearest to `node`.
+
+        Parameters:
+            node_list (list): exploring list
+            node (Node): currently generated node
+
+        Returns:
+            node (Node): nearest node
+        """
+        # find nearest neighbor
+        dist = [self.dist(node, nd) for nd in node_list]
+        node_near = node_list[int(np.argmin(dist))]
+
+        # regular and generate new node
+        dist, theta = self.dist(node_near, node), self.angle(node_near, node)
+        dist = min(self.max_dist, dist)
+        node_new = Node((node_near.x + dist * math.cos(theta),
+                        (node_near.y + dist * math.sin(theta))),
+                         node_near.current, node_near.g + dist, 0)
+        
+        # obstacle check
+        if self.isCollision(node_new, node_near):
+            return None
+        return node_new
+
+    def extractPath(self, closed_list: dict) -> tuple:
+        """
+        Extract the path based on the CLOSED list.
+
+        Parameters:
+            closed_list (dict): CLOSED list
+
+        Returns
+            cost (float): the cost of planning path
+            path (list): the planning path
+        """
+        node = closed_list[self.goal.current]
+        path = [node.current]
+        cost = node.g
+        while node != self.start:
+            node_parent = closed_list[node.parent]
+            node = node_parent
+            path.append(node.current)
+
+        return cost, path

python_motion_planning/global_planner/sample_search/rrt_connect.py

@@ -0,0 +1,147 @@
+"""
+@file: rrt_connected.py
+@breif: RRT-Connected motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+import math
+
+from .rrt import RRT
+from python_motion_planning.utils import Env, Node
+
+
+class RRTConnect(RRT):
+    """
+    Class for RRT-Connect motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        max_dist (float): Maximum expansion distance one step
+        sample_num (int): Maximum number of sample points
+        goal_sample_rate (float): heuristic sample
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.RRTConnect((5, 5), (45, 25), pmp.Map(51, 31))
+        >>> cost, path, expand = planner.plan()     # planning results only
+        >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] RRT-Connect: An Efficient Approach to Single-Query Path Planning
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, max_dist: float = 0.5,
+        sample_num: int = 10000, goal_sample_rate: float = 0.05) -> None:
+        super().__init__(start, goal, env, max_dist, sample_num, goal_sample_rate)
+    
+    def __str__(self) -> str:
+        return "RRT-Connect"
+
+    def plan(self) -> tuple:
+        """
+        RRT-Connected motion plan function.
+
+        Returns:
+            cost (float): path cost
+            path (list): planning path
+            expand (list): expanded (sampled) nodes list
+        """
+        # Sampled list forward
+        sample_list_f = {self.start.current: self.start}
+        # Sampled list backward
+        sample_list_b = {self.goal.current: self.goal}
+
+        for _ in range(self.sample_num):
+            # generate a random node in the map
+            node_rand = self.generateRandomNode()            
+            # generate new node
+            node_new = self.getNearest(list(sample_list_f.values()), node_rand)
+            if node_new:
+                sample_list_f[node_new.current] = node_new
+                # backward exploring
+                node_new_b = self.getNearest(list(sample_list_b.values()), node_new)
+                if node_new_b:
+                    sample_list_b[node_new_b.current] = node_new_b
+
+                    # greedy extending
+                    while True:
+                        if node_new_b == node_new:
+                            cost, path = self.extractPath(node_new, sample_list_b, sample_list_f)
+                            expand = self.getExpand(list(sample_list_b.values()), list(sample_list_f.values()))
+                            return cost, path, expand
+
+                        dist = min(self.max_dist, self.dist(node_new, node_new_b))
+                        theta = self.angle(node_new_b, node_new)
+                        node_new_b2 = Node((node_new_b.x + dist * math.cos(theta),
+                                           (node_new_b.y + dist * math.sin(theta))),
+                                            node_new_b.current, node_new_b.g + dist, 0)
+
+                        if not self.isCollision(node_new_b2, node_new_b):
+                            sample_list_b[node_new_b2.current] = node_new_b2
+                            node_new_b = node_new_b2
+                        else:
+                            break
+
+            if len(sample_list_b) < len(sample_list_f):
+                sample_list_f, sample_list_b = sample_list_b, sample_list_f
+
+        return 0, None, None
+
+    def extractPath(self, boundary: Node, sample_list_b: dict, sample_list_f: dict) -> tuple:
+        """
+        Extract the path based on the CLOSED set.
+
+        Parameters:
+            boundary (Node): the boundary node
+            sample_list_b (dict): Sample list backward
+            sample_list_f (dict): Sample list forward
+
+        Returns:
+            cost (float): the cost of planning path
+            path (list): the planning path
+        """
+        if self.start.current in sample_list_b:
+            sample_list_f, sample_list_b = sample_list_b, sample_list_f
+
+        # forward
+        node = sample_list_f[boundary.current]
+        path_f = [node.current]
+        cost = node.g
+        while node != self.start:
+            node_parent = sample_list_f[node.parent]
+            node = node_parent
+            path_f.append(node.current)
+
+        # backward
+        node = sample_list_b[boundary.current]
+        path_b = []
+        cost += node.g
+        while node != self.goal:
+            node_parent = sample_list_b[node.parent]
+            node = node_parent
+            path_b.append(node.current)        
+
+        return cost, list(reversed(path_f)) + path_b
+
+    def getExpand(self, sample_list_b: list, sample_list_f: list) -> list:
+        """
+        Get the expand list from sample list.
+
+        Parameters:
+            sample_list_b (list): Sample list backward
+            sample_list_f (list): Sample list forward
+
+        Returns:
+            expand (list): expand list
+        """
+        expand = []
+        tree_size = max(len(sample_list_f), len(sample_list_b))
+        for k in range(tree_size):
+            if k < len(sample_list_f):
+                expand.append(sample_list_f[k])
+            if k < len(sample_list_b):
+                expand.append(sample_list_b[k])
+
+        return expand

python_motion_planning/global_planner/sample_search/rrt_star.py

@@ -0,0 +1,77 @@
+"""
+@file: rrt_star.py
+@breif: RRT-Star motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.23
+"""
+from .rrt import RRT
+from python_motion_planning.utils import Env, Node
+
+
+class RRTStar(RRT):
+    """
+    Class for RRT-Star motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        max_dist (float): Maximum expansion distance one step
+        sample_num (int): Maximum number of sample points
+        r (float): optimization radius
+        goal_sample_rate (float): heuristic sample
+
+    Examples:
+        >>> import python_motion_planning as pmp
+        >>> planner = pmp.RRTStar((5, 5), (45, 25), pmp.Map(51, 31))
+        >>> cost, path, expand = planner.plan()     # planning results only
+        >>> planner.plot.animation(path, str(planner), cost, expand)  # animation
+        >>> planner.run()       # run both planning and animation
+
+    References:
+        [1] Sampling-based algorithms for optimal motion planning
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, max_dist: float = 0.5,
+                 sample_num: int = 10000, r: float = 10.0, goal_sample_rate: float = 0.05) -> None:
+        super().__init__(start, goal, env, max_dist, sample_num, goal_sample_rate)
+        # optimization radius
+        self.r = r
+    
+    def __str__(self) -> str:
+        return "RRT*"
+    
+    def getNearest(self, node_list: list, node: Node) -> Node:
+        """
+        Get the node from `node_list` that is nearest to `node` with optimization.
+
+        Parameters:
+            node_list (list): exploring list
+            node (Node): currently generated node
+
+        Returns:
+            node (Node): nearest node
+        """
+        node_new = super().getNearest(node_list, node)
+        if node_new:
+            #  rewire optimization
+            for node_n in node_list:
+                #  inside the optimization circle
+                new_dist = self.dist(node_n, node_new)
+                if new_dist < self.r:
+                    cost = node_n.g + new_dist
+                    #  update new sample node's cost and parent
+                    if node_new.g > cost and not self.isCollision(node_n, node_new):
+                        node_new.parent = node_n.current
+                        node_new.g = cost
+                    else:
+                        #  update nodes' cost inside the radius
+                        cost = node_new.g + new_dist
+                        if node_n.g > cost and not self.isCollision(node_n, node_new):
+                            node_n.parent = node_new.current
+                            node_n.g = cost
+                else:
+                    continue
+            return node_new
+        else:
+            return None 
+        

python_motion_planning/global_planner/sample_search/sample_search.py

@@ -0,0 +1,135 @@
+"""
+@file: graph_search.py
+@breif: Base class for planner based on graph searching
+@author: Winter
+@update: 2023.1.17
+"""
+import numpy as np
+from itertools import combinations
+import math
+
+from python_motion_planning.utils import Env, Node, Planner
+
+class SampleSearcher(Planner):
+    """
+    Base class for planner based on sample searching.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, delta: float=0.5) -> None:
+        super().__init__(start, goal, env)
+        # inflation bias
+        self.delta = delta
+
+    def isCollision(self, node1: Node, node2: Node) -> bool:
+        """
+        Judge collision when moving from node1 to node2.
+
+        Parameters:
+            node1 (Node): node 1
+            node2 (Node): node 2
+
+        Returns:
+            collision (bool): True if collision exists else False
+        """
+        if self.isInsideObs(node1) or self.isInsideObs(node2):
+            return True
+
+        for rect in self.env.obs_rect:
+            if self.isInterRect(node1, node2, rect):
+                return True
+
+        for circle in self.env.obs_circ:
+            if self.isInterCircle(node1, node2, circle):
+                return True
+
+        return False
+
+    def isInsideObs(self, node: Node) -> bool:
+        """
+        Judge whether a node inside tht obstacles or not.
+
+        Parameters:
+            node (Node): node
+
+        Returns:
+            inside (bool): True if inside the obstacles else False
+        """
+        x, y = node.current
+
+        for (ox, oy, r) in self.env.obs_circ:
+            if math.hypot(x - ox, y - oy) <= r + self.delta:
+                return True
+
+        for (ox, oy, w, h) in self.env.obs_rect:
+            if 0 <= x - (ox - self.delta) <= w + 2 * self.delta \
+                and 0 <= y - (oy - self.delta) <= h + 2 * self.delta:
+                return True
+
+        for (ox, oy, w, h) in self.env.boundary:
+            if 0 <= x - (ox - self.delta) <= w + 2 * self.delta \
+                and 0 <= y - (oy - self.delta) <= h + 2 * self.delta:
+                return True
+
+        return False
+
+    def isInterRect(self, node1: Node, node2: Node, rect: list) -> bool:
+        # obstacle and it's vertex
+        ox, oy, w, h = rect
+        vertex = [[ox - self.delta, oy - self.delta],
+                  [ox + w + self.delta, oy - self.delta],
+                  [ox + w + self.delta, oy + h + self.delta],
+                  [ox - self.delta, oy + h + self.delta]]
+        
+        # node
+        x1, y1 = node1.current
+        x2, y2 = node2.current
+
+        def cross(p1, p2, p3):
+            x1 = p2[0] - p1[0]
+            y1 = p2[1] - p1[1]
+            x2 = p3[0] - p1[0]
+            y2 = p3[1] - p1[1]
+            return x1 * y2 - x2 * y1
+
+        for v1, v2 in combinations(vertex, 2):
+            # rapid repulsion
+            if  max(x1, x2) >= min(v1[0], v2[0]) and \
+                min(x1, x2) <= max(v1[0], v2[0]) and \
+                max(y1, y2) >= min(v1[1], v2[1]) and \
+                min(y1, y2) <= max(v1[1], v2[1]): 
+                # cross
+                if cross(v1, v2, node1.current) * cross(v1, v2, node2.current) <= 0 and \
+                   cross(node1.current, node2.current, v1) * cross(node1.current, node2.current, v2) <= 0:
+                    return True
+
+        return False
+
+    def isInterCircle(self, node1: Node, node2: Node, circle: list) -> bool:
+        # obstacle
+        ox, oy, r = circle
+
+        # origin
+        x, y = node1.current
+
+        # direction
+        dx = node2.x - node1.x
+        dy = node2.y - node1.y
+        d  = [dx, dy]
+        d2 = np.dot(d, d)
+
+        if d2 == 0:
+            return False
+
+        # projection
+        t = np.dot([ox - x, oy - y], d) / d2
+        if 0 <= t <= 1:
+            shot = Node((x + t * dx, y + t * dy), None, None, None)
+            center = Node((ox, oy), None, None, None)
+            if self.dist(shot, center) <= r + self.delta:
+                return True
+
+        return False

python_motion_planning/local_planner/__init__.py

@@ -0,0 +1,19 @@
+from .dwa import DWA
+from .pid import PID
+from .apf import APF
+from .rpp import RPP
+from .lqr import LQR
+from .mpc import MPC
+from .ddpg import DDPG
+from .dqn import DQNPlanner
+
+__all__ = [
+    "DWA",
+    "PID",
+    "APF",
+    "RPP",
+    "LQR",
+    "MPC",
+    "DDPG",
+    "DQNPlanner"
+]

python_motion_planning/local_planner/apf.py

@@ -0,0 +1,144 @@
+"""
+@file: apf.py
+@breif: Artificial Potential Field(APF) motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.5.21
+"""
+import math
+import numpy as np
+from scipy.spatial.distance import cdist
+
+from .local_planner import LocalPlanner
+from python_motion_planning.utils import Env
+
+class APF(LocalPlanner):
+    """
+    Class for Artificial Potential Field(APF) motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        heuristic_type (str): heuristic function type
+        **params: other parameters can be found in the parent class LocalPlanner
+
+    Examples:
+        >>> from python_motion_planning.utils import Grid
+        >>> from python_motion_planning.local_planner import APF
+        >>> start = (5, 5, 0)
+        >>> goal = (45, 25, 0)
+        >>> env = Grid(51, 31)
+        >>> planner = APF(start, goal, env)
+        >>> planner.run()
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean", **params) -> None:
+        super().__init__(start, goal, env, heuristic_type, **params)
+        # APF parameters
+        self.zeta = 1.0
+        self.eta = 1.0
+        self.d_0 = 1.0
+
+        # global planner
+        g_start = (start[0], start[1])
+        g_goal  = (goal[0], goal[1])
+        self.g_planner = {"planner_name": "a_star", "start": g_start, "goal": g_goal, "env": env}
+        self.path = self.g_path[::-1]
+
+    def __str__(self) -> str:
+        return "Artificial Potential Field(APF)"
+    
+    def plan(self):
+        """
+        APF motion plan function.
+
+        Returns:
+            flag (bool): planning successful if true else failed
+            pose_list (list): history poses of robot
+        """
+        dt = self.params["TIME_STEP"]
+        for _ in range(self.params["MAX_ITERATION"]):
+            # break until goal reached
+            if self.reachGoal(tuple(self.robot.state.squeeze(axis=1)[0:3]), self.goal):
+                return True, self.robot.history_pose
+            
+            # compute the tatget pose and force at the current step
+            lookahead_pt, _, _ = self.getLookaheadPoint()
+            rep_force = self.getRepulsiveForce()
+            attr_force = self.getAttractiveForce(np.array(self.robot.position), np.array(lookahead_pt))
+            net_force = self.zeta * attr_force + self.eta * rep_force
+            
+            # compute desired velocity
+            v, theta = self.robot.v, self.robot.theta
+            new_v = np.array([v * math.cos(theta), v * math.sin(theta)]) + net_force
+            new_v /= np.linalg.norm(new_v)
+            new_v *= self.params["MAX_V"]
+            theta_d = math.atan2(new_v[1], new_v[0])
+
+            # calculate velocity command
+            e_theta = self.regularizeAngle(self.robot.theta - self.goal[2])
+            if not self.shouldMoveToGoal(self.robot.position, self.goal):
+                if not self.shouldRotateToPath(abs(e_theta)):
+                    u = np.array([[0], [0]])
+                else:
+                    u = np.array([[0], [self.angularRegularization(e_theta / dt)]])
+            else:
+                e_theta = self.regularizeAngle(theta_d - self.robot.theta)
+                if self.shouldRotateToPath(abs(e_theta)):
+                    u = np.array([[0], [self.angularRegularization(e_theta / dt)]])
+                else:
+                    v_d = np.linalg.norm(new_v)
+                    u = np.array([[self.linearRegularization(v_d)], [self.angularRegularization(e_theta / dt)]])
+            
+            # feed into robotic kinematic
+            self.robot.kinematic(u, dt)
+        
+        return False, None
+    
+    def run(self):
+        """
+        Running both plannig and animation.
+        """
+        _, history_pose = self.plan()
+        if not history_pose:
+            raise ValueError("Path not found and planning failed!")
+
+        path = np.array(history_pose)[:, 0:2]
+        cost = np.sum(np.sqrt(np.sum(np.diff(path, axis=0)**2, axis=1, keepdims=True)))
+        self.plot.plotPath(self.path, path_color="r", path_style="--")
+        self.plot.animation(path, str(self), cost, history_pose=history_pose)
+
+    def getRepulsiveForce(self) -> np.ndarray:
+        """
+        Get the repulsive force of APF.
+
+        Returns:
+            rep_force (np.ndarray): repulsive force of APF
+        """
+        obstacles = np.array(list(self.obstacles))
+        cur_pos = np.array([[self.robot.px, self.robot.py]])
+        D = cdist(obstacles, cur_pos)
+        rep_force = (1 / D - 1 / self.d_0) * (1 / D) ** 2 * (cur_pos - obstacles)
+        valid_mask = np.argwhere((1 / D - 1 / self.d_0) > 0)[:, 0]
+        rep_force = np.sum(rep_force[valid_mask, :], axis=0)
+
+        if not np.all(rep_force == 0):
+            rep_force = rep_force / np.linalg.norm(rep_force)
+        
+        return rep_force
+    
+    def getAttractiveForce(self, cur_pos: np.ndarray, tgt_pos: np.ndarray) -> np.ndarray:
+        """
+        Get the attractive force of APF.
+
+        Parameters:
+            cur_pos (np.ndarray): current position of robot
+            tgt_pos (np.ndarray): target position of robot
+
+        Returns
+            attr_force (np.ndarray): attractive force
+        """
+        attr_force = tgt_pos - cur_pos
+        if not np.all(attr_force == 0):
+            attr_force = attr_force / np.linalg.norm(attr_force)
+        
+        return attr_force