Skip to content

Commit cd350cf

Browse files
petercorkepetercorke
committed
workable set of planners plus occgrid/polygon maps
1 parent 945192f commit cd350cf

11 files changed

+514
-492
lines changed

‎roboticstoolbox/mobile/CurvaturePolyPlanner.py

Lines changed: 56 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -5,134 +5,81 @@
55
import numpy as np
66
import matplotlib.pyplot as plt
77
from collections import namedtuple
8-
from roboticstoolbox.mobile.PlannerBase import PlannerBase
8+
from roboticstoolbox.mobile import *
99

10-
11-
def solvepath(poly, s_f, x0=[0, 0, 0], **kwargs):
12-
# poly is 4 coeffs of curvature polynomial
13-
# x0 is initial state of the vehicle
10+
def solvepath(x, q0=[0, 0, 0], **kwargs):
11+
# x[:4] is 4 coeffs of curvature polynomial
12+
# x[4] is total path length
13+
# q0 is initial state of the vehicle
1414
maxcurvature = 0
1515

16-
def dotfunc(s, x, poly):
17-
# x = (x, y, θ)
18-
# xdot = (cosθ, sinθ, ϰ)
16+
def dotfunc(s, q, poly):
17+
# q = (x, y, θ)
18+
# qdot = (cosθ, sinθ, ϰ)
1919
k = poly[0] * s ** 3 + poly[1] * s ** 2 + poly[2] * s + poly[3]
20+
# k = ((poly[0] * s + poly[1]) * s + poly[2]) * s + poly[3]
2021

2122
# save maximum curvature for this path solution
2223
nonlocal maxcurvature
2324
maxcurvature = max(maxcurvature, abs(k))
2425

25-
theta = x[2]
26+
theta = q[2]
2627
return math.cos(theta), math.sin(theta), k
2728

28-
sol = scipy.integrate.solve_ivp(dotfunc, [0, s_f], x0, args=(poly,), **kwargs)
29+
cpoly = x[:4]
30+
s_f = x[4]
31+
sol = scipy.integrate.solve_ivp(dotfunc, [0, s_f], q0, args=(cpoly,), **kwargs)
2932
return sol.y, maxcurvature
3033

31-
def costfunc(unknowns, start, goal, curvature):
34+
def xcurvature(x):
35+
# inequality constraint function, must be non-negative
36+
_, maxcurvature = solvepath(x, q0=(0, 0, 0))
37+
return maxcurvature
38+
39+
def costfunc(x, start, goal):
3240
# final cost of path from start with params
33-
# p[0:4] is polynomial
41+
# p[0:4] is polynomial: k0, a, b, c
3442
# p[4] is s_f
3543

3644
# integrate the path for this curvature polynomial and length
37-
path, maxcurvature = solvepath(poly=unknowns[:4], s_f=unknowns[4], x0=start)
45+
# path is 3xN
46+
path, _ = solvepath(x, q0=start)
3847

3948
# cost is configuration error at end of path
4049
e = np.linalg.norm(path[:, -1] - np.r_[goal])
4150

42-
if curvature is not None and maxcurvature > curvature:
43-
# e *= np.exp(maxcurvature - curvature)
44-
e += 0.1 * (maxcurvature - curvature)
45-
# print(e, path[:, -1], np.r_[goal])
4651
return e
4752
class CurvaturePolyPlanner(PlannerBase):
4853

