workable set of planners plus occgrid/polygon maps

Author: petercorke

roboticstoolbox/mobile/CurvaturePolyPlanner.py

@@ -5,134 +5,81 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from collections import namedtuple
-from roboticstoolbox.mobile.PlannerBase import PlannerBase
+from roboticstoolbox.mobile import *
 
-
-def solvepath(poly, s_f, x0=[0, 0, 0], **kwargs):
-    # poly is 4 coeffs of curvature polynomial
-    # x0 is initial state of the vehicle
+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, x, poly):
-        # x = (x, y, θ)
-        # xdot = (cosθ, sinθ, ϰ)
+    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 = x[2]
+        theta = q[2]
         return math.cos(theta), math.sin(theta), k
 
-    sol = scipy.integrate.solve_ivp(dotfunc, [0, s_f], x0, args=(poly,), **kwargs)
+    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 costfunc(unknowns, start, goal, curvature):
+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
+    # p[0:4] is polynomial: k0, a, b, c
     # p[4] is s_f
 
     # integrate the path for this curvature polynomial and length
-    path, maxcurvature = solvepath(poly=unknowns[:4], s_f=unknowns[4], x0=start)
+    # 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])
 
-    if curvature is not None and maxcurvature > curvature:
-        # e *= np.exp(maxcurvature - curvature)
-        e += 0.1 * (maxcurvature - curvature)
-    # print(e, path[:, -1], np.r_[goal])
     return e
 class CurvaturePolyPlanner(PlannerBase):
 
-    r"""
-    Curvature polynomial planner
-
-    :return: curvature polynomial path planner
-    :rtype: CurvaturePolyPlanner instance
-
-    ==================   ========================
-    Feature              Capability
-    ==================   ========================
-    Plan                 Configuration space
-    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 cubic polynomial
-    in curvature:
-
-    .. math::
-
-        \kappa(s) = \kappa_0 + as + b s^2 + c s^3, 0 \le s \le s_f 
-
-    where :math:`\kappa_0` is the initial path curvature.  This is integrated along the path
-
-    .. math::
-
-        \theta(s) &= \theta_0 + \kappa_0 s + \frac{a}{2}s^2 + \frac{b}{3}s^3 + \frac{c}{4}s^4 \\
-        x(s) &= x_0 + \int_0^s \cos \theta(t) dt \\
-        y(s) &= y_0 + \int_0^s \sin \theta(t) dt
-
-    where the initial configuration is :math:`(x_0, y_0, \theta_0)` and the final
-    configuration is :math:`(x_{s_f}, y_{s_f}, \theta_{s_f})`.
-
-    Numerical optimization is used to find the parameters path length
-    :math:`s_f` and the polynomial coefficients :math:`(\kappa_0, a, b, c)`.
-
-    :reference: Mobile Robotics, Cambridge 2013. Alonzo Kelly
-
-    :seealso: :class:`Planner`
-    """
-
     def __init__(self, curvature=None):
         super().__init__(ndims=3)
         self.curvature = curvature
 
     def query(self, start, goal):
-        r"""
-        Find a curvature 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
-        ==========  ===================================================
-        ``length``  the length of the path, :math:`s_f`
-        ``poly``    the polynomial coefficients :math:`(\kappa_0, a, b, c)`
-
-        ==========  ===================================================
-    
-        :seealso: :meth:`Planner.query`
-        """
         goal = np.r_[goal]
         start = np.r_[start]
         self._start = start
         self._goal = goal
 
-        delta = goal[:2] - start[:2]
-        d = np.linalg.norm(delta)
-        # theta = math.atan2(delta[1], delta[0])
-        sol = scipy.optimize.minimize(costfunc, [0, 0, 1, 0, d],
-            bounds=[(None, None), (None, None), (None, None), (None, None), (d, None)],
-            args=(start, goal, self.curvature))
+        # 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
 
-        path, maxcurvature = solvepath(sol.x[:4], s_f=sol.x[4], x0=start, dense_output=True, max_step = 1e-2)
+        if self.curvature is not None:
+            nlcontraints = (scipy.optimize.NonlinearConstraint(xcurvature, 0, self.curvature),)
+        else:
+            nlcontraints = ()
 
