Пример #1
0
    def get_dixon_polynomial(self):
        r"""
        Returns
        =======

        dixon_polynomial: polynomial
            Dixon's polynomial is calculated as:

            delta = Delta(A) / ((x_1 - a_1) ... (x_n - a_n)) where,

            A =  |p_1(x_1,... x_n), ..., p_n(x_1,... x_n)|
                 |p_1(a_1,... x_n), ..., p_n(a_1,... x_n)|
                 |...             , ...,              ...|
                 |p_1(a_1,... a_n), ..., p_n(a_1,... a_n)|
        """
        if self.m != (self.n + 1):
            raise ValueError('Method invalid for given combination.')

        # First row
        rows = [self.polynomials]

        temp = list(self.variables)

        for idx in range(self.n):
            temp[idx] = self.dummy_variables[idx]
            substitution = {var: t for var, t in zip(self.variables, temp)}
            rows.append([f.subs(substitution) for f in self.polynomials])

        A = Matrix(rows)

        terms = zip(self.variables, self.dummy_variables)
        product_of_differences = Mul(*[a - b for a, b in terms])
        dixon_polynomial = (A.det() / product_of_differences).factor()

        return poly_from_expr(dixon_polynomial, self.dummy_variables)[0]
Пример #2
0
def linear_rref(A, b, Matrix=None, S=None):
    """ Transform a linear system to reduced row-echelon form

    Transforms both the matrix and right-hand side of a linear
    system of equations to reduced row echelon form

    Parameters
    ----------
    A: Matrix-like
        iterable of rows
    b: iterable

    Returns
    -------
    A', b' - transformed versions

    """
    if Matrix is None:
        from sympy import Matrix
    if S is None:
        from sympy import S
    mat_rows = [_map2l(S, list(row) + [v]) for row, v in zip(A, b)]
    aug = Matrix(mat_rows)
    raug, pivot = aug.rref()
    nindep = len(pivot)
    return raug[:nindep, :-1], raug[:nindep, -1]
Пример #3
0
class MatrixGenerator:
    
    def __init__(self, row_size):
        first_row = [0] * row_size
        self.matrix = Matrix([first_row])
        self.has_added_row = False
        self.count = 0
        
    def AddRow(self, row):
        self.matrix = self.matrix.row_insert(0, Matrix([row]))
        self.count += 1
        if not self.has_added_row:
            self.matrix.row_del(1)
            self.has_added_row = True
            
    
        
    
        
        
        
        
                
        
                
                
Пример #4
0
def _compute_alpha_wrench(model, robo, j):
    """
    Compute the wrench as a function of tau - joint torque without
    friction.

    Args:
        model: An instance of DynModel
        robo: An instance of Robot
        j: joint number

    Returns:
        An instance of DynModel that contains all the new values.
    """
    j_alpha_j = Screw()
    # local variables
    j_a_j = robo.geos[j].axisa
    j_k_j = model.no_qddot_inertias[j].val
    j_gamma_j = model.gammas[j].val
    j_inertia_j_s = model.star_inertias[j].val
    j_beta_j_s = model.star_betas[j].val
    h_j = Matrix([model.joint_inertias[j]])
    tau_j = Matrix([model.taus[j]])
    # actual computation
    j_alpha_j.val = (j_k_j * j_gamma_j) + \
        (j_inertia_j_s * j_a_j * h_j.inv() * \
        (tau_j + j_a_j.transpose() * j_beta_j_s)) - j_beta_j_s
    # store in model
    model.alphas[j] = j_alpha_j
    return model
Пример #5
0
def _compute_joint_acceleration(model, robo, j, i):
    """
    Compute the joint acceleration for joint j.

    Args:
        model: An instance of DynModel
        robo: An instance of Robot
        j: link number
        i: antecedent value

    Returns:
        An instance of DynModel that contains all the new values.
    """
    # local variables
    j_a_j = robo.geos[j].axisa
    j_s_i = robo.geos[j].tmat.s_j_wrt_i
    j_gamma_j = model.gammas[j].val
    j_inertia_j_s = model.star_inertias[j].val
    j_beta_j_s = model.star_betas[j].val
    h_j = Matrix([model.joint_inertias[j]])
    tau_j = Matrix([model.taus[j]])
    i_vdot_i = model.accels[i].val
    # actual computation
    qddot_j = h_j.inv() * (-j_a_j.transpose() * j_inertia_j_s * \
        ((j_s_i * i_vdot_i) + j_gamma_j) + tau_j + \
        (j_a_j.transpose() * j_beta_j_s))
    # store in model
    model.qddots[j] = qddot_j[0, 0]
    return model
Пример #6
0
def generalized_active_forces_V(V, q, u, kde_map, vc_map=None):
    """Returns a list of the generalized active forces using equation 5.1.18
    from Kane 1985.

    'V' is a potential energy function for the system.
    'q' is a list of generalized coordinates.
    'u' is a list of the independent generalized speeds.
    'kde_map' is a dictionary with q dots as keys and the equivalent
        expressions in terms of q's and u's as values.
    'vc_map' is a dictionay with the dependent u's as keys and the expression
        in terms of independent u's as values.
    """
    n = len(q)
    p = len(u)
    m = n - p
    if vc_map is None:
        A_kr = Matrix.zeros(m, p)
    else:
        A_kr, _ = vc_matrix(u, vc_map)
        u_ = u + sorted(vc_map.keys(), cmp=lambda x, y: x.compare(y))
    W_sr, _ = kde_matrix(u_, kde_map)

    dV_dq = map(lambda x: diff(V, x), q)
    Fr = Matrix.zeros(1, p)
    for s in range(n):
        Fr -= dV_dq[s] * (W_sr[s, :p] + W_sr[s, p:]*A_kr[:, :p])
    return Fr[:]
Пример #7
0
def test_extract():
    m = Matrix(4, 3, lambda i, j: i*3 + j)
    assert m.extract([0,1,3],[0,1]) == Matrix(3,2,[0,1,3,4,9,10])
    assert m.extract([0,3],[0,0,2]) == Matrix(2,3,[0,0,2,9,9,11])
    assert m.extract(range(4),range(3)) == m
    raises(IndexError, 'm.extract([4], [0])')
    raises(IndexError, 'm.extract([0], [3])')
Пример #8
0
def test_symbolic_twoport():
    circuit.default_toolkit = symbolic
    cir = SubCircuit()

    k = symbolic.kboltzmann
    var('R1 R0 C1 w T', real=True, positive=True)
    s = 1j*w

    cir['R0'] = R(1, gnd, r=R0)
    cir['R1'] = R(1, 2, r=R1)
#    cir['C1'] = C(2, gnd, c=C1)

    ## Add an AC source to verify that the source will not affect results
#    cir['IS'] = IS(1, gnd, iac=1) 

    ## Run symbolic 2-port analysis
    twoport_ana = TwoPortAnalysis(cir, Node('1'), gnd, Node('2'), gnd,
                                  noise = True, toolkit=symbolic,
                                  noise_outquantity = 'v')
    result = twoport_ana.solve(freqs=s, complexfreq=True)
    
    ABCD = Matrix(result['twoport'].A)
    ABCD.simplify()

    assert_array_equal(ABCD, np.array([[1 + 0*R1*C1*s, R1],
                                    [(1 + 0*R0*C1*s + 0*R1*C1*s) / R0,  (R0 + R1)/R0]]))

    assert_array_equal(simplify(result['Sin'] - (4*k*T/R0 + 4*R1*k*T/R0**2)), 0)
    assert_array_equal(simplify(result['Svn']), 4*k*T*R1)
Пример #9
0
def generalized_inertia_forces_K(K, q, u, kde_map, vc_map=None):
    """Returns a list of the generalized inertia forces using equation 5.6.6
    from Kane 1985.

    'K' is a potential energy function for the system.
    'q' is a list of generalized coordinates.
    'u' is a list of the independent generalized speeds.
    'kde_map' is a dictionary with q dots as keys and the equivalent
        expressions in terms of q's and u's as values.
    'vc_map' is a dictionay with the dependent u's as keys and the expression
        in terms of independent u's as values.
    """
    n = len(q)
    p = len(u)
    m = n - p
    t = symbols('t')
    if vc_map is None:
        A_kr = Matrix.zeros(m, p)
    else:
        A_kr, _ = vc_matrix(u, vc_map)
        u_ = u + sorted(vc_map.keys(), cmp=lambda x, y: x.compare(y))
    W_sr, _ = kde_matrix(u_, kde_map)

    qd = map(lambda x: x.diff(t), q)
    K_partial_term = [K.diff(qd_s).diff(t) - K.diff(q_s)
                      for qd_s, q_s in zip(qd, q)]
    K_partial_term = subs(K_partial_term, kde_map)
    if vc_map is not None:
        K_partial_term = subs(K_partial_term, vc_map)
    Fr_star = Matrix.zeros(1, p)
    for s in range(n):
        Fr_star -= K_partial_term[s] * (W_sr[s, :p] + W_sr[s, p:]*A_kr[:, :p])
    return Fr_star[:]
Пример #10
0
    def _form_permutation_matrices(self):
        """Form the permutation matrices Pq and Pu."""

        # Extract dimension variables
        l, m, n, o, s, k = self._dims
        # Compute permutation matrices
        if n != 0:
            self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
            if l > 0:
                self._Pqi = self._Pq[:, :-l]
                self._Pqd = self._Pq[:, -l:]
            else:
                self._Pqi = self._Pq
                self._Pqd = Matrix()
        if o != 0:
            self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
            if m > 0:
                self._Pui = self._Pu[:, :-m]
                self._Pud = self._Pu[:, -m:]
            else:
                self._Pui = self._Pu
                self._Pud = Matrix()
        # Compute combination permutation matrix for computing A and B
        P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
        P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
        if P_col1:
            if P_col2:
                self.perm_mat = P_col1.row_join(P_col2)
            else:
                self.perm_mat = P_col1
        else:
            self.perm_mat = P_col2
Пример #11
0
def tet4():
    N1 = z
    N2 = n
    N3 = b
    N4 = 1 - z - n - b

    X = Matrix([[1, x, y, z]])
    X2 = Matrix([[1, x1, y1, z1],
                 [1, x2, y2, z2],
                 [1, x3, y3, z3],
                 [1, x4, y4, z4]])
    N = X * X2.inv()
    N1 = N[0, 0]
    N2 = N[0, 1]
    N3 = N[0, 2]
    N4 = N[0, 3]

    N = Matrix([[N1, 0, 0, N2, 0, 0, N3, 0, 0, N4, 0, 0],
                [0, N1, 0, 0, N2, 0, 0, N3, 0, 0, N4, 0],
                [0, 0, N1, 0, 0, N2, 0, 0, N3, 0, 0, N4]])
    NT = N.transpose()
    #pdV = p*v
    pdV = 3
    factorI = 1
    M = makeM(pdV, NT, factorI, levels=3)
    print "Mtet = \n", M
Пример #12
0
def test_hessenberg():
    A = Matrix([[3, 4, 1],[2, 4 ,5],[0, 1, 2]])
    assert A.is_upper_hessenberg()
    assert A.transpose().is_lower_hessenberg()

    A = Matrix([[3, 4, 1],[2, 4 ,5],[3, 1, 2]])
    assert not A.is_upper_hessenberg()
Пример #13
0
    def _kindiffeq(self, kdeqs):
        """Supply all the kinematic differential equations in a list.

        They should be in the form [Expr1, Expr2, ...] where Expri is equal to
        zero

        Parameters
        ==========

        kdeqs : list (of Expr)
            The listof kinematic differential equations

        """
        if len(self._q) != len(kdeqs):
            raise ValueError('There must be an equal number of kinematic '
                             'differential equations and coordinates.')

        uaux = self._uaux
        # dictionary of auxiliary speeds which are equal to zero
        uaz = dict(zip(uaux, [0] * len(uaux)))

        kdeqs = Matrix(kdeqs).subs(uaz)

        qdot = self._qdot
        qdotzero = dict(zip(qdot, [0] * len(qdot)))
        u = self._u
        uzero = dict(zip(u, [0] * len(u)))

        f_k = kdeqs.subs(uzero).subs(qdotzero)
        k_kqdot = (kdeqs.subs(uzero) - f_k).jacobian(Matrix(qdot))
        k_ku = (kdeqs.subs(qdotzero) - f_k).jacobian(Matrix(u))

        self._k_ku = self._mat_inv_mul(k_kqdot, k_ku)
        self._f_k = self._mat_inv_mul(k_kqdot, f_k)
        self._k_kqdot = eye(len(qdot))
Пример #14
0
    def __init__(self, frame):
        """Supply the inertial frame for Kane initialization. """
        # Big storage things
        self._inertial = frame
        self._forcelist = None
        self._bodylist = None
        self._fr = None
        self._frstar = None
        self._rhs = None
        self._aux_eq = None

        # States
        self._q = None
        self._qdep = []
        self._qdot = None
        self._u = None
        self._udep = []
        self._udot = None
        self._uaux = None

        # Differential Equations Matrices
        self._k_d = None
        self._f_d = None
        self._k_kqdot = None
        self._k_ku = None
        self._f_k = None

        # Constraint Matrices
        self._f_h = Matrix([])
        self._k_nh = Matrix([])
        self._f_nh = Matrix([])
        self._k_dnh = Matrix([])
        self._f_dnh = Matrix([])
