Beispiel #1
0
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")
Beispiel #2
0
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]    
Beispiel #4
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])
Beispiel #7
0
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
    ])
Beispiel #9
0
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]])
Beispiel #10
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
Beispiel #11
0
    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],
Beispiel #12
0
    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])
Beispiel #13
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)