
    QiR                        d dl mZ d dlmZ d dlmZ d dlmZ d dlm	Z	m
Z
 d dlmZ d dlmZmZmZmZ d dlmZmZmZmZmZmZmZmZmZ d d	lmZ d d
lmZ  ed          \  Z Z!Z"Z#Z$ ed          Z%e%&                    dde e%j'        g          Z(e(&                    dde!e(j)        g          Z*e*&                    dde"e*j+        g          Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d Z5d Z6d Z7dS )    )pi)S)symbols)sqrt)cossin)Integral)DyadicPointReferenceFrameVector)	crossdotexpresstime_derivativekinematic_equationsouterpartial_velocityget_motion_paramsdynamicsymbols)trigsimp)raiseszq1 q2 q3 q4 q5NAAxisBCc                     t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ d S )N   r   )r   r   xyz     /var/www/development/aibuddy-work/election-extract/venv/lib/python3.11/site-packages/sympy/physics/vector/tests/test_functions.pytest_dotr&      s   qsAC==AqsAC==AqsAC==AqsAC==AqsAC==AqsAC==AqsAC==AqsAC==AqsAC==Ar$   c                  .
   t          t          j        t          j                  t	          t
                    k    sJ t          t          j        t          j                  t          t
                     k    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  t          t
                    k    sJ t          t          j        t          j                  t	          t
                    k    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  dk    sJ t          t          t          j        t          j        t          j        z                       t          d          t	          t
          t          dz  z             z  k    sJ t          t          t          j        t          j        t          j        z                       t          t          t          j        t          j        z   t          j                            k    sJ t          t          j        t          j                  t	          t                    k    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  t          t                    k    sJ t          t          j        t          j                  t          t                    t          t                    z  k    sJ t          t          j        t          j                  t	          t                    k    sJ t          t          j        t          j                  t          t                     t	          t                    z  k    sJ t          t          j        t          j                  t	          t                     t          t                    z  k    sJ t          t          j        t          j                  t          t                    k    sJ t          t          j        t          j                  t	          t                    t	          t                    z  k    sJ d S )Nr   r         )r   r   r    r   r   q1r!   r   r"   r   r   r   r   q3q2r#   r$   r%   test_dot_different_framesr-   %   s   qsAC==CGG####qsAC==SWWH$$$$qsAC==AqsAC==CGG####qsAC==CGG####qsAC==AqsAC==AqsAC==AqsAC==ACQS13Y''((DGGCRT	NN,BBBBBCQS13Y''((HSqsAC5H5H,I,IIIIIqsAC==CGG####qsAC==AqsAC==CGG####qsAC==CGGCGGO++++qsAC==CGG####qsAC==SWWHSWW,,,,,qsAC==SWWHSWW,,,,,qsAC==CGG####qsAC==CGGCGGO++++++r$   c                  x   t          t          j        t          j                  dk    sJ t          t          j        t          j                  t          j        k    sJ t          t          j        t          j                  t          j         k    sJ t          t          j        t          j                  t          j         k    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  t          j        k    sJ t          t          j        t          j                  t          j        k    sJ t          t          j        t          j                  t          j         k    sJ t          t          j        t          j                  dk    sJ d S Nr   )r   r   r    r!   r"   r#   r$   r%   
