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

local_planner/dwa.py

@@ -0,0 +1,212 @@
+"""
+@file: dwa.py
+@breif: Dynamic Window Approach(DWA) motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.25
+"""
+import numpy as np
+from itertools import product
+from scipy.spatial.distance import cdist
+
+from .local_planner import LocalPlanner
+from python_motion_planning.utils import Env
+
+
+class DWA(LocalPlanner):
+    """
+    Class for Dynamic Window Approach(DWA) motion planning.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        heuristic_type (str): heuristic function type
+        heading_weight (float): weight for heading cost
+        obstacle_weight (float): weight for obstacle cost
+        velocity_weight (float): weight for velocity cost
+        predict_time (float): predict time for trajectory
+        obstacle_inflation_radius (float): inflation radius for obstacles
+        v_resolution (float): velocity resolution in evaulation
+        w_resolution (float): angular velocity resolution in evaulation
+        **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 DWA
+        >>> start = (5, 5, 0)
+        >>> goal = (45, 25, 0)
+        >>> env = Grid(51, 31)
+        >>> planner = DWA(start, goal, env)
+        >>> planner.run()
+
+    References:
+        [1] The Dynamic Window Approach to Collision Avoidance.
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str = "euclidean",
+                 heading_weight: float = 0.2, obstacle_weight: float = 0.1, velocity_weight: float = 0.05,
+                 predict_time: float = 1.5, obstacle_inflation_radius: float = 1.0,
+                 v_resolution: float = 0.05, w_resolution: float = 0.05, **params) -> None:
+        super().__init__(start, goal, env, heuristic_type, **params)
+        self.heading_weight = heading_weight
+        self.obstacle_weight = obstacle_weight
+        self.velocity_weight = velocity_weight
+        self.predict_time = predict_time
+        self.obstacle_inflation_radius = obstacle_inflation_radius
+        self.v_resolution = v_resolution
+        self.w_resolution = w_resolution
+
+        # 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 "Dynamic Window Approach(DWA)"
+
+    def plan(self) -> tuple:
+        """
+        Dynamic Window Approach(DWA) motion plan function.
+        """
+        history_traj = []
+        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, history_traj, self.robot.history_pose
+
+            # get the particular point on the path at the lookahead distance to track
+            lookahead_pt, theta_trj, kappa = self.getLookaheadPoint()
+
+            # dynamic configure
+            vr = self.calDynamicWin()
+            eval_win, traj_win = self.evaluation(vr, lookahead_pt)
+
+            # failed
+            if not len(eval_win):
+                break
+
+            # update
+            max_index = np.argmax(eval_win[:, -1])
+            u = np.expand_dims(eval_win[max_index, 0:-1], axis=1)
+
+            self.robot.kinematic(u, self.params["TIME_STEP"])
+            history_traj.append(traj_win[max_index])
+
+        return False, None, None
+
+    def run(self) -> None:
+        """
+        Running both plannig and animation.
+        """
+        _, history_traj, 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, predict_path=history_traj)
+
+    def calDynamicWin(self) -> list:
+        """
+        Calculate dynamic window.
+
+        Returns:
+            v_reference (list): reference velocity
+        """
+        # hard margin
+        vs = (self.params["MIN_V"], self.params["MAX_V"], self.params["MIN_W"], self.params["MAX_W"])
+        # predict margin
+        vd = (
+            self.robot.v + self.params["MIN_V_INC"] * self.params["TIME_STEP"],
+            self.robot.v + self.params["MAX_V_INC"] * self.params["TIME_STEP"],
+            self.robot.w +self.params["MIN_W_INC"] * self.params["TIME_STEP"],
+            self.robot.w + self.params["MAX_W_INC"] * self.params["TIME_STEP"]
+        )
+
+        # dynamic window
+        v_tmp = np.array([vs, vd])
+        # reference velocity
+        vr = [
+            float(np.max(v_tmp[:, 0])), float(np.min(v_tmp[:, 1])), 
+            float(np.max(v_tmp[:, 2])), float(np.min(v_tmp[:, 3]))
+        ]
+        return vr
+
+    def evaluation(self, vr: list, goal: tuple) -> tuple:
+        """
+        Extract the path based on the CLOSED set.
+
+        Parameters:
+            closed_set (list): CLOSED set
+            goal (tuple): goal point coordinate
+
+        Returns:
+            cost (float): the cost of planning path
+            path (list): the planning path
+        """
+        v_start, v_end, w_start, w_end = vr
+        v = np.linspace(v_start, v_end, num=int((v_end - v_start) / self.v_resolution)).tolist()
+        w = np.linspace(w_start, w_end, num=int((w_end - w_start) / self.w_resolution)).tolist()
+        
+        eval_win, traj_win = [], []
+        for v_, w_ in product(v, w):
+            # trajectory prediction, consistent of poses
+            traj = self.generateTraj(v_, w_)
+            end_state = traj[-1].squeeze().tolist()
+            
+            # heading evaluation
+            theta = self.angle((end_state[0], end_state[1]), goal)
+            heading = np.pi - abs(theta - end_state[2])
+
+            # obstacle evaluation
+            D = cdist(np.array(tuple(self.obstacles)), traj[:, 0:2])
+            min_D = np.min(D)
+            obstacle = min(min_D, self.obstacle_inflation_radius)
+
+            # velocity evaluation
+            velocity = abs(v_)
+            
+            eval_win.append((v_, w_, heading, obstacle, velocity))
+            traj_win.append(traj)
+    
+        # normalization
+        eval_win = np.array(eval_win)
+        if np.sum(eval_win[:, 2]) != 0:
+            eval_win[:, 2] = eval_win[:, 2] / np.sum(eval_win[:, 2])
+        if np.sum(eval_win[:, 3]) != 0:
+            eval_win[:, 3] = eval_win[:, 3] / np.sum(eval_win[:, 3])
+        if np.sum(eval_win[:, 4]) != 0:
+            eval_win[:, 4] = eval_win[:, 4] / np.sum(eval_win[:, 4])
+        
+        # evaluation
+        factor = np.array([[1, 0,                    0],
+                           [0, 1,                    0],
+                           [0, 0, self.heading_weight ],
+                           [0, 0, self.obstacle_weight],
+                           [0, 0, self.velocity_weight]])
+
+        return eval_win @ factor, traj_win
+
+    def generateTraj(self, v: float, w: float) -> np.ndarray:
+        """
+        Generate predict trajectory.
+
+        Parameters:
+            v (float): velocity
+            w (float): angular velocity
+
+        Returns:
+            v_reference (np.ndarray): reference velocity
+        """
+        u = np.array([[v], [w]])
+        state = self.robot.state
+        time_steps = int(self.predict_time / self.params["TIME_STEP"])
+        
+        traj = []
+        for _ in range(time_steps):
+            state = self.robot.lookforward(state, u, self.params["TIME_STEP"])
+            traj.append(state)
+        
+        return np.array(traj).squeeze()

local_planner/local_planner.py

@@ -0,0 +1,262 @@
+"""
+@file: local_planner.py
+@breif: Base class for local planner.
+@author: Yang Haodong, Wu Maojia
+@update: 2024.5.20
+"""
+import math
+
+from python_motion_planning.utils import Env, Planner, SearchFactory, Plot, Robot, MathHelper
+
+
+class LocalPlanner(Planner):
+    """
+    Base class for local planner.
+
+    Parameters:
+        start (tuple): start point coordinate
+        goal (tuple): goal point coordinate
+        env (Env): environment
+        heuristic_type (str): heuristic function type
+        **params: other parameters
+    """
+    def __init__(self, start: tuple, goal: tuple, env: Env, heuristic_type: str="euclidean", **params) -> None:
+        # start and goal pose
+        assert len(start) == 3 and len(goal) == 3, \
+            "Start and goal parameters must be (x, y, theta)"
+        self.start, self.goal = start, goal
+        # heuristic type
+        self.heuristic_type = heuristic_type
+        # environment
+        self.env = env
+        # obstacles
+        self.obstacles = self.env.obstacles
+        # graph handler
+        self.plot = Plot(start, goal, env)
+        # robot
+        self.robot = Robot(start[0], start[1], start[2], 0, 0)
+
+        # parameters and default value
+        self.params = {}
+        self.params["TIME_STEP"] = params["TIME_STEP"] if "TIME_STEP" in params.keys() else 0.1
+        self.params["MAX_ITERATION"] = params["MAX_ITERATION"] if "MAX_ITERATION" in params.keys() else 1500
+        self.params["LOOKAHEAD_TIME"] = params["LOOKAHEAD_TIME"] if "LOOKAHEAD_TIME" in params.keys() else 1.0
+        self.params["MAX_LOOKAHEAD_DIST"] = params["MAX_LOOKAHEAD_DIST"] if "MAX_LOOKAHEAD_DIST" in params.keys() else 2.5
+        self.params["MIN_LOOKAHEAD_DIST"] = params["MIN_LOOKAHEAD_DIST"] if "MIN_LOOKAHEAD_DIST" in params.keys() else 1.0
+        self.params["MAX_V_INC"] = params["MAX_V_INC"] if "MAX_V_INC" in params.keys() else 1.0
+        self.params["MIN_V_INC"] = params["MIN_V_INC"] if "MIN_V_INC" in params.keys() else -1.0
+        self.params["MAX_V"] = params["MAX_V"] if "MAX_V" in params.keys() else 0.5
+        self.params["MIN_V"] = params["MIN_V"] if "MIN_V" in params.keys() else 0.0
+        self.params["MAX_W_INC"] = params["MAX_W_INC"] if "MAX_W_INC" in params.keys() else math.pi
+        self.params["MIN_W_INC"] = params["MIN_W_INC"] if "MIN_W_INC" in params.keys() else -math.pi
+        self.params["MAX_W"] = params["MAX_W"] if "MAX_W" in params.keys() else math.pi / 2
+        self.params["MIN_W"] = params["MIN_W"] if "MIN_W" in params.keys() else -math.pi / 2
+        self.params["GOAL_DIST_TOL"] = params["GOAL_DIST_TOL"] if "GOAL_DIST_TOL" in params.keys() else 0.5
+        self.params["ROTATE_TOL"] = params["ROTATE_TOL"] if "ROTATE_TOL" in params.keys() else 0.5
+
+        # global planner
+        self.g_planner_ = None
+        # global path
+        self.path = None
+        # search factory
+        self.search_factory_ = SearchFactory()
+    
+    @property
+    def g_planner(self):
+        return str(self.g_planner_)
+    
+    @g_planner.setter
+    def g_planner(self, config):
+        if "planner_name" in config:
+            self.g_planner_ = self.search_factory_(**config)
+        else:
+            raise RuntimeError("Please set planner name!")
+    
+    @property
+    def g_path(self):
+        """
+        [property]Global path.
+        """
+        if self.g_planner_ is None:
+            raise AttributeError("Global path searcher is None, please set it first!")
+        
+        cost, path, _ = self.g_planner_.plan()
+        return path
+
+    @property
+    def lookahead_dist(self):
+        return MathHelper.clamp(
+            abs(self.robot.v) * self.params["LOOKAHEAD_TIME"],
+            self.params["MIN_LOOKAHEAD_DIST"],
+            self.params["MAX_LOOKAHEAD_DIST"]
+        )
+
+    def dist(self, start: tuple, end: tuple) -> float:
+        return math.hypot(end[0] - start[0], end[1] - start[1])
+    
+    def angle(self, start: tuple, end: tuple) -> float:
+        return math.atan2(end[1] - start[1], end[0] - start[0])
+
+    def regularizeAngle(self, angle: float):
+        return angle - 2.0 * math.pi * math.floor((angle + math.pi) / (2.0 * math.pi))
+
+    def getLookaheadPoint(self):
+        """
+        Find the point on the path that is exactly the lookahead distance away from the robot
+
+        Returns:
+            lookahead_pt (tuple): lookahead point
+            theta (float): the angle on trajectory
+            kappa (float): the curvature on trajectory
+        """
+        if self.path is None:
+            assert RuntimeError("Please plan the path using g_planner!")
+
+        # Find the first pose which is at a distance greater than the lookahead distance
+        dist_to_robot = [self.dist(p, (self.robot.px, self.robot.py)) for p in self.path]
+        idx_closest = dist_to_robot.index(min(dist_to_robot))
+        idx_goal = len(self.path) - 1
+        idx_prev = idx_goal - 1
+        for i in range(idx_closest, len(self.path)):
+            if self.dist(self.path[i], (self.robot.px, self.robot.py)) >= self.lookahead_dist:
+                idx_goal = i
+                break
+
+        pt_x, pt_y = None, None
+        if idx_goal == len(self.path) - 1:
+            # If the no pose is not far enough, take the last pose
+            pt_x = self.path[idx_goal][0]
+            pt_y = self.path[idx_goal][1]
+        else:
+            if idx_goal == 0:
+                idx_goal = idx_goal + 1
+            #  find the point on the line segment between the two poses
+            #  that is exactly the lookahead distance away from the robot pose (the origin)
+            #  This can be found with a closed form for the intersection of a segment and a circle
+            idx_prev = idx_goal - 1
+            px, py = self.path[idx_prev][0], self.path[idx_prev][1]
+            gx, gy = self.path[idx_goal][0], self.path[idx_goal][1]
+
+            # transform to the robot frame so that the circle centers at (0,0)
+            prev_p = (px - self.robot.px, py - self.robot.py)
+            goal_p = (gx - self.robot.px, gy - self.robot.py)
+            i_points = MathHelper.circleSegmentIntersection(prev_p, goal_p, self.lookahead_dist)
+            if len(i_points) == 0:
+                # If there is no intersection, take the closest intersection point (foot of a perpendicular)
+                # between the current position and the line segment
+                i_points.append(MathHelper.closestPointOnLine(prev_p, goal_p))
+            pt_x = i_points[0][0] + self.robot.px
+            pt_y = i_points[0][1] + self.robot.py
+
+        # calculate the angle on trajectory
+        theta = self.angle(self.path[idx_prev], self.path[idx_goal])
+
+        # calculate the curvature on trajectory
+        if idx_goal == 1:
+            idx_goal = idx_goal + 1
+        idx_prev = idx_goal - 1
+        idx_pprev = idx_prev - 1
+        a = self.dist(self.path[idx_prev], self.path[idx_goal])
+        b = self.dist(self.path[idx_pprev], self.path[idx_goal])
+        c = self.dist(self.path[idx_pprev], self.path[idx_prev])
+        cosB = (a * a + c * c - b * b) / (2 * a * c)
+        sinB = math.sin(math.acos(cosB))
+        cross = (self.path[idx_prev][0] - self.path[idx_pprev][0]) * \
+                (self.path[idx_goal][1] - self.path[idx_pprev][1]) - \
+                (self.path[idx_prev][1] - self.path[idx_pprev][1]) * \
+                (self.path[idx_goal][0] - self.path[idx_pprev][0])
+        kappa = math.copysign(2 * sinB / b, cross)
+
+        return (pt_x, pt_y), theta, kappa
+
+    def linearRegularization(self, v_d: float) -> float:
+        """
+        Linear velocity regularization
+
+        Parameters:
+            v_d (float): reference velocity input
+
+        Returns:
+            v (float): control velocity output
+        """
+        v_inc = v_d - self.robot.v
+        v_inc = MathHelper.clamp(v_inc, self.params["MIN_V_INC"], self.params["MAX_V_INC"])
+
+        v = self.robot.v + v_inc
+        v = MathHelper.clamp(v, self.params["MIN_V"], self.params["MAX_V"])
+
+        return v
+
+    def angularRegularization(self, w_d: float) -> float:
+        """
+        Angular velocity regularization
+
+        Parameters:
+            w_d (float): reference angular velocity input
+
+        Returns:
+            w (float): control angular velocity output
+        """
+        w_inc = w_d - self.robot.w
+        w_inc = MathHelper.clamp(w_inc, self.params["MIN_W_INC"], self.params["MAX_W_INC"])
+
+        w = self.robot.w + w_inc
+        w = MathHelper.clamp(w, self.params["MIN_W"], self.params["MAX_W"])
+
+        return w
+
+    def shouldMoveToGoal(self, cur: tuple, goal: tuple) -> bool:
+        """
+        Whether to move to the goal pose
+
+        Parameters:
+            cur (tuple): current pose of robot
+            goal (tuple): goal pose of robot
+
+        Returns:
+            flag (bool): true if robot should perform movement
+        """
+        return self.dist(cur, goal) > self.params["GOAL_DIST_TOL"]
+    
+    def shouldRotateToPath(self, angle_to_path: float) -> bool:
+        """
+        Whether to correct the tracking path with rotation operation
+
+        Parameters:
+            angle_to_path (float): the angle deviation
+
+        Returns:
+            flag (bool): true if robot should perform rotation
+        """
+        return angle_to_path > self.params["ROTATE_TOL"]
+
+    def reachGoal(self, cur: tuple, goal: tuple) -> bool:
+        """
+        Whether the robot has reached the goal pose
+
+        Parameters:
+            cur (tuple): current pose of robot
+            goal (tuple): goal pose of robot
+
+        Returns:
+            flag (bool): true if robot has reached the goal
+        """
+        e_theta = self.regularizeAngle(cur[2] - goal[2])
+        return not (self.shouldMoveToGoal((cur[0], cur[1]), (goal[0], goal[1]))
+                    or self.shouldRotateToPath(abs(e_theta)))
+
+    def isCollision(self, cur_pos: tuple):
+        """
+        Whether the robot is in collision with obstacles
+
+        Parameters:
+            cur_pos (tuple): current position of robot
+
+        Returns:
+            flag (bool): true if robot is in collision
+        """
+        obstacles = self.obstacles
+        for obs in obstacles:
+            if abs(cur_pos[0] - obs[0]) < 0.5 and abs(cur_pos[1] - obs[1]) < 0.5:
+                return True
+        return False

local_planner/lqr.py

@@ -0,0 +1,146 @@
+"""
+@file: lqr.py
+@breif: Linear Quadratic Regulator(LQR) motion planning
+@author: Yang Haodong, Wu Maojia
+@update: 2024.6.25
+"""
+import numpy as np
+
+from .local_planner import LocalPlanner
+from python_motion_planning.utils import Env
+
+class LQR(LocalPlanner):
+    """
+    Class for Linear Quadratic Regulator(LQR) 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 LQR
+        >>> start = (5, 5, 0)
+        >>> goal = (45, 25, 0)
+        >>> env = Grid(51, 31)
+        >>> planner = LQR(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)
+        # LQR parameters
+        self.Q = np.diag([1, 1, 1])
+        self.R = np.diag([1, 1])
+        self.lqr_iteration = 100
+        self.eps_iter = 1e-1
+
+        # 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 "Linear Quadratic Regulator (LQR)"
+
+    def plan(self):
+        """
+        LQR 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
+
+            # get the particular point on the path at the lookahead distance
+            lookahead_pt, theta_trj, kappa = self.getLookaheadPoint()
+
+            # 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(
+                    self.angle(self.robot.position, lookahead_pt) - self.robot.theta
+                )
+                if self.shouldRotateToPath(abs(e_theta)):
+                    u = np.array([[0], [self.angularRegularization(e_theta / dt)]])
+                else:
+                    s = (self.robot.px, self.robot.py, self.robot.theta) # current state
+                    s_d = (lookahead_pt[0], lookahead_pt[1], theta_trj)  # desired state
+                    u_r = (self.robot.v, self.robot.v * kappa)           # refered input
+                    u = self.lqrControl(s, s_d, u_r)
+
+            # 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 lqrControl(self, s: tuple, s_d: tuple, u_r: tuple) -> np.ndarray:
+        """
+        Execute LQR control process.
+
+        Parameters:
+            s (tuple): current state
+            s_d (tuple): desired state
+            u_r (tuple): refered control
+
+        Returns:
+            u (np.ndarray): control vector
+        """
+        dt = self.params["TIME_STEP"]
+
+        # state equation on error
+        A = np.identity(3)
+        A[0, 2] = -u_r[0] * np.sin(s_d[2]) * dt
+        A[1, 2] = u_r[0] * np.cos(s_d[2]) * dt
+
+        B = np.zeros((3, 2))
+        B[0, 0] = np.cos(s_d[2]) * dt
+        B[1, 0] = np.sin(s_d[2]) * dt
+        B[2, 1] = dt
+
+        # discrete iteration Ricatti equation
+        P, P_ = np.zeros((3, 3)), np.zeros((3, 3))
+        P = self.Q
+
+        # iteration
+        for _ in range(self.lqr_iteration):
+            P_ = self.Q + A.T @ P @ A - A.T @ P @ B @ np.linalg.inv(self.R + B.T @ P @ B) @ B.T @ P @ A
+            if np.max(P - P_) < self.eps_iter:
+                break
+            P = P_
+
+        # feedback
+        K = -np.linalg.inv(self.R + B.T @ P_ @ B) @ B.T @ P_ @ A
+        e = np.array([[s[0] - s_d[0]], [s[1] - s_d[1]], [self.regularizeAngle(s[2] - s_d[2])]])
+        u = np.array([[u_r[0]], [u_r[1]]]) + K @ e
+
+        return np.array([
+            [self.linearRegularization(float(u[0]))], 
+            [self.angularRegularization(float(u[1]))]
+        ])