Ejemplo n.º 1
0
    def _form_coefficient_matrices(self):
        """Form the coefficient matrices C_0, C_1, and C_2."""

        # Extract dimension variables
        l, m, n, o, s, k = self._dims
        # Build up the coefficient matrices C_0, C_1, and C_2
        # If there are configuration constraints (l > 0), form C_0 as normal.
        # If not, C_0 is I_(nxn). Note that this works even if n=0
        if l > 0:
            f_c_jac_q = self.f_c.jacobian(self.q)
            self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q *
                    self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
        else:
            self._C_0 = eye(n)
        # If there are motion constraints (m > 0), form C_1 and C_2 as normal.
        # If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
        # o = 0.
        if m > 0:
            f_v_jac_u = self.f_v.jacobian(self.u)
            temp = f_v_jac_u * self._Pud
            if n != 0:
                f_v_jac_q = self.f_v.jacobian(self.q)
                self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
            else:
                self._C_1 = zeros(o, n)
            self._C_2 = (eye(o) - self._Pud *
                    temp.LUsolve(f_v_jac_u)) * self._Pui
        else:
            self._C_1 = zeros(o, n)
            self._C_2 = eye(o)
Ejemplo n.º 2
0
    def _form_coefficient_matrices(self):
        """Form the coefficient matrices C_0, C_1, and C_2."""

        # Extract dimension variables
        l, m, n, o, s, k = self._dims
        # Build up the coefficient matrices C_0, C_1, and C_2
        # If there are configuration constraints (l > 0), form C_0 as normal.
        # If not, C_0 is I_(nxn). Note that this works even if n=0
        if l > 0:
            f_c_jac_q = self.f_c.jacobian(self.q)
            self._C_0 = (
                eye(n) - self._Pqd *
                (f_c_jac_q * self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
        else:
            self._C_0 = eye(n)
        # If there are motion constraints (m > 0), form C_1 and C_2 as normal.
        # If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
        # o = 0.
        if m > 0:
            f_v_jac_u = self.f_v.jacobian(self.u)
            temp = f_v_jac_u * self._Pud
            if n != 0:
                f_v_jac_q = self.f_v.jacobian(self.q)
                self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
            else:
                self._C_1 = zeros(o, n)
            self._C_2 = (eye(o) -
                         self._Pud * temp.LUsolve(f_v_jac_u)) * self._Pui
        else:
            self._C_1 = zeros(o, n)
            self._C_2 = eye(o)
Ejemplo n.º 3
0
    def cartan_matrix(self):
        """The Cartan matrix for C_n

        The Cartan matrix matrix for a Lie algebra is
        generated by assigning an ordering to the simple
        roots, (alpha[1], ...., alpha[l]).  Then the ijth
        entry of the Cartan matrix is (<alpha[i],alpha[j]>).

        Examples
        ========

        >>> from sympy.liealgebras.cartan_type import CartanType
        >>> c = CartanType('C4')
        >>> c.cartan_matrix()
        Matrix([
        [ 2, -1,  0,  0],
        [-1,  2, -1,  0],
        [ 0, -1,  2, -1],
        [ 0,  0, -2,  2]])

        """

        n = self.n
        m = 2 * eye(n)
        i = 1
        while i < n-1:
           m[i, i+1] = -1
           m[i, i-1] = -1
           i += 1
        m[0,1] = -1
        m[n-1, n-2] = -2
        return m
Ejemplo n.º 4
0
Archivo: kane.py Proyecto: yyht/sympy
    def _initialize_kindiffeq_matrices(self, kdeqs):
        """Initialize the kinematic differential equation matrices."""

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

            u = self.u
            qdot = self._qdot
            # Dictionaries setting things to zero
            u_zero = dict((i, 0) for i in u)
            uaux_zero = dict((i, 0) for i in self._uaux)
            qdot_zero = dict((i, 0) for i in qdot)

            f_k = msubs(kdeqs, u_zero, qdot_zero)
            k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u)
            k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot)

            f_k = k_kqdot.LUsolve(f_k)
            k_ku = k_kqdot.LUsolve(k_ku)
            k_kqdot = eye(len(qdot))

            self._qdot_u_map = solve_linear_system_LU(
                Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot)

            self._f_k = msubs(f_k, uaux_zero)
            self._k_ku = msubs(k_ku, uaux_zero)
            self._k_kqdot = k_kqdot
        else:
            self._qdot_u_map = None
            self._f_k = Matrix()
            self._k_ku = Matrix()
            self._k_kqdot = Matrix()
Ejemplo n.º 5
0
Archivo: kane.py Proyecto: Lenqth/sympy
    def _initialize_kindiffeq_matrices(self, kdeqs):
        """Initialize the kinematic differential equation matrices."""

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

            u = self.u
            qdot = self._qdot
            # Dictionaries setting things to zero
            u_zero = dict((i, 0) for i in u)
            uaux_zero = dict((i, 0) for i in self._uaux)
            qdot_zero = dict((i, 0) for i in qdot)

            f_k = msubs(kdeqs, u_zero, qdot_zero)
            k_ku = (msubs(kdeqs, qdot_zero) - f_k).jacobian(u)
            k_kqdot = (msubs(kdeqs, u_zero) - f_k).jacobian(qdot)

            f_k = k_kqdot.LUsolve(f_k)
            k_ku = k_kqdot.LUsolve(k_ku)
            k_kqdot = eye(len(qdot))

            self._qdot_u_map = solve_linear_system_LU(
                    Matrix([k_kqdot.T, -(k_ku * u + f_k).T]).T, qdot)

            self._f_k = msubs(f_k, uaux_zero)
            self._k_ku = msubs(k_ku, uaux_zero)
            self._k_kqdot = k_kqdot
        else:
            self._qdot_u_map = None
            self._f_k = Matrix()
            self._k_ku = Matrix()
            self._k_kqdot = Matrix()
Ejemplo n.º 6
0
    def cartan_matrix(self):
        """
        Returns the Cartan matrix for B_n.
        The Cartan matrix matrix for a Lie algebra is
        generated by assigning an ordering to the simple
        roots, (alpha[1], ...., alpha[l]).  Then the ijth
        entry of the Cartan matrix is (<alpha[i],alpha[j]>).

        Examples
        ========

        >>> from sympy.liealgebras.cartan_type import CartanType
        >>> c = CartanType('B4')
        >>> c.cartan_matrix()
        Matrix([
        [ 2, -1,  0,  0],
        [-1,  2, -1,  0],
        [ 0, -1,  2, -2],
        [ 0,  0, -1,  2]])

        """

        n = self.n
        m = 2 * eye(n)
        i = 1
        while i < n - 1:
            m[i, i + 1] = -1
            m[i, i - 1] = -1
            i += 1
        m[0, 1] = -1
        m[n - 2, n - 1] = -2
        m[n - 1, n - 2] = -1
        return m
Ejemplo n.º 7
0
    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)
Ejemplo n.º 8
0
    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)
Ejemplo n.º 9
0
 def comb_implicit_mat(self):
     """Returns the matrix, M, corresponding to the equations of motion in
     implicit form (form [2]), M x' = F, where the kinematical equations are
     included"""
     if self._comb_implicit_mat is None:
         if self._dyn_implicit_mat is not None:
             num_kin_eqns = len(self._kin_explicit_rhs)
             num_dyn_eqns = len(self._dyn_implicit_rhs)
             zeros1 = zeros(num_kin_eqns, num_dyn_eqns)
             zeros2 = zeros(num_dyn_eqns, num_kin_eqns)
             inter1 = eye(num_kin_eqns).row_join(zeros1)
             inter2 = zeros2.row_join(self._dyn_implicit_mat)
             self._comb_implicit_mat = inter1.col_join(inter2)
             return self._comb_implicit_mat
         else:
             raise AttributeError("comb_implicit_mat is not specified for "
                                  "equations of motion form [1].")
     else:
         return self._comb_implicit_mat