test_crossr0   >   s   ac??aac??ac!!!!ac??qsd""""ac??qsd""""ac??aac??ac!!!!ac??ac!!!!ac??qsd""""ac??ar$   c                  x   t          t          j        t          j                  t	          t
                    t          j        z  k    sJ t          t          j        t          j                  t          t
                    t          j        z  k    sJ t          t          j        t          j                  t	          t
                     t          j        z  t          t
                    t          j        z  z
  k    sJ t          t          j        t          j                  t          t
                     t          j        z  k    sJ t          t          j        t          j                  t	          t
                    t          j        z  k    sJ t          t          j        t          j                  t          t
                    t          j        z  t	          t
                    t          j        z  z
  k    sJ t          t          j        t          j                  t          j        k    sJ t          t          j        t          j                  t          j         k    sJ t          t          j        t          j                  dk    sJ t          t          j        t          j                  t	          t
                    t          j        z  k    sJ t          t          j        t          j                  t          t
                    t          j        z  k    sJ t          t          j        t          j        t          j        z             t	          t
                    t          j        z  t          t
                    t          j        z  z   k    sJ t          t          j        t          j        z   t          j                  t	          t
                     t          j        z  t          t
                    t          j        z  z
  k    sJ t          t          j        t          j                  t	          t                    t          j        z  k    sJ t          t          j        t          j                  t	          t                     t          j        z  t          t                    t          j        z  z   k    sJ t          t          j        t          j                  t          t                     t          j        z  k    sJ t          t          j        t          j                  t	          t                     t          j        z  k    sJ t          t          j        t          j                                      t                                                    t	          t                    t          j        z  t          t                    t          j        z  z
  k    sJ t          t          j        t          j                  t          t                    t          j        z  k    sJ d S r/   )r   r   r    r   r   r*   r"   r!   r   r   r+   r   simplifyr#   r$   r%   test_cross_different_framesr3   L   s   ac??c"ggack))))ac??c"ggack))))ac??s2wwhqslSWWQS[88888ac??s2wwhqsl****ac??c"ggack))))ac??c"ggackCGGACK77777ac??ac!!!!ac??qsd""""ac??aac??c"ggack))))ac??c"ggack))))acACi  CGGACK#b''!#+$=====qsAC  SWWHQSL3r7713;$>>>>>ac??c"ggack))))ac??s2wwhqslSWWQS[88888ac??s2wwhqsl****ac??s2wwhqsl****ac??""1%%..00CGGACK#b''!#+4MMMMMac??c"ggack))))))r$   c                  L   t          d          } | j        | j        z   z  t          d          t	          d          z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ t          t          fd           t          t          fd           t          t          fd           t          t          fd           z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ t          t          fd           t          t          fd           t          t          fd	           t          t          fd
           t          t          fd           t          t          fd           t          t          fd           z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ t          t          fd           t          t          fd           t          t          fd           t          t          fd           z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd            t          t          fd!           z  t                    k    sJ z  t                    k    sJ z  t                    k    sJ t          t          fd"           t          t          fd#           t          t          fd$           t          t          fd%           d&S )'zUTest that the output of dot, cross, outer functions match
    operator behavior.
    r   r   c                  8    t           t          j                  S Nr   r   Zerods   r%   <lambda>z%test_operator_match.<locals>.<lambda>u       c!QVnn r$   c                  8    t          t          j                   S r6   r7   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>v       c!&!nn r$   c                  $    t           d          S r/   r   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>w       c!Qii r$   c                  $    t          d           S r/   r@   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>x   rA   r$   c                  8    t           t          j                  S r6   r7   vs   r%   r;   z%test_operator_match.<locals>.<lambda>|   r<   r$   c                  8    t          t          j                   S r6   r7   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>}   r>   r$   c                  $    t           d          S r/   r@   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>~   rA   r$   c                  $    t          d           S r/   r@   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rA   r$   c                  $    t                      S r6   r   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eAqkk r$   c                  $    t                     S r6   rJ   r:   zerods   r%   r;   z%test_operator_match.<locals>.<lambda>       eAuoo r$   c                  $    t                     S r6   rJ   rM   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eE1oo r$   c                  8    t           t          j                  S r6   r   r   r8   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eAqv.. r$   c                  8    t          t          j                   S r6   rS   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>       eAFA.. r$   c                  $    t           d          S r/   rJ   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t          d           S r/   rJ   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  8    t           t          j                  S r6   rS   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rT   r$   c                  8    t          t          j                   S r6   rS   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rV   r$   c                  $    t           d          S r/   rJ   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t          d           S r/   rJ   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t                      S r6   r   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t                     S r6   r^   rM   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rO   r$   c                  $    t                     S r6   r^   rM   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rQ   r$   c                  $    t                     S r6   r^   r:   rE   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t                     S r6   r^   rb   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t                     S r6   r^   r:   zerovs   r%   r;   z%test_operator_match.<locals>.<lambda>   rO   r$   c                  $    t                     S r6   r^   re   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rQ   r$   c                  $    t                     S r6   r^   re   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rQ   r$   c                  8    t           t          j                  S r6   r   r   r8   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rT   r$   c                  8    t          t          j                   S r6   rj   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rV   r$   c                  $    t           d          S r/   r^   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t          d           S r/   r^   r9   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  8    t           t          j                  S r6   rj   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rT   r$   c                  8    t          t          j                   S r6   rj   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rV   r$   c                  $    t           d          S r/   r^   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   c                  $    t          d           S r/   r^   rD   s   r%   r;   z%test_operator_match.<locals>.<lambda>   rK   r$   N)
