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 = DiffDrive(dt, x)
state_rec, ref_rec, u_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="Linearized 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:
latexutils.savefig("linearized_diff_drive_nonrotated_firstorder_xy")
plt.figure(2)
diff_drive.plot_time_responses(t, state_rec, ref_rec, u_rec)
if "--noninteractive" in sys.argv:
latexutils.savefig(
"linearized_diff_drive_nonrotated_firstorder_response")
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.0274 # V/rad/s
Kt = 0.0274 # N-m/Amp
K = Ke # Ke = Kt
R = 4 # Ohms
L = 2.75e-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 = cnt.TransferFunction(K, [J * L, J * R + b * L, b * R + K**2, 0])
cnt.root_locus(G, grid=True)
plt.xlabel("Real Axis (seconds$^{-1}$)")
plt.ylabel("Imaginary Axis (seconds$^{-1}$)")
if "--noninteractive" in sys.argv:
latexutils.savefig("highfreq_unstable_rlocus")
plt.figure(2)
plt.xlabel("Time ($s$)")
plt.ylabel("Position ($m$)")
sim(cnt.TransferFunction(1, 1), T, "Reference")
Gcl = make_closed_loop_plant(G, 3)
sim(Gcl, T, "Step response")
if "--noninteractive" in sys.argv:
latexutils.savefig("highfreq_unstable_step")
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:
latexutils.savefig("tf_3d")
else:
plt.show()
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 = cnt.TransferFunction(1, [1, -0.6], dt)
sim(tf, T, "Single pole in RHP")
tf = cnt.TransferFunction(1, [1, 0.6], dt)
sim(tf, T, "Single pole in LHP")
if "--noninteractive" in sys.argv:
latexutils.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 = cnt.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 = cnt.TransferFunction(1, den, dt)
sim(tf, T, "Complex conjugate poles in RHP")
if "--noninteractive" in sys.argv:
latexutils.savefig("z_oscillations_2p")
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] = Drivetrain(dt, x_linear).K
Kapprox_rec[:, :, i] = ApproxDrivetrain(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:
latexutils.savefig(f"linearized_diff_drive_lqr_fit_{i}")
if "--noninteractive" not in sys.argv:
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.matrix([[0.0], [0.0]])
elif t[i] < l1:
r = np.matrix([[1.524], [0.0]])
else:
r = np.matrix([[0.0], [0.0]])
refs.append(r)
state_rec, ref_rec, u_rec = elevator.generate_time_responses(t, refs)
pos = elevator.extract_row(state_rec, 0)
vel = elevator.extract_row(state_rec, 1)
plt.figure(1)
plt.xlabel("Time (s)")
plt.ylabel("Velocity (m/s)")
plt.plot(t, vel, label="Continuous")
y = generate_forward_euler_vel(vel, dt, sample_period)
plt.plot(t, y, label="Forward Euler (T={}s)".format(sample_period))
y = generate_backward_euler_vel(vel, dt, sample_period)
plt.plot(t, y, label="Backward Euler (T={}s)".format(sample_period))
y = generate_bilinear_transform_vel(vel, dt, sample_period)
plt.plot(t, y, label="Bilinear transform (T={}s)".format(sample_period))
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("discretization_methods_vel")
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.plot(t, pos, label="Continuous")
y = generate_forward_euler_pos(vel, dt, sample_period)
plt.plot(t, y, label="Forward Euler (T={}s)".format(sample_period))
y = generate_backward_euler_pos(vel, dt, sample_period)
plt.plot(t, y, label="Backward Euler (T={}s)".format(sample_period))
y = generate_bilinear_transform_pos(vel, dt, sample_period)
plt.plot(t, y, label="Bilinear transform (T={}s)".format(sample_period))
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("discretization_methods_pos")
else:
plt.show()
def main():
plt.figure(1)
draw_s_plane()
if "--noninteractive" in sys.argv:
latexutils.savefig("s_plane")
plt.figure(2)
draw_z_plane()
if "--noninteractive" in sys.argv:
latexutils.savefig("z_plane")
else:
plt.show()
def main():
x = np.arange(-7.5, 9.5, 0.001)
plt.plot(x, norm.pdf(x, loc=1, scale=2))
plt.axvline(x=2.0, label="$x_1$", color="k", linestyle="--")
plt.axvline(x=2.5, label="$x_1 + dx_1$", color="k", linestyle="-.")
plt.xlabel("$x$")
plt.ylabel("$p(x)$")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("pdf")
else:
plt.show()
def main():
# 1
# G(s) = -----
# s - 1
G = cnt.tf([1], [1, -1])
cnt.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:
latexutils.savefig("rlocus_infty")
else:
plt.show()
def main():
# 1
# G(s) = --------------
# (s + 2)(s + 3)
G = cnt.tf(1, conv([1, 0], [1, 2], [1, 3]))
cnt.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:
latexutils.savefig("rlocus_asymptotes")
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:
latexutils.savefig("pareto_boundary")
else:
plt.show()
def main():
# (s - 9 + 9i)(s - 9 - 9i)
# G(s) = ------------------------
# s(s + 10)
G = cnt.tf(conv([1, -9 + 9j], [1, -9 - 9j]), conv([1, 0], [1, 10]))
cnt.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:
latexutils.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:
latexutils.savefig("taylor_series")
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:
latexutils.savefig("joint_pdf")
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 = cnt.TransferFunction(1, conv([1, 5], [1, 0]))
sim(cnt.TransferFunction(1, 1), T, "Reference")
Gcl = make_closed_loop_plant(G, 120)
sim(Gcl, T, "Underdamped")
Gcl = make_closed_loop_plant(G, 3)
sim(Gcl, T, "Overdamped")
Gcl = make_closed_loop_plant(G, 6.268)
sim(Gcl, T, "Critically damped")
if "--noninteractive" in sys.argv:
latexutils.savefig("pid_responses")
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 = 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:
latexutils.savefig("zoh")
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:
latexutils.savefig("nyquist")
else:
plt.show()
def main():
t, x, v, a = frccnt.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:
latexutils.savefig("trapezoid_profile")
t, x, v, a = frccnt.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:
latexutils.savefig("s_curve_profile")
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:
latexutils.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:
latexutils.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:
latexutils.savefig("sampling_simulation_004")
else:
plt.show()
def main():
i_stall = 134
i_free = 0.7
rpm_free = 18730
p_max = 347
t_stall = 0.71
# 775pro data
fig, ax_left = plt.subplots()
ax_left.set_xlabel("Speed (RPM)")
rpm = np.arange(0, rpm_free, 50)
line1 = ax_left.plot(rpm, [i_stall - i_stall / rpm_free * x for x in rpm],
"b",
label="Current (A)")
ax_left.annotate(
"Stall current: " + str(i_stall) + " A",
xy=(0, i_stall),
xytext=(0, 160),
arrowprops=dict(arrowstyle="->"),
)
line2 = ax_left.plot(
rpm,
[
-p_max / (rpm_free / 2.0)**2 * (x - rpm_free / 2.0)**2 + p_max
for x in rpm
],
"g",
label="Output power (W)",
)
ax_left.annotate(
"Peak power: " + str(p_max) + " W" + os.linesep + "(at " +
str(rpm_free / 2.0) + " RPM)",
xy=(rpm_free / 2.0, p_max),
xytext=(7000, 365),
arrowprops=dict(arrowstyle="->"),
)
ax_left.set_ylabel("Current (A), Power (W)")
ax_left.set_ylim([0, 400])
plt.legend(loc=3)
ax_right = ax_left.twinx()
line3 = ax_right.plot(rpm, [t_stall - t_stall / rpm_free * x for x in rpm],
"y",
label="Torque (N-m)")
ax_right.annotate(
"Stall torque: " + str(t_stall) + " N-m",
xy=(0, t_stall),
xytext=(0, 0.75),
arrowprops=dict(arrowstyle="->"),
)
ax_right.annotate(
"Free speed: " + str(rpm_free) + " RPM" + os.linesep +
"Free current: " + str(i_free) + " A",
xy=(rpm_free, 0),
xytext=(11000, 0.3),
arrowprops=dict(arrowstyle="->"),
)
ax_right.set_ylabel("Torque (N-m)")
ax_right.set_ylim([0, 0.8])
plt.legend(loc=1)
if "--noninteractive" in sys.argv:
latexutils.savefig("motor_data")
else:
plt.show()
def main():
dt = 0.00505
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = frccnt.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 = drivetrain.generate_time_responses(t, refs)
plt.figure(1)
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:
latexutils.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:
latexutils.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:
latexutils.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.matrix([[-b / J, Kt / J],
[-Ke / L, -R / L]])
B = np.matrix([[0],
[1 / L]])
C = np.matrix([[1, 0]])
D = np.matrix([[0]])
# fmt: on
sysc = cnt.StateSpace(A, B, C, D)
dt = 0.0001
tmax = 0.025
sysd = sysc.sample(dt)
# fmt: off
Q = np.matrix([[1 / 20**2, 0],
[0, 1 / 40**2]])
R = np.matrix([[1 / 12**2]])
# fmt: on
K = frccnt.dlqr(sysd, Q, R)
# Steady-state feedforward
tmp1 = concatenate(
(
concatenate((sysd.A - np.eye(sysd.A.shape[0]), sysd.B), axis=1),
concatenate((sysd.C, sysd.D), axis=1),
),
axis=0,
)
tmp2 = concatenate(
(np.zeros((sysd.A.shape[0], 1)), np.ones((sysd.C.shape[0], 1))), axis=0
)
NxNu = np.linalg.pinv(tmp1) * tmp2
Nx = NxNu[0 : sysd.A.shape[0], 0]
Nu = NxNu[sysd.C.shape[0] + 1 :, 0]
Kff_ss = Nu * np.linalg.pinv(Nx)
# Two-state feedforwards
Kff_ts1 = np.linalg.inv(sysd.B.T * Q * sysd.B + R) * (sysd.B.T * Q)
Kff_ts2 = np.linalg.inv(sysd.B.T * Q * sysd.B) * (sysd.B.T * Q)
t = np.arange(0, tmax, dt)
r = np.matrix([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# No feedforward
x = np.matrix([[0], [0]])
x_rec = np.zeros((2, 1, len(t)))
u_rec = np.zeros((1, 1, len(t)))
# Steady-state feedforward
x_ss = np.matrix([[0], [0]])
x_ss_rec = np.zeros((2, 1, len(t)))
u_ss_rec = np.zeros((1, 1, len(t)))
# Two-state feedforward
x_ts1 = np.matrix([[0], [0]])
x_ts1_rec = np.zeros((2, 1, len(t)))
u_ts1_rec = np.zeros((1, 1, len(t)))
# Two-state feedforward (no R cost)
x_ts2 = np.matrix([[0], [0]])
x_ts2_rec = np.zeros((2, 1, len(t)))
u_ts2_rec = np.zeros((1, 1, len(t)))
u_min = np.asmatrix(-12)
u_max = np.asmatrix(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
# With steady-state feedforward
u_ss = K * (r - x_ss) + Kff_ss * r
u_ss = np.clip(u_ss, u_min, u_max)
x_ss = sysd.A * x_ss + sysd.B * u_ss
x_ss_rec[:, :, k] = x_ss
u_ss_rec[:, :, k] = u_ss
# With two-state feedforward
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
# With two-state feedforward (no R cost)
u_ts2 = K * (r - x_ts2) + Kff_ts2 * (r - sysd.A * r)
u_ts2 = np.clip(u_ts2, u_min, u_max)
x_ts2 = sysd.A * x_ts2 + sysd.B * u_ts2
x_ts2_rec[:, :, k] = x_ts2
u_ts2_rec[:, :, k] = u_ts2
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 FF")
plt.plot(t, x_ss_rec[0, 0, :], label="Steady-state FF")
plt.plot(t, x_ts1_rec[0, 0, :], label="Two-state FF")
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 FF")
plt.plot(t, x_ss_rec[1, 0, :], label="Steady-state FF")
plt.plot(t, x_ts1_rec[1, 0, :], label="Two-state FF")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_rec[0, 0, :], label="No FF")
plt.plot(t, u_ss_rec[0, 0, :], label="Steady-state FF")
plt.plot(t, u_ts1_rec[0, 0, :], label="Two-state FF")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("case_study_ff1")
plt.figure(2)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t, x_ts1_rec[0, 0, :], label="Two-state FF")
plt.plot(t, x_ts2_rec[0, 0, :], label="Two-state FF (no R cost)")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_ts1_rec[1, 0, :], label="Two-state FF")
plt.plot(t, x_ts2_rec[1, 0, :], label="Two-state FF (no R cost)")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_ts1_rec[0, 0, :], label="Two-state FF")
plt.plot(t, u_ts2_rec[0, 0, :], label="Two-state FF (no R cost)")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("case_study_ff2")
else:
plt.show()
def main():
T = 0.000001
xlim = [0, 0.05]
ylim = [-3, 3]
x = np.arange(xlim[0], xlim[1], T)
plt.xlim(xlim)
plt.ylim(ylim)
f_f = 349.23
f_a = 440
f_c = 261.63
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
num_plots = 4
plt.subplot(num_plots, 1, 1)
plt.ylim(ylim)
plt.ylabel("Fmaj4")
plt.plot(x, ysum)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 2)
plt.ylim(ylim)
plt.ylabel("$F_4$")
plt.plot(x, yf)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 3)
plt.ylim(ylim)
plt.ylabel("$A_4$")
plt.plot(x, ya)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 4)
plt.ylim(ylim)
plt.ylabel("$C_4$")
plt.plot(x, yc)
plt.xlabel("$t$")
if "--noninteractive" in sys.argv:
latexutils.savefig("fourier_chord")
plt.figure(2)
N = 1000000 # Number of samples
T = 1.0 / 2000.0 # Sample spacing
x = np.arange(0.0, N * T, T)
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
yf = scipy.fftpack.fft(ysum)
xf = np.linspace(0.0, 1.0 / (2.0 * T), N // 2)
plt.plot(xf, 2.0 / N * np.abs(yf[:N // 2]))
plt.xlabel("Frequency (Hz)")
plt.xlim([0, 700])
if "--noninteractive" in sys.argv:
latexutils.savefig("fourier_chord_fft")
else:
plt.show()
def main():
dt = 0.00505
tTotal = 5
drivetrain = Drivetrain(dt)
t = np.linspace(0, tTotal, tTotal / dt)
# Initial robot pose
pose = Pose2d(0, 0, 0)
desired_pose = Pose2d()
# Ramsete tuning constants
b = 2
zeta = 0.75
vl = float("inf")
vr = float("inf")
vref_rec = []
omegaref_rec = []
x_rec = []
y_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
# Run Ramsete
drivetrain.reset()
i = 0
while i < len(t) - 1:
desired_pose.x = pose.x + 0.5
desired_pose.y = 2
desired_pose.theta = 0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, 1, 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)
# Update nonlinear observer
pose.x += vref * math.cos(pose.theta) * dt
pose.y += vref * math.sin(pose.theta) * dt
pose.theta += omegaref * dt
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
print (f" x: {pose.x}, y: {pose.y}")
if i < len(t) - 1:
i += 1
plt.figure(2)
plt.plot(x_rec, y_rec, label="Ramsete controller")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_decoupled_response")
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_decoupled_vel_lqr_response")
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:
latexutils.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:
latexutils.savefig("laplace_chord_3d")
else:
plt.show()
def main():
# x_1: robot position measured from corner
# x_2: robot velocity with positive direction toward wall
# x_3: wall position measured from corner
xhat = np.array([[0.0], [0.0], [0.0]])
# y_1: measurement of distance from robot to corner
# y_2: measurement of distance from robot to wall
y = np.genfromtxt("kalman_robot.csv", delimiter=",")
# fmt: off
phi = np.array([[1, 1, 0], [0, 1, 0], [0, 0, 1]])
gamma = np.array([[0], [0.1], [0]])
Q = np.array([[1]])
R = np.array([[10, 0], [0, 10]])
P = np.zeros((3, 3))
K = np.zeros((3, 2))
H = np.array([[1, 0, 0], [-1, 0, 1]])
# fmt: on
# Initialize matrix storage
xhat_pre_rec = np.zeros((3, 1, y.shape[1]))
xhat_post_rec = np.zeros((3, 1, y.shape[1]))
P_pre_rec = np.zeros((3, 3, y.shape[1]))
P_post_rec = np.zeros((3, 3, y.shape[1]))
xhat_smooth_rec = np.zeros((3, 1, y.shape[1]))
P_smooth_rec = np.zeros((3, 3, y.shape[1]))
t = [0] * (y.shape[1])
# Forward Kalman filter
# Set up initial conditions for first measurement
xhat[0] = y[0, 0]
xhat[1] = 0.2
xhat[2] = y[0, 0] + y[1, 0]
# fmt: off
P = np.array([[10, 0, 10], [0, 1, 0], [10, 0, 20]])
# fmt: on
xhat_pre_rec[:, :, 1] = xhat
P_pre_rec[:, :, 1] = P
xhat_post_rec[:, :, 1] = xhat
P_post_rec[:, :, 1] = P
t[1] = 1
for k in range(2, 100):
# Predict
xhat = phi @ xhat
P = phi @ P @ phi.T + gamma @ Q @ gamma.T
xhat_pre_rec[:, :, k] = xhat
P_pre_rec[:, :, k] = P
# Update
K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
xhat += K @ (y[:, k:k + 1] - H @ xhat)
P = (np.eye(3, 3) - K @ H) @ P
xhat_post_rec[:, :, k] = xhat
P_post_rec[:, :, k] = P
t[k] = k
# Kalman smoother
# Last estimate is already optional, so add it to the record
xhat_smooth_rec[:, :, -1] = xhat_post_rec[:, :, -1]
P_smooth_rec[:, :, -1] = P_post_rec[:, :, -1]
for k in range(y.shape[1] - 2, 0, -1):
A = P_post_rec[:, :, k] @ phi.T @ np.linalg.inv(P_pre_rec[:, :, k + 1])
xhat = xhat_post_rec[:, :, k] + A @ (xhat_smooth_rec[:, :, k + 1] -
xhat_pre_rec[:, :, k + 1])
P = P_post_rec[:, :, k] + A @ (P - P_pre_rec[:, :, k + 1]) @ A.T
xhat_smooth_rec[:, :, k] = xhat
P_smooth_rec[:, :, k] = P
# Robot position
plt.figure(1)
plt.xlabel("Time (s)")
plt.ylabel("Position (cm)")
plt.plot(t[1:], xhat_post_rec[0, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[0, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_robot_pos")
# Robot velocity
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Velocity (cm)")
plt.plot(t[1:], xhat_post_rec[1, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[1, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_robot_vel")
# Wall position
plt.figure(3)
plt.xlabel("Time (s)")
plt.ylabel("Wall position (cm)")
plt.plot(t[1:], xhat_post_rec[2, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[2, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_wall_pos")
# Robot position variance
plt.figure(4)
plt.xlabel("Time (s)")
plt.ylabel("Robot position variance ($cm^2$)")
plt.plot(t[1:], P_post_rec[1, 1, 1:], label="Kalman filter")
plt.plot(t[1:], P_smooth_rec[1, 1, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_robot_pos_variance")
else:
plt.show()
def main():
# x_1: robot position measured from corner
# x_2: robot velocity with positive direction toward wall
# x_3: wall position measured from corner
xhat = np.matrix([[0], [0], [0]])
# y_1: measurement of distance from robot to corner
# y_2: measurement of distance from robot to wall
y = []
import csv
with open("kalman_robot.csv", newline="") as data:
reader = csv.reader(data)
for i, row in enumerate(reader):
yrow = np.asmatrix([float(x) for x in row])
if len(y) == 0:
y = yrow
else:
y = np.concatenate((y, yrow))
# fmt: off
phi = np.matrix([[1, 1, 0], [0, 1, 0], [0, 0, 1]])
gamma = np.matrix([[0], [0.1], [0]])
Q = np.matrix([[1]])
R = np.matrix([[10, 0], [0, 10]])
P = np.zeros((3, 3))
K = np.zeros((3, 2))
H = np.matrix([[1, 0, 0], [-1, 0, 1]])
# fmt: on
# Initialize matrix storage
xhat_pre_rec = np.zeros((3, 1, y.shape[1]))
xhat_post_rec = np.zeros((3, 1, y.shape[1]))
P_pre_rec = np.zeros((3, 3, y.shape[1]))
P_post_rec = np.zeros((3, 3, y.shape[1]))
xhat_smooth_rec = np.zeros((3, 1, y.shape[1]))
P_smooth_rec = np.zeros((3, 3, y.shape[1]))
t = [0] * (y.shape[1])
# Forward Kalman filter
# Set up initial conditions for first measurement
xhat[0] = y[0, 0]
xhat[1] = 0.2
xhat[2] = y[0, 0] + y[1, 0]
# fmt: off
P = np.matrix([[10, 0, 10], [0, 1, 0], [10, 0, 20]])
# fmt: on
xhat_pre_rec[:, :, 1] = xhat
P_pre_rec[:, :, 1] = P
xhat_post_rec[:, :, 1] = xhat
P_post_rec[:, :, 1] = P
t[1] = 1
for k in range(2, 100):
# Predict
xhat = phi * xhat
P = phi * P * phi.T + gamma * Q * gamma.T
xhat_pre_rec[:, :, k] = xhat
P_pre_rec[:, :, k] = P
# Update
K = P * H.T * np.linalg.inv(H * P * H.T + R)
xhat = xhat + K * (y[:, k] - H * xhat)
P = (np.eye(3, 3) - K * H) * P
xhat_post_rec[:, :, k] = xhat
P_post_rec[:, :, k] = P
t[k] = k
# Kalman smoother
# Last estimate is already optional, so add it to the record
xhat_smooth_rec[:, :, -1] = xhat_post_rec[:, :, -1]
P_smooth_rec[:, :, -1] = P_post_rec[:, :, -1]
for k in range(y.shape[1] - 2, 0, -1):
A = P_post_rec[:, :, k] * phi.T * np.linalg.inv(P_pre_rec[:, :, k + 1])
xhat = xhat_post_rec[:, :, k] + A * (xhat_smooth_rec[:, :, k + 1] -
xhat_pre_rec[:, :, k + 1])
P = P_post_rec[:, :, k] + A * (P - P_pre_rec[:, :, k + 1]) * A.T
xhat_smooth_rec[:, :, k] = xhat
P_smooth_rec[:, :, k] = P
# Robot position
plt.figure(1)
plt.xlabel("Time (s)")
plt.ylabel("Position (cm)")
plt.plot(t[1:], xhat_post_rec[0, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[0, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_robot_pos")
# Robot velocity
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Velocity (cm)")
plt.plot(t[1:], xhat_post_rec[1, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[1, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_robot_vel")
# Wall position
plt.figure(3)
plt.xlabel("Time (s)")
plt.ylabel("Wall position (cm)")
plt.plot(t[1:], xhat_post_rec[2, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[2, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_wall_pos")
# Robot position variance
plt.figure(4)
plt.xlabel("Time (s)")
plt.ylabel("Robot position variance ($cm^2$)")
plt.plot(t[1:], P_post_rec[1, 1, 1:], label="Kalman filter")
plt.plot(t[1:], P_smooth_rec[1, 1, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latexutils.savefig("kalman_smoother_robot_pos_variance")
else:
plt.show()
def main():
dt = 0.00505
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = frccnt.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.matrix([[vprof[i]], [vprof[i]]])
refs.append(r)
# Run LQR
state_rec, ref_rec, u_rec = drivetrain.generate_time_responses(t, refs)
plt.figure(1)
drivetrain.plot_time_responses(t, state_rec, ref_rec, u_rec)
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_decoupled_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
vl = float("inf")
vr = 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
drivetrain.reset()
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.matrix([[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)
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
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
ylim = plt.ylim()
width = abs(ylim[0]) + abs(ylim[1])
plt.xlim([-width / 2, width / 2])
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_decoupled_response")
plt.figure(3)
num_plots = 4
plt.subplot(num_plots, 1, 1)
plt.title("Time-domain responses")
plt.ylabel("Velocity (m/s)")
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)")
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)")
plt.plot(t, ul_rec, label="Control effort")
plt.legend()
plt.subplot(num_plots, 1, 4)
plt.ylabel("Right voltage (V)")
plt.plot(t, ur_rec, label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_decoupled_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 = cnt.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_pp1 = cnt.place(sysd.A, sysd.B, [0.1, 0.9])
K_pp2 = cnt.place(sysd.A, sysd.B, [0.1, 0.8])
K_lqr = frccnt.lqr(sysd, Q, R)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# Pole placement 1
x_pp1 = np.array([[0], [0]])
x_pp1_rec = np.zeros((2, 1, len(t)))
u_pp1_rec = np.zeros((1, 1, len(t)))
# Pole placement 2
x_pp2 = np.array([[0], [0]])
x_pp2_rec = np.zeros((2, 1, len(t)))
u_pp2_rec = np.zeros((1, 1, len(t)))
# LQR
x_lqr = np.array([[0], [0]])
x_lqr_rec = np.zeros((2, 1, len(t)))
u_lqr_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
# Pole placement 1
u_pp1 = K_pp1 @ (r - x_pp1)
# Pole placement 2
u_pp2 = K_pp2 @ (r - x_pp2)
# LQR
u_lqr = K_lqr @ (r - x_lqr)
u_pp1 = np.clip(u_pp1, u_min, u_max)
x_pp1 = sysd.A @ x_pp1 + sysd.B @ u_pp1
u_pp2 = np.clip(u_pp2, u_min, u_max)
x_pp2 = sysd.A @ x_pp2 + sysd.B @ u_pp2
u_lqr = np.clip(u_lqr, u_min, u_max)
x_lqr = sysd.A @ x_lqr + sysd.B @ u_lqr
r_rec[:, :, k] = r
x_pp1_rec[:, :, k] = x_pp1
u_pp1_rec[:, :, k] = u_pp1
x_pp2_rec[:, :, k] = x_pp2
u_pp2_rec[:, :, k] = u_pp2
x_lqr_rec[:, :, k] = x_lqr
u_lqr_rec[:, :, k] = u_lqr
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_pp1_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
x_pp2_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, x_lqr_rec[0, 0, :], label="LQR")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t,
x_pp1_rec[1, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
x_pp2_rec[1, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, x_lqr_rec[1, 0, :], label="LQR")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t,
u_pp1_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
u_pp2_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, u_lqr_rec[0, 0, :], label="LQR")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("case_study_pp_lqr")
else:
plt.show()
def main():
dt = 0.05
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = frccnt.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.apply(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:
latexutils.savefig("ramsete_twist_odometry_error")
else:
plt.show()