Ejemplo n.º 10
0
    def dcm(self, otherframe):
        """The direction cosine matrix between frames.

        This gives the DCM between this frame and the otherframe.
        The format is N.xyz = N.dcm(B) * B.xyz
        A SymPy Matrix is returned.

        Parameters
        ==========

        otherframe : ReferenceFrame
            The otherframe which the DCM is generated to.

        Examples
        ========

        >>> from sympy.physics.vector import ReferenceFrame, Vector
        >>> from sympy import symbols
        >>> q1 = symbols('q1')
        >>> N = ReferenceFrame('N')
        >>> A = N.orientnew('A', 'Axis', [q1, N.x])
        >>> N.dcm(A)
        Matrix([
        [1,       0,        0],
        [0, cos(q1), -sin(q1)],
        [0, sin(q1),  cos(q1)]])

        """

        _check_frame(otherframe)
        #Check if the dcm wrt that frame has already been calculated
        if otherframe in self._dcm_cache:
            return self._dcm_cache[otherframe]
        flist = self._dict_list(otherframe, 0)
        outdcm = eye(3)
        for i in range(len(flist) - 1):
            outdcm = outdcm * flist[i]._dcm_dict[flist[i + 1]]
        #After calculation, store the dcm in dcm cache for faster
        #future retrieval
        self._dcm_cache[otherframe] = outdcm
        otherframe._dcm_cache[self] = outdcm.T
        return outdcm
Ejemplo n.º 11
0
    def dcm(self, otherframe):
        """The direction cosine matrix between frames.

        This gives the DCM between this frame and the otherframe.
        The format is N.xyz = N.dcm(B) * B.xyz
        A SymPy Matrix is returned.

        Parameters
        ==========

        otherframe : ReferenceFrame
            The otherframe which the DCM is generated to.

        Examples
        ========

        >>> from sympy.physics.vector import ReferenceFrame, Vector
        >>> from sympy import symbols
        >>> q1 = symbols('q1')
        >>> N = ReferenceFrame('N')
        >>> A = N.orientnew('A', 'Axis', [q1, N.x])
        >>> N.dcm(A)
        Matrix([
        [1,       0,        0],
        [0, cos(q1), -sin(q1)],
        [0, sin(q1),  cos(q1)]])

        """

        _check_frame(otherframe)
        #Check if the dcm wrt that frame has already been calculated
        if otherframe in self._dcm_cache:
            return self._dcm_cache[otherframe]
        flist = self._dict_list(otherframe, 0)
        outdcm = eye(3)
        for i in range(len(flist) - 1):
            outdcm = outdcm * flist[i]._dcm_dict[flist[i + 1]]
        #After calculation, store the dcm in dcm cache for faster
        #future retrieval
        self._dcm_cache[otherframe] = outdcm
        otherframe._dcm_cache[self] = outdcm.T
        return outdcm
Ejemplo n.º 12
0
    def dcm(self, otherframe):
        r"""Returns the direction cosine matrix relative to the provided
        reference frame.

        The returned matrix can be used to express the orthogonal unit vectors
        of this frame in terms of the orthogonal unit vectors of
        ``otherframe``.

        Parameters
        ==========

        otherframe : ReferenceFrame
            The reference frame which the direction cosine matrix of this frame
            is formed relative to.

        Examples
        ========

        The following example rotates the reference frame A relative to N by a
        simple rotation and then calculates the direction cosine matrix of N
        relative to A.

        >>> from sympy import symbols, sin, cos
        >>> from sympy.physics.vector import ReferenceFrame
        >>> q1 = symbols('q1')
        >>> N = ReferenceFrame('N')
        >>> A = N.orientnew('A', 'Axis', (q1, N.x))
        >>> N.dcm(A)
        Matrix([
        [1,       0,        0],
        [0, cos(q1), -sin(q1)],
        [0, sin(q1),  cos(q1)]])

        The second row of the above direction cosine matrix represents the
        ``N.y`` unit vector in N expressed in A. Like so:

        >>> Ny = 0*A.x + cos(q1)*A.y - sin(q1)*A.z

        Thus, expressing ``N.y`` in A should return the same result:

        >>> N.y.express(A)
        cos(q1)*A.y - sin(q1)*A.z

        Notes
        =====

        It is import to know what form of the direction cosine matrix is
        returned. If ``B.dcm(A)`` is called, it means the "direction cosine
        matrix of B relative to A". This is the matrix :math:`{}^A\mathbf{R}^B`
        shown in the following relationship:

        .. math::

           \begin{bmatrix}
             \hat{\mathbf{b}}_1 \\
             \hat{\mathbf{b}}_2 \\
             \hat{\mathbf{b}}_3
           \end{bmatrix}
           =
           {}^A\mathbf{R}^B
           \begin{bmatrix}
             \hat{\mathbf{a}}_1 \\
             \hat{\mathbf{a}}_2 \\
             \hat{\mathbf{a}}_3
           \end{bmatrix}.

        :math:`^{}A\mathbf{R}^B` is the matrix that expresses the B unit
        vectors in terms of the A unit vectors.

        """

        _check_frame(otherframe)
        # Check if the dcm wrt that frame has already been calculated
        if otherframe in self._dcm_cache:
            return self._dcm_cache[otherframe]
        flist = self._dict_list(otherframe, 0)
        outdcm = eye(3)
        for i in range(len(flist) - 1):
            outdcm = outdcm * flist[i]._dcm_dict[flist[i + 1]]
        # After calculation, store the dcm in dcm cache for faster future
        # retrieval
        self._dcm_cache[otherframe] = outdcm
        otherframe._dcm_cache[self] = outdcm.T
        return outdcm