r   r    r!   r   r
   r   r   	TypeErrorr   r   )r   r:   rE   rN   rf   s    @@@@r%   test_operator_matchrs   c   sW    	sA	ac	A	AA1IIE1IIE q5C1IIu9Au%%%%19E1%%%%q5C1IIq5C1IIu9Au%%%%19E1%%%%
9,,,,---
9,,,,---
9''''(((
9''''(((q5C1IIu9Au%%%%19E1%%%%
9,,,,---
9,,,,---
9''''(((
9''''((( 9))))***
9-----...
9-----...q5E!QKKq5E!QKKu9a''''19eQ''''19eQ''''
9....///
9....///
9))))***
9))))***q5E!QKKu9a''''19eQ''''
9....///
9....///
9))))***
9))))*** 9))))***
9-----...
9-----...
9)))))***
9)))))***
9-----...
9-----...
9-----...
9....///
9....///
9))))***
9))))***q5E!QKKu9a''''19eQ''''
9....///
9....///
9))))***
9))))*****r$   c                  S   t          t          d          t                    t          d          k    sJ t          t          j        t                    t          j        u sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          t                    t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z
  k    sJ t          t
          j        t                    t          t                     t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z   k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          t                     t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          j        k    sJ t          t
          j        t
                    t
          j        k    sJ t          t
          j        t
                    t
          j        k    sJ t          t
          j        t
                    t
          j        k    sJ t          t
          j        t                    t          j        k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z
  k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          t                    t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z
  k    sJ t          t
          j        t                    t          t                     t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z   k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t
                    t          t                    t
          j        z  t          t                    t
          j        z  z
  k    sJ t          t          j        t
                    t          t                    t
          j        z  t          t                    t
          j        z  z   k    sJ t          t          j        t
                    t
          j        k    sJ t          t          j        t                    t          t                    t          j        z  t          t                    t          t                    z  t          j        z  z
  t          t                    t          t                    z  t          j        z  z   k    sJ t          t          j        t                    t          t                    t          j        z  t          t                    t          t                    z  t          j        z  z   t          t                    t          t                    z  t          j        z  z
  k    sJ t          t          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t          j        t                    t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z
  t          j        z  t          t                    t          t                    z  t          j        z  z
  t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z   t          j        z  z   k    sJ t          t          j        t                    t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z   t          j        z  t          t                    t          t                    z  t          j        z  z   t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z
  t          j        z  z   k    sJ t          t          j        t                    t          t                     t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z   k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          t                     t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          j        k    sJ t          t
          j        t
                    t
          j        k    sJ t          t
          j        t
                    t
          j        k    sJ t          t
          j        t
                    t
          j        k    sJ t          t
          j        t                    t          j        k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z
  k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t
          j        t                    t          t                    t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z
  k    sJ t          t
          j        t                    t          t                     t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z   k    sJ t          t          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t          j        t                    t          t                     t          t                    z  t          j        z  t          t                    t          t                    z  t          j        z  z   t          t                    t          j        z  z   k    sJ t          t          j        t                    t          t                    t          t                    z  t          j        z  t          t                    t          t                    z  t          j        z  z
  t          t                    t          j        z  z   k    sJ t          t          j        t
                    t
          j        k    sJ t          t          j        t
                    t          t                    t
          j        z  t          t                    t
          j        z  z   k    sJ t          t          j        t
                    t          t                     t
          j        z  t          t                    t
          j        z  z   k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          t                     t          j        z  t          t                    t          j        z  z   k    sJ t          t          j        t                    t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z
  t          j        z  t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z   t          j        z  z   t          t                    t          t                    z  t          j        z  z
  k    sJ t          t          j        t                    t          t                     t          t                    z  t          j        z  t          t                    t          t                    z  t          j        z  z   t          t                    t          j        z  z   k    sJ t          t          j        t                    t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z   t          j        z  t          t                    t          t                    z  t          t                    t          t                    z  t          t                    z  z
  t          j        z  z   t          t                    t          t                    z  t          j        z  z   k    sJ t          t          j        t
                    t          t                    t
          j        z  t          t                    t          t                    z  t
          j        z  z   t          t                    t          t                    z  t
          j        z  z
  k    sJ t          t          j        t
                    t          t                    t
          j        z  t          t                    t
          j        z  z   k    sJ t          t          j        t
                    t          t                    t
          j        z  t          t                    t          t                    z  t
          j        z  z
  t          t                    t          t                    z  t
          j        z  z   k    sJ t          t          j        t                    t          t                    t          j        z  t          t                    t          j        z  z
  k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          t                    t          j        z  t          t                    t          j        z  z   k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          j        k    sJ t          t          j        t                    t          j        cxk    rt          j        k    sn J t          j        t          t          t                    t
          j        z  t          t                    t
          j        z  z
  t                                                    k    sJ t          j        t          t          t                    t
          j        z  t          t                    t
          j        z  z   t                                                    k    sJ t          j        t          t          t                    t          j        z  t          t                    t          t                    z  t          j        z  z
  t          t                    t          t                    z  t          j        z  z   t                                                    k    sJ t          j        t          t          t                    t          j        z  t          t                    t          t                    z  t          j        z  z   t          t                    t          t                    z  t          j        z  z
  t                                                    k    sJ t          j        t          t          t                    t          j        z  t          t                    t          j        z  z   t                                                    k    sJ 	 t
          j        t          t          t                    t          j        z  t          t                    t          j        z  z   t
                                                    k    sJ t
          j        t          t          t                     t          j        z  t          t                    t          j        z  z   t
                                                    k    sJ t
          j        t          t          t                    t          j        z  t          t                    t          j        z  z
  t
                                                    k    sJ t
          j        t          t          t                    t          j        z  t          t                    t          j        z  z   t
                                                    k    sJ t
          j        t          t          t                    t          j        z  t          t                    t          j        z  z   t
                                                    k    sJ t
          j        t          t          t                    t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z
  t
                                                    k    sJ t
          j        t          t          t                     t          t                    z  t          j        z  t          t                    t          j        z  z   t          t                    t          t                    z  t          j        z  z   t
                                                    k    sJ t          j        t          t          t                    t          j        z  t          t                    t          j        z  z   t                                                    k    sJ t          j        t          t          t                     t          t                    z  t          j        z  t          t                    t          t                    z  t          j        z  z   t          t                    t          j        z  z   t                                                    k    sJ t          j        t          t          t                    t          t                    z  t          j        z  t          t                    t          t                    z  t          j        z  z
  t          t                    t          j        z  z   t                                                    k    sJ t          j        t          t          t                    t
          j        z  t          t                    t
          j        z  z   t                                                    k    sJ t          j        t          t          t                     t
          j        z  t          t                    t
          j        z  z   t                                                    k    sJ t          j        t          t          t                    t          j        z  t          t                    t          j        z  z   t                                                    k    sJ t          j        t          t          t                     t          j        z  t          t                    t          j        z  z   t                                                    k    sJ 	 t          j        t          t          t                    t
          j        z  t          t                    t          t                    z  t
          j        z  z   t          t                    t          t                    z  t
          j        z  z
  t                                                    k    sJ t          j        t          t          t                    t
          j        z  t          t                    t
          j        z  z   t                                                    k    sJ t          j        t          t          t                    t
          j        z  t          t                    t          t                    z  t
          j        z  z
  t          t                    t          t                    z  t
          j        z  z   t                                                    k    sJ t          j        t          t          t                    t          j        z  t          t                    t          j        z  z
  t                                                    k    sJ t          j        t          t          t                    t          j        z  t          t                    t          j        z  z   t                                                    k    sJ d S r/   )r   r   r   r   r8   r   r    r   r   r+   r   r"   r!   r,   r*   r   r2   r#   r$   r%   test_expressru      s	   6!99a  F1II----161''''13??c"ggackCGGACK7777713??c"ggc"ggoac1CGGACK?BB    13??s2wwhs2ww.qs2SWWQS[@BB    13??c"ggackCGGACK7777713??s2wwhqslSWWQS[8888813??ac!!!!13??ac!!!!13??ac!!!!13??ac!!!!13??ac!!!!13??c"ggackCGGACK7777713??c"ggackCGGACK7777713??c"ggackCGGACK7777713??c"ggc"ggoac1CGGACK?BB    13??s2wwhs2ww.qs2SWWQS[@BB     13??ac!!!!13??ac!!!!13??ac!!!!13??s2wwqs{SWWQS[8999913??s2wwqs{SWWQS[8999913??ac!!!!13??s2wwqs{SWWSWW_QS-@@GGCGGOAC   ! ! ! !13??s2wwqs{SWWSWW_QS-@@GGCGGOAC   ! ! ! !13??s2wwqs{SWWQS[8999913??	RR3r773r77?3r772	2AC7BB		RR3r773r77?3r772	2AC7	89 9 9 9 13??	RR3r773r77?3r772	2AC7BB		RR3r773r77?3r772	2AC7	89 9 9 9 13??BxB/3c"ggackAGGCGGOAC   ! ! ! ! 13??s2wwqs{SWWQS[8999913??Bx|c"ggack9::::13??ac!!!!13??ac!!!!13??ac!!!!13??ac!!!!13??ac!!!!13??s2wwqs{SWWQS[8999913??s2wwqs{SWWQS[8999913??s2wwqs{SWWQS[8999913??s2wws2wwqs2SWWQS[@GGCGGOAC   ! ! ! !13??BxB/3c"ggackAGGCGGOAC   ! ! ! ! 13??s2wwqs{SWWQS[8999913??BxB/3GGCGGOAC  "%b''!#+ . / / / /13??s2wws2wwqs2GGCGGOAC  "%b''!#+ . / / / /13??ac!!!!13??s2wwqs{SWWQS[8999913??Bx|c"ggack9::::13??ac!!!!13??ac!!!!13??ac!!!!13??s2wwqs{SWWQS[8999913??ac!!!!13??Bx|c"ggack9::::13??	RR3r773r77?3r772	2AC7	RR3r773r77?3r772	2AC7	8BB	    13??	RRs2wws2wwqs22SWWQS[@B B B B13??	RR3r773r77?3r772	2AC7	RR3r773r77?3r772	2AC7	8BB	    13??s2wwqs{SWWSWW_QS-@@GGCGGOAC   ! ! ! !13??s2wwqs{SWWQS[8999913??s2wwqs{SWWSWW_QS-@@GGCGGOAC   ! ! ! !13??s2wwqs{SWWQS[8999913??ac!!!!13??s2wwqs{SWWQS[8999913??ac!!!!13??ac!!!!13??ac****ac****** 3'3r7713;R4q99BBDDDDDD3'3r7713;R4q99BBDDDDDD3'3r7713;RR)<<GGCGGOAC "#% %%-XZZ0 0 0 03'3r7713;RR)<<BB! !!), , , ,3'3r7713;R4q99BBDDDDDD 3'3r7713;R4q99BBDDDDDD3'CGG8AC<#b''!#+5::CCEEEEEE3'3r7713;R4q99BBDDDDDD3'3r7713;R4q99BBDDDDDD3'3r7713;R4q99BBDDDDDD
 3'3r773r77?13.R<GGCGGOAC "#% %%-XZZ0 0 0 0 3'CGG8CGG+AC/#b''!#+=GGCGGOAC "#% %%-XZZ0 0 0 03'3r7713;R4q99BBDDDDDD3'CGG8CGG+AC/GGCGGOAC "%b''!#+.013 33;8::> > > > 3'3r773r77?13.GGCGGOAC "%b''!#+.013 33;8::> > > > 3'3r7713;R4q99BBDDDDDD3'CGG8AC<#b''!#+5::CCEEEEEE3'3r7713;R4q99BBDDDDDD3'CGG8AC<#b''!#+5::CCEEEEEE 3'3r7713;RR)<<GGCGGOAC "#% %%-XZZ0 0 0 03'3r7713;R4q99BBDDDDDD3'3r7713;RR)<<GGCGGOAC "#% %%-XZZ0 0 0 03'3r7713;R4q99BBDDDDDD3'3r7713;R4q99BBDDDDDDDDr$   c                  F	   t          d          } t          d          }t          dd          }|                     dd|| j        g          | j        | j        z  }t          |          | | j        | j        z  z  | | j        | j        z  z  z   k    sJ | j        j        z  }t          ||           | | j        j        z  z  k    sJ t          |          | | j        j        z  z  k    sJ | j        j        z  }t          ||           || j        j        z  z  k    sJ t          |          | | j        j        z  z  k    sJ | j        j        z  }t          ||           dk    sJ t          |          | | j        j        z  z  k    sJ t          d          \  }}}	}
