o
    *Î®cQ  ã                   @   sT  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$ %ddee$j&g¡Z'e' %dde e'j(g¡Z)e) %dde!e)j*g¡Z+dd„ Z,dd„ Z-dd„ Z.dd„ Z/dd„ Z0dd„ Z1dd„ Z2dd „ Z3d!d"„ Z4d#d$„ Z5d%d&„ Z6d'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)ÚraisesTzq1 q2 q3 q4 q5ÚNÚAÚAxisÚBÚCc                   C   sÊ   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s7J ‚t tjtjƒdksBJ ‚t tjtjƒdksMJ ‚t tjtjƒdksXJ ‚t tjtjƒdkscJ ‚d S )Né   r   )r   r   ÚxÚyÚz© r    r    úP/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/vector/tests/test_functions.pyÚtest_dot   s   r"   c                   C   s:  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s3J ‚t tjtjƒttƒks@J ‚t tjtjƒdksKJ ‚t tjtjƒdksVJ ‚t tjtjƒdksaJ ‚t tjtjƒdkslJ ‚t tjtjtj ƒt	dƒttt
d  ƒ   kr‘t tjtj 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ƒ ksÊJ ‚t tjtjƒttƒks×J ‚t tjtjƒttƒ ttƒ kséJ ‚t tjtjƒttƒ ttƒ ksûJ ‚t tjtjƒttƒks	J ‚t tjtjƒttƒttƒ ksJ ‚d S )Nr   r   é   é   )r   r   r   r   r   Úq1r   r   r   r   r   r   Úq3Úq2r    r    r    r!   Útest_dot_different_frames%   s&   P"$$(r(   c                   C   sÜ   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s1J ‚t tjtjƒdks<J ‚t tjtjƒtjksHJ ‚t tjtjƒtjksTJ ‚t tjtjƒtj ksaJ ‚t tjtjƒdkslJ ‚d S ©Nr   )r   r   r   r   r   r    r    r    r!   Ú
test_cross=   s   r*   c                   C   sÂ  t tjtjƒttƒtj ksJ ‚t tjtjƒttƒtj ks J ‚t tjtjƒttƒ tj ttƒtj  ks8J ‚t tjtjƒttƒ tj ksIJ ‚t tjtjƒttƒtj ksYJ ‚t tjtjƒttƒtj ttƒtj  kspJ ‚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 ks¤J ‚t tjtjƒttƒtj ks´J ‚t tjtjtj ƒttƒtj ttƒtj  ksÎJ ‚t tjtj tjƒttƒ tj ttƒtj  kséJ ‚t tjt	jƒtt
ƒt	j ksùJ ‚t tjt	jƒtt
ƒ t	j tt
ƒt	j  ksJ ‚t tjt	jƒtt
ƒ t	j ks$J ‚t t	jtjƒtt
ƒ t	j ks6J ‚t t	jtjƒtt
ƒt	j tt
ƒt	j  ksNJ ‚t t	jtjƒtt
ƒt	j ks_J ‚d S r)   )r   r   r   r   r   r%   r   r   r   r   r&   r    r    r    r!   Útest_cross_different_framesK   s&     0" .  46 2$$0&r+   c                     s˜  t dƒ} | j| j ‰ˆˆB ‰ tdƒ‰tdƒ‰ˆ ˆ @ tˆ ˆ ƒks!J ‚ˆ ˆ@ tˆ ˆƒks,J ‚ˆˆ @ tˆˆ ƒks7J ‚ˆ ˆ@ tˆ ˆƒksBJ ‚ˆˆ @ tˆˆ ƒksMJ ‚ˆ ˆ@ tˆ ˆƒksXJ ‚ˆˆ @ tˆˆ ƒkscJ ‚tt‡ fdd„ƒ tt‡ fdd„ƒ tt‡ fdd„ƒ tt‡ fdd„ƒ ˆˆ@ tˆˆƒks’J ‚ˆˆ@ tˆˆƒksJ ‚ˆˆ@ tˆˆƒks¨J ‚tt‡fdd„ƒ tt‡fd	d„ƒ tt‡fd
d„ƒ tt‡fdd„ƒ tt‡ fdd„ƒ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ ˆ ˆA tˆ ˆƒksôJ ‚ˆˆ A tˆˆ ƒksÿJ ‚ˆ ˆA tˆ ˆƒksJ ‚ˆˆ A tˆˆ ƒksJ ‚ˆˆ A tˆˆ ƒks#J ‚tt‡ fdd„ƒ tt‡ fdd„ƒ tt‡ fdd„ƒ tt‡ fdd„ƒ ˆˆA tˆˆƒksSJ ‚ˆˆA tˆˆƒks_J ‚ˆˆA tˆˆƒkskJ ‚tt‡fdd„ƒ tt‡fdd„ƒ tt‡fdd„ƒ tt‡fdd„ƒ tt‡ fdd„ƒ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ tt‡ fdd„ƒ tt‡ fd d„ƒ tt‡ fd!d„ƒ tt‡ fd"d„ƒ ˆˆB t	ˆˆƒksJ ‚ˆˆB t	ˆˆƒksJ ‚ˆˆB t	ˆˆƒks&J ‚tt‡fd#d„ƒ tt‡fd$d„ƒ tt‡fd%d„ƒ tt‡fd&d„ƒ d'S )(zUTest that the output of dot, cross, outer functions match
    operator behavior.
    r   r   c                      ó   t ˆ tjƒS ©N©r   r   ÚZeror    ©Údr    r!   Ú<lambda>t   ó    z%test_operator_match.<locals>.<lambda>c                      ó   t tjˆ ƒS r-   r.   r    r0   r    r!   r2   u   r3   c                      ó
   t ˆ dƒS r)   ©r   r    r0   r    r!   r2   v   ó   
 c                      ó
   t dˆ ƒS r)   r6   r    r0   r    r!   r2   w   r7   c                      r,   r-   r.   r    ©Úvr    r!   r2   {   r3   c                      r4   r-   r.   r    r9   r    r!   r2   |   r3   c                      r5   r)   r6   r    r9   r    r!   r2   }   r7   c                      r8   r)   r6   r    r9   r    r!   r2   ~   r7   c                      ó
   t ˆ ˆ ƒS r-   ©r   r    r0   r    r!   r2      r7   c                      ó
   t ˆ ˆƒS r-   r<   r    ©r1   Úzerodr    r!   r2   ‚   r7   c                      ó
   t ˆˆ ƒS r-   r<   r    r>   r    r!   r2   ƒ   r7   c                      r,   r-   ©r   r   r/   r    r0   r    r!   r2   ‰   r3   c                      r4   r-   rA   r    r0   r    r!   r2   Š   r3   c                      r5   r)   r<   r    r0   r    r!   r2   ‹   r7   c                      r8   r)   r<   r    r0   r    r!   r2   Œ   r7   c                      r,   r-   rA   r    r9   r    r!   r2      r3   c                      r4   r-   rA   r    r9   r    r!   r2   ‘   r3   c                      r5   r)   r<   r    r9   r    r!   r2   ’   r7   c                      r8   r)   r<   r    r9   r    r!   r2   “   r7   c                      r;   r-   ©r   r    r0   r    r!   r2   –   r7   c                      r=   r-   rB   r    r>   r    r!   r2   —   r7   c                      r@   r-   rB   r    r>   r    r!   r2   ˜   r7   c                      r=   r-   rB   r    ©r1   r:   r    r!   r2   ™   r7   c                      r@   r-   rB   r    rC   r    r!   r2   š   r7   c                      r=   r-   rB   r    ©r1   Úzerovr    r!   r2   ›   r7   c                      r@   r-   rB   r    rD   r    r!   r2   œ   r7   c                      r@   r-   rB   r    rD   r    r!   r2      r7   c                      r,   r-   ©r   r   r/   r    r0   r    r!   r2   ž   r3   c                      r4   r-   rF   r    r0   r    r!   r2   Ÿ   r3   c                      r5   r)   rB   r    r0   r    r!   r2       r7   c                      r8   r)   rB   r    r0   r    r!   r2   ¡   r7   c                      r,   r-   rF   r    r9   r    r!   r2   ¥   r3   c                      r4   r-   rF   r    r9   r    r!   r2   ¦   r3   c                      r5   r)   rB   r    r9   r    r!   r2   §   r7   c                      r8   r)   rB   r    r9   r    r!   r2   ¨   r7   N)
