def f_7_d(): pvt = Sim.PlanarVTOL() proportional_gain = 0.75 derivative_gain = 0.09 pvt.add_controller(PD.PlanarVTOL, proportional_gain, derivative_gain, 0, 0, 0, 0) request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=10), sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=10) ) handle = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def e_8_f(): bnb = Sim.BallAndBeam() max_force = 15 rise_time_theta = 1 beam_mass = bnb.dynamics.beam_mass ball_mass = bnb.dynamics.ball_mass beam_length = bnb.dynamics.beam_length gravity = bnb.dynamics.gravity c_theta = 1 / (beam_length * (ball_mass / 4 + beam_mass / 3)) natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1/2)) proportional_gain_theta = natural_frequency_theta**2 / c_theta derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / c_theta rise_time_z = 10 * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1/2)) proportional_gain_z = natural_frequency_z**2 / -gravity derivative_gain_z = 2 * damping_ratio * natural_frequency_z / -gravity bnb.add_controller(PD.BallAndBeam, proportional_gain_z, derivative_gain_z, proportional_gain_theta, derivative_gain_theta, max_force) # a square wave with magnitude 0.25±0.15 meters and frequency 0.01 Hz requests = sg.generator(sg.constant, amplitude=0.0, t_step=bnb.seconds_per_sim_step, t_final=10) handle = bnb.view_animation(requests) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def f_8_a(): pvt = Sim.PlanarVTOL() rise_time = 8 natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1/2)) c_h = 1 / (pvt.dynamics.center_mass + 2 * pvt.dynamics.wing_mass) proportional_gain_h = natural_frequency**2 / c_h derivative_gain_h = 2 * damping_ratio * natural_frequency / c_h pvt.add_controller(PD.PlanarVTOL, proportional_gain_h, derivative_gain_h, 0, 0, 0, 0) request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=10), sg.generator(sg.sin, frequency=0.01, t_step=pvt.seconds_per_sim_step, t_final=10), ) handle = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def d_7_d(): msd = Sim.MassSpringDamper() proportional_gain = 4.5 derivative_gain = 12 msd.add_controller(PD.MassSpringDamper, proportional_gain, derivative_gain) request = sg.generator(sg.constant, t_step=msd.seconds_per_sim_step, t_final=5) handle = msd.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def e_12(): print('\n--E--\n') bnb = Sim.BallAndBeam() max_force = 15 damping_ratio = 1 / (2**(1 / 2)) # Tuning variables rise_time_theta = .5 bandwidth_separation = 2.4 integrator_pole = 2. natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = rise_time_theta * bandwidth_separation natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] coefs_I = [1, integrator_pole] roots = np.roots(np.convolve(coefs_I, np.convolve(coefs_theta, coefs_z))) A = bnb.dynamics.A B = bnb.dynamics.B C = bnb.dynamics.C A_I, B_I = augment_matrices(A, B, C[0:1, :]) gains = ctrl.place(A_I, B_I, roots) dim = A.shape[1] feedback_gain = gains[0:1, 0:dim] integrator_gain = gains[0, dim] print( f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}') bnb.add_controller(Control.BallAndBeam, feedback_gain, integrator_gain, max_force=max_force) requests = sg.generator(sg.square, amplitude=0.15, y_offset=0.25, frequency=0.1, t_step=bnb.seconds_per_sim_step, t_final=90) handle = bnb.view_animation(requests) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def f_8_g(): pvt = Sim.PlanarVTOL() rise_time_h = 8 max_force = 10 natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1/2)) c_h = 1 / (pvt.dynamics.center_mass + 2 * pvt.dynamics.wing_mass) proportional_gain_h = natural_frequency_h**2 / c_h derivative_gain_h = 2 * damping_ratio * natural_frequency_h / c_h rise_time_theta = 0.8 natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1/2)) j_center = pvt.dynamics.center_moi wing_mass = pvt.dynamics.wing_mass wing_spacing = pvt.dynamics.wing_spacing c_theta = 1 / (j_center + 2 * wing_mass * wing_spacing**2) proportional_gain_theta = natural_frequency_theta**2 / c_theta derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / c_theta rise_time_z = 10 * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1/2)) mu = pvt.dynamics.drag gravity = pvt.dynamics.gravity proportional_gain_z = natural_frequency_z**2 / -gravity derivative_gain_z = (2 * damping_ratio * natural_frequency_z - mu * c_h) / -gravity pvt.add_controller(PD.PlanarVTOL, proportional_gain_h, derivative_gain_h, proportional_gain_z, derivative_gain_z, proportional_gain_theta, derivative_gain_theta, max_force) request = zip( sg.generator(sg.constant, y_offset=0, t_step=pvt.seconds_per_sim_step, t_final=10), # a square wave with magnitude 3 ± 2.5 meters and frequency 0.08 Hz sg.generator(sg.sin, frequency=0.08, y_offset=3, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=10), ) handle = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def d_8_a(): msd = Sim.MassSpringDamper() rise_time = 2 natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1/2)) proportional_gain = 5 * (natural_frequency**2 - 0.6) derivative_gain = 5 * (2 * damping_ratio * natural_frequency - 0.1) msd.add_controller(PD.MassSpringDamper, proportional_gain, derivative_gain) request = sg.generator(sg.constant, t_step=msd.seconds_per_sim_step, t_final=5) handle = msd.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def d_11(): print('\n--D--\n') msd = Sim.MassSpringDamper() max_force = 2.0 damping_ratio = 1 / (2**(1 / 2)) rise_time = 2.0 # mass = msd.dynamics.mass # spring_const = msd.dynamics.spring_const # natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2) natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs = [1, 2 * damping_ratio * natural_frequency, natural_frequency**2] # Small error roots = np.roots(coefs) print(f"Roots: {roots}") # Part B A = msd.dynamics.A B = msd.dynamics.B C = msd.dynamics.C # Part C control_mat = ctrl.ctrb(A, B) rank = np.linalg.matrix_rank(control_mat) order = A.shape[0] print(f"controllability matrix rank: {rank}") print(f"System order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain = ctrl.place(A, B, roots) print(f'Feedback Gain: {feedback_gain}') reference_gain = -1 / (C @ np.linalg.inv(A - B @ feedback_gain) @ B) print(f"Reference gain: {reference_gain}") # Part E msd.add_controller(SSController.MassSpringDamper, feedback_gain, reference_gain, max_force) request = sg.generator(sg.constant, amplitude=2 / 3, t_step=msd.seconds_per_sim_step, t_final=20) handle = msd.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def e_10(): bnb = Sim.BallAndBeam() max_force = 15.0 damping_ratio = 1 / (2**(1 / 2)) + 0.2 length = bnb.dynamics.beam_length ball_mass = bnb.dynamics.ball_mass beam_mass = bnb.dynamics.beam_mass z_equilibrium = length / 2 gravity = bnb.dynamics.gravity b_theta = length / ( (beam_mass * length**2) / 3 + ball_mass * z_equilibrium**2) # Tuning variables rise_time_theta = .5 bandwidth_separation = 10 integration_gain_theta = .01 integration_gain_z = 0.001 threshold_theta = 0.0005 threshold_z = 0.02 natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) proportional_gain_theta = natural_frequency_theta**2 / b_theta derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / b_theta rise_time_z = rise_time_theta * bandwidth_separation natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) proportional_gain_z = -natural_frequency_z**2 / gravity derivative_gain_z = -2 * damping_ratio * natural_frequency_z / gravity bnb.add_controller(PID.BallAndBeam, proportional_gain_z, derivative_gain_z, integration_gain_z, proportional_gain_theta, derivative_gain_theta, integration_gain_theta, threshold_z, threshold_theta, max_force) # a square wave with magnitude 0.25±0.15 meters and frequency 0.05 Hz requests = sg.generator(sg.square, amplitude=0.15, y_offset=0.25, frequency=0.05, t_step=bnb.seconds_per_sim_step, t_final=20) handle = bnb.save_movie(requests, 'out/HW7E.mp4') # Sim.Animations.plt.waitforbuttonpress() # Sim.Animations.plt.close() return handle
def d_12(): print('\n--D--\n') msd = Sim.MassSpringDamper() max_force = 2.0 damping_ratio = 1 / (2**(1 / 2)) rise_time = 2.0 # mass = msd.dynamics.mass # spring_const = msd.dynamics.spring_const # natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2) natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1 / 2)) coefs = [1, 2 * damping_ratio * natural_frequency, natural_frequency**2] integrator_root = 1.0 roots = np.roots(np.convolve(coefs, [1, integrator_root])) print(f"Roots: {roots}") # Part B A = msd.dynamics.A B = msd.dynamics.B C = msd.dynamics.C A_I, B_I = augment_matrices(A, B, C) gains = ctrl.place(A_I, B_I, roots) feedback_gain = gains[:, 0:2] integrator_gain = gains[0, 2] print( f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}') msd.add_controller(Control.MassSpringDamper, feedback_gain, integrator_gain, max_force) request = sg.generator(sg.constant, amplitude=2 / 3, t_step=msd.seconds_per_sim_step, t_final=20) handle = msd.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def d_10(): msd = Sim.MassSpringDamper() max_force = 2.0 damping_ratio = 1 / (2**(1 / 2)) proportional_gain = max_force natural_frequency = ((msd.dynamics.spring_const + proportional_gain) / msd.dynamics.mass)**(1 / 2) derivative_gain = msd.dynamics.mass * 2 * damping_ratio * natural_frequency - msd.dynamics.damping integrator_gain = 1 msd.add_controller(PID.MassSpringDamper, proportional_gain, derivative_gain, integrator_gain, max_force) request = sg.generator(sg.constant, amplitude=2 / 3, t_step=msd.seconds_per_sim_step, t_final=20) handle = msd.save_movie(request, 'out/HW7D.mp4') # Sim.Animations.plt.waitforbuttonpress() # Sim.Animations.plt.close() return handle
def f_12(): print('\n--F--\n') pvt = Sim.PlanarVTOL() max_force = 10 damping_ratio = 1 / (2**(1 / 2)) + 0.2 # Tuning variable rise_time_h = 2.5 rise_time_theta = 0.1 bandwidth_separation = 7 integrator_pole_lat = 1.5 integrator_pole_lon = 1 natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1 / 2)) natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = bandwidth_separation * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] coefs_I_lat = [1, integrator_pole_lat] roots_lat = np.roots( np.convolve(coefs_I_lat, np.convolve(coefs_theta, coefs_z))) print(f"Latitudinal roots: {roots_lat}") coefs_h = [ 1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2 ] coefs_I_lon = [1, integrator_pole_lon] roots_lon = np.roots(np.convolve(coefs_h, coefs_I_lon)) print(f"Longitudinal roots: {roots_lon}") # Part B A_lat = pvt.dynamics.A_lat B_lat = pvt.dynamics.B_lat C_lat = pvt.dynamics.C_lat A_I_lat, B_I_lat = augment_matrices(A_lat, B_lat, C_lat[0:1, :]) A_lon = pvt.dynamics.A_lon B_lon = pvt.dynamics.B_lon C_lon = pvt.dynamics.C_lon A_I_lon, B_I_lon = augment_matrices(A_lon, B_lon, C_lon) gains_lat = ctrl.place(A_I_lat, B_I_lat, roots_lat) dim_lat = A_lat.shape[1] feedback_gain_lat = gains_lat[0:1, 0:dim_lat] int_gain_lat = gains_lat[0, dim_lat] print( f'Latitudinal feedback Gain: {feedback_gain_lat}\nLatitudinal integrator gain: {int_gain_lat}' ) gains_lon = ctrl.place(A_I_lon, B_I_lon, roots_lon) dim_lon = A_lon.shape[1] feedback_gain_lon = gains_lon[0:1, 0:dim_lon] int_gain_lon = gains_lon[0, dim_lon] print( f'Longitudinal feedback Gain: {feedback_gain_lon}\nLongitudinal integrator gain: {int_gain_lon}' ) request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=20), sg.generator(sg.sin, frequency=0.08, y_offset=0, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=20), ) pvt.add_controller(Control.PlanarVTOL, feedback_gain_lat, int_gain_lat, feedback_gain_lon, int_gain_lon, max_force) handel = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handel
def e_11(): print('\n--E--\n') bnb = Sim.BallAndBeam() max_force = 15 damping_ratio = 1 / (2**(1 / 2)) # Tuning variables rise_time_theta = .5 bandwidth_separation = 2.4 natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = rise_time_theta * bandwidth_separation natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] roots = np.concatenate(( np.roots(coefs_theta), np.roots(coefs_z), )) # Part B A = bnb.dynamics.A B = bnb.dynamics.B C = bnb.dynamics.C # Part C control_mat = ctrl.ctrb(A, B) rank = np.linalg.matrix_rank(control_mat) order = A.shape[0] print(f"controllability matrix rank: {rank}") print(f"System order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain = ctrl.place(A, B, roots) print(f'Feedback Gain: {feedback_gain}') reference_gain = -1 / (C[0] @ np.linalg.inv(A - B @ feedback_gain) @ B) print(f"Reference gain: {reference_gain}") # Part E bnb.add_controller(SSController.BallAndBeam, feedback_gain, reference_gain, max_force) requests = sg.generator(sg.square, amplitude=0.15, y_offset=0.25, frequency=0.05, t_step=bnb.seconds_per_sim_step, t_final=20) handle = bnb.view_animation(requests) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def f_11(): print('\n--F--\n') pvt = Sim.PlanarVTOL() max_force = 10 damping_ratio = 1 / (2**(1 / 2)) + 0.2 # Tuning variable rise_time_h = 2.5 rise_time_theta = 0.1 bandwidth_separation = 7 natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1 / 2)) natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = bandwidth_separation * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] roots_lat = np.concatenate(( np.roots(coefs_theta), np.roots(coefs_z), )) print(f"Latitudinal roots: {roots_lat}") coefs_h = [ 1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2 ] roots_lon = np.roots(coefs_h) print(f"Longitudinal roots: {roots_lon}") # Part B A_lat = pvt.dynamics.A_lat B_lat = pvt.dynamics.B_lat C_lat = pvt.dynamics.C_lat A_lon = pvt.dynamics.A_lon B_lon = pvt.dynamics.B_lon C_lon = pvt.dynamics.C_lon # Part C control_mat_lat = ctrl.ctrb(A_lat, B_lat) rank = np.linalg.matrix_rank(control_mat_lat) order = A_lat.shape[0] print(f"Latitudinal controllability matrix rank: {rank}") print(f"Latitudinal system order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") control_mat_lon = ctrl.ctrb(A_lon, B_lon) rank = np.linalg.matrix_rank(control_mat_lon) order = A_lon.shape[0] print(f"Longitudinal Controllability matrix rank: {rank}") print(f"Longitudinal system order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain_lat = ctrl.place(A_lat, B_lat, roots_lat) print(f'Latitudinal feedback Gain: {feedback_gain_lat}') reference_gain_lat = -1 / ( C_lat[0] @ np.linalg.inv(A_lat - B_lat @ feedback_gain_lat) @ B_lat) print(f"Latitudinal reference gain: {reference_gain_lat}") feedback_gain_lon = ctrl.place(A_lon, B_lon, roots_lon) print(f'Longitudinal feedback Gain: {feedback_gain_lon}') reference_gain_lon = -1 / ( C_lon @ np.linalg.inv(A_lon - B_lon @ feedback_gain_lon) @ B_lon) print(f"Longitudinal reference gain: {reference_gain_lon}") # Part E request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=20), sg.generator(sg.sin, frequency=0.08, y_offset=0, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=20), ) pvt.add_controller(SSController.PlanarVTOL, feedback_gain_lat, reference_gain_lat, feedback_gain_lon, reference_gain_lon, max_force) handel = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handel
def f_10(): pvt = Sim.PlanarVTOL() max_force = 10 damping_ratio = 1 / (2**(1 / 2)) + 0.2 # Tuning variable rise_time_h = 2.5 rise_time_theta = 0.1 bandwidth_separation = 7 integration_gain_h = 0.0001 integration_gain_z = 0.01 integration_gain_theta = 0.001 threshold_h = 0.01 threshold_z = 0.01 threshold_theta = 0.01 natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1 / 2)) c_h = 1 / (pvt.dynamics.center_mass + 2 * pvt.dynamics.wing_mass) proportional_gain_h = natural_frequency_h**2 / c_h derivative_gain_h = 2 * damping_ratio * natural_frequency_h / c_h natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) j_center = pvt.dynamics.center_moi wing_mass = pvt.dynamics.wing_mass wing_spacing = pvt.dynamics.wing_spacing c_theta = 1 / (j_center + 2 * wing_mass * wing_spacing**2) proportional_gain_theta = natural_frequency_theta**2 / c_theta derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / c_theta rise_time_z = bandwidth_separation * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) mu = pvt.dynamics.drag gravity = pvt.dynamics.gravity proportional_gain_z = natural_frequency_z**2 / -gravity derivative_gain_z = (2 * damping_ratio * natural_frequency_z - mu * c_h) / -gravity pvt.add_controller(PID.PlanarVTOL, proportional_gain_h, derivative_gain_h, integration_gain_h, proportional_gain_z, derivative_gain_z, integration_gain_z, proportional_gain_theta, derivative_gain_theta, integration_gain_theta, threshold_h, threshold_z, threshold_theta, max_force) request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=20), sg.generator(sg.sin, frequency=0.08, y_offset=0, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=20), ) handle = pvt.save_movie(request, 'out/HW7F.mp4') # Sim.Animations.plt.waitforbuttonpress() # Sim.Animations.plt.close() return handle