-        status = namedtuple('CurvaturePolyStatus', 
-            ['length', 'maxcurvature', 'poly', 'success', 'iterations', 'message'])
+        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)
 
-        return path, status(sol.x[4], maxcurvature, sol.x[:4], sol.success, sol.nit, sol.message)
+        status = namedtuple('CurvaturePolyStatus', ['length', 'maxcurvature', 'poly'])(sol.x[4], maxcurvature, sol.x[:4])
+
+        return path.T, status
 
 if __name__ == '__main__':
     from math import pi
@@ -142,13 +89,26 @@ def query(self, start, goal):
     start = (0, 0, -pi/4)
     goal = (1, 2, pi/4)
 
-    # start = (0, 0, pi/2)
-    # goal = (1, 0, pi/2)
+    start = (0, 0, pi/2)
+    goal = (1, 0, pi/2)
 
-    planner = CurvaturePolyPlanner(curvature=1)
+    planner = CurvaturePolyPlanner()
     path, status = planner.query(start, goal)
-    print('start', path[:, 0])
-    print('goal', path[:, -1])
+    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)

roboticstoolbox/mobile/DistanceTransformPlanner.py

@@ -74,7 +74,7 @@ def __str__(self):
     def goal_change(self, goal):
         self._distancemap = np.array([])
 
-    def plan(self, goal=None, animate=False):
+    def plan(self, goal=None, animate=False, verbose=False):
         # show = None
         # if animate:
         #     show = 0.05
@@ -88,7 +88,8 @@ def plan(self, goal=None, animate=False):
             raise ValueError('No goal specified here or in constructor')
 
         self._distancemap = distancexform(self.occgrid.grid,
-                goal=self._goal, metric=self._metric, animate=animate)
+                goal=self._goal, metric=self._metric, animate=animate,
+                verbose=verbose)
 
     # Use plot from parent class
 
@@ -155,25 +156,9 @@ def plot_3d(self, p=None, ls=None):
         return ax
 
 
-# Sourced from: https://stackoverflow.com/questions/28995146/matlab-ind2sub-equivalent-in-python/28995315#28995315
-def sub2ind(array_shape, rows, cols):
-    ind = rows*array_shape[1] + cols
-    ind[ind < 0] = -1
-    ind[ind >= array_shape[0]*array_shape[1]] = -1
-    return ind
-
-
-def ind2sub(array_shape, ind):
-    ind[ind < 0] = -1
-    ind[ind >= array_shape[0]*array_shape[1]] = -1
-    rows = (ind.astype('int') / array_shape[1])
-    cols = ind % array_shape[1]
-    return rows, cols
-
-
 import numpy as np
 
-def distancexform(occgrid, goal, metric='cityblock', animate=False):
+def distancexform(occgrid, goal, metric='cityblock', animate=False, verbose=False):
     """
     Distance transform for path planning
 
@@ -273,7 +258,8 @@ def distancexform(occgrid, goal, metric='cityblock', animate=False):
 
         ninf = ninfnow
 
-    print(f"{count:d} iterations, {ninf:d} unreachable cells")
+    if verbose:
+        print(f"{count:d} iterations, {ninf:d} unreachable cells")
     return distance
 
 def grassfire_step(G, D):
@@ -311,7 +297,7 @@ def grassfire_step(G, D):
     # print(dx)
 
 
-    from roboticstoolbox import DistanceTransformPlanner, loadmat
+    from roboticstoolbox import DistanceTransformPlanner, rtb_loadmat
 
     house = rtb_loadmat('data/house.mat')
     floorplan = house['floorplan']
@@ -321,4 +307,5 @@ def grassfire_step(G, D):
     print(dx)
     dx.goal = [1,2]
     dx.plan(places.kitchen)
-    dx.plot()
+    path = dx.query(places.br3)
+    dx.plot(path, block=True)

roboticstoolbox/mobile/DubinsPlanner.py

@@ -394,7 +394,7 @@ def query(self, start, goal, **kwargs):
 
     print(path)
     print(status)
-    dubins.plot(path, twod=True)
+    dubins.plot(path, configspace=True)
 
     plt.show(block=True)
     # plt.plot(path_x, path_y, label="final course " + "".join(mode))