Пример #15
0
def Newton_solver_pq(error, hx, ht, gx, gt, x0):
    
    p, q  = symbols('p q')
    
    epsilon1 = gx*hx[-2]**p + gt*ht[-2]**q - error[-2]
    epsilon2 = gx*hx[-1]**p + gt*ht[-1]**q - error[-1]


    epsilon = [epsilon1, epsilon2]
    x = [p, q]
    
    def f(i,j):
        return diff(epsilon[i], x[j])
        
    Jacobi = Matrix(2, 2, f)

    JacobiInv = Jacobi.inv()
    epsilonfunc = lambdify(x, epsilon)
    JacobiInvfunc = lambdify(x, JacobiInv)
    x = x0
    
    
    

    F = np.asarray(epsilonfunc(x))
    
    for n in range(8):
        Jinv = np.asarray(JacobiInvfunc(x))
        F = np.asarray(epsilonfunc(x))
        x = x - np.dot(Jinv, F)

    return x
Пример #16
0
def Newton_solver_gxgt(error, hx, ht, x0, p_value=1, q_value=1):
    
    gx, gt, p, q, hx1, hx2, ht1, ht2  = symbols('gx gt p q qhx1 hx2 ht1 ht2')
    epsilon1 = gx*hx1**p + gt*ht1**q - error[-2]
    epsilon2 = gx*hx2**p + gt*ht2**q - error[-1]


    epsilon = [epsilon1, epsilon2]
    x = [gx, gt]
    knowns = [p, q, hx1, hx2, ht1, ht2]
    
    def f(i,j):
        return diff(epsilon[i], x[j])
        
    Jacobi = Matrix(2, 2, f)

    JacobiInv = Jacobi.inv()
    epsilonfunc = lambdify([x, knowns], epsilon)
    JacobiInvfunc = lambdify([x, knowns], JacobiInv)
    x = x0
    knowns = [p_value, q_value, hx[-2], hx[-1], ht[-2], ht[-1]]
    F = np.asarray(epsilonfunc(x, knowns))
    
    for n in range(8):
        Jinv = np.asarray(JacobiInvfunc(x, knowns))
        F = np.asarray(epsilonfunc(x, knowns))
        x = x - np.dot(Jinv, F)

        print "n, x: ", n, x
    
    return x
Пример #17
0
def test_Matrix_sum():
    x, y, z = Symbol('x'), Symbol('y'), Symbol('z')
    M = Matrix([[1,2,3],[x,y,x],[2*y,-50,z*x]])
    m = matrix([[2,3,4],[x,5,6],[x,y,z**2]])
    assert M+m == Matrix([[3,5,7],[2*x,y+5,x+6],[2*y+x,y-50,z*x+z**2]])
    assert m+M == Matrix([[3,5,7],[2*x,y+5,x+6],[2*y+x,y-50,z*x+z**2]])
    assert M+m == M.add(m)
Пример #18
0
def main():
    fp=open('clusters','w') 
    fp2=open('eigenvalues and best cluster number k','w')
    fp3=open('cluster sizes','w')
    trainingSet=[]
    loadDataset1('roadNet-PA.csv',trainingSet)
    G=Graph_build(trainingSet)
    print len(G.nodes())
    N_to_M, M_to_N=Node_num_and_matrix_num(G)
    similarity_matrix=similarity_cal(G,N_to_M)
    degree_matrix=degree_cal(similarity_matrix)
    L_matrix=L_cal(degree_matrix,similarity_matrix)
    M=Matrix(L_matrix)
    print 'start eigenvectors'
    MM=M.eigenvects()
    print 'end eigenvectors'
    v,w=get_eigen(L_matrix)
    L=len(G.nodes())
    k=get_k(MM)
    w=get_new_eigenvectors(MM,w)
    for x in range(len(v)):
        fp2.write(repr(v[x])+'\n')
    Y=get_Y(w,L,k)
    Y_pred=KMeans(n_clusters=k, max_iter=600).fit_predict(Y)
    clusterList=clusters_cal(k,Y_pred,M_to_N)
    for x in range(len(clusterList)):
        fp.write('This is '+repr(x)+'th cluster and it has members:\n')
        fp.write(repr(clusterList[x])+'\n\n\n\n')
        fp3.write(repr(len(clusterList[x]))+'\n')
    for x in range(len(clusterList)):
        G_new=new_Graph_build(G,clusterList[x])
        random_mqi(G_new,x)
def spin_vector_to_state(spin):
    from sympy import Matrix
    from sglib_v0 import sigma_x, sigma_y, sigma_z, find_eigenvectors
    from numpy import shape
    # The spin vector must have three components.
    if shape(spin) == () or len(spin) != 3:
        print('Error: spin vector must have three elements.')
        return
        
    # Make sure the values are floats and make sure it's really
    # a column vector.
    spin = [ float(spin[0]), float(spin[1]), float(spin[2]) ]
    spin = Matrix(spin)
    
    # Make sure its normalized. Do this silently, since it will
    # probably happen most of the time due to imprecision.
    if spin.norm() != 1.0: spin = spin/spin.norm()
    
    # Calculate the measurement operator as a weighted combination
    # of the three Pauli matrices. 
    O_1 = spin[0]*sigma_x + spin[1]*sigma_y + spin[2]*sigma_z
    O_1.simplify()
    
    # the eigenvectors will be the two possible states. The
    # "minus" state will be the first one.
    evals, evecs = find_eigenvectors(O_1)
    plus_state = evecs[1]
    minus_state = evecs[0]
    return(spin, plus_state, minus_state)
Пример #20
0
def getj_0j_1():
    a1, a2 = dynamicsymbols('a1, a2')

    mass_matrix = kane.mass_matrix
    forcing = kane.forcing
    tf_0 = Matrix([[cos(theta1), -sin(theta1), -leg_length*sin(theta1)], [sin(theta1), cos(theta1), leg_length*cos(theta1)], [0,0,1]])

    tf_1 = Matrix([[cos(theta2), -sin(theta2), -body_length*sin(theta2)], [sin(theta2), cos(theta2), body_length*cos(theta2)], [0,0,1]])
        
    tf_01 = simplify(tf_0*tf_1)
    
    com_0 = Matrix([-leg_length*sin(theta1), leg_length*cos(theta1), 1])
    
    com_1 = Matrix([-body_length*sin(theta2), body_length*cos(theta2), 1])
    
    com_01 = simplify(tf_0*com_1)
    
    com = Matrix(com_0 + com_01)
    
    j_0 = com_0.jacobian([theta1, theta2])
    j_1 = simplify(com_01.jacobian([theta1, theta2]))
    j = com.jacobian([theta1, theta2])
    
    q = Matrix(coordinates)
    qdot = Matrix(speeds)
    qddot = Matrix([a1, a2])
    return [j_0, j_1]
Пример #21
0
def main():
    u, v, R = symbols('u v R', real=True)
    xi, eta = symbols(r'\xi \eta', cls=Function)

    numer = 4*R**2
    denom = u**2 + v**2 + numer

    # inverse of a stereographic projection from the south pole
    # onto the XY plane:
    pinv = Matrix([numer * u / denom,
                   numer * v / denom,
                   -(2 * R * (u**2 + v**2)) / denom]) # OK
    if False:
        # textbook style
        Dpinv = simplify(pinv.jacobian([u, v]))
        print_latex(Dpinv, mat_str='pmatrix', mat_delim=None) # OK?

        tDpinvDpinv = factor(Dpinv.transpose() @ Dpinv)
        print_latex(tDpinvDpinv, mat_str='pmatrix', mat_delim=None) # OK

        tDpinvDpinv = tDpinvDpinv.subs([(u, xi(t)), (v, eta(t))])
        dcdt = Matrix([xi(t).diff(), eta(t).diff()])
        print_latex(simplify(
            sqrt((dcdt.transpose() @ tDpinvDpinv).dot(dcdt))))
    else:
        # directly 
        dpinvc = pinv.subs([(u, xi(t)), (v, eta(t))]).diff(t, 1)
        print_latex(sqrt(factor(dpinvc.dot(dpinvc))))
Пример #22
0
def automaton_growth_func( automaton, initial_state, variable='z' ):
    """Returns Z-transform of the growth function of this automaton
    Uses SYmpy for calculations"""
    from sympy import Symbol, Matrix, eye, cancel
    z = variable if isinstance(variable,Symbol) else Symbol(variable)
    
    n  = automaton.num_states()
    a_,b_,c_ = growth_matrices(automaton, initial_state)
    #a.print_nonzero()
    c = Matrix( 1, n, c_)
    b = Matrix( n, 1, b_)
    a = Matrix( a_)
    del a_, b_, c_
    
    #(z*eye(n)-a).inv().print_nonzero()

    if False: #naiive implementation
        q = z*eye(n)-a
        f = c * q.LUsolve(b) * z
        assert f.shape == (1,1)
        return f[0,0]
    
    if True:
        #use trick...
        den = a.berkowitz_charpoly(z)
        #print("#### charpoly:", den)
        num = (a - b*c).berkowitz_charpoly(z) - den
        #print("#### numerator:", num)
        return num/den
def MatrizInversa(a, b,lista):
    
    ma = json.loads(a)
    
    mb = json.loads(b)
    
    A = Matrix(ma)
    
    B = Matrix(mb)
    
    try:
    
        A_inv = A.inv()
    
    except ValueError:
    
        lista.append('\n')
    
        lista.append( 'Matriz es singular, no se puede resolver!')
    
        lista.append('\n') 
    
    else:
    
        ans = A_inv*B
    
        MA = A.tolist()
    
        MB = B.tolist()
Пример #24
0
def compute_lambda(robo, symo, j, antRj, antPj, lam):
    """Internal function. Computes the inertia parameters
    transformation matrix

    Notes
    =====
    lam is the output paramete
    """
    lamJJ_list = []
    lamJMS_list = []
    for e1 in xrange(3):
        for e2 in xrange(e1, 3):
            u = vec_mut_J(antRj[j][:, e1], antRj[j][:, e2])
            if e1 != e2:
                u += vec_mut_J(antRj[j][:, e2], antRj[j][:, e1])
            lamJJ_list.append(u.T)
    for e1 in xrange(3):
        v = vec_mut_MS(antRj[j][:, e1], antPj[j])
        lamJMS_list.append(v.T)
    lamJJ = Matrix(lamJJ_list).T  # , 'LamJ', j)
    lamJMS = symo.mat_replace(Matrix(lamJMS_list).T, 'LamMS', j)
    lamJM = symo.mat_replace(vec_mut_M(antPj[j]), 'LamM', j)
    lamJ = lamJJ.row_join(lamJMS).row_join(lamJM)
    lamMS = sympy.zeros(3, 6).row_join(antRj[j]).row_join(antPj[j])
    lamM = sympy.zeros(1, 10)
    lamM[9] = 1
    lam[j] = Matrix([lamJ, lamMS, lamM])
Пример #25
0
    def etape(self, frontier, i_graph):
        """ Calculates the most probable seed within the infected nodes"""

        # Taking the actual submatrix, not the laplacian matrix. The change
        # lies in the total number of connections (The diagonal terms) for the
        # infected nodes connected to uninfected ones in the initial graph
        i_laplacian_matrix = nx.laplacian_matrix(i_graph)
        for i in range(0, len(i_graph.nodes())):
            if frontier.has_node(i_graph.nodes()[i]):
                i_laplacian_matrix[i, i] +=\
                                frontier.node[i_graph.nodes()[i]]['clear']

        # SymPy
        Lm = Matrix(i_laplacian_matrix.todense())
        i = self.Sym2NumArray(Matrix(Lm.eigenvects()[0][2][0])).argmax()

        # NumPy
        # val, vect = linalg.eigh(i_laplacian_matrix.todense())
        #i = vect[0].argmax()

        # SciPY
        # val, vect = eigs(i_laplacian_matrix.rint())
        # i = vect[:, 0].argmax()

        seed = (i_graph.nodes()[i])

        return seed
Пример #26
0
def printnp(m):
    """
    Prints a sympy matrix in the same format as a numpy array
    """
    m = Matrix(m)
    if m.shape[1] == 1:
        m = m.reshape(1, m.shape[0])
    elem_len = max(num_chars(d) for d in m.vec())
    if m.shape[0] > 1:
        outstr = '[['
    else:
        outstr = '['
    for i in xrange(m.shape[0]):
        if i:
            outstr += ' ['
        char_count = 0
        for j, elem in enumerate(m[i, :]):
            char_count += elem_len
            if char_count > 77:
                outstr += '\n '
                char_count = elem_len + 2
            # Add spaces
            outstr += ' ' * (elem_len - num_chars(elem) +
                             int(j != 0)) + str(elem)

        if i < m.shape[0] - 1:
            outstr += ']\n'
    if m.shape[0] > 1:
        outstr += ']]'
    else:
        outstr += ']'
    print outstr