49-
r"""
50-
Curvature polynomial planner
51-
52-
:return: curvature polynomial path planner
53-
:rtype: CurvaturePolyPlanner instance
54-
55-
================== ========================
56-
Feature Capability
57-
================== ========================
58-
Plan Configuration space
59-
Obstacle avoidance No
60-
Curvature Continuous
61-
Motion Forwards only
62-
================== ========================
63-
64-
Creates a planner that finds the path between two configurations in the
65-
plane using forward motion only. The path is a continuous cubic polynomial
66-
in curvature:
67-
68-
.. math::
69-
70-
\kappa(s) = \kappa_0 + as + b s^2 + c s^3, 0 \le s \le s_f
71-
72-
where :math:`\kappa_0` is the initial path curvature. This is integrated along the path
73-
74-
.. math::
75-
76-
\theta(s) &= \theta_0 + \kappa_0 s + \frac{a}{2}s^2 + \frac{b}{3}s^3 + \frac{c}{4}s^4 \\
77-
x(s) &= x_0 + \int_0^s \cos \theta(t) dt \\
78-
y(s) &= y_0 + \int_0^s \sin \theta(t) dt
79-
80-
where the initial configuration is :math:`(x_0, y_0, \theta_0)` and the final
81-
configuration is :math:`(x_{s_f}, y_{s_f}, \theta_{s_f})`.
82-
83-
Numerical optimization is used to find the parameters path length
84-
:math:`s_f` and the polynomial coefficients :math:`(\kappa_0, a, b, c)`.
85-
86-
:reference: Mobile Robotics, Cambridge 2013. Alonzo Kelly
87-
88-
:seealso: :class:`Planner`
89-
"""
90-
9154
def __init__(self, curvature=None):
9255
super().__init__(ndims=3)
9356
self.curvature = curvature
9457

9558
def query(self, start, goal):
96-
r"""
97-
Find a curvature polynomial path
98-
99-
:param start: start configuration :math:`(x, y, \theta)`
100-
:type start: array_like(3), optional
101-
:param goal: goal configuration :math:`(x, y, \theta)`
102-
:type goal: array_like(3), optional
103-
:return: path and status
104-
:rtype: ndarray(N,3), namedtuple
105-
106-
The returned status value has elements:
107-
108-
========== ===================================================
109-
Element Description
110-
========== ===================================================
111-
``length`` the length of the path, :math:`s_f`
112-
``poly`` the polynomial coefficients :math:`(\kappa_0, a, b, c)`
113-
114-
========== ===================================================
115-
116-
:seealso: :meth:`Planner.query`
117-
"""
11859
goal = np.r_[goal]
11960
start = np.r_[start]
12061
self._start = start
12162
self._goal = goal
12263

123-
delta = goal[:2] - start[:2]
124-
d = np.linalg.norm(delta)
125-
# theta = math.atan2(delta[1], delta[0])
126-
sol = scipy.optimize.minimize(costfunc, [0, 0, 1, 0, d],
127-
bounds=[(None, None), (None, None), (None, None), (None, None), (d, None)],
128-
args=(start, goal, self.curvature))
64+
# initial estimate of path length is Euclidean distance
65+
d = np.linalg.norm(goal[:2] - start[:2])
66+
# state vector is kappa_0, a, b, c, s_f
12967

130-
path, maxcurvature = solvepath(sol.x[:4], s_f=sol.x[4], x0=start, dense_output=True, max_step = 1e-2)
68+
if self.curvature is not None:
69+
nlcontraints = (scipy.optimize.NonlinearConstraint(xcurvature, 0, self.curvature),)
70+
else:
71+
nlcontraints = ()
13172

132-
status = namedtuple('CurvaturePolyStatus',
133-
['length', 'maxcurvature', 'poly', 'success', 'iterations', 'message'])
73+
sol = scipy.optimize.minimize(costfunc, [0, 0, 0, 0, d],
74+
constraints=nlcontraints,
75+
bounds=[(None, None), (None, None), (None, None), (None, None), (d, None)],
76+
args=(start, goal))
77+
print(sol)
78+
path, maxcurvature = solvepath(sol.x, q0=start, dense_output=True, max_step = 1e-2)
13479

135-
return path, status(sol.x[4], maxcurvature, sol.x[:4], sol.success, sol.nit, sol.message)
80+
status = namedtuple('CurvaturePolyStatus', ['length', 'maxcurvature', 'poly'])(sol.x[4], maxcurvature, sol.x[:4])
81+
82+
return path.T, status
13683

13784
if __name__ == '__main__':
13885
from math import pi
@@ -142,13 +89,26 @@ def query(self, start, goal):
14289
start = (0, 0, -pi/4)
14390
goal = (1, 2, pi/4)
14491

145-
# start = (0, 0, pi/2)
146-
# goal = (1, 0, pi/2)
92+
start = (0, 0, pi/2)
93+
goal = (1, 0, pi/2)
14794

