o
    *ήcl8                     @   s   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
mZ d dlmZ d dlmZmZmZmZmZmZmZmZ d dlmZmZmZ ed	d
 ZdS )    )evalf)pi)symbols)sqrt)acossincos)Matrix)ReferenceFramedynamicsymbolsKanesMethodinertiamsubsPoint	RigidBodydot)slow	ON_TRAVISskipc            m   
      s  t rtd td\} }}}tdd\}}}}td\}}	}
}}}tdd\}}}}}}td\}}}}td\}}}td\}}}}td\}} }!}"td	\}#}$}%}&td
\}'}(})}*td\}+},}-}.td}/|/dd| |/jg}0|0dd||0jg}1|1dd|| |1jg}2td}3|2dd| |2jg}4|2dd||2jg}5|5dd| |5jg}6td}7t	d}8|8
d||1j }9|9
d||2j }:|9
d| |2j ||2j  };|:
d| |5j ||5j  }<|:
d||5j ||5j  }=|=
d|t|5j|0j|5j |0j   }>|0|/||0j  |1|0|	|1j  |3|2|
|2j  |2|1||2j  |5|2||5j  |7|5||5j  |8|/d |9|8|/|3 |:|9|/|2 |;|9|/|2 |<|:|/|5 |=|:|/|5 |>|=|/|7 t|4|"|#|$dd|%|;f}?t|6|&|'|(dd|)|<f}@t|2||||9f}At|5| |!| |=f}Btd|;|2|+|?}Ctd|<|5|,|@}Dtd |9|3|.|A}Etd!|=|7|-|B}F|| ||	 || || g}G|>|/|0j@ |>|/|0j@ |>|/|0j@ g}H|>|8|0j@ g}I|;|+ |* |0j f|<|, |* |0j f|=|- |* |0j f|9|. |* |0j fg}J|C|D|E|Fg}Kt|/| ||g|g|I|	|
|g|||g|H|Gd"}L|L|K|J\}M}Nd#}Od$}Pttd% td&  }Qd'}Rt|Rt|Q |Pt|Q   }Sd(}Td#}Ud)}Vd)}Wd*}Xt|Tt|Q |S|P|O t|Q   }Yt|V|O |Ut|Q t|Q  t|Q }Zt|Ut|Q |V|O |Ut|Q t|Q  t|Q  }[t|X|P }\t|T|W }]tt|\d% |]d%  }^t|Tt|Q |P|O t|Q  }_t|S|^ttd% |Q t|\|^    }`t|^ttd% |Q t|\|^   |_ }atd+}bi ||P||O||Q||S||_||Y||a||`||Z||[|d,|d-| d.|!d/|&d0|'d1|(d2i |)d3|"d4|#d5|$d6|%d7|,d8|+d9|-d:|.d%|*d;| d|d|d|d|d|	d|
|b|O |d|d||b|P i}c|L d }d|Lj}et|e|c}ft|d|L  |c}g|f }f|g }g|f! |g }h|h"g d<g d=}it#g d>g d?d@dA|bd%  dB dC|b dD|b gdEdF|bd%  dG dH|b dI|b gg}jdJ t$dKD ]}k|j%|b|k|i%|b|k }lt& fdLdM|lD sJ qd S )NNzToo slow for travis.zq1 q2 q4 q5   zu1 u2 u3 u4 u5 u6zWFrad WRrad htangle forkoffsetzforklength framelength forkcg1zforkcg3 framecg1 framecg3 Iwr11zIwr22 Iwf11 Iwf22 Iframe11z"Iframe22 Iframe33 Iframe31 Ifork11zIfork22 Ifork33 Ifork31 gzmframe mfork mwf mwrNYAxisRFrameWR	TempFrameForkTempForkWFWR_contWR_mcSteerFrame_mcFork_mcWF_mcWF_contr   	BodyFrameBodyForkBodyWRBodyWF)q_indq_dependentconfiguration_constraintsu_indu_dependentvelocity_constraintskd_eqsg333333?gffffff?   
   g{Gz?gRQ?g?gffffff?vg?߮?gQ?gCl?gQ?g)"*?gQ?gwJ|?g<~?gffffff"@   gffffff@g333333   U      gQ#@)r   r2   r6      )r   r2   r8      )r   r         ?r   )r   r   r   r;   g'^"@gltggWI?g>g{_*'տgE_p'@g%g>@gHj@gaGͭg-q=r9   c                 3   s    | ]	}t | k V  qd S )N)abs).0xeps O/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/mechanics/tests/test_kane3.py	<genexpr>%  s    ztest_bicycle.<locals>.<genexpr>)'r   r   r   r   r
   	orientnewzr>   yr   	locatenewr   	normalizeset_ang_velset_velv2pt_theoryr   r   velpos_fromr   kanes_equationsr   r   r   r   r   r   r   	linearizemass_matrix_fullr   kindiffdictinvextractr	   rangesubsall)mq1q2q4q5q1dq2dq4dq5du1u2u3u4u5u6u1du2du3du4du5du6dWFradWRradhtangle
forkoffset
forklengthframelengthforkcg1forkcg3framecg1framecg3Iwr11Iwr22Iwf11Iwf22Iframe11Iframe22Iframe33Iframe31Ifork11Ifork22Ifork33Ifork31gmframemforkmwfmwrr   r   r   r   r   r   r   r   r   r    r!   r"   r#   r$   r%   r&   Frame_IFork_IWR_IWF_Ir'   r(   r)   r*   kdconlist_speedconlist_coordFLBLKMfrfrstarPaperRadRearPaperRadFrontHTA
TrailPaperrakePaperWbPaperFrameCgXPaperFrameCgZPaperForkCgXPaperForkCgZFrameLengthFrameCGNorm
FrameCGPartempatempbtempc
PaperForkL
ForkCGNorm	ForkCGParr4   val_dictforcing_linMM_full	MM_full_sforcing_lin_sAmatAResierrorrA   r?   rB   test_bicycle   sn  .	
 &*6"**	
 !".
 r   N)sympy.core.evalfr   sympy.core.numbersr   sympy.core.symbolr   (sympy.functions.elementary.miscellaneousr   (sympy.functions.elementary.trigonometricr   r   r   sympy.matrices.denser	   sympy.physics.mechanicsr
   r   r   r   r   r   r   r   sympy.testing.pytestr   r   r   r   rA   rA   rA   rB   <module>   s    (