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

fix code for coveralls

Browse Source
This commit is contained in:
Atsushi Sakai
2019-02-02 16:51:00 +09:00
parent 1119587b15
commit 4880986bb5
24 changed files with 122 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
PathTracking/cgmres_nmpc/cgmres_nmpc.py
View File

@@ -126,8 +126,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
@@ -359,8 +359,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])
@@ -371,7 +371,7 @@ class NMPCController_with_CGMRES():
return np.array(F)
def plot_figures(plant_system, controller, iteration_num, dt):
def plot_figures(plant_system, controller, iteration_num, dt): # pragma: no cover
# figure
# time history
fig_p = plt.figure()
@@ -421,13 +421,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")
@@ -462,7 +462,7 @@ def plot_figures(plant_system, controller, iteration_num, dt):
plt.show()
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover
# Vehicle parameters
LENGTH = 0.4 # [m]
@@ -550,9 +550,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")

11
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
View File

@@ -105,9 +105,6 @@ def get_linear_model_matrix(v, phi, delta):
return A, B, C
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
@@ -276,8 +273,8 @@ def linear_mpc_control(xref, xbar, x0, dref):
if t < (T - 1):
cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)
constraints += [cvxpy.abs(u[1, t + 1] - u[1, t])
<= MAX_DSTEER * DT]
constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <=
MAX_DSTEER * DT]
cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf)
@@ -438,8 +435,8 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
plot_car(state.x, state.y, state.yaw, steer=di)
plt.axis("equal")
plt.grid(True)
plt.title("Time[s]:" + str(round(time, 2)) +
", speed[km/h]:" + str(round(state.v * 3.6, 2)))
plt.title("Time[s]:" + str(round(time, 2))
+ ", speed[km/h]:" + str(round(state.v * 3.6, 2)))
plt.pause(0.0001)
return t, x, y, yaw, v, d, a

6
PathTracking/move_to_pose/move_to_pose.py
View File

@@ -53,8 +53,8 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
# 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) -
theta + np.pi) % (2 * np.pi) - np.pi
alpha = (np.arctan2(y_diff, x_diff)
- theta + np.pi) % (2 * np.pi) - np.pi
beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
v = Kp_rho * rho
@@ -76,7 +76,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
plot_vehicle(x, y, theta, x_traj, y_traj)
def plot_vehicle(x, y, theta, x_traj, y_traj):
def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
# Corners of triangular vehicle when pointing to the right (0 radians)
p1_i = np.array([0.5, 0, 1]).T
p2_i = np.array([-0.5, 0.25, 1]).T