def test_sparc_model(print_stuff=False, trajectory=True, record_trajectory=True, read_trajectory=False, control=[True] * 4, readfiles=[True] * 4, recordfiles=[True] * 4): # Connect to Client (VREP) client = serve_socket(int(argv[1])) # Receive working directory path from client pyquadsim_directory = receive_string(client) # List for plotting time axis kpoints = [] time_elapsed = 0.0 # Instantiates figure for plotting motor angular velocities: fig_motors = plt.figure('Motors', figsize=(14,10)) axes_motors = fig_motors.add_axes([0.1, 0.1, 0.8, 0.8]) motor_points = [[], [], [], []] # Instantiates figure for plotting stabilization results: fig_control, (axes_alt, axes_pitch, axes_roll, axes_yaw) = plt.subplots(4, 1, sharex=True, sharey=False, figsize=(14,10)) fig_control.canvas.set_window_title('Quadcopter Stability - Commanded(RED), Performed(BLUE)') axes_alt.set_title('Alt') axes_pitch.set_title('Pitch') axes_roll.set_title('Roll') axes_yaw.set_title('Yaw') ypoints_alt = [] refpoints_alt = [] ypoints_pitch = [] refpoints_pitch = [] ypoints_roll = [] refpoints_roll = [] ypoints_yaw = [] refpoints_yaw = [] # Instantiates figure for plotting navigation results: fig_nav = plt.figure('Quadcopter Navigation', figsize=(14,10)) axes_x = fig_nav.add_subplot(221) axes_x.set_title('X') ypoints_x = [] refpoints_x = [] axes_y = fig_nav.add_subplot(222) axes_y.set_title('Y') ypoints_y = [] refpoints_y = [] axes_xy = fig_nav.add_subplot(212) axes_xy.set_title('XY') # Instantiates figure for plotting control signals: fig_control, (ax_c_alt, ax_c_yaw, ax_c_pitch, ax_c_roll) = plt.subplots(4, 1, sharex=True, sharey=False, figsize=(14,10)) fig_control.canvas.set_window_title('Control Effort') ax_c_alt.set_title('Alt') ax_c_yaw.set_title('Yaw') ax_c_pitch.set_title('Pitch') ax_c_roll.set_title('Roll') c_alt_points = [] c_pitch_points = [] c_roll_points = [] c_yaw_points = [] # Instantiates figure for plotting clouds: fig_clouds = plt.figure('Data Clouds', figsize=(14,10)) ax_cloud_alt = fig_clouds.add_subplot(231) ax_cloud_alt.set_title('Alt') ax_cloud_yaw = fig_clouds.add_subplot(232) ax_cloud_yaw.set_title('Yaw') ax_cloud_pitch = fig_clouds.add_subplot(233) ax_cloud_pitch.set_title('Pitch') ax_cloud_roll = fig_clouds.add_subplot(234) ax_cloud_roll.set_title('Roll') ax_cloud_nav_x = fig_clouds.add_subplot(235) ax_cloud_nav_x.set_title('Nav X') ax_cloud_nav_y = fig_clouds.add_subplot(236) ax_cloud_nav_y.set_title('Nav Y') # Reference new_reference = True # Forever loop will be halted by VREP client or by exception first_iteration = True k = 1 # Iterations Counter # Instantiate Trajectory Object (list) trajectory_path = [] # Read Starting clouds from files: if readfiles[0]: f_controller_alt_init = open(CONTROL_INIT_PATH + 'controller_alt_init') controller_alt_init = pickle.load(f_controller_alt_init) if print_stuff: print 'SPARC: Alt Controller Loaded', f_controller_alt_init.name if readfiles[1]: f_controller_yaw_init = open(CONTROL_INIT_PATH + 'controller_yaw_init') controller_yaw_init = pickle.load(f_controller_yaw_init) if print_stuff: print 'SPARC: Yaw Controller Loaded', f_controller_yaw_init.name if readfiles[2]: f_controller_pitch_init = open(CONTROL_INIT_PATH + 'controller_pitch_init') controller_pitch_init = pickle.load(f_controller_pitch_init) if print_stuff: print 'SPARC: Pitch Controller Loaded', f_controller_pitch_init.name if readfiles[3]: f_controller_roll_init = open(CONTROL_INIT_PATH + 'controller_roll_init') controller_roll_init = pickle.load(f_controller_roll_init) if print_stuff: print 'SPARC: Roll Controller Loaded', f_controller_roll_init.name if readfiles[4]: f_controller_nav_y_init = open(CONTROL_INIT_PATH + 'controller_navy_init') controller_nav_y_init = pickle.load(f_controller_nav_y_init) if print_stuff: print 'SPARC: NavY Controller Loaded', f_controller_nav_y_init.name else: controller_nav_y_init = None if readfiles[5]: f_controller_nav_x_init = open(CONTROL_INIT_PATH + 'controller_navx_init') controller_nav_x_init = pickle.load(f_controller_nav_x_init) if print_stuff: print 'SPARC: NavX Controller Loaded', f_controller_nav_x_init.name else: controller_nav_x_init = None # Read Starting trajectory from file: if read_trajectory: f_trajectory = open(CONTROL_INIT_PATH + 'trajectory') trajectory_path = pickle.load(f_trajectory) f_trajectory.close() if print_stuff: print 'SPARC: Trajectory Loaded', f_trajectory.name curr_traj_idx = 0 # Instantiate Navigation Controllers and References #x_reference = Reference([0.0, 0.0, 5., 5.], [10., 10., 10., 10.], 1) #y_reference = Reference([0.0, 5., 5., 0.], [10., 10., 10., 10.], 1) # x_reference = Reference([0.0, 5., 5., 0.], [5., 10., 5., 10.], 1) # y_reference = Reference([0.0, 5., 5., 0.], [5., 10., 5., 10.], 1) x_reference = Reference([0.0, 0.0, 5.0, 5.0, 0., 0., 5., 5., 5., 0.], [5., 10., 5., 10., 5., 10., 10., 5., 10., 5.], mode = 2) y_reference = Reference([0.0, 0.0, 5.0, 5.0, 0., 0., 5., 5., 5., 0.], [5., 10., 5., 10., 5., 10., 10., 5., 10., 5.], mode = 2) #x_reference = Reference([0.0, 0.0, 5.0, 5.0], [5., 10., 5., 10.], mode = 2) #y_reference = Reference([0.0, 0.0, 5.0, 5.0], [5., 10., 5., 10.], mode = 2) #xy_reference = Reference([[0.0, 0.0], [0.0, 0.0], 5], [10., 10.], mode = 4) nav = navigation_controller_classical.navigation_controller(np.array([0., 0.]), np.array([0., 0.]), np.array([0., 0.]), np.array([0., 0.])) if control[0]: #alt_reference = Reference([1.0, 1.0, 4., 4.], [5., 5., 24., 5.], 1) #alt_reference = Reference([0.16, 2.0, 2.0, 2.0, 2., 2., 2., 0.14, 2., 2.], [5., 10., 5., 10., 5., 10., 10., 5., 10., 5.], mode=2) alt_reference = Reference([0.16, 2.0, 2.0, 0.20, 2.0, 2.0, 2., 2.], [5., 10., 5., 10., 5., 10., 5., 5.], mode = 2) else: alt_reference = Reference([0.0], [10.]) if control[1]: #yaw_reference = Reference([0.0, 0.0, 45., 45.], [5., 5., 24., 5.], 1) yaw_reference = Reference([0.0], [10.]) else: yaw_reference = Reference([0.0], [10.]) prev_yaw = 0.0 prev_pitch = 0.0 prev_roll = 0.0 curr_yaw = 0.0 curr_pitch = 0.0 curr_roll = 0.0 while True: # Get core data from client clientData = receive_floats(client, 10) if print_stuff: print 'SPARC: k=', k if print_stuff: print 'SPARC: ClientData Received:' #if not clientData: # break # Quit on timeout if not clientData: break # Unpack IMU data timestepSeconds = clientData[0] positionXMeters = clientData[1] positionYMeters = clientData[2] positionZMeters = clientData[3] alphaRadians = clientData[4] betaRadians = clientData[5] gammaRadians = clientData[6] target_x = clientData[7] target_y = clientData[8] target_z = clientData[9] # Add some Guassian noise (example) # positionXMeters = random.gauss(positionXMeters, GPS_NOISE_METERS) # positionYMeters = random.gauss(positionYMeters, GPS_NOISE_METERS) # Convert Euler angles to pitch, roll, yaw # See http://en.wikipedia.org/wiki/Flight_dynamics_(fixed-wing_aircraft) for positive/negative orientation rollAngleRadians, pitchAngleRadians = rotate((alphaRadians, betaRadians), gammaRadians) pitchAngleRadians = -pitchAngleRadians yawAngleRadians = -gammaRadians yawAngleDegrees = yawAngleRadians*RAD2DEG pitchAngleDegrees = pitchAngleRadians*RAD2DEG rollAngleDegrees = rollAngleRadians*RAD2DEG dr = rollAngleDegrees - prev_roll dp = pitchAngleDegrees - prev_pitch dy = yawAngleDegrees - prev_yaw if dr >= 0: dr = math.fmod(dr + 180., 2 * 180.) - 180. else: dr = math.fmod(dr - 180., 2 * 180.) + 180. if dp >= 0: dp = math.fmod(dp + 180., 2 * 180.) - 180. else: dp = math.fmod(dp - 180., 2 * 180.) + 180. if dy >= 0: dy = math.fmod(dy + 180., 2 * 180.) - 180. else: dy = math.fmod(dy - 180., 2 * 180.) + 180. prev_yaw = yawAngleDegrees prev_pitch = pitchAngleDegrees prev_roll = rollAngleDegrees curr_yaw = curr_yaw + dy curr_pitch = curr_pitch + dp curr_roll = curr_roll + dr # Get altitude directly from position Z altitudeMeters = positionZMeters # y : [alt, yaw, pitch, roll] curr_y = [altitudeMeters, curr_yaw, curr_pitch, curr_roll] curr_ref = [0., 0., 0., 0.] if read_trajectory: if curr_traj_idx == len(trajectory_path): trajectory_path.reverse() curr_traj_idx = 0 x_curr_ref = trajectory_path[curr_traj_idx][0] y_curr_ref = trajectory_path[curr_traj_idx][1] curr_ref[0] = trajectory_path[curr_traj_idx][2] curr_traj_idx += 1 elif trajectory: x_curr_ref = target_x y_curr_ref = target_y curr_ref[0] = target_z else: x_curr_ref = x_reference.get_next(timestepSeconds) y_curr_ref = y_reference.get_next(timestepSeconds) curr_ref[0] = alt_reference.get_next(timestepSeconds) # Set References. curr_ref[1] = yaw_reference.get_next(timestepSeconds) curr_ref[2], curr_ref[3] = nav.update([positionXMeters, positionYMeters], [x_curr_ref, y_curr_ref]) curr_ref[2] = -curr_ref[2] curr_ref[3] = -curr_ref[3] if print_stuff: print 'SPARC: curr_y =', curr_y if print_stuff: print 'SPARC: curr_ref =', curr_ref # Start prev_ values as the first values on the first iteration: if first_iteration: prev_y = curr_y[:] prev_ref = curr_ref[:] # Generate Input (Pack error and error derivative to form x) curr_x = [generate_input(curr_y[0], prev_y[0], curr_ref[0], prev_ref[0], timestepSeconds), generate_input(curr_y[1], prev_y[1], curr_ref[1], prev_ref[1], timestepSeconds), generate_input(curr_y[2], prev_y[2], curr_ref[2], prev_ref[2], timestepSeconds), generate_input(curr_y[3], prev_y[3], curr_ref[3], prev_ref[3], timestepSeconds)] # Stores on list for plotting: ypoints_alt.append(curr_y[0]) refpoints_alt.append(curr_ref[0]) # Stores on list for plotting: ypoints_pitch.append(curr_y[2]) refpoints_pitch.append(curr_ref[2]) # Stores on list for plotting: ypoints_roll.append(curr_y[3]) refpoints_roll.append(curr_ref[3]) # Stores on list for plotting: ypoints_yaw.append(curr_y[1]) refpoints_yaw.append(curr_ref[1]) # Stores on list for plotting: ypoints_x.append(positionXMeters) refpoints_x.append(x_curr_ref) # Stores on list for plotting: ypoints_y.append(positionYMeters) refpoints_y.append(y_curr_ref) # Save trajectory if needed (does not let the quadcopter run): if record_trajectory: trajectory_path.append([x_curr_ref, y_curr_ref, curr_ref[0], timestepSeconds]) control = [False]*4 # if print_stuff: print result (curr_ref - curr_y) # if print_stuff: print "Step:", k, " | y:", curr_y, " | err:", np.subtract(curr_y,curr_ref).tolist(), " | u:", curr_u # if k % 100 == 0: # if print_stuff: print 't[s]:', k*timestepSeconds # if print_stuff: print '#clouds:', 'alt =', len(controller_alt.clouds),\ # 'yaw =', len(controller_yaw.clouds),\ # 'pitch =', len(controller_pitch.clouds),\ # 'roll =', len(controller_roll.clouds) # On the first iteration, initializes the controller with the first values if first_iteration: curr_u = [0., 0., 0., 0.] prev_u = [0., 0., 0., 0.] # Instantiates Controller and does not update model: # Altitude Controller if readfiles[0]: controller_alt = controller_alt_init curr_u[0] = controller_alt.update(curr_x[0], curr_y[0], curr_ref[0], prev_u[0]) if control[0] else 0.0 else: controller_alt = fuzzy_control.fuzzyController(ALTITUDE_RANGE,[-1000,1000], variance=ALT_VAR) # Yaw Controller if readfiles[1]: controller_yaw = controller_yaw_init curr_u[1] = controller_yaw.update(curr_x[1], curr_y[1], curr_ref[1], prev_u[1]) if control[1] else 0.0 else: controller_yaw = fuzzy_control.fuzzyController(YAW_RANGE,[-100,100], variance=ANGLE_VAR) # Pitch Controller if readfiles[2]: controller_pitch = controller_pitch_init curr_u[2] = controller_pitch.update(curr_x[2], curr_y[2], curr_ref[2], prev_u[2]) if control[2] else 0.0 else: controller_pitch = fuzzy_control.fuzzyController(PITCH_RANGE,[-100,100], variance=ANGLE_VAR) # Roll Controller if readfiles[3]: controller_roll = controller_roll_init curr_u[3] = controller_roll.update(curr_x[3], curr_y[3], curr_ref[3], prev_u[3]) if control[3] else 0.0 else: controller_roll = fuzzy_control.fuzzyController(ROLL_RANGE,[-100,100], variance=ANGLE_VAR) first_iteration = False else: # if print_stuff: print tested inputs if control[0]: if print_stuff: print 'SPARC: Alt_input = ', curr_x[0], curr_y[0], curr_ref[0] if control[1]: if print_stuff: print 'SPARC: Yaw_input = ', curr_x[1], curr_y[1], curr_ref[1] if control[2]: if print_stuff: print 'SPARC: Pitch_input = ', curr_x[2], curr_y[2], curr_ref[2] if control[3]: if print_stuff: print 'SPARC: Roll_input = ', curr_x[3], curr_y[3], curr_ref[3] # Gets the output of the controller for the current input x if control[0]: alt_u = 10*controller_alt.output([curr_x[0][0],curr_x[0][1]]) if control[1]: yaw_u = controller_yaw.output([curr_x[1][0],curr_x[1][1]]) if control[2]: pitch_u = controller_pitch.output([curr_x[2][0],curr_x[2][1]]) if control[3]: roll_u = controller_roll.output([curr_x[3][0],curr_x[3][1]]) # if print_stuff: print 'SPARC: Yaw_control = ', yaw_u # curr_u = [alt_u, yaw_u, pitch_u, roll_u] curr_u = [0.0, 0.0, 0.0, 0.0] damped_u = [0.0, 0.0, 0.0, 0.0] curr_u[0] = alt_u if control[0] else 0.0 curr_u[1] = yaw_u if control[1] else 0.0 curr_u[2] = pitch_u if control[2] else 0.0 curr_u[3] = roll_u if control[3] else 0.0 # Add artificial damping # print 'timestep: ', timestepSeconds damped_u[0] = curr_u[0] - Kv_h * (curr_y[0] - prev_y[0])/timestepSeconds damped_u[1] = curr_u[1] - Kv_y * (curr_y[1] - prev_y[1])/timestepSeconds damped_u[2] = curr_u[2] - Kv_pr * (curr_y[2] - prev_y[2])/timestepSeconds damped_u[3] = curr_u[3] - Kv_pr * (curr_y[3] - prev_y[3])/timestepSeconds # if print_stuff: print 'SPARC: Damping (undamped_u, damped_u) = ', curr_u[3], damped_u[3] curr_u[:] = damped_u[:] # Prevent Over Excursion if curr_u[0] > UMAX_ALT: curr_u[0] = UMAX_ALT if curr_u[0] < UMIN_ALT: curr_u[0] = UMIN_ALT if curr_u[1] > UMAX_YAW: curr_u[1] = UMAX_YAW if curr_u[1] < UMIN_YAW: curr_u[1] = UMIN_YAW if curr_u[2] > UMAX_PITCHROLL: curr_u[2] = UMAX_PITCHROLL if curr_u[2] < UMIN_PITCHROLL: curr_u[2] = UMIN_PITCHROLL if curr_u[3] > UMAX_PITCHROLL: curr_u[3] = UMAX_PITCHROLL if curr_u[3] < UMIN_PITCHROLL: curr_u[3] = UMIN_PITCHROLL # Speed on Engines: motors = [0.] * 4 #motors[0] = float(UPARKED + curr_u[0] + curr_u[1] - curr_u[2] + curr_u[3]) #motors[1] = float(UPARKED + curr_u[0] - curr_u[1] + curr_u[2] + curr_u[3]) #motors[2] = float(UPARKED + curr_u[0] + curr_u[1] + curr_u[2] - curr_u[3]) #motors[3] = float(UPARKED + curr_u[0] - curr_u[1] - curr_u[2] - curr_u[3]) motors[0] = float((UPARKED + curr_u[0]) * (1 - curr_u[1]/100. + curr_u[2]/100. - curr_u[3]/100.)) motors[1] = float((UPARKED + curr_u[0]) * (1 + curr_u[1]/100. + curr_u[2]/100. + curr_u[3]/100.)) motors[2] = float((UPARKED + curr_u[0]) * (1 - curr_u[1]/100. - curr_u[2]/100. + curr_u[3]/100.)) motors[3] = float((UPARKED + curr_u[0]) * (1 + curr_u[1]/100. - curr_u[2]/100. - curr_u[3]/100.)) # Stores on list for plotting: motor_points[0].append(motors[0]) motor_points[1].append(motors[1]) motor_points[2].append(motors[2]) motor_points[3].append(motors[3]) c_alt_points.append(UPARKED + curr_u[0]) c_yaw_points.append((curr_u[1])) c_pitch_points.append((curr_u[2])) c_roll_points.append((curr_u[3])) kpoints.append(time_elapsed) # Send speed to Engines send_floats(client, motors) if print_stuff: print 'SPARC: Control Signal Sent!' if print_stuff: print 'SPARC: Control Signal: ', curr_u # debug = 'T' # if debug == 'T': # if print_stuff: print 'SPARC: #CloudsYaw= ', len(controller_yaw.clouds) # Update prev values prev_u = curr_u[:] prev_y = curr_y[:] prev_ref = curr_ref[:] # Increment K k += 1 time_elapsed += timestepSeconds # Plotting and saving if k > 10: # Plot Motors axes_motors.plot(kpoints, motor_points[0], 'r') axes_motors.plot(kpoints, motor_points[1], 'y') axes_motors.plot(kpoints, motor_points[2], 'b') axes_motors.plot(kpoints, motor_points[3], 'g') # Plot Altitude (Reference and Result) axes_alt.plot(kpoints, refpoints_alt, 'r') axes_alt.plot(kpoints, ypoints_alt, 'b') # Plot Roll (Reference and Result) axes_roll.plot(kpoints, refpoints_roll, 'r') axes_roll.plot(kpoints, ypoints_roll, 'b') # Plot Pitch (Reference and Result) axes_pitch.plot(kpoints, refpoints_pitch, 'r') axes_pitch.plot(kpoints, ypoints_pitch, 'b') # Plot Yaw (Reference and Result) axes_yaw.plot(kpoints, refpoints_yaw, 'r') axes_yaw.plot(kpoints, ypoints_yaw, 'b') # Plot Y (Reference and Result) axes_y.plot(kpoints, refpoints_y, 'r') axes_y.plot(kpoints, ypoints_y, 'b') # Plot X (Reference and Result) axes_x.plot(kpoints, refpoints_x, 'r') axes_x.plot(kpoints, ypoints_x, 'b') # Plot XY (Reference and Result) axes_xy.plot(refpoints_x, refpoints_y, 'r') axes_xy.plot(ypoints_x, ypoints_y, 'b') # Reconfigure limits of XY to keep proportion. axes_xy.set_xlim([-10, 10]) axes_xy.set_ylim([-10, 10]) # Plot Control Signals ax_c_alt.plot(kpoints, c_alt_points, 'r') ax_c_pitch.plot(kpoints, c_pitch_points, 'y') ax_c_roll.plot(kpoints, c_roll_points, 'b') ax_c_yaw.plot(kpoints, c_yaw_points, 'g') # Record Trajectory: if record_trajectory: f_trajectory = open(CONTROL_INIT_PATH + 'trajectory', 'w') pickle.dump(trajectory_path, f_trajectory) f_trajectory.close() if print_stuff: print 'SPARC: Trajectory Serialized', f_trajectory.name # Calculate Quadratic Error Avg quad_err_alt = [i**2 for i in np.array(ypoints_alt) - np.array(refpoints_alt)] quad_err_pitch = [i**2 for i in np.array(ypoints_pitch) - np.array(refpoints_pitch)] quad_err_roll = [i**2 for i in np.array(ypoints_roll) - np.array(refpoints_roll)] quad_err_yaw = [i**2 for i in np.array(ypoints_yaw) - np.array(refpoints_yaw)] quad_err_x = [i**2 for i in np.array(ypoints_x) - np.array(refpoints_x)] quad_err_y = [i**2 for i in np.array(ypoints_y) - np.array(refpoints_y)] quad_err_alt_avg = np.mean(quad_err_alt) quad_err_pitch_avg = np.mean(quad_err_pitch) quad_err_roll_avg = np.mean(quad_err_roll) quad_err_yaw_avg = np.mean(quad_err_yaw) quad_err_x_avg = np.mean(quad_err_x) quad_err_y_avg = np.mean(quad_err_y) # Print Error Values print 'Square Mean Alt:', quad_err_alt_avg print 'Square Mean Pitch:', quad_err_pitch_avg print 'Square Mean Roll:', quad_err_roll_avg print 'Square Mean Yaw:', quad_err_yaw_avg print 'Square Mean X:', quad_err_x_avg print 'Square Mean Y:', quad_err_y_avg plt.show()
def writeln(self, string): self.fd.write(string + '\n') self.fd.flush() def close(self): self.fd.close() # Initialization ========================================================================================================== # Require controller controller = Controller(('Stabilize', 'Alt-hold', 'Pos-hold')) # Serve a socket on the port indicated in the first command-line argument client = serve_socket(int(argv[1])) # Receive working directory path from client pyquadsim_directory = receiveString(client) # Receive particle info from client particleInfo = receiveFloats(client, 6) particleSizes = particleInfo[0:4] particleDensity = particleInfo[4] particleCountPerSecond = particleInfo[5] # Create logs folder if needed logdir = pyquadsim_directory + '/logs' if not os.path.exists(logdir): os.mkdir(logdir)
parser = MyParser() parser.set_ATTITUDE_Handler(parser.attitudeHandler) parser.set_ALTITUDE_Handler(parser.altitudeHandler) # Serialize the telemetry message requests that we'll send to the "firwmare" attitude_request = serialize_ATTITUDE_Request() altitude_request = serialize_ALTITUDE_Request() # More than two command-line arguments means simulation mode. First arg is camera-client port, # second is MSP port, third is input image file name, fourth is outpt image file name. if len(sys.argv) > 2: from socket_server import serve_socket # Serve a socket for camera synching, and a socket for comms camera_client = serve_socket(int(sys.argv[1])) comms_to_client = serve_socket(int(sys.argv[2])) comms_from_client = serve_socket(int(sys.argv[3])) image_from_sim_name = sys.argv[4] image_to_sim_name = sys.argv[5] # Run serial comms telemetry reading on its own thread thread = threading.Thread(target=commsReader, args=(comms_from_client, parser)) thread.daemon = True thread.start() while True: # Receive the camera sync byte from the client camera_client.recv(1)
self.fd.write(string + '\n') self.fd.flush() def close(self): self.fd.close() # Initialization ===================================================================================================== # Require controller controller = Controller(('Stabilize', 'Hold Altitude', 'Unused')) # Serve a socket on the port indicated in the first command-line argument client = serve_socket(int(argv[1])) # Receive working directory path from client pyquadsim_directory = receiveString(client) # Create logs folder if needed logdir = pyquadsim_directory + '/logs' if not os.path.exists(logdir): os.mkdir(logdir) # Open logfile named by current date, time logfile = LogFile(logdir) # Create an FMU object, passing it the logfile object in case it needs to write to the logfile. fmu = FMU(logfile)
parser = MyParser() parser.set_ATTITUDE_Handler(parser.attitudeHandler) parser.set_ALTITUDE_Handler(parser.altitudeHandler) # Serialize the telemetry message requests that we'll send to the "firwmare" attitude_request = serialize_ATTITUDE_Request() altitude_request = serialize_ALTITUDE_Request() # More than two command-line arguments means simulation mode. First arg is camera-client port, # second is MSP port, third is input image file name, fourth is outpt image file name. if len(sys.argv) > 2: from socket_server import serve_socket # Serve a socket for camera synching, and a socket for comms camera_client = serve_socket(int(sys.argv[1])) comms_to_client = serve_socket(int(sys.argv[2])) comms_from_client = serve_socket(int(sys.argv[3])) image_from_sim_name = sys.argv[4] image_to_sim_name = sys.argv[5] # Run serial comms telemetry reading on its own thread thread = threading.Thread(target=commsReader, args = (comms_from_client,parser)) thread.daemon = True thread.start() while True: # Receive the camera sync byte from the client camera_client.recv(1)
parser = MyParser() parser.set_ATTITUDE_Handler(parser.attitudeHandler) parser.set_ALTITUDE_Handler(parser.altitudeHandler) # Serialize the telemetry message requests that we'll send to the "firwmare" attitude_request = serialize_ATTITUDE_Request() altitude_request = serialize_ALTITUDE_Request() # More than two command-line arguments means simulation mode. First arg is camera-client port, # second is MSP port, third is input image file name, fourth is outpt image file name. if len(sys.argv) > 2: from socket_server import serve_socket # Serve a socket for camera synching, and a socket for comms camera_client = serve_socket(int(sys.argv[1])) comms_out_client = serve_socket(int(sys.argv[2])) comms_in_client = serve_socket(int(sys.argv[3])) image_from_sim_name = sys.argv[4] image_to_sim_name = sys.argv[5] # Run serial comms telemetry reading on its own thread thread = threading.Thread(target=commsReader, args = (comms_in_client,parser)) thread.daemon = True thread.start() while True: # Receive the camera sync byte from the client camera_client.recv(1)
def test_sparc_model(print_stuff=False, trajectory=True, record_trajectory=True, read_trajectory=False, control=[True] * 4, readfiles=[True] * 4, recordfiles=[True] * 4): # Connect to Client (V-REP) try: client = serve_socket(int(argv[1])) except: print("Client not found. Exiting...") exit(0) # Receive working directory path from client # _ = receive_string(client) # List for plotting time axis kpoints = [] time_elapsed = 0.0 # Instantiates figure for plotting motor angular velocities: fig_motors = plt.figure('Motors', figsize=(14, 10)) axes_motors = fig_motors.add_axes([0.1, 0.1, 0.8, 0.8]) motor_points = [[], [], [], []] """:type : list[list[float]]""" # Instantiates figure for plotting stabilization results: fig_control, (axes_alt, axes_pitch, axes_roll, axes_yaw) = plt.subplots(4, 1, sharex=True, sharey=False, figsize=(14, 10)) fig_control.canvas.set_window_title( 'Quadcopter Stability - Commanded(RED), Performed(BLUE)') axes_alt.set_title('Alt') axes_pitch.set_title('Pitch') axes_roll.set_title('Roll') axes_yaw.set_title('Yaw') ypoints_alt = [] refpoints_alt = [] ypoints_pitch = [] refpoints_pitch = [] ypoints_roll = [] refpoints_roll = [] ypoints_yaw = [] refpoints_yaw = [] # Instantiates figure for plotting navigation results: fig_nav = plt.figure('Quadcopter Navigation', figsize=(14, 10)) axes_x = fig_nav.add_subplot(221) axes_x.set_title('X') ypoints_x = [] refpoints_x = [] axes_y = fig_nav.add_subplot(222) axes_y.set_title('Y') ypoints_y = [] refpoints_y = [] axes_xy = fig_nav.add_subplot(212) axes_xy.set_title('XY') # Instantiates figure for plotting control signals: fig_control, (ax_c_alt, ax_c_yaw, ax_c_pitch, ax_c_roll) = plt.subplots(4, 1, sharex=True, sharey=False, figsize=(14, 10)) fig_control.canvas.set_window_title('Control Effort') ax_c_alt.set_title('Alt') ax_c_yaw.set_title('Yaw') ax_c_pitch.set_title('Pitch') ax_c_roll.set_title('Roll') c_alt_points = [] c_pitch_points = [] c_roll_points = [] c_yaw_points = [] # Instantiates figure for plotting clouds: fig_clouds = plt.figure('Data Clouds', figsize=(14, 10)) ax_cloud_alt = fig_clouds.add_subplot(231) ax_cloud_alt.set_title('Alt') ax_cloud_yaw = fig_clouds.add_subplot(232) ax_cloud_yaw.set_title('Yaw') ax_cloud_pitch = fig_clouds.add_subplot(233) ax_cloud_pitch.set_title('Pitch') ax_cloud_roll = fig_clouds.add_subplot(234) ax_cloud_roll.set_title('Roll') ax_cloud_nav_x = fig_clouds.add_subplot(235) ax_cloud_nav_x.set_title('Nav X') ax_cloud_nav_y = fig_clouds.add_subplot(236) ax_cloud_nav_y.set_title('Nav Y') # Reference new_reference = True # Forever loop will be halted by V-REP client or by exception first_iteration = True k = 1 # Iterations Counter # Instantiate Trajectory Object (list) trajectory_path = [] curr_traj_idx = 0 # Read Starting clouds from files: controller_alt_init, controller_yaw_init, controller_pitch_init, controller_roll_init = 0, 0, 0, 0 if readfiles[0]: f_controller_alt_init = open(CONTROL_INIT_PATH + 'controller_alt_init') controller_alt_init = pickle.load(f_controller_alt_init) if print_stuff: print 'SPARC: Alt Controller Loaded', f_controller_alt_init.name if readfiles[1]: f_controller_yaw_init = open(CONTROL_INIT_PATH + 'controller_yaw_init') controller_yaw_init = pickle.load(f_controller_yaw_init) if print_stuff: print 'SPARC: Yaw Controller Loaded', f_controller_yaw_init.name if readfiles[2]: f_controller_pitch_init = open(CONTROL_INIT_PATH + 'controller_pitch_init') controller_pitch_init = pickle.load(f_controller_pitch_init) if print_stuff: print 'SPARC: Pitch Controller Loaded', f_controller_pitch_init.name if readfiles[3]: f_controller_roll_init = open(CONTROL_INIT_PATH + 'controller_roll_init') controller_roll_init = pickle.load(f_controller_roll_init) if print_stuff: print 'SPARC: Roll Controller Loaded', f_controller_roll_init.name if readfiles[4]: f_controller_nav_y_init = open(CONTROL_INIT_PATH + 'controller_navy_init') controller_nav_y_init = pickle.load(f_controller_nav_y_init) if print_stuff: print 'SPARC: NavY Controller Loaded', f_controller_nav_y_init.name else: controller_nav_y_init = None if readfiles[5]: f_controller_nav_x_init = open(CONTROL_INIT_PATH + 'controller_navx_init') controller_nav_x_init = pickle.load(f_controller_nav_x_init) if print_stuff: print 'SPARC: NavX Controller Loaded', f_controller_nav_x_init.name else: controller_nav_x_init = None # Read Starting trajectory from file: if read_trajectory: f_trajectory = open(CONTROL_INIT_PATH + 'trajectory') trajectory_path = pickle.load(f_trajectory) f_trajectory.close() if print_stuff: print 'SPARC: Trajectory Loaded', f_trajectory.name curr_traj_idx = 0 # Instantiate Navigation Controllers and References # x_reference = Reference([0.0, 0.0, 5., 5.], [10., 10., 10., 10.], 1) # y_reference = Reference([0.0, 5., 5., 0.], [10., 10., 10., 10.], 1) # x_reference = Reference([0.0, 5., 5., 0.], [5., 10., 5., 10.], 1) # y_reference = Reference([0.0, 5., 5., 0.], [5., 10., 5., 10.], 1) x_reference = Reference([0.0, 0.0, 5.0, 5.0, 0., 0., 5., 5., 5., 0.], [5., 10., 5., 10., 5., 10., 10., 5., 10., 5.], mode=2) y_reference = Reference([0.0, 0.0, 5.0, 5.0, 0., 0., 5., 5., 5., 0.], [5., 10., 5., 10., 5., 10., 10., 5., 10., 5.], mode=2) # x_reference = Reference([0.0, 0.0, 5.0, 5.0], [5., 10., 5., 10.], mode = 2) # y_reference = Reference([0.0, 0.0, 5.0, 5.0], [5., 10., 5., 10.], mode = 2) # xy_reference = Reference([[0.0, 0.0], [0.0, 0.0], 5], [10., 10.], mode = 4) nav = navigationcontroller.NavigationController(np.array([0., 0.]), np.array([0., 0.]), np.array([0., 0.]), np.array([0., 0.]), controller_nav_y_init, controller_nav_x_init) # Alt Reference if control[0]: # alt_reference = Reference([1.0, 1.0, 4., 4.], [5., 5., 24., 5.], 1) # alt_reference = Reference([0.16, 2.0, 2.0, 2.0, 2., 2., 2., 0.14, 2., 2.], [5., 10., 5., 10., 5., 10., 10., # 5.,10., 5.], mode=2) alt_reference = Reference([0.16, 2.0, 2.0, 0.20, 2.0, 2.0, 2., 2.], [5., 10., 5., 10., 5., 10., 5., 5.], mode=2) else: alt_reference = Reference([0.0], [10.]) # Yaw Reference is Disabled by Default # if control[1]: # yaw_reference = Reference([0.0, 0.0, 45., 45., 90., 90., -90., -90., -30, -30], # [5., 5., 24., 5., .5, .5, .5, .5, .5, .5], 1) # else: # yaw_reference = Reference([0.0], [10.]) prev_y = [0., 0., 0., 0.] prev_u = [0., 0., 0., 0.] prev_ref = [0., 0., 0., 0.] prev_yaw = 0.0 prev_pitch = 0.0 prev_roll = 0.0 curr_yaw = 0.0 curr_pitch = 0.0 curr_roll = 0.0 upside_down = False while True: # Get core data from client clientdata = receive_floats(client, 10) if print_stuff: print 'SPARC: k=', k print 'SPARC: ClientData Received:' # Quit on timeout if not clientdata: break # Unpack IMU data timestepseconds = clientdata[0] positionxmeters = clientdata[1] positionymeters = clientdata[2] positionzmeters = clientdata[3] alpharadians = clientdata[4] betaradians = clientdata[5] gammaradians = clientdata[6] target_x = clientdata[7] target_y = clientdata[8] target_z = clientdata[9] # Add some Gaussian noise (example) # positionxmeters = random.gauss(positionxmeters, GPS_NOISE_METERS) # positionymeters = random.gauss(positionymeters, GPS_NOISE_METERS) # Convert Euler angles to pitch, roll, yaw # See http://en.wikipedia.org/wiki/Flight_dynamics_(fixed-wing_aircraft) for positive/negative orientation rollangleradians, pitchangleradians = rotate( (alpharadians, betaradians), gammaradians) pitchangleradians = -pitchangleradians yawangleradians = -gammaradians yawangledegrees = yawangleradians * RAD2DEG pitchangledegrees = pitchangleradians * RAD2DEG rollangledegrees = rollangleradians * RAD2DEG dr = rollangledegrees - prev_roll dp = pitchangledegrees - prev_pitch dy = yawangledegrees - prev_yaw if dr >= 0: dr = math.fmod(dr + 180., 2 * 180.) - 180. else: dr = math.fmod(dr - 180., 2 * 180.) + 180. if dp >= 0: dp = math.fmod(dp + 180., 2 * 180.) - 180. else: dp = math.fmod(dp - 180., 2 * 180.) + 180. if dy >= 0: dy = math.fmod(dy + 180., 2 * 180.) - 180. else: dy = math.fmod(dy - 180., 2 * 180.) + 180. prev_yaw = yawangledegrees prev_pitch = pitchangledegrees prev_roll = rollangledegrees curr_yaw += dy curr_pitch += dp curr_roll += dr # Get altitude directly from position Z altitudemeters = positionzmeters # y : [alt, yaw, pitch, roll] curr_y = [altitudemeters, curr_yaw, curr_pitch, curr_roll] curr_ref = [0., 0., 0., 0.] # Nav References if read_trajectory: if curr_traj_idx == len(trajectory_path): trajectory_path.reverse() curr_traj_idx = 0 x_curr_ref = trajectory_path[curr_traj_idx][0] y_curr_ref = trajectory_path[curr_traj_idx][1] curr_ref[0] = trajectory_path[curr_traj_idx][2] curr_traj_idx += 1 elif trajectory: x_curr_ref = target_x y_curr_ref = target_y curr_ref[0] = target_z else: x_curr_ref = x_reference.get_next(timestepseconds) y_curr_ref = y_reference.get_next(timestepseconds) curr_ref[0] = alt_reference.get_next(timestepseconds) # Hard Coded Yaw Reference (Keep on zero) curr_ref[1] = 0.0 # Pitch/Roll References from Nav References curr_ref[2], curr_ref[3] = nav.update( [positionxmeters, positionymeters], [x_curr_ref, y_curr_ref]) # Convert pitch/roll input from radians to degrees curr_ref[2] = -curr_ref[2] * RAD2DEG curr_ref[3] = -curr_ref[3] * RAD2DEG if print_stuff: print 'SPARC: curr_y =', curr_y print 'SPARC: curr_ref =', curr_ref # Start prev_ values as the first values on the first iteration: if first_iteration: prev_y = curr_y[:] prev_ref = curr_ref[:] # If reference curve has changed, update C. if (not first_iteration) and new_reference: if control[0]: controller_alt.update_reference_range(REFMIN_ALT, REFMAX_ALT) if control[2]: controller_pitch.update_reference_range( REFMIN_PITCHROLL, REFMAX_PITCHROLL) if control[3]: controller_roll.update_reference_range(REFMIN_PITCHROLL, REFMAX_PITCHROLL) if control[1]: controller_yaw.update_reference_range(REFMIN_YAW, REFMAX_YAW) new_reference = False # Generate Input (Pack error and error derivative to form x) curr_x = [ generate_input(curr_y[0], prev_y[0], curr_ref[0], prev_ref[0]), generate_input(curr_y[1], prev_y[1], curr_ref[1], prev_ref[1]), generate_input(curr_y[2], prev_y[2], curr_ref[2], prev_ref[2]), generate_input(curr_y[3], prev_y[3], curr_ref[3], prev_ref[3]) ] # Stores on list for plotting: ypoints_alt.append(curr_y[0]) refpoints_alt.append(curr_ref[0]) # Stores on list for plotting: ypoints_pitch.append(curr_y[2]) refpoints_pitch.append(curr_ref[2]) # Stores on list for plotting: ypoints_roll.append(curr_y[3]) refpoints_roll.append(curr_ref[3]) # Stores on list for plotting: ypoints_yaw.append(curr_y[1]) refpoints_yaw.append(curr_ref[1]) # Stores on list for plotting: ypoints_x.append(positionxmeters) refpoints_x.append(x_curr_ref) # Stores on list for plotting: ypoints_y.append(positionymeters) refpoints_y.append(y_curr_ref) # Save trajectory if needed (does not let the quad run): if record_trajectory: trajectory_path.append( [x_curr_ref, y_curr_ref, curr_ref[0], timestepseconds]) control = [False] * 4 # On the first iteration, initializes the controller with the first values if first_iteration: curr_u = [0., 0., 0., 0.] prev_u = [0., 0., 0., 0.] # Instantiates Controller and does not update model: # Altitude Controller if readfiles[0]: controller_alt = controller_alt_init curr_u[0] = controller_alt.update( curr_x[0], curr_y[0], curr_ref[0], prev_u[0]) if control[0] else 0.0 else: controller_alt = sparc.SparcController( (UMIN_ALT, UMAX_ALT), (REFMIN_ALT, REFMAX_ALT), X_SIZE, curr_x[0], curr_ref[0], curr_y[0]) curr_u[0] = controller_alt.clouds[0].get_consequent( ) if control[0] else 0.0 # Yaw Controller if readfiles[1]: controller_yaw = controller_yaw_init curr_u[1] = controller_yaw.update( curr_x[1], curr_y[1], curr_ref[1], prev_u[1]) if control[1] else 0.0 else: controller_yaw = sparc.SparcController( (UMIN_YAW, UMAX_YAW), (REFMIN_YAW, REFMAX_YAW), X_SIZE, curr_x[1], curr_ref[1], curr_y[1]) curr_u[1] = controller_yaw.clouds[0].get_consequent( ) if control[1] else 0.0 # Pitch Controller if readfiles[2]: controller_pitch = controller_pitch_init curr_u[2] = controller_pitch.update( curr_x[2], curr_y[2], curr_ref[2], prev_u[2]) if control[2] else 0.0 else: controller_pitch = sparc.SparcController( (UMIN_PITCHROLL, UMAX_PITCHROLL), (REFMIN_PITCHROLL, REFMAX_PITCHROLL), X_SIZE, curr_x[2], curr_ref[2], curr_y[2]) curr_u[2] = controller_pitch.clouds[0].get_consequent( ) if control[2] else 0.0 # Roll Controller if readfiles[3]: controller_roll = controller_roll_init curr_u[3] = controller_roll.update( curr_x[3], curr_y[3], curr_ref[3], prev_u[3]) if control[3] else 0.0 else: controller_roll = sparc.SparcController( (UMIN_PITCHROLL, UMAX_PITCHROLL), (REFMIN_PITCHROLL, REFMAX_PITCHROLL), X_SIZE, curr_x[3], curr_ref[3], curr_y[3]) curr_u[3] = controller_roll.clouds[0].get_consequent( ) if control[3] else 0.0 else: # if print_stuff: print tested inputs if control[0] and print_stuff: print 'SPARC: Alt_input = ', curr_x[0], curr_y[0], curr_ref[0] if control[1] and print_stuff: print 'SPARC: Yaw_input = ', curr_x[1], curr_y[1], curr_ref[1] if control[2] and print_stuff: print 'SPARC: Pitch_input = ', curr_x[2], curr_y[2], curr_ref[ 2] if control[3] and print_stuff: print 'SPARC: Roll_input = ', curr_x[3], curr_y[3], curr_ref[3] # Gets the output of the controller for the current input x alt_u = controller_alt.update(curr_x[0], curr_y[0], curr_ref[0], prev_u[0]) if control[0] else 0 yaw_u = controller_yaw.update(curr_x[1], curr_y[1], curr_ref[1], prev_u[1]) if control[1] else 0 pitch_u = controller_pitch.update(curr_x[2], curr_y[2], curr_ref[2], prev_u[2]) if control[2] else 0 roll_u = controller_roll.update(curr_x[3], curr_y[3], curr_ref[3], prev_u[3]) if control[3] else 0 # if print_stuff: print 'SPARC: Yaw_control = ', yaw_u # curr_u = [alt_u, yaw_u, pitch_u, roll_u] curr_u = [0.0, 0.0, 0.0, 0.0] damped_u = [0.0, 0.0, 0.0, 0.0] curr_u[0] = alt_u if control[0] else 0.0 curr_u[1] = yaw_u if control[1] else 0.0 curr_u[2] = pitch_u if control[2] else 0.0 curr_u[3] = roll_u if control[3] else 0.0 # Add artificial damping # print 'timestep: ', timestepseconds damped_u[0] = curr_u[0] - Kv_h * (curr_y[0] - prev_y[0]) / timestepseconds damped_u[1] = curr_u[1] - Kv_y * (curr_y[1] - prev_y[1]) / timestepseconds damped_u[2] = curr_u[2] - Kv_pr * (curr_y[2] - prev_y[2]) / timestepseconds damped_u[3] = curr_u[3] - Kv_pr * (curr_y[3] - prev_y[3]) / timestepseconds # if print_stuff: print 'SPARC: Damping (undamped_u, damped_u) = ', curr_u[3], damped_u[3] curr_u[:] = damped_u[:] # Prevent Over Excursion curr_u[0] = np.median(np.array([UMIN_ALT, curr_u[0], UMAX_ALT])) curr_u[1] = np.median(np.array([UMIN_YAW, curr_u[1], UMAX_YAW])) curr_u[2] = np.median( np.array([UMIN_PITCHROLL, curr_u[2], UMAX_PITCHROLL])) curr_u[3] = np.median( np.array([UMIN_PITCHROLL, curr_u[3], UMAX_PITCHROLL])) # Speed on Engines: motors = [0., 0., 0., 0.] # motors[0] = float(UPARKED + curr_u[0] + curr_u[1] - curr_u[2] + curr_u[3]) # motors[1] = float(UPARKED + curr_u[0] - curr_u[1] + curr_u[2] + curr_u[3]) # motors[2] = float(UPARKED + curr_u[0] + curr_u[1] + curr_u[2] - curr_u[3]) # motors[3] = float(UPARKED + curr_u[0] - curr_u[1] - curr_u[2] - curr_u[3]) motors[0] = float( (UPARKED + curr_u[0]) * (1 - curr_u[1] / 100. + curr_u[2] / 100. - curr_u[3] / 100.)) motors[1] = float( (UPARKED + curr_u[0]) * (1 + curr_u[1] / 100. + curr_u[2] / 100. + curr_u[3] / 100.)) motors[2] = float( (UPARKED + curr_u[0]) * (1 - curr_u[1] / 100. - curr_u[2] / 100. + curr_u[3] / 100.)) motors[3] = float( (UPARKED + curr_u[0]) * (1 + curr_u[1] / 100. - curr_u[2] / 100. - curr_u[3] / 100.)) # Stores on list for plotting: motor_points[0].append(motors[0]) motor_points[1].append(motors[1]) motor_points[2].append(motors[2]) motor_points[3].append(motors[3]) c_alt_points.append(curr_u[0]) c_yaw_points.append(curr_u[1]) c_pitch_points.append(curr_u[2]) c_roll_points.append(curr_u[3]) kpoints.append(time_elapsed) # Send speed to Engines send_floats(client, motors) if print_stuff: print 'SPARC: Control Signal Sent!' print 'SPARC: Control Signal: ', curr_u # Update prev values prev_u = curr_u[:] prev_y = curr_y[:] prev_ref = curr_ref[:] # Check if its upside down. If it is, do not save new clouds. if curr_y[2] > 110. or curr_y[3] > 110.: upside_down = True # Increment K k += 1 time_elapsed += timestepseconds if first_iteration: first_iteration = False # Plotting and saving if k > 10: # Plot Motors axes_motors.plot(kpoints, motor_points[0], 'r') axes_motors.plot(kpoints, motor_points[1], 'y') axes_motors.plot(kpoints, motor_points[2], 'b') axes_motors.plot(kpoints, motor_points[3], 'g') # Plot Altitude (Reference and Result) axes_alt.plot(kpoints, refpoints_alt, 'r') axes_alt.plot(kpoints, ypoints_alt, 'b') # Plot Roll (Reference and Result) axes_roll.plot(kpoints, refpoints_roll, 'r') axes_roll.plot(kpoints, ypoints_roll, 'b') # Plot Pitch (Reference and Result) axes_pitch.plot(kpoints, refpoints_pitch, 'r') axes_pitch.plot(kpoints, ypoints_pitch, 'b') # Plot Yaw (Reference and Result) axes_yaw.plot(kpoints, refpoints_yaw, 'r') axes_yaw.plot(kpoints, ypoints_yaw, 'b') # Plot Y (Reference and Result) axes_y.plot(kpoints, refpoints_y, 'r') axes_y.plot(kpoints, ypoints_y, 'b') # Plot X (Reference and Result) axes_x.plot(kpoints, refpoints_x, 'r') axes_x.plot(kpoints, ypoints_x, 'b') # Plot XY (Reference and Result) axes_xy.plot(refpoints_x, refpoints_y, 'r') axes_xy.plot(ypoints_x, ypoints_y, 'b') # Reconfigure limits of XY to keep proportion. # xlim_range = max(axes_xy.get_xlim()) - min(axes_xy.get_xlim()) # ylim_range = max(axes_xy.get_ylim()) - min(axes_xy.get_ylim()) # # if xlim_range > ylim_range: # axes_xy.set_xlim(axes_xy.get_xlim()) # axes_xy.set_ylim(axes_xy.get_xlim()) # else: # axes_xy.set_xlim(axes_xy.get_ylim()) # axes_xy.set_ylim(axes_xy.get_ylim()) axes_xy.set_xlim([-10, 10]) axes_xy.set_ylim([-10, 10]) # Plot Control Signals ax_c_alt.plot(kpoints, c_alt_points, 'r') ax_c_pitch.plot(kpoints, c_pitch_points, 'y') ax_c_roll.plot(kpoints, c_roll_points, 'b') ax_c_yaw.plot(kpoints, c_yaw_points, 'g') # Plot Clouds if control[0]: e, de, u, r = [], [], [], [] for c in controller_alt.clouds: e.append(c.zf[0]) de.append(c.zf[1]) u.append(c.zf[2]) r = c.r ax_cloud_alt.add_patch( Ellipse((e[-1], de[-1]), r[0], r[1], facecolor='none', linestyle='dashed')) im_alt = ax_cloud_alt.scatter(e, de, c=u, cmap=plt.get_cmap("jet")) ax_cloud_alt.set_xlabel('Error') ax_cloud_alt.set_ylabel('DError') plt.colorbar(im_alt, ax=ax_cloud_alt) if recordfiles[0] and not upside_down: f_alt = open(CONTROL_INIT_PATH + 'controller_alt_init', 'w') pickle.dump(controller_alt, f_alt) f_alt.close() if print_stuff: print 'SPARC: Controller Serialized', f_alt.name if control[1]: e, de, u, r = [], [], [], [] for c in controller_yaw.clouds: e.append(c.zf[0]) de.append(c.zf[1]) u.append(c.zf[2]) r = c.r ax_cloud_yaw.add_patch( Ellipse((e[-1], de[-1]), r[0], r[1], facecolor='none', linestyle='dashed')) im_yaw = ax_cloud_yaw.scatter(e, de, c=u, cmap=plt.get_cmap("jet")) ax_cloud_yaw.set_xlabel('Error') ax_cloud_yaw.set_ylabel('DError') plt.colorbar(im_yaw, ax=ax_cloud_yaw) if recordfiles[1] and not upside_down: f_yaw = open(CONTROL_INIT_PATH + 'controller_yaw_init', 'w') pickle.dump(controller_yaw, f_yaw) f_yaw.close() if print_stuff: print 'SPARC: Controller Serialized', f_yaw.name if control[2]: e, de, u, r = [], [], [], [] for c in controller_pitch.clouds: e.append(c.zf[0]) de.append(c.zf[1]) u.append(c.zf[2]) r = c.r ax_cloud_pitch.add_patch( Ellipse((e[-1], de[-1]), r[0], r[1], facecolor='none', linestyle='dashed')) im_pitch = ax_cloud_pitch.scatter(e, de, c=u, cmap=plt.get_cmap("jet")) ax_cloud_pitch.set_xlabel('Error') ax_cloud_pitch.set_ylabel('DError') plt.colorbar(im_pitch, ax=ax_cloud_pitch) if recordfiles[2] and not upside_down: f_pitch = open(CONTROL_INIT_PATH + 'controller_pitch_init', 'w') pickle.dump(controller_pitch, f_pitch) f_pitch.close() if print_stuff: print 'SPARC: Controller Serialized', f_pitch.name if control[3]: e, de, u, r = [], [], [], [] for c in controller_roll.clouds: e.append(c.zf[0]) de.append(c.zf[1]) u.append(c.zf[2]) r = c.r ax_cloud_roll.add_patch( Ellipse((e[-1], de[-1]), r[0], r[1], facecolor='none', linestyle='dashed')) # im_roll = ax_cloud_roll.scatter(e, de, c=u, cmap=plt.cm.jet) im_roll = ax_cloud_roll.scatter(e, de, c=u, cmap=plt.get_cmap("jet")) ax_cloud_roll.set_xlabel('Error') ax_cloud_roll.set_ylabel('DError') plt.colorbar(im_roll, ax=ax_cloud_roll) if recordfiles[3] and not upside_down: f_roll = open(CONTROL_INIT_PATH + 'controller_roll_init', 'w') pickle.dump(controller_roll, f_roll) f_roll.close() if print_stuff: print 'SPARC: Controller Serialized', f_roll.name # Navigation Clouds (Y) e, de, u, r = [], [], [], [] for c in nav.latitudeController.clouds: e.append(c.zf[0]) de.append(c.zf[1]) u.append(c.zf[2]) r = c.r ax_cloud_nav_y.add_patch( Ellipse((e[-1], de[-1]), r[0], r[1], facecolor='none', linestyle='dashed')) im_nav_y = ax_cloud_nav_y.scatter(e, de, c=u, cmap=plt.get_cmap("jet")) ax_cloud_nav_y.set_xlabel('Error') ax_cloud_nav_y.set_ylabel('DError') plt.colorbar(im_nav_y, ax=ax_cloud_nav_y) if recordfiles[4] and not upside_down: f_nav_y = open(CONTROL_INIT_PATH + 'controller_navy_init', 'w') pickle.dump(nav.latitudeController, f_nav_y) f_nav_y.close() if print_stuff: print 'SPARC: Controller Serialized', f_nav_y.name # Navigation Clouds (X) e, de, u, r = [], [], [], [] for c in nav.longitudeController.clouds: e.append(c.zf[0]) de.append(c.zf[1]) u.append(c.zf[2]) r = c.r ax_cloud_nav_x.add_patch( Ellipse((e[-1], de[-1]), r[0], r[1], facecolor='none', linestyle='dashed')) im_nav_x = ax_cloud_nav_x.scatter(e, de, c=u, cmap=plt.get_cmap("jet")) ax_cloud_nav_x.set_xlabel('Error') ax_cloud_nav_x.set_ylabel('DError') plt.colorbar(im_nav_x, ax=ax_cloud_nav_x) if recordfiles[5]: f_nav_x = open(CONTROL_INIT_PATH + 'controller_navx_init', 'w') pickle.dump(nav.longitudeController, f_nav_x) f_nav_x.close() if print_stuff: print 'SPARC: Controller Serialized', f_nav_x.name # Record Trajectory: if record_trajectory: f_trajectory = open(CONTROL_INIT_PATH + 'trajectory', 'w') pickle.dump(trajectory_path, f_trajectory) f_trajectory.close() if print_stuff: print 'SPARC: Trajectory Serialized', f_trajectory.name # Calculate Quadratic Error Avg quad_err_alt = [ i**2 for i in np.array(ypoints_alt) - np.array(refpoints_alt) ] quad_err_pitch = [ i**2 for i in np.array(ypoints_pitch) - np.array(refpoints_pitch) ] quad_err_roll = [ i**2 for i in np.array(ypoints_roll) - np.array(refpoints_roll) ] quad_err_yaw = [ i**2 for i in np.array(ypoints_yaw) - np.array(refpoints_yaw) ] quad_err_x = [ i**2 for i in np.array(ypoints_x) - np.array(refpoints_x) ] quad_err_y = [ i**2 for i in np.array(ypoints_y) - np.array(refpoints_y) ] quad_err_alt_avg = np.mean(quad_err_alt) quad_err_pitch_avg = np.mean(quad_err_pitch) quad_err_roll_avg = np.mean(quad_err_roll) quad_err_yaw_avg = np.mean(quad_err_yaw) quad_err_x_avg = np.mean(quad_err_x) quad_err_y_avg = np.mean(quad_err_y) # Print Error Values print 'Square Mean Alt:', quad_err_alt_avg print 'Square Mean Pitch:', quad_err_pitch_avg print 'Square Mean Roll:', quad_err_roll_avg print 'Square Mean Yaw:', quad_err_yaw_avg print 'Square Mean X:', quad_err_x_avg print 'Square Mean Y:', quad_err_y_avg # Print Number of Clouds: if control[0]: print '#Clouds Alt: ', len(controller_alt.clouds) if control[1]: print '#Clouds Yaw: ', len(controller_yaw.clouds) if control[2]: print '#Clouds Pitch: ', len(controller_pitch.clouds) if control[3]: print '#Clouds Roll: ', len(controller_roll.clouds) # Print Consequents for c in controller_alt.clouds: print c.zf plt.show()