def __init__(self, parent=None):
super(Window, self).__init__(parent)
plt.style.use("ggplot")
# Field Canvas
self.canvas = FigureCanvas(plt.figure())
self.canvas.axes = self.canvas.figure.add_subplot(111)
plt.axis("equal")
plt.xlabel("X position (m)")
plt.ylabel("Y position (m)")
plt.xlim(-1, in2m(360) + 1)
plt.ylim(-1, in2m(180) + 1)
(self.line, ) = self.canvas.axes.plot(
[in2m(30)], [in2m(30)], marker="o",
color="cornflowerblue") # Start pos
self.lb = LineBuilder(self.line)
self.toolbar = NavigationToolbar(self.canvas, self)
self.canvas.setFocusPolicy(QtCore.Qt.ClickFocus)
self.canvas.setFocus()
# Obstacles List
obstaclesList = QListWidget()
for obstacle in PATH_OBSTACLES:
obstaclesList.addItem(QListWidgetItem(obstacle))
obstaclesList.itemClicked.connect(self.add_obstacles)
# Save Button
saveButton = QPushButton()
saveButton.setText("Save to JSON")
saveButton.clicked.connect(self.write_json)
# Layout
main_layout = QHBoxLayout()
plot_layout = QVBoxLayout()
plot_layout.addWidget(self.toolbar)
plot_layout.addWidget(self.canvas)
settings_layout = QVBoxLayout()
settings_layout.addWidget(obstaclesList)
settings_layout.addWidget(saveButton)
main_layout.addLayout(settings_layout)
main_layout.addLayout(plot_layout)
self.setLayout(main_layout)
self.canvas.draw()
def __init__(self):
# Geometry
self.WIDTH = in2m(34) # m
self.LENGTH = in2m(39) # m
self.TRACK_WIDTH = 0.7269198037390904 # m
self.WHEEL_DIA = in2m(6) # m
self.GEOMETRY = [
[
(self.LENGTH / 2, self.WIDTH / 2),
(self.LENGTH / 2, -self.WIDTH / 2),
], # Front
[
(-self.LENGTH / 2, self.WIDTH / 2),
(-self.LENGTH / 2, -self.WIDTH / 2),
], # Back
[
(self.LENGTH / 2, self.WIDTH / 2),
(-self.LENGTH / 2, self.WIDTH / 2),
], # Left
[
(self.LENGTH / 2, -self.WIDTH / 2),
(-self.LENGTH / 2, -self.WIDTH / 2),
], # Right
]
self.AXIS_SIZE = self.WIDTH / 4
self.OBSTACLE_BUFFER = DEFAULT_OBSTACLE_BUFFER
# Mass / Inertia
self.MU = DEFAULT_MU
self.MASS = 50.0 # kg
self.J = (
self.MASS * (self.LENGTH ** 2 + self.WIDTH ** 2)
) / 12.0 # Moment of inertia
# Motors
self.MOTORS_PER_SIDE = 2
self.MOTOR_MAX_TORQUE = DEFAULT_MAX_TORQUE
self.MOTOR_MAX_RPM = DEFAULT_MAX_RPM
self.DRIVE_RATIO = 9.5625 # 9.5625:1
self.WHEEL_40A_FORCE = (
self.MOTORS_PER_SIDE
* self.MOTOR_MAX_TORQUE
* self.DRIVE_RATIO
/ (self.WHEEL_DIA / 2)
)
def plan(robot=Robot(), plot=False):
N = 200 # Number of control intervals
OBSTACLES = create_obstacles("barrel-racing")
FINISH_LINE_BUFFER = 0.1
# Setup Optimization
opti = ca.Opti()
# State variables
X = opti.variable(len(StateVars), N + 1)
xpos = X[StateVars.xIdx.value, :] # X position
ypos = X[StateVars.yIdx.value, :] # Y-position
theta = X[StateVars.thetaIdx.value, :] # Theta
vl = X[StateVars.vlIdx.value, :] # Left wheel velocity
vr = X[StateVars.vrIdx.value, :] # Right wheel velocity
al = X[StateVars.alIdx.value, :] # Left wheel acceleration
ar = X[StateVars.arIdx.value, :] # Right wheel acceleration
# Control variables
U = opti.variable(len(ControlVars), N)
jl = U[ControlVars.jlIdx.value, :] # Left wheel jerk
jr = U[ControlVars.jrIdx.value, :] # Right wheel jerk
# Total time variable
T = opti.variable()
dt = T / N # length of one control interval
# Minimize time
opti.minimize(T)
# Apply dynamic constriants
for k in range(N):
x_next = X[:, k] + robot.dynamics_model(X[:, k], U[:, k]) * dt
opti.subject_to(X[:, k + 1] == x_next)
# Wheel constraints
robot.apply_wheel_constraints(opti, vl, vr, al, ar, jl, jr)
# Boundary conditions
# Start
opti.subject_to(xpos[0] == in2m(60) - robot.LENGTH / 2)
opti.subject_to(ypos[0] == in2m(90))
opti.subject_to(theta[0] == 0)
opti.subject_to(vl[0] == 0)
opti.subject_to(vr[0] == 0)
opti.subject_to(al[0] == 0)
opti.subject_to(ar[0] == 0)
opti.subject_to(jl[0] == 0)
opti.subject_to(jr[0] == 0)
# End
robot.apply_finish_line_constraints(
opti,
xpos[-1],
ypos[-1],
theta[-1],
(
(in2m(60) - FINISH_LINE_BUFFER, in2m(60)),
(in2m(60) - FINISH_LINE_BUFFER, in2m(120)),
),
"left",
)
# Obstacles
robot.apply_obstacle_constraints(opti, xpos, ypos, theta, OBSTACLES)
# Time constraints
opti.subject_to(T >= 0)
# Compute initial guess from init traj
x_init, y_init, theta_init = load_init_json("init_traj/barrel_racing.json",
(in2m(30), in2m(90), 0.0), N)
# Initial guess
opti.set_initial(xpos, x_init)
opti.set_initial(ypos, y_init)
opti.set_initial(theta, theta_init)
opti.set_initial(vl, 0)
opti.set_initial(vr, 0)
opti.set_initial(al, 0)
opti.set_initial(ar, 0)
opti.set_initial(jl, 0)
opti.set_initial(jr, 0)
opti.set_initial(T, 10)
if plot:
# Plot initialization
plot_traj(
"Initial Trajectory",
x_init,
y_init,
theta_init,
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
)
# Solve non-linear program
opti.solver("ipopt", {}, {"mu_init": 1e-3}) # set numerical backend
sol = opti.solve()
if plot:
# Plot result without wheel force limits
plot_traj(
"Before Wheel Force Limits",
sol.value(xpos),
sol.value(ypos),
sol.value(theta),
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
)
# Solve the problem again, but this time with wheel force & friction limit constraints
robot.apply_wheel_force_constraints(opti, al, ar)
robot.apply_wheel_friction_constraints(opti, vl, vr, al, ar)
# Copy over X, U, and T to initialize
opti.set_initial(X, sol.value(X))
opti.set_initial(U, sol.value(U))
opti.set_initial(T, sol.value(T))
sol = opti.solve()
times = np.linspace(0, sol.value(T), N)
if plot:
# Plot final result
plot_traj(
"Final Result",
sol.value(xpos),
sol.value(ypos),
sol.value(theta),
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
)
plt.figure()
plot_wheel_vel_accel_jerk(
times,
sol.value(vl)[:-1],
sol.value(vr)[:-1],
sol.value(al)[:-1],
sol.value(ar)[:-1],
sol.value(jl),
sol.value(jr),
)
lon_fl, lon_fr = robot.get_longitudinal_wheel_forces(al, ar)
lat_f = robot.get_lateral_wheel_force(vl, vr)
plt.figure()
plot_wheel_forces(
times,
sol.value(lon_fl)[:-1],
sol.value(lon_fr)[:-1],
sol.value(lat_f)[:-1],
)
plt.figure()
plot_total_force(
times,
np.sqrt(sol.value(lon_fl)**2 + sol.value(lat_f)**2)[:-1],
np.sqrt(sol.value(lon_fr)**2 + sol.value(lat_f)**2)[:-1],
)
interp_time = 0.02 # seconds
interp_x = interp_state_vector(times, sol.value(xpos), interp_time)
interp_y = interp_state_vector(times, sol.value(ypos), interp_time)
interp_theta = interp_state_vector(times, sol.value(theta),
interp_time)
plot_traj(
"Interp",
interp_x,
interp_y,
interp_theta,
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
save_png=True,
)
anim = anim_traj(
"Final Result",
interp_x,
interp_y,
interp_theta,
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
20, # milliseconds
save_gif=False,
)
plt.show()
print(f"Trajectory time: {sol.value(T)} seconds")