Python Matrix.row_join方法代码示例

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import row_join [as 别名]
    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

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import row_join [as 别名]
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])

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import row_join [as 别名]

#.........这里部分代码省略.........
            flist = zip(*_f_list_parser(self.forcelist, N))
            for i, qd in enumerate(qds):
                for obj, force in self.forcelist:
                    self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist)
        else:
            self._term4 = zeros(n, 1)

        # Form the dynamic mass and forcing matrices
        without_lam = self._term1 - self._term2 - self._term4
        self._m_d = without_lam.jacobian(self._qdoubledots)
        self._f_d = -without_lam.subs(qdd_zero)

        # Form the EOM
        self.eom = without_lam - self._term3
        return self.eom

    @property
    def mass_matrix(self):
        """Returns the mass matrix, which is augmented by the Lagrange
        multipliers, if necessary.

        If the system is described by 'n' generalized coordinates and there are
        no constraint equations then an n X n matrix is returned.

        If there are 'n' generalized coordinates and 'm' constraint equations
        have been supplied during initialization then an n X (n+m) matrix is
        returned. The (n + m - 1)th and (n + m)th columns contain the
        coefficients of the Lagrange multipliers.
        """

        if self.eom is None:
            raise ValueError('Need to compute the equations of motion first')
        if self.coneqs:
            return (self._m_d).row_join(self.lam_coeffs.T)
        else:
            return self._m_d

    @property
    def mass_matrix_full(self):
        """Augments the coefficients of qdots to the mass_matrix."""

        if self.eom is None:
            raise ValueError('Need to compute the equations of motion first')
        n = len(self.q)
        m = len(self.coneqs)
        row1 = eye(n).row_join(zeros(n, n + m))
        row2 = zeros(n, n).row_join(self.mass_matrix)
        if self.coneqs:
            row3 = zeros(m, n).row_join(self._m_cd).row_join(zeros(m, m))
            return row1.col_join(row2).col_join(row3)
        else:
            return row1.col_join(row2)

    @property
    def forcing(self):
        """Returns the forcing vector from 'lagranges_equations' method."""

        if self.eom is None:
            raise ValueError('Need to compute the equations of motion first')
        return self._f_d

    @property
    def forcing_full(self):
        """Augments qdots to the forcing vector above."""

        if self.eom is None:

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import row_join [as 别名]
    def linearize(self, op_point=None, A_and_B=False, simplify=False):
        """Linearize the system about the operating point. Note that
        q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
        These may be either symbolic or numeric.

        Parameters
        ----------
        op_point : dict or iterable of dicts, optional
            Dictionary or iterable of dictionaries containing the operating
            point conditions. These will be substituted in to the linearized
            system before the linearization is complete. Leave blank if you
            want a completely symbolic form. Note that any reduction in
            symbols (whether substituted for numbers or expressions with a
            common parameter) will result in faster runtime.

        A_and_B : bool, optional
            If A_and_B=False (default), (M, A, B) is returned for forming
            [M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True,
            (A, B) is returned for forming dx = [A]x + [B]r, where
            x = [q_ind, u_ind]^T.

        simplify : bool, optional
            Determines if returned values are simplified before return.
            For large expressions this may be time consuming. Default is False.

        Note that the process of solving with A_and_B=True is computationally
        intensive if there are many symbolic parameters. For this reason,
        it may be more desirable to use the default A_and_B=False,
        returning M, A, and B. More values may then be substituted in to these
        matrices later on. The state space form can then be found as
        A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where
        P = Linearizer.perm_mat.
        """

        # Compose dict of operating conditions
        if isinstance(op_point, dict):
            op_point_dict = op_point
        elif isinstance(op_point, collections.Iterable):
            op_point_dict = {}
            for op in op_point:
                op_point_dict.update(op)
        else:
            op_point_dict = {}

        # Extract dimension variables
        l, m, n, o, s, k = self._dims

        # Rename terms to shorten expressions
        M_qq = self._M_qq
        M_uqc = self._M_uqc
        M_uqd = self._M_uqd
        M_uuc = self._M_uuc
        M_uud = self._M_uud
        M_uld = self._M_uld
        A_qq = self._A_qq
        A_uqc = self._A_uqc
        A_uqd = self._A_uqd
        A_qu = self._A_qu
        A_uuc = self._A_uuc
        A_uud = self._A_uud
        B_u = self._B_u
        C_0 = self._C_0
        C_1 = self._C_1
        C_2 = self._C_2

        # Build up Mass Matrix
        #     |M_qq    0_nxo   0_nxk|
        # M = |M_uqc   M_uuc   0_mxk|
        #     |M_uqd   M_uud   M_uld|
        if o != 0:
            col2 = Matrix([zeros(n, o), M_uuc, M_uud])
        if k != 0:
            col3 = Matrix([zeros(n + m, k), M_uld])
        if n != 0:
            col1 = Matrix([M_qq, M_uqc, M_uqd])
            if o != 0 and k != 0:
                M = col1.row_join(col2).row_join(col3)
            elif o != 0:
                M = col1.row_join(col2)
            else:
                M = col1
        elif k != 0:
            M = col2.row_join(col3)
        else:
            M = col2
        M_eq = _subs_keep_derivs(M, op_point_dict)

        # Build up state coefficient matrix A
        #     |(A_qq + A_qu*C_1)*C_0       A_qu*C_2|
        # A = |(A_uqc + A_uuc*C_1)*C_0    A_uuc*C_2|
        #     |(A_uqd + A_uud*C_1)*C_0    A_uud*C_2|
        # Col 1 is only defined if n != 0
        if n != 0:
            r1c1 = A_qq
            if o != 0:
                r1c1 += (A_qu * C_1)
            r1c1 = r1c1 * C_0
            if m != 0:
                r2c1 = A_uqc
                if o != 0:
