github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2019-02-02 15:23:12 +09:00
parent f9a4fc1580
commit b4de4a1db7
12 changed files with 46 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
PathTracking/cgmres_nmpc/cgmres_nmpc.py
View File

@@ -127,8 +127,8 @@ class NMPCSimulatorSystem():
pre_lam_3 = lam_3 + dt * \
(- lam_1 * math.sin(yaw) * v + lam_2 * math.cos(yaw) * v)
pre_lam_4 = lam_4 + dt * \
(lam_1 * math.cos(yaw) + lam_2 *
math.sin(yaw) + lam_3 * math.sin(u_2) / WB)
(lam_1 * math.cos(yaw) + lam_2
* math.sin(yaw) + lam_3 * math.sin(u_2) / WB)
return pre_lam_1, pre_lam_2, pre_lam_3, pre_lam_4
@@ -275,6 +275,8 @@ class NMPCController_with_CGMRES():
e = np.zeros((self.max_iteration + 1, 1))
e[0] = 1.0
ys_pre = None
for i in range(self.max_iteration):
du_1 = vs[::self.input_num, i] * self.ht
du_2 = vs[1::self.input_num, i] * self.ht
@@ -358,8 +360,8 @@ class NMPCController_with_CGMRES():
for i in range(N):
# ∂H/∂u(xi, ui, λi)
F.append(u_1s[i] + lam_4s[i] + 2.0 * raw_1s[i] * u_1s[i])
F.append(u_2s[i] + lam_3s[i] * v_s[i] /
WB * math.cos(u_2s[i])**2 + 2.0 * raw_2s[i] * u_2s[i])
F.append(u_2s[i] + lam_3s[i] * v_s[i]
/ WB * math.cos(u_2s[i])**2 + 2.0 * raw_2s[i] * u_2s[i])
F.append(-PHI_V + 2.0 * raw_1s[i] * dummy_u_1s[i])
F.append(-PHI_OMEGA + 2.0 * raw_2s[i] * dummy_u_2s[i])
@@ -420,13 +422,13 @@ def plot_figures(plant_system, controller, iteration_num, dt):
u_2_fig.set_xlabel("time [s]")
u_2_fig.set_ylabel("u_omega")
dummy_1_fig.plot(np.arange(iteration_num - 1) *
dt, controller.history_dummy_u_1)
dummy_1_fig.plot(np.arange(iteration_num - 1)
* dt, controller.history_dummy_u_1)
dummy_1_fig.set_xlabel("time [s]")
dummy_1_fig.set_ylabel("dummy u_1")
dummy_2_fig.plot(np.arange(iteration_num - 1) *
dt, controller.history_dummy_u_2)
dummy_2_fig.plot(np.arange(iteration_num - 1)
* dt, controller.history_dummy_u_2)
dummy_2_fig.set_xlabel("time [s]")
dummy_2_fig.set_ylabel("dummy u_2")
@@ -476,8 +478,8 @@ def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
[-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH
- TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])
[-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH -
TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])
rr_wheel = np.copy(fr_wheel)
@@ -549,9 +551,9 @@ def animation(plant, controller, dt):
plot_car(x, y, yaw, steer=steer)
plt.axis("equal")
plt.grid(True)
plt.title("Time[s]:" + str(round(time, 2)) +
", accel[m/s]:" + str(round(accel, 2)) +
", speed[km/h]:" + str(round(v * 3.6, 2)))
plt.title("Time[s]:" + str(round(time, 2))
+ ", accel[m/s]:" + str(round(accel, 2))
+ ", speed[km/h]:" + str(round(v * 3.6, 2)))
plt.pause(0.0001)
plt.close("all")

8
PathTracking/move_to_pose/move_to_pose.py
View File

@@ -48,11 +48,9 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
x_diff = x_goal - x
y_diff = y_goal - y
"""
Restrict alpha and beta (angle differences) to the range
[-pi, pi] to prevent unstable behavior e.g. difference going
from 0 rad to 2*pi rad with slight turn
"""
# Restrict alpha and beta (angle differences) to the range
# [-pi, pi] to prevent unstable behavior e.g. difference going
# from 0 rad to 2*pi rad with slight turn
rho = np.sqrt(x_diff**2 + y_diff**2)
alpha = (np.arctan2(y_diff, x_diff) -