Ejemplo n.º 13
0
    def matrix_form(self, weylelt):
        """
        This method takes input from the user in the form of products of the
        generating reflections, and returns the matrix corresponding to the
        element of the Weyl group.  Since each element of the Weyl group is
        a reflection of some type, there is a corresponding matrix representation.
        This method uses the standard representation for all the generating
        reflections.

        Examples
        ========

        >>> from sympy.liealgebras.weyl_group import WeylGroup
        >>> f = WeylGroup("F4")
        >>> f.matrix_form('r2*r3')
        Matrix([
        [1, 0, 0,  0],
        [0, 1, 0,  0],
        [0, 0, 0, -1],
        [0, 0, 1,  0]])

        """
        elts = list(weylelt)
        reflections = elts[1::3]
        n = self.cartan_type.rank()
        if self.cartan_type.series == 'A':
            matrixform = eye(n+1)
            for elt in reflections:
                a = int(elt)
                mat = eye(n+1)
                mat[a-1, a-1] = 0
                mat[a-1, a] = 1
                mat[a, a-1] = 1
                mat[a, a] = 0
                matrixform *= mat
            return matrixform

        if self.cartan_type.series == 'D':
            matrixform = eye(n)
            for elt in reflections:
                a = int(elt)
                mat = eye(n)
                if a < n:
                    mat[a-1, a-1] = 0
                    mat[a-1, a] = 1
                    mat[a, a-1] = 1
                    mat[a, a] = 0
                    matrixform *= mat
                else:
                    mat[n-2, n-1] = -1
                    mat[n-2, n-2] = 0
                    mat[n-1, n-2] = -1
                    mat[n-1, n-1] = 0
                    matrixform *= mat
            return matrixform

        if self.cartan_type.series == 'G':
            matrixform = eye(3)
            for elt in reflections:
                a = int(elt)
                if a == 1:
                    gen1 = Matrix([[1, 0, 0], [0, 0, 1], [0, 1, 0]])
                    matrixform *= gen1
                else:
                    gen2 = Matrix([[Rational(2, 3), Rational(2, 3), -Rational(1, 3)],
                        [Rational(2, 3), Rational(-1, 3), Rational(2, 3)], [Rational(-1, 3),
                            Rational(2, 3), Rational(2, 3)]])
                    matrixform *= gen2
            return matrixform

        if self.cartan_type.series == 'F':
            matrixform = eye(4)
            for elt in reflections:
                a = int(elt)
                if a == 1:
                    mat = Matrix([[1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
                    matrixform *= mat
                elif a == 2:
                    mat = Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 1], [0, 0, 1, 0]])
                    matrixform *= mat
                elif a == 3:
                    mat = Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, -1]])
                    matrixform *= mat
                else:

                    mat = Matrix([[Rational(1, 2), Rational(1, 2), Rational(1, 2), Rational(1, 2)],
                        [Rational(1, 2), Rational(1, 2), Rational(-1, 2), Rational(-1, 2)],
                        [Rational(1, 2), Rational(-1, 2), Rational(1, 2), Rational(-1, 2)],
                        [Rational(1, 2), Rational(-1, 2), Rational(-1, 2), Rational(1, 2)]])
                    matrixform *= mat
            return matrixform

        if self.cartan_type.series == 'E':
            matrixform = eye(8)
            for elt in reflections:
                a = int(elt)
                if a == 1:
                    mat = Matrix([[Rational(3, 4), Rational(1, 4), Rational(1, 4), Rational(1, 4),
                        Rational(1, 4), Rational(1, 4), Rational(1, 4), Rational(-1, 4)],
                        [Rational(1, 4), Rational(3, 4), Rational(-1, 4), Rational(-1, 4),
                            Rational(-1, 4), Rational(-1, 4), Rational(1, 4), Rational(-1, 4)],
                        [Rational(1, 4), Rational(-1, 4), Rational(3, 4), Rational(-1, 4),
                        Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(1, 4)],
                        [Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(3, 4),
                        Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(1, 4)],
                        [Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4),
                        Rational(3, 4), Rational(-1, 4), Rational(-1, 4), Rational(1, 4)],
                        [Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4),
                        Rational(-1, 4), Rational(3, 4), Rational(-1, 4), Rational(1, 4)],
                        [Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4),
                        Rational(-1, 4), Rational(-1, 4), Rational(-3, 4), Rational(1, 4)],
                        [Rational(1, 4), Rational(-1, 4), Rational(-1, 4), Rational(-1, 4),
                        Rational(-1, 4), Rational(-1, 4), Rational(-1, 4), Rational(3, 4)]])
                    matrixform *= mat
                elif a == 2:
                    mat = eye(8)
                    mat[0, 0] = 0
                    mat[0, 1] = -1
                    mat[1, 0] = -1
                    mat[1, 1] = 0
                    matrixform *= mat
                else:
                    mat = eye(8)
                    mat[a-3, a-3] = 0
                    mat[a-3, a-2] = 1
                    mat[a-2, a-3] = 1
                    mat[a-2, a-2] = 0
                    matrixform *= mat
            return matrixform


        if self.cartan_type.series == 'B' or self.cartan_type.series == 'C':
            matrixform = eye(n)
            for elt in reflections:
                a = int(elt)
                mat = eye(n)
                if a == 1:
                    mat[0, 0] = -1
                    matrixform *= mat
                else:
                    mat[a - 2, a - 2] = 0
                    mat[a-2, a-1] = 1
                    mat[a - 1, a - 2] = 1
                    mat[a -1, a - 1] = 0
                    matrixform *= mat
            return matrixform
Ejemplo n.º 14
0
    def _initialize_kindiffeq_matrices(self, kdeqs):
        """Initialize the kinematic differential equation matrices.

        Parameters
        ==========
        kdeqs : sequence of sympy expressions
            Kinematic differential equations in the form of f(u,q',q,t) where
            f() = 0. The equations have to be linear in the generalized
            coordinates and generalized speeds.

        """

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

            u = self.u
            qdot = self._qdot

            kdeqs = Matrix(kdeqs)

            u_zero = {ui: 0 for ui in u}
            uaux_zero = {uai: 0 for uai in self._uaux}
            qdot_zero = {qdi: 0 for qdi in qdot}

            # Extract the linear coefficient matrices as per the following
            # equation:
            #
            # k_ku(q,t)*u(t) + k_kqdot(q,t)*q'(t) + f_k(q,t) = 0
            #
            k_ku = kdeqs.jacobian(u)
            k_kqdot = kdeqs.jacobian(qdot)
            f_k = kdeqs.xreplace(u_zero).xreplace(qdot_zero)

            # The kinematic differential equations should be linear in both q'
            # and u, so check for u and q' in the components.
            dy_syms = find_dynamicsymbols(k_ku.row_join(k_kqdot).row_join(f_k))
            nonlin_vars = [vari for vari in u[:] + qdot[:] if vari in dy_syms]
            if nonlin_vars:
                msg = ('The provided kinematic differential equations are '
                       'nonlinear in {}. They must be linear in the '
                       'generalized speeds and derivatives of the generalized '
                       'coordinates.')
                raise ValueError(msg.format(nonlin_vars))

            # Solve for q'(t) such that the coefficient matrices are now in
            # this form:
            #
            # k_kqdot^-1*k_ku*u(t) + I*q'(t) + k_kqdot^-1*f_k = 0
            #
            # NOTE : Solving the kinematic differential equations here is not
            # necessary and prevents the equations from being provided in fully
            # implicit form.
            f_k = k_kqdot.LUsolve(f_k)
            k_ku = k_kqdot.LUsolve(k_ku)
            k_kqdot = eye(len(qdot))
            self._qdot_u_map = dict(zip(qdot, -(k_ku * u + f_k)))

            self._f_k = f_k.xreplace(uaux_zero)
            self._k_ku = k_ku.xreplace(uaux_zero)
            self._k_kqdot = k_kqdot
        else:
            self._qdot_u_map = None
            self._f_k = Matrix()
            self._k_ku = Matrix()
            self._k_kqdot = Matrix()
Ejemplo n.º 15
0
    def element_order(self, weylelt):
        """
        This method returns the order of a given Weyl group element, which should
        be specified by the user in the form of products of the generating
        reflections, i.e. of the form r1*r2 etc.

        For types A-F, this method current works by taking the matrix form of
        the specified element, and then finding what power of the matrix is the
        identity.  It then returns this power.

        Examples
        ========

        >>> from sympy.liealgebras.weyl_group import WeylGroup
        >>> b = WeylGroup("B4")
        >>> b.element_order('r1*r4*r2')
        4
        """
        n = self.cartan_type.rank()
        if self.cartan_type.series == "A":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(n + 1):
                a *= self.matrix_form(weylelt)
                order += 1
            return order

        if self.cartan_type.series == "D":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(n):
                a *= self.matrix_form(weylelt)
                order += 1
            return order

        if self.cartan_type.series == "E":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(8):
                a *= self.matrix_form(weylelt)
                order += 1
            return order

        if self.cartan_type.series == "G":
            elts = list(weylelt)
            reflections = elts[1::3]
            m = self.delete_doubles(reflections)
            while self.delete_doubles(m) != m:
                m = self.delete_doubles(m)
                reflections = m
            if len(reflections) % 2 == 1:
                return 2

            elif len(reflections) == 0:
                return 1

            else:
                if len(reflections) == 1:
                    return 2
                else:
                    m = len(reflections) // 2
                    lcm = (6 * m) / igcd(m, 6)
                order = lcm / m
                return order

        if self.cartan_type.series == "F":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(4):
                a *= self.matrix_form(weylelt)
                order += 1
            return order

        if self.cartan_type.series == "B" or self.cartan_type.series == "C":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(n):
                a *= self.matrix_form(weylelt)
                order += 1
            return order