t          dd          \  }}}}t          dd          \  }}}}                    d	d|
j        g          || j        z  }|| j        z  |	j        z  z   }|| j        z  || j        z  z   |	| j        z  z   }t          j                  dk    sJ t          j                  | j        z  k    sJ t          j                  |j        z  k    sJ t          |          || j        z  k    sJ t          |          | t          |          z  |z  | j        z  |t          |          z  |z  | j        z  z   || j        z  z   k    sJ t          ||           || j        z  |	|z  j        z  z
  |j        z  z   k    sJ t          |          || j        z  ||z  | j        z  z
  |t          |          z  |z  | j        z  z   |j        z  z   |	|z  j        z  z
  k    sJ t          |          ||z  |z   | j        z  | |z  |z   | j        z  z   || j        z  z   k    sJ t          |          | | j        | j        z  z  t          |          |z  | j        | j        z  z  z   || j        | j        z  z  z
  t          |          |z  | j        | j        z  z  z   k    sJ t          t          fd
           t          t          fd           d S )Nr   qr   r   r   r   zq1 q2 q3 q4r(   r   c                  2    t           j        d          S )N      ?orderr   r    r   r   s   r%   r;   z&test_time_derivative.<locals>.<lambda>}  s    qsASAAA r$   c                  2    t           j        d          S )Nrz   r|   r}   s   r%   r;   z&test_time_derivative.<locals>.<lambda>~  s    qsAR@@@ r$   )r   r   	orientnewr"   r    r   r!   r   r   r   