148-
planner = CurvaturePolyPlanner(curvature=1)
95+
planner = CurvaturePolyPlanner()
14996
path, status = planner.query(start, goal)
150-
print('start', path[:, 0])
151-
print('goal', path[:, -1])
97+
print('start', path[0,:])
98+
print('goal', path[-1, :])
15299

153100
print(status)
154101
planner.plot(path, block=True)
102+
103+
## attempt polynomial scaling, doesnt seem to work
104+
# sf = status.s_f
105+
# c = status.poly
106+
# print(c)
107+
108+
# print(solvepath(np.r_[c, sf], start))
109+
110+
# for i in range(4):
111+
# c[i] /= sf ** (3-i)
112+
113+
# print(solvepath(np.r_[c, 1], start))
114+
# print(c)

‎roboticstoolbox/mobile/DistanceTransformPlanner.py

Lines changed: 9 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ def __str__(self):
7474
def goal_change(self, goal):
7575
self._distancemap = np.array([])
7676

77-
def plan(self, goal=None, animate=False):
77+
def plan(self, goal=None, animate=False, verbose=False):
7878
# show = None
7979
# if animate:
8080
# show = 0.05
@@ -88,7 +88,8 @@ def plan(self, goal=None, animate=False):
8888
raise ValueError('No goal specified here or in constructor')
8989

9090
self._distancemap = distancexform(self.occgrid.grid,
91-
goal=self._goal, metric=self._metric, animate=animate)
91+
goal=self._goal, metric=self._metric, animate=animate,
92+
verbose=verbose)
9293

9394
# Use plot from parent class
9495

@@ -155,25 +156,9 @@ def plot_3d(self, p=None, ls=None):
155156
return ax
156157

157158

158-
# Sourced from: https://stackoverflow.com/questions/28995146/matlab-ind2sub-equivalent-in-python/28995315#28995315
159-
def sub2ind(array_shape, rows, cols):
160-
ind = rows*array_shape[1] + cols
161-
ind[ind < 0] = -1
162-
ind[ind >= array_shape[0]*array_shape[1]] = -1
163-
return ind
164-
165-
166-
def ind2sub(array_shape, ind):
167-
ind[ind < 0] = -1
168-
ind[ind >= array_shape[0]*array_shape[1]] = -1
169-
rows = (ind.astype('int') / array_shape[1])
170-
cols = ind % array_shape[1]
171-
return rows, cols
172-
173-
174159
import numpy as np
175160

176-
def distancexform(occgrid, goal, metric='cityblock', animate=False):
161+
def distancexform(occgrid, goal, metric='cityblock', animate=False, verbose=False):
177162
"""
178163
Distance transform for path planning
179164
@@ -273,7 +258,8 @@ def distancexform(occgrid, goal, metric='cityblock', animate=False):
273258

274259
ninf = ninfnow
275260

276-
print(f"{count:d} iterations, {ninf:d} unreachable cells")
261+
if verbose:
262+
print(f"{count:d} iterations, {ninf:d} unreachable cells")
277263
return distance
278264

279265
def grassfire_step(G, D):
@@ -311,7 +297,7 @@ def grassfire_step(G, D):
311297
# print(dx)
312298

313299

314-
from roboticstoolbox import DistanceTransformPlanner, loadmat
300+
from roboticstoolbox import DistanceTransformPlanner, rtb_loadmat
315301

316302
house = rtb_loadmat('data/house.mat')
317303
floorplan = house['floorplan']
@@ -321,4 +307,5 @@ def grassfire_step(G, D):
321307
print(dx)
322308
dx.goal = [1,2]
323309
dx.plan(places.kitchen)
324-
dx.plot()
310+
path = dx.query(places.br3)
311+
dx.plot(path, block=True)

‎roboticstoolbox/mobile/DubinsPlanner.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -394,7 +394,7 @@ def query(self, start, goal, **kwargs):
394394

395395
print(path)
396396
print(status)
397-
dubins.plot(path, twod=True)
397+
dubins.plot(path, configspace=True)
398398

399399
plt.show(block=True)
400400
# plt.plot(path_x, path_y, label="final course " + "".join(mode))

0 commit comments

Comments
 (0)