Ejemplo n.º 16
0
    def matrix_form(self, weylelt):
        """
        This method takes input from the user in the form of products of the
        generating reflections, and returns the matrix corresponding to the
        element of the Weyl group.  Since each element of the Weyl group is
        a reflection of some type, there is a corresponding matrix representation.
        This method uses the standard representation for all the generating
        reflections.

        Examples
        ========

        >>> from sympy.liealgebras.weyl_group import WeylGroup
        >>> f = WeylGroup("F4")
        >>> f.matrix_form('r2*r3')
        Matrix([
        [1, 0, 0,  0],
        [0, 1, 0,  0],
        [0, 0, 0, -1],
        [0, 0, 1,  0]])

        """
        elts = list(weylelt)
        reflections = elts[1::3]
        n = self.cartan_type.rank()
        if self.cartan_type.series == "A":
            matrixform = eye(n + 1)
            for elt in reflections:
                a = int(elt)
                mat = eye(n + 1)
                mat[a - 1, a - 1] = 0
                mat[a - 1, a] = 1
                mat[a, a - 1] = 1
                mat[a, a] = 0
                matrixform *= mat
            return matrixform

        if self.cartan_type.series == "D":
            matrixform = eye(n)
            for elt in reflections:
                a = int(elt)
                mat = eye(n)
                if a < n:
                    mat[a - 1, a - 1] = 0
                    mat[a - 1, a] = 1
                    mat[a, a - 1] = 1
                    mat[a, a] = 0
                    matrixform *= mat
                else:
                    mat[n - 2, n - 1] = -1
                    mat[n - 2, n - 2] = 0
                    mat[n - 1, n - 2] = -1
                    mat[n - 1, n - 1] = 0
                    matrixform *= mat
            return matrixform

        if self.cartan_type.series == "G":
            matrixform = eye(3)
            for elt in reflections:
                a = int(elt)
                if a == 1:
                    gen1 = Matrix([[1, 0, 0], [0, 0, 1], [0, 1, 0]])
                    matrixform *= gen1
                else:
                    gen2 = Matrix(
                        [
                            [Rational(2, 3), Rational(2, 3), Rational(-1, 3)],
                            [Rational(2, 3), Rational(-1, 3), Rational(2, 3)],
                            [Rational(-1, 3), Rational(2, 3), Rational(2, 3)],
                        ]
                    )
                    matrixform *= gen2
            return matrixform

        if self.cartan_type.series == "F":
            matrixform = eye(4)
            for elt in reflections:
                a = int(elt)
                if a == 1:
                    mat = Matrix(
                        [[1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]]
                    )
                    matrixform *= mat
                elif a == 2:
                    mat = Matrix(
                        [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 1], [0, 0, 1, 0]]
                    )
                    matrixform *= mat
                elif a == 3:
                    mat = Matrix(
                        [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, -1]]
                    )
                    matrixform *= mat
                else:

                    mat = Matrix(
                        [
                            [
                                Rational(1, 2),
                                Rational(1, 2),
                                Rational(1, 2),
                                Rational(1, 2),
                            ],
                            [
                                Rational(1, 2),
                                Rational(1, 2),
                                Rational(-1, 2),
                                Rational(-1, 2),
                            ],
                            [
                                Rational(1, 2),
                                Rational(-1, 2),
                                Rational(1, 2),
                                Rational(-1, 2),
                            ],
                            [
                                Rational(1, 2),
                                Rational(-1, 2),
                                Rational(-1, 2),
                                Rational(1, 2),
                            ],
                        ]
                    )
                    matrixform *= mat
            return matrixform

        if self.cartan_type.series == "E":
            matrixform = eye(8)
            for elt in reflections:
                a = int(elt)
                if a == 1:
                    mat = Matrix(
                        [
                            [
                                Rational(3, 4),
                                Rational(1, 4),
                                Rational(1, 4),
                                Rational(1, 4),
                                Rational(1, 4),
                                Rational(1, 4),
                                Rational(1, 4),
                                Rational(-1, 4),
                            ],
                            [
                                Rational(1, 4),
                                Rational(3, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(1, 4),
                                Rational(-1, 4),
                            ],
                            [
                                Rational(1, 4),
                                Rational(-1, 4),
                                Rational(3, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(1, 4),
                            ],
                            [
                                Rational(1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(3, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(1, 4),
                            ],
                            [
                                Rational(1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(3, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(1, 4),
                            ],
                            [
                                Rational(1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(3, 4),
                                Rational(-1, 4),
                                Rational(1, 4),
                            ],
                            [
                                Rational(1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-3, 4),
                                Rational(1, 4),
                            ],
                            [
                                Rational(1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(-1, 4),
                                Rational(3, 4),
                            ],
                        ]
                    )
                    matrixform *= mat
                elif a == 2:
                    mat = eye(8)
                    mat[0, 0] = 0
                    mat[0, 1] = -1
                    mat[1, 0] = -1
                    mat[1, 1] = 0
                    matrixform *= mat
                else:
                    mat = eye(8)
                    mat[a - 3, a - 3] = 0
                    mat[a - 3, a - 2] = 1
                    mat[a - 2, a - 3] = 1
                    mat[a - 2, a - 2] = 0
                    matrixform *= mat
            return matrixform

        if self.cartan_type.series == "B" or self.cartan_type.series == "C":
            matrixform = eye(n)
            for elt in reflections:
                a = int(elt)
                mat = eye(n)
                if a == 1:
                    mat[0, 0] = -1
                    matrixform *= mat
                else:
                    mat[a - 2, a - 2] = 0
                    mat[a - 2, a - 1] = 1
                    mat[a - 1, a - 2] = 1
                    mat[a - 1, a - 1] = 0
                    matrixform *= mat
            return matrixform
Ejemplo n.º 17
0
    def orient(self, parent, rot_type, amounts, rot_order=''):
        """Defines the orientation of this frame relative to a parent frame.

        Parameters
        ==========

        parent : ReferenceFrame
            The frame that this ReferenceFrame will have its orientation matrix
            defined in relation to.
        rot_type : str
            The type of orientation matrix that is being created. Supported
            types are 'Body', 'Space', 'Quaternion', 'Axis', and 'DCM'.
            See examples for correct usage.
        amounts : list OR value
            The quantities that the orientation matrix will be defined by.
            In case of rot_type='DCM', value must be a
            sympy.matrices.MatrixBase object (or subclasses of it).
        rot_order : str or int
            If applicable, the order of a series of rotations.

        Examples
        ========

        >>> from sympy.physics.vector import ReferenceFrame, Vector
        >>> from sympy import symbols, eye, ImmutableMatrix
        >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
        >>> N = ReferenceFrame('N')
        >>> B = ReferenceFrame('B')

        Now we have a choice of how to implement the orientation. First is
        Body. Body orientation takes this reference frame through three
        successive simple rotations. Acceptable rotation orders are of length
        3, expressed in XYZ or 123, and cannot have a rotation about about an
        axis twice in a row.

        >>> B.orient(N, 'Body', [q1, q2, q3], 123)
        >>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ')
        >>> B.orient(N, 'Body', [0, 0, 0], 'XYX')

        Next is Space. Space is like Body, but the rotations are applied in the
        opposite order.

        >>> B.orient(N, 'Space', [q1, q2, q3], '312')

        Next is Quaternion. This orients the new ReferenceFrame with
        Quaternions, defined as a finite rotation about lambda, a unit vector,
        by some amount theta.
        This orientation is described by four parameters:
        q0 = cos(theta/2)
        q1 = lambda_x sin(theta/2)
        q2 = lambda_y sin(theta/2)
        q3 = lambda_z sin(theta/2)
        Quaternion does not take in a rotation order.

        >>> B.orient(N, 'Quaternion', [q0, q1, q2, q3])

        Next is Axis. This is a rotation about an arbitrary, non-time-varying
        axis by some angle. The axis is supplied as a Vector. This is how
        simple rotations are defined.

        >>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y])

        Last is DCM (Direction Cosine Matrix). This is a rotation matrix
        given manually.

        >>> B.orient(N, 'DCM', eye(3))
        >>> B.orient(N, 'DCM', ImmutableMatrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))

        """

        from sympy.physics.vector.functions import dynamicsymbols
        _check_frame(parent)

        # Allow passing a rotation matrix manually.
        if rot_type == 'DCM':
            # When rot_type == 'DCM', then amounts must be a Matrix type object
            # (e.g. sympy.matrices.dense.MutableDenseMatrix).
            if not isinstance(amounts, MatrixBase):
                raise TypeError("Amounts must be a sympy Matrix type object.")
        else:
            amounts = list(amounts)
            for i, v in enumerate(amounts):
                if not isinstance(v, Vector):
                    amounts[i] = sympify(v)

        def _rot(axis, angle):
            """DCM for simple axis 1,2,or 3 rotations. """
            if axis == 1:
                return Matrix([[1, 0, 0], [0, cos(angle), -sin(angle)],
                               [0, sin(angle), cos(angle)]])
            elif axis == 2:
                return Matrix([[cos(angle), 0, sin(angle)], [0, 1, 0],
                               [-sin(angle), 0, cos(angle)]])
            elif axis == 3:
                return Matrix([[cos(angle), -sin(angle), 0],
                               [sin(angle), cos(angle), 0], [0, 0, 1]])

        approved_orders = ('123', '231', '312', '132', '213', '321', '121',
                           '131', '212', '232', '313', '323', '')
        # make sure XYZ => 123 and rot_type is in upper case
        rot_order = translate(str(rot_order), 'XYZxyz', '123123')
        rot_type = rot_type.upper()
        if not rot_order in approved_orders:
            raise TypeError('The supplied order is not an approved type')
        parent_orient = []
        if rot_type == 'AXIS':
            if not rot_order == '':
                raise TypeError('Axis orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)):
                raise TypeError('Amounts are a list or tuple of length 2')
            theta = amounts[0]
            axis = amounts[1]
            axis = _check_vector(axis)
            if not axis.dt(parent) == 0:
                raise ValueError('Axis cannot be time-varying')
            axis = axis.express(parent).normalize()
            axis = axis.args[0][0]
            parent_orient = (
                (eye(3) - axis * axis.T) * cos(theta) +
                Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
                        [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
        elif rot_type == 'QUATERNION':
            if not rot_order == '':
                raise TypeError(
                    'Quaternion orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)):
                raise TypeError('Amounts are a list or tuple of length 4')
            q0, q1, q2, q3 = amounts
            parent_orient = (Matrix([[
                q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3),
                2 * (q0 * q2 + q1 * q3)
            ],
                                     [
                                         2 * (q1 * q2 + q0 * q3),
                                         q0**2 - q1**2 + q2**2 - q3**2,
                                         2 * (q2 * q3 - q0 * q1)
                                     ],
                                     [
                                         2 * (q1 * q3 - q0 * q2),
                                         2 * (q0 * q1 + q2 * q3),
                                         q0**2 - q1**2 - q2**2 + q3**2
                                     ]]))
        elif rot_type == 'BODY':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Body orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1]) *
                             _rot(a3, amounts[2]))
        elif rot_type == 'SPACE':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Space orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1]) *
                             _rot(a1, amounts[0]))
        elif rot_type == 'DCM':
            parent_orient = amounts
        else:
            raise NotImplementedError('That is not an implemented rotation')
        #Reset the _dcm_cache of this frame, and remove it from the _dcm_caches
        #of the frames it is linked to. Also remove it from the _dcm_dict of
        #its parent
        frames = self._dcm_cache.keys()
        dcm_dict_del = []
        dcm_cache_del = []
        for frame in frames:
            if frame in self._dcm_dict:
                dcm_dict_del += [frame]
            dcm_cache_del += [frame]
        for frame in dcm_dict_del:
            del frame._dcm_dict[self]
        for frame in dcm_cache_del:
            del frame._dcm_cache[self]
        #Add the dcm relationship to _dcm_dict
        self._dcm_dict = self._dlist[0] = {}
        self._dcm_dict.update({parent: parent_orient.T})
        parent._dcm_dict.update({self: parent_orient})
        #Also update the dcm cache after resetting it
        self._dcm_cache = {}
        self._dcm_cache.update({parent: parent_orient.T})
        parent._dcm_cache.update({self: parent_orient})
        if rot_type == 'QUATERNION':
            t = dynamicsymbols._t
            q0, q1, q2, q3 = amounts
            q0d = diff(q0, t)
            q1d = diff(q1, t)
            q2d = diff(q2, t)
            q3d = diff(q3, t)
            w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
            w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
            w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
            wvec = Vector([(Matrix([w1, w2, w3]), self)])
        elif rot_type == 'AXIS':
            thetad = (amounts[0]).diff(dynamicsymbols._t)
            wvec = thetad * amounts[1].express(parent).normalize()
        elif rot_type == 'DCM':
            wvec = self._w_diff_dcm(parent)
        else:
            try:
                from sympy.polys.polyerrors import CoercionFailed
                from sympy.physics.vector.functions import kinematic_equations
                q1, q2, q3 = amounts
                u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
                templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
                                               rot_type, rot_order)
                templist = [expand(i) for i in templist]
                td = solve(templist, [u1, u2, u3])
                u1 = expand(td[u1])
                u2 = expand(td[u2])
                u3 = expand(td[u3])
                wvec = u1 * self.x + u2 * self.y + u3 * self.z
            except (CoercionFailed, AssertionError):
                wvec = self._w_diff_dcm(parent)
        self._ang_vel_dict.update({parent: wvec})
        parent._ang_vel_dict.update({self: -wvec})
        self._var_dict = {}
Ejemplo n.º 18
0
    def orient(self, parent, rot_type, amounts, rot_order=''):
        """Defines the orientation of this frame relative to a parent frame.

        Parameters
        ==========

        parent : ReferenceFrame
            The frame that this ReferenceFrame will have its orientation matrix
            defined in relation to.
        rot_type : str
            The type of orientation matrix that is being created. Supported
            types are 'Body', 'Space', 'Quaternion', 'Axis', and 'DCM'.
            See examples for correct usage.
        amounts : list OR value
            The quantities that the orientation matrix will be defined by.
            In case of rot_type='DCM', value must be a
            sympy.matrices.MatrixBase object (or subclasses of it).
        rot_order : str
            If applicable, the order of a series of rotations.

        Examples
        ========

        >>> from sympy.physics.vector import ReferenceFrame, Vector
        >>> from sympy import symbols, eye, ImmutableMatrix
        >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
        >>> N = ReferenceFrame('N')
        >>> B = ReferenceFrame('B')

        Now we have a choice of how to implement the orientation. First is
        Body. Body orientation takes this reference frame through three
        successive simple rotations. Acceptable rotation orders are of length
        3, expressed in XYZ or 123, and cannot have a rotation about about an
        axis twice in a row.

        >>> B.orient(N, 'Body', [q1, q2, q3], '123')
        >>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ')
        >>> B.orient(N, 'Body', [0, 0, 0], 'XYX')

        Next is Space. Space is like Body, but the rotations are applied in the
        opposite order.

        >>> B.orient(N, 'Space', [q1, q2, q3], '312')

        Next is Quaternion. This orients the new ReferenceFrame with
        Quaternions, defined as a finite rotation about lambda, a unit vector,
        by some amount theta.
        This orientation is described by four parameters:
        q0 = cos(theta/2)
        q1 = lambda_x sin(theta/2)
        q2 = lambda_y sin(theta/2)
        q3 = lambda_z sin(theta/2)
        Quaternion does not take in a rotation order.

        >>> B.orient(N, 'Quaternion', [q0, q1, q2, q3])

        Next is Axis. This is a rotation about an arbitrary, non-time-varying
        axis by some angle. The axis is supplied as a Vector. This is how
        simple rotations are defined.

        >>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y])

        Last is DCM (Direction Cosine Matrix). This is a rotation matrix
        given manually.

        >>> B.orient(N, 'DCM', eye(3))
        >>> B.orient(N, 'DCM', ImmutableMatrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))

        """

        from sympy.physics.vector.functions import dynamicsymbols
        _check_frame(parent)

        # Allow passing a rotation matrix manually.
        if rot_type == 'DCM':
            # When rot_type == 'DCM', then amounts must be a Matrix type object
            # (e.g. sympy.matrices.dense.MutableDenseMatrix).
            if not isinstance(amounts, MatrixBase):
                raise TypeError("Amounts must be a sympy Matrix type object.")
        else:
            amounts = list(amounts)
            for i, v in enumerate(amounts):
                if not isinstance(v, Vector):
                    amounts[i] = sympify(v)

        def _rot(axis, angle):
            """DCM for simple axis 1,2,or 3 rotations. """
            if axis == 1:
                return Matrix([[1, 0, 0],
                    [0, cos(angle), -sin(angle)],
                    [0, sin(angle), cos(angle)]])
            elif axis == 2:
                return Matrix([[cos(angle), 0, sin(angle)],
                    [0, 1, 0],
                    [-sin(angle), 0, cos(angle)]])
            elif axis == 3:
                return Matrix([[cos(angle), -sin(angle), 0],
                    [sin(angle), cos(angle), 0],
                    [0, 0, 1]])

        approved_orders = ('123', '231', '312', '132', '213', '321', '121',
                           '131', '212', '232', '313', '323', '')
        rot_order = str(
            rot_order).upper()  # Now we need to make sure XYZ = 123
        rot_type = rot_type.upper()
        rot_order = [i.replace('X', '1') for i in rot_order]
        rot_order = [i.replace('Y', '2') for i in rot_order]
        rot_order = [i.replace('Z', '3') for i in rot_order]
        rot_order = ''.join(rot_order)
        if not rot_order in approved_orders:
            raise TypeError('The supplied order is not an approved type')
        parent_orient = []
        if rot_type == 'AXIS':
            if not rot_order == '':
                raise TypeError('Axis orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)):
                raise TypeError('Amounts are a list or tuple of length 2')
            theta = amounts[0]
            axis = amounts[1]
            axis = _check_vector(axis)
            if not axis.dt(parent) == 0:
                raise ValueError('Axis cannot be time-varying')
            axis = axis.express(parent).normalize()
            axis = axis.args[0][0]
            parent_orient = ((eye(3) - axis * axis.T) * cos(theta) +
                    Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
                        [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
        elif rot_type == 'QUATERNION':
            if not rot_order == '':
                raise TypeError(
                    'Quaternion orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)):
                raise TypeError('Amounts are a list or tuple of length 4')
            q0, q1, q2, q3 = amounts
            parent_orient = (Matrix([[q0 ** 2 + q1 ** 2 - q2 ** 2 - q3 **
                2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)],
                [2 * (q1 * q2 + q0 * q3), q0 ** 2 - q1 ** 2 + q2 ** 2 - q3 ** 2,
                2 * (q2 * q3 - q0 * q1)], [2 * (q1 * q3 - q0 * q2), 2 * (q0 *
                q1 + q2 * q3), q0 ** 2 - q1 ** 2 - q2 ** 2 + q3 ** 2]]))
        elif rot_type == 'BODY':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Body orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1])
                    * _rot(a3, amounts[2]))
        elif rot_type == 'SPACE':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Space orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1])
                    * _rot(a1, amounts[0]))
        elif rot_type == 'DCM':
            parent_orient = amounts
        else:
            raise NotImplementedError('That is not an implemented rotation')
        #Reset the _dcm_cache of this frame, and remove it from the _dcm_caches
        #of the frames it is linked to. Also remove it from the _dcm_dict of
        #its parent
        frames = self._dcm_cache.keys()
        dcm_dict_del = []
        dcm_cache_del = []
        for frame in frames:
            if frame in self._dcm_dict:
                dcm_dict_del += [frame]
            dcm_cache_del += [frame]
        for frame in dcm_dict_del:
            del frame._dcm_dict[self]
        for frame in dcm_cache_del:
            del frame._dcm_cache[self]
        #Add the dcm relationship to _dcm_dict
        self._dcm_dict = self._dlist[0] = {}
        self._dcm_dict.update({parent: parent_orient.T})
        parent._dcm_dict.update({self: parent_orient})
        #Also update the dcm cache after resetting it
        self._dcm_cache = {}
        self._dcm_cache.update({parent: parent_orient.T})
        parent._dcm_cache.update({self: parent_orient})
        if rot_type == 'QUATERNION':
            t = dynamicsymbols._t
            q0, q1, q2, q3 = amounts
            q0d = diff(q0, t)
            q1d = diff(q1, t)
            q2d = diff(q2, t)
            q3d = diff(q3, t)
            w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
            w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
            w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
            wvec = Vector([(Matrix([w1, w2, w3]), self)])
        elif rot_type == 'AXIS':
            thetad = (amounts[0]).diff(dynamicsymbols._t)
            wvec = thetad * amounts[1].express(parent).normalize()
        elif rot_type == 'DCM':
            wvec = self._w_diff_dcm(parent)
        else:
            try:
                from sympy.polys.polyerrors import CoercionFailed
                from sympy.physics.vector.functions import kinematic_equations
                q1, q2, q3 = amounts
                u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
                templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
                                               rot_type, rot_order)
                templist = [expand(i) for i in templist]
                td = solve(templist, [u1, u2, u3])
                u1 = expand(td[u1])
                u2 = expand(td[u2])
                u3 = expand(td[u3])
                wvec = u1 * self.x + u2 * self.y + u3 * self.z
            except (CoercionFailed, AssertionError):
                wvec = self._w_diff_dcm(parent)
        self._ang_vel_dict.update({parent: wvec})
        parent._ang_vel_dict.update({self: -wvec})
        self._var_dict = {}
