Beispiel #1
0
 def sym_grad_vector_basis(self, p=None):
     phi = self.basis(p=p)
     zero = sp.ZeroMatrix(1, phi.cols)
     A00 = sp.BlockMatrix([[phi.diff(self.x), zero]])
     A11 = sp.BlockMatrix([[zero, phi.diff(self.y)]])
     A01 = sp.BlockMatrix([[phi.diff(self.y) / 2, phi.diff(self.x) / 2]])
     return A00, A11, A01
Beispiel #2
0
    def matrix_E(self, p=None):
        p = self.p if p is None else p  # p >=2
        c = np.repeat(np.arange(1, p), np.arange(1, p))
        ldof0 = self.number_of_dofs(p=p - 2)
        E00 = sp.zeros(ldof0, ldof0)
        E10 = sp.zeros(ldof0, ldof0)

        for i in range(ldof0):
            E00[i, i] = self.h**2 / c[i]
            E10[i, i] = self.h**2 / c[i]

        if p == 2:
            return sp.BlockMatrix([[E00], [E10]])
        else:
            ldof1 = self.number_of_dofs(p=p - 3)
            E01 = sp.zeros(ldof0, ldof1)
            E11 = sp.zeros(ldof0, ldof1)

            idx = self.diff_index_1(p=p - 2)
            x = idx['x']
            y = idx['y']
            for i in range(ldof1):
                E01[y[0][i], i] = sp.Rational(y[1][i] * self.h**2, c[y[0][i]])
                E11[x[0][i], i] = sp.Rational(-x[1][i] * self.h**2, c[x[0][i]])
            return sp.BlockMatrix([[E00, E01], [E10, E11]])
Beispiel #3
0
 def get_tf_sp(self):
     self.T0S = sp.BlockMatrix([
         [self.R0S, sp.zeros(3, 1)], [sp.zeros(1, 3),
                                      sp.eye(1)]
     ]).as_explicit()  # Transformation function for parallel system.
     self.T0S_M = sp.BlockMatrix([
         [self.R0S_M, sp.zeros(3, 1)], [sp.zeros(1, 3),
                                        sp.eye(1)]
     ]).as_explicit()  # Transformation function for parallel system.
Beispiel #4
0
 def _construct_force_vector(self):
     # Ti_e = 2E(Pi).T * (M + (ui x Fi))
     Fi = self.Fi(self.t)
     Ti = self.Ti(self.t)
     Ti_e = 2*E(self.Pi).T * (Ti + Skew(self.ui)*Fi)
     self._Qi = sm.BlockMatrix([[Fi], [Ti_e]])
     
     Fj = -Fi
     Tj = -Ti
     Tj_e = 2*E(self.Pj).T * (Ti + Skew(self.uj)*Fj)
     self._Qj = sm.BlockMatrix([[Fj], [Tj_e]])
