def make_prototype(triangles, spot_based=True): trans_triangles = [] for t in triangles: # Centralize the triangle in the plane t += np.array([250.0, 250.0]) - geometry.centroid(t) # If non-spot-based pototype is requested, swap the vertices around so that vertex 1 is # the pointiest one. if spot_based == False: angles = [geometry.angle(t,1), geometry.angle(t,2), geometry.angle(t,3)] min_angle = angles.index(min(angles)) if min_angle == 0: t = np.array([t[0], t[1], t[2]]) elif min_angle == 1: t = np.array([t[1], t[2], t[0]]) elif min_angle == 2: t = np.array([t[2], t[0], t[1]]) # Rotate the triangle around its centroid so that vertex 1 points North t = geometry.rotate(t) # Ensure that vertex 2 is to the left of vertex 3 to prevent cancelling out if t[1,0] > t[2,0]: t = np.array([t[0], t[2], t[1]]) trans_triangles.append(t) # Reformat as Numpy array and take the mean of the coordinates to form the prototype trans_triangles = np.asarray(trans_triangles, dtype=float) prototype = trans_triangles.mean(axis=0) # Shift the prototype such that its bounding box is vertically centralized in the plane prototype[:, 1] += ((500.0 - (max([prototype[1,1], prototype[2,1]]) - prototype[0,1])) / 2.0) - prototype[0,1] return prototype
def rotate_camera(self, dx, dy): dx = 2 * self.r * -dx dy = 2 * self.r * -dy self._direction, self._top = geometry.rotate( self.direction, self.top, dx, dy) self.direction = self.direction self.cache = None return map(lambda x: (x * 180 / math.pi) % 360, geometry.get_angles(self.direction))
def _is_reversed(a: model.Node, b: model.Node) -> bool: """Прямой или обратный порядок построения ЛФ""" # pylint: disable=C0103 # 1. получаем цветные вершины треугольников со стороной - отрезком лф colored = set(a.neighbors) & set(b.neighbors) # 2. проверяем, какая из них слева от отрезка лф for c in colored: rotation = geometry.rotate(a, b, c) if rotation < 0 and c.country == 101: return True return False
def solve_states2(initial_states, desired_states, center_line, extern_conditions, plane_specs, constraints, time_step=1, sim_time=10): acceleration_constraint, turning_constraint = constraints rejection, init_velocity, init_heading = initial_states desired_heading, desired_velocity = desired_states init_heading = solver_heading(init_heading) init_guess = formulate_guess(sim_time) bounds = [(-acceleration_constraint, acceleration_constraint), (-turning_constraint, turning_constraint)] * sim_time rinit_x, rinit_y = rotate(0, rejection, solver_heading(desired_heading) - 360) state0 = [rinit_x, rinit_y, init_velocity, init_heading] obj = formulate_objective( state0, center_line, [solver_heading(desired_heading), desired_velocity], extern_conditions, plane_specs, time_step=time_step) start_time = time.time() result = opt.minimize(obj, init_guess, method='SLSQP', bounds=bounds, options={ 'eps': 0.2, 'maxiter': 300 }) # print('-----------------------------------------') # print('----', time.time() - start_time, 'seconds ----') # print('-----------------------------------------\n') states = compute_states(state0, result.x, extern_conditions[0], plane_specs, time_step=time_step) states = get_states_controls(states) return get_kalman_controls(states, time_step), result.fun
def run(args): y_margin_mm = 30 x_margin_mm = 30 H = 210 # A4 W = 297 # A4 grid_gap_mm = args.grid_gap_mm points = grid((x_margin_mm, y_margin_mm), (W - x_margin_mm, H - y_margin_mm), (grid_gap_mm, grid_gap_mm)) points = optimize(points) if args.rot != 0: points = rotate(points, args.rot) with Plotter('/dev/ttyUSB0', 115200) as p: p.load_config('config.json') p.set_input_limits((0, 0), (W, 0), (0, H), (W, H)) p.draw_polylines(points) return 0
def move_cor(self, dt): if not self.speed: return d = self.speed * dt if not self.steering: self.x += d * math.sin(self.rot) self.y += d * math.cos(self.rot) return st = self.steering steer = (self.hwbase * math.sin(self.rot), self.hwbase * math.cos(self.rot)) steer2 = geometry.translate(steer, self.rot + st + math.pi/2, 10) back = (self.htrack * math.cos(self.rot) - self.hwbase * math.sin(self.rot), -self.htrack * math.sin(self.rot) - self.hwbase * math.cos(self.rot)) back2 = geometry.translate(back, self.rot + math.pi/2, 10) centre = (0,0) # the car centre point, same as origin as other things are shifted cor = geometry.line_intersection(steer + steer2, back2 + back) r = ((cor[0] - centre[0]) ** 2 + (cor[1] - centre[1]) ** 2) ** 0.5 angle = d / r * [1, -1][self.steering < 0] * 1 npos = geometry.rotate(centre, cor, angle) self.x += npos[0] - centre[0] self.y += npos[1] - centre[1] self.rot += angle
gammaRadians = clientData[6] # Unpack propeller matrices propellerMatrices = [[0]*12 for i in range(4)] propellerMatrices[0] = clientData[7 :19] propellerMatrices[1] = clientData[19:31] propellerMatrices[2] = clientData[31:43] propellerMatrices[3] = clientData[43:55] # Add some Guassian noise to position 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 # Get altitude directly from position Z altitudeMeters = positionZMeters # Poll controller demands = controller.poll() # Get motor thrusts from FMU model thrusts = fmu.getMotors((pitchAngleRadians, rollAngleRadians, yawAngleRadians), altitudeMeters, \ coordcalc.metersToDegrees(positionXMeters, positionYMeters),\ demands, timestepSeconds) # Force is a function of particle count
def test_rotation(self): p, q = geometry.rotate(Point(1, 0, 0), Point(0, 1, 0), 1, 1) self.assertAlmostEqual(p.x, 0.25) self.assertAlmostEqual(q.y, 0.5) self.assertAlmostEqual(q.z, 0.75)
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 getMotors(self, imuAngles, altitude, gpsCoords, controllerInput, timestep): ''' Gets motor thrusts based on current telemetry: imuAngles IMU pitch, roll, yaw angles in radians altitude altitude in meters gpsCoords GPS coordinates (latitude, longitude) in degrees controllInput (pitchDemand, rollDemand, yawDemand, climbDemand, switch) timestep timestep in seconds ''' # Convert flight-stick controllerInput pitchDemand = controllerInput[0] * PITCH_DEMAND_FACTOR rollDemand = controllerInput[1] * ROLL_DEMAND_FACTOR yawDemand = controllerInput[2] * YAW_DEMAND_FACTOR climbDemand = controllerInput[3] * CLIMB_DEMAND_FACTOR # Combine pitch and roll demand into one value for PID control pitchrollDemand = math.sqrt(pitchDemand**2 + rollDemand**2) gpsLatCorrection = self.latitude_PID.getCorrection(gpsCoords[0], pitchrollDemand, timestep=timestep) gpsLongCorrection = self.longitude_PID.getCorrection(gpsCoords[1], pitchrollDemand, timestep=timestep) # Compute GPS-based pitch, roll correction if we want position-hold switch = controllerInput[4] gpsPitchCorrection, gpsRollCorrection = 0,0 if switch == 2: gpsPitchCorrection, gpsRollCorrection = rotate((gpsLatCorrection, gpsLongCorrection), -imuAngles[2]) gpsPitchCorrection = -gpsPitchCorrection # Compute altitude hold if we want it altitudeHold = 0 if switch > 0: altitudeHold = self.altitude_PID.getCorrection(altitude, timestep=timestep) # PID control for pitch, roll based on angles from Inertial Measurement Unit (IMU) imuPitchCorrection = self.pitch_Stability_PID.getCorrection(imuAngles[0], timestep) imuRollCorrection = self.roll_Stability_PID.getCorrection(-imuAngles[1], timestep) # Special PID for yaw yawCorrection = self.yaw_IMU_PID.getCorrection(imuAngles[2], yawDemand, timestep) # Overall pitch, roll correction is sum of stability and position-hold pitchCorrection = imuPitchCorrection + gpsPitchCorrection rollCorrection = imuRollCorrection + gpsRollCorrection # Overall thrust is baseline plus climb demand plus correction from PD controller thrust = THRUST_BASELINE + climbDemand + altitudeHold # Change the thrust values depending upon the pitch, roll, yaw and climb values # received from the joystick and the # quadrotor model corrections. A positive # pitch value means, thrust increases for two back propellers and a negative # is opposite; similarly for roll and yaw. A positive climb value means thrust # increases for all 4 propellers. psign = [+1, -1, -1, +1] rsign = [-1, -1, +1, +1] ysign = [+1, -1, +1, -1] thrusts = [0]*4 for i in range(4): thrusts[i] = (thrust + rsign[i]*rollDemand + psign[i]*pitchDemand + ysign[i]*yawDemand) * \ (1 + rsign[i]*rollCorrection + psign[i]*pitchCorrection + ysign[i]*yawCorrection) return thrusts
class MatrixHandler(object): def __init__(self): self.mvMat = geo.identity() self.pMat = geo.identity() def pushModelMatrix(self, matrix): self.mvMat = self.mvMat * matrix def pushProjectMatrix(self, matrix): self.pMat = self.pMat * matrix def getMvpMatrix(self): return self.pMat * self.mvMat def clear(self): self.mvMat = geo.identity() self.pMat = geo.identity() def __repr__(self): return repr(self.mvMat) if __name__ == '__main__': s = MatrixHandler() r = geo.rotate(10, (1, 0, 0)) print s print r s.pushModelMatrix(r) s.pushModelMatrix(r) print s
def rotate(self, ang): o = (self.cx, self.cy) for i in range(len(self.p)): self.p[i] = geometry.rotate(o, self.p[i], ang)
wind_direction = runway_heading + wind_degrees xp_wind_direction = -1 * wind_degrees + runway_heading # since positive rotation to right xp_wind_direction += 180 # wind is counter clockwise of true north print("Using (wind speed, wind heading):", wind_speed, wind_degrees) friction = 0 xp_client.sendDREFs(XPlaneDefs.condition_drefs, [friction, xp_wind_direction, wind_speed]) start = time.time() read_drefs = XPlaneDefs.control_dref + XPlaneDefs.position_dref gs, psi, throttle, x, _, z = xp_client.getDREFs(read_drefs) gs, psi, throttle, x, z = gs[0], psi[0], throttle[0], x[0], z[0] d_x, d_z = rotate(x - origin_x, z - origin_z, -(runway_heading + XPlaneDefs.zero_heading - 360)) print("POSITION ON RUNWAY:", d_x, d_z) start_time = time.time() if TAKEOFF and gs > desired_velocity and abs(psi - runway_heading) < 10 and \ d_x - TAKEOFF_DIST > 0: takeoff(xp_client) break # TODO: add zero heading in solver new_init_states = [x - origin_x, z - origin_z, gs, psi + XPlaneDefs.zero_heading] desired_states = [desired_x, desired_z, desired_velocity] winds = [[kn_to_ms(wind_speed), (wind_direction + XPlaneDefs.zero_heading)]] if SAMPLE: winds = np.concatenate((winds, np.column_stack((np.random.randint(ws_bins[0], ws_bins[1] + 1, size=num_environment_samples), np.random.randint(wh_bins[0], wh_bins[1] + 1, size=num_environment_samples) + XPlaneDefs.zero_heading)))) cld = rejection_dist(desired_x, desired_z, x - origin_x, z - origin_z)
def main(): figure_1 = Figure(points={ 'A': (0, 0, 0), 'B': (0, 0, 128), 'C': (128, 0, 128), 'D': (128, 0, 0), 'E': (0, 384, 0), 'F': (0, 384, 128), 'G': (128, 384, 128), 'H': (128, 384, 0), }, pairs=( ('A', 'B'), ('B', 'C'), ('C', 'D'), ('D', 'A'), ('E', 'F'), ('F', 'G'), ('G', 'H'), ('H', 'E'), ('A', 'E'), ('B', 'F'), ('C', 'G'), ('D', 'H'), )) figure_2 = Figure(points={ 'A': (0, 0, 256), 'B': (0, 0, 384), 'C': (128, 0, 384), 'D': (128, 0, 256), 'E': (0, 128, 256), 'F': (0, 128, 384), 'G': (128, 128, 384), 'H': (128, 128, 256), }, pairs=( ('A', 'B'), ('B', 'C'), ('C', 'D'), ('D', 'A'), ('E', 'F'), ('F', 'G'), ('G', 'H'), ('H', 'E'), ('A', 'E'), ('B', 'F'), ('C', 'G'), ('D', 'H'), )) figure_3 = Figure( points={ 'A1': (256, 0, 0), 'B1': (256, 0, 128), 'C1': (367, 0, 64), 'D1': (293, 104, 64) }, pairs=(('A1', 'B1'), ('B1', 'C1'), ('C1', 'A1'), ('A1', 'D1'), ('B1', 'D1'), ('C1', 'D1')), ) figure_4 = Figure(points={ 'A': (256, 0, 256), 'B': (256, 0, 512), 'C': (512, 0, 512), 'D': (512, 0, 256), 'E': (256, 256, 256), 'F': (256, 256, 512), 'G': (512, 256, 512), 'H': (512, 256, 256), }, pairs=( ('A', 'B'), ('B', 'C'), ('C', 'D'), ('D', 'A'), ('E', 'F'), ('F', 'G'), ('G', 'H'), ('H', 'E'), ('A', 'E'), ('B', 'F'), ('C', 'G'), ('D', 'H'), )) ang_x = -45 ang_y = 45 ang_z = 0 cam_x = 1024 cam_y = 1024 cam_z = 1024 focal_length = 512 canvas = Canvas() canvas.add_figure(figure_1) canvas.add_figure(figure_2) canvas.add_figure(figure_3) canvas.add_figure(figure_4) while True: cv2.imshow( 'image', canvas.get_image(cam_x, cam_y, cam_z, ang_x, ang_y, ang_z, focal_length)) k = cv2.waitKey(0) # Shift matrix p = [0.0, 0.0, 0.0] # Translation if k == 83: p = [8.0, 0.0, 0.0] elif k == 81: p = [-8.0, 0.0, 0.0] elif k == 82: p = [0.0, 8.0, 0.0] elif k == 84: p = [0.0, -8.0, 0.0] elif k == ord('-'): p = [0.0, 0.0, 8.0] elif k == ord('+'): p = [0.0, 0.0, -8.0] # Rotation elif k == ord('8'): ang_x += 2 elif k == ord('2'): ang_x -= 2 elif k == ord('4'): ang_y += 2 elif k == ord('6'): ang_y -= 2 elif k == ord('7'): ang_z += 2 elif k == ord('9'): ang_z -= 2 # Focal length size elif k == ord('['): focal_length *= 1.1 elif k == ord(']'): focal_length /= 1.1 r = rotate(p, ang_x, ang_y, ang_z) cam_x = int(round(cam_x + r[0])) cam_y = int(round(cam_y + r[1])) cam_z = int(round(cam_z + r[2]))
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()
def animateRotation(): canvas.delete("all") rotate(triangles) #translate(triangles, 0.02, 0.02, 0.0) drawFrame() canvas.after(20, animateRotation)