Ejemplo n.º 19
0
    def element_order(self, weylelt):
        """
        This method returns the order of a given Weyl group element, which should
        be specified by the user in the form of products of the generating
        reflections, i.e. of the form r1*r2 etc.

        For types A-F, this method current works by taking the matrix form of
        the specified element, and then finding what power of the matrix is the
        identity.  It then returns this power.

        Examples
        ========

        >>> from sympy.liealgebras.weyl_group import WeylGroup
        >>> b = WeylGroup("B4")
        >>> b.element_order('r1*r4*r2')
        4
        """
        n = self.cartan_type.rank()
        if self.cartan_type.series == "A":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(n+1):
                a *= self.matrix_form(weylelt)
                order += 1
            return order

        if self.cartan_type.series == "D":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(n):
                a *= self.matrix_form(weylelt)
                order += 1
            return order

        if self.cartan_type.series == "E":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(8):
                a *= self.matrix_form(weylelt)
                order += 1
            return order

        if self.cartan_type.series == "G":
            elts = list(weylelt)
            reflections = elts[1::3]
            m = self.delete_doubles(reflections)
            while self.delete_doubles(m) != m:
                m = self.delete_doubles(m)
                reflections = m
            if len(reflections) % 2 == 1:
                return 2

            elif len(reflections) == 0:
                return 1

            else:
                if len(reflections) == 1:
                    return 2
                else:
                    m = len(reflections) // 2
                    lcm = (6 * m)/ igcd(m, 6)
                order = lcm / m
                return order


        if self.cartan_type.series == 'F':
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(4):
                a *= self.matrix_form(weylelt)
                order += 1
            return order


        if self.cartan_type.series == "B" or self.cartan_type.series == "C":
            a = self.matrix_form(weylelt)
            order = 1
            while a != eye(n):
                a *= self.matrix_form(weylelt)
                order += 1
            return order