Beispiel #5
0
def get_foreman():
    # Basis transformation
    v_n = lambda i, N: sympy.Matrix(  # noqa: E731
        [1 if i == n else 0 for n in range(N)])
    S, X, Y, Z = [v_n(i, 4) for i in range(4)]
    up, dw = [v_n(i, 2) for i in range(2)]

    X = sympy.I * X
    Y = sympy.I * Y
    Z = sympy.I * Z

    molenkamp_basis = [
        kr(up, S),
        kr(dw, S),
        +(1 / sympy.sqrt(2)) * kr(up, X + sympy.I * Y),
        +(1 / sympy.sqrt(6)) * (kr(dw, X + sympy.I * Y) - kr(up, 2 * Z)),
        -(1 / sympy.sqrt(6)) * (kr(up, X - sympy.I * Y) + kr(dw, 2 * Z)),
        -(1 / sympy.sqrt(2)) * kr(dw, X - sympy.I * Y),
        +(1 / sympy.sqrt(3)) * (kr(dw, X + sympy.I * Y) + kr(up, Z)),
        +(1 / sympy.sqrt(3)) * (kr(up, X - sympy.I * Y) - kr(dw, Z)),
    ]

    subs_notation = {
        Ec: Ev + E0,
        L: -(g1 + 4 * g2) * (hbar**2 / 2 / m0),
        M: -(g1 - 2 * g2) * (hbar**2 / 2 / m0),
        Np: -(3 * g3 + (3 * kappa + 1)) * (hbar**2 / 2 / m0),
        Nm: -(3 * g3 - (3 * kappa + 1)) * (hbar**2 / 2 / m0),
        Ac: (g0 * hbar**2 / 2 / m0),
    }

    Hs = spin_orbit()

    Hcc = sympy.Matrix([Ec + kx * Ac * kx + ky * Ac * ky + kz * Ac * kz])
    Hcv = +sympy.I * sympy.Matrix([[P * kx, P * ky, P * kz]])
    Hvc = -sympy.I * sympy.Matrix([kx * P, ky * P, kz * P])

    data = [[valence_term(i, j) for j in range(3)] for i in range(3)]
    Hvv = sympy.Matrix(data)

    H4 = sympy.BlockMatrix([[Hcc, Hcv], [Hvc, Hvv]])
    H8 = sympy.Matrix(sympy.BlockDiagMatrix(H4, H4)) + Hs

    dag = lambda x: x.conjugate().transpose()  # noqa: E731
    U_dag = sympy.Matrix(sympy.BlockMatrix([molenkamp_basis]))
    U = dag(U_dag)

    hamiltonian = U * H8 * dag(U)
    hamiltonian = hamiltonian.subs(subs_notation)
    hamiltonian = hamiltonian + (Ev - Delta / 3) * sympy.diag(
        0, 0, 1, 1, 1, 1, 1, 1)
    return sympy.ImmutableMatrix(hamiltonian.expand())
Beispiel #6
0
    def get_rotation_matrix(self):
        self.jyhat = -self.XOE  # make x-axis of joystick to be on XOE plane. Since x & z-axis used to make xoe plane and y-axis has to perpendicular to both,
        # y-axis is thus perpendicular to XOE, i.e. the norm of XOE.
        # make x-axis to be on xoe plane, and hence to find y axis, cross(jyz,xhat) or y=z X x. therefore jyhat=-(XOE), !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        self.jxhat = self.jyhat.cross(
            self.jzhat
        )  # already unit vector, since jzhat and jyhat are perpendicular and are unit vectors.

        self.R0S = sp.BlockMatrix([[self.jxhat, self.jyhat,
                                    self.jzhat]]).as_explicit()
        self.R0S_M = sp.BlockMatrix(
            [[self.jxhat_M, self.jyhat_M,
              self.jzhat_M]]).as_explicit()  # todo: check
Beispiel #7
0
    def __init__(self, *args, **kwargs):
        name = 'ground'
        super().__init__(name)
        self.P_ground = quatrenion('Pg_%s' % self.name,
                                   format_as=r'{Pg_{%s}}' % self.name)

        self.normalized_pos_equation = sm.BlockMatrix(
            [[self.R], [self.P - self.P_ground]])
        self.normalized_vel_equation = sm.BlockMatrix([[zero_matrix(3, 1)],
                                                       [zero_matrix(4, 1)]])
        self.normalized_acc_equation = sm.BlockMatrix([[zero_matrix(3, 1)],
                                                       [zero_matrix(4, 1)]])
        self.normalized_jacobian = sm.BlockMatrix(
            [[sm.Identity(3), zero_matrix(3, 4)],
             [zero_matrix(4, 3), sm.Identity(4)]])
