def svmTrain(X,y,C,funcStr,tol=None,max_passes=None):
if not tol:
tol=1e-3
if not max_passes:
max_passes=5
clf=svm.SVC(C,kernel=funcStr, gamma=20)
clf.fit(X,y.ravel())
#get the parameters
if funcStr=='linear':
xx=np.linspace(min(X[:,0]),max(X[:,0]),100)
w=clf.coef_[0]
a=-w[0]/w[1]
yy=a*xx-clf.intercept_[0]/w[1]
plt.plot(xx,yy,'-b')
PD.plotData(X,y)
else:
print 'Gaussian Kernel'
x_min,x_max=X[:,0].min()-1,X[:,0].max()+1
y_min,y_max=X[:,1].min()-1,X[:,1].max()+1
xx,yy=np.meshgrid(np.arange(x_min,x_max,0.005),np.arange(y_min,y_max,0.005))
Z=clf.predict(np.c_[xx.ravel(),yy.ravel()])
Z=Z.reshape(xx.shape)
plt.contour(xx,yy,Z,levels=[0], linewidths=1, colors='blue' )
PD.plotData(X,y)
# plt.axis(xmin=-1,xmax=1,ymin=-1,ymax=1)
return clf
def PCA():
#=============================================================================
#select which elements of the "data" matirx you wish to analyse
data_set = [2,3,8,12,30,17,34,35]
#define the variable column for which predictors need to be identified
res = 30
PD._covariance(data,headings,data_set,res)
def shootingPath(tL, pL, pR, trd, prd, goal, pyv, bR=92):
tLb = set_dist(tL, goal, -bR)
ltL = local(tL, pL, pR)
ltLb = local(tLb, pL, pR)
path = None
for t in range(2):
ltcL = set_dist_ang(ltLb, ltL, trd, PI / 2 * sign(t))
for p in range(2):
for f in range(2):
lpcL = a3([prd * sign(p) * sign(f), 55, 0])
lpcTL, ltcTL = circles_tangent(lpcL, prd, -sign(p), ltcL, trd,
-sign(t))
pca = abs(relative_angle(lpcTL, [0, 55], lpcL))
tca = Range360(relative_angle(ltcTL, ltLb, ltcL) * sign(t), PI)
td = pca * prd + dist2d(lpcTL, ltcTL) + tca * trd + (
sign(f) != sign(pyv)) * abs(pyv) * PD.tfs(pyv) * 2
pspeed = turning_speed(prd)
tspeed = turning_speed(trd)
tt = pca * prd / pspeed + dist2d(lpcTL, ltcTL) / (
(pspeed + tspeed) / 2) + tca * trd / tspeed
if path is None or td < path[4]:
path = [lpcL, ltcL, lpcTL, ltcTL, td, tt, f]
return path
def __init__(self,
logSize,
mass=0.5,
MomentOfInertiaTotal=np.array([[3.2e-3, 0.0, 0.0],
[0.0, 3.2e-3, 0.0],
[0.0, 0.0, 5.5e-3]]),
MomentOfInertiaProp=np.array([[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.5e-5]])):
super(Quadrotor, self).__init__()
self.m = mass
self.ITotal = MomentOfInertiaTotal
self.IProp = MomentOfInertiaProp
self.IBody = MomentOfInertiaTotal - 4 * MomentOfInertiaProp
self.IBodyI = npl.inv(self.IBody)
self.PQR = np.zeros(3) # Angular Velocity [rad/s]
self.PQRDot = np.zeros(3) # Angular Acceleration[rad/s^2]
self.YPR = np.zeros(3) # Euler Angle [rad]
self.YPRDot = np.zeros(3) # Euler Angular Velocity [rad/s]
self.R = np.zeros((3, 3)) # TODO Rotation Matrix X_E = R X_B
self.RDot = np.zeros((3, 3)) # TODO Derivative of Rotation Matrix
self.Q = np.zeros(4) # TODO Quartanion
self.QDot = np.zeros(4) # TODO Derivative of Quartanion
self.M = np.zeros(3) # Moments [Nm]
self.F = np.zeros(3) # TODO Force [N]
self.FProp = np.zeros(4) # thrust of each propellers
self.PQRProp = np.zeros(
(3, 4)) # TODO Rotation of Propellers, in body frame
# self.CTRL = PD.Controller(omega=3, zeta=0.5) # Controller(regular state)
self.CTRL = PD.Controller(omega=40,
zeta=0.5) # Controller (Fault state)
self.CTRLPOS = PD.Controller(omega=1, zeta=0.7) # Position Controller
# self.ALLC = # TODO Allocator
self.kt = 1.69e-2 # coefficient from the thrust to the reaction torque
self.arm = 0.17 # Moment arms between GoM and each propellers
self.normality = np.ones(
4) # fault degree of the each propellers: zero -> completely fault
self.gamma = 2.75e-3 # rotational drag
self.XYZ = np.zeros(3) # Position [m]
self.UVW = np.zeros(3) # Velocity [m/s]
self.UVWDot = np.zeros(3) # Acceleration [m/s^2]
self.log = np.zeros((1, 23))
self.K = np.zeros((2, 4))
def Minimize(coefficients):
A_f = wavefront.pupil_foc(coefficients, size, rpupil, co_num)
A_def = wavefront.pupil_defocus(coefficients, size, del_z, rpupil, co_num)
psf_foc = wavefront.PSF(Mask, A_f, False)
psf_defoc = wavefront.PSF(Mask, A_def, False)
t0 = wavefront.OTF(psf_foc)
tk = wavefront.OTF(psf_defoc)
q, q2 = PD.Q_matrix(t0, tk, reg, gam)
#noise_filter = sch_filter(N0, t0, tk, d0, dk,reg)
F_m = PD.F_M(q2, d0, dk, t0, tk, noise_filter, gam)
E_metric = PD.Error_metric(t0, tk, d0, dk, q, noise_filter)
L_m = PD.L_M(E_metric, size)
return L_m
def __init__(self, spec, *arg, **kw):
self.sensor_enable = True
self.motor_enable = True
self.teleop = True
self.file_angle = False
try:
opts, args = getopt.getopt(sys.argv[1:], "hwsmaf", [
"wireless", "sensor_disable", "motor_disable",
"autonomous_mode", "file_angle"
])
except getopt.GetoptError:
print """ -h for help
-w (--wireless) for wireless mode
-s (--sensor_disable) for sensor disable
-m (--motor_disable) for motor disabl
-a (--autonomous_mode) for autonomous
-f (--file_angle) for using previous read angle"""
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print """ -h for help
-w (--wireless) for wireless mode
-s (--sensor_disable) for sensor disable
-m (--motor_disable) for motor disable
-a (--autonomous_mode) for autonomous
-f (--file_angle) for using previous read angle"""
sys.exit()
elif opt in ("-w", "--wireless"):
L.DEFAULT_PORT = "/dev/ttyACM0"
progress("starting in wireless mode")
elif opt in ("-s", "--sensor_disable"):
self.sensor_enable = False
progress("sensor has been disabled")
elif opt in ("-m", "--motor_disable"):
self.motor_enable = False
elif opt in ("-a", "--autonomous_mode"):
self.teleop = False
elif opt in ("-f", "--file_angle"):
self.file_angle = True
JoyApp.__init__(self, robot={'count': 3}, *arg, **kw)
self.laser_initial_pos = math.pi / 2
self.knob_pos = 0
self.laser_PD = PD.PD(-790.0 / 1270.0, -320.0 / 1270.0)
def ColourMapPlot():
fig_num = 'Colourmap plots'
plt.figure (fig_num)
plt.clf()
n_plot_row = 4 #Number of rows in a multiplot plot
n_plot_col = 1 #Number of columns in a multiplot plot
y_col = 2
colour_col = 3
n_plot_num = 1
PD._cm_plot(data,headings,dates,ave_data,fig_num,colour_col,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 2
colour_col = 8
n_plot_num = n_plot_num + 1
PD._cm_plot(data,headings,dates,ave_data,fig_num,colour_col,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 2
colour_col = 12
n_plot_num = n_plot_num + 1
PD._cm_plot(data,headings,dates,ave_data,fig_num,colour_col,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 2
colour_col = 30
n_plot_num = n_plot_num + 1
PD._cm_plot(data,headings,dates,ave_data,fig_num,colour_col,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
fig = plt.gcf()
fig.subplots_adjust(top=0.95)
fig.subplots_adjust(bottom=0.12)
fig.subplots_adjust(left=0.05)
fig.subplots_adjust(right=0.97)
ax = plt.gca()
ax.xaxis.label.set_color([0,0,0])
ax.tick_params(axis='x', colors=[0,0,0])
plt.show()
def CuSumPlots():
#=============================================================================
cusum_data = PD._cusum(data)
def change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col):
plt.subplot(row,col,plot_num)
plt.plot(dates,cusum_data[:,y_col])
date = np.int(np.floor(len(dates) * DateLine))
plt.axvline(dates[date], color='red')
plt.grid(True)
plt.title(headings[y_col])
plt.xticks(rotation = 90)
fig = plt.gcf()
fig.subplots_adjust(top=0.95)
fig.subplots_adjust(bottom=0.12)
fig.subplots_adjust(left=0.08)
fig.subplots_adjust(right=0.97)
ax = plt.gca()
ax.xaxis.label.set_color([0.75,0.75,0.75])
ax.tick_params(axis='x', colors=[0.75,0.75,0.75])
plt.figure('CuSum plots')
plt.clf()
row = 7
col = 1
DateLine = 0.85
y_col = 3
plot_num = 1
change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col)
y_col = 4
plot_num = plot_num + 1
change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col)
y_col = 5
plot_num = plot_num + 1
change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col)
y_col = 8
plot_num = plot_num + 1
change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col)
y_col = 12
plot_num = plot_num + 1
change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col)
y_col = 10
plot_num = plot_num + 1
change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col)
y_col = 11
plot_num = plot_num + 1
change(dates,headings,cusum_data,DateLine,row,col,plot_num,y_col)
ax = plt.gca()
ax.xaxis.label.set_color([0,0,0])
ax.tick_params(axis='x', colors=[0,0,0])
plt.show()
def RunningAvePlots():
#=============================================================================
fig_num = 'Running average plots'
plt.figure(fig_num)
plt.clf()
#Set the running average points at beginning of script
n_plot_row = 7 #Number of rows in a multiplot plot
n_plot_col = 1 #Number of columns in a multiplot plot
y_col = 3
n_plot_num = 1
PD._ra_plot(data,headings,dates,ave_data,fig_num,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 4
n_plot_num = n_plot_num + 1
PD._ra_plot(data,headings,dates,ave_data,fig_num,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 5
n_plot_num = n_plot_num + 1
PD._ra_plot(data,headings,dates,ave_data,fig_num,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 8
n_plot_num = n_plot_num + 1
PD._ra_plot(data,headings,dates,ave_data,fig_num,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 12
n_plot_num = n_plot_num + 1
PD._ra_plot(data,headings,dates,ave_data,fig_num,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 10
n_plot_num = n_plot_num + 1
PD._ra_plot(data,headings,dates,ave_data,fig_num,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
y_col = 11
n_plot_num = n_plot_num + 1
PD._ra_plot(data,headings,dates,ave_data,fig_num,
y_col,run_ave_points,n_plot_row,n_plot_col,n_plot_num)
ax = plt.gca()
ax.xaxis.label.set_color([0,0,0])
ax.tick_params(axis='x', colors=[0,0,0])
@author: pedawson
"""
print('============================= Running ================================')
import datetime
start_script = datetime.datetime.now()
#==========================================================================
import numpy as np
import matplotlib.pyplot as plt
import xlrd
import PD
run_ave_points = 50 #For running average filter
(data, headings, dates, ave_data) = PD._sort('data.xlsx',run_ave_points)
a = np.shape(data)
data1 = np.zeros((a))
ave_data1 = np.zeros((a))
m = 0
for i in range(len(data)):
if (data[i,2] > 0 and
data[i,2] > 0):
data1[m,] = data[i,]
ave_data1[m,] = ave_data[i,]
m = m + 1
data = np.zeros((m,a[1]))
ave_data = np.zeros((m,a[1]))
input_layer_size = 400
hidden_layer_size = 25
num_labels = 10
#Part One
print 'Loading and Visualizing Data...'
data = sio.loadmat('ex3data1')
X = data['X']
y = data['y']
m = X.shape[0]
sel = range(m)
np.random.shuffle(sel)
sel = sel[0:100]
#DD.displayData(X[sel,:])
#Part Two
print 'Loading Saved Neural Network Parameters...'
Theta = sio.loadmat('ex3weights')
Theta1 = Theta['Theta1']
Theta2 = Theta['Theta2']
pred = PD.predict(Theta1, Theta2, X)
pred.shape = (pred.shape[0], 1)
print 'Training Set Accuracy: %5.3f%%' % (np.mean(pred == y) * 100)
def QuickSortTipo(Lista, li, lf):
pila = PD.PilaD()
PD.crearpilaD(pila)
PD.apilar(pila, [li, lf])
while (not PD.pilavacia(pila)):
x = PD.desapilar(pila)
li, lf = x[0], x[1]
pivot = lf
i = li
j = lf - 1
while (i <= j):
while (Lista[i].tipo < Lista[pivot].tipo):
i = i + 1
while Lista[j].tipo > Lista[pivot].tipo:
j = j - 1
if (i <= j):
aux = Lista[i]
Lista[i] = Lista[j]
Lista[j] = aux
i = i + 1
j = j - 1
aux = Lista[i]
Lista[i] = Lista[lf]
Lista[lf] = aux
if (i < lf):
PD.apilar(pila, [(i), lf])
if (j > li):
PD.apilar(pila, [li, (j)])
print("p: ", end = " ")
print([tasks[k].p for k in range(len(tasks))])
print("w: ", end = " ")
print([tasks[k].w for k in range(len(tasks))])
print("d: ", end = " ")
print([tasks[k].d for k in range(len(tasks))])
print()
pi_optymalne, czas = bf_algorithm(tasks,n)
print("Algorytm BF:\npi: ",end = " ")
print([pi_optymalne[k].id+1 for k in range(0,len(pi_optymalne))])
Fmin = FMIN(pi_optymalne)
print("Fmin: " + str(Fmin))
print("czas dzialanian programu: {0:02f}s\n".format(czas))
pi_optymalne, czas = greedy(tasks)
print("Algorytm greedy:\npi: ",end = " ")
print([pi_optymalne[k].id+1 for k in range(0,len(pi_optymalne))])
Fmin = FMIN(pi_optymalne)
print("Fmin: " + str(Fmin))
print("czas dzialanian programu: {0:02f}s\n".format(czas))
print("Algorytm PD:")
start = time.time()
result,order=PD(tasks)
czas = time.time() - start
order=list(reversed(order))
print("pi: ",order)
print("Fmin: ",result)
print("czas dzialanian programu: {0:02f}s\n".format(czas))
imk = imk[y1:y2, x1:x2]
imk = tools.imreg(im0, imk)
## apodizing focused-defocused data
im0 = tools.apo2d(im0, ap)
imk = tools.apo2d(imk, ap)
## Zernike modes to be fitted
co_num = 3
rpupil = telescope.pupil_size(D, lam, pix, size)
Mask = aperture.mask(rpupil, size)
d0, dk = PD.FT(im0, imk)
#freq_mask =noise.noise_mask(size,cut_off)
#N0 = noise.Noise_d0(d0, freq_mask)
noise_filter = fftshift(noise.noise_mask_high(size, cut_off))
#M_gamma = noise_mask(size,0.5)
gam = 1 #noise.Gamma(d0,dk,M_gamma)
def Minimize(coefficients):
A_f = wavefront.pupil_foc(coefficients, size, rpupil, co_num)
A_def = wavefront.pupil_defocus(coefficients, size, del_z, rpupil, co_num)
psf_foc = wavefront.PSF(Mask, A_f, False)
psf_defoc = wavefront.PSF(Mask, A_def, False)
# Set object detection mode
is_objectDetect = True
if is_objectDetect:
import objectDetect
detector = objectDetect.detector(threshold=0.5)
# Imports modules according to mode
if cam.cvMode:
from Direction import *
else:
import Classifier
classifier = Classifier.interpreter()
driver.drive()
controller = PD()
offRoadCounter = 0
clockCounter = 0
try:
time.sleep(2)
startTime = time.time()
while True:
clockCounter += 1
cam.capture()
if is_objectDetect and clockCounter % 20 == 0:
detector.setTensors(cv2.resize(cam.frame, dsize=(300, 300)))
results = detector.detect_objects()
if len(results) > 0:
def __init__(self, robot): self.max_speed = .4 self.center_speed = .1 self.r = robot.at self.PD = PD.PD(0.005, 0.1)
input_layer_size=400
hidden_layer_size=25
num_labels=10
#Part One
print 'Loading and Visualizing Data...'
data=sio.loadmat('ex3data1')
X=data['X']
y=data['y']
m=X.shape[0]
sel=range(m)
np.random.shuffle(sel)
sel=sel[0:100]
#DD.displayData(X[sel,:])
#Part Two
print 'Loading Saved Neural Network Parameters...'
Theta=sio.loadmat('ex3weights')
Theta1=Theta['Theta1']
Theta2=Theta['Theta2']
pred=PD.predict(Theta1,Theta2,X)
pred.shape=(pred.shape[0],1)
print 'Training Set Accuracy: %5.3f%%' %(np.mean(pred==y)*100)
def QuickSortStock(Lista, li, lf):
pila = PD.PilaD()
PD.crearpilaD(pila)
PD.apilar(pila, [li, lf])
while (not PD.pilavacia(pila)):
x = PD.desapilar(pila)
li, lf = x[0], x[1]
pivot = lf
i = li
j = lf - 1
while (i <= j):
while (float(Lista[i].stock) < float(Lista[pivot].stock)):
i = i + 1
while float(Lista[j].stock) > float(Lista[pivot].stock):
j = j - 1
if (i <= j):
aux = Lista[i]
Lista[i] = Lista[j]
Lista[j] = aux
i = i + 1
j = j - 1
aux = Lista[i]
Lista[i] = Lista[lf]
Lista[lf] = aux
if (i < lf):
PD.apilar(pila, [(i), lf])
if (j > li):
PD.apilar(pila, [li, (j)])