Пример #27
0
def jacobian_dHdV(nK, y, cap, inds):
	# Set up equations of power flow (power at line from nodal voltage) as symbolic equations and
	# calculate the corresponding Jacobian matrix.
	from sympy import symbols, Matrix

	g = Matrix(np.real(y))
	b = Matrix(np.imag(y))

	e_J = symbols("e1:%d"%(nK+1))
	f_J = symbols("f1:%d"%(nK+1))
	hSluV = []

	if "Pl" in inds:
		nPl = np.asarray(inds["Pl"]).astype(int)
		for k in range(nPl.shape[0]):
			i,j = nPl[k,:]
			hSluV.append((e_J[i]**2+f_J[i]**2)*g[i,j]-(e_J[i]*e_J[j]+f_J[i]*f_J[j])*g[i,j]+(e_J[i]*f_J[j]-e_J[j]*f_J[i])*b[i,j])
	if "Ql" in inds:
		nQl = np.asarray(inds["Ql"]).astype(int)
		for k in range(nQl.shape[0]):
			i,j = nQl[k,:]
			hSluV.append(-(e_J[i]**2+f_J[i]**2)*b[i,j]+(e_J[i]*e_J[j]+f_J[i]*f_J[j])*b[i,j]+(e_J[i]*f_J[j]-e_J[j]*f_J[i])*g[i,j]-(e_J[i]**2+f_J[i]**2)*cap[i,j]/2)
	if "Vm" in inds:
		nVk = np.asarray(inds["Vm"]).astype(int)
		for k in range(nVk.shape[0]):
			i = nVk[k]
			hSluV.append((e_J[i]**2+f_J[i]**2)**0.5)
	if len(hSluV)>0:
		hSluV = Matrix(hSluV)
		ef_J = e_J[1:] + f_J[1:]
		JMatrix_dHdV = hSluV.jacobian(ef_J)
		return JMatrix_dHdV, hSluV
	else:
		return None, None
Пример #28
0
    def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
        """Initialize the coordinate and speed vectors."""

        none_handler = lambda x: Matrix(x) if x else Matrix()

        # Initialize generalized coordinates
        q_dep = none_handler(q_dep)
        if not iterable(q_ind):
            raise TypeError('Generalized coordinates must be an iterable.')
        if not iterable(q_dep):
            raise TypeError('Dependent coordinates must be an iterable.')
        q_ind = Matrix(q_ind)
        self._qdep = q_dep
        self._q = Matrix([q_ind, q_dep])
        self._qdot = self._q.diff(dynamicsymbols._t)

        # Initialize generalized speeds
        u_dep = none_handler(u_dep)
        if not iterable(u_ind):
            raise TypeError('Generalized speeds must be an iterable.')
        if not iterable(u_dep):
            raise TypeError('Dependent speeds must be an iterable.')
        u_ind = Matrix(u_ind)
        self._udep = u_dep
        self._u = Matrix([u_ind, u_dep])
        self._udot = self._u.diff(dynamicsymbols._t)
        self._uaux = none_handler(u_aux)
Пример #29
0
    def test_matrix_tensor_product():
        l1 = zeros(4)
        for i in range(16):
            l1[i] = 2**i
        l2 = zeros(4)
        for i in range(16):
            l2[i] = i
        l3 = zeros(2)
        for i in range(4):
            l3[i] = i
        vec = Matrix([1,2,3])

        #test for Matrix known 4x4 matricies
        numpyl1 = np.matrix(l1.tolist())
        numpyl2 = np.matrix(l2.tolist())
        numpy_product = np.kron(numpyl1,numpyl2)
        args = [l1, l2]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()
        numpy_product = np.kron(numpyl2,numpyl1)
        args = [l2, l1]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()

        #test for other known matrix of different dimensions
        numpyl2 = np.matrix(l3.tolist())
        numpy_product = np.kron(numpyl1,numpyl2)
        args = [l1, l3]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()
        numpy_product = np.kron(numpyl2,numpyl1)
        args = [l3, l1]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()

        #test for non square matrix
        numpyl2 = np.matrix(vec.tolist())
        numpy_product = np.kron(numpyl1,numpyl2)
        args = [l1, vec]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()
        numpy_product = np.kron(numpyl2,numpyl1)
        args = [vec, l1]
        sympy_product = matrix_tensor_product(*args)
        assert numpy_product.tolist() == sympy_product.tolist()

        #test for random matrix with random values that are floats
        random_matrix1 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
        random_matrix2 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
        numpy_product = np.kron(random_matrix1,random_matrix2)
        args = [Matrix(random_matrix1.tolist()),Matrix(random_matrix2.tolist())]
        sympy_product = matrix_tensor_product(*args)
        assert not (sympy_product - Matrix(numpy_product.tolist())).tolist() > \
        (ones((sympy_product.rows,sympy_product.cols))*epsilon).tolist()

        #test for three matrix kronecker
        sympy_product = matrix_tensor_product(l1,vec,l2)

        numpy_product = np.kron(l1,np.kron(vec,l2))
        assert numpy_product.tolist() == sympy_product.tolist()
Пример #30
0
def calculate_dependencies(equations, task_variables, variables_values):
    
    replacements = replace.get_replacements(task_variables)[1]
    transformed_equations = replace.get_transformed_expressions(equations, task_variables)
    
    #prepare data for Numpy manipulations - change variables names to the names of special format - 'x%sy', also 
    #convert some strings to Symbols so they could be processed by Sympy and Numpy libs
        
    symbolized_replaced_task_variables = sorted(symbols(replacements.keys()))        
    replaced_variables_values = {}
    for k,v in replacements.iteritems():
        replaced_variables_values[Symbol(k)] = variables_values[v]
        
    #prepare the system of equations for solving - find Jacobian, in Jacobian matrix that was found replace variables by their values
    
    F = Matrix(transformed_equations)
    J = F.jacobian(symbolized_replaced_task_variables).evalf(subs=replaced_variables_values)
    X = np.transpose(np.array(J))
    
    #solve the system
    n,m = X.shape
    if (n != m - 1): 
        # if the system of equations is overdetermined (the number of equations is bigger than number of variables 
        #then we have to make regular, square systems of equations out of it (we do that by grabbing only some of the equations)
        square_systems = suggest_square_systems_list(X, n, m)
        for sq in square_systems:
            try:
                solve_system(sq, equations)
            except Exception as e:
                print e.args
                print sq
                
                
    else:
        solve_system(X, equations)  
Пример #31
0
def jacobian(functions, variables, constants):
    symbols = dict((v, Symbol("x[%d]" % i)) for (i, v) in enumerate(variables))
    f = Matrix([parse_func(expr, symbols, constants) for expr in functions])
    x = Matrix(list(symbols.values()))
    return lambdify("x", f), lambdify("x", f.jacobian(x))
Пример #32
0
# 
# $$Q = G\cdot G^T \cdot \sigma_a^2$$
# 
# with $G = \begin{bmatrix}0.5dt^2 & 0.5dt^2 & dt & dt & 1.0 & 1.0\end{bmatrix}^T$ and $\sigma_a$ as the acceleration process noise.

# <headingcell level=4>

# Symbolic Calculation

# <codecell>

from sympy import Symbol, Matrix
from sympy.interactive import printing
printing.init_printing()
dts = Symbol('\Delta t')
Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[dts],[dts],[1.0],[1.0]])
Qs*Qs.T

# <codecell>

sa = 0.001
G = np.matrix([[1/2.0*dt**2],
               [1/2.0*dt**2],
               [dt],
               [dt],
               [1.0],
               [1.0]])
Q = G*G.T*sa**2

print(Q, Q.shape)
        # Primary State Rates
        x_dot = v * cos(theta)
        y_dot = v * sin(theta)
        theta_dot = u_max * sin(w)

        writeList = [x_dot, y_dot, theta_dot]

        # Covariance Calculations

        p11, p12, \
        p22, \
            = symbols('p11 p12 \
                       p22'                           )

        P = Matrix([[p11, p12], [p12, p22]])

        F = Matrix([[diff(x_dot, x), diff(x_dot, y)],
                    [diff(y_dot, x), diff(y_dot, y)]])

        G = Matrix([[cos(theta)], [sin(theta)]])

        h = sqrt((x - xb)**2 + (y - yb)**2)

        H = Matrix([[diff(h, x), diff(h, y)]])

        Q = Dt * diag(sigv**2)

        R = Dt * diag(sigr**2)

        P_dot = (F * P + P * F.T - P * H.T * (R**-1) * H * P + G * Q * G.T)
import sympy
from sympy.abc import alpha, x, y, v, w, R, theta
from sympy import symbols, Matrix
from numpy import array
import numpy as np

# px, py = symbols('p_x, p_y')
# # h of landmark: (x,y,yaw)-->(range,bearing)
# z = Matrix([[sympy.sqrt((px-x)**2 + (py-y)**2)],
#             [sympy.atan2(py-y, px-x) - theta]])
# # H jacobian
# print(z.jacobian(Matrix([x, y, theta])))

# 定义符号
a, x, y, v, w, theta, time = symbols('a, x, y, v, w, theta, t')
d = v * time  # km
beta = (d / w) * sympy.tan(a)  # rad
r = w / sympy.tan(a)  # km

r_lo = r / 111 / sympy.cos(y / 180 * np.pi)
r_la = r / 111

# 定义predict转移函数h形式,单位
fxu = Matrix([[x - r_lo * sympy.sin(theta) + r_lo * sympy.sin(theta + beta)],
              [y + r_la * sympy.cos(theta) - r_la * sympy.cos(theta + beta)],
              [theta + beta]])
print(fxu)
              "r"]  #l=v_l, r=v_r

# Differential drive approximation
# Velocity calculations
v_k = 0.5 * wheel_rad * (l + r)
x_dot_k = v_k * cos(phi)
y_dot_k = v_k * sin(phi)
phi_dot_k = (wheel_rad / wheelbase_len) * (l - r)

# Position calculations
x_k = x + x_dot * t
y_k = y + y_dot * t
phi_k = phi + phi_dot * t

# Setup vector of the functions
f = Matrix([x_k, x_dot_k, y_k, y_dot_k, phi_k, phi_dot_k, l, r])

# Setup vector of the variables
X = Matrix([x, x_dot, y, y_dot, phi, phi_dot, l, r])

# IGVC Fk matrix
model = f
jac_model = f.jacobian(X)

# # Input dictionary setup
# input_dict = {self.state_vars[i]: last_xk[i] for i in range(0, len(self.state_vars))}
# input_dict['t'] = dt
# ctrl_dict = {self.ctrl_vars[i]: ctrl[i] for i in range(0, len(self.ctrl_vars))}