Beispiel #8
0
def hamilton(M):
    text = "Metoda Cayleya-Hamiltona<br>"
    eigen_values, eigen_vectors = eigen(M)
    text += 'Eigenvalues: $' + sp.latex(eigen_values) + ' = ' + sp.latex(
        eigen_values.evalf(5)) + '$<br>'
    eigen_values = eigen_values.evalf(5)

    x = sp.symbols(
        str(['a' + str(i)
             for i in range(len(eigen_values))])[1:-1].replace("'", ""))
    if len(eigen_values) == 1:
        x = [x]

    A = sp.ones(len(x), 1)

    for i in range(1, len(x)):
        A = sp.Matrix(sp.BlockMatrix([A, sp.HadamardPower(eigen_values, i)]))

    b = sp.Matrix(sp.HadamardPower(sp.E, eigen_values * t))
    result = sp.Matrix(list(sp.linsolve((A, b), *x))[0])
    text += "$"+sp.latex(b)+' = '+sp.latex(A)+'\\bullet'+sp.latex(sp.Matrix(x))+'\\implies' + \
            sp.latex(sp.Matrix(x))+' = '+sp.latex(result) + "$<br>"

    s = sp.diag(*tuple([1] * len(x))) * result[0, 0]
    for i in range(1, len(x)):
        s += M * result[i, 0]

    text += "$e^{\\mathbb{A}t} = " + sp.latex(sp.simplify(s)) + "$<br><br>"

    return s, text
Beispiel #9
0
    def _preare_second_phase(self):
        self.non_basic_variables = [
            i for i in self.non_basic_variables
            if i not in self.artificial_variables
        ]
        A = self.T_star[:, self.non_basic_variables]
        I = sy.eye(len(self.basic_variables))

        b = self.T_star[:, -1]
        T = sy.Matrix(sy.BlockMatrix([[A, I, b]]))

        var_names = np.array(self.variable_names)
        non_basic_varnames = list(var_names[self.non_basic_variables])
        basic_varnames = list(var_names[self.basic_variables])
        new_varnames = non_basic_varnames + basic_varnames
        t = sy.zeros(1, len(new_varnames) + 1)
        for var_name in self.objective:
            index = new_varnames.index(var_name)
            t[0, index] = -self.objective[var_name]

        for var_name in self.objective:
            index = basic_varnames.index(var_name)
            t = t + self.objective[var_name] * T[index, :]

        # print(t)
        # print(T)
        # print(self.variable_names)
        # print(self.basic_variables)
        # print(self.non_basic_variables)
        self.t = t
        self.T = T
        self.running_variable_names = new_varnames
Beispiel #10
0
def M(i, n=None):
    """Elimination theory matrix: i^th matrix in the recursion with n external legs. Default is i = n."""
    if n is None:
        n = i
    zs = punctures(n)
    if i < 3:
        raise Exception("Elimination theory matrix: invalid argument.")
    elif i == 3:
        return sympy.Matrix([])
    elif i == 4:
        return hms(n)
    else:
        M_im1 = M(i - 1, n)
        M_im1_diff = sympy.diff(M_im1, zs[i - 3 - 1])
        M_im1 = M_im1.subs(
            zs[i - 3 - 1], 0
        )  # just a trick to simplify calculation, nothing to do with Mobius
        rows, columns = M_im1.shape
        M_i = sympy.Matrix(
            sympy.BlockMatrix(
                tuple(
                    tuple([
                        M_im1 if _i == _j else M_im1_diff if _i == _j +
                        1 else sympy.zeros(rows, columns)
                        for _i in range(i - 3)
                    ]) for _j in range(i - 4))))
        return M_i