r   r   r   r   r	   r   r   Ú	TypeErrorr   r   )r   r    )r1   r:   r?   rE   r!   Útest_operator_matchb   sz   rH   c                   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 t
t	ƒtj  ks.J ‚t tjtƒt
tƒt
t	ƒ tj ttƒtj  t
tƒtt	ƒ tj  ksSJ ‚t tjtƒt
t	ƒ ttƒ tj t
tƒtj  ttƒtt	ƒ tj  ksyJ ‚t tjtƒttƒtj t
tƒtj  ksJ ‚t tjtƒt
tƒ tj t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jksÝJ ‚t tjtƒttƒtj t
tƒtj  ksóJ ‚t tjtƒt
tƒtj ttƒtj  ks
J ‚t tjtƒtt	ƒtj t
t	ƒtj  ks!J ‚t tjtƒt
tƒt
t	ƒ tj ttƒtj  t
tƒtt	ƒ tj  ksGJ ‚t tjtƒt
t	ƒ ttƒ tj t
tƒtj  ttƒtt	ƒ tj  ksnJ ‚t tjtƒtjkszJ ‚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 t
tƒtj  ks©J ‚t tjtƒt
tƒtj ttƒtj  ksÀJ ‚t tjtƒtjksÌJ ‚t tjtƒttƒtj t
tƒttƒ tj  t
tƒt
tƒ tj  ksòJ ‚t tjtƒt
tƒtj ttƒttƒ tj  t
tƒttƒ tj  ksJ ‚t tjtƒt
tƒtj ttƒtj  ks/J ‚t tjtƒttƒtt	ƒ t
tƒt
tƒ t
t	ƒ  tj t
tƒttƒ tj  t
t	ƒttƒ t
tƒt
tƒ tt	ƒ  tj  ksqJ ‚t tjtƒt
tƒtt	ƒ t
tƒt
t	ƒ ttƒ  tj ttƒttƒ tj  t
tƒt
t	ƒ t
tƒttƒ tt	ƒ  tj  ks³J ‚t tjtƒt
t	ƒ ttƒ tj t
tƒtj  ttƒtt	ƒ tj  ksÚJ ‚t tjtƒttƒtj t
tƒtj  ksñJ ‚t tjtƒt
tƒ tj t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s9J ‚t tjtƒtjksEJ ‚t tjtƒttƒtj t
tƒtj  ks\J ‚t tjtƒt
tƒtj ttƒtj  kssJ ‚t tjtƒtt	ƒtj t
t	ƒtj  ksŠJ ‚t tjtƒt
tƒt
t	ƒ tj ttƒtj  t
tƒtt	ƒ tj  ks°J ‚t tjtƒt
t	ƒ ttƒ tj t
tƒtj  ttƒtt	ƒ tj  ks×J ‚t tjtƒttƒtj t
tƒtj  ksîJ ‚t tjtƒt
tƒ ttƒ tj ttƒttƒ tj  t
tƒtj  ksJ ‚t tjtƒt
tƒt
tƒ tj t
tƒttƒ tj  ttƒtj  ks;J ‚t tjtƒtjksGJ ‚t tjtƒttƒtj t
tƒtj  ks^J ‚t tjtƒt
tƒ tj ttƒtj  ksvJ ‚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 t
t	ƒtj  ks±J ‚t tjtƒtjks½J ‚t tjtƒt
t	ƒ tj tt	ƒtj  ksÕJ ‚t tjtƒttƒtt	ƒ t
tƒt
tƒ t
t	ƒ  tj t
tƒtt	ƒ t
tƒt
t	ƒ ttƒ  tj  t
t	ƒttƒ tj  ksJ ‚t tjtƒt
tƒ ttƒ tj ttƒttƒ tj  t
tƒtj  ks>J ‚t tjtƒt
t	ƒttƒ t
tƒt
tƒ tt	ƒ  tj t
tƒt
t	ƒ t
tƒttƒ tt	ƒ  tj  ttƒtt	ƒ tj  ks€J ‚t tjtƒtt	ƒtj t
tƒt
t	ƒ tj  t
t	ƒttƒ tj  ks¦J ‚t tjtƒttƒtj t
tƒtj  ks½J ‚t tjtƒt
t	ƒtj t
tƒtt	ƒ tj  ttƒtt	ƒ tj  ksãJ ‚t tjtƒtt	ƒtj t
t	ƒtj  ksúJ ‚t tjtƒtjksJ ‚t tjtƒt
t	ƒtj tt	ƒtj  ksJ ‚t tjtƒtjks)J ‚t tjtƒtjks5J ‚t tjtƒtj  krHtjksKJ ‚ J ‚tjt ttƒtj t
tƒtj  tƒksbJ ‚tjt t
tƒtj ttƒtj  tƒksyJ ‚tjt ttƒtj t
tƒttƒ tj  t
tƒt
tƒ tj  tƒksŸJ ‚tjt t
tƒtj ttƒttƒ tj  t
tƒttƒ tj  tƒksÅJ ‚tjt t
tƒtj ttƒtj  tƒksÜJ ‚	 tjt ttƒtj t
tƒtj  tƒksôJ ‚tjt t
tƒ tj ttƒtj  tƒksJ ‚tjt ttƒtj t
tƒtj  tƒks#J ‚tjt t
tƒtj ttƒtj  tƒks:J ‚tjt tt	ƒtj t
t	ƒtj  tƒksQJ ‚tjt t
tƒt
t	ƒ tj ttƒtj  t
tƒtt	ƒ tj  tƒkswJ ‚tjt t
t	ƒ ttƒ tj t
tƒtj  ttƒtt	ƒ tj  tƒksžJ ‚tjt ttƒtj t
tƒtj  tƒksµJ ‚tjt t
tƒ ttƒ tj ttƒttƒ tj  t
tƒtj  tƒksÜJ ‚tjt t
tƒt
tƒ tj t
tƒttƒ tj  ttƒtj  tƒksJ ‚tjt ttƒtj t
tƒtj  tƒksJ ‚tjt t
tƒ tj ttƒtj  tƒks1J ‚tjt tt	ƒtj t
t	ƒtj  tƒksHJ ‚tjt t
t	ƒ tj tt	ƒtj  tƒks`J ‚	 tjt tt	ƒtj t
tƒt
t	ƒ tj  t
t	ƒttƒ tj  tƒks‡J ‚tjt ttƒtj t
tƒtj  tƒksžJ ‚tjt t
t	ƒtj t
tƒtt	ƒ tj  ttƒtt	ƒ tj  tƒksÄJ ‚tjt tt	ƒtj t
t	ƒtj  tƒksÛJ ‚tjt t
t	ƒtj tt	ƒtj  tƒksòJ ‚d S r)   )r   r   r   r   r/   r   r   r   r   r&   r   r   r   r'   r%   r   r    r    r    r!   Útest_express«   sn  ,,
ÿ.
ÿ,.,..,ÿ.ÿ..,ÿ,ÿ.
,ÿ,þ
ÿ
,ÿ,þ
ÿ.ÿ.0...,ÿ.ÿ. ÿÿÿÿ.0.0
,,ÿþ
ÿ
:
ÿ
,,ÿþ
ÿ,ÿ.,ÿ..,..(ÿÿ(ÿÿ..0...(ÿÿ*ÿÿ.ÿÿÿÿÿÿ.0.0(ÿÿ.(ÿÿ.2rI   c                     sÈ  t dƒ} tdƒ}tddƒ}|  dd|| jg¡‰ | j| jB }t|ˆ ƒ| | j| jB  | | j| jB   ks6J ‚| jˆ jB }t|| ƒ| | jˆ jB  ksLJ ‚t|ˆ ƒ| | jˆ jB  ks\J ‚| jˆ jB }t|| ƒ|| jˆ jB  ksqJ ‚t|ˆ ƒ| | jˆ jB  ksJ ‚| jˆ jB }t|| ƒdksJ ‚t|ˆ ƒ| | jˆ jB  ks J ‚tdƒ\}}}	}
tddƒ\}}}}tddƒ\}}}}ˆ  d	d|
ˆ jg¡‰|| j }|| j |	ˆ j  }|| j || j  |	| j  }tˆ jˆƒdksìJ ‚tˆ jˆƒ| ˆ j ksúJ ‚tˆ jˆƒ|ˆ j ksJ ‚t|ˆ ƒ|| j ksJ ‚t|ˆƒ| t|ƒ | | j |t|ƒ | | j  || j  ks9J ‚t|| ƒ|| j |	| ˆ j  |ˆ j  ksRJ ‚t|ˆƒ|| j || | j  |t|ƒ | | j  |ˆ j  |	| ˆ j  ks}J ‚t|ˆ ƒ|| | | j | | | | j  || j  ksJ ‚t|ˆƒ| | j| jB  t|ƒ| | j| jB   || j| jB   t|ƒ| | j| jB   ksÎJ ‚t	t
‡ ‡fd
d„ƒ t	t
‡ ‡fdd„ƒ d S )Nr   Úqr   r   r   r   zq1 q2 q3 q4r#   r   c                      ó   t ˆ jˆddS )Nç      à?©Úorder©r   r   r    ©r   r   r    r!   r2   |  ó    z&test_time_derivative.<locals>.<lambda>c                      rK   )NéÿÿÿÿrM   rO   r    rP   r    r!   r2   }  rQ   )r   r   Ú	orientnewr   r   r   r   r   r   r   Ú
ValueError)r   rJ   Úqdr1   Úd1Úd2Úd3r%   r'   r&   Úq4Úq1dÚq2dÚq3dÚq4dÚq1ddÚq2ddÚq3ddÚq4ddÚv1Úv2Úv3r    rP   r!   Útest_time_derivativeU  sn   

ÿ    
ÿÿ2ÿÿÿÿÿÿÿÿre   c                  C   sü  t j} tdƒ\}}}tdƒ\}}}tdƒ\}}}	tdƒ\}
}t dƒ\}}}t ddƒ\}}}t ddƒ\}}}|tj |tj  |tj  }|tj |tj  |	tj  }|tj |tj  |tj  }|tj |tj  |tj  }|tj |tj  |tj  }|tj |tj  |tj  }ttdd	ks˜J ‚tt|d
dd|fks¥J ‚tt|d
|||fks²J ‚tt|dd|||  fksÁJ ‚tt|||
dd|||| |
   fksÖJ ‚tt|||
d}d|||  ||
  | | |
¡ f}||ksõJ ‚tt|||
d}t	|| |
| fƒtj t	|| |
| fƒtj  t	|| |
| fƒtj  }|||| f}||ks,J ‚tt|d|||  || d  d fksBJ ‚tt||||
|d||||   ||  | |
 || d  d  || |
  ||
d  d  | |||    | | |
¡ fksƒJ ‚tt||||
|d||||   ||  | |
 | || d  d  || |
  ||
d  d  | |||    fksÀJ ‚tt|tj |tj |tj |
|d}t	|| || fƒ}|tj || tj |t	|| | |
| fƒ tj 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)ri   rh   Ú
timevalue1)Úacceleration)rk   ri   rh   rj   Ú
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Úv01rb   rc   Úv2dÚv2ddÚanswerÚanswer_expectedÚintegral_vectorÚi1r    r    r!   Útest_get_motion_methods€  s†   ÿ"*ÿ

ÿÿ$ÿÿ
þÿ
þÿ
ÿÿþÿ
þÿÿrŒ   c               	      sV  t dƒ\‰ ‰‰‰t ddƒ\} }}}t dƒ\‰‰‰tˆˆˆgˆˆˆgddƒ}|tˆˆˆgˆˆˆgddƒks6J ‚tˆˆˆgˆ ˆˆˆgdƒ}|dˆ  ˆ d	ˆ ˆ  d	ˆ ˆ  | dˆ  ˆ d	ˆ ˆ  d	ˆ ˆ  | dˆ  ˆ d	ˆ ˆ  d	ˆ ˆ  | d	ˆ ˆ d	ˆ ˆ  d	ˆ ˆ  |  gks•J ‚tt‡ ‡‡‡‡‡fd
d„ƒ tt‡ ‡‡‡‡‡‡fdd„ƒ tt‡ ‡‡‡‡‡‡fdd„ƒ tt‡ ‡‡‡‡fdd„ƒ tt‡ ‡‡‡‡fdd„ƒ tt‡ ‡‡‡fdd„ƒ tt‡ ‡‡‡‡‡‡fdd„ƒ tt‡ ‡‡‡‡‡‡fdd„ƒ tt‡ ‡‡‡‡‡fd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      à¿rL   c                      s   t ˆˆˆgˆ ˆˆgdƒS ©Nr   ©r   r    ©Úq0r%   r'   Úu1Úu2Úu3r    r!   r2   Ä  s    ztest_kin_eqs.<locals>.<lambda>c                      s   t ˆˆˆgˆ ˆˆˆgddƒS )Nr   Ú123r‘   r    ©r“   r%   r'   r&   r”   r•   r–   r    r!   r2   Å  s    c                      ó   t ˆˆˆgˆ ˆˆˆgdƒS )NÚfoor‘   r    r˜   r    r!   r2   Æ  ó    c                      s   t ˆˆ ˆˆˆgdƒS r   r‘   r    ©r“   r%   r'   r&   r”   r    r!   r2   Ç  s    c                      s   t ˆgˆ ˆˆˆgdƒS r   r‘   r    rœ   r    r!   r2   È  s    c                      s   t ˆˆˆgˆ dƒS r   r‘   r    )r“   r”   r•   r–   r    r!   r2   É  ó    c                      r™   )Nr   r‘   r    r˜   r    r!   r2   Ê  r›   c                      r™   )NÚspacer‘   r    r˜   r    r!   r2   Ë  r›   c                      s   t ˆˆˆgˆ ˆˆgddƒS )Nr   Ú222r‘   r    r’   r    r!   r2   Ì  r›   rg   rž   )r   r   r   rT   rG   r   r/   )Úq0drZ   r[   r\   ÚkeÚkdsr    r˜   r!   Útest_kin_eqs¹  s*   "(&&&
ý0r£   c               	      sþ  t dƒ\} }}‰}}t dƒ\}}tdƒ}tdƒ‰ˆ dd| ˆjg¡}| dd||jg¡}	|	 dd||	jg¡}
|
 ˆˆ|	j ||	j  ||	j  ¡ td	ƒ}| 	ˆ||	j ||j|	jA   ¡ | 
d
||	j ¡‰ ˆ  |ˆ|
¡ ˆ  ˆ¡| ˆ¡|
 ˆ¡g‰ˆ||||g‰tˆˆˆƒ| |	j ||	j d|	jt|ƒ|	j t|ƒ|	j  gddd|	jt|ƒ|	j t|ƒ|	j  g|	j|	j|	jddggksÃJ ‚tdƒ}tdƒ}||j ||j  }t|f||f|ƒ|j|jggksçJ ‚tt‡ ‡‡fdd„ƒ tt‡‡‡fdd„ƒ d S )Nzq1 q2 q3 u1 u2 u3zu4, u5Úrr   ÚYr   ÚLÚRr   ÚDmcr   r   r   c                      s   t ˆ  ˆ¡ˆˆƒS r-   )r   Úvelr    )r¨   r   Úu_listr    r!   r2   î  r   z'test_partial_velocity.<locals>.<lambda>c                      s   t ˆˆˆ ƒS r-   )r   r    )r   r”   Úvel_listr    r!   r2   ï  r3   )r   r   r   rS   r   r   r   Úset_ang_velr
   Úset_velÚ	locatenewÚv2pt_theoryr©   Ú
ang_vel_inr   r   r   r   rG   )r%   r'   r&   r•   r–   Úu4Úu5r¤   r¥   r¦   r§   r   r   r   r:   r    )r¨   r   r”   rª   r«   r!   Útest_partial_velocityÐ  s4   &"
4&þÿ$r³   c                  C   s€   t dƒ} t ddd}t ddd}t ddd	\}}t d
dd}| jd u s%J ‚|js*J ‚|js/J ‚|| || ks9J ‚|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º   r    r    r!   Útest_dynamicsymbolsñ  s   

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&   rY   Úq5r   rS   r   r   r   r   r   r   r"   r(   r*   r+   rH   rI   re   rŒ   r£   r³   rÁ   r    r    r    r!   Ú<module>   s6    ,I ++9!