# F = self.jac_model.subs(input_dict).subs(ctrl_dict)
# new_state = self.model.subs(input_dict).subs(ctrl_dict)
Пример #36
0
        alpha_dot = u_max * sin(u)

        writeList = [h_dot, theta_dot, v_dot, gam_dot, alpha_dot]

        # Covariance Calculations

        p11, p12, p13, p14, \
            p22, p23, p24,  \
            p33, p34,       \
            p44                    \
            = symbols('p11 p12 p13 p14 \
                       p22 p23 p24 \
                       p33 p34 \
                       p44'                           )

        P = Matrix([[p11, p12, p13, p14], [p12, p22, p23, p24],
                    [p13, p23, p33, p34], [p14, p24, p34, p44]])

        F = Matrix([[
            diff(h_dot, h),
            diff(theta_dot, h),
            diff(v_dot, h),
            diff(gam_dot, h)
        ],
                    [
                        diff(h_dot, theta),
                        diff(theta_dot, theta),
                        diff(v_dot, theta),
                        diff(gam_dot, theta)
                    ],
                    [
                        diff(h_dot, v),
Пример #37
0
B = A.rotate('B', 2, a)
# C rotates about an axis along the tangent to the center of the circular cross
# section by an angle b
C = B.rotate('C', 3, b)

# Locate an arbitrary point on the torus
p = Vector(r1*B[1] + c*C[2])

# Express it in the A reference frame
x = dot(p, A[1])
y = dot(p, A[2])
z = dot(p, A[3])

# Determinant of the Jacobian of the mapping from a, b, c to x, y, z
X = [a, b, c]
F = Matrix([x, y, z])
J = F.jacobian(X)
dv = (J.det(method='berkowitz')).expand().subs({cos(a)**2:1-sin(a)**2,
        cos(b)**2:1-sin(b)**2}).expand()

print 'dx*dy*dz =(', dv, ')*da*db*dc'

# Integrands of definition of inertia scalars
i11 = rho * dot(cross(p, A[1]), cross(p, A[1])) * dv
i22 = rho * dot(cross(p, A[2]), cross(p, A[2])) * dv
i33 = rho * dot(cross(p, A[3]), cross(p, A[3])) * dv
i12 = rho * dot(cross(p, A[1]), cross(p, A[2])) * dv
i13 = rho * dot(cross(p, A[1]), cross(p, A[3])) * dv
i23 = rho * dot(cross(p, A[2]), cross(p, A[3])) * dv

I11 = Int(Int(Int(i11, (a, 0, 2*pi)), (b, 0, 2*pi)), (c, 0, r2))
Пример #38
0
        Pos_scaled[0, i] = scale(Pos_origin[i, 0], True)
        Pos_scaled[1, i] = scale(Pos_origin[i, 1], False)


update_points()

print("init: ready")

# sympy
q = MatrixSymbol('q', 1, low_dim)  # updated position
p_i = MatrixSymbol('p_i', 1, high_dim)  # selected point (high_dim) (P[thisID])
E = MatrixSymbol('E', high_dim, high_dim)  # = (e1 e2 e0 0 ...) : 縦ベクトルの列
A_inputEs = MatrixSymbol('A_inputEs', low_dim + 1, low_dim)  # ei' の係数行列
A_inputRs = MatrixSymbol('A_inputRs', low_dim, low_dim - 1)  # Ri の係数行列
var = (q, p_i, E, A_inputEs, A_inputRs)  # 変数のリスト
A = Matrix(np.zeros(high_dim * high_dim).reshape(high_dim, high_dim))
A[0:low_dim + 1, 0:low_dim] = Matrix(A_inputEs)
A[0:low_dim, low_dim:low_dim + 1] = Matrix(A_inputRs)

_E = E * A  # = (e1' e2' R1 0 ...) : 更新後の射影ベクトルは縦ベクトルで格納

# 制約解消における前計算
constraints1 = (refine(_E.T * _E, Q.orthogonal(E))).doit()
constraints2 = (refine(E.T * _E, Q.orthogonal(E))).doit()
constraints3 = p_i * _E

# ei' * ej' = δij (クロネッカーのデルタ)
bases_e = Matrix(constraints1[0:low_dim, 0:low_dim] -
                 Matrix(Identity(low_dim)))
_bases_e = bases_e[0, 0]**2 + bases_e[0, 1]**2 + bases_e[1, 1]**2
Пример #39
0
def inertia_matrix(dyadic, rf):
    """Return the inertia matrix of a given dyadic for a specified
    reference frame.
    """
    return Matrix([[dot(dot(dyadic, i), j) for j in rf] for i in rf])
Пример #40
0
def test_simplify_expr():
    x, y, z, k, n, m, w, s, A = symbols('x,y,z,k,n,m,w,s,A')
    f = Function('f')

    assert all(simplify(tmp) == tmp for tmp in [I, E, oo, x, -x, -oo, -E, -I])

    e = 1 / x + 1 / y
    assert e != (x + y) / (x * y)
    assert simplify(e) == (x + y) / (x * y)

    e = A**2 * s**4 / (4 * pi * k * m**3)
    assert simplify(e) == e

    e = (4 + 4 * x - 2 * (2 + 2 * x)) / (2 + 2 * x)
    assert simplify(e) == 0

    e = (-4 * x * y**2 - 2 * y**3 - 2 * x**2 * y) / (x + y)**2
    assert simplify(e) == -2 * y

    e = -x - y - (x + y)**(-1) * y**2 + (x + y)**(-1) * x**2
    assert simplify(e) == -2 * y

    e = (x + x * y) / x
    assert simplify(e) == 1 + y

    e = (f(x) + y * f(x)) / f(x)
    assert simplify(e) == 1 + y

    e = (2 * (1 / n - cos(n * pi) / n)) / pi
    assert simplify(e) == (-cos(pi * n) + 1) / (pi * n) * 2

    e = integrate(1 / (x**3 + 1), x).diff(x)
    assert simplify(e) == 1 / (x**3 + 1)

    e = integrate(x / (x**2 + 3 * x + 1), x).diff(x)
    assert simplify(e) == x / (x**2 + 3 * x + 1)

    f = Symbol('f')
    A = Matrix([[2 * k - m * w**2, -k], [-k, k - m * w**2]]).inv()
    assert simplify((A * Matrix([0, f]))[1] - (-f * (2 * k - m * w**2) /
                                               (k**2 - (k - m * w**2) *
                                                (2 * k - m * w**2)))) == 0

    f = -x + y / (z + t) + z * x / (z + t) + z * a / (z + t) + t * x / (z + t)
    assert simplify(f) == (y + a * z) / (z + t)

    # issue 10347
    expr = -x * (y**2 - 1) * (
        2 * y**2 * (x**2 - 1) / (a * (x**2 - y**2)**2) + (x**2 - 1) /
        (a * (x**2 - y**2))) / (a * (x**2 - y**2)) + x * (
            -2 * x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
            (a * (x**2 - y**2)**2) -
            x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
            (a * (x**2 - 1) * (x**2 - y**2)) +
            (x**2 * sqrt((-x**2 + 1) * (y**2 - 1)) *
             sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) / (x**2 - 1) + sqrt(
                 (-x**2 + 1) * (y**2 - 1)) *
             (x * (-x * y**2 + x) / sqrt(-x**2 * y**2 + x**2 + y**2 - 1) +
              sqrt(-x**2 * y**2 + x**2 + y**2 - 1)) * sin(z)) / (a * sqrt(
                  (-x**2 + 1) * (y**2 - 1)) * (x**2 - y**2))
        ) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) / (
            a * (x**2 - y**2)) + x * (
                -2 * x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
                (a * (x**2 - y**2)**2) -
                x**2 * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
                (a * (x**2 - 1) * (x**2 - y**2)) +
                (x**2 * sqrt((-x**2 + 1) *
                             (y**2 - 1)) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1)
                 * cos(z) / (x**2 - 1) + x * sqrt(
                     (-x**2 + 1) * (y**2 - 1)) * (-x * y**2 + x) * cos(z) /
                 sqrt(-x**2 * y**2 + x**2 + y**2 - 1) + sqrt(
                     (-x**2 + 1) *
                     (y**2 - 1)) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) *
                 cos(z)) / (a * sqrt((-x**2 + 1) * (y**2 - 1)) * (x**2 - y**2))
            ) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) / (
                a *
                (x**2 - y**2)) - y * sqrt((-x**2 + 1) * (y**2 - 1)) * (
                    -x * y * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
                    (a * (x**2 - y**2) *
                     (y**2 - 1)) +
                    2 * x * y * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
                    (a * (x**2 - y**2)**2) +
                    (x * y * sqrt((-x**2 + 1) * (y**2 - 1)) *
                     sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(z) /
                     (y**2 - 1) + x * sqrt(
                         (-x**2 + 1) * (y**2 - 1)) * (-x**2 * y + y) * sin(z) /
                     sqrt(-x**2 * y**2 + x**2 + y**2 - 1)) / (a * sqrt(
                         (-x**2 + 1) * (y**2 - 1)) * (x**2 - y**2))
                ) * sin(z) / (a * (x**2 - y**2)) + y * (x**2 - 1) * (
                    -2 * x * y *
                    (x**2 - 1) /
                    (a * (x**2 - y**2)**2) + 2 * x * y / (a * (x**2 - y**2))
                ) / (a *
                     (x**2 - y**2)) + y * (x**2 - 1) * (y**2 - 1) * (
                         -x * y * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) *
                         cos(z) / (a * (x**2 - y**2) *
                                   (y**2 - 1)) + 2 * x * y *
                         sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
                         (a *
                          (x**2 - y**2)**2) +
                         (x * y * sqrt((-x**2 + 1) * (y**2 - 1)) *
                          sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * cos(z) /
                          (y**2 - 1) + x * sqrt(
                              (-x**2 + 1) * (y**2 - 1)) * (-x**2 * y + y) *
                          cos(z) / sqrt(-x**2 * y**2 + x**2 + y**2 - 1)) /
                         (a * sqrt((-x**2 + 1) * (y**2 - 1)) *
                          (x**2 - y**2))) * cos(z) / (a * sqrt(
                              (-x**2 + 1) *
                              (y**2 - 1)) * (x**2 - y**2)) - x * sqrt(
                                  (-x**2 + 1) * (y**2 - 1)
                              ) * sqrt(-x**2 * y**2 + x**2 + y**2 - 1) * sin(
                                  z)**2 / (a**2 * (x**2 - 1) * (x**2 - y**2) *
                                           (y**2 - 1)) - x * sqrt(
                                               (-x**2 + 1) *
                                               (y**2 - 1)) * sqrt(
                                                   -x**2 * y**2 + x**2 + y**2 -
                                                   1) * cos(z)**2 / (
                                                       a**2 * (x**2 - 1) *
                                                       (x**2 - y**2) *
                                                       (y**2 - 1))
    assert simplify(expr) == 2 * x / (a**2 * (x**2 - y**2))

    #issue 17631
    assert simplify('((-1/2)*Boole(True)*Boole(False)-1)*Boole(True)') == \
            Mul(sympify('(2 + Boole(True)*Boole(False))'), sympify('-Boole(True)/2'))

    A, B = symbols('A,B', commutative=False)

    assert simplify(A * B - B * A) == A * B - B * A
    assert simplify(A / (1 + y / x)) == x * A / (x + y)
    assert simplify(A * (1 / x + 1 / y)) == A / x + A / y  #(x + y)*A/(x*y)

    assert simplify(log(2) + log(3)) == log(6)
    assert simplify(log(2 * x) - log(2)) == log(x)

    assert simplify(hyper([], [], x)) == exp(x)
Пример #41
0
def test_issue_10567():
    a, b, c, t = symbols('a b c t')
    vt = Matrix([a*t, b, c])
    assert integrate(vt, t) == Integral(vt, t).doit()
    assert integrate(vt, t) == Matrix([[a*t**2/2], [b*t], [c*t]])