Beispiel #11
0
def Psi(n):
    """Psi anti-symmetric matrix (reduced)."""
    ks = sympy.symbols('k1:{}'.format(n + 1))
    es = sympy.symbols('e1:{}'.format(n + 1))
    zs = sympy.symbols('z1:{}'.format(n + 1))

    # A Matrix
    A = sympy.zeros(n, n)
    for a in range(n):
        for b in range(a + 1, n):
            A[a, b] = 2 * ks[a] * ks[b] / (zs[a] - zs[b])
            A[b, a] = -A[a, b]

    # B Matrix
    B = sympy.zeros(n, n)
    for a in range(n):
        for b in range(a + 1, n):
            B[a, b] = 2 * es[a] * es[b] / (zs[a] - zs[b])
            B[b, a] = -B[a, b]

    # C Matrix
    C = sympy.zeros(n, n)
    for a in range(n):
        for b in range(n):
            if a == b:
                C[a, b] = -sum([
                    2 * es[a] * ks[j] / (zs[a] - zs[j])
                    for j in range(n) if j != a
                ])
            else:
                C[a, b] = 2 * es[a] * ks[b] / (zs[a] - zs[b])

    # Psi - from block matrix
    Psi = sympy.Matrix(sympy.BlockMatrix(((A, -C.T), (C, B))))

    # drop rows & cols 1 and 2 (i.e. 0 and 1)
    Psi = Psi[range(2, 2 * n), range(2, 2 * n)]

    # simplify operations involving infinity
    for i in range(1, n):
        Psi = Psi.subs(((zs[0] - zs[i], zs[0]), (zs[i] - zs[0], -zs[0]),
                        (zs[i] + zs[0], zs[0]), (-zs[i] - zs[0], -zs[0])))

    # pull out z1 ** -2  ---  Pfaffian goes aas root of this
    for i in range(2 * n - 2):
        if Psi[-n, i] != 0:
            Psi[-n, i] = Psi[-n, i] * zs[0]
        if Psi[i, -n] != 0:
            Psi[i, -n] = Psi[i, -n] * zs[0]

    # Mobius fix
    Psi = Psi.subs(((zs[0], sympy.oo), (zs[1], 1), (zs[n - 1], 0)))

    # simplify
    for i in range(2, n):
        Psi = Psi.subs(((sympy.oo - zs[i], sympy.oo), ))
        Psi = Psi.subs(((-sympy.oo + zs[i], -sympy.oo), ))
        Psi = Psi.subs(((sympy.oo + zs[i], sympy.oo), ))

    return Psi
def Displace(d_row):
    d1, d2, d3 = d_row
    I = smp.Matrix.eye(3)
    Z = smp.zeros(1, 3)
    p = smp.Matrix([[d1], [d2], [d3]])
    blockMatrix = smp.BlockMatrix([[I, p], [Z, smp.Matrix([1])]])
    return smp.Matrix(blockMatrix)
Beispiel #13
0
 def pos_level_equations(self):
     """
     A block matrix that stores the set of matrix/vector equations that
     represents the position constraints equations. The shape of the blocks
     is (nve, 1), where the scalar shape is (nc, 1).
     """
     return sm.BlockMatrix(self._pos_level_equations)
Beispiel #14
0
 def perp_grad_space_basis(self, p=None):
     p = self.p if p is None else p
     if p > 1:
         phi = self.basis(p=p - 1)
         return sp.BlockMatrix([phi[2] * phi, -phi[1] * phi])
     else:
         return sp.ZeroMatrix(2, 1)
Beispiel #15
0
 def jacobian_j(self):
     """
     A block matrix that stores the jacobian of the constraint equations
     with respect to the body_j generalized coordinates. The shape of the 
     blocks is (nve, 2), where the scalar shape is (nc, 7).
     """
     return sm.BlockMatrix(self._jacobian_j)
Beispiel #16
0
 def acc_level_equations(self):
     """
     A block matrix that stores the set of matrix/vector equations that
     represents the right hand-side of the acceleration level of the 
     constraints equations. The shape of the blocks is (nve, 1), where the 
     scalar shape is (nc, 1).
     """
     return sm.BlockMatrix(self._acc_level_equations)
Beispiel #17
0
def construct_matrix_from_block_matrix(A_expr):

    A_expr = sympy.BlockMatrix(A_expr)
    A_collapsed_expr = sympy.Matrix.zeros(A_expr.rows, A_expr.cols)
    for r in range(A_expr.rows):
        for c in range(A_expr.cols):
            A_collapsed_expr[r, c] = A_expr[r, c]
    return A_collapsed_expr