Ejemplo n.º 20
0
    def orient(self, parent, rot_type, amounts, rot_order=''):
        """Sets the orientation of this reference frame relative to another
        (parent) reference frame.

        Parameters
        ==========

        parent : ReferenceFrame
            Reference frame that this reference frame will be rotated relative
            to.
        rot_type : str
            The method used to generate the direction cosine matrix. Supported
            methods are:

            - ``'Axis'``: simple rotations about a single common axis
            - ``'DCM'``: for setting the direction cosine matrix directly
            - ``'Body'``: three successive rotations about new intermediate
              axes, also called "Euler and Tait-Bryan angles"
            - ``'Space'``: three successive rotations about the parent
              frames' unit vectors
            - ``'Quaternion'``: rotations defined by four parameters which
              result in a singularity free direction cosine matrix

        amounts :
            Expressions defining the rotation angles or direction cosine
            matrix. These must match the ``rot_type``. See examples below for
            details. The input types are:

            - ``'Axis'``: 2-tuple (expr/sym/func, Vector)
            - ``'DCM'``: Matrix, shape(3,3)
            - ``'Body'``: 3-tuple of expressions, symbols, or functions
            - ``'Space'``: 3-tuple of expressions, symbols, or functions
            - ``'Quaternion'``: 4-tuple of expressions, symbols, or
              functions

        rot_order : str or int, optional
            If applicable, the order of the successive of rotations. The string
            ``'123'`` and integer ``123`` are equivalent, for example. Required
            for ``'Body'`` and ``'Space'``.

        Examples
        ========

        Setup variables for the examples:

        >>> from sympy import symbols
        >>> from sympy.physics.vector import ReferenceFrame
        >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
        >>> N = ReferenceFrame('N')
        >>> B = ReferenceFrame('B')
        >>> B1 = ReferenceFrame('B')
        >>> B2 = ReferenceFrame('B2')

        Axis
        ----

        ``rot_type='Axis'`` creates a direction cosine matrix defined by a
        simple rotation about a single axis fixed in both reference frames.
        This is a rotation about an arbitrary, non-time-varying
        axis by some angle. The axis is supplied as a Vector. This is how
        simple rotations are defined.

        >>> B.orient(N, 'Axis', (q1, N.x))

        The ``orient()`` method generates a direction cosine matrix and its
        transpose which defines the orientation of B relative to N and vice
        versa. Once orient is called, ``dcm()`` outputs the appropriate
        direction cosine matrix.

        >>> B.dcm(N)
        Matrix([
        [1,       0,      0],
        [0,  cos(q1), sin(q1)],
        [0, -sin(q1), cos(q1)]])

        The following two lines show how the sense of the rotation can be
        defined. Both lines produce the same result.

        >>> B.orient(N, 'Axis', (q1, -N.x))
        >>> B.orient(N, 'Axis', (-q1, N.x))

        The axis does not have to be defined by a unit vector, it can be any
        vector in the parent frame.

        >>> B.orient(N, 'Axis', (q1, N.x + 2 * N.y))

        DCM
        ---

        The direction cosine matrix can be set directly. The orientation of a
        frame A can be set to be the same as the frame B above like so:

        >>> B.orient(N, 'Axis', (q1, N.x))
        >>> A = ReferenceFrame('A')
        >>> A.orient(N, 'DCM', N.dcm(B))
        >>> A.dcm(N)
        Matrix([
        [1,       0,      0],
        [0,  cos(q1), sin(q1)],
        [0, -sin(q1), cos(q1)]])

        **Note carefully that** ``N.dcm(B)`` **was passed into** ``orient()``
        **for** ``A.dcm(N)`` **to match** ``B.dcm(N)``.

        Body
        ----

        ``rot_type='Body'`` rotates this reference frame relative to the
        provided reference frame by rotating through three successive simple
        rotations.  Each subsequent axis of rotation is about the "body fixed"
        unit vectors of the new intermediate reference frame. This type of
        rotation is also referred to rotating through the `Euler and Tait-Bryan
        Angles <https://en.wikipedia.org/wiki/Euler_angles>`_.

        For example, the classic Euler Angle rotation can be done by:

        >>> B.orient(N, 'Body', (q1, q2, q3), 'XYX')
        >>> B.dcm(N)
        Matrix([
        [        cos(q2),                            sin(q1)*sin(q2),                           -sin(q2)*cos(q1)],
        [sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3),  sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
        [sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])

        This rotates B relative to N through ``q1`` about ``N.x``, then rotates
        B again through q2 about B.y, and finally through q3 about B.x. It is
        equivalent to:

        >>> B1.orient(N, 'Axis', (q1, N.x))
        >>> B2.orient(B1, 'Axis', (q2, B1.y))
        >>> B.orient(B2, 'Axis', (q3, B2.x))
        >>> B.dcm(N)
        Matrix([
        [        cos(q2),                            sin(q1)*sin(q2),                           -sin(q2)*cos(q1)],
        [sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3),  sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
        [sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])

        Acceptable rotation orders are of length 3, expressed in as a string
        ``'XYZ'`` or ``'123'`` or integer ``123``. Rotations about an axis
        twice in a row are prohibited.

        >>> B.orient(N, 'Body', (q1, q2, 0), 'ZXZ')
        >>> B.orient(N, 'Body', (q1, q2, 0), '121')
        >>> B.orient(N, 'Body', (q1, q2, q3), 123)

        Space
        -----

        ``rot_type='Space'`` also rotates the reference frame in three
        successive simple rotations but the axes of rotation are the
        "Space-fixed" axes. For example:

        >>> B.orient(N, 'Space', (q1, q2, q3), '312')
        >>> B.dcm(N)
        Matrix([
        [ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
        [-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
        [                           sin(q3)*cos(q2),        -sin(q2),                           cos(q2)*cos(q3)]])

        is equivalent to:

        >>> B1.orient(N, 'Axis', (q1, N.z))
        >>> B2.orient(B1, 'Axis', (q2, N.x))
        >>> B.orient(B2, 'Axis', (q3, N.y))
        >>> B.dcm(N).simplify()  # doctest: +SKIP
        Matrix([
        [ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
        [-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
        [                           sin(q3)*cos(q2),        -sin(q2),                           cos(q2)*cos(q3)]])

        It is worth noting that space-fixed and body-fixed rotations are
        related by the order of the rotations, i.e. the reverse order of body
        fixed will give space fixed and vice versa.

        >>> B.orient(N, 'Space', (q1, q2, q3), '231')
        >>> B.dcm(N)
        Matrix([
        [cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
        [       -sin(q2),                           cos(q2)*cos(q3),                            sin(q3)*cos(q2)],
        [sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1),  sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])

        >>> B.orient(N, 'Body', (q3, q2, q1), '132')
        >>> B.dcm(N)
        Matrix([
        [cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
        [       -sin(q2),                           cos(q2)*cos(q3),                            sin(q3)*cos(q2)],
        [sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1),  sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])

        Quaternion
        ----------

        ``rot_type='Quaternion'`` orients the reference frame using
        quaternions. Quaternion rotation is defined as a finite rotation about
        lambda, a unit vector, by an amount theta. This orientation is
        described by four parameters:

        - ``q0 = cos(theta/2)``
        - ``q1 = lambda_x sin(theta/2)``
        - ``q2 = lambda_y sin(theta/2)``
        - ``q3 = lambda_z sin(theta/2)``

        This type does not need a ``rot_order``.

        >>> B.orient(N, 'Quaternion', (q0, q1, q2, q3))
        >>> B.dcm(N)
        Matrix([
        [q0**2 + q1**2 - q2**2 - q3**2,             2*q0*q3 + 2*q1*q2,            -2*q0*q2 + 2*q1*q3],
        [           -2*q0*q3 + 2*q1*q2, q0**2 - q1**2 + q2**2 - q3**2,             2*q0*q1 + 2*q2*q3],
        [            2*q0*q2 + 2*q1*q3,            -2*q0*q1 + 2*q2*q3, q0**2 - q1**2 - q2**2 + q3**2]])

        """

        from sympy.physics.vector.functions import dynamicsymbols
        _check_frame(parent)

        # Allow passing a rotation matrix manually.
        if rot_type == 'DCM':
            # When rot_type == 'DCM', then amounts must be a Matrix type object
            # (e.g. sympy.matrices.dense.MutableDenseMatrix).
            if not isinstance(amounts, MatrixBase):
                raise TypeError("Amounts must be a sympy Matrix type object.")
        else:
            amounts = list(amounts)
            for i, v in enumerate(amounts):
                if not isinstance(v, Vector):
                    amounts[i] = sympify(v)

        def _rot(axis, angle):
            """DCM for simple axis 1,2,or 3 rotations. """
            if axis == 1:
                return Matrix([[1, 0, 0], [0, cos(angle), -sin(angle)],
                               [0, sin(angle), cos(angle)]])
            elif axis == 2:
                return Matrix([[cos(angle), 0, sin(angle)], [0, 1, 0],
                               [-sin(angle), 0, cos(angle)]])
            elif axis == 3:
                return Matrix([[cos(angle), -sin(angle), 0],
                               [sin(angle), cos(angle), 0], [0, 0, 1]])

        approved_orders = ('123', '231', '312', '132', '213', '321', '121',
                           '131', '212', '232', '313', '323', '')
        # make sure XYZ => 123 and rot_type is in upper case
        rot_order = translate(str(rot_order), 'XYZxyz', '123123')
        rot_type = rot_type.upper()
        if rot_order not in approved_orders:
            raise TypeError('The supplied order is not an approved type')
        parent_orient = []
        if rot_type == 'AXIS':
            if not rot_order == '':
                raise TypeError('Axis orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)):
                raise TypeError('Amounts are a list or tuple of length 2')
            theta = amounts[0]
            axis = amounts[1]
            axis = _check_vector(axis)
            if not axis.dt(parent) == 0:
                raise ValueError('Axis cannot be time-varying')
            axis = axis.express(parent).normalize()
            axis = axis.args[0][0]
            parent_orient = (
                (eye(3) - axis * axis.T) * cos(theta) +
                Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
                        [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
        elif rot_type == 'QUATERNION':
            if not rot_order == '':
                raise TypeError(
                    'Quaternion orientation takes no rotation order')
            if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)):
                raise TypeError('Amounts are a list or tuple of length 4')
            q0, q1, q2, q3 = amounts
            parent_orient = (Matrix([[
                q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3),
                2 * (q0 * q2 + q1 * q3)
            ],
                                     [
                                         2 * (q1 * q2 + q0 * q3),
                                         q0**2 - q1**2 + q2**2 - q3**2,
                                         2 * (q2 * q3 - q0 * q1)
                                     ],
                                     [
                                         2 * (q1 * q3 - q0 * q2),
                                         2 * (q0 * q1 + q2 * q3),
                                         q0**2 - q1**2 - q2**2 + q3**2
                                     ]]))
        elif rot_type == 'BODY':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Body orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1]) *
                             _rot(a3, amounts[2]))
        elif rot_type == 'SPACE':
            if not (len(amounts) == 3 & len(rot_order) == 3):
                raise TypeError('Space orientation takes 3 values & 3 orders')
            a1 = int(rot_order[0])
            a2 = int(rot_order[1])
            a3 = int(rot_order[2])
            parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1]) *
                             _rot(a1, amounts[0]))
        elif rot_type == 'DCM':
            parent_orient = amounts
        else:
            raise NotImplementedError('That is not an implemented rotation')
        # Reset the _dcm_cache of this frame, and remove it from the
        # _dcm_caches of the frames it is linked to. Also remove it from the
        # _dcm_dict of its parent
        frames = self._dcm_cache.keys()
        dcm_dict_del = []
        dcm_cache_del = []
        for frame in frames:
            if frame in self._dcm_dict:
                dcm_dict_del += [frame]
            dcm_cache_del += [frame]
        for frame in dcm_dict_del:
            del frame._dcm_dict[self]
        for frame in dcm_cache_del:
            del frame._dcm_cache[self]
        # Add the dcm relationship to _dcm_dict
        self._dcm_dict = self._dlist[0] = {}
        self._dcm_dict.update({parent: parent_orient.T})
        parent._dcm_dict.update({self: parent_orient})
        # Also update the dcm cache after resetting it
        self._dcm_cache = {}
        self._dcm_cache.update({parent: parent_orient.T})
        parent._dcm_cache.update({self: parent_orient})
        if rot_type == 'QUATERNION':
            t = dynamicsymbols._t
            q0, q1, q2, q3 = amounts
            q0d = diff(q0, t)
            q1d = diff(q1, t)
            q2d = diff(q2, t)
            q3d = diff(q3, t)
            w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
            w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
            w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
            wvec = Vector([(Matrix([w1, w2, w3]), self)])
        elif rot_type == 'AXIS':
            thetad = (amounts[0]).diff(dynamicsymbols._t)
            wvec = thetad * amounts[1].express(parent).normalize()
        elif rot_type == 'DCM':
            wvec = self._w_diff_dcm(parent)
        else:
            try:
                from sympy.polys.polyerrors import CoercionFailed
                from sympy.physics.vector.functions import kinematic_equations
                q1, q2, q3 = amounts
                u1, u2, u3 = symbols('u1, u2, u3', cls=Dummy)
                templist = kinematic_equations([u1, u2, u3], [q1, q2, q3],
                                               rot_type, rot_order)
                templist = [expand(i) for i in templist]
                td = solve(templist, [u1, u2, u3])
                u1 = expand(td[u1])
                u2 = expand(td[u2])
                u3 = expand(td[u3])
                wvec = u1 * self.x + u2 * self.y + u3 * self.z
            except (CoercionFailed, AssertionError):
                wvec = self._w_diff_dcm(parent)
        self._ang_vel_dict.update({parent: wvec})
        parent._ang_vel_dict.update({self: -wvec})
        self._var_dict = {}
