
    sgo:                         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        Zy	)
    )pi)symbols)sqrt)acossincos)Matrix)ReferenceFramedynamicsymbolsKanesMethodinertiaPoint	RigidBodydot)slowc                    l 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      }/|/j                  dd| |/j                  g      }0|0j                  dd||0j
                  g      }1|1j                  dd||z   |1j                  g      }2t        d      }3|2j                  dd| |2j                  g      }4|2j                  dd||2j
                  g      }5|5j                  dd| |5j                  g      }6t        d      }7t        d      }8|8j                  d||1j                  z        }9|9j                  d||2j                  z        }:|9j                  d| |2j
                  z  ||2j                  z  z         };|:j                  d| |5j
                  z  ||5j                  z  z         }<|:j                  d||5j
                  z  ||5j                  z  z         }=|=j                  d|t        |5j                  |0j                        |5j                  z  |0j                  z
  j                         z        }>|0j                  |/||0j                  z         |1j                  |0|	|1j
                  z         |3j                  |2|
|2j                  z         |2j                  |1||2j                  z         |5j                  |2||5j
                  z         |7j                  |5||5j                  z         |8j                  |/d       |9j                  |8|/|3       |:j                  |9|/|2       |;j                  |9|/|2       |<j                  |:|/|5       |=j                  |:|/|5       |>j                  |=|/|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||z
  ||	z
  ||z
  ||z
  g}G|>j!                  |/      |0j
                  z  |>j!                  |/      |0j                  z  |>j!                  |/      |0j                  z  g}H|>j#                  |8      |0j                  z  g}I|;|+ |*z  |0j                  z  f|<|, |*z  |0j                  z  f|=|- |*z  |0j                  z  f|9|. |*z  |0j                  z  fg}J|C|D|E|Fg}Kt%        |/| ||g|g|I|	|
|g|||g|H|Gd!"	      }L|Lj'                  |K|J      \  }M}Nd#}Od$}Pt(        d%z  t(        d&z  z
  j+                         }Qd'}R|Rt-        |Q      z  |Pt/        |Q      z  z
   j+                         }Sd(}Td#}Ud)}Vd)}Wd*}X|Tt-        |Q      z  |S|P|Oz
  t/        |Q      z  z
  z
  j+                         }Y|V|Oz
  |Ut-        |Q      z  t/        |Q      z  z
  t-        |Q      z  j+                         }Z|Ut-        |Q      z  |V|Oz
  |Ut-        |Q      z  t/        |Q      z  z
  t/        |Q      z  z   j+                         }[|X|Pz
  }\|T|Wz
  }]t1        |\d%z  |]d%z  z         j+                         }^|Tt/        |Q      z  |P|Oz
  t-        |Q      z  z
  j+                         }_|S|^t-        t(        d%z  |Qz
  t3        |\|^z        z
        z  z   j+                         }`|^t/        t(        d%z  |Qz
  t3        |\|^z        z
        z  |_z
  j+                         }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|Oz  |d|d||b|Pz  i}c|Lj5                  d<i |j7                         d|	j7                         d|
j7                         d|j7                         d|j7                         d|j7                         d|d|	d|
|b|Oz  |d|d||b|Pz  | d|d|d|dd!=      \  }d}e}f|dj9                  |c      }g|ej9                  |c      }h|gj+                         }g|hj+                         }h|gj;                  g d>g d>      }dt=        g d?g d@dAdB|bd%z  z  dCz
  dD|bz  dE|bz  gdFdG|bd%z  z  dHz   dI|bz  dJ|bz  gg      }idKlt?        dL      D ]=  }jijA                  b|j      djA                  |b|j      z
  }ktC        lfdM|kD              r=J  y )NNz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   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=   c              3   :   K   | ]  }t        |      k    y w)N)abs).0xepss     [/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane3.py	<genexpr>ztest_bicycle.<locals>.<genexpr>;  s     /A3q6C</s   )"r   r   r
   	orientnewzrB   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)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AB_A_sB_sResierrorrC   sm                                                                                                               @rD   test_bicycler      s   , $M2NBB'q9Cc3+,?@BBB#12Eq#I Cc3S )00P(Q%E5':'./O'P$JW)01R)S&GXx$+,H$I!E5%,34X,Y)Hh'#*+F#G GWgq%&<=FE3 	sA	C"acc+A	C"acc+AKK"w,)<=E		BVwh5HII??66B=9D~~j&G8TVV2DEH		B IGguqss{3EOOG[577%:;EzX:+?-5-?,@ AHooi7TVV);+2TVV+;*< =GOOGZ$&&%8:;N%NOEooi#dffacc2BTVV2K23##367@y{*C DG MM!R!##XMM!R!##XNN5"uww,'	aegg&UBK(NN4dff%
 OOAq	gq"%	eQ&5)q$'	eQ%q"% y(Hh1hOQYZGh'1aI7SFE5%/7DD%.6D +xHIWdE6BHxC6FxC6F (C"HcBhb	1B [[^acc)7;;q>ACC+?QRSRURUAUVM %%g.45M fWq[133&
'	5&1*qss"#	q133	q133
!B Xvv	.B 
Qr2rlr2,R=&
(B %%b"-LR (+L'+M(*Qb(8'?'?'AC'+J*4SX*=}SQTX?U*V(W'^'^'`D'+G'*M'*M'*L'*L(/C(8$l@Z\_`c\d?d:d(e'l'l'nK)6)E}UXY\U]G]_bcf_gFg)gilmpiq(q'x'x'zK(5C(@MT`D`cpsvwzs{c{  B  CF  G  dG  EG  KN  OR  KS  DS  )S  (Z  (Z  (\J(4}(DE(/(<E(,UAXeQh->(?'F'F'HE(/C(8-:TVYZ]V^9^(^'e'e'gJ(,ec"Q$s(4eCT:T6U.U(V']']'_J(-RT#XtE%K?P4P0Q(QR\(\'c'c'eI 	A$'} $'|$'$' D$' J	$'
 [$' $' $' +$' *$' v$' t$' v$' t$' $' $'  !$'" #$'$ #%$'& "'$'( #)$'* $+$', q-$'. /$'0 Q1$'2 Q3$'4 45$'6 A7$'8 A9$': A;$'< A=$'> A?$'@ AA$'B A$C$'D AAA%I$'H\ ll
 GGIq	

 GGIq
 GGIq
 GGIq
 GGIq
 GGIq
 
 
 L 
 
 
 M!
  !
" #
$ %
& '
* /  GAq!: **X
C
**X
C
))+C
))+C 	L,/A }}#%71%<?P%PRdefRfhz{|h|}#'8A'=@P'PTdefTfiz{|i|} @C C1X 0A1-/////0    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    r   rD   <module>r      sA    ! % 9 C C '* * * & n0 n0r   