ValueError)r   rw   qdr:   d1d2d3r*   r,   r+   q4q1dq2dq3dq4dq1ddq2ddq3ddq4ddv1v2v3r   r   s                         @@r%   test_time_derivativer   V  s	    	sAsA	Q		B	C!QS**A	ac	A1a  bSQS13Y$7CAC!#I%    	
qsB2q!!rT13qs7^33332q!!rT13qs7^3333	
qsB2q!!RQS\11112q!!rT13qs7^3333	
qsB2q!!Q&&&&2q!!rT13qs7^3333#M22NBB'q99Cc3+M1==D$d	C"ac++A	acB	AC"QS&B	AC"QS&2ac6	!B13""a''''13""eACi////13""c!#g----2q!!SW,,,,2q!!rT#a&&[_QS%8c!ff9S=&"13w&' ' ' ' '2q!!SWr"uQSy%83qs7%BBBBB2q!!SWr"uQSy%8c!ff9S=&"13w&')+C&4 4 4 4 42q!!beck13%6CFSL!#& #AC&( ( ( ( (1a  bD!#ac'Nq66#:qs13w% "$ac!#g,%/14QQSW1E%F F F F F
:AAAAABBB
:@@@@@AAAAAr$   c            	      	   t           j        } t          d          \  }}}t          d          \  }}}t          d          \  }}}	t          d          \  }
}t          d          \  }}}t          dd          \  }}}t          dd          \  }}}|t          j        z  |t          j        z  z   |t          j        z  z   }|t          j        z  |t          j        z  z   |	t          j        z  z   }|t          j        z  |t          j        z  z   |t          j        z  z   }|t          j        z  |t          j        z  z   |t          j        z  z   }|t          j        z  |t          j        z  z   |t          j        z  z   }|t          j        z  |t          j        z  z   |t          j        z  z   }t          t                    d	k    sJ t          t          |
          dd|fk    sJ t          t          |
          |||fk    sJ t          t          |          d||| z  fk    sJ t          t          |||
          d|||| |
