
    go:                         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d	S )
    )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          }/|/                    dd| |/j        g          }0|0                    dd||0j        g          }1|1                    dd||z   |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        z            }9|9                    d||2j        z            }:|9                    d| |2j        z  ||2j        z  z             };|:                    d| |5j        z  ||5j        z  z             }<|:                    d||5j        z  ||5j        z  z             }=|=                    d|t          |5j        |0j                  |5j        z  |0j        z
  
                                z            }>|0                    |/||0j        z             |1                    |0|	|1j        z             |3                    |2|
|2j        z             |2                    |1||2j        z             |5                    |2||5j        z             |7                    |5||5j        z             |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||z
  ||	z
  ||z
  ||z
  g}G|>                    |/          |0j        z  |>                    |/          |0j        z  |>                    |/          |0j        z  g}H|>                    |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|L                    |K|J          \  }M}Nd#}Od$}Pt(          d%z  t(          d&z  z
                                  }Qd'}R|Rt-          |Q          z  |Pt/          |Q          z  z
                                   }Sd(}Td#}Ud)}Vd)}Wd*}X|Tt-          |Q          z  |S|P|Oz
  t/          |Q          z  z
  z
                                  }Y|V|Oz
  |Ut-          |Q          z  t/          |Q          z  z
  t-          |Q          z                                  }Z|Ut-          |Q          z  |V|Oz
  |Ut-          |Q          z  t/          |Q          z  z
  t/          |Q          z  z                                   }[|X|Pz
  }\|T|Wz
  }]t1          |\d%z  |]d%z  z                                             }^|Tt/          |Q          z  |P|Oz
  t-          |Q          z  z
                                  }_|S|^t-          t(          d%z  |Qz
  t3          |\|^z            z
            z  z                                   }`|^t/          t(          d%z  |Qz
  t3          |\|^z            z
            z  |_z
                                  }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|L                    d<i |                                d|	                                d|
                                d|                                d|                                d|                                d|d|	d|
|b|Oz  |d|d||b|Pz  | 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%z  z  dCz
  dD|bz  dE|bz  gdFdG|bd%z  z  dHz   dI|bz  dJ|bz  gg          }idKlt?          dL          D ]L}j|i                     |b|j          |d                     |b|j          z
  }ktC          lfdM|kD                       sJ Md S )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     V  d S )N)abs).0xepss     d/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane3.py	<genexpr>ztest_bicycle.<locals>.<genexpr>;  s-      //A3q66C<//////    )"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22NBB'q99Cc3+,?@@BBB#12Eq#I#I Cc3S )00P(Q(Q%E5':'./O'P'P$JW)01R)S)S&GXx$+,H$I$I!E5%,34X,Y,Y)Hh'#*+F#G#G GWgq%&<==FE3 	sA	C"ac++A	C"ac++AKK"w,)<==E			BVwh5HIII??66B=99D~~j&G8TV2DEEH			B IGguqs{33EOOG[57%:;;EzX:+?-5-?,@ A AHooi7TV);+2TV+;*< = =GOOGZ$&%8:;N%NOOEooi#dfac2B2BTV2K23#367@y{{*C D DG MM!R!#XMM!R!#XNN5"uw,'''	aeg&&&UBK(((NN4df%%%
 OOAq	gq"%%%	eQ&&&5)))q$'''	eQ%%%q"%%% y(Hh1hOOQYZGh'1aII7SFE5%//7DD%..6D +xHHIWdE6BBHxC66FxC66F (C"HcBhb	1B [[^^ac)7;;q>>AC+?QRSRUAUVM %%g..45M fWq[13&
'	5&1*qs"#	q13	q13
!B Xvv	.B 
Qr2rlr2,R=&
( 
( 
(B %%b"--LR (+L'+M(*Qb(8'?'?'A'AC'+J*4SXX*=}SQTXX?U*V(W'^'^'`'`D'+G'*M'*M'*L'*L(/C(8$l@Z\_`c\d\d?d:d(e'l'l'n'nK)6)E}UXY\U]U]G]_bcf_g_gFg)gilmpiqiq(q'x'x'z'zK(5C(@MT`D`cpsvwzs{s{c{  B  CF  G  G  dG  EG  KN  OR  KS  KS  DS  )S  (Z  (Z  (\  (\J(4}(DE(/(<E(,UAXeQh->(?(?'F'F'H'HE(/C(8-:TVYZ]V^V^9^(^'e'e'g'gJ(,ec"Q$s(4eCTCT:T6U6U.U(V']']'_'_J(-RT#XtE%K?P?P4P0Q0Q(QR\(\'c'c'e'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
 GGIIq	

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