Пример #42
0
def test_aux_dep():
    # This test is about rolling disc dynamics, comparing the results found
    # with KanesMethod to those found when deriving the equations "manually"
    # with SymPy.
    # The terms Fr, Fr*, and Fr*_steady are all compared between the two
    # methods. Here, Fr*_steady refers to the generalized inertia forces for an
    # equilibrium configuration.
    # Note: comparing to the test of test_rolling_disc() in test_kane.py, this
    # test also tests auxiliary speeds and configuration and motion constraints
    #, seen in  the generalized dependent coordinates q[3], and depend speeds
    # u[3], u[4] and u[5].


    # First, mannual derivation of Fr, Fr_star, Fr_star_steady.

    # Symbols for time and constant parameters.
    # Symbols for contact forces: Fx, Fy, Fz.
    t, r, m, g, I, J = symbols('t r m g I J')
    Fx, Fy, Fz = symbols('Fx Fy Fz')

    # Configuration variables and their time derivatives:
    # q[0] -- yaw
    # q[1] -- lean
    # q[2] -- spin
    # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
    #         A.z direction
    # Generalized speeds and their time derivatives:
    # u[0] -- disc angular velocity component, disc fixed x direction
    # u[1] -- disc angular velocity component, disc fixed y direction
    # u[2] -- disc angular velocity component, disc fixed z direction
    # u[3] -- disc velocity component, A.x direction
    # u[4] -- disc velocity component, A.y direction
    # u[5] -- disc velocity component, A.z direction
    # Auxiliary generalized speeds:
    # ua[0] -- contact point auxiliary generalized speed, A.x direction
    # ua[1] -- contact point auxiliary generalized speed, A.y direction
    # ua[2] -- contact point auxiliary generalized speed, A.z direction
    q = dynamicsymbols('q:4')
    qd = [qi.diff(t) for qi in q]
    u = dynamicsymbols('u:6')
    ud = [ui.diff(t) for ui in u]
    ud_zero = dict(zip(ud, [0.]*len(ud)))
    ua = dynamicsymbols('ua:3')
    ua_zero = dict(zip(ua, [0.]*len(ua)))

    # Reference frames:
    # Yaw intermediate frame: A.
    # Lean intermediate frame: B.
    # Disc fixed frame: C.
    N = ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', [q[0], N.z])
    B = A.orientnew('B', 'Axis', [q[1], A.x])
    C = B.orientnew('C', 'Axis', [q[2], B.y])

    # Angular velocity and angular acceleration of disc fixed frame
    # u[0], u[1] and u[2] are generalized independent speeds.
    C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
    C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
                   + cross(B.ang_vel_in(N), C.ang_vel_in(N)))

    # Velocity and acceleration of points:
    # Disc-ground contact point: P.
    # Center of disc: O, defined from point P with depend coordinate: q[3]
    # u[3], u[4] and u[5] are generalized dependent speeds.
    P = Point('P')
    P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
    O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
    O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
    O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))

    # Kinematic differential equations:
    # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
    #                 directions of B, for qd0, qd1 and qd2.
    #                 the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
    # Then, solve for dq/dt's in terms of u's: qd_kd.
    w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
    v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
    kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
                      [dot(v_o_n_qd - O.vel(N), A.z)])
    qd_kd = solve(kindiffs, qd)

    # Values of generalized speeds during a steady turn for later substitution
    # into the Fr_star_steady.
    steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
    steady_conditions.update({qd[1] : 0, qd[3] : 0})

    # Partial angular velocities and velocities.
    partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
    partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
    partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]

    # Configuration constraint: f_c, the projection of radius r in A.z direction
    #                                is q[3].
    # Velocity constraints: f_v, for u3, u4 and u5.
    # Acceleration constraints: f_a.
    f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
    f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
        O.pos_from(P))), ai).expand() for ai in A])
    v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
    a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
    f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])

    # Solve for constraint equations in the form of
    #                           u_dependent = A_rs * [u_i; u_aux].
    # First, obtain constraint coefficient matrix:  M_v * [u; ua] = 0;
    # Second, taking u[0], u[1], u[2] as independent,
    #         taking u[3], u[4], u[5] as dependent,
    #         rearranging the matrix of M_v to be A_rs for u_dependent.
    # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
    M_v = zeros(3, 9)
    for i in range(3):
        for j, ui in enumerate(u + ua):
            M_v[i, j] = f_v[i].diff(ui)

    M_v_i = M_v[:, :3]
    M_v_d = M_v[:, 3:6]
    M_v_aux = M_v[:, 6:]
    M_v_i_aux = M_v_i.row_join(M_v_aux)
    A_rs = - M_v_d.inv() * M_v_i_aux

    u_dep = A_rs[:, :3] * Matrix(u[:3])
    u_dep_dict = dict(zip(u[3:], u_dep))

    # Active forces: F_O acting on point O; F_P acting on point P.
    # Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
    F_O = m*g*A.z
    F_P = Fx * A.x + Fy * A.y + Fz * A.z
    Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
            zip(partial_v_O, partial_v_P)])

    # Inertia force: R_star_O.
    # Inertia of disc: I_C_O, where J is a inertia component about principal axis.
    # Inertia torque: T_star_C.
    # Generalized inertia forces (unconstrained): Fr_star_u.
    R_star_O = -m*O.acc(N)
    I_C_O = inertia(B, I, J, I)
    T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
                 + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
    Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
                        zip(partial_v_O, partial_w_C)])

    # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
    # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
    Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
    Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
                + A_rs.T * Fr_star_u[3:6, :]
    Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
            .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()


    # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.

    # Rigid Bodies: disc, with inertia I_C_O.
    iner_tuple = (I_C_O, O)
    disc = RigidBody('disc', O, C, m, iner_tuple)
    bodyList = [disc]

    # Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
    F_o = (O, F_O)
    F_p = (P, F_P)
    forceList = [F_o,  F_p]

    # KanesMethod.
    kane = KanesMethod(
        N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
        q_dependent=q[3:], configuration_constraints = f_c,
        u_dependent=u[3:], velocity_constraints= f_v,
        u_auxiliary=ua
        )

    # fr, frstar, frstar_steady and kdd(kinematic differential equations).
    (fr, frstar)= kane.kanes_equations(forceList, bodyList)
    frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
                    .subs({q[3]: -r*cos(q[1])}).expand()
    kdd = kane.kindiffdict()

    assert Matrix(Fr_c).expand() == fr.expand()
    assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
    assert (simplify(Matrix(Fr_star_steady).expand()) ==
            simplify(frstar_steady.expand()))
Пример #43
0
    def __init__(self,
                 site_species,
                 site_species_energies,
                 bond_energies,
                 intracluster_connections,
                 intercluster_connections,
                 compositional_interactions=None):

        self.site_species = site_species
        self.site_species_energies = site_species_energies
        self.bond_energies = bond_energies
        self.intracluster_connections = intracluster_connections
        self.intercluster_connections = intercluster_connections

        self.n_species_per_site = np.array([len(s) for s in site_species])
        self.site_start_indices = (np.cumsum(self.n_species_per_site) -
                                   self.n_species_per_site[0])
        self.site_index_tuples = np.array(
            [self.site_start_indices, self.n_species_per_site]).T

        site_bounds = [0]
        site_bounds.extend(list(np.cumsum(self.n_species_per_site)))
        self.site_bounds = np.array([site_bounds[:-1], site_bounds[1:]]).T

        self.n_sites = len(self.n_species_per_site)

        self.n_site_species = np.sum(self.n_species_per_site)
        self.n_ind = self.n_site_species - self.n_sites + 1

        if not len(site_species) == len(self.n_species_per_site):
            raise Exception('site_species must be a list of lists, '
                            'with the number of outermost lists equal to '
                            'the number of sites.')

        if not all([
                len(s) == self.n_species_per_site[i]
                for i, s in enumerate(site_species)
        ]):
            raise Exception('site_species must have the correct '
                            'number of species on each site')
        self.site_species = site_species
        self.site_species_flat = [
            species for site in site_species for species in site
        ]
        self.components = sorted(list(set(self.site_species_flat)))
        self.n_components = len(self.components)

        if compositional_interactions is None:
            compositional_interactions = np.zeros(
                (self.n_components, self.n_components))

        # Make correlation matrix between composition and site species
        self.site_species_compositions = np.zeros(
            (len(self.site_species_flat), len(self.components)), dtype='int')
        for i, ss in enumerate(self.site_species_flat):
            self.site_species_compositions[i, self.components.index(ss)] = 1

        clusters = product(*[
            np.identity(n_species, dtype='int')
            for n_species in self.n_species_per_site
        ])
        self.n_clusters = np.prod(self.n_species_per_site)

        self.cluster_occupancies = np.array([np.hstack(cl) for cl in clusters])
        self.cluster_compositions = np.einsum('ij, jk->ik',
                                              self.cluster_occupancies,
                                              self.site_species_compositions)
        self.pivots_flat = list(
            sorted(set([list(c).index(1)
                        for c in self.cluster_occupancies.T])))

        ind_cl_occs = np.array(
            [self.cluster_occupancies[p] for p in self.pivots_flat],
            dtype='int')
        ind_cl_comps = np.array(
            [self.cluster_compositions[p] for p in self.pivots_flat],
            dtype='int')
        self.independent_cluster_occupancies = ind_cl_occs
        self.independent_cluster_compositions = ind_cl_comps

        self.independent_interactions = np.einsum('ij, lk, jk->il',
                                                  ind_cl_comps, ind_cl_comps,
                                                  compositional_interactions)

        null = Matrix(self.independent_cluster_compositions.T).nullspace()
        rxn_matrix = np.array([np.array(v).T[0] for v in null])
        self.isochemical_reactions = rxn_matrix
        self.n_reactions = len(rxn_matrix)
        self._ps_to_p_ind = pinv(self.independent_cluster_occupancies.T)

        A_ind_shape = list(self.n_species_per_site)
        A_ind_shape.append(self.n_ind)
        self.A_ind = lstsq(self.independent_cluster_occupancies.T,
                           self.cluster_occupancies.T,
                           rcond=None)[0].round(decimals=10).T

        self._AA = np.einsum('ik, ij -> ijk', self.A_ind, self.A_ind)

        self.pivots = np.argwhere(np.sum(np.abs(self.A_ind), axis=-1) == 1)

        shp = np.concatenate(
            (self.n_species_per_site, self.n_species_per_site))
        self.cluster_identity_matrix = np.eye(self.n_clusters).reshape(shp)
        self.cluster_ones = np.ones(self.n_species_per_site)

        self.cluster_energies = (
            np.einsum('i, ki', self.site_species_energies,
                      self.cluster_occupancies) +
            np.einsum('ij, ij, ki, kj->k', self.bond_energies,
                      (self.intercluster_connections +
                       self.intracluster_connections),
                      self.cluster_occupancies, self.cluster_occupancies))

        self.intracluster_energies = (
            np.einsum('i, ki', self.site_species_energies,
                      self.cluster_occupancies) +
            np.einsum('ij, ij, ki, kj->k', self.bond_energies,
                      self.intracluster_connections, self.cluster_occupancies,
                      self.cluster_occupancies))

        self.cluster_interactions = np.einsum('ij, ij, kj->ki',
                                              self.bond_energies,
                                              self.intercluster_connections,
                                              self.cluster_occupancies)

        np.random.seed(seed=19)
        std = np.std(self.cluster_energies)
        delta = np.random.rand(len(self.cluster_energies)) * std * 1.e-10
        self._delta_cluster_energies = delta
Пример #44
0
def test_represent():
    assert represent(Jz) == hbar * Matrix([[1, 0], [0, -1]]) / 2
    assert represent(Jz,
                     j=1) == hbar * Matrix([[1, 0, 0], [0, 0, 0], [0, 0, -1]])
Пример #45
0
def test_non_central_inertia():
    # This tests that the calculation of Fr* does not depend the point
    # about which the inertia of a rigid body is defined. This test solves
    # exercises 8.12, 8.17 from Kane 1985.

    # Declare symbols
    q1, q2, q3 = dynamicsymbols('q1:4')
    q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
    u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
    u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
    a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
    Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
    IA22, IA23, IA33 = symbols('IA22 IA23 IA33')

    # Reference Frames
    F = ReferenceFrame('F')
    P = F.orientnew('P', 'axis', [-theta, F.y])
    A = P.orientnew('A', 'axis', [q1, P.x])
    A.set_ang_vel(F, u1*A.x + u3*A.z)
    # define frames for wheels
    B = A.orientnew('B', 'axis', [q2, A.z])
    C = A.orientnew('C', 'axis', [q3, A.z])
    B.set_ang_vel(A, u4 * A.z)
    C.set_ang_vel(A, u5 * A.z)

    # define points D, S*, Q on frame A and their velocities
    pD = Point('D')
    pD.set_vel(A, 0)
    # u3 will not change v_D_F since wheels are still assumed to roll without slip.
    pD.set_vel(F, u2 * A.y)

    pS_star = pD.locatenew('S*', e*A.y)
    pQ = pD.locatenew('Q', f*A.y - R*A.x)
    for p in [pS_star, pQ]:
        p.v2pt_theory(pD, F, A)

    # masscenters of bodies A, B, C
    pA_star = pD.locatenew('A*', a*A.y)
    pB_star = pD.locatenew('B*', b*A.z)
    pC_star = pD.locatenew('C*', -b*A.z)
    for p in [pA_star, pB_star, pC_star]:
        p.v2pt_theory(pD, F, A)

    # points of B, C touching the plane P
    pB_hat = pB_star.locatenew('B^', -R*A.x)
    pC_hat = pC_star.locatenew('C^', -R*A.x)
    pB_hat.v2pt_theory(pB_star, F, B)
    pC_hat.v2pt_theory(pC_star, F, C)

    # the velocities of B^, C^ are zero since B, C are assumed to roll without slip
    kde = [q1d - u1, q2d - u4, q3d - u5]
    vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]

    # inertias of bodies A, B, C
    # IA22, IA23, IA33 are not specified in the problem statement, but are
    # necessary to define an inertia object. Although the values of
    # IA22, IA23, IA33 are not known in terms of the variables given in the
    # problem statement, they do not appear in the general inertia terms.
    inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
    inertia_B = inertia(B, K, K, J)
    inertia_C = inertia(C, K, K, J)

    # define the rigid bodies A, B, C
    rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
    rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
    rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))

    km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde,
                     u_dependent=[u4, u5], velocity_constraints=vc,
                     u_auxiliary=[u3])

    forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
    bodies = [rbA, rbB, rbC]
    fr, fr_star = km.kanes_equations(forces, bodies)
    vc_map = solve(vc, [u4, u5])

    # KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
    fr_star_expected = Matrix([
            -(IA + 2*J*b**2/R**2 + 2*K +
              mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
            -(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
            0])
    assert (trigsimp(fr_star.subs(vc_map).subs(u3, 0)).doit().expand() ==
            fr_star_expected.expand())

    # define inertias of rigid bodies A, B, C about point D
    # I_S/O = I_S/S* + I_S*/O
    bodies2 = []
    for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
        I = I_star + inertia_of_point_mass(rb.mass,
                                           rb.masscenter.pos_from(pD),
                                           rb.frame)
        bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
                                 (I, pD)))
    fr2, fr_star2 = km.kanes_equations(forces, bodies2)
    assert (trigsimp(fr_star2.subs(vc_map).subs(u3, 0)).doit().expand() ==
            fr_star_expected.expand())
Пример #46
0
def prefixed_matrix(prefix):
    """
    Returns a matrix where each entry is of the for prefix___name.
    """

    return Matrix(4, 4, [symbols(prefix + "___" + i) for i in matrix_names])
