
    MZdQ                        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
e_         ed      \  ZZ Z!Z"Z# ed      Z$e$jK                  ddee$jL                  g      Z'e'jK                  dde e'jP                  g      Z)e)jK                  dde!e)jT                  g      Z+d Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d Z5d Z6y)    )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)raisesTz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 y )N   r   )r   r   xyz     K/usr/lib/python3/dist-packages/sympy/physics/vector/tests/test_functions.pytest_dotr%      s    qssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=AqssACC=Ar#   c                  X	   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        j                  t        j                  t        j                  z         t        d      t	        t
        t        dz  z         z  cxk(  r=t        t        j                  t        j                  z   t        j                        k(  sJ  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 y )Nr   r         )r   r   r   r   r   q1r    r   r!   r   r   r   q3q2r"   r#   r$   test_dot_different_framesr,   %   s   qssACC=CG###qssACC=SWH$$$qssACC=AqssACC=CG###qssACC=CG###qssACC=AqssACC=AqssACC=AqssACC=AqssACC!##I$q'#b2a4i."8OCacc	133<OOOOOOqssACC=CG###qssACC=AqssACC=CG###qssACC=CGCGO+++qssACC=CG###qssACC=SWHSW,,,,qssACC=SWHSW,,,,qssACC=CG###qssACC=CGCGO+++r#   c                     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 y Nr   )r   r   r   r    r!   r"   r#   r$   
test_crossr/   =   s   acc?aacc?acc!!!acc?qssd"""acc?qssd"""acc?aacc?acc!!!acc?acc!!!acc?qssd"""acc?ar#   c                     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        j                  z  t        t              t        j                  z  z
  k(  sJ t        t        j                  t        j                        t        t              t        j                  z  k(  sJ y r.   )r   r   r   r   r   r)   r!   r    r   r   r*   r"   r#   r$   test_cross_different_framesr1   K   sd   acc?c"gacck)))acc?c"gacck)))acc?s2whqsslSWQSS[8888acc?s2whqssl***acc?c"gacck)))acc?c"gacckCGACCK7777acc?acc!!!acc?qssd"""acc?aacc?c"gacck)))acc?c"gacck)))accACCi CGACCK#b'!##+$====qssACC SWHQSSL3r7133;$>>>>acc?c"gacck)))acc?s2whqsslSWQSS[8888acc?s2whqssl***acc?s2whqssl***acc?c"gacckCGACCK7777acc?c"gacck)))r#   c                  	   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%       y&)'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>t       c!QVVn r#   c                  8    t        t        j                         S r4   r5   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>u       c!&&!n r#   c                      t         d      S r.   r   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>v       c!Qi r#   c                      t        d       S r.   r>   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>w   r?   r#   c                  8    t         t        j                        S r4   r5   vs   r$   r9   z%test_operator_match.<locals>.<lambda>{   r:   r#   c                  8    t        t        j                         S r4   r5   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>|   r<   r#   c                      t         d      S r.   r>   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>}   r?   r#   c                      t        d       S r.   r>   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>~   r?   r#   c                      t                S r4   r   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>       eAqk r#   c                      t               S r4   rH   r8   zerods   r$   r9   z%test_operator_match.<locals>.<lambda>       eAuo r#   c                      t               S r4   rH   rK   s   r$   r9   z%test_operator_match.<locals>.<lambda>       eE1o r#   c                  8    t         t        j                        S r4   r   r   r6   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>       eAqvv. r#   c                  8    t        t        j                         S r4   rQ   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>       eAFFA. r#   c                      t         d      S r.   rH   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t        d       S r.   rH   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                  8    t         t        j                        S r4   rQ   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rR   r#   c                  8    t        t        j                         S r4   rQ   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rT   r#   c                      t         d      S r.   rH   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t        d       S r.   rH   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t                S r4   r   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t               S r4   r\   rK   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rM   r#   c                      t               S r4   r\   rK   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rO   r#   c                      t               S r4   r\   r8   rC   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t               S r4   r\   r`   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t               S r4   r\   r8   zerovs   r$   r9   z%test_operator_match.<locals>.<lambda>   rM   r#   c                      t               S r4   r\   rc   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rO   r#   c                      t               S r4   r\   rc   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rO   r#   c                  8    t         t        j                        S r4   r   r   r6   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rR   r#   c                  8    t        t        j                         S r4   rh   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rT   r#   c                      t         d      S r.   r\   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t        d       S r.   r\   r7   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                  8    t         t        j                        S r4   rh   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rR   r#   c                  8    t        t        j                         S r4   rh   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rT   r#   c                      t         d      S r.   r\   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   c                      t        d       S r.   r\   rB   s   r$   r9   z%test_operator_match.<locals>.<lambda>   rI   r#   N)
r   r   r    r   r
   r   r   	TypeErrorr   r   )r   r8   rC   rL   rd   s    @@@@r$   test_operator_matchrq   b   s    	sA	acc	A	AA1IE1IE q5C1Iu9Au%%%19E1%%%q5C1Iq5C1Iu9Au%%%19E1%%%
