def main():
P = [[40], [40]]
V0 = [[30], [30]]
V1 = [[40], [70]]
V2 = [[50], [30]]
points = [V0, V1, V2]
aspect_correct = [[1, 0], [0, 4 / 7]]
home = "\N{escape}[H"
wakeup_time = time.time()
print(home + "\N{escape}[J\N{escape}[?25l")
t = 0
try:
while True:
kanvas = canvas.Canvas()
rot = matrix.rotate_2D(t * 0.5)
r_points = []
for point in points:
C = matrix.sub(point, P)
C = matrix.mult(rot, C)
C = matrix.add(P, C)
C = matrix.mult(aspect_correct, C)
r_points.append(C)
t += 1
kanvas.triangle(round(r_points[0][0][0]), round(r_points[0][1][0]),
round(r_points[1][0][0]), round(r_points[1][1][0]),
round(r_points[2][0][0]), round(r_points[2][1][0]))
image = home + kanvas.get_image()
print(image)
wakeup_time += 0.05
time.sleep(max(0, wakeup_time - time.time()))
finally:
print(home + "\N{escape}[J \N{escape}[?25h")
def main():
P = [[40],
[40],
[0]]
Axis = [[10],
[5],
[3]]
# V0 = [[30],
# [30],
# [0]]
# V1 = [[40],
# [70],
# [0]]
# V2 = [[50],
# [30],
# [0]]
# points = [V0, V1, V2]
points = []
for i in range(15):
points.append([[random.randint(20, 60)],
[random.randint(30, 60)],
[random.randint(-20, 20)]])
aspect_correct = [[1, 0, 0],
[0, 4/7, 0],
[0, 0, 1]]
home = "\N{escape}[H"
wakeup_time = time.time()
print(home + "\N{escape}[J\N{escape}[?25l")
t = 0
try:
while True:
kanvas = canvas.Canvas()
rot = matrix.rotate_3D(t * 0.1, Axis)
r_points = []
for point in points:
C = matrix.sub(point, P)
C = matrix.mult(rot, C)
C = matrix.add(P, C)
C = matrix.mult(aspect_correct, C)
r_points.append(C)
t += 1
for r_point in r_points:
kanvas.dab(round(r_point[0][0]),
round(r_point[1][0]))
# kanvas.triangle(round(r_points[0][0][0]),
# round(r_points[0][1][0]),
# round(r_points[1][0][0]),
# round(r_points[1][1][0]),
# round(r_points[2][0][0]),
# round(r_points[2][1][0]))
image = home + kanvas.get_image()
print(image)
wakeup_time += 0.05
time.sleep(max(0, wakeup_time - time.time()))
finally:
print(home + "\N{escape}[J \N{escape}[?25h")
def iterate(self, measurement, control ):
if measurement:
self.lost = False
velocity = control
# known are speeds, convert to absolute movements
nao_movement_x = velocity[0]
nao_movement_y = velocity[1]
nao_movement_t = velocity[2]
# step forward using control vector u
self.u[0][0] = nao_movement_x
self.u[1][0] = nao_movement_y
muBelief = self.mu
# ________ PREDICTION ________
# rotate using nao_movements theta
t = nao_movement_t
rotationmatrix = [[ math.cos( t ), -math.sin(t) ],[ math.sin(t), math.cos(t) ]]
# Predict new measurement based on nao movement.
# Nao moves x,y towards the ball
muBelief = matrix.subtract( muBelief , self.u )
#print muBelief
# Nao rotates theta
muBelief = matrix.mult( rotationmatrix, muBelief )
# add noise to motion
muBelief = sample( muBelief, self.Sigma, 2)
# covariance matrix update
SigmaBelief = matrix.plus( self.Sigma , self.R)
# ________ CORRECTION _________
if measurement:
self.z[0][0] = measurement[0]
self.z[1][0] = measurement[1]
# Since C = [1,0;0,1], drop it
s = matrix.inverse2D( matrix.plus( SigmaBelief, self.Q) )
K = matrix.mult(SigmaBelief, s )
self.mu = matrix.plus( muBelief, matrix.mult(K, matrix.subtract(self.z , muBelief)) )
self.Sigma = matrix.mult(matrix.subtract( self.I, K ), SigmaBelief)
else:
# if no ball is found, use the predicted state!
self.mu = muBelief
self.Sigma = SigmaBelief
self.ballPos = self.mu[0][0], self.mu[1][0]
def solve(m, bs, p):
"""
Solve a triangularized linear system, writing the solutions to bs, p being the number of variables.
Returns False if there are no solutions.
"""
for i in range(len(m) - 1, -1, -1):
z = first_nonzero(m[i])
if z > p:
continue
elif z == p:
return False
bs[z][0] = m[i][p]
for j in range(z+1, p):
matrix.add(bs, z, -m[i][j], j)
matrix.mult(bs, z, 1.0/m[i][z])
return True
def sample(mean, cov , dimensions):
r = matrix.transpose( matrix.Cholesky(cov) )
randoms = matrix.zero(dimensions, 1)
for i in range(len(randoms)):
randoms[i][0] = random.gauss(0,0.025)
return matrix.plus( mean , matrix.mult(r, randoms))
def poly(x,y):
if NUMPY:
c = [str(x) for x in polyfit(x,y,len(x)-1)]
else:
a = [[a**b for b in range(len(x)-1,-1,-1)] for a in x]
b = [[t] for t in y]
c = [str(round(A,14)) for B in matrix.mult(matrix.inv(a),b) for A in B]
return ' + '.join([X for X in (c[A]+'x'+''.join([['⁰','¹','²','³','⁴','⁵','⁶','⁷','⁸','⁹'][int(y)] for y in str(len(c)-int(A)-1)]) if float(c[A])!=0 else '' for A in range(len(c))) if X])
def sample(mean, cov , dimensions):
r = matrix.transpose( matrix.Cholesky(cov) )
randoms = matrix.zero(dimensions, 1)
for i in range(len(randoms)):
randoms[i][0] = random.gauss(0,0.05)
return matrix.plus( mean , matrix.mult(r, randoms))
def poly(x, y):
if NUMPY:
c = [str(x) for x in polyfit(x, y, len(x) - 1)]
else:
a = [[a**b for b in range(len(x) - 1, -1, -1)] for a in x]
b = [[t] for t in y]
c = [
str(round(A, 14)) for B in matrix.mult(matrix.inv(a), b) for A in B
]
return ' + '.join([
X for X in (
c[A] + 'x' +
''.join([['⁰', '¹', '²', '³', '⁴', '⁵', '⁶', '⁷', '⁸', '⁹'][int(y)]
for y in str(len(c) - int(A) -
1)]) if float(c[A]) != 0 else ''
for A in range(len(c))) if X
])
import matrix a = matrix.rand_matrix(4, 4) b = matrix.rand_matrix(4, 4) c = matrix.create_matrix(4, 4) matrix.mult(c, a, b) print c a = matrix.rand_matrix(4, 2) b = matrix.rand_matrix(2, 4) c = matrix.create_matrix(4, 4) c = matrix.mult(c, a, b) print c r = matrix.create_matrix(2, 2) print matrix.mult(r, [[1, 0], [0, 1]], [[1, 0], [0, 1]]) print matrix.mult(r, [[2, 0], [0, 2]], [[0, 2], [2, 0]])
for i in range(len(mat)):
add = [0] * len(mat)
add[i] = 1
mat[i] += add
latex_line(mat)
# Eliminating
for i in range(len(mat)):
if is_zero(mat[i][i]):
print("Can't invert the matrix.")
latex.output("\\end{array}")
latex.enq()
latex.output("\\\\Can't invert the matrix.\n")
latex.end("Inverting the matrix")
exit()
matrix.mult(mat, i, 1/mat[i][i])
for j in range(len(mat)):
if j == i:
continue
matrix.add(mat, j, -mat[j][i], i)
latex_line(mat)
# Output
invert = []
for i in range(len(mat)):
invert += [[0] * size]
for j in range(size):
invert[i][j] = mat[i][j + size]
matrix.output(invert)
# LaTeX output
with open(path) as f:
data = json.load(f)
# Словарь функций
dictionary = {
'1':
matrix.add(data['first_matrix_dimensions'],
data['second_matrix_dimensions'], data['first_matrix'],
data['second_matrix']),
'2':
matrix.sub(data['first_matrix_dimensions'],
data['second_matrix_dimensions'], data['first_matrix'],
data['second_matrix']),
'3':
matrix.mult(data['first_matrix_dimensions'],
data['second_matrix_dimensions'], data['first_matrix'],
data['second_matrix']),
'4':
matrix.det(data['number'], data['first_matrix'])
}
# Вызов функции из словоря по списку входных данных
for param in calls:
if isinstance(dictionary[param],
list) and not (isinstance(dictionary[param][0], int) or
isinstance(dictionary[param][0], float)):
for i in range(len(dictionary[param])):
for j in range(len(dictionary[param][i])):
print(dictionary[param][i][j], '', end='')
print()
elif isinstance(dictionary[param],
def iterate(self, measurement, control):
now = time.time()
# timestamps matter. IMPORTANT: Do not use if first iteration has not been set.
if self.firstCall:
self.firstCall = False
timeTaken = time.time() - self.timeStamp
# major screwup if this happens
if timeTaken > 1.0:
print 'Interval was way too long: ', timeTaken, 'seconds.'
timeTaken = 0.0
self.timeStamp = time.time()
#velocity = motProxy.getRobotVelocity()
velocity = control
# known are speeds, convert to absolute movements
nao_movement_x = velocity[0] * timeTaken
nao_movement_y = velocity[1] * timeTaken
nao_movement_t = velocity[2] * timeTaken
#print timeTaken
# step forward using control vector u
self.u[0][0] = nao_movement_x
self.u[1][0] = nao_movement_y
print 'Increment control ', self.u
muBelief = self.mu
# ________ PREDICTION ________
# rotate using nao_movements theta
t = nao_movement_t
rotationmatrix = [[ math.cos( t ), -math.sin(t) ],[ math.sin(t), math.cos(t) ]]
# Predict new measurement based on nao movement.
# Nao moves x,y towards the ball
muBelief = matrix.subtract( muBelief , self.u )
#print muBelief
# Nao rotates theta
muBelief = matrix.mult( rotationmatrix, muBelief )
# add noise to motion
muBelief = sample( muBelief, self.Sigma, 2)
# covariance matrix update
SigmaBelief = matrix.plus( self.Sigma , self.R)
# ________ CORRECTION _________
if measurement:
self.z[0][0] = measurement[0]
self.z[1][0] = measurement[1]
# Since C = [1,0;0,1], drop it
s = matrix.inverse2D( matrix.plus( SigmaBelief, self.Q) )
K = matrix.mult(SigmaBelief, s )
self.mu = matrix.plus( muBelief, matrix.mult(K, matrix.subtract(self.z , muBelief)) )
self.Sigma = matrix.mult(matrix.subtract( self.I, K ), SigmaBelief)
else:
# if no ball is found, use the predicted state!
self.mu = muBelief
self.Sigma = SigmaBelief
#print 'Mu:',self.mu
#print 'Sigma: '
#matrix.show(self.Sigma)
#print ''
return (self.mu[0][0], self.mu[1][0])
import matrix import numpy as np mat1 = np.random.rand(8, 8) mat2 = np.random.rand(8) mat3 = np.random.rand(8, 8) r1, r2 = matrix.gauss(mat1, mat2, 8) result = matrix.mult(mat3, mat1, 8, 8) print(result)
