|
| 1 | + |
| 2 | +import math |
| 3 | +import scipy.integrate |
| 4 | +import scipy.optimize |
| 5 | +import numpy as np |
| 6 | +import matplotlib.pyplot as plt |
| 7 | +from collections import namedtuple |
| 8 | +from roboticstoolbox.mobile.Planner import Planner |
| 9 | + |
| 10 | + |
| 11 | +def solvepath(poly, s_f, x0=[0, 0, 0], **kwargs): |
| 12 | + |
| 13 | + def dotfunc(t, x, poly): |
| 14 | + theta = x[2] |
| 15 | + k = poly[0] * t ** 3 + poly[1] * t ** 2 + poly[2] * t + poly[3] |
| 16 | + return math.cos(theta), math.sin(theta), k |
| 17 | + |
| 18 | + sol = scipy.integrate.solve_ivp(dotfunc, [0, s_f], start, args=(poly,), **kwargs) |
| 19 | + return sol.y |
| 20 | + |
| 21 | +def costfunc(unknowns, start, goal): |
| 22 | + # final cost of path from start with params |
| 23 | + # p[0:4] is polynomial |
| 24 | + # p[4] is s_f |
| 25 | + path = solvepath(poly=unknowns[:4], s_f=unknowns[4], x0=start) |
| 26 | + return np.linalg.norm(path[:, -1] - np.r_[goal]) |
| 27 | + |
| 28 | + |
| 29 | +class CurvaturePolyPlanner(Planner): |
| 30 | + |
| 31 | + def query(self, start, goal): |
| 32 | + goal = np.r_[goal] |
| 33 | + start = np.r_[start] |
| 34 | + self._start = start |
| 35 | + self._goal = goal |
| 36 | + |
| 37 | + delta = goal[:2] - start[:2] |
| 38 | + d = np.linalg.norm(delta) |
| 39 | + theta = math.atan2(delta[1], delta[0]) |
| 40 | + sol = scipy.optimize.minimize(costfunc, [0, 0, 0, 1, theta, d], args=(start, goal,)) |
| 41 | + |
| 42 | + path = solvepath(sol.x[:4], sol.x[4], dense_output=True, max_step = 1e-2) |
| 43 | + |
| 44 | + status = namedtuple('CurvaturePolyStatus', ['s_f', 'poly'])(sol.x[5], sol.x[:5]) |
| 45 | + |
| 46 | + return path.T, status |
| 47 | + |
| 48 | +if __name__ == '__main__': |
| 49 | + from math import pi |
| 50 | + |
| 51 | + # start = (1, 1, pi/4) |
| 52 | + # goal = (-3, -3, -pi/4) |
| 53 | + start = (0, 0, -pi/4) |
| 54 | + goal = (1, 2, pi/4) |
| 55 | + |
| 56 | + # start = (0, 0, pi/2) |
| 57 | + # goal = (1, 0, pi/2) |
| 58 | + |
| 59 | + planner = CurvaturePolyPlanner() |
| 60 | + path, status = planner.query(start, goal) |
| 61 | + |
| 62 | + print(status) |
| 63 | + planner.plot(path, block=True) |
0 commit comments