#.........这里部分代码省略.........

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import row_join [as 别名]
             (A*B).transpose(),\
             (A**2*B).transpose(),\
             (A**3*B).transpose()])
Qs = QsT.transpose()
print 'Qs: ',Qs
print 'A: ',A
print 'B: ',B
print 'C: ',C


'''
Nachweis der Steuerbarkeit des erweiterten Zustandsraummodells
'''
Aquer = A.col_join(C).row_join(sp.zeros(5,1))
Bquer = B.col_join(sp.zeros(1,1))
Cquer = C.row_join(sp.zeros(1,1))


'''
Übertragungsfunktion des Linearisierten Systems
'''
G = C*(sp.eye(4)*s - A)**-1*B

# Steuerbarkeitsmatrix berechnen
n = 5
Qsquer = sp.Matrix()
for i in range(5):
    q = (Aquer**i)*Bquer
    if len(Qsquer) == 0:
        Qsquer = Bquer
    else:

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import row_join [as 别名]
U1=A[1]*A[2]*A[3]*A[4]*A[5]
J1=Matrix([[U1[0,3]*U1[1,0]-U1[1,3]*U1[0,0]],[U1[0,3]*U1[1,1]-U0[1,3]*U1[0,1]],[U1[0,3]*U1[1,2]-U1[1,3]*U1[0,2]],[U1[2,0]],[U1[2,1]],[U1[2,2]]])

U2=A[2]*A[3]*A[4]*A[5]
J2=Matrix([[U2[0,3]*U2[1,0]-U2[1,3]*U2[0,0]],[U2[0,3]*U2[1,1]-U2[1,3]*U2[0,1]],[U2[0,3]*U2[1,2]-U2[1,3]*U2[0,2]],[U2[2,0]],[U2[2,1]],[U2[2,2]]])

U3=A[3]*A[4]*A[5]
J3=Matrix([[U3[0,3]*U3[1,0]-U3[1,3]*U3[0,0]],[U3[0,3]*U3[1,1]-U3[1,3]*U3[0,1]],[U3[0,3]*U3[1,2]-U3[1,3]*U3[0,2]],[U3[2,0]],[U3[2,1]],[U3[2,2]]])

U4=A[4]*A[5]
J4=Matrix([[U4[0,3]*U4[1,0]-U4[1,3]*U4[0,0]],[U4[0,3]*U4[1,1]-U4[1,3]*U4[0,1]],[U4[0,3]*U4[1,2]-U4[1,3]*U4[0,2]],[U4[2,0]],[U4[2,1]],[U4[2,2]]])