Ejemplo n.º 21
0
    def orient_axis(self, parent, axis, angle):
        """Sets the orientation of this reference frame with respect to a
        parent reference frame by rotating through an angle about an axis fixed
        in the parent reference frame.

        Parameters
        ==========

        parent : ReferenceFrame
            Reference frame that this reference frame will be rotated relative
            to.
        axis : Vector
            Vector fixed in the parent frame about about which this frame is
            rotated. It need not be a unit vector and the rotation follows the
            right hand rule.
        angle : sympifiable
            Angle in radians by which it the frame is to be rotated.

        Examples
        ========

        Setup variables for the examples:

        >>> from sympy import symbols
        >>> from sympy.physics.vector import ReferenceFrame
        >>> q1 = symbols('q1')
        >>> N = ReferenceFrame('N')
        >>> B = ReferenceFrame('B')
        >>> B.orient_axis(N, N.x, q1)

        The ``orient_axis()`` method generates a direction cosine matrix and
        its transpose which defines the orientation of B relative to N and vice
        versa. Once orient is called, ``dcm()`` outputs the appropriate
        direction cosine matrix:

        >>> B.dcm(N)
        Matrix([
        [1,       0,      0],
        [0,  cos(q1), sin(q1)],
        [0, -sin(q1), cos(q1)]])
        >>> N.dcm(B)
        Matrix([
        [1,       0,        0],
        [0, cos(q1), -sin(q1)],
        [0, sin(q1),  cos(q1)]])

        The following two lines show that the sense of the rotation can be
        defined by negating the vector direction or the angle. Both lines
        produce the same result.

        >>> B.orient_axis(N, -N.x, q1)
        >>> B.orient_axis(N, N.x, -q1)

        """

        from sympy.physics.vector.functions import dynamicsymbols
        _check_frame(parent)

        amount = sympify(angle)
        theta = amount
        axis = _check_vector(axis)
        parent_orient_axis = []

        if not axis.dt(parent) == 0:
            raise ValueError('Axis cannot be time-varying.')
        unit_axis = axis.express(parent).normalize()
        unit_col = unit_axis.args[0][0]
        parent_orient_axis = (
            (eye(3) - unit_col * unit_col.T) * cos(theta) +
            Matrix([[0, -unit_col[2], unit_col[1]],
                    [unit_col[2], 0, -unit_col[0]],
                    [-unit_col[1], unit_col[0], 0]]) *
            sin(theta) + unit_col * unit_col.T)

        self._dcm(parent, parent_orient_axis)

        thetad = (amount).diff(dynamicsymbols._t)
        wvec = thetad*axis.express(parent).normalize()
        self._ang_vel_dict.update({parent: wvec})
        parent._ang_vel_dict.update({self: -wvec})
        self._var_dict = {}