roboticstoolbox-python 1.3.0__cp313-cp313-win_amd64.whl

roboticstoolbox/mobile/QuinticPolyPlanner.py

@@ -0,0 +1,349 @@
+import math
+from collections import namedtuple
+
+import matplotlib.pyplot as plt
+import numpy as np
+from roboticstoolbox.mobile.PlannerBase import PlannerBase
+
+# parameter
+
+
+show_animation = True
+
+# ======================================================================== #
+
+# The following code is modified from Python Robotics
+# https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning
+# Quintic Polynomial Planner
+# Author: Atsushi Sakai
+# Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors: https://github.com/AtsushiSakai/PythonRobotics/contributors
+# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE
+
+
+class _QuinticPolynomial:
+    def __init__(self, xs, vxs, axs, xe, vxe, axe, time):
+        # calc coefficient of quintic polynomial
+        # See jupyter notebook document for derivation of this equation.
+        self.a0 = xs
+        self.a1 = vxs
+        self.a2 = axs / 2.0
+
+        A = np.array(
+            [
+                [time**3, time**4, time**5],
+                [3 * time**2, 4 * time**3, 5 * time**4],
+                [6 * time, 12 * time**2, 20 * time**3],
+            ]
+        )
+        b = np.array(
+            [
+                xe - self.a0 - self.a1 * time - self.a2 * time**2,
+                vxe - self.a1 - 2 * self.a2 * time,
+                axe - 2 * self.a2,
+            ]
+        )
+        x = np.linalg.solve(A, b)
+
+        self.a3 = x[0]
+        self.a4 = x[1]
+        self.a5 = x[2]
+
+    def calc_point(self, t):
+        xt = (
+            self.a0
+            + self.a1 * t
+            + self.a2 * t**2
+            + self.a3 * t**3
+            + self.a4 * t**4
+            + self.a5 * t**5
+        )
+
+        return xt
+
+    def calc_first_derivative(self, t):
+        xt = (
+            self.a1
+            + 2 * self.a2 * t
+            + 3 * self.a3 * t**2
+            + 4 * self.a4 * t**3
+            + 5 * self.a5 * t**4
+        )
+
+        return xt
+
+    def calc_second_derivative(self, t):
+        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + 20 * self.a5 * t**3
+
+        return xt
+
+    def calc_third_derivative(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2
+
+        return xt
+
+
+def quintic_polynomials_planner(
+    sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt, MIN_T, MAX_T
+):
+    """
+    quintic polynomial planner
+
+    input
+        s_x: start x position [m]
+        s_y: start y position [m]
+        s_yaw: start yaw angle [rad]
+        sa: start accel [m/ss]
+        gx: goal x position [m]
+        gy: goal y position [m]
+        gyaw: goal yaw angle [rad]
+        ga: goal accel [m/ss]
+        max_accel: maximum accel [m/ss]
+        max_jerk: maximum jerk [m/sss]
+        dt: time tick [s]
+
+    return
+        time: time result
+        rx: x position result list
+        ry: y position result list
+        ryaw: yaw angle result list
+        rv: velocity result list
+        ra: accel result list
+
+    """
+
+    vxs = sv * math.cos(syaw)
+    vys = sv * math.sin(syaw)
+    vxg = gv * math.cos(gyaw)
+    vyg = gv * math.sin(gyaw)
+
+    axs = sa * math.cos(syaw)
+    ays = sa * math.sin(syaw)
+    axg = ga * math.cos(gyaw)
+    ayg = ga * math.sin(gyaw)
+
+    time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
+
+    for T in np.arange(MIN_T, MAX_T, MIN_T):
+        xqp = _QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)
+        yqp = _QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)
+
+        time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
+
+        for t in np.arange(0.0, T + dt, dt):
+            time.append(t)
+            rx.append(xqp.calc_point(t))
+            ry.append(yqp.calc_point(t))
+
+            vx = xqp.calc_first_derivative(t)
+            vy = yqp.calc_first_derivative(t)
+            v = np.hypot(vx, vy)
+            yaw = math.atan2(vy, vx)
+            rv.append(v)
+            ryaw.append(yaw)
+
+            ax = xqp.calc_second_derivative(t)
+            ay = yqp.calc_second_derivative(t)
+            a = np.hypot(ax, ay)
+            if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0:
+                a *= -1
+            ra.append(a)
+
+            jx = xqp.calc_third_derivative(t)
+            jy = yqp.calc_third_derivative(t)
+            j = np.hypot(jx, jy)
+            if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:
+                j *= -1
+            rj.append(j)
+
+        if (
+            max([abs(i) for i in ra]) <= max_accel
+            and max([abs(i) for i in rj]) <= max_jerk
+        ):
+            print("find path!!")
+            break
+
+    return time, np.c_[rx, ry, ryaw], rv, ra, rj
+
+
+# ====================== RTB wrapper ============================= #
+
+
+# Copyright (c) 2022 Peter Corke: https://github.com/petercorke/robotics-toolbox-python
+# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE
+class QuinticPolyPlanner(PlannerBase):
+    r"""
+    Quintic polynomial path planner
+
+    :param dt: time step, defaults to 0.1
+    :type dt: float, optional
+    :param start_vel: initial velocity, defaults to 0
+    :type start_vel: float, optional
+    :param start_acc: initial acceleration, defaults to 0
+    :type start_acc: float, optional
+    :param goal_vel: goal velocity, defaults to 0
+    :type goal_vel: float, optional
+    :param goal_acc: goal acceleration, defaults to 0
+    :type goal_acc: float, optional
+    :param max_acc: maximum acceleration, defaults to 1
+    :type max_acc: int, optional
+    :param max_jerk: maximum jerk, defaults to 0.5
+    :type min_t: float, optional
+    :param min_t: minimum path time, defaults to 5
+    :type max_t: float, optional
+    :param max_t: maximum path time, defaults to 100
+    :type max_jerk: float, optional
+    :return: Quintic polynomial path planner
+    :rtype: QuinticPolyPlanner instance
+
+    ==================   ========================
+    Feature              Capability
+    ==================   ========================
+    Plan                 :math:`\SE{2}`
+    Obstacle avoidance   No
+    Curvature            Continuous
+    Motion               Forwards only
+    ==================   ========================
+
+    Creates a planner that finds the path between two configurations in the
+    plane using forward motion only.  The path is a continuous quintic polynomial
+    for x and y
+
+    .. math::
+
+            x(t) &= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \\
+            y(t) &= b_0 + b_1 t + b_2 t^2 + b_3 t^3 + b_4 t^4 + b_5 t^5
+
+    that meets the given constraints.  Trajectory time is given as a range.
+    
+    :reference: "Local Path Planning And Motion Control For AGV In
+        Positioning",  Takahashi, T. Hongo, Y. Ninomiya and G.
+        Sugimoto; Proceedings. IEEE/RSJ International Workshop on
+        Intelligent Robots and Systems (IROS '89)  doi: 10.1109/IROS.1989.637936
+
+    .. note:: The path time is searched in the interval [``min_t``, ``max_t``] in steps
+        of ``min_t``.
+
+    Example:
+
+    .. runblock:: pycon
+
+        >>> from roboticstoolbox import QuinticPolyPlanner
+        >>> import numpy as np
+        >>> start = (10, 10, np.deg2rad(10.0))
+        >>> goal = (30, -10, np.deg2rad(20.0))
+        >>> quintic = QuinticPolyPlanner(start_vel=1)
+        >>> path, status = quintic.query(start, goal)
+        >>> print(path[:5,:])
+
+    :thanks: based on quintic polynomial planning from `Python Robotics <https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning>`_
+
+
+    :seealso: :class:`Planner`
+    """
+
+    def __init__(
+        self,
+        dt=0.1,
+        start_vel=0,
+        start_acc=0,
+        goal_vel=0,
+        goal_acc=0,
+        max_acc=1,
+        max_jerk=0.5,
+        min_t=5,
+        max_t=100,
+    ):
+
+        super().__init__(ndims=3)
+        self.dt = dt
+        self.start_vel = start_vel
+        self.start_acc = start_acc
+        self.goal_vel = goal_vel
+        self.goal_acc = goal_acc
+        self.max_acc = max_acc
+        self.max_jerk = max_jerk
+        self.min_t = min_t
+        self.max_t = max_t
+
+    def query(self, start, goal):
+        r"""
+        Find a quintic polynomial path
+
+        :param start: start configuration :math:`(x, y, \theta)`
+        :type start: array_like(3), optional
+        :param goal: goal configuration :math:`(x, y, \theta)`
+        :type goal: array_like(3), optional
+        :return: path and status
+        :rtype: ndarray(N,3), namedtuple
+
+        The returned status value has elements:
+
+        ==========  ===================================================
+        Element     Description
+        ==========  ===================================================
+        ``t``       time to execute the path
+        ``vel``     velocity profile along the path
+        ``accel``   acceleration profile along the path
+        ``jerk``    jerk profile along the path
+        ==========  ===================================================
+
+        :seealso: :meth:`Planner.query`
+        """
+        self._start = start
+        self._goal = goal
+
+        time, path, v, a, j = quintic_polynomials_planner(
+            start[0],
+            start[1],
+            start[2],
+            self.start_vel,
+            self.start_acc,
+            goal[0],
+            goal[1],
+            goal[2],
+            self.start_vel,
+            self.start_acc,
+            self.max_acc,
+            self.max_jerk,
+            dt=self.dt,
+            MIN_T=self.min_t,
+            MAX_T=self.max_t,
+        )
+
+        status = namedtuple("QuinticPolyStatus", ["t", "vel", "acc", "jerk"])(
+            time, v, a, j
+        )
+
+        return path, status
+
+
+if __name__ == "__main__":
+    from math import pi
+
+    start = (10, 10, np.deg2rad(10.0))
+    goal = (30, -10, np.deg2rad(20.0))
+
+    quintic = QuinticPolyPlanner(start_vel=1)
+    path, status = quintic.query(start, goal)
+
+    print(status)
+    quintic.plot(path)
+
+    plt.figure()
+    plt.subplot(311)
+    plt.plot(status.t, status.vel, "-r")
+    plt.ylabel("velocity (m/s)")
+    plt.grid(True)
+
+    plt.subplot(312)
+    plt.plot(status.t, status.acc, "-r")
+    plt.ylabel("accel (m/s2)")
+    plt.grid(True)
+
+    plt.subplot(313)
+    plt.plot(status.t, status.jerk, "-r")
+    plt.xlabel("Time (s)")
+    plt.ylabel("jerk (m/s3)")
+    plt.grid(True)
+
+    plt.show(block=True)