Beispiel #18
0
def test_BlockMatrix():
    n = sy.Symbol('n', integer=True)
    A, B, C, D = [sy.MatrixSymbol(name, n, n) for name in 'ABCD']
    At, Bt, Ct, Dt = map(theano_code_, (A, B, C, D))
    Block = sy.BlockMatrix([[A, B], [C, D]])
    Blockt = theano_code_(Block)
    solutions = [tt.join(0, tt.join(1, At, Bt), tt.join(1, Ct, Dt)),
                 tt.join(1, tt.join(0, At, Ct), tt.join(0, Bt, Dt))]
    assert any(theq(Blockt, solution) for solution in solutions)
Beispiel #19
0
    def _construct_force_vector(self):

        dij = self.joint.dij
        distance = sm.sqrt(dij.T * dij)
        unit_vector = dij / distance

        defflection = self.LF - distance[0, 0]
        velocity = (unit_vector.T * self.joint.dijd)
        velocity = sm.sqrt(velocity.T * velocity)[0, 0]

        self.Fi = unit_vector * (self.Fs(defflection) - self.Fd(velocity))
        Ti_e = 2 * G(self.Pi).T * (self.Ti + Skew(self.ui).T * self.Fi)

        self._Qi = sm.BlockMatrix([[self.Fi], [Ti_e]])

        self.Fj = -self.Fi
        Tj_e = 2 * G(self.Pj).T * (self.Tj + Skew(self.uj).T * self.Fj)
        self._Qj = sm.BlockMatrix([[self.Fj], [Tj_e]])
def HomogeneousInverse(T):
    R = T[0:3, 0:3]
    p = T[0:3, 3]
    R_inv = smp.Matrix.transpose(R)
    p_inv = -R_inv * p
    blockMatrix = smp.BlockMatrix([[R_inv, p_inv],
                                   [smp.zeros(1, 3),
                                    smp.Matrix([1])]])
    return smp.Matrix(blockMatrix)
def ScrewTose3(S):
    S1 = smp.Matrix(S).reshape(6, 1)
    w = S1[0:3, 0].reshape(1, 3)
    v = S1[3:, 0].reshape(3, 1)
    wSkew = SkewSymmetric(w)
    blockMatrix = smp.BlockMatrix([[wSkew, v],
                                   [smp.zeros(1, 3),
                                    smp.Matrix([0])]])
    return smp.Matrix(blockMatrix)
Beispiel #22
0
    def _construct_force_vector(self):
        
        dij  = (self.Ri + self.ui - self.Rj - self.uj)
        dijd = (self.Rdi + self.Bui*self.Pdi - self.Rdj - self.Buj*self.Pdj)

        bush_trasformation = self.mi_bar.A.T * self.Ai.T

        F_bush_i = self.Fs(bush_trasformation, dij) \
                 + self.Fd(bush_trasformation, dijd)

        self.Fi = self.Ai * self.mi_bar.A * -F_bush_i
        Ti_e = - 2*E(self.Pi).T * Skew(self.ui).T * self.Fi
        #Ti_e = -(self.Ai * Skew(self.ui_bar) * 2*G(self.Pi)).T * self.Fi
        self._Qi = sm.BlockMatrix([[self.Fi], [Ti_e]])
        
        self.Fj = -self.Fi
        Tj_e = - 2*E(self.Pj).T * Skew(self.uj).T * self.Fj
        #Tj_e = -(self.Aj * Skew(self.uj_bar) * 2*G(self.Pj)).T * self.Fj
        self._Qj = sm.BlockMatrix([[self.Fj], [Tj_e]])
Beispiel #23
0
 def r_lambda(M):
     _N = M[:-1, :-1]
     nb_rows, nb_cols = _N.shape
     fac = sp.factorial
     eN = sum([_N**i / fac(i) for i in range(0, n + 1)],
              start=sp.zeros(*_N.shape))
     return sp.Matrix(
         sp.BlockMatrix(
             [[eN, sp.zeros(nb_rows, 1)],
              [sp.zeros(1, nb_cols),
               sp.Matrix([sp.exp(M[-1, -1])])]]))
