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