roboticstoolbox/mobile/RRTPlanner.py

@@ -0,0 +1,359 @@
+# ======================================================================== #
+
+# The following code is based on code from Python Robotics
+# https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning
+# RRTDubins planning
+# Author: Atsushi Sakai
+# Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors: https://github.com/AtsushiSakai/PythonRobotics/contributors
+# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE
+
+import math
+from collections import namedtuple
+from roboticstoolbox.mobile.OccGrid import PolygonMap
+
+# import rvcprint
+from roboticstoolbox import *
+import numpy as np
+import matplotlib.pyplot as plt
+
+from spatialmath import Polygon2, SE2, base
+from roboticstoolbox.mobile.PlannerBase import PlannerBase
+from roboticstoolbox.mobile.DubinsPlanner import DubinsPlanner
+
+# from roboticstoolbox.mobile.OccupancyGrid import OccupancyGrid
+try:
+    from pgraph import DGraph
+except ImportError:
+
+    class DGraph:  # pragma: no cover
+        def __init__(self, *args, **kwargs):
+            raise ImportError(
+                "RRTPlanner requires optional dependency 'pgraph'. "
+                "Install it with: pip install pgraph-python"
+            )
+
+
+class RRTPlanner(PlannerBase):
+    r"""
+    Rapidly exploring tree planner
+
+    :param map: occupancy grid
+    :type map: :class:`PolygonMap`
+    :param vehicle: vehicle kinematic model
+    :type vehicle: :class:`VehicleBase` subclass
+    :param curvature: maximum path curvature, defaults to 1.0
+    :type curvature: float, optional
+    :param stepsize: spacing between points on the path, defaults to 0.2
+    :type stepsize: float, optional
+    :param npoints: number of vertices in random tree, defaults to 50
+    :type npoints: int, optional
+
+    ==================   ========================
+    Feature              Capability
+    ==================   ========================
+    Plan                 :math:`\SE{2}`
+    Obstacle avoidance   Yes, polygons
+    Curvature            Discontinuous
+    Motion               Bidirectional
+    ==================   ========================
+
+    Creates a planner that finds the obstacle-free path between two
+    configurations in the plane using forward and backward motion.  The path
+    comprises multiple Dubins curves comprising straight lines, or arcs with
+    curvature of :math:`\pm` ``curvature``. Motion along the segments may be in
+    the forward or backward direction.
+
+    Polygons are used for obstacle avoidance:
+
+    - the environment is defined by a set of polygons represented by a :class:`PolygonMap`
+    - the vehicle is defined by a single polygon specified by the ``polygon``
+      argument to its constructor
+
+    Example::
+
+        from roboticstoolbox import RRTPlanner
+        from spatialmath import Polygon2
+        from math import pi
+
+        # create polygonal obstacles
+        map = PolygonMap(workspace=[0, 10])
+        map.add([(5, 50), (5, 6), (6, 6), (6, 50)])
+        map.add([(5, 4), (5, -50), (6, -50), (6, 4)])
+
+        # create outline polygon for vehicle
+        l, w = 3, 1.5
+        vpolygon = Polygon2([(-l/2, w/2), (-l/2, -w/2), (l/2, -w/2), (l/2, w/2)])
+
+        # create vehicle model
+        vehicle = Bicycle(steer_max=1, L=2, polygon=vpolygon)
+
+        # create planner
+        rrt = RRTPlanner(map=map, vehicle=vehicle, npoints=50, seed=0)
+        # start and goal configuration
+        qs = (2, 8, -pi/2)
+        qg = (8, 2, -pi/2)
+
+        # plan path
+        rrt.plan(goal=qg)
+        path, status = rrt.query(start=qs)
+        print(path[:5,:])
+        print(status)
+
+
+    :seealso: :class:`DubinsPlanner` :class:`Vehicle` :class:`PlannerBase`
+    """
+
+    def __init__(
+        self,
+        map,
+        vehicle,
+        curvature=1.0,
+        stepsize=0.2,
+        npoints=50,
+        **kwargs,
+    ):
+        super().__init__(ndims=2, **kwargs)
+
+        self.npoints = npoints
+        self.map = map
+
+        self.g = DGraph(metric="SE2")
+
+        self.vehicle = vehicle
+        if curvature is None:
+            if vehicle is not None:
+                curvature = vehicle.curvature_max
+            else:
+                curvature = 1
+
+        # print("curvature", curvature)
+        self.dubins = DubinsPlanner(curvature=curvature, stepsize=stepsize)
+
+        # self.goal_yaw_th = np.deg2rad(1.0)
+        # self.goal_xy_th = 0.5
+
+    def plan(self, goal, showsamples=True, showvalid=True, animate=False, block=None):
+        r"""
+        Plan paths to goal using RRT
+
+        :param goal: goal pose :math:`(x, y, \theta)`, defaults to previously set value
+        :type goal: array_like(3), optional
+        :param showsamples: display position part of configurations overlaid on the map, defaults to True
+        :type showsamples: bool, optional
+        :param showvalid: display valid configurations as vehicle polygons overlaid on the map, defaults to False
+        :type showvalid: bool, optional
+        :param animate: update the display as configurations are tested, defaults to False
+        :type animate: bool, optional
+
+        Compute a rapidly exploring random tree with its root at the ``goal``.
+        The tree will have ``npoints`` vertices spread uniformly randomly over
+        the workspace which is an attribute of the ``map``.
+
+        For every new point added, a Dubins path is computed to the nearest
+        vertex already in the graph.  Each configuration on that path, with
+        spacing of ``stepsize``, is tested for obstacle intersection.
+
+        The configurations tested are displayed (translation only) if ``showsamples`` is
+        True.  The valid configurations are displayed as vehicle polygones if ``showvalid``
+        is True.  If ``animate`` is True these points are displayed during the search
+        process, otherwise a single figure is displayed at the end.
+
+        :seealso: :meth:`query`
+        """
+        # TODO use validate
+        self.goal = np.r_[goal]
+        # self.goal = np.r_[goal]
+        self.random_init()
+
+        v = self.g.add_vertex(coord=goal)
+        v.path = None
+
+        if showsamples or showvalid:
+            self.map.plot()
+
+        self.progress_start(self.npoints)
+        count = 0
+        while count < self.npoints:
+            random_point = self.qrandom_free()
+
+            if showsamples:
+                plt.plot(random_point[0], random_point[1], "ok", markersize=2)
+                if animate:
+                    plt.pause(0.02)
+
+            vnearest, d = self.g.closest(random_point)
+
+            if d > 6:
+                continue
+            path, pstatus = self.dubins.query(random_point, vnearest.coord)
+            if path is None:
+                continue
+
+            collision = False
+            for config in path:
+                if self.map.iscollision(self.vehicle.polygon(config)):
+                    collision = True
+                    break
+            if collision:
+                # print('collision')
+                continue
+
+            if pstatus.length > 6:
+                # print('too long')
+                continue
+
+            # we have a valid configuration to add to the graph
+            count += 1
+            self.progress_next()
+
+            # add new vertex to graph
+            vnew = self.g.add_vertex(random_point)
+            self.g.add_edge(vnew, vnearest, cost=pstatus.length)
+            vnew.path = path
+
+            if showvalid:
+                self.vehicle.polygon(random_point).plot(color="b", alpha=0.1)
+                if animate:
+                    plt.pause(0.02)
+
+        if (showvalid or showsamples) and not animate:
+            if block is not None:
+                plt.show(block=block)
+
+        self.progress_end()
+
+    def query(self, start):
+        r"""
+        Find a path from start configuration
+
+        :param start: start configuration :math:`(x, y, \theta)`
+        :type start: array_like(3), optional
+        :return: path and status
+        :rtype: ndarray(N,3), namedtuple
+
+        The path comprises points equally spaced at a distance of ``stepsize``.
+
+        The returned status value has elements:
+
+        +---------------+---------------------------------------------------+
+        | Element       |  Description                                      |
+        +---------------+---------------------------------------------------+
+        | ``length``    | total path length                                 |
+        +-------------+-----------------------------------------------------+
+        | ``initial_d`` | distance from start to first vertex in graph      |
+        +---------------+---------------------------------------------------+
+        | ``vertices``  | sequence of vertices in the graph                 |
+        +---------------+---------------------------------------------------+
+
+        """
+        self._start = start
+        vstart, d = self.g.closest(start)
+
+        vpath, cost, _ = self.g.path_UCS(vstart, self.g[0])
+
+        print(vpath)
+        # stack the Dubins path segments
+        path = np.empty((0, 3))
+        for vertex in vpath:
+            if vertex.path is not None:
+                path = np.vstack((path, vertex.path))
+
+        status = namedtuple("RRTStatus", ["length", "initial_d", "vertices"])(
+            cost, d, vpath
+        )
+
+        return path, status
+
+    # def _generate_final_course(self, goal_ind):
+    #     path = [[self.end.x, self.end.y]]
+    #     node = self.node_list[goal_ind]
+    #     while node.parent is not None:
+    #         path.append([node.x, node.y])
+    #         node = node.parent
+    #     path.append([node.x, node.y])
+
+    #     return path
+
+    # def _calc_dist_to_goal(self, x, y):
+
+    #     dx = x - self.goal.x
+    #     dy = y - self.end.y
+    #     return math.hypot(dx, dy)
+
+    def qrandom(self):
+        r"""
+        Random configuration
+
+        :return: random configuration :math:`(x, y, \theta)`
+        :rtype: ndarray(3)
+
+        Returns a random configuration where position :math:`(x, y)`
+        lies within the bounds of the ``map`` associated with this planner.
+
+        :seealso: :meth:`qrandom_free`
+        """
+        return self.random.uniform(
+            low=(self.map.workspace[0], self.map.workspace[2], -np.pi),
+            high=(self.map.workspace[1], self.map.workspace[3], np.pi),
+        )
+
+    def qrandom_free(self):
+        r"""
+        Random obstacle free configuration
+
+        :return: random configuration :math:`(x, y, \theta)`
+        :rtype: ndarray(3)
+
+        Returns a random obstacle free configuration where position :math:`(x,
+        y)` lies within the bounds of the ``map`` associated with this planner.
+        Iterates on :meth:`qrandom`
+
+        :seealso: :meth:`qrandom` :meth:`iscollision`
+        """
+        # iterate for a random freespace configuration
+        while True:
+            q = self.qrandom()
+            if not self.iscollision(q):
+                return q
+
+    def iscollision(self, q):
+        r"""
+        Test if configuration is collision
+
+        :param q: vehicle configuration :math:`(x, y, \theta)`
+        :type q: array_like(3)
+        :return: collision status
+        :rtype: bool
+
+        Transforms the vehicle polygon and tests for intersection against
+        the polygonal obstacle map.
+        """
+        return self.map.iscollision(self.vehicle.polygon(q))
+
+
+if __name__ == "__main__":
+    from roboticstoolbox.mobile.Vehicle import Bicycle
+
+    # start and goal configuration
+    qs = (2, 8, -np.pi / 2)
+    qg = (8, 2, -np.pi / 2)
+
+    # obstacle map
+    map = PolygonMap(workspace=[0, 10])
+    map.add([(5, 50), (5, 6), (6, 6), (6, 50)])
+    # map.add([(5, 0), (6, 0), (6, 4), (5, 4)])
+    map.add([(5, 4), (5, -50), (6, -50), (6, 4)])
+
+    l = 3
+    w = 1.5
+    v0 = Polygon2([(-l / 2, w / 2), (-l / 2, -w / 2), (l / 2, -w / 2), (l / 2, w / 2)])
+
+    vehicle = Bicycle(steer_max=0.4, l=2, polygon=v0)
+
+    rrt = RRTPlanner(map=map, vehicle=vehicle, verbose=False, seed=0)
+
+    rrt.plan(goal=qg)
+    path, status = rrt.query(start=qs)
+    rrt.plot(path)
+
+    plt.show(block=True)