z
  z  z   fk    sJ t          t          |||
          }d||| z  ||
z  z
  |                    | |
          z   f}||k    sJ t          t          |||
          }t          || |
| f          t          j        z  t          || |
| f          t          j        z  z   t          || |
| f          t          j        z  z   }||||z   f}||k    sJ t          t          |          ||| z  || dz  z  dz  fk    sJ t          t          ||||
|          |||| z  z   ||z  z
  | |
z  || dz  z  dz  z   ||z  |
z  z   ||
dz  z  dz  z
  | |||z  z
  z  z   |                    | |
          z   fk    sJ t          t          ||||
|          |||| z  z   ||z  z
  | |
z  |z   || dz  z  dz  z   ||z  |
z  z   ||
dz  z  dz  z
  | |||z  z
  z  z   fk    sJ t          t          |t          j        z  |t          j        z  |t          j        z  |
|          }t          || || f          }|t          j        z  ||z   t          j        z  |t          ||z   | |
| f          z   t          j        z  f}||k    sJ d S )Nzs1 s2 s3zS1 S2 S3zS4 S5 S6zt1 t2za b cr   r(   )framer   r   r   )positionr   )velocity)r   r   
timevalue1)acceleration)r   r   r   r   
timevalue2)
r   _tr   r   r    r!   r"   r   subsr	   )ts1s2s3S1S2S3S4S5S6t1t2abcadbdcda2db2dc2dv0v01r   r   v2dv2ddansweranswer_expectedintegral_vectori1s                                  r%   test_get_motion_methodsr     s<   A$$JBB$$JBB$$JBBWFBW%%GAq!++JBB"7A..MCc	AC"QS&2ac6	!B
QS&2ac6/BqsF
"C	AC"QS&2ac6	!B	
131313	B
QS&2ac6/BqsF
"Cqs7SWs13w&DQ'''94444Q,,,Ar
::::Q,,,sB????Q,,,BQ????QbRHHHr2AF#$% % % %q2rJJJF"bdRUlRWWQ^^;<O_$$$$q2rJJJFq1b!*--ac1HQB
4K4KAC4OOq1b!*%%ac)*OB_ 45O_$$$$ QR000QQT	!$% % % %QR"#%"E E EbdRU"CFR1WQYBr)r1uHQJBBJ(GGArNN    QR"&)bRI I IRT	BrE!CFSL2ad719$rE"H"a%xz"rBrEzN    qqur!#v#%ac6bRI I IF	!aQZ	 	 BurBwm	hrBwB
++	+QS02O_$$$$$$r$   c            
        	
 t          d          \  	t          dd          \  } }}}t          d          \  