Пример #47
0
def perspective(g):
    """
    Returns the Ren'Py projection matrix. This is a view into a 3d space
    where (0, 0) is the top left corner (`w`/2, `h`/2) is the center, and
    (`w`,`h`) is the bottom right, when the z coordinate is 0.

    `w`, `h`
        The width and height of the input plane, in pixels.

    `n`
        The distance of the near plane from the camera.

    `p`
        The distance of the 1:1 plane from the camera. This is where 1 pixel
        is one coordinate unit.

    `f`
        The distance of the far plane from the camera.
    """

    w, h, n, p, f = g.parameters('w h n p f')

    offset = Matrix(4, 4, [
        1.0,
        0.0,
        0.0,
        -w / 2.0,
        0.0,
        1.0,
        0.0,
        -h / 2.0,
        0.0,
        0.0,
        1.0,
        -p,
        0.0,
        0.0,
        0.0,
        1.0,
    ])

    projection = Matrix(4, 4, [
        2.0 * p / w,
        0.0,
        0.0,
        0.0,
        0.0,
        2.0 * p / h,
        0.0,
        0.0,
        0.0,
        0.0,
        -(f + n) / (f - n),
        -2 * f * n / (f - n),
        0.0,
        0.0,
        -1.0,
        0.0,
    ])

    reverse_offset = Matrix(4, 4, [
        w / 2.0,
        0.0,
        0.0,
        w / 2.0,
        0.0,
        h / 2.0,
        0.0,
        h / 2.0,
        0.0,
        0.0,
        1.0,
        0.0,
        0.0,
        0.0,
        0.0,
        1.0,
    ])

    g.matrix(reverse_offset * projection * offset)
