roboticstoolbox-python 1.3.0__cp313-cp313-win_amd64.whl

roboticstoolbox/mobile/Bug2.py

@@ -0,0 +1,455 @@
+"""
+Python Bug Planner
+@Author: Peter Corke, original MATLAB code and Python version
+@Author: Kristian Gibson, initial MATLAB port
+"""
+# from numpy import disp
+# from scipy import integrate
+
+# from spatialmath.pose2d import SE2
+from spatialmath import base
+from spatialmath.base.animate import *
+from scipy.ndimage import *
+
+# from matplotlib import cm
+import matplotlib.pyplot as plt
+
+# from matplotlib import animation
+from roboticstoolbox.mobile.PlannerBase import PlannerBase
+
+
+class Bug2(PlannerBase):
+    """
+    Construct a Bug2 reactive navigator
+
+    :param occgrid: occupancy grid
+    :type occgrid: :class:`OccGrid` instance or ndarray(N,M)
+    :param kwargs: common arguments for :class:`PlannerBase` superclass
+    :return: Bug2 reactive navigator
+    :rtype: Bug2 instance
+
+    Creates an object which simulates an automaton, capable of omnidirectional
+    motion, finding a path across an occupancy grid using only a bump sensor.
+
+    :reference: "Path-Planning Strategies for a Point Mobile Automaton Moving
+        Amidst Unknown Obstacles of Arbitrary Shape", Lumelsky and Stepanov,
+        Algorithmica (1987)2, pp.403-430
+
+    .. note:: This class is not a planner, even though it subclasses
+        :class:`PlannerBase`. It can produce very inefficient paths.
+
+    :author: Kristian Gibson and Peter Corke, based on MATLAB version by Peter Corke
+    :seealso: :class:`PlannerBase`
+    """
+
+    def __init__(self, **kwargs):
+        super().__init__(ndims=2, **kwargs)
+
+        self.H = []  # hit points
+        self._j = 0
+        self._step = 1
+        self._m_line = None
+        self._edge = None
+        self._k = None
+
+    @property
+    def m_line(self):
+        """
+        Get m-line
+
+        :return: m-line in homogeneous form
+        :rtype: ndarray(3)
+
+        This is the m-line computed for the last :meth:`run`.
+        """
+        return self._m_line
+
+    # override query method of base class
+    def run(
+        self,
+        start=None,
+        goal=None,
+        animate=False,
+        pause=0.001,
+        trail=True,
+        movie=None,
+        **kwargs,
+    ):
+        """
+        Find a path using Bug2 reactive navigation algorithm
+
+        :param start: starting position
+        :type start: array_like(2)
+        :param goal: goal position
+        :type goal: array_like(2)
+        :param animate: show animation of robot moving over the map,
+            defaults to False
+        :type animate: bool, optional
+        :param movie: save animation as a movie, defaults to None. Is either
+            name of movie or a tuple (filename, frame interval)
+        :type movie: str or tuple(str, float), optional
+        :param trail: show the path followed by the robot, defaults to True
+        :type current: bool, optional
+        :return: path from ``start`` to ``goal``
+        :rtype: ndarray(2,N)
+
+        Compute the path from ``start`` to ``goal`` assuming the robot is capable
+        of 8-way motion from its current cell to the next.
+
+        .. note:: ``start`` and ``goal`` are given as (x,y) coordinates in the
+              occupancy grid map, not as matrix row and column coordinates.
+
+        :seealso: :meth:`Bug2.plot`
+        """
+
+        # make sure start and goal are set and valid
+        # super().query(start=start, goal=goal, dtype=np.int, **kwargs)
+        # make sure start and goal are set and valid
+        self.start = self.validate_endpoint(start, dtype=int)
+        self.goal = self.validate_endpoint(goal, dtype=int)
+
+        # compute the m-line
+        #  create homogeneous representation of the line
+        #  line*[x y 1]' = 0
+        self._m_line = hom_line(self.start, self.goal)
+
+        if movie is not None:
+            animate = True
+
+        if animate:
+            self.plot()
+            self.plot_m_line()
+            plt.pause(0.05)
+
+        # movie = MovieWriter(movie)
+
+        robot = self.start
+        self._step = 1
+        path = robot
+        h = None
+
+        (trail_line,) = plt.plot(0, 0, "y.", label="robot path")
+        (trailHead,) = plt.plot(0, 0, "ko", zorder=10)
+
+        # iterate using the next() method until we reach the goal
+        while True:
+            if animate:
+                trailHead.set_data(robot[0], robot[1])
+                if trail:
+                    trail_line.set_data(path.T)
+
+                if pause > 0:
+                    plt.pause(pause)
+                # plt.draw()
+                # plt.show(block=False)
+                # plt.gcf().canvas.flush_events()
+
+                # movie.add()
+
+            # move to next point on path
+            robot = self.next(robot)
+
+            # # have we been here before, ie. in a loop
+            # if any([all(robot == x) for x in path]):
+            #     raise RuntimeError('trapped')
+
+            # are we there yet?
+            if robot is None:
+                break
+            else:
+                path = np.vstack((path, robot))
+
+        # movie.done()
+        if animate:
+            trailHead.remove()
+
+        return path
+
+    def plot_m_line(self, ls=None):
+        """
+        Plot m-line
+
+        :param ls: linestyle, defaults to ``"k--"``
+        :type ls: str, optional
+
+        Plots the m-line on the current axes.
+        """
+        if ls is None:
+            ls = "k--"
+
+        x_min, x_max = plt.gca().get_xlim()
+        y_min, y_max = plt.gca().get_ylim()
+        if self._m_line[1] == 0:
+            # handle the case that the line is vertical
+            plt.plot(
+                [self._start[0], self._start[0]], [y_min, y_max], "k--", label="m-line"
+            )
+        else:
+            # regular line
+            x = np.array([[x_min, 1], [x_max, 1]])
+            y = -x @ np.r_[self._m_line[0], self._m_line[2]]
+            y = y / self._m_line[1]
+            plt.plot([x_min, x_max], y, ls, zorder=10, label="m-line")
+
+    def next(self, position):
+        """
+        One step of the finite state automaton
+
+        :param position: current robot position
+        :type position: ndarray(2)
+        :raises RuntimeError: robot is trapped
+        :return: next robot position
+        :rtype: ndarray(2)
+        """
+
+        l = None
+        y = None
+
+        if all(self._goal == position):
+            return None  # we have arrived
+
+        if self._step == 1:
+            # Step 1.  Move along the M-line toward the goal
+            self.message(f"{position}: moving along the M-line (step 1)")
+            # motion on line toward goal
+            d = self._goal - position
+            if abs(d[0]) > abs(d[1]):
+                # line slope less than 45 deg
+                dx = 1 if d[0] >= 0 else -1  # np.sign(d[0])
+                l = self._m_line
+                y = -((position[0] + dx) * l[0] + l[2]) / l[1]
+                dy = int(round(y - position[1]))
+            else:
+                # line slope greater than 45 deg
+                dy = 1 if d[1] >= 0 else -1  # np.sign(d[1])
+                l = self._m_line
+                x = -((position[1] + dy) * l[1] + l[2]) / l[0]
+                dx = int(round(x - position[0]))
+
+            # detect if next step is an obstacle
+            if self.isoccupied(position + np.r_[dx, dy]):
+                self.message(f"  {position}: obstacle at {position + np.r_[dx, dy]}")
+                self.H.append(position)  # save hit point
+                self._step = 2  # transition to step 2
+                self.message(f"  {position}: change to step 2")
+
+                # get a list of all the points around the obstacle
+                self._edge, _ = edgelist(self.occgrid.grid == 0, position)
+                self._k = 0
+            else:
+                n = position + np.array([dx, dy])
+
+        if self._step == 2:
+            # Step 2.  Move around the obstacle until we reach a point
+            # on the M-line closer than when we started.
+
+            self.message(f"{position}: moving around the obstacle (step 2)")
+            if self._k < len(self._edge):
+                n = self._edge[self._k]  # next edge point
+            else:
+                # we are at the end of the list of edge points, we
+                # are back where we started.  Step 2.c test.
+                plt.show(block=True)
+                raise RuntimeError("robot is trapped")
+
+            # are we on the M-line now ?
+            if abs(np.inner(np.r_[position, 1], self._m_line)) <= 0.5:
+                self.message(f"  {position}: crossed the M-line")
+
+                # are we closer than when we encountered the obstacle?
+                if base.norm(position - self._goal) < base.norm(
+                    self.H[-1] - self._goal
+                ):
+                    self._step = 1  # transition to step 1
+                    self.message(f"  {position}: change to step 1")
+                    return n
+
+            # no, keep going around
+            self._k += 1
+
+        return n
+
+    def query(self):
+        raise NotImplementedError("This class has no query method")
+
+    def plan(self):
+        raise NotImplementedError("This class has no plan method")
+
+
+# Ported from Peter Corke's edgelist function found:
+# https://github.com/petercorke/toolbox-common-matlab/blob/master/edgelist.m
+
+
+#  these are directions of 8-neighbours in a clockwise direction
+# fmt: off
+_dirs = np.array([
+        [-1,  0],
+        [-1,  1],
+        [ 0,  1],
+        [ 1,  1],
+        [ 1,  0],
+        [ 1, -1],
+        [ 0, -1],
+        [-1, -1],
+    ])
+# fmt: on
+def edgelist(im, p, direction=1):
+    """
+    Find edge of a region
+
+    :param im: binary image
+    :type im: ndarray(h,w,int)
+    :param p: initial point
+    :type p: array_like(2)
+    :param direction: direction to traverse region, +1 clockwise [default], -1
+        counter-clockwise
+    :type direction: int, optional
+    :raises ValueError: initial point is not on the edge
+    :raises RuntimeError: not able to find path to the goal
+    :return: edge list, direction vector list
+    :rtype: tuple of lists
+
+    ``edge, dirs = edgelist(im, seed)`` is the boundary/contour/edge of a region
+    in the binary image ``im``.  ``seed=[X,Y]`` is the coordinate of a point on
+    the edge of the region of interest, but belonging to the region.
+
+    ``edge`` is a list of coordinates (2) of edge pixels of a region in theThe
+    elements of the edgelist are NumPy ndarray(2).
+
+    ``dirs`` is a list of integers representing the direction of the edge from
+    the corresponding point in ``edge`` to the next point in ``edge``.  The
+    integers in the range 0 to 7 represent directions: W SW S SE E NW N NW
+    respectively.
+
+    - Coordinates are given and returned assuming the matrix is an image, so the
+      indices are always in the form (x,y) or (column,row).
+    - ``im` is a binary image where 0 is assumed to be background, non-zero
+      is an object.
+    - ``p`` must be a point on the edge of the region.
+    - The initial point is always the first and last element of the returned edgelist.
+    - 8-direction chain coding can give incorrect results when used with
+      blobs founds using 4-way connectivty.
+
+    :Reference:
+
+    - METHODS TO ESTIMATE AREAS AND PERIMETERS OF BLOB-LIKE OBJECTS: A COMPARISON
+      Luren Yang, Fritz Albregtsen, Tor Lgnnestad and Per Grgttum
+      IAPR Workshop on Machine Vision Applications Dec. 13-15, 1994, Kawasaki
+
+    """
+    p = base.getvector(p, 2, dtype=int)
+    if direction > 0:
+        neighbours = np.arange(start=0, stop=8, step=1)
+    else:
+        neighbours = np.arange(start=7, stop=-1, step=-1)
+
+    try:
+        pix0 = im[p[1], p[0]]  # color of pixel we start at
+    except:
+        raise ValueError("specified coordinate is not within image")
+
+    q = adjacent_point(im, p, pix0)
+
+    # find an adjacent point outside the blob
+    if q is None:
+        raise ValueError("no neighbour outside the blob")
+
+    d = None
+    e = [p]  # initialize the edge list
+    dir = []  # initialize the direction list
+    p0 = None
+
+    while True:
+        # find which direction is Q
+        dq = q - p
+        for kq in range(0, 8):
+            # get index of neighbour's direction in range [1,8]
+            if np.all(dq == _dirs[kq]):
+                break
+
+        # now test for directions relative to Q
+        for j in neighbours:
+            # get index of neighbour's direction in range [1,8]
+            k = (j + kq) % 8
+            # if k > 7:
+            #     k = k - 7
+
+            # compute coordinate of the k'th neighbour
+            nk = p + _dirs[k]
+
+            try:
+                if im[nk[1], nk[0]] == pix0:
+                    # if this neighbour is in the blob it is the next edge pixel
+                    p = nk
+                    break
+            except:
+                raise ValueError("Something went wrong calculating edgelist")
+
+            q = nk
+
+        dir.append(k)
+        # check if we are back where we started
+        if p0 is None:
+            p0 = p  # make a note of where we started
+        else:
+            if all(p == p0):
+                break
+
+        # keep going, add this point to the edgelist
+        e.append(p)
+
+    return e, dir
+
+
+# Ported from Peter Corke's adjacent_point function found:
+# https://github.com/petercorke/toolbox-common-matlab/blob/master/edgelist.m
+
+
+def adjacent_point(im, seed, pix0):
+    """Find adjacent point
+
+    :param im: occupancy grid
+    :type im: ndarray(m,n)
+    :param seed: initial point
+    :type seed: ndarray(2)
+    :param pix0: value of occupancy grid at ``seed`` coordinate
+    :type pix0: int
+    :return: coordinate of a neighbour
+    :rtype: ndarray(2) or None
+
+    Is a neighbouring point of the coordinate ``seed`` that is not within the
+    region containing the coordinate ``seed``, ie. it is a neighbour but
+    outside.
+    """
+    p = None
+
+    for d in _dirs:
+        p = seed + d
+        try:
+            if im[p[1], p[0]] != pix0:
+                return p
+        except:
+            pass
+
+    return None
+
+
+# Implementation of Peter Corke's matlab homline function from:
+# https://github.com/petercorke/spatialmath-matlab/blob/master/homline.m
+def hom_line(p1, p2):
+    line = np.cross(np.r_[p1[0], p1[1], 1], np.r_[p2[0], p2[1], 1])
+
+    # normalize so that the result of x*l' is the pixel distance
+    # from the line
+    return line / np.linalg.norm(line[0:2])
+
+
+if __name__ == "__main__":
+    from roboticstoolbox import rtb_load_matfile
+
+    vars = rtb_load_matfile("data/map1.mat")
+    map = vars["map"]
+
+    bug2 = Bug2(occgrid=map)
+    # bug.plan()
+    path = bug2.run([20, 10], [50, 35], animate=True)