U5=A[5]
J5=Matrix([[U5[0,3]*U5[1,0]-U5[1,3]*U5[0,0]],[U5[0,3]*U5[1,1]-U5[1,3]*U5[0,1]],[U5[0,3]*U5[1,2]-U5[1,3]*U5[0,2]],[U5[2,0]],[U5[2,1]],[U5[2,2]]])

Jd=J0.row_join(J1).row_join(J2).row_join(J3).row_join(J4).row_join(J5)

# The vector cross product sub-routine

def JacoV(QJ):
 return np.array(Jv.subs(q[0],QJ[0]).subs(q[1],QJ[1]).subs(q[2],QJ[2]).subs(q[3],QJ[3]).subs(q[4],QJ[4]).subs(q[5],QJ[5]))

# Defining Camera Transformation
Tstart=np.array([[ 0.9983323 ,  0.03651881,  0.04471023, -0.12004546],[-0.02196539, -0.47593777,  0.87920462, -4.18794775],[ 0.0533868 , -0.87872044, -0.47434189,  2.22462606],[ 0.        ,  0.        ,  0.        ,  1.        ]])

Tcirc=np.array([[ 0.20604119,  0.33831506, -0.9181993 ,  3.84545088],[ 0.97824604, -0.09434114,  0.18475504, -0.79177779],[-0.02411856, -0.93629198, -0.35039353,  1.40385485],[ 0.        ,  0.        ,  0.        ,  1.        ]])

Tline=np.array([[ 0.02187936,  0.99890052,  0.04146134,  0.02088733],[ 0.98965908, -0.01575924, -0.14257124,  0.30318534],[-0.14176108,  0.04415196, -0.98891577,  2.93201566],[ 0.        ,  0.        ,  0.        ,  1.        ]])

p=raw_input('Enter Path for XML File\n') # Change the path here
env=Environment();

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import row_join [as 别名]
def solve(A, b):
    """
    Finds small solutions to systems of diophantine equations, A x = b, where A
    is a M x N matrix of coefficents, b is a M x 1 vector and x is the
    N x 1 solution vector, e.g.

    >>> from sympy import Matrix
    >>> from diophantine import solve
    >>> A = Matrix([[1, 0, 0, 2], [0, 2, 3, 5], [2, 0, 3, 1], [-6, -1, 0, 2],
                    [0, 1, 1, 1], [-1, 2, 0,1], [-1, -2, 1, 0]]).T
    >>> b = Matrix([1, 1, 1, 1])
    >>> solve(A, b)
    [Matrix([
    [-1],
    [ 1],
    [ 0],
    [ 0],
    [-1],
    [-1],
    [-1]])]

    The returned solution vector will tend to be one with the smallest norms.
    If multiple solutions with the same norm are found they will all be
    returned. If there are no solutions the empty list will be returned.
    """
    A = Matrix(A)
    b = Matrix(b)
    if b.shape != (A.shape[0], 1):
        raise Exception("Length of b vector ({}) does not match number of rows"
                        " in A matrix ({})".format(b.shape[0], A.shape[0]))
    if verbose_solve:
        Ab = A.row_join(b)
        print 'Ab: "' + ' '.join(str(v) for v in Ab.T.vec()) + '"'
    G = zeros(A.shape[1] + 1, A.shape[0] + 1)
    G[:-1, :-1] = A.T
    G[-1, :-1] = b.reshape(1, b.shape[0])
    G[-1, -1] = 1
    # A is m x n, b is m x 1, solving AX=b, X is n x 1+
    # Ab is the (n+1) x m transposed augmented matrix. G=[A^t|0] [b^t]1]
    if verbose_solve:
        print "G:"
        printnp(G)
    hnf, P, rank = lllhermite(G)
    if verbose_solve:
        print "HNF(G):"
        printnp(hnf)
        print "P:"
        printnp(P)
        print "Rank: {}".format(rank)
    r = rank - 1  # For convenience
    if not any(chain(hnf[:r, -1], hnf[r, :-1])) and hnf[r, -1] == 1:
        nullity = hnf.shape[0] - rank
        if nullity:
            basis = P[rank:, :-1].col_join(-P[r, :-1])
            if verbose_solve:
                print "Basis:\n"
                printnp(basis)
            solutions = get_solutions(basis)
        else:
            raise NotImplementedError("Ax=B has unique solution in integers")
    else:
        solutions = []
    return solutions