Пример #48
0
def test_sub_qdot():
    # This test solves exercises 8.12, 8.17 from Kane 1985 and defines
    # some velocities in terms of q, qdot.

    ## --- Declare symbols ---
    q1, q2, q3 = dynamicsymbols('q1:4')
    q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
    u1, u2, u3 = dynamicsymbols('u1:4')
    u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
    a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
    IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
    Q1, Q2, Q3 = symbols('Q1 Q2 Q3')

    # --- Reference Frames ---
    F = ReferenceFrame('F')
    P = F.orientnew('P', 'axis', [-theta, F.y])
    A = P.orientnew('A', 'axis', [q1, P.x])
    A.set_ang_vel(F, u1*A.x + u3*A.z)
    # define frames for wheels
    B = A.orientnew('B', 'axis', [q2, A.z])
    C = A.orientnew('C', 'axis', [q3, A.z])

    ## --- define points D, S*, Q on frame A and their velocities ---
    pD = Point('D')
    pD.set_vel(A, 0)
    # u3 will not change v_D_F since wheels are still assumed to roll w/o slip
    pD.set_vel(F, u2 * A.y)

    pS_star = pD.locatenew('S*', e*A.y)
    pQ = pD.locatenew('Q', f*A.y - R*A.x)
    # masscenters of bodies A, B, C
    pA_star = pD.locatenew('A*', a*A.y)
    pB_star = pD.locatenew('B*', b*A.z)
    pC_star = pD.locatenew('C*', -b*A.z)
    for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
        p.v2pt_theory(pD, F, A)

    # points of B, C touching the plane P
    pB_hat = pB_star.locatenew('B^', -R*A.x)
    pC_hat = pC_star.locatenew('C^', -R*A.x)
    pB_hat.v2pt_theory(pB_star, F, B)
    pC_hat.v2pt_theory(pC_star, F, C)

    # --- relate qdot, u ---
    # the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
    kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
    kde += [u1 - q1d]
    kde_map = solve(kde, [q1d, q2d, q3d])
    for k, v in list(kde_map.items()):
        kde_map[k.diff(t)] = v.diff(t)

    # inertias of bodies A, B, C
    # IA22, IA23, IA33 are not specified in the problem statement, but are
    # necessary to define an inertia object. Although the values of
    # IA22, IA23, IA33 are not known in terms of the variables given in the
    # problem statement, they do not appear in the general inertia terms.
    inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
    inertia_B = inertia(B, K, K, J)
    inertia_C = inertia(C, K, K, J)

    # define the rigid bodies A, B, C
    rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
    rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
    rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))

    ## --- use kanes method ---
    km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])

    forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
    bodies = [rbA, rbB, rbC]

    # Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
    # -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
    fr_expected = Matrix([
            f*Q3 + M*g*e*sin(theta)*cos(q1),
            Q2 + M*g*sin(theta)*sin(q1),
            e*M*g*cos(theta) - Q1*f - Q2*R])
             #Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
    fr_star_expected = Matrix([
            -(IA + 2*J*b**2/R**2 + 2*K +
              mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
            -(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
            0])

    fr, fr_star = km.kanes_equations(forces, bodies)
    assert (fr.expand() == fr_expected.expand())
    assert (trigsimp(fr_star).expand() == fr_star_expected.expand())
Пример #49
0
class IKSolver:
    """Class for performing iterative inverse kinematics for the blueberry robot"""

    #Note: Joint arrays are listed in order according to the following indexing:
    #[0] = shoulder rotation, [0] = shoulder flexure, [0] = elbow flexure, [0] = wrist rotation, [4] = wrist flexure

    #Joint Limits in radians as tuples of [0] = lower bound and [1] = upper bound
    _joint_limits = [(-1.134, 1.833), (-2.182, 0.785), (-1.309, 1.658),
                     (-1.571, 1.396), (-1.396, 1.571)]
    #Joints for which the servos were mounted in reverse (I.E. positive increase in input value equals radian decrease)
    _rev_joints = [False, True, False, False, True]
    #Home position for the robot (pointing straight up)
    _home = [70, 130, 80, 95, 85]
    #Neutral gripper position to avoid servo strain
    _grip_neutral = 30
    #Fine tuning parameters for iterative inverse kinematics
    #---How many gradient descents to perform while looking for a solution
    _maxIterations = 5
    #---How far to move on each step of an interation
    _stepSize = .01
    #---How close to the goal the solution must be to be sufficient
    _goalAccuracy = .05
    #---How many steps for each gradient descent
    _subIterations = 250
    #Current joint angles of the robot
    _current = [0, 0, 0, 0, 0]

    #Homogeneous transforms representing each frame from the base of the robot
    # (Directly under the center of the robot)
    # to the gripper ee (1 cm below the deepest part of the gripper while open)
    # used to calculate forward kinematics
    _q1, _q2, _q3, _q4, _q5 = symbols('_q1 _q2 _q3 _q4 _q5')
    _T1 = Matrix([[cos(_q1), -sin(_q1), 0, 0], [sin(_q1),
                                                cos(_q1), 0, 0],
                  [0, 0, 1, 0.085], [0, 0, 0, 1]])
    _T2 = Matrix([[cos(_q2), 0,
                   sin(_q2),
                   cos(_q2 + np.pi / 2) * 0.085], [0, 1, 0, 0],
                  [-sin(_q2), 0,
                   cos(_q2),
                   sin(_q2 + np.pi / 2) * 0.085], [0, 0, 0, 1]])
    _T3 = Matrix([[cos(_q3), 0,
                   sin(_q3),
                   cos(_q3 + np.pi / 2) * 0.115], [0, 1, 0, 0],
                  [-sin(_q3), 0,
                   cos(_q3),
                   sin(_q3 + np.pi / 2) * 0.115], [0, 0, 0, 1]])
    _T4 = Matrix([[cos(_q4), -sin(_q4), 0, 0], [sin(_q4),
                                                cos(_q4), 0, 0], [0, 0, 1, 0],
                  [0, 0, 0, 1]])
    _T5 = Matrix([[cos(_q5), 0,
                   sin(_q5),
                   cos(_q5 + np.pi / 2) * 0.06], [0, 1, 0, 0],
                  [-sin(_q5), 0,
                   cos(_q5),
                   sin(_q5 + np.pi / 2) * 0.06], [0, 0, 0, 1]])
    _FK = _T1 * _T2 * _T3 * _T4 * _T5

    def __init__(self, usbPort):
        """Initialize a connection to the robot with inverse kinematic control"""
        #Connect to board via usb (must have BlueberryHardwareControl loaded on it)
        self.ser = serial.Serial(usbPort, 9600, timeout=.5)
        #Get the translation only portion of the forward kinematics (since we don't have full 6 DOF for translation and orientation)
        justTranslation = self._FK.col(-1)
        justTranslation.row_del(-1)
        input = Matrix([self._q1, self._q2, self._q3, self._q4, self._q5])
        #Get jacobian for gradient descent
        self.translationJacobian = justTranslation.jacobian(input)

    def getEE(self, q):
        """Get the homogeneous transform of the end effector in the base frame given an array of joint angles"""
        substituteDict = {
            self._q1: q[0],
            self._q2: q[1],
            self._q3: q[2],
            self._q4: q[3],
            self._q5: q[4]
        }
        return self._FK.evalf(subs=substituteDict)

    def solveForEFTranslation(self, goalX):
        """Get the joint configuration that will get the robot's end effector to (or close to) the goal position. 
        Takes in a 3d vector as a goal position
        Returns a vector of joint angles """
        iterations = 0
        goal = Matrix(goalX)
        dx = Matrix([1, 1, 1])
        bestDX = Matrix([999, 999, 999])
        bestQ = []
        #Iterate until you find an acceptable end effector position or hit the max iterations
        while iterations < self._maxIterations and bestDX.norm(
        ) > self._goalAccuracy:
            iterations += 1
            #Initialize to a random initial configuration (avoid local minimums)
            q = [
                self._joint_limits[0][0] +
                random.random() * self._joint_limits[0][1],
                self._joint_limits[1][0] +
                random.random() * self._joint_limits[1][1],
                self._joint_limits[2][0] +
                random.random() * self._joint_limits[2][1],
                self._joint_limits[3][0] +
                random.random() * self._joint_limits[3][1],
                self._joint_limits[4][0] +
                random.random() * self._joint_limits[4][1]
            ]
            k = 0
            #Perfrom gradient descent over the next subIteration number of steps
            numNudges = 0
            while k < self._subIterations:
                k += 1
                #Get the difference of the end effector from the goal position
                x = self.getEE(q)
                x = x.col(-1)
                x.row_del(-1)
                dx = goal - x
                #Use the inverse (pseduo, as this isn't square) of the jacobian at the current joint configuration to get a
                #change in q that lowers the difference of the end effector from the goal position
                substituteDict = {
                    self._q1: q[0],
                    self._q2: q[1],
                    self._q3: q[2],
                    self._q4: q[3],
                    self._q5: q[4]
                }
                jacobOutput = self.translationJacobian.evalf(
                    subs=substituteDict)
                dq = self._stepSize * (jacobOutput.pinv() * dx)
                #Use change in q to approach the best joint configuration
                newQ = Matrix(q) + dq
                #Exit solution attempt if too many nudges are occurring relative to the maximum number of gradient descent steps
                #We don't want to spend too much time on a solution that is boarderline not reachable
                if numNudges >= self._subIterations / 2:
                    break
                #Check the new configuration to make sure it is in bounds, and nudge it back in bounds if so
                ind = 0
                for angle in newQ:
                    nudged = True
                    if not self._rev_joints[ind]:
                        if angle < self._joint_limits[ind][0]:
                            q[ind] = self._joint_limits[ind][0]
                        elif angle > self._joint_limits[ind][1]:
                            q[ind] = self._joint_limits[ind][1]
                        else:
                            q[ind] = angle
                            nudged = False
                    else:
                        if angle < -1 * self._joint_limits[ind][1]:
                            q[ind] = -1 * self._joint_limits[ind][1]
                        elif angle > -1 * self._joint_limits[ind][0]:
                            q[ind] = -1 * self._joint_limits[ind][0]
                        else:
                            q[ind] = angle
                            nudged = False
                    ind += 1
                if nudged:
                    numNudges += 1

            #Recalculate the distance to the goal angle post nudging to check if it is the best solution yet
            x = self.getEE(q)
            x = x.col(-1)
            x.row_del(-1)
            dx = goal - x
            #Store the configuration if it is the best one we've seen thusfar
            if dx.norm() < bestDX.norm():
                print("Best dx (" + str(bestDX.norm()) +
                      ") replaced with new diff: " + str(dx.norm()))
                bestDX = dx
                bestQ = q
            else:
                print("dx not replaced")
        return bestQ

    def setAnglesAndGrip(self, qVector, grip):
        """Send a vector of joint angles and a gripper position over serial to the robot
        Returns the new joint angles"""
        #Clear the serial connection for a new exchange
        self.ser.flushInput()
        self.ser.flushOutput()
        #Combine joint vector into a single string command for sending over serial
        intQ = []
        for floatVal in qVector:
            intQ.append(int(round(floatVal)))
        command = str(intQ[0]) + ',' + str(intQ[1]) + ',' + str(
            intQ[2]) + ',' + str(intQ[3]) + ',' + str(
                intQ[4]) + ',' + str(grip) + '\n'
        print("Sending: " + command)
        self.ser.write(command.encode('utf-8'))

        #wait until a repsonse if found from the arduino
        OK = 'no'
        while (OK != 'd'):
            print("WaitingOnResponse: " + OK)
            OK = self.ser.read(1).decode("utf-8")
        print("ReceivedResponse")

        return intQ + [grip]

    def convertIKQToDegrees(self, q):
        """convert a vector of joint angles in radians from IK solution to degrees suitable for sending to the servos"""
        qNew = []
        ind = 0
        for angle in q:
            if not self._rev_joints[ind]:
                qNew.append((angle * (180.0 / np.pi)) + self._home[ind])
            else:
                qNew.append(self._home[ind] - (angle * (180.0 / np.pi)))
            ind += 1
        return qNew

    def home(self):
        """Send robot to home position and record angles"""
        self._current = self.setAnglesAndGrip(self._home, self._grip_neutral)

    def moveToGoalAbsolute(self, point):
        """Send robot to the given goal position and record angles"""
        pointAsVector = [point.x, point.y, point.z]
        bestConfig = self.solveForEFTranslation(pointAsVector)
        print("output angles")
        print(bestConfig)
        print("fk of output angles")
        print(self.getEE(bestConfig))
        qNew = self.convertIKQToDegrees(bestConfig)
        self._current = self.setAnglesAndGrip(qNew, self._grip_neutral)

    def moveToGoalRelative(self, point):
        """Adjust robot by the given differential vector and record angles"""
        pointAsVector = [point.x, point.y, point.z]
        x = self.getEE(self._current)
        x = x.col(-1)
        x.row_del(-1)
        relativePoint = x + Matrix(pointAsVector)
        bestConfig = self.solveForEFTranslation(relativePoint)
        print("output angles")
        print(bestConfig)
        print("fk of output angles")
        print(self.getEE(bestConfig))
        qNew = self.convertIKQToDegrees(bestConfig)
        self._current = self.setAnglesAndGrip(qNew, self._grip_neutral)
Пример #50
0
def test_lambdify_matrix():
    x = Symbol("x")
    f = lambdify(x, Matrix([[x, 2 * x], [1, 2]]), "numpy")
    assert (f(1) == matrix([[1, 2], [1, 2]])).all()
Пример #51
0
def test_vector():
    """
    Tests the effects of orientation of coordinate systems on
    basic vector operations.
    """
    N = CoordSysCartesian('N')
    A = N.orient_new_axis('A', q1, N.k)
    B = A.orient_new_axis('B', q2, A.i)
    C = B.orient_new_axis('C', q3, B.j)

    #Test to_matrix
    v1 = a*N.i + b*N.j + c*N.k
    assert v1.to_matrix(A) == Matrix([[ a*cos(q1) + b*sin(q1)],
                                      [-a*sin(q1) + b*cos(q1)],
                                      [                     c]])

    #Test dot
    assert N.i.dot(A.i) == cos(q1)
    assert N.i.dot(A.j) == -sin(q1)
    assert N.i.dot(A.k) == 0
    assert N.j.dot(A.i) == sin(q1)
    assert N.j.dot(A.j) == cos(q1)
    assert N.j.dot(A.k) == 0
    assert N.k.dot(A.i) == 0
    assert N.k.dot(A.j) == 0
    assert N.k.dot(A.k) == 1

    assert N.i.dot(A.i + A.j) == -sin(q1) + cos(q1) == \
           (A.i + A.j).dot(N.i)

    assert A.i.dot(C.i) == cos(q3)
    assert A.i.dot(C.j) == 0
    assert A.i.dot(C.k) == sin(q3)
    assert A.j.dot(C.i) == sin(q2)*sin(q3)
    assert A.j.dot(C.j) == cos(q2)
    assert A.j.dot(C.k) == -sin(q2)*cos(q3)
    assert A.k.dot(C.i) == -cos(q2)*sin(q3)
    assert A.k.dot(C.j) == sin(q2)
    assert A.k.dot(C.k) == cos(q2)*cos(q3)

    #Test cross
    assert N.i.cross(A.i) == sin(q1)*A.k
    assert N.i.cross(A.j) == cos(q1)*A.k
    assert N.i.cross(A.k) == -sin(q1)*A.i - cos(q1)*A.j
    assert N.j.cross(A.i) == -cos(q1)*A.k
    assert N.j.cross(A.j) == sin(q1)*A.k
    assert N.j.cross(A.k) == cos(q1)*A.i - sin(q1)*A.j
    assert N.k.cross(A.i) == A.j
    assert N.k.cross(A.j) == -A.i
    assert N.k.cross(A.k) == Vector.zero

    assert N.i.cross(A.i) == sin(q1)*A.k
    assert N.i.cross(A.j) == cos(q1)*A.k
    assert N.i.cross(A.i + A.j) == sin(q1)*A.k + cos(q1)*A.k
    assert (A.i + A.j).cross(N.i) == (-sin(q1) - cos(q1))*N.k

    assert A.i.cross(C.i) == sin(q3)*C.j
    assert A.i.cross(C.j) == -sin(q3)*C.i + cos(q3)*C.k
    assert A.i.cross(C.k) == -cos(q3)*C.j
    assert C.i.cross(A.i) == (-sin(q3)*cos(q2))*A.j + \
           (-sin(q2)*sin(q3))*A.k
    assert C.j.cross(A.i) == (sin(q2))*A.j + (-cos(q2))*A.k
    assert express(C.k.cross(A.i), C).trigsimp() == cos(q3)*C.j
Пример #52
0
    def solveForEFTranslation(self, goalX):
        """Get the joint configuration that will get the robot's end effector to (or close to) the goal position. 
        Takes in a 3d vector as a goal position
        Returns a vector of joint angles """
        iterations = 0
        goal = Matrix(goalX)
        dx = Matrix([1, 1, 1])
        bestDX = Matrix([999, 999, 999])
        bestQ = []
        #Iterate until you find an acceptable end effector position or hit the max iterations
        while iterations < self._maxIterations and bestDX.norm(
        ) > self._goalAccuracy:
            iterations += 1
            #Initialize to a random initial configuration (avoid local minimums)
            q = [
                self._joint_limits[0][0] +
                random.random() * self._joint_limits[0][1],
                self._joint_limits[1][0] +
                random.random() * self._joint_limits[1][1],
                self._joint_limits[2][0] +
                random.random() * self._joint_limits[2][1],
                self._joint_limits[3][0] +
                random.random() * self._joint_limits[3][1],
                self._joint_limits[4][0] +
                random.random() * self._joint_limits[4][1]
            ]
            k = 0
            #Perfrom gradient descent over the next subIteration number of steps
            numNudges = 0
            while k < self._subIterations:
                k += 1
                #Get the difference of the end effector from the goal position
                x = self.getEE(q)
                x = x.col(-1)
                x.row_del(-1)
                dx = goal - x
                #Use the inverse (pseduo, as this isn't square) of the jacobian at the current joint configuration to get a
                #change in q that lowers the difference of the end effector from the goal position
                substituteDict = {
                    self._q1: q[0],
                    self._q2: q[1],
                    self._q3: q[2],
                    self._q4: q[3],
                    self._q5: q[4]
                }
                jacobOutput = self.translationJacobian.evalf(
                    subs=substituteDict)
                dq = self._stepSize * (jacobOutput.pinv() * dx)
                #Use change in q to approach the best joint configuration
                newQ = Matrix(q) + dq
                #Exit solution attempt if too many nudges are occurring relative to the maximum number of gradient descent steps
                #We don't want to spend too much time on a solution that is boarderline not reachable
                if numNudges >= self._subIterations / 2:
                    break
                #Check the new configuration to make sure it is in bounds, and nudge it back in bounds if so
                ind = 0
                for angle in newQ:
                    nudged = True
                    if not self._rev_joints[ind]:
                        if angle < self._joint_limits[ind][0]:
                            q[ind] = self._joint_limits[ind][0]
                        elif angle > self._joint_limits[ind][1]:
                            q[ind] = self._joint_limits[ind][1]
                        else:
                            q[ind] = angle
                            nudged = False
                    else:
                        if angle < -1 * self._joint_limits[ind][1]:
                            q[ind] = -1 * self._joint_limits[ind][1]
                        elif angle > -1 * self._joint_limits[ind][0]:
                            q[ind] = -1 * self._joint_limits[ind][0]
                        else:
                            q[ind] = angle
                            nudged = False
                    ind += 1
                if nudged:
                    numNudges += 1

            #Recalculate the distance to the goal angle post nudging to check if it is the best solution yet
            x = self.getEE(q)
            x = x.col(-1)
            x.row_del(-1)
            dx = goal - x
            #Store the configuration if it is the best one we've seen thusfar
            if dx.norm() < bestDX.norm():
                print("Best dx (" + str(bestDX.norm()) +
                      ") replaced with new diff: " + str(dx.norm()))
                bestDX = dx
                bestQ = q
            else:
                print("dx not replaced")
        return bestQ
Пример #53
0

def curvature(Rmn):
    return Rmn.ud(0, 0) + Rmn.ud(1, 1) + Rmn.ud(2, 2) + Rmn.ud(3, 3)


B = Function('B')
A = Function('A')

t = Symbol('t')
r = Symbol('r')
theta = Symbol('theta')
phi = Symbol('phi')

#general, spherically symmetric metric
gdd = Matrix(((-B(r), 0, 0, 0), (0, A(r), 0, 0), (0, 0, r**2, 0),
              (0, 0, 0, r**2 * sin(theta)**2)))

g = MT(gdd)
X = (t, r, theta, phi)
Gamma = G(g, X)
Rmn = Ricci(Riemann(Gamma, X), X)


def pprint_Gamma_udd(i, k, l):
    pprint(Eq(Symbol('Gamma^%i_%i%i' % (i, k, l)), Gamma.udd(i, k, l)))


def pprint_Rmn_dd(i, j):
    pprint(Eq(Symbol('R_%i%i' % (i, j)), Rmn.dd(i, j)))

Пример #54
0
def test_as_immutable():
    X = Matrix([[1,2], [3,4]])
    assert X.as_immutable() == ImmutableMatrix([[1,2],[3,4]])
Пример #55
0
def test_issue_14950():
    x = Matrix(symbols('t s'))
    x0 = Matrix([17, 23])
    eqn = x + x0
    assert nsolve(eqn, x, x0) == -x0
    assert nsolve(eqn.T, x.T, x0.T) == -x0
Пример #56
0
def test_Matrix2():
    x = Symbol("x")
    m = Matrix([[x, x**2], [5, 2 / x]])
    assert (matrix(m.subs(x, 2)) == matrix([[2, 4], [5, 1]])).all()
    m = Matrix([[sin(x), x**2], [5, 2 / x]])
    assert (matrix(m.subs(x, 2)) == matrix([[sin(2), 4], [5, 1]])).all()
Пример #57
0
def diop_quadratic(var, coeff, t):
    """
    Solves quadratic diophantine equations, i.e equations of the form
    Ax**2 + Bxy + Cy**2 + Dx + Ey + F = 0. Returns a set containing
    the tuples (x, y) which contains the solutions.

    Usage
    =====

        diop_quadratic(var, coeff) -> var is a list of variables and
        coeff is a dictionary containing coefficients of the symbols.

    Details
    =======

        ``var`` a list which contains two variables x and y.
        ``coeff`` a dict which generally contains six key value pairs.
        The set of keys is {x**2, y**2, x*y, x, y, Integer(1)}.
        ``t`` the parameter to be used in the solution.

    Examples
    ========

    >>> from sympy.abc import x, y, t
    >>> from sympy import Integer
    >>> from sympy.solvers.diophantine import diop_quadratic
    >>> diop_quadratic([x, y], {x**2: 1, y**2: 1, x*y: 0, x: 2, y: 2, Integer(1): 2}, t)
    set([(-1, -1)])

    References
    ==========

    .. [1] http://www.alpertron.com.ar/METHODS.HTM
    """
    x = var[0]
    y = var[1]

    for term in [x**2, y**2, x*y, x, y, Integer(1)]:
        if term not in coeff.keys():
            coeff[term] = Integer(0)

    A = coeff[x**2]
    B = coeff[x*y]
    C = coeff[y**2]
    D = coeff[x]
    E = coeff[y]
    F = coeff[Integer(1)]

    d = igcd(A, igcd(B, igcd(C, igcd(D, igcd(E, F)))))
    A = A // d
    B = B // d
    C = C // d
    D = D // d
    E = E // d
    F = F // d

    # (1) Linear case: A = B = C = 0 -> considered under linear diophantine equations

    # (2) Simple-Hyperbolic case:A = C = 0, B != 0
    # In this case equation can be converted to (Bx + E)(By + D) = DE - BF
    # We consider two cases; DE - BF = 0 and DE - BF != 0
    # More details, http://www.alpertron.com.ar/METHODS.HTM#SHyperb

    l = set([])

    if A == 0 and C == 0 and B != 0:

        if D*E - B*F == 0:
            if divisible(int(E), int(B)):
                l.add((-E/B, t))
            if divisible(int(D), int(B)):
                l.add((t, -D/B))

        else:
            div = divisors(D*E - B*F)
            div = div + [-term for term in div]

            for d in div:
                if divisible(int(d - E), int(B)):
                    x0  = (d - E) // B
                    if divisible(int(D*E - B*F), int(d)):
                        if divisible(int((D*E - B*F)// d - D), int(B)):
                            y0 = ((D*E - B*F) // d - D) // B
                            l.add((x0, y0))

    # (3) Elliptical case: B**2 - 4AC < 0
    # More Details, http://www.alpertron.com.ar/METHODS.HTM#Ellipse
    # In this case x should lie between the roots of
    # (B**2 - 4AC)x**2 + 2(BE - 2CD)x + (E**2 - 4CF) = 0

    elif B**2 - 4*A*C < 0:

        z = symbols("z", real=True)
        roots = solve((B**2 - 4*A*C)*z**2 + 2*(B*E - 2*C*D)*z + E**2 - 4*C*F)

        solve_y = lambda x, e: (-(B*x + E) + e*sqrt((B*x + E)**2 - 4*C*(A*x**2 + D*x + F)))/(2*C)

        if len(roots) == 1 and isinstance(roots[0], Integer):
            x_vals = [roots[0]]
        elif len(roots) == 2:
            x_vals = [i for i in range(ceiling(min(roots)), ceiling(max(roots)))] # ceiling = floor +/- 1
        else:
            x_vals = []

        for x0 in x_vals:
            if isinstance(sqrt((B*x0 + E)**2 - 4*C*(A*x0**2 + D*x0 + F)), Integer):
                if isinstance(solve_y(x0, 1), Integer):
                    l.add((Integer(x0), solve_y(x0, 1)))
                if isinstance(solve_y(x0, -1), Integer):
                    l.add((Integer(x0), solve_y(x0, -1)))

    # (4) Parabolic case: B**2 - 4*A*C = 0
    # There are two subcases to be considered in this case.
    # sqrt(c)D - sqrt(a)E = 0 and sqrt(c)D - sqrt(a)E != 0
    # More Details, http://www.alpertron.com.ar/METHODS.HTM#Parabol

    elif B**2 - 4*A*C == 0:

        g = igcd(A, C)
        g = abs(g) * sign(A)
        a = A // g
        b = B // g
        c = C // g
        e = sign(B/A)


        if e*sqrt(c)*D - sqrt(a)*E == 0:
            z = symbols("z", real=True)
            roots = solve(sqrt(a)*g*z**2 + D*z + sqrt(a)*F)

            for root in roots:
                if isinstance(root, Integer):
                    l.add((diop_solve(sqrt(a)*x + e*sqrt(c)*y - root)[x], diop_solve(sqrt(a)*x + e*sqrt(c)*y - root)[y]))

        elif isinstance(e*sqrt(c)*D - sqrt(a)*E, Integer):
            solve_x = lambda u: e*sqrt(c)*g*(sqrt(a)*E - e*sqrt(c)*D)*t**2 - (E + 2*e*sqrt(c)*g*u)*t\
                - (e*sqrt(c)*g*u**2 + E*u + e*sqrt(c)*F) // (e*sqrt(c)*D - sqrt(a)*E)

            solve_y = lambda u: sqrt(a)*g*(e*sqrt(c)*D - sqrt(a)*E)*t**2 + (D + 2*sqrt(a)*g*u)*t \
                + (sqrt(a)*g*u**2 + D*u + sqrt(a)*F) // (e*sqrt(c)*D - sqrt(a)*E)

            for z0 in range(0, abs(e*sqrt(c)*D - sqrt(a)*E)):
                if divisible(sqrt(a)*g*z0**2 + D*z0 + sqrt(a)*F, e*sqrt(c)*D - sqrt(a)*E):
                    l.add((solve_x(z0), solve_y(z0)))

    # (5) B**2 - 4*A*C > 0

    elif B**2 - 4*A*C > 0:
        # Method used when B**2 - 4*A*C is a square, is descibed in p. 6 of the below paper
        # by John P. Robertson.
        # http://www.jpr2718.org/ax2p.pdf

        if isinstance(sqrt(B**2 - 4*A*C), Integer):
            if A != 0:
                r = sqrt(B**2 - 4*A*C)
                u, v = symbols("u, v", integer=True)
                eq = simplify(4*A*r*u*v + 4*A*D*(B*v + r*u + r*v - B*u) + 2*A*4*A*E*(u - v) + 4*A*r*4*A*F)

                sol = diop_solve(eq, t)
                sol = list(sol)

                for solution in sol:
                    s0 = solution[0]
                    t0 = solution[1]

                    x_0 = S(B*t0 + r*s0 + r*t0 - B*s0)/(4*A*r)
                    y_0 = S(s0 - t0)/(2*r)

                    if isinstance(s0, Symbol) or isinstance(t0, Symbol):
                        if check_param(x_0, y_0, 4*A*r, t) != (None, None):
                            l.add((check_param(x_0, y_0, 4*A*r, t)[0], check_param(x_0, y_0, 4*A*r, t)[1]))

                    elif divisible(B*t0 + r*s0 + r*t0 - B*s0, 4*A*r):
                        if divisible(s0 - t0, 2*r):
                            if is_solution_quad(var, coeff, x_0, y_0):
                                l.add((x_0, y_0))
            else:
                var[0], var[1] = var[1], var[0] # Interchange x and y
                s = diop_quadratic(var, coeff, t)

                while len(s) > 0:
                    sol = s.pop()
                    l.add((sol[1], sol[0]))

        else:
            # In this case equation can be transformed into a Pell equation
            A, B = _transformation_to_pell(var, coeff)
            D, N = _find_DN(var, coeff)
            solns_pell = diop_pell(D, N)

            n = symbols("n", integer=True)

            a = diop_pell(D, 1)
            T = a[0][0]
            U = a[0][1]

            if (isinstance(A[0], Integer) and isinstance(A[1], Integer) and isinstance(A[2], Integer)
                and isinstance(A[3], Integer) and isinstance(B[0], Integer) and isinstance(B[1], Integer)):
                for sol in solns_pell:

                    r = sol[0]
                    s = sol[1]
                    x_n = S((r + s*sqrt(D))*(T + U*sqrt(D))**n + (r - s*sqrt(D))*(T - U*sqrt(D))**n)/2
                    y_n = S((r + s*sqrt(D))*(T + U*sqrt(D))**n - (r - s*sqrt(D))*(T - U*sqrt(D))**n)/(2*sqrt(D))

                    x_n, y_n = (A*Matrix([x_n, y_n]) + B)[0], (A*Matrix([x_n, y_n]) + B)[1]

                    l.add((x_n, y_n))

            else:
                L = ilcm(S(A[0]).q, ilcm(S(A[1]).q, ilcm(S(A[2]).q, ilcm(S(A[3]).q, ilcm(S(B[0]).q, S(B[1]).q)))))

                k = 0
                done = False
                T_k = T
                U_k = U

                while not done:
                    k = k + 1
                    if (T_k - 1) % L == 0 and U_k % L == 0:
                        done = True
                    T_k, U_k = T_k*T + D*U_k*U, T_k*U + U_k*T

                for soln in solns_pell:
                    x_0 = soln[0]
                    y_0 = soln[1]

                    x_i = x_0
                    y_i = y_0

                    for i in range(k):

                        X = (A*Matrix([x_i, y_i]) + B)[0]
                        Y = (A*Matrix([x_i, y_i]) + B)[1]

                        if isinstance(X, Integer) and isinstance(Y, Integer):
                            if is_solution_quad(var, coeff, X, Y):
                                x_n = S( (x_i + sqrt(D)*y_i)*(T + sqrt(D)*U)**(n*L) + (x_i - sqrt(D)*y_i)*(T - sqrt(D)*U)**(n*L) )/ 2
                                y_n = S( (x_i + sqrt(D)*y_i)*(T + sqrt(D)*U)**(n*L) - (x_i - sqrt(D)*y_i)*(T - sqrt(D)*U)**(n*L) )/ (2*sqrt(D))

                                x_n, y_n = (A*Matrix([x_n, y_n]) + B)[0], (A*Matrix([x_n, y_n]) + B)[1]
                                l.add((x_n, y_n))

                        x_i = x_i*T + D*U*y_i
                        y_i = x_i*U + y_i*T

    return l
Пример #58
0
import numpy as np
from sympy import simplify, Matrix, var

xi = var('xi')
eta = var('eta')

print('Chebyshev Polynomials of the Second Kind')
u = [1, 2 * xi]
for i in range(2, 15):
    ui = simplify(2 * xi * u[i - 1] - u[i - 2])
    u.append(ui)
for i, ui in enumerate(u):
    print('u({0}) = {1}'.format(i, ui))

with open('cheb.txt', 'w') as f:
    f.write('Number of terms: {0}\n\n'.format(len(u)))
    f.write(','.join(map(str, u)).replace('**', '^') + '\n\n')
    f.write(','.join(map(str, u)).replace('xi', 'eta').replace('**', '^'))

print np.outer(u, [ui.subs({xi: eta}) for ui in u])

if False:
    m = Matrix([[2 * xi, 1, 0, 0], [1, 2 * xi, 1, 0], [0, 1, 2 * xi, 1],
                [0, 0, 1, 2 * xi]])

    print('m.det() {0}'.format(simplify(m.det())))
Пример #59
0
def _transformation_to_pell(var, coeff):

    x = var[0]
    y = var[1]

    a = coeff[x**2]
    b = coeff[x*y]
    c = coeff[y**2]
    d = coeff[x]
    e = coeff[y]
    f = coeff[Integer(1)]

    g = igcd(a, igcd(b, igcd(c, igcd(d, igcd(e, f)))))
    a = a // g
    b = b // g
    c = c // g
    d = d // g
    e = e // g
    f = f // g

    X, Y = symbols("X, Y", integer=True)

    if b != Integer(0):
        B = (S(2*a)/b).p
        C = (S(2*a)/b).q
        A = (S(a)/B**2).p
        T = (S(a)/B**2).q

        # eq_1 = A*B*X**2 + B*(c*T - A*C**2)*Y**2 + d*T*X + (B*e*T - d*T*C)*Y + f*T*B
        coeff = {X**2: A*B, X*Y: 0, Y**2: B*(c*T - A*C**2), X: d*T, Y: B*e*T - d*T*C, Integer(1): f*T*B}
        A_0, B_0 = _transformation_to_pell([X, Y], coeff)
        return Matrix(2, 2, [S(1)/B, -S(C)/B, 0, 1])*A_0, Matrix(2, 2, [S(1)/B, -S(C)/B, 0, 1])*B_0

    else:
        if d != Integer(0):
            B = (S(2*a)/d).p
            C = (S(2*a)/d).q
            A = (S(a)/B**2).p
            T = (S(a)/B**2).q

            # eq_2 = A*X**2 + c*T*Y**2 + e*T*Y + f*T - A*C**2
            coeff = {X**2: A, X*Y: 0, Y**2: c*T, X: 0, Y: e*T, Integer(1): f*T - A*C**2}
            A_0, B_0 = _transformation_to_pell([X, Y], coeff)
            return Matrix(2, 2, [S(1)/B, 0, 0, 1])*A_0, Matrix(2, 2, [S(1)/B, 0, 0, 1])*B_0 + Matrix([-S(C)/B, 0])

        else:
            if e != Integer(0):
                B = (S(2*c)/e).p
                C = (S(2*c)/e).q
                A = (S(c)/B**2).p
                T = (S(c)/B**2).q

                # eq_3 = a*T*X**2 + A*Y**2 + f*T - A*C**2
                coeff = {X**2: a*T, X*Y: 0, Y**2: A, X: 0, Y: 0, Integer(1): f*T - A*C**2}
                A_0, B_0 = _transformation_to_pell([X, Y], coeff)
                return Matrix(2, 2, [1, 0, 0, S(1)/B])*A_0, Matrix(2, 2, [1, 0, 0, S(1)/B])*B_0 + Matrix([0, -S(C)/B])

            else:
                # TODO: pre-simplification: Not necessary but may simplify
                # the equation.

                return Matrix(2, 2, [S(1)/a, 0, 0, 1]), Matrix([0, 0])
Пример #60
0
dt = 1.0/50.0 # Sample Rate of the Measurements is 50Hz
dtGPS=1.0/10.0 # Sample Rate of GPS is 10Hz


# ### Developing the math behind dynamic model
# 
# All symbolic calculations are made with [Sympy](http://nbviewer.ipython.org/github/jrjohansson/scientific-python-lectures/blob/master/Lecture-5-Sympy.ipynb). Thanks!

# In[112]:

vs, psis, dpsis, dts, xs, ys, lats, lons = symbols('v \psi \dot\psi T x y lat lon')

gs = Matrix([[xs+(vs/dpsis)*(sin(psis+dpsis*dts)-sin(psis))],
             [ys+(vs/dpsis)*(-cos(psis+dpsis*dts)+cos(psis))],
             [psis+dpsis*dts],
             [vs],
             [dpsis]])
state = Matrix([xs,ys,psis,vs,dpsis])


# ## Dynamic Function $g$
# 
# This formulas calculate how the state is evolving from one to the next time step

# In[113]:

gs


# ### Calculate the Jacobian of the Dynamic function $g$ with respect to the state vector $x$