9,-
9,-
9'(
9'(q5C1Iu9Au%%%19E1%%%
9,-
9,-
9'(
9'( 9)*
9-.
9-.q5E!QKq5E!QKu9a'''19eQ'''19eQ'''
9./
9./
9)*
9)*q5E!QKu9a'''19eQ'''
9./
9./
9)*
9)* 9)*
9-.
9-.
9)*
9)*
9-.
9-.
9-.
9./
9./
9)*
9)*q5E!QKu9a'''19eQ'''
9./
9./
9)*
9)*r#   c                  K   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J  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 y r.   )r   r   r   r   r6   r   r   r   r   r*   r   r!   r    r+   r)   r   r"   r#   r$   test_expressrs      s   6!9a F1I---1661'''133?c"gacckCGACCK7777133?c"gc"goacc1CGACCK?BB   133?s2whs2w.qss2SWQSS[@BB   133?c"gacckCGACCK7777133?s2whqsslSWQSS[8888133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?c"gacckCGACCK7777133?c"gacckCGACCK7777133?c"gacckCGACCK7777133?c"gc"goacc1CGACCK?BB   133?s2whs2w.qss2SWQSS[@BB    133?acc!!!133?acc!!!133?acc!!!133?s2wqss{SWQSS[8999133?s2wqss{SWQSS[8999133?acc!!!133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWQSS[8999133?	RR3r73r7?3r72	2ACC7BB		RR3r73r7?3r72	2ACC7	89 9 9 133?	RR3r73r7?3r72	2ACC7BB		RR3r73r7?3r72	2ACC7	89 9 9 133?BxB/3c"gacckAGCGOACC   ! ! ! 133?s2wqss{SWQSS[8999133?Bx|c"gacck9:::133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?acc!!!133?s2wqss{SWQSS[8999133?s2wqss{SWQSS[8999133?s2wqss{SWQSS[8999133?s2ws2wqss2SWQSS[@GCGOACC   ! ! !133?BxB/3c"gacckAGCGOACC   ! ! ! 133?s2wqss{SWQSS[8999133?BxB/3GCGOACC  "%b'!##+ . / / /133?s2ws2wqss2GCGOACC  "%b'!##+ . / / /133?acc!!!133?s2wqss{SWQSS[8999133?Bx|c"gacck9:::133?acc!!!133?acc!!!133?acc!!!133?s2wqss{SWQSS[8999133?acc!!!133?Bx|c"gacck9:::133?	RR3r73r7?3r72	2ACC7	RR3r73r7?3r72	2ACC7	8BB	   133?	RRs2ws2wqss22SWQSS[@B B B133?	RR3r73r7?3r72	2ACC7	RR3r73r7?3r72	2ACC7	8BB	   133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWQSS[8999133?s2wqss{SWSW_QSS-@@GCGOACC   ! ! !133?s2wqss{SWQSS[8999133?acc!!!133?s2wqss{SWQSS[8999133?acc!!!133?acc!!!133?acc*acc***** 33'3r7133;R4q999933'3r7133;R4q999933'3r7133;RR)<<GCGOACC "#% % % %33'3r7133;RR)<<BB! ! ! !33'3r7133;R4q9999 33'3r7133;R4q999933'CG8ACC<#b'!##+5::::33'3r7133;R4q999933'3r7133;R4q999933'3r7133;R4q9999
 33'3r73r7?133.R<GCGOACC "#% % % % 33'CG8CG+ACC/#b'!##+=GCGOACC "#% % % %33'3r7133;R4q999933'CG8CG+ACC/GCGOACC "%b'!##+.013 3 3 3 33'3r73r7?133.GCGOACC "%b'!##+.013 3 3 3 33'3r7133;R4q999933'CG8ACC<#b'!##+5::::33'3r7133;R4q999933'CG8ACC<#b'!##+5:::: 33'3r7133;RR)<<GCGOACC "#% % % %33'3r7133;R4q999933'3r7133;RR)<<GCGOACC "#% % % %33'3r7133;R4q999933'3r7133;R4q9999r#   c                  l
   t        d      } t        d      }t        dd      }| j                  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      \  }}}}j                  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       y )Nr   qr   r   r   r   zq1 q2 q3 q4r'   r   c                  4    t         j                  d      S )N      ?orderr   r   r   r   s   r$   r9   z&test_time_derivative.<locals>.<lambda>|  s    qssASA r#   c                  4    t         j                  d      S )Nrx   rz   r{   s   r$   r9   z&test_time_derivative.<locals>.<lambda>}  s    qssAR@ r#   )r   r   	orientnewr!   r   r   r    r   r   r   
