o
    go:                     @   s   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 d dlmZ edd	 Zd
S )    )pi)symbols)sqrt)acossincos)Matrix)ReferenceFramedynamicsymbolsKanesMethodinertiaPoint	RigidBodydot)slowc            l         s  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!d"	}L|L|K|J\}M}Nd#}Od$}Ptd% td&   }Qd'}R|Rt|Q |Pt|Q    }Sd(}Td#}Ud)}Vd)}Wd*}X|Tt|Q |S|P|O t|Q    }Y|V|O |Ut|Q t|Q  t|Q  }Z|Ut|Q |V|O |Ut|Q t|Q  t|Q   }[|X|P }\|T|W }]t|\d% |]d%   }^|Tt|Q |P|O t|Q   }_|S|^ttd% |Q 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|Ljd<i | d|	 d|
 d| d| d| d|d|	d|
|b|O |d|d||b|P | d|d|d|dd!d=\}d}e}f|d|c}g|e|c}h|g }g|h }h|gg d>g d>}dtg d?g d@dAdB|bd%  dC dD|b dE|b gdFdG|bd%  dH dI|b dJ|b gg}idK tdLD ]}j|i |b|j|d |b|j }kt! fdMdN|kD s	J qd S )ONz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CRAMER)q_indq_dependentconfiguration_constraintsu_indu_dependentvelocity_constraintskd_eqsconstraint_solverg333333?gffffff?   
   g{Gz?gRQ?g?gffffff?vg?߮?gQ?gCl?gQ?g)"*?gQ?gwJ|?g<~?gffffff"@   gffffff@g333333   U      gQ#@T)A_and_Bop_pointlinear_solver)r   r0   r6      )r   r         ?r   )r   r   r   r;   g'^"@gltggWI?g>g{_*'տgE_p'@g%g>@gHj@gaGͭg-q=   c                 3   s    | ]	}t | k V  qd S )N)abs).0xeps _/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane3.py	<genexpr>;  s    ztest_bicycle.<locals>.<genexpr>)"r
   r   r	   	orientnewzr?   yr   	locatenewr   	normalizeset_ang_velset_velv2pt_theoryr   r   velpos_fromr   kanes_equationsr   evalfr   r   r   r   	linearizediffxreplaceextractr   rangesubsall)l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	ForkCGParr2   val_dictAB_A_sB_sResierrorrB   r@   rC   test_bicycle   s  .	
$(4 ((	
 !".	


 r   N)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   sympy.testing.pytestr   r   rB   rB   rB   rC   <module>   s    $