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()