ValueError)r   ru   qdr8   d1d2d3r)   r+   r*   q4q1dq2dq3dq4dq1ddq2ddq3ddq4ddv1v2v3r   r   s                         @@r$   test_time_derivativer   U  s    	sAsA	Q	B	C!QSS*A	acc	A1a bSQSS133Y$7CACC!##I%   	
qssB2q!rT133qss7^3332q!rT133qss7^333	
qssB2q!RQSS\1112q!rT133qss7^333	
qssB2q!Q&&&2q!rT133qss7^333#M2NBB'q9Cc3+M1=D$d	C"acc+A	accB	ACC"QSS&B	ACC"QSS&2acc6	!B133"a'''133"eACCi///133"c!##g---2q!SW,,,2q!rT#a&[_QSS%8c!f9S=&"133w&' ' ' '2q!SWr"uQSSy%83qss7%BBBB2q!SWr"uQSSy%8c!f9S=&"133w&')+C&4 4 4 42q!beck133%6CFSL!##& #ACC&( ( ( (1a bD!##acc'Nq6#:qss133w% "$acc!##g,%/14QQSSW1E%F F F F
:AB
:@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
  |j                  | |
      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   |j                  | |
      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 y )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	ACC"QSS&2acc6	!B
QSS&2acc6/BqssF
"C	ACC"QSS&2acc6	!B	
133133133	B
QSS&2acc6/BqssF
"Cqss7SWs133w&DQ'9444Q,Ar
:::Q,sB???Q,BQ???QbRHr2AF#$% % %q2rJF"bdRUlRWWQ^;<O_$$$q2rJFq1b!*-acc1HQB
4KACC4OOq1b!*%acc)*OB_ 45O_$$$ QR0QQT	!$% % %QR"#%"EbdRU"CFR1WQYBr)r1uHQJBBJ(GGArN   QR"&)bRIRT	BrE!CFSL2ad719$rE"H"a%xz"rBrEzN   qqur!##v#%acc6bRIF	!aQZ	 BurBwm	hrBwB
+	+QSS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 y )Nzq0 q1 q2 q3r   zu1 u2 u3bodyi9  313
quaterniong      rw   c                  *    t        g gd      S Nr   r   q0r)   r+   u1u2u3s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s    2BB<"b"|\ r#   c                  .    t        g gdd      S )Nr   123r   r   r)   r+   r*   r   r   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s$    2BB<"b"bAQS_afg r#   c                  ,    t        g gd      S )Nfoor   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s!    2BB<"b"bAQSXY r#   c                  &    t         gd      S r   r   r   r)   r+   r*   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s    1"r2r26FU r#   c                  (    t        g gd      S r   r   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s    12$RR8H,W r#   c                  $    t        g d      S r   r   )r   r   r   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s    12r2,LQ r#   c                  ,    t        g gd      S )Nr   r   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s!    2BB<"b"bAQSYZ r#   c                  ,    t        g gd      S )Nspacer   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s!    2BB<"b"bAQSZ[ r#   c                  ,    t        g gdd      S )Nr   222r   r   s   r$   r9   ztest_kin_eqs.<locals>.<lambda>  s     2BB<"b"vW\] r#   r   r   )r   r   r   r   rp   r   r6   )q0dr   r   r   kekdsr   r)   r+   r*   r   r   r   s         @@@@@@@r$   test_kin_eqsr     s   #M2NBB'q9Cc3
+JBB	bBZ"RVS	AB$bBZ"RVUKKKK
r2rlRR,<l
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 :\\]
:ggh
:YYZ
9UV
9WX
9QR
:ZZ[
:[[\
:]]^y2r2,@QVVQVVUVU[U[D\\\\r#   c            
         t        d      \  } }}}}t        d      \  }}t        d      }t        d      j                  dd| j                  g      }|j                  dd||j
                  g      }	|	j                  dd||	j                  g      }
|
j                  |	j
                  z  ||	j                  z  z   ||	j                  z  z          t        d	      }|j                  ||	j
                  z  ||j                  |	j
                  z  z  z          |j                  d
||	j                  z        j                  ||
       j                        |j                        |
j                        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       y )Nzq1 q2 q3 u1 u2 u3zu4, u5rr   Yr   LRr   Dmcr   r   r   c                  <    t         j                              S r4   )r   vel)r   r   u_lists   r$   r9   z'test_partial_velocity.<locals>.<lambda>  s    .swwqz61E r#   c                      t               S r4   )r   )r   r   vel_lists   r$   r9   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   rp   )r)   r+   r*   r   r   u4u5r   r   r   r   r   r   r   rC   r   r   r   r   r   s                  @@@@@r$   test_partial_velocityr     sf   +,?@BBBH%FBAsA	C"acc+A	C"acc+A	C"acc+AMM!R!##XQSS(2834c
AIIaaccB!##),,-
++eQW
%COOAq!
AEE!Hall1o6H"b"b!FXvq1c!##gquac"gacckCGACCK&?@1acc3r7133;R45SS!##qssAq!## $ # 	sAsA
QSS28AQEB8Q/QSS!##J<???
9E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  k7  sJ |j                  sJ y )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7FB	d	+B:::::>>>b5BrE>>===r#   N)7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.testing.pytestr   simpr)   r+   r*   r   q5r   r~   r!   r   r   r   r    r   r%   r,   r/   r1   rq   rs   r   r   r   r   r   r"   r#   r$   <module>r     s    ! " % 9 ? . F FO O O
 (-. BB3KKVb!##Y'KKVb!##Y'KKVb!##Y',0 *.F+Rg:T(BV6%r].ABr#   