Beispiel #24
0
 def r_lambda(M):
     _N = M[:-1, :-1]
     nb_rows, nb_cols = _N.shape
     hnm = _h(n, m)(_N).evalf()
     knm = _k(n, m)(_N).evalf()
     iknm = sp.refine(invert(knm), sp.Q.integer(k))
     eN = sp.refine(sp.simplify(hnm) * iknm, sp.Q.integer(k))
     return sp.Matrix(
         sp.BlockMatrix(
             [[eN, sp.zeros(nb_rows, 1)],
              [sp.zeros(1, nb_cols),
               sp.Matrix([sp.exp(M[-1, -1])])]]))
Beispiel #25
0
def test_BlockMatrix():
    n = sympy.Symbol('n', integer=True)
    A = sympy.MatrixSymbol('A', n, n)
    B = sympy.MatrixSymbol('B', n, n)
    C = sympy.MatrixSymbol('C', n, n)
    D = sympy.MatrixSymbol('D', n, n)
    At, Bt, Ct, Dt = map(theano_code, (A, B, C, D))
    Block = sympy.BlockMatrix([[A, B], [C, D]])
    Blockt = theano_code(Block)
    solutions = [tt.join(0, tt.join(1, At, Bt), tt.join(1, Ct, Dt)),
                 tt.join(1, tt.join(0, At, Ct), tt.join(0, Bt, Dt))]
    assert any(theq(Blockt, solution) for solution in solutions)
Beispiel #26
0
def balancer(left_side, right_side): #Algorithm based on: http://www.logical.ai/chemistry/html/chem-nullspace.html
    """Takes in left_side, which is a list of tuples (left side is number of molecules (0 if unknown), right side is the molecule itself)."""
    #Test to ensure each side has the same elements
    left_element_list = []
    for mol in left_side: #Loads atoms into list
        for atom in mol[1].atoms:
            if atom[0] not in left_element_list:
                left_element_list.append(atom[0])
    right_element_list = []
    for mol in right_side: #Loads atoms into other list
        for atom in mol[1].atoms:
            if atom[0] not in right_element_list:
                right_element_list.append(atom[0])
    left_element_list.sort() #Sorts lists into order (so they can be compared)
    right_element_list.sort()
    if left_element_list != right_element_list:
        raise Exception("Different elements on left and right side of chemical equation")
    molecule_list = left_side + right_side
    #Create array
    element_matrix = sympy.zeros(len(left_element_list), len(left_side) + len(right_side))
    for i in range(0, len(molecule_list)):
        for atom in molecule_list[i][1].atoms:
            element_matrix[left_element_list.index(atom[0]), i] = atom[1] #Uses the left side list to determine the order that different elements go into the matrix- it is arbitrary, as long as it is consistent
    element_matrix = element_matrix.T
    element_matrix = sympy.BlockMatrix([element_matrix, sympy.eye(len(left_side + right_side))], axis=1)
    element_matrix_rref = sympy.Matrix(element_matrix).rref()
    solution_list = []
    for i in range(0, element_matrix_rref[0].shape[0]): #Loop through list, and test if ansewers are solutions according to the algorithm (i.e. their pivots are further right than the number of atoms added)
        if element_matrix_rref[1][i] >= len(left_element_list):
            solution_list.append(sympy.Matrix(element_matrix_rref[0].row(i)))
    if len(solution_list) == 0:
        return None
    #Make negatives positive
    for solution in solution_list:
        for i in range(0, len(solution)):
                if solution[i] < 0:
                    solution[i] = solution[i] * -1
    #Turn fractions into whole numbers
    for i in range(0, len(solution_list)):
        while True:
            largest_denominator = 1 #Finds largest denominator, then multiplies by that until all denominators are 1
            for j in range(0, solution_list[i].shape[1]):
                if solution_list[i][j].q != 1:
                    if solution_list[i][j].q > largest_denominator:
                        largest_denominator = solution_list[i][j].q
            if largest_denominator != 1:
                solution_list[i] = solution_list[i] * largest_denominator
            else:
                break
    solution = solution_list[0].tolist()[0] #Convert to list
    solution = solution[len(left_element_list):]
    return solution
