|
6 | 6 |
|
7 | 7 | """
|
8 | 8 |
|
9 |
| -import numpy as np |
10 |
| -import matplotlib.pyplot as plt |
11 | 9 | import math
|
| 10 | + |
| 11 | +import matplotlib.pyplot as plt |
| 12 | +import numpy as np |
| 13 | + |
12 | 14 | import motion_model
|
13 | 15 |
|
14 | 16 | # optimization parameter
|
@@ -37,7 +39,7 @@ def calc_diff(target, x, y, yaw):
|
37 | 39 | return d
|
38 | 40 |
|
39 | 41 |
|
40 |
| -def calc_J(target, p, h, k0): |
| 42 | +def calc_j(target, p, h, k0): |
41 | 43 | xp, yp, yawp = motion_model.generate_last_state(
|
42 | 44 | p[0, 0] + h[0], p[1, 0], p[2, 0], k0)
|
43 | 45 | dp = calc_diff(target, [xp], [yp], [yawp])
|
@@ -68,7 +70,6 @@ def calc_J(target, p, h, k0):
|
68 | 70 |
|
69 | 71 |
|
70 | 72 | def selection_learning_param(dp, p, k0, target):
|
71 |
| - |
72 | 73 | mincost = float("inf")
|
73 | 74 | mina = 1.0
|
74 | 75 | maxa = 2.0
|
@@ -110,7 +111,7 @@ def optimize_trajectory(target, k0, p):
|
110 | 111 | print("path is ok cost is:" + str(cost))
|
111 | 112 | break
|
112 | 113 |
|
113 |
| - J = calc_J(target, p, h, k0) |
| 114 | + J = calc_j(target, p, h, k0) |
114 | 115 | try:
|
115 | 116 | dp = - np.linalg.inv(J) @ dc
|
116 | 117 | except np.linalg.linalg.LinAlgError:
|
|
0 commit comments