def Jacobian(self):
o = []
z = []
on = [self.Hmat[i][-1] for i in range(self.Hmat.n - 1)]
for i in range(self.nojoints + 1):
if i == 0:
z.append([0, 0, 1])
o.append([0, 0, 0])
else:
z.append([
self.setup[i - 1]['Hmat'][j][2]
for j in range(self.Hmat.n - 1)
])
o.append([
self.setup[i - 1]['Hmat'][j][3]
for j in range(self.Hmat.n - 1)
])
Jv = []
Jw = []
for i in range(1, self.nojoints + 1):
if self.setup[i - 1]['type'] == 'p':
Jv.append(z[i - 1])
Jw.append([0, 0, 0])
else:
Jv.append(
linear.cross(z[i - 1],
[on[j] - o[i - 1][j]
for j in range(len(on))]))
Jw.append(z[i - 1])
Jv = matrix(Jv).t()
Jw = matrix(Jw).t()
for each in Jw.A:
Jv = Jv.appendRow(each)
return Jv
def eulerP(e):
if isinstance(e, list):
return 2 * matrix([[
e[0]**2 + e[1]**2 - 0.5, e[1] * e[2] - e[0] * e[3],
e[3] * e[1] + e[0] * e[2]
],
[
e[1] * e[2] + e[0] * e[3], e[0]**2 + e[2]**2 -
0.5, e[2] * e[3] - e[0] * e[1]
],
[
e[1] * e[3] - e[0] * e[2], e[2] * e[3] +
e[0] * e[1], e[0]**2 + e[3]**2 - 0.5
]])
return 2 * matrix(
[[
e[0][0]**2 + e[1][0]**2 - 0.5, e[1][0] * e[2][0] -
e[0][0] * e[3][0], e[3][0] * e[1][0] + e[0][0] * e[2][0]
],
[
e[1][0] * e[2][0] + e[0][0] * e[3][0], e[0][0]**2 + e[2][0]**2 -
0.5, e[2][0] * e[3][0] - e[0][0] * e[1][0]
],
[
e[1][0] * e[3][0] - e[0][0] * e[2][0], e[2][0] * e[3][0] +
e[0][0] * e[1][0], e[0][0]**2 + e[3][0]**2 - 0.5
]])
def p4():
t = symbols('t')
qi = [t**5, t**4, t**3, t**2, t, t**0]
dqi = [sympy.diff(i, t) for i in qi]
ddqi = [sympy.diff(i, t) for i in dqi]
qi0 = [i.subs(t, 0) for i in qi]
qi2 = [i.subs(t, 2) for i in qi]
dqi0 = [i.subs(t, 0) for i in dqi]
dqi2 = [i.subs(t, 2) for i in dqi]
ddqi0 = [i.subs(t, 0) for i in ddqi]
ddqi2 = [i.subs(t, 2) for i in ddqi]
A = matrix([qi0, qi2, dqi0, dqi2, ddqi0, ddqi2])
GM_mat = []
knot = matrix([[
-0.321750554396642, -4.49454236888801 + 2 * math.pi, 1.61941347407016,
2.92374604209311
],
[
0.26396372362570, -4.53174363933971 + 2 * math.pi,
1.94807597387782, 2.96094731254482
],
[
-0.223476601140633, -4.55848107238232 + 2 * math.pi,
2.28309001136617, 2.98768474558743
],
[
0.193621992855945, -4.57850459598843 + 2 * math.pi,
2.62202212042538, 3.00770826919353
],
[
-0.170735211475283, -4.59401022420542 + 2 * math.pi,
2.96352830254749, 3.02321389741052
]])
q = matrix(knot.n - 1, knot.m)
f = open('p4.txt', 'w+')
for i in range(1, knot.n):
lat = []
for j in range(knot.m):
sol = A**-1 * matrix([knot[i - 1][j], knot[i][j], 0, 0, 0, 0]).t()
str1 = "{}*t**5 + {}*t**4 + {}*t**3 + {}*t**2 + {}*t + {}".format(
sol[0][0], sol[1][0], sol[2][0], sol[3][0], sol[4][0],
sol[5][0])
str2 = "{}*t**5 + {}*t**4 + {}*t**3 + {}*t**2 + {}*t + {}".format(
round(sol[0][0], 3), round(sol[1][0], 3), round(sol[2][0], 3),
round(sol[3][0], 3), round(sol[4][0], 3), round(sol[5][0], 3))
GM_mat.append(sol.t().A[0])
q[i - 1][j] = str1
lat.append('q{}{} = '.format(i, j + 1) +
str2.replace('**', '^').replace('*', ''))
f.write(latex.Lcases(lat))
print("[")
for each in GM_mat:
print(str(each)[1:-1] + ';')
print("]")
f.close()
return q
def RotX(th=symbols('th')):
if isinstance(th, Symbol):
return matrix([[1, 0, 0], [0, sympy.cos(th), -sympy.sin(th)],
[0, sympy.sin(th), sympy.cos(th)]])
elif isinstance(th, str):
return RotX(symbols(th))
else:
return matrix([[1, 0, 0], [0, math.cos(th), -math.sin(th)],
[0, math.sin(th), math.cos(th)]]).smooth()
def dRot2d(th=symbols('th')):
if isinstance(th, Symbol):
return matrix([[-sympy.sin(th), -sympy.cos(th)],
[sympy.cos(th), -sympy.sin(th)]])
elif isinstance(th, str):
return dRot2d(symbols(th))
else:
return matrix([[-math.sin(th), -math.cos(th)],
[math.cos(th), -math.sin(th)]]).smooth()
def RotY4(th=symbols('th')):
if isinstance(th, Symbol):
return matrix([[sympy.cos(th), 0, -sympy.sin(th), 0], [0, 1, 0, 0],
[sympy.sin(th), 0, sympy.cos(th), 0], [0, 0, 0, 1]])
elif isinstance(th, str):
return RotY(symbols(th))
else:
return matrix([[math.cos(th), 0, -math.sin(th), 0], [0, 1, 0, 0],
[math.sin(th), 0, math.cos(th), 0], [0, 0, 0,
1]]).smooth()
def isolate(mat):
check = matrix(mat.n, mat.m)
k = 0
for i in range(mat.n):
for j in range(mat.m):
if mat[i][j] != 0 and check[i][j] == 0:
minx, maxx, miny, maxy, check = dim(mat,j,i, check)
if minx-maxx != 0 :
k += 1
temp = matrix(mat[miny:maxy+1]).t()
temp = matrix(temp[minx:maxx+1]).t()
yield minx, maxx, miny, maxy, temp
def edge8(A):
temp = matrix(A.n, A.m)
for i in range(A.n):
for j in range(A.m):
if A[i][j] == 1:
tot = 0
if i != 0:
if checkN(A, 0, i, j):
tot += 1
if j != 0:
if checkW(A, 0, i, j):
tot += 1
if i != A.n - 1:
if checkS(A, 0, i, j):
tot += 1
if j != A.m - 1:
if checkE(A, 0, i, j):
tot += 1
if i != 0 and j != 0:
if checkNW(A, 0, i, j):
tot += 1
if i != A.n - 1 and j != 0:
if checkSW(A, 0, i, j):
tot += 1
if i != A.n - 1 and j != A.m - 1:
if checkSE(A, 0, i, j):
tot += 1
if i != 0 and j != A.m - 1:
if checkNE(A, 0, i, j):
tot += 1
if tot >= 4:
temp[i][j] = 1
return temp
def blobify(A):
temp = matrix(A.n, A.m)
for i in range(A.n):
for j in range(A.m):
if A[i][j] == 1 and checkSurr(A, 0, i, j):
temp[i][j] = 1
return temp
def isOne(A):
t = A.copy()
h = t.n
no = int(0.2*h)
t = matrix(t.A[no:])
t = matrix(t.A[:t.n-no])
ll, _ = leftLaser(t)
if ll[0] == 0:
return False
for i in ll:
if i != ll[0]:
return False
ll, _ = rightLaser(t)
for i in ll:
if i != ll[0]:
return False
return True
def solveABC(A, b):
q = []
for i in range(len(b)):
temp = [b[i], 0, 0]
abc = A**-1 * matrix([[i] for i in temp])
abc = [abc[0][0], abc[1][0], abc[2][0]]
q.append(abc)
return q
def p9():
L1 = 0.7
r1 = 0.04
L2 = 1.68
b2 = 0.2
L3 = 1.7
b3 = 0.15
a4 = 1.02
m1 = 3
m2 = 5
m3 = 3
m4 = 1
I1, I1yy, I2, I2zz, I3, I3yy, I4 = p7()
DELTA2 = 0.36
D = matrix([['D11', 0, 0, 0],
[0, 'K3 + 2*f1 + f2*sin(q4)', 'f(q4)', '2*K1+0.5*f2*sin(q4)'],
[0, 'f(q4)', 'K4', 'f(q4)'],
[0, '2*K1+0.5*f2(q3)*sin(q4)', 'f(q4)', '2*K1']])
D11 = 'K5 + f1-(K2+f1)*cos(2*q2)+K1*cos(2*(q2+q4))-f2*cos(q2+q4)*sin(q2)'
K0 = 'm2*(0.5*L2-DELTA2)**2 + m3*(0.5*L3)**2'
K1 = '0.5*(I4 + 0.25*m4*a4**2)'
K2 = '0.5*(I2-I2zz+I3-I3yy + K0)'
K3 = 'I2 + I3 + K0 + 2*K1'
K4 = 'm3+m4'
K5 = '0.5*(2*I1yy + I2zz + I3yy + K3)'
f1 = '0.5*K4*q3**2 - 0.5*m3*L3*q3'
f2 = 'm3*a4*q3'
f3 = '0.5*a4*m4*cos(q4)'
q1, q2, q3, q4 = symbols('q1 q2 q3 q4')
K0 = round(eval(K0), 3)
K1 = round(eval(K1), 3)
K2 = round(eval(K2), 3)
K3 = round(eval(K3), 3)
K4 = round(eval(K4), 3)
K5 = round(eval(K5), 3)
f1 = eval(f1)
f2 = eval(f2)
f3 = eval(f3)
D11 = eval(D11)
print(latex.Leq(eval(D[0][0]), varname='D_{11}'))
print(latex.Leq(eval(D[1][1]), varname='D_{22}'))
print(latex.Leq(eval(D[2][2]), varname='D_{33}'))
print(latex.Leq(eval(D[3][3]), varname='D_{44}'))
print(eval(D[3][3]))
print(latex.Lmatrix(D))
print(latex.Leq(K0, varname='K0'))
print(latex.Leq(K1, varname='K1'))
print(latex.Leq(K2, varname='K2'))
print(latex.Leq(K3, varname='K3'))
print(latex.Leq(K4, varname='K4'))
print(latex.Leq(K5, varname='K5'))
print(latex.Leq(f1, varname='f1'))
print(latex.Leq(f2, varname='f2'))
print(latex.Leq(f3, varname='f3'))
print(latex.Leq(D11, varname='D_{11}'))
def update(self):
sys.stdout.write('\r UPDATING MESH...')
ttt = time.time()
temp = []
for i in reversed(self.floor):
for j in i.A:
temp.append(j)
self.mesh = matrix(temp).copy()
sys.stdout.write('\r UPDATING MESH... DONE! {}\n'.format(time.time() -
ttt))
def scalex(A, n):
S = []
for i in A.A:
if 0 not in i or 1 not in i:
S.append(i*n)
else:
s = []
for j in i:
s += [j]*n
S.append(s)
return matrix(S)
def matrify(img):
w, h = img.size
px = img.load()
mat = matrix(h, w)
for i in range(h):
for j in range(w):
if (255, 255, 255, 255) != px[j, i] \
and (242, 242, 242, 255) != px[j, i] \
and (238, 238, 238, 255) != px[j, i]:
mat[i][j] = 1
return mat
def add_floor(self, h=None, w=None, start=None, stop=None, material=1):
sys.stdout.write('\r ADDING FLOOR...')
ttt = time.time()
if self.atmat < material:
self.atmat = material
if not h:
h = self.fh
if not w:
w = self.w
else:
w = int(w * self.scale)
if not start:
start = 0
if not stop:
stop = w
if start != 0:
tmesh = matrix([[0 for j in range(start)] for i in range(h)])
for i in range(self.x1):
tmesh.appendCol(material)
else:
tmesh = matrix([[material for j in range(self.x1)]
for i in range(h)])
for i in range(w - self.x1 - self.x2):
col = [material for j in range(self.x3)
] + [-1 for j in range(self.mesh.n - self.x3)]
tmesh = tmesh.appendCol(col)
for i in range(self.x2):
tmesh = tmesh.appendCol(1)
for i in range(self.mesh.m - tmesh.m):
tmesh = tmesh.appendCol(0)
self.floor.append(tmesh)
for i in self.mesh.A:
tmesh = tmesh.appendRow(i)
self.w = tmesh.m
self.h = tmesh.n
self.mesh = tmesh
sys.stdout.write('\r ADDING FLOOR... DONE! {}\n'.format(time.time() -
ttt))
def isEight(A):
intx = intersectionx(A)
count = 0
for i in range(len(intx)-1):
if intx[i+1] == 2:
intx[i] = 0
elif intx[i] == 1:
intx[i] = 0
intx[-1] = 0
if sum(intx,0) != 4:
return False
xy = []
for i in range(len(intx)):
if intx[i] == 2:
start = False
for j in range(len(A[i])):
if A[i][j] == 1:
start = True
if start and A[i][j] == 0:
xy.append([j,i])
break
check = matrix(A.n, A.m)
temp = matrix(A.n, A.m)
for i in range(A.n):
for j in range(A.m):
if A[i][j] == 1: temp[i][j] = 0
else: temp[i][j] = 1
check, _, _, _, _ = through(surr0(temp), surr0(check), 1, 1, 0, 0, 0, 0)
check, _, _, _, _ = through(surr0(temp), check, 1, A.n, 0, 0, 0, 0)
check, _, _, _, _ = through(surr0(temp), check, A.m, 1, 0, 0, 0, 0)
check, _, _, _, _ = through(surr0(temp), check, A.m, A.n, 0, 0, 0, 0)
if check[xy[0][1]+1][xy[0][0]+1] != 0:
return False
if check[xy[1][1]+1][xy[1][0]+1] != 0:
return False
return True
def create_random_values_in_db(db, conn, curs):
a = sql_class.sql_cmds(db, project='test_shit')
A = matrix(100, 100)
for i in range(A.n):
for j in range(A.m):
A[i][j] = 10*sin(i) + 20*cos(j) + 10*3**cos(i*j)
count = 1
for row in A.A:
a.create_table(curs, "table_number%s"%count, ['ival{}'.format(i+1) for i in range(A.m)])
for i in range(len(row)):
a.insert_table(curs, "table_number%s"%count, [j for j in row])
count += 1
def make_block(self, h, w, acc):
"""
Makes a block for testing
:param h:
:param w:
:param acc:
:return:
"""
mesh = matrix(h * acc, w * acc)
for i in range(mesh.n):
for j in range(mesh.m):
mesh[i][j] = 1
self.mesh = mesh.copy()
def fillout(A, x=1, y=1):
temp = surr0(A)
for i in range(temp.n):
for j in range(temp.m):
if temp[i][j] == 1:
temp[i][j] = 0
else:
temp[i][j] = 1
temp = surr0(temp)
check = matrix(temp.n,temp.m)
check = fill(temp, check, x, y)
return unsurr(unsurr(check))
def __init__(self, x1, x2, h, w, acc=20):
sys.stdout.write('\r INITIALIZING HOUSE DESIGN...')
ttt = time.time()
self.roof_len_out = 0
self.costh = 0
self.roof_len_in = 0
scale = acc
self.scale = scale
self.acc = acc
self.roof = False
self.chimney = False
x3 = x1
self.hasroof = False
x1 = int(x1 * scale)
x2 = int(x2 * scale)
x3 = int(x3 * scale)
h = int(h * scale)
w = int(w * scale)
self.x1 = x1
self.x2 = x2
self.x3 = x3
self.w = w
self.h = h
self.fh = h
mesh = matrix(h, w)
for i in range(mesh.n):
for j in range(mesh.m):
if j < x1:
mesh[i][
j] = 1 # when within first wall thickness all x,y are 1
elif j > w - 1 - x2:
mesh[i][
j] = 1 # when within the right wall thickness all x,y are 1
elif i < x3:
mesh[i][j] = 1 # when within the roof all x,y are 1
else:
mesh[i][
j] = -1 # inside of wall gets -1 value to help with knowing where inside is, maybe not needed
self.mesh = mesh
self.atmat = 1
self.floor = [mesh]
sys.stdout.write(
'\r INITIALIZING HOUSE DESIGN... DONE! {}\n'.format(time.time() -
ttt))
def add_roof(self, h=None, thickness=None, material=1):
sys.stdout.write('\r ADDING ROOF...')
ttt = time.time()
if not self.roof:
if material > self.atmat:
self.atmat = material
if not h:
h = self.fh
if not thickness:
thickness = self.x3
temp = matrix(h, math.ceil(self.w / 2))
for i in range(temp.n):
for j in range(temp.m):
if int(-2 * h * j / self.w + temp.n) <= i and int(
-2 * h * j / self.w + temp.n +
(h**2 + 0.25 * self.w**2)**0.5 * thickness / h *
self.w / (2 *
(h**2 + 0.25 * self.w**2)**0.5)) >= i:
temp[i][j] = material
elif int(-2 * h * j / self.w + temp.n +
(h**2 + 0.25 * self.w**2)**0.5 * thickness / h *
self.w / (2 *
(h**2 + 0.25 * self.w**2)**0.5)) < i:
temp[i][j] = -1
other = temp.copy()
if self.w % 2 == 1:
other = other.delCol(-1)
other = other.flipLR().t()
for i in range(other.n):
temp = temp.appendCol(other[i])
self.floor.append(temp)
for i in self.mesh.A:
temp = temp.appendRow(i)
self.mesh = temp
self.h = temp.n
self.w = temp.m
self.roof = True
sys.stdout.write('\r ADDING ROOF... DONE!{} \n'.format(time.time() -
ttt))
def add_roof(self, h=None, thickness=None, material=1):
self.hasroof = True
sys.stdout.write('\r ADDING ROOF...')
ttt = time.time()
if not self.roof:
if material > self.atmat:
self.atmat = material
if not h:
h = self.fh
if not thickness:
thickness = self.x1
temp = matrix(h, math.ceil(self.w / 2))
self.roof_len_out = (h**2 + (self.w / 2)**2)**0.5
self.costh = (self.w / 2) / self.roof_len_out
self.roof_len_in = (self.w / 2 - thickness) / self.costh
eq = lambda x: -(x - h) * self.w / (2 * h)
for i in range(temp.n):
for j in range(temp.m):
if eq(i) <= j < eq(i) + thickness:
temp[i][j] = material
elif j >= eq(i) + thickness:
temp[i][j] = -1
other = temp.copy()
if self.w % 2 == 1:
other = other.delCol(-1)
other = other.flipLR().t()
for i in range(other.n):
temp = temp.appendCol(other[i])
self.floor.append(temp)
for i in self.mesh.A:
temp = temp.appendRow(i)
self.mesh = temp
self.h = temp.n
self.w = temp.m
self.roof = True
sys.stdout.write('\r ADDING ROOF... DONE!{} \n'.format(time.time() -
ttt))
def skew(vec):
if isinstance(vec, matrix):
return matrix([[0, -vec[2][0], vec[1][0]], [vec[2][0], 0, -vec[0][0]],
[-vec[1][0], vec[0][0], 0]])
return matrix([[0, -vec[2], vec[1]], [vec[2], 0, -vec[0]],
[-vec[1], vec[0], 0]])
def isZero(A):
intx = intersectionx(A)
inty = intersectiony(A)
# FIRST TEST CHECK NUMBER OF INTERSECTIONS OF HORIZONTAL AND VERTICAL LINES
intcheckx = []
intchecky = []
test = [1,2,1]
for i in intx:
if not intcheckx:
intcheckx.append(i)
elif i != intcheckx[-1]:
intcheckx.append(i)
for i in inty:
if not intchecky:
intchecky.append(i)
elif i != intchecky[-1]:
intchecky.append(i)
if intcheckx != test or intchecky != test:
return False
# SECOND TEST, SEE WEATHER IT CAN COMPLETE A ROTATION
curr = A[0][0]
x = 0
y = 0
while x < A.m:
y = 0
while y < A.n:
if curr == 1:
break
y = y + 1
curr = A[y][x]
if curr == 1:
break
x = x + 1
run = True
check = matrix(A.n, A.m)
xc = x
temp = A.copy()
try:
while A[y][xc] != 0:
temp[y][xc] = 2
xc += 1
except IndexError:
return False
# break the next layer and iterate through, if anything touches the 2's we have a circle
# 6, 8, 9 would supposedly pass as well but they don't pass the first test
xs = 0
ys = 0
for i in range(x, xc):
if A[y-1][i] == 1:
xc2 = i
while A[y-1][xc2] != 0:
temp[y-1][xc2] = 0
if temp[y-2][xc2] != 0:
xs = xc2
ys = y-2
xc2 += 1
break
temp = surr0(temp)
check = surr0(check)
check,_,_,_,two = through(temp, check, xs, ys, 0, 0, 0, 0)
if two == -2:
return True
return False
def surr0(A):
temp = matrix(A.n+2, A.m+2)
for i in range(A.n):
for j in range(A.m):
temp[i+1][j+1] = A[i][j]
return temp
def unsurr(A):
temp = matrix(A.n-2,A.m-2)
for i in range(1,A.n-1):
for j in range(1,A.m-1):
temp[i-1][j-1] = A[i][j]
return temp
def H(self):
A = matrix(4)
dic = self.setup
for joint in dic:
A = A * DH(joint['th'], joint['d'], joint['a'], joint['al'])
return A
def __init__(self):
self.setup = []
self.nojoints = 0
self.coord = []
self.Hmat = matrix(4)
def DH(thz, dz, dx, thx):
tz = matrix(4)
tz.A[2][3] = dz
tx = matrix(4)
tx.A[0][3] = dx
return RotZ4(thz) * tz * tx * RotX4(thx)