Beispiel #27
0
    def _construct_force_vector(self):
        
        dij  = (self.Ri + self.ui - self.Rj - self.uj)
        dijd = (self.Rdi + self.Bui*self.Pdi - self.Rdj - self.Buj*self.Pdj)

        dij_bush_i  = self.mi_bar.A.T * self.Ai.T * dij
        dijd_bush_i = self.mi_bar.A.T * self.Ai.T * dijd
        F_bush_i = (self.Kt*sm.Identity(3) * dij_bush_i) \
                 + (self.Ct*sm.Identity(3) * dijd_bush_i)

        self.Fi = self.Ai * self.mi_bar.A * -F_bush_i
        Ti_e = - 2*E(self.Pi).T * Skew(self.ui).T * self.Fi
        self._Qi = sm.BlockMatrix([[self.Fi], [Ti_e]])
        
        dij_bush_j  = self.mj_bar.A.T * self.Aj.T * dij
        dijd_bush_j = self.mj_bar.A.T * self.Aj.T * dijd
        F_bush_j = (self.Kt*sm.Identity(3) * dij_bush_j) \
                 + (self.Ct*sm.Identity(3) * dijd_bush_j)

        self.Fj = self.Aj * self.mj_bar.A * F_bush_j
        Tj_e = - 2*E(self.Pj).T * Skew(self.uj).T * self.Fj
        self._Qj = sm.BlockMatrix([[self.Fj], [Tj_e]])
Beispiel #28
0
    def _construct_force_vector(self):
        
        dij  = (self.Ri + self.ui - self.Rj - self.uj)
        dijd = (self.Rdi + self.Bui*self.Pdi - self.Rdj - self.Buj*self.Pdj)

        # Fs = K(l - l0) + C*ld + Fa
        l  = sm.sqrt(dij.T*dij)[0,0]
        l0 = self.LF
        unit_vector = dij/l
        ld = -((unit_vector).T * dijd)

        defflection = l0 - l

        Fs = -self.Fs(defflection) - self.Fd(ld)

        self.Fi = -Fs * unit_vector
        Ti_e = Fs * 2*E(self.Pi).T * Skew(self.ui).T * unit_vector        
        
        self.Fj = -self.Fi
        Tj_e = -Fs * 2*E(self.Pj).T * Skew(self.uj).T * unit_vector

        self._Qi = sm.BlockMatrix([[self.Fi], [Ti_e]])
        self._Qj = sm.BlockMatrix([[self.Fj], [Tj_e]])
Beispiel #29
0
def my_exp(L, t, k):
    """
    In fact we don't need to a `sp.MatrixSymbol` exponential
    We only need to return a matrix of function : (`matrix_ij(dt,k)`)_{i,j}
  """
    nb_cols, nb_rows = L.shape
    matExp = sp.Matrix(
        sp.BlockMatrix([[
            sp.Matrix([[
                sp.Function("matrixExpr{}{}".format(i, j))(t, k)
                for j in range(nb_cols - 1)
            ] for i in range(nb_rows - 1)]),
            sp.zeros(nb_rows - 1, 1)
        ], [sp.zeros(1, nb_cols - 1),
            sp.Matrix([sp.exp(t * L[-1, -1])])]]))
    return matExp
Beispiel #30
0
def funcMatExp(L, t, k):
    """
    In fact we don't need to compute the real $e^{tL}$ with a classic
    exponential function, or with Pade approximant or Taylor serie, we
    only need to manipulate the matrix of IT functions.
    This function returns the matrix of IT functions.
  """
    nb_cols, nb_rows = L.shape
    B = sp.Matrix([[
        sp.Function("matrixExpr{}{}".format(i, j))(t, k)
        for j in range(nb_cols - 1)
    ] for i in range(nb_rows - 1)])
    matExp = sp.Matrix(
        sp.BlockMatrix(
            [[B, sp.zeros(nb_rows - 1, 1)],
             [sp.zeros(1, nb_cols - 1),
              sp.Matrix([sp.exp(t * L[-1, -1])])]]))
    return matExp