roboticstoolbox/mobile/CurvaturePolyPlanner.py

@@ -0,0 +1,179 @@
+import math
+import scipy.integrate
+import scipy.optimize
+import numpy as np
+
+# import matplotlib.pyplot as plt
+from collections import namedtuple
+from roboticstoolbox.mobile import *
+
+
+def solvepath(x, q0=[0, 0, 0], **kwargs):
+    # x[:4] is 4 coeffs of curvature polynomial
+    # x[4] is total path length
+    # q0 is initial state of the vehicle
+    maxcurvature = 0
+
+    def dotfunc(s, q, poly):
+        # q = (x, y, θ)
+        # qdot = (cosθ, sinθ, ϰ)
+        k = poly[0] * s**3 + poly[1] * s**2 + poly[2] * s + poly[3]
+        # k = ((poly[0] * s + poly[1]) * s  + poly[2]) * s + poly[3]
+
+        # save maximum curvature for this path solution
+        nonlocal maxcurvature
+        maxcurvature = max(maxcurvature, abs(k))
+
+        theta = q[2]
+        return math.cos(theta), math.sin(theta), k
+
+    cpoly = x[:4]
+    s_f = x[4]
+    sol = scipy.integrate.solve_ivp(dotfunc, [0, s_f], q0, args=(cpoly,), **kwargs)
+    return sol.y, maxcurvature
+
+
+def xcurvature(x):
+    # inequality constraint function, must be non-negative
+    _, maxcurvature = solvepath(x, q0=(0, 0, 0))
+    return maxcurvature
+
+
+def costfunc(x, start, goal):
+    # final cost of path from start with params
+    # p[0:4] is polynomial: k0, a, b, c
+    # p[4] is s_f
+
+    # integrate the path for this curvature polynomial and length
+    # path is 3xN
+    path, _ = solvepath(x, q0=start)
+
+    # cost is configuration error at end of path
+    e = np.linalg.norm(path[:, -1] - np.r_[goal])
+
+    return e
+
+
+class CurvaturePolyPlanner(PlannerBase):
+    def __init__(self, curvature=None):
+        r"""
+        Continuous curvature path planner
+
+        :param curvature: , defaults to None
+        :type curvature: _type_, optional
+
+        ==================   ========================
+        Feature              Capability
+        ==================   ========================
+        Plan                 :math:`\SE{2}`
+        Obstacle avoidance   No
+        Curvature            Continuous
+        Motion               Forwards only
+        ==================   ========================
+
+        Example:
+
+        .. runblock:: pycon
+
+            >>> from roboticstoolbox import DubinsPlanner
+            >>> from math import pi
+            >>> planner = CurvaturePolyPlanner()
+            >>> path, status = planner.query(start=(0, 0, pi/2), goal=(1, 0, pi/2))
+            >>> print(path[:5,:])
+            >>> print(status)
+        """
+        super().__init__(ndims=3)
+        self.curvature = curvature
+
+    def query(self, start, goal):
+        r"""
+        Find a path betwee two configurations
+
+        :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 path comprises points equally spaced at a distance of ``stepsize``.
+
+        The returned status value has elements:
+
+        +-------------------+-----------------------------------------------+
+        | Element           |  Description                                  |
+        +-------------------+-----------------------------------------------+
+        | ``length``        | total path length                             |
+        +-------------------+-----------------------------------------------+
+        | ``maxcurvature``  | maximum curvature on path                     |
+        +-------------------+-----------------------------------------------+
+        | ``poly``          | curvature polynomial coefficients             |
+        +-------------------+-----------------------------------------------+
+
+        """
+        goal = np.r_[goal]
+        start = np.r_[start]
+        self._start = start
+        self._goal = goal
+
+        # initial estimate of path length is Euclidean distance
+        d = np.linalg.norm(goal[:2] - start[:2])
+        # state vector is kappa_0, a, b, c, s_f
+
+        if self.curvature is not None:
+            nlcontraints = (
+                scipy.optimize.NonlinearConstraint(xcurvature, 0, self.curvature),
+            )
+        else:
+            nlcontraints = ()
+
+        sol = scipy.optimize.minimize(
+            costfunc,
+            [0, 0, 0, 0, d],
+            constraints=nlcontraints,
+            bounds=[(None, None), (None, None), (None, None), (None, None), (d, None)],
+            args=(start, goal),
+        )
+        print(sol)
+        path, maxcurvature = solvepath(
+            sol.x, q0=start, dense_output=True, max_step=1e-2
+        )
+
+        status = namedtuple("CurvaturePolyStatus", ["length", "maxcurvature", "poly"])(
+            sol.x[4], maxcurvature, sol.x[:4]
+        )
+
+        return path.T, status
+
+
+if __name__ == "__main__":
+    from math import pi
+
+    # start = (1, 1, pi/4)
+    # goal = (-3, -3, -pi/4)
+    # start = (0, 0, -pi/4)
+    # goal = (1, 2, pi/4)
+
+    start = (0, 0, pi / 2)
+    goal = (1, 0, pi / 2)
+
+    planner = CurvaturePolyPlanner()
+    path, status = planner.query(start, goal)
+    print("start", path[0, :])
+    print("goal", path[-1, :])
+
+    print(status)
+    planner.plot(path, block=True)
+
+    ## attempt polynomial scaling, doesnt seem to work
+    # sf = status.s_f
+    # c = status.poly
+    # print(c)
+
+    # print(solvepath(np.r_[c, sf], start))
+
+    # for i in range(4):
+    #     c[i] /= sf ** (3-i)
+
+    # print(solvepath(np.r_[c, 1], start))
+    # print(c)