t          
g	gdd          }|t          
g	gdd          k    sJ t          
g	gd          }|dz  
z  d	z  z  z
  d		z  z  z   |z   dz  z  d	z  z  z   d		z  
z  z
  |z   dz  z  d	z  z  z
  d	z  
z  z   |z   d	z  
z  d	z  z  z   d		z  z  z   | z   gk    sJ t          t          
fd
           t          t          	
fd           t          t          	
fd           t          t          	
fd           t          t          	
fd           t          t          
fd           t          t          	
fd           t          t          	
fd           t          t          
fd           t          g dgd          t
          j        t
          j        t
          j        gk    sJ d S )Nzq0 q1 q2 q3r   zu1 u2 u3bodyi9  313
quaterniong      ry   c                  2    t          g gd          S Nr   r   q0r*   r,   u1u2u3s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s     2BB<"b"|\\ r$   c                  6    t          g gdd          S )Nr   123r   r   r*   r,   r+   r   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s'    2BB<"b"bAQS_afgg r$   c                  4    t          g gd          S )Nfoor   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s$    2BB<"b"bAQSXYY r$   c                  .    t           gd          S r   r   r   r*   r,   r+   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s    1"r2r26FUU r$   c                  0    t          g gd          S r   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s    12$RR8H,WW r$   c                  ,    t          g d          S r   r   )r   r   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s    12r2,LQQ r$   c                  4    t          g gd          S )Nr   r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s$    2BB<"b"bAQSYZZ r$   c                  4    t          g gd          S )Nspacer   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s$    2BB<"b"bAQSZ[[ r$   c                  4    t          g gdd          S )Nr   222r   r   s   r%   r;   ztest_kin_eqs.<locals>.<lambda>  s#    2BB<"b"vW\]] r$   r   r   )r   r   r   r   rr   r   r8   )q0dr   r   r   kekdsr   r*   r,   r+   r   r   r   s         @@@@@@@r%   test_kin_eqsr     s+   #M22NBB'q99Cc3
++JBB	bBZ"RVS	A	AB$bBZ"RVUKKKKKK
r2rlRR,<l
K
KC4"9r>C"HrM1C"HrMACG2INS2X]*S2X]:S@2INS2X]*S2X]:S@"HrMC"HrM)C"HrM9C?A A A A A :\\\\\\\\\]]]
:gggggggggghhh
:YYYYYYYYYYZZZ
9UUUUUUUUVVV
9WWWWWWWWXXX
9QQQQQQQRRR
:ZZZZZZZZZZ[[[
:[[[[[[[[[[\\\
:]]]]]]]]]^^^yyy2r2,@@QVQVUVU[D\\\\\\\r$   c            
         t          d          \  } }}}}t          d          \  }}t          d          }t          d                              dd| j        g          }|                    dd||j        g          }	|	                    dd||	j        g          }
