roboticstoolbox-python 1.3.0__cp313-cp313-win_amd64.whl

roboticstoolbox/mobile/LatticePlanner.py

@@ -0,0 +1,419 @@
+try:
+    from pgraph import DGraph, DVertex, Edge
+except ImportError:
+    class _PGraphMissing:  # pragma: no cover
+        def __init__(self, *args, **kwargs):
+            raise ImportError(
+                "LatticePlanner requires optional dependency 'pgraph'. "
+                "Install it with: pip install pgraph-python"
+            )
+
+    DGraph = _PGraphMissing
+    DVertex = _PGraphMissing
+    Edge = _PGraphMissing
+import numpy as np
+from spatialmath import SE2
+import matplotlib.pyplot as plt
+import itertools
+from roboticstoolbox.mobile.PlannerBase import PlannerBase
+from roboticstoolbox.mobile.OccGrid import BinaryOccupancyGrid
+from collections import namedtuple
+
+
+def make_arc(dir, radius=1, npoints=20):
+    points = []
+
+    if dir == "S":
+        points.append((0, 0))
+        points.append((radius, 0))
+
+    elif dir == "L":
+        for theta in np.linspace(0, np.pi / 2, npoints):
+            x = radius * np.sin(theta)
+            y = radius * (1 - np.cos(theta))
+            points.append((x, y))
+
+    elif dir == "R":
+        for theta in np.linspace(0, np.pi / 2, npoints):
+            x = radius * np.sin(theta)
+            y = radius * (-1 + np.cos(theta))
+            points.append((x, y))
+
+    return np.array(points).T
+
+
+arcs = {}
+
+
+class LatticeVertex(DVertex):
+    def __init__(self, move=None, pose=None, name=None):
+        super().__init__(name=name)
+        self.move = move
+        self.pose = pose
+        self.coord = pose.xyt()
+
+    def icoord(self):
+        xyt = self.coord
+        ix = int(round(xyt[0]))
+        iy = int(round(xyt[1]))
+        it = int(round(xyt[2] * 2 / np.pi))
+        return f"({ix:d},{iy:d},{it:d}), {self.name}"
+
+
+class LatticeEdge(Edge):
+    def __init__(self, v1, v2, cost, pose, move):
+        super().__init__(v1, v2, cost)
+        self.pose = pose
+        self.move = move
+        self.arc = arcs[move]
+
+    def plot(self, configspace=False, unwrap=False, **kwargs):
+        T = self.pose
+        xy = self.pose * self.arc
+        if configspace:
+            # 3D plot
+            theta0 = self.pose.theta()
+            if self.move == "L":
+                thetaf = theta0 + np.pi / 2
+            elif self.move == "R":
+                thetaf = theta0 - np.pi / 2
+            elif self.move == "S":
+                thetaf = theta0
+            theta = np.linspace(theta0, thetaf, self.arc.shape[1])
+            if unwrap:
+                theta = np.unwrap(theta)
+            plt.plot(xy[0, :], xy[1, :], theta, **kwargs)
+        else:
+            # 2D plot
+            plt.plot(xy[0, :], xy[1, :], **kwargs)
+
+
+class LatticePlanner(PlannerBase):
+    r"""
+    Lattice planner
+
+    :param costs: cost for straight, left-turn, right-turn, defaults to :math:`(1, \pi/2, \pi/2)`
+    :type costs: array_like(3), optional
+    :param root: configuration of root node, defaults to (0,0,0)
+    :type root: array_like(3), optional
+    :param kwargs: arguments passed to ``PlannerBase`` constructor
+
+    ==================   ========================
+    Feature              Capability
+    ==================   ========================
+    Plan                 :math:`\SE{2}`
+    Obstacle avoidance   Yes, occupancy grid
+    Curvature            Discontinuous
+    Motion               Forwards only
+    ==================   ========================
+
+    The lattice planner incrementally builds a graph from the root vertex, at
+    each iteration adding three edges to the graph:
+
+    =====   ================
+    code    direction
+    =====   ================
+    'S'     straight ahead
+    'L'     turn left
+    'R'     turn right
+    =====   ================
+
+    If the configuration is already in the graph, the edge connects to that
+    existing vertex.  The vertex is named after the sequence of moves required
+    to reach it from the root.  This means, that any configuration, ie.
+    :math:`(x, y, \theta)` can be reached by multiple paths and potentially have
+    multiple names.  The first name assigned to a vertex is permanent and is not
+    overriden.
+
+    If an occupancy grid exists and the configuration is an obstacle, then
+    the vertex is not added.
+
+    The path through the lattice is found using A* graph search, and ``costs``
+    changes the weighting for path costs at query time.
+
+    Example:
+
+    .. runblock:: pycon
+
+        >>> from roboticstoolbox import LatticePlanner
+        >>> import numpy as np
+        >>> lattice = LatticePlanner();
+        >>> lattice.plan(iterations=6)
+        >>> path, status = lattice.query(start=(0, 0, 0), goal=(1, 2, np.pi/2))
+        >>> print(path.T)
+        >>> print(status)
+
+    :seealso: :meth:`plan` :meth:`query` :class:`PlannerBase`
+    """
+
+    def __init__(self, costs=None, root=(0, 0, 0), **kwargs):
+
+        global arcs
+
+        super().__init__(ndims=3, **kwargs)
+
+        self.poses = [SE2(1, 0, 0), SE2(1, 1, np.pi / 2), SE2(1, -1, -np.pi / 2)]
+        self.moves = ["S", "L", "R"]
+        if costs is None:
+            costs = [1, np.pi / 2, np.pi / 2]
+        self.costs = costs
+        self.root = root
+
+        # create the set of possible moves
+        for move in self.moves:
+            arcs[move] = make_arc(move)
+
+    def __str__(self):
+        s = (
+            super().__str__()
+            + f"\n  curvature={self.curvature}, stepsize={self.stepsize}"
+        )
+
+    def _icoord(self, xyt):
+        ix = int(round(xyt[0]))
+        iy = int(round(xyt[1]))
+        it = int(round(xyt[2] * 2 / np.pi))
+        return f"({ix:d},{iy:d},{it:d})"
+
+    def plan(self, iterations=None, verbose=False, summary=False):
+        """
+        Create a lattice plan
+
+        :param iterations: number of iterations, defaults to None
+        :type iterations: int, optional
+        :param verbose: show frontier and added vertices/edges at each iteration, defaults to False
+        :type verbose: bool, optional
+
+        If an occupancy grid exists the if ``iterations`` is None the area of the
+        grid will be completely filled.
+
+        :seealso: :meth:`query`
+        """
+        if iterations is None and self.occgrid is None:
+            raise ValueError(
+                "iterations must be finite if no occupancy grid is specified"
+            )
+
+        self.graph = DGraph(metric="SE2")
+
+        # add root vertex to the graph, place it in the frontier
+        v0 = LatticeVertex(pose=SE2(self.root))
+        self.graph.add_vertex(v0, name="0")
+        frontier = [v0]
+
+        iteration = 0
+        while True:
+            newfrontier = []
+            for vertex in frontier:
+                if verbose:
+                    print("EXPAND:", vertex.icoord())
+
+                for pose, move, cost in zip(self.poses, self.moves, self.costs):
+                    newpose = vertex.pose * pose
+                    xyt = newpose.xyt()
+                    # theta is guaranteed to be in range [-pi, pi)
+
+                    if verbose:
+                        print("  MOVE", move, self._icoord(xyt))
+
+                    if self.isoccupied(xyt[:2]):
+                        if verbose:
+                            print("    is occupied")
+                        continue
+                    vclose, d = self.graph.closest(xyt)
+
+                    if d > 0.01:
+                        # vertex does not already exists
+                        vnew = LatticeVertex(move, newpose, name=vertex.name + move)
+
+                        # ix = int(xyt[0])
+                        # iy = int(xyt[1])
+                        # it = int(round(xyt[2]*2/np.pi))
+                        # vnew = LatticeVertex(move, newpose, name=f"{ix:d},{iy:d},{it:d}")
+                        self.graph.add_vertex(vnew)
+                        if verbose:
+                            print("    add to graph as", vnew.name)
+
+                        edge = LatticeEdge(
+                            vertex, vnew, cost=cost, pose=vertex.pose, move=move
+                        )
+
+                        # connect it into the graph, add to frontier
+                        vertex.connect(vnew, edge=edge)
+                        newfrontier.append(vnew)
+                    else:
+                        # vertex already exists
+                        # print('exists', vertex, move, vclose)
+
+                        # connect it into the graph, don't add to frontier
+                        if verbose:
+                            print("    already in graph, connect to", vclose.icoord())
+                        edge = LatticeEdge(
+                            vertex, vclose, cost=cost, pose=vertex.pose, move=move
+                        )
+                        vertex.connect(vclose, edge=edge)
+
+            frontier = newfrontier
+
+            iteration += 1
+            if iterations is None:
+                if len(frontier) == 0:
+                    print(f"finished after {iteration} iterations")
+                    break
+                print(f"iteration {iteration}, frontier length {len(frontier)}")
+            elif iteration >= iterations:
+                break
+        if summary:
+            print(f"{self.graph.n} vertices and {self.graph.ne} edges created")
+
+    def query(self, start, goal):
+        r"""
+        Find a path through the lattice
+
+        :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                                        |
+        +-------------+-----------------------------------------------------+
+        | ``cost``    | path cost                                           |
+        +-------------+-----------------------------------------------------+
+        |``segments`` | a list containing the type of each path segment as  |
+        |             | a single letter code: either "L", "R" or "S" for    |
+        |             | left turn, right turn or straight line respectively.|
+        +-------------+-----------------------------------------------------+
+        |``edges``    | successive edges of the graph ``LatticeEdge`` type  |
+        +-------------+-----------------------------------------------------+
+
+        :seealso: :meth:`plan`
+        """
+
+        vs, ds = self.graph.closest(start)
+        if ds > 0.001:
+            raise ValueError("start configuration is not in the lattice")
+        vg, dg = self.graph.closest(goal)
+        if dg > 0.001:
+            raise ValueError("goal configuration is not in the lattice")
+
+        try:
+            path, cost, _ = self.graph.path_Astar(vs, vg, verbose=False)
+        except TypeError:
+            raise RuntimeError("no path found") from None
+
+        status = namedtuple("LatticeStatus", ["cost", "segments", "edges"])
+
+        segments = []
+        edges = []
+        for p, n in zip(path[:-1], path[1:]):
+            e = p.edgeto(n)
+            edges.append(e)
+            segments.append(e.move)
+
+        return np.array([p.coord for p in path]), status(cost, segments, edges)
+
+    def plot(self, path=None, **kwargs):
+        super().plot(**kwargs)
+
+        if kwargs.get("configspace", False):
+            # 3D plot
+            for k, vertex in enumerate(self.graph):
+                # for every node
+                # if k == 0:
+                #     plt.plot(vertex.coord[0], vertex.coord[1], vertex.coord[2], 'k>', markersize=10)
+                # else:
+                plt.plot(vertex.coord[0], vertex.coord[1], vertex.coord[2], "bo")
+
+            for edge in self.graph.edges():
+                edge.plot(color="k", **kwargs)
+
+            if path is not None:
+                for p, n in zip(path[:-1], path[1:]):
+                    # turn coordinaets back into vertices
+                    vp, _ = self.graph.closest(p)
+                    vn, _ = self.graph.closest(n)
+                    e = vp.edgeto(vn)
+
+                    # e.plot(color='b', linewidth=4)
+
+                    e.plot(color="k", linewidth=4)
+                    e.plot(color="yellow", linewidth=3, dashes=(4, 4))
+        else:
+            # 2D plot
+            for k, vertex in enumerate(self.graph):
+                # for every node
+                # if k == 0:
+                #     plt.plot(vertex.coord[0], vertex.coord[1], 'k>', markersize=10)
+                # else:
+                plt.plot(vertex.coord[0], vertex.coord[1], "bo")
+
+            for edge in self.graph.edges():
+                edge.plot(color="k")
+
+            if path is not None:
+                for p, n in zip(path[:-1], path[1:]):
+                    # turn coordinaets back into vertices
+                    vp, _ = self.graph.closest(p)
+                    vn, _ = self.graph.closest(n)
+                    e = vp.edgeto(vn)
+
+                    # e.plot(color='b', linewidth=4)
+
+                    e.plot(color="k", linewidth=4)
+                    e.plot(color="yellow", linewidth=3, dashes=(4, 4))
+
+
+if __name__ == "__main__":
+    lattice = LatticePlanner()
+    lattice.plan(iterations=6)
+    path = lattice.query(start=(0, 0, np.pi / 2), goal=(1, 1, 0))
+    print(path)
+
+    # og = BinaryOccupancyGrid(workspace=[-5, 5, -5, 5], value=False)
+    # og.set([3, 3, -2, 3], True)
+
+    # lattice = LatticePlanner(occgrid=og)
+
+    # lattice.plan(iterations=10)
+    # print(lattice.graph)
+
+    # qs = (0, 0, np.pi/2)
+    # qg = (1, 0, np.pi/2)
+
+    # print('qs')
+    # vs, d = lattice.graph.closest(qs)
+    # print(vs, d, vs.coord)
+    # print(vs.neighbours())
+    # print()
+
+    # print('[-1, 1, -np.pi]')
+    # vs, d = lattice.graph.closest([-1, 1, -np.pi])
+    # print(vs, d, vs.coord)
+    # print(vs.neighbours())
+    # print()
+
+    # print('0L')
+    # vs = lattice.graph['0L']
+    # print(vs, vs.coord)
+    # print(vs.neighbours())
+    # print()
+
+    # path, status = lattice.query(qs, qg)
+
+    # print(path)
+    # print(status)
+
+    # lattice.plot(path=path)
+
+    # plt.show(block=True)
+
+    # ax = plt.gca()
+    # ax.set_aspect('equal')
+    # ax.grid(True)
+    # ax.set_xlim(-4, 4)
+    # ax.set_ylim(-4, 4)