def main():
dt = 0.1
T = np.arange(0, 6, dt)
plt.figure(1)
plt.xticks(np.arange(min(T), max(T) + 1, 1.0))
plt.xlabel("Time (s)")
plt.ylabel("Step response")
tf = ct.TransferFunction(1, [1, -0.6], dt)
sim(tf, T, "Single pole in RHP")
tf = ct.TransferFunction(1, [1, 0.6], dt)
sim(tf, T, "Single pole in LHP")
if "--noninteractive" in sys.argv:
latex.savefig("z_oscillations_1p")
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Step response")
den = [np.real(x) for x in conv([1, 0.6 + 0.6j], [1, 0.6 - 0.6j])]
tf = ct.TransferFunction(1, den, dt)
sim(tf, T, "Complex conjugate poles in LHP")
den = [np.real(x) for x in conv([1, -0.6 + 0.6j], [1, -0.6 - 0.6j])]
tf = ct.TransferFunction(1, den, dt)
sim(tf, T, "Complex conjugate poles in RHP")
if "--noninteractive" in sys.argv:
latex.savefig("z_oscillations_2p")
else:
plt.show()
def main():
dt = 0.0001
T = np.arange(0, 0.25, dt)
# Make plant
J = 3.2284e-6 # kg-m^2
b = 3.5077e-6 # N-m-s
Ke = 0.0181 # V/rad/s
Kt = 0.0181 # N-m/Amp
K = Ke # Ke = Kt
R = 0.0902 # Ohms
L = 230e-6 # H
# Unstable plant
# s((Js + b)(Ls + R) + K^2)
# s(JLs^2 + JRs + bLs + bR + K^2)
# JLs^3 + JRs^2 + bLs^2 + bRs + K^2s
# JLs^3 + (JR + bL)s^2 + (bR + K^2)s
G = ct.TransferFunction(K, [J * L, J * R + b * L, b * R + K**2, 0])
ct.root_locus(G, grid=True)
plt.xlabel("Real Axis (seconds$^{-1}$)")
plt.ylabel("Imaginary Axis (seconds$^{-1}$)")
if "--noninteractive" in sys.argv:
latex.savefig("highfreq_unstable_rlocus")
plt.figure(2)
plt.xlabel("Time ($s$)")
plt.ylabel("Position ($m$)")
sim(ct.TransferFunction(1, 1), T, "Reference")
Gcl = make_closed_loop_plant(G, 1)
sim(Gcl, T, "Step response")
if "--noninteractive" in sys.argv:
latex.savefig("highfreq_unstable_step")
else:
plt.show()
def main():
dt = 0.00505
elevator = Elevator(dt)
# Set up graphing
l0 = 0.1
l1 = l0 + 5.0
l2 = l1 + 0.1
t = np.arange(0, l2 + 5.0, dt)
t, xprof, vprof, aprof = fct.generate_trapezoid_profile(
max_v=0.5, time_to_max_v=0.5, dt=dt, goal=3
)
refs = []
# Generate references for simulation
for i in range(len(t)):
r = np.array([[xprof[i]], [vprof[i]]])
refs.append(r)
x_rec, ref_rec, u_rec, y_rec = elevator.generate_time_responses(t, refs)
latex.plot_time_responses(
elevator, t, x_rec, ref_rec, u_rec, 2, use_pid_labels=True
)
if "--noninteractive" in sys.argv:
latex.savefig("pd_controller")
else:
plt.show()
def main():
x, y = np.mgrid[-20.0:20.0:250j, -20.0:20.0:250j]
# Need an (N, 2) array of (x, y) pairs.
xy = np.column_stack([x.flat, y.flat])
z = np.zeros(xy.shape[0])
for i, pair in enumerate(xy):
s = pair[0] + pair[1] * 1j
h = (s - 9 + 9j) * (s - 9 - 9j) / (s * (s + 10))
z[i] = clamp(math.sqrt(h.real**2 + h.imag**2), -30, 30)
z = z.reshape(x.shape)
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
ax.plot_surface(x, y, z, cmap=cm.coolwarm)
ax.set_xlabel("$Re(\sigma)$")
ax.set_ylabel("$Im(j\omega)$")
ax.set_zlabel("$H(s)$")
ax.set_zticks([])
if "--noninteractive" in sys.argv:
latex.savefig("tf_3d")
else:
plt.show()
def main():
dt = 0.02
vs = np.arange(-1.1, 1.1, 0.01)
K_rec = np.zeros((2, 3, len(vs)))
for i, v in enumerate(vs):
x_linear = np.array([[0], [0], [0]])
u_linear = np.array([[v], [v]])
K_rec[:, :, i] = Unicycle(dt, x_linear, u_linear).K
state_labels = ["$x$", "$y$", "$\\theta$"]
input_labels = ["Velocity", "Angular velocity"]
for i in range(len(state_labels)):
plt.figure(i + 1)
plt.plot(vs, K_rec[0, i, :], label=f"{input_labels[0]}")
plt.plot(vs, K_rec[1, i, :], label=f"{input_labels[1]}")
plt.xlabel("v (m/s)")
plt.ylabel(f"Gain {state_labels[i]} error to input")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig(f"ltv_unicycle_cascaded_lqr_{i}")
if "--noninteractive" not in sys.argv:
plt.show()
def main():
# Set up graphing
l0 = 0.1
l1 = l0 + 5.0
l2 = l1 + 0.1
t = np.arange(0, l2 + 5.0, DT)
refs = []
# Generate references for simulation
for i in range(len(t)):
if t[i] < l0:
r = np.array([[0.0], [0.0]])
elif t[i] < l1:
r = np.array([[1.524], [0.0]])
else:
r = np.array([[0.0], [0.0]])
refs.append(r)
elevator = Elevator(DT)
x_rec, ref_rec, u_rec, y_rec = elevator.generate_time_responses(t, refs)
latex.plot_time_responses(elevator, t, x_rec, ref_rec, u_rec, 2)
if "--noninteractive" in sys.argv:
latex.savefig("elevator_time_delay_no_comp")
elevator = Elevator(DT, latency_comp=True)
x_rec, ref_rec, u_rec, y_rec = elevator.generate_time_responses(t, refs)
latex.plot_time_responses(elevator, t, x_rec, ref_rec, u_rec, 2)
if "--noninteractive" in sys.argv:
latex.savefig("elevator_time_delay_comp")
else:
plt.show()
def main():
dt = 0.02
vs = np.arange(-1.1, 1.1, 0.01)
K_rec = np.zeros((2, 5, len(vs)))
Kapprox_rec = np.zeros((2, 5, len(vs)))
for i, v in enumerate(vs):
x_linear = np.array([[0], [0], [0], [v], [v]])
K_rec[:, :, i] = DifferentialDrive(dt, x_linear).K
Kapprox_rec[:, :, i] = ApproxDifferentialDrive(dt, x_linear).K
state_labels = ["$x$", "$y$", "$\\theta$", "$v_l$", "$v_r$"]
input_labels = ["Left voltage", "Right voltage"]
for i in range(len(state_labels)):
plt.figure(i + 1)
plt.plot(vs, K_rec[0, i, :], label=f"{input_labels[0]}")
plt.plot(vs,
Kapprox_rec[0, i, :],
label=f"Approx {input_labels[0].lower()}")
plt.plot(vs, K_rec[1, i, :], label=f"{input_labels[1]}")
plt.plot(vs,
Kapprox_rec[1, i, :],
label=f"Approx {input_labels[1].lower()}")
plt.xlabel("v (m/s)")
plt.ylabel(f"Gain from {state_labels[i]} error to input")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig(f"ltv_diff_drive_lqr_fit_{i}")
if "--noninteractive" not in sys.argv:
plt.show()
def main():
# Set up graphing
l0 = 0.1
l1 = l0 + 5.0
l2 = l1 + 0.1
t = np.arange(0, l2 + 5.0, DT)
refs = []
# Generate references for simulation
for i in range(len(t)):
if t[i] < l0:
r = np.array([[0]])
elif t[i] < l1:
r = np.array([[2]])
else:
r = np.array([[0]])
refs.append(r)
flywheel = DrivetrainVelocitySystem(DT)
x_rec, ref_rec, u_rec, y_rec = flywheel.generate_time_responses(t, refs)
latex.plot_time_responses(flywheel, t, x_rec, ref_rec, u_rec, 2)
if "--noninteractive" in sys.argv:
latex.savefig("drivetrain_time_delay_no_comp")
flywheel = DrivetrainVelocitySystem(DT, latency_comp=True)
x_rec, ref_rec, u_rec, y_rec = flywheel.generate_time_responses(t, refs)
latex.plot_time_responses(flywheel, t, x_rec, ref_rec, u_rec, 8)
if "--noninteractive" in sys.argv:
latex.savefig("drivetrain_time_delay_comp")
else:
plt.show()
def main():
dt = 0.0001
T = np.arange(0, 6, dt)
plt.xlabel("Time ($s$)")
plt.ylabel("Position ($m$)")
# Make plant
G = ct.TransferFunction(1, conv([1, 5], [1, 0]))
sim(ct.TransferFunction(1, 1), T, "Setpoint")
K = ct.TransferFunction(120, 1)
Gcl = ct.feedback(G, K)
sim(Gcl, T, "Underdamped")
K = ct.TransferFunction(3, 1)
Gcl = ct.feedback(G, K)
sim(Gcl, T, "Overdamped")
K = ct.TransferFunction(6.268, 1)
Gcl = ct.feedback(G, K)
sim(Gcl, T, "Critically damped")
if "--noninteractive" in sys.argv:
latex.savefig("pid_responses")
else:
plt.show()
def main():
dt = 0.00505
flywheel = Flywheel(dt)
# Set up graphing
l0 = 0.1
l1 = l0 + 5.0
l2 = l1 + 0.1
t = np.arange(0, l2 + 5.0, dt)
refs = []
# Generate references for simulation
for i in range(len(t)):
if t[i] < l0:
r = np.array([[0]])
elif t[i] < l1:
r = np.array([[9000 / 60 * 2 * math.pi]])
else:
r = np.array([[0]])
refs.append(r)
plt.figure(1)
x_rec, ref_rec, u_rec, y_rec = flywheel.generate_time_responses(t, refs)
plt.ylabel(flywheel.state_labels[0])
plt.plot(t, flywheel.extract_row(x_rec, 0), label="Output")
plt.plot(t, flywheel.extract_row(ref_rec, 0), label="Setpoint")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("pi_controller_ss_error_overshoot")
else:
plt.show()
def main():
t = []
refs = []
# Radius of robot in meters
rb = 0.59055 / 2.0
with open("ramsete_traj.csv", "r") as trajectory_file:
import csv
current_t = 0
reader = csv.reader(trajectory_file, delimiter=",")
trajectory_file.readline()
for row in reader:
t.append(float(row[0]))
x = float(row[1])
y = float(row[2])
theta = float(row[3])
vl, vr = get_diff_vels(float(row[4]), float(row[5]), rb * 2.0)
ref = np.array([[x], [y], [theta], [vl], [vr]])
refs.append(ref)
dt = 0.02
x = np.array([[refs[0][0, 0] + 0.5], [refs[0][1, 0] + 0.5], [np.pi / 2],
[0], [0]])
diff_drive = DifferentialDrive(dt, x)
state_rec, ref_rec, u_rec, y_rec = diff_drive.generate_time_responses(
t, refs)
plt.figure(1)
x_rec = np.squeeze(np.asarray(state_rec[0, :]))
y_rec = np.squeeze(np.asarray(state_rec[1, :]))
plt.plot(x_rec, y_rec, label="LTV controller")
plt.plot(ref_rec[0, :], ref_rec[1, :], label="Reference trajectory")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
# Equalize aspect ratio
xlim = plt.xlim()
width = abs(xlim[0]) + abs(xlim[1])
ylim = plt.ylim()
height = abs(ylim[0]) + abs(ylim[1])
if width > height:
plt.ylim([-width / 2, width / 2])
else:
plt.xlim([-height / 2, height / 2])
if "--noninteractive" in sys.argv:
latex.savefig("ltv_diff_drive_nonrotated_firstorder_xy")
diff_drive.plot_time_responses(t, state_rec, ref_rec, u_rec)
if "--noninteractive" in sys.argv:
latex.savefig("ltv_diff_drive_nonrotated_firstorder_response")
else:
plt.show()
def main():
plt.figure(1)
draw_s_plane()
if "--noninteractive" in sys.argv:
latex.savefig("s_plane")
plt.figure(2)
draw_z_plane()
if "--noninteractive" in sys.argv:
latex.savefig("z_plane")
else:
plt.show()
def main():
# 1
# G(s) = --------------
# (s + 2)(s + 3)
G = ct.tf(1, conv([1, 0], [1, 2], [1, 3]))
ct.root_locus(G, grid=True)
plt.title("Root Locus")
plt.xlabel("Real Axis (seconds$^{-1}$)")
plt.ylabel("Imaginary Axis (seconds$^{-1}$)")
if "--noninteractive" in sys.argv:
latex.savefig("rlocus_asymptotes")
else:
plt.show()
def main():
# 1
# G(s) = -----
# s - 1
G = ct.tf([1], [1, -1])
ct.root_locus(G, grid=True)
plt.title("Root Locus")
plt.xlabel("Real Axis (seconds$^{-1}$)")
plt.ylabel("Imaginary Axis (seconds$^{-1}$)")
if "--noninteractive" in sys.argv:
latex.savefig("rlocus_infty")
else:
plt.show()
def main():
# (s - 9 + 9i)(s - 9 - 9i)
# G(s) = ------------------------
# s(s + 10)
G = ct.tf(conv([1, -9 + 9j], [1, -9 - 9j]), conv([1, 0], [1, 10]))
ct.root_locus(G, grid=True)
# Show plot
plt.title("Root Locus")
plt.xlabel("Real Axis (seconds$^{-1}$)")
plt.ylabel("Imaginary Axis (seconds$^{-1}$)")
plt.gca().set_aspect("equal")
if "--noninteractive" in sys.argv:
latex.savefig("rlocus_zeroes")
else:
plt.show()
def main():
xlim = [-5, 5]
xs = np.arange(xlim[0], xlim[1], 0.001)
plt.xlim(xlim)
plt.ylim([-2, 20])
plt.plot(xs, [math.exp(x) for x in xs], label="$e^t$")
for i in range(5, -1, -1):
plt.plot(
xs,
[taylor_exp(x, i) for x in xs],
label="Taylor series of $e^t$ ($n = " + str(i) + "$)",
)
plt.xlabel("$t$")
plt.ylabel("$f(t)$")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("taylor_series")
else:
plt.show()
def main():
themecolor = "#386ba6"
x1 = np.arange(-7.5, 9.5, 0.001)
plt.plot(x1, norm.pdf(x1, loc=1, scale=2))
x2 = np.arange(2.0, 2.5, 0.001)
plt.fill_between(x2, [0] * len(x2),
norm.pdf(x2, loc=1, scale=2),
facecolor=themecolor,
alpha=0.5)
plt.plot([2.0, 2.0], [0.0, norm.pdf([2.0], loc=1, scale=2)],
color=themecolor)
plt.plot([2.5, 2.5], [0.0, norm.pdf([2.5], loc=1, scale=2)],
color=themecolor)
# Left arrow
plt.annotate(
"$x_1$",
xy=(2.0, 0.025), # Start coord of arrow
xycoords="data",
xytext=(2.0 - 1.0, 0.025), # End coord of arrow
textcoords="data",
arrowprops=dict(arrowstyle="->", connectionstyle="arc3,rad=0"),
ha="center",
va="center",
)
plt.annotate(
"$dx$",
xy=(2.25, -0.005),
xycoords="data",
ha="center",
va="center",
)
plt.xlabel("$x$")
plt.ylabel("$p(x)$")
if "--noninteractive" in sys.argv:
latex.savefig("pdf")
else:
plt.show()
def main():
x = np.arange(1 / 3, 3, 0.001)
y1 = [1 / val for val in x]
y2 = [3 for val in x]
plt.plot(x, y1, label="LQR")
plt.fill_between(x,
y1,
y2,
color=(1, 0.5, 0.05),
alpha=0.5,
label="Pole placement")
plt.xlabel("$\Vert u^TRu \Vert_2$")
plt.ylabel("$\Vert x^TQx \Vert_2$")
plt.xticks([])
plt.yticks([])
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("pareto_boundary")
else:
plt.show()
def main():
t, x, v, a = fct.generate_trapezoid_profile(max_v=7.0,
time_to_max_v=2.0,
dt=0.05,
goal=50.0)
plt.figure(1)
plt.subplot(3, 1, 1)
plt.ylabel("Position (m)")
plt.plot(t, x, label="Position")
plt.subplot(3, 1, 2)
plt.ylabel("Velocity (m/s)")
plt.plot(t, v, label="Velocity")
plt.subplot(3, 1, 3)
plt.xlabel("Time (s)")
plt.ylabel("Acceleration ($m/s^2$)")
plt.plot(t, a, label="Acceleration")
if "--noninteractive" in sys.argv:
latex.savefig("trapezoid_profile")
t, x, v, a = fct.generate_s_curve_profile(max_v=7.0,
max_a=3.5,
time_to_max_a=1.0,
dt=0.05,
goal=50.0)
plt.figure(2)
plt.subplot(3, 1, 1)
plt.ylabel("Position (m)")
plt.plot(t, x, label="Position")
plt.subplot(3, 1, 2)
plt.ylabel("Velocity (m/s)")
plt.plot(t, v, label="Velocity")
plt.subplot(3, 1, 3)
plt.xlabel("Time (s)")
plt.ylabel("Acceleration ($m/s^2$)")
plt.plot(t, a, label="Acceleration")
if "--noninteractive" in sys.argv:
latex.savefig("s_curve_profile")
else:
plt.show()
def main():
x, y = np.mgrid[-1.0:1.0:30j, 0.0:2.0:30j]
# Need an (N, 2) array of (x, y) pairs.
xy = np.column_stack([x.flat, y.flat])
mu = np.array([0.0, 1.0])
sigma = np.array([0.5, 0.5])
covariance = np.diag(sigma**2)
z = multivariate_normal.pdf(xy, mean=mu, cov=covariance).reshape(x.shape)
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
ax.plot_surface(x, y, z)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
ax.set_zlabel("$p(x, y)$")
if "--noninteractive" in sys.argv:
latex.savefig("joint_pdf")
else:
plt.show()
def main():
dt = 0.00505
sample_period = 0.1
elevator = Elevator(dt)
# Set up graphing
l0 = 0.1
l1 = l0 + 5.0
l2 = l1 + 0.1
l3 = l2 + 1.0
t = np.arange(0, l3, dt)
refs = []
# Generate references for simulation
for i in range(len(t)):
if t[i] < l0:
r = np.array([[0.0], [0.0]])
elif t[i] < l1:
r = np.array([[1.524], [0.0]])
else:
r = np.array([[0.0], [0.0]])
refs.append(r)
state_rec, ref_rec, u_rec, y_rec = elevator.generate_time_responses(
t, refs)
pos = elevator.extract_row(state_rec, 0)
plt.figure(1)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.plot(t, pos, label="Continuous")
y = generate_zoh(pos, dt, sample_period)
plt.plot(t, y, label="Zero-order hold (T={}s)".format(sample_period))
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("zoh")
else:
plt.show()
def main():
x, y = np.mgrid[-20.0:20.0:250j, -20.0:20.0:250j]
# Need an (N, 2) array of (x, y) pairs.
xy = np.column_stack([x.flat, y.flat])
z = np.zeros(xy.shape[0])
for i, pair in enumerate(xy):
z[i] = func(pair[0], pair[1])
z = z.reshape(x.shape)
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
ax.plot_surface(x, y, z, cmap=cm.coolwarm)
ax.set_xlabel("$Re(\sigma)$")
ax.set_ylabel("$Im(j\omega)$")
ax.set_zlabel("$H(s)$")
ax.set_zticks([])
if "--noninteractive" in sys.argv:
latex.savefig("tf_3d")
else:
plt.show()
def main():
xlim = [0, 3]
x = np.arange(xlim[0], xlim[1], 0.001)
plt.xlim(xlim)
y1 = np.sin(2 * math.pi * x)
plt.plot(x, y1, label="Signal at $1$ Hz")
y2 = np.sin(2 * math.pi * x / 2 + math.pi / 4)
plt.plot(x, y2, color="k", linestyle="--", label="Signal at $2$ Hz")
# Draw intersection points
idx = np.argwhere(np.diff(np.sign(y1 - y2)) != 0)
plt.plot(x[idx], y1[idx], "ko", label="Samples at $3$ Hz")
plt.xlabel("$t$")
plt.ylabel("$f(t)$")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("nyquist")
else:
plt.show()
def main():
elevator = Elevator(0.1)
plt.figure(1)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
simulate(elevator, 0.1, "zoh")
simulate(elevator, 0.1, "euler")
simulate(elevator, 0.1, "backward_diff")
simulate(elevator, 0.1, "bilinear")
plt.ylim([-2, 3])
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("sampling_simulation_010")
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
simulate(elevator, 0.05, "zoh")
simulate(elevator, 0.05, "euler")
simulate(elevator, 0.05, "backward_diff")
simulate(elevator, 0.05, "bilinear")
plt.ylim([-2, 3])
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("sampling_simulation_005")
plt.figure(3)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
simulate(elevator, 0.01, "zoh")
simulate(elevator, 0.01, "euler")
simulate(elevator, 0.01, "backward_diff")
simulate(elevator, 0.01, "bilinear")
plt.ylim([-0.25, 2])
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("sampling_simulation_004")
else:
plt.show()
def main():
xlim = [-math.pi, math.pi]
ylim = [-1, 6.5]
# Figure 1
plt.figure()
ax = plt.gca()
config_plot(xlim, ylim)
# Draw invalid region
ax.add_patch(make_invalid_region(xlim, ylim))
# Draw valid region
ax.add_patch(
make_valid_region([
(-3 / 4 * math.pi, 0),
(3 / 4 * math.pi, 0),
(3 / 4 * math.pi, 5),
(-3 / 4 * math.pi, 5),
]))
ax.legend()
if "--noninteractive" in sys.argv:
latex.savefig("configuration_spaces_fig1")
# Figure 2
plt.figure()
ax = plt.gca()
config_plot(xlim, ylim)
# Draw invalid region
ax.add_patch(make_invalid_region(xlim, ylim))
# Draw valid region
ax.add_patch(
make_valid_region([
(-3 / 4 * math.pi, 0),
(-math.pi / 4, 0),
(-math.pi / 4, 2),
(math.pi / 4, 2),
(math.pi / 4, 0),
(3 / 4 * math.pi, 0),
(3 / 4 * math.pi, 5),
(-3 / 4 * math.pi, 5),
]))
ax.legend()
if "--noninteractive" in sys.argv:
latex.savefig("configuration_spaces_fig2")
# Figure 3
plt.figure()
ax = plt.gca()
config_plot(xlim, ylim)
# Draw invalid region
ax.add_patch(make_invalid_region(xlim, ylim))
# Draw valid region
ax.add_patch(
make_valid_region([
(-3 / 4 * math.pi, 0),
(-math.pi / 4, 0),
(-math.pi / 4, 2),
(math.pi / 4, 2),
(math.pi / 4, 0),
(3 / 4 * math.pi, 0),
(3 / 4 * math.pi, 5),
(-3 / 4 * math.pi, 5),
]))
# Draw start and end points
draw_point(ax, -1, 0.5, label="x")
draw_point(ax, 1, 0.5, label="r")
ax.legend()
if "--noninteractive" in sys.argv:
latex.savefig("configuration_spaces_fig3")
# Figure 4
plt.figure()
ax = plt.gca()
config_plot(xlim, ylim)
# Draw invalid region
ax.add_patch(make_invalid_region(xlim, ylim))
# Draw valid region
ax.add_patch(
make_valid_region([
(-3 / 4 * math.pi, 0),
(-math.pi / 4, 0),
(-math.pi / 4, 2),
(math.pi / 4, 2),
(math.pi / 4, 0),
(3 / 4 * math.pi, 0),
(3 / 4 * math.pi, 5),
(-3 / 4 * math.pi, 5),
]))
# Draw path between start and goal
points = [(-1, 0.5), (-math.pi / 4, 2), (math.pi / 4, 2), (1, 0.5)]
ax.plot([p[0] for p in points], [p[1] for p in points], color="C0")
# Draw start and end points
draw_point(ax, -1, 0.5, label="x")
draw_point(ax, -math.pi / 4, 2, label="a", verticalalignment="bottom")
draw_point(ax, math.pi / 4, 2, label="b", verticalalignment="bottom")
draw_point(ax, 1, 0.5, label="r")
ax.legend()
if "--noninteractive" in sys.argv:
latex.savefig("configuration_spaces_fig4")
else:
plt.show()
def main():
f_f = 349.23
f_a = 440
f_c = 261.63
T = 0.000001
xlim = [0, 0.05]
ylim = [-3, 3]
x = np.arange(xlim[0], xlim[1], T)
plt.xlim(xlim)
plt.ylim(ylim)
yf = np.sin(f_f * 2 * math.pi * x)
ya = np.sin(f_a * 2 * math.pi * x)
yc = np.sin(f_c * 2 * math.pi * x)
ysum = yf + ya + yc
ysum_attenuating = ysum * np.exp(-25 * x)
num_plots = 2
plt.subplot(num_plots, 1, 1)
plt.ylim(ylim)
plt.ylabel("Fmaj4 ($\sigma = 0$)")
plt.plot(x, ysum)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 2)
plt.ylim(ylim)
plt.ylabel("Attenuating Fmaj4 ($\sigma = -25$)")
plt.plot(x, ysum_attenuating)
plt.gca().axes.get_xaxis().set_ticks([])
plt.xlabel("$t$")
if "--noninteractive" in sys.argv:
latex.savefig("laplace_chord_attenuating")
x, y = np.mgrid[-150.0:150.0:500j, 200.0:500.0:500j]
# Need an (N, 2) array of (x, y) pairs.
xy = np.column_stack([x.flat, y.flat])
z = np.zeros(xy.shape[0])
for i, pair in enumerate(xy):
s = pair[0] + pair[1] * 1j
h = sin_tf(f_f, s) * sin_tf(f_a, s) * sin_tf(f_c, s)
z[i] = clamp(math.sqrt(h.real**2 + h.imag**2), -30, 30)
z = z.reshape(x.shape)
fig = plt.figure(2)
ax = fig.add_subplot(111, projection="3d")
ax.plot_surface(x, y, z, cmap=cm.coolwarm)
ax.set_xlabel("$Re(\sigma)$")
ax.set_ylabel("$Im(j\omega)$")
ax.set_zlabel("$H(s)$")
ax.set_zticks([])
if "--noninteractive" in sys.argv:
latex.savefig("laplace_chord_3d")
else:
plt.show()
def main():
dt = 0.00505
elevator = Elevator(dt)
# Set up graphing
l0 = 0.1
l1 = l0 + 5.0
l2 = l1 + 0.1
t = np.arange(0, l2 + 5.0, dt)
t, xprof, vprof, aprof = fct.generate_trapezoid_profile(
max_v=0.5, time_to_max_v=0.5, dt=dt, goal=3
)
refs = []
# Generate references for simulation
for i in range(len(t)):
r = np.array([[xprof[i]], [vprof[i]]])
refs.append(r)
x_rec, ref_rec, u_rec, y_rec = elevator.generate_time_responses(t, refs)
plt.figure()
subplot_max = elevator.sysd.states + elevator.sysd.inputs
for i in range(elevator.sysd.states):
plt.subplot(subplot_max, 1, i + 1)
if elevator.sysd.states + elevator.sysd.inputs > 3:
plt.ylabel(
elevator.state_labels[i],
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
else:
plt.ylabel(elevator.state_labels[i])
label = "Output"
if i == 0:
label += f" ($K_p = {round(elevator.K[0, 0], 2)}$)"
elif i == 1:
label += f" ($K_d = {round(elevator.K[0, 1], 2)}$)"
plt.plot(t, elevator.extract_row(x_rec, i), label=label)
plt.plot(t, elevator.extract_row(ref_rec, i), label="Setpoint")
plt.legend()
for i in range(elevator.sysd.inputs):
plt.subplot(subplot_max, 1, elevator.sysd.states + i + 1)
if elevator.sysd.states + elevator.sysd.inputs > 3:
plt.ylabel(
elevator.u_labels[i],
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
else:
plt.ylabel(elevator.u_labels[i])
plt.plot(t, elevator.extract_row(u_rec, i), label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("pd_controller")
else:
plt.show()
def main():
dt = 0.05
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = fct.generate_s_curve_profile(max_v=4.0,
max_a=3.5,
time_to_max_a=1.0,
dt=dt,
goal=10.0)
# Initial robot pose
pose = Pose2d(2, 0, np.pi / 2.0)
desired_pose = Pose2d()
twist_pose = Pose2d(2, 0, np.pi / 2.0)
# Ramsete tuning constants
b = 2
zeta = 0.7
vl = float("inf")
vr = float("inf")
x_rec = []
y_rec = []
twist_x_rec = []
twist_y_rec = []
vref_rec = []
omegaref_rec = []
v_rec = []
omega_rec = []
ul_rec = []
ur_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
twist_x_rec.append(twist_pose.x)
twist_y_rec.append(twist_pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(0)
omega_rec.append(0)
# Run Ramsete
i = 0
while i < len(t) - 1:
desired_pose.x = 0
desired_pose.y = xprof[i]
desired_pose.theta = np.pi / 2.0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], 0, pose, b, zeta)
vl, vr = get_diff_vels(vref, omegaref, drivetrain.rb * 2.0)
next_r = np.array([[vl], [vr]])
drivetrain.update(next_r)
vc = (drivetrain.x[0, 0] + drivetrain.x[1, 0]) / 2.0
omega = (drivetrain.x[1, 0] - drivetrain.x[0, 0]) / (2.0 *
drivetrain.rb)
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
twist_x_rec.append(twist_pose.x)
twist_y_rec.append(twist_pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(vc)
omega_rec.append(omega)
# Update nonlinear observer
pose.x += vc * math.cos(pose.theta) * dt
pose.y += vc * math.sin(pose.theta) * dt
pose.theta += omega * dt
twist_pose.exp(Twist2d(vc, 0, omega), dt)
if i < len(t) - 1:
i += 1
plt.figure(1)
plt.ylabel("Odometry error (m)")
plt.plot(t, np.subtract(twist_x_rec, x_rec), label="Error in x")
plt.plot(t, np.subtract(twist_y_rec, y_rec), label="Error in y")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_twist_odometry_error")
else:
plt.show()
def main():
dt = 0.00505
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = fct.generate_s_curve_profile(max_v=4.0,
max_a=3.5,
time_to_max_a=1.0,
dt=dt,
goal=10.0)
# Generate references for LQR
refs = []
for i in range(len(t)):
r = np.array([[vprof[i]], [0]])
refs.append(r)
# Run LQR
state_rec, ref_rec, u_rec, y_rec = drivetrain.generate_time_responses(
t, refs)
subplot_max = drivetrain.sysd.states + drivetrain.sysd.inputs
for i in range(drivetrain.sysd.states):
plt.subplot(subplot_max, 1, i + 1)
plt.ylabel(
drivetrain.state_labels[i],
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
if i == 0:
plt.title("Time domain responses")
if i == 1:
plt.ylim([-3, 3])
plt.plot(t,
drivetrain.extract_row(state_rec, i),
label="Estimated state")
plt.plot(t, drivetrain.extract_row(ref_rec, i), label="Reference")
plt.legend()
for i in range(drivetrain.sysd.inputs):
plt.subplot(subplot_max, 1, drivetrain.sysd.states + i + 1)
plt.ylabel(
drivetrain.u_labels[i],
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, drivetrain.extract_row(u_rec, i), label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_coupled_vel_lqr_profile")
# Initial robot pose
pose = Pose2d(2, 0, np.pi / 2.0)
desired_pose = Pose2d()
# Ramsete tuning constants
b = 2
zeta = 0.7
vref = float("inf")
omegaref = float("inf")
x_rec = []
y_rec = []
vref_rec = []
omegaref_rec = []
v_rec = []
omega_rec = []
ul_rec = []
ur_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(0)
omega_rec.append(0)
# Run Ramsete
i = 0
while i < len(t) - 1:
desired_pose.x = 0
desired_pose.y = xprof[i]
desired_pose.theta = np.pi / 2.0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], 0, pose, b, zeta)
next_r = np.array([[vref], [omegaref]])
drivetrain.update(next_r)
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(drivetrain.x[0, 0])
omega_rec.append(drivetrain.x[1, 0])
# Update nonlinear observer
pose.x += drivetrain.x[0, 0] * math.cos(pose.theta) * dt
pose.y += drivetrain.x[0, 0] * math.sin(pose.theta) * dt
pose.theta += drivetrain.x[1, 0] * dt
if i < len(t) - 1:
i += 1
plt.figure(2)
plt.plot([0] * len(t), xprof, label="Reference trajectory")
plt.plot(x_rec, y_rec, label="Ramsete controller")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
# Equalize aspect ratio
xlim = plt.xlim()
width = abs(xlim[0]) + abs(xlim[1])
ylim = plt.ylim()
height = abs(ylim[0]) + abs(ylim[1])
if width > height:
plt.ylim([-width / 2, width / 2])
else:
plt.xlim([-height / 2, height / 2])
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_coupled_response")
plt.figure(3)
num_plots = 4
plt.subplot(num_plots, 1, 1)
plt.title("Time domain responses")
plt.ylabel(
"Velocity (m/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, vref_rec, label="Reference")
plt.plot(t, v_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 2)
plt.ylabel(
"Angular rate (rad/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, omegaref_rec, label="Reference")
plt.plot(t, omega_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 3)
plt.ylabel(
"Left voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ul_rec, label="Control effort")
plt.legend()
plt.subplot(num_plots, 1, 4)
plt.ylabel(
"Right voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ur_rec, label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_coupled_vel_lqr_response")
else:
plt.show()
def main():
J = 7.7500e-05
b = 8.9100e-05
Kt = 0.0184
Ke = 0.0211
R = 0.0916
L = 5.9000e-05
# fmt: off
A = np.array([[-b / J, Kt / J], [-Ke / L, -R / L]])
B = np.array([[0], [1 / L]])
C = np.array([[1, 0]])
D = np.array([[0]])
# fmt: on
sysc = ct.StateSpace(A, B, C, D)
dt = 0.0001
tmax = 0.025
sysd = sysc.sample(dt)
# fmt: off
Q = np.array([[1 / 20**2, 0], [0, 1 / 40**2]])
R = np.array([[1 / 12**2]])
# fmt: on
K = fct.lqr(sysd, Q, R)
# Plant inversions
Kff_ts1 = np.linalg.pinv(sysd.B)
Kff_ts2 = np.linalg.inv(sysd.B.T * Q * sysd.B + R) * (sysd.B.T * Q)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# No feedforward
x = np.array([[0], [0]])
x_rec = np.zeros((2, 1, len(t)))
u_rec = np.zeros((1, 1, len(t)))
# Plant inversion
x_ts1 = np.array([[0], [0]])
x_ts1_rec = np.zeros((2, 1, len(t)))
u_ts1_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q and R cost)
x_ts2 = np.array([[0], [0]])
x_ts2_rec = np.zeros((2, 1, len(t)))
u_ts2_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
r_rec[:, :, k] = r
# Without feedforward
u = K @ (r - x)
u = np.clip(u, u_min, u_max)
x = sysd.A @ x + sysd.B @ u
x_rec[:, :, k] = x
u_rec[:, :, k] = u
# Plant inversion
u_ts1 = K @ (r - x_ts1) + Kff_ts1 @ (r - sysd.A @ r)
u_ts1 = np.clip(u_ts1, u_min, u_max)
x_ts1 = sysd.A @ x_ts1 + sysd.B @ u_ts1
x_ts1_rec[:, :, k] = x_ts1
u_ts1_rec[:, :, k] = u_ts1
plt.figure(1)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t, x_rec[0, 0, :], label="No feedforward")
plt.plot(t, x_ts1_rec[0, 0, :], label="Plant inversion")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_rec[1, 0, :], label="No feedforward")
plt.plot(t, x_ts1_rec[1, 0, :], label="Plant inversion")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_rec[0, 0, :], label="No feedforward")
plt.plot(t, u_ts1_rec[0, 0, :], label="Plant inversion")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("case_study_ff")
else:
plt.show()
