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)