|
                    |	j        z  ||	j        z  z   ||	j        z  z              t          d	          }|	                    ||	j        z  ||j        |	j        z  z  z              |
                    d
||	j        z                                ||
                                         |                              |
                              g||||gt                    | |	j        z  ||	j        z  d|	j        t          |          |	j        z  t!          |          |	j        z  z
  gddd|	j        t          |          |	j        z  t!          |          |	j        z  z
  g|	j        |	j        |	j        ddggk    sJ t          d          }t          d          }||j        z  ||j        z  z   }t          |f||f|          |j        |j        ggk    sJ t#          t$          fd           t#          t$          fd           d S )Nzq1 q2 q3 u1 u2 u3zu4, u5rr   Yr   LRr   Dmcr   r   r   c                  L    t                                                   S r6   )r   vel)r   r   u_lists   r%   r;   z'test_partial_velocity.<locals>.<lambda>  s    .swwqzz61EE r$   c                  &    t                     S r6   )r   )r   r   vel_lists   r%   r;   z'test_partial_velocity.<locals>.<lambda>  s    .xQ?? r$   )r   r   r   r   r"   r    r!   set_ang_velr   set_vel	locatenewv2pt_theoryr   
ang_vel_inr   r   r   r   rr   )r*   r,   r+   r   r   u4u5r   r   r   r   r   r   r   rE   r   r   r   r   r   s                  @@@@@r%   test_partial_velocityr     s   +,?@@BBBH%%FBAsA	C"ac++A	C"ac++A	C"ac++AMM!R!#XQS(283444c

AIIaacB!#),,---
++eQW
%
%COOAq!

AEE!HHall1oo6H"b"b!FXvq11c!#gquac"ggackCGGACK&?@1ac3r7713;R45S!#qsAq!## # # # 	sAsA
QS28AQEB8Q//QS!#J<????
9EEEEEEFFF
9??????@@@@@r$   c                     t          d          } t          dd          }t          dd          }t          dd	          \  }}t          d
d          }| j        J |j        sJ |j        sJ ||z  ||z  k    sJ |j        sJ d S )Nf1f2T)realf3)positivezf4,f5F)commutativef6)integer)r   is_realis_positive
is_integer)r   r   r   f4f5r   s         r%   test_dynamicsymbolsr     s    			B	4	(	(	(B	t	,	,	,BG777FB	d	+	+	+B::>b5BrE>>>>=r$   N)8sympy.core.numbersr   sympy.core.singletonr   sympy.core.symbolr   (sympy.functions.elementary.miscellaneousr   (sympy.functions.elementary.trigonometricr   r   sympy.integrals.integralsr	   sympy.physics.vectorr
   r   r   r   sympy.physics.vector.functionsr   r   r   r   r   r   r   r   r   sympy.simplifyr   sympy.testing.pytestr   r*   r,   r+   r   q5r   r   r"   r   r    r   r!   r   r&   r-   r0   r3   rs   ru   r   r   r   r   r   r#   r$   r%   <module>r     s   ! ! ! ! ! ! " " " " " " % % % % % % 9 9 9 9 9 9 ? ? ? ? ? ? ? ? . . . . . . F F F F F F F F F F F FO O O O O O O O O O O O O O O O O O O O O O
 $ # # # # # ' ' ' ' ' 'W-.. BBN3KKVb!#Y''KKVb!#Y''KKVb!#Y''  , , ,2     * * *.F+ F+ F+RgE gE gET(B (B (BV6% 6% 6%r] ] ].A A AB    r$   