
    Rie`8                         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	             Zd
S )    )evalf)pi)symbols)sqrt)acossincos)Matrix)ReferenceFramedynamicsymbolsKanesMethodinertiamsubsPoint	RigidBodydot)slowON_CIskipc                  h  m 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||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|G"          }L|L                    |K|J          \  }M}Nd#}Od$}Pt-          j        t0          d%z  t0          d&z  z
            }Qd'}Rt-          j        |Rt3          |Q          z  |Pt5          |Q          z  z
             }Sd(}Td#}Ud)}Vd)}Wd*}Xt-          j        |Tt3          |Q          z  |S|P|Oz
  t5          |Q          z  z
  z
            }Yt-          j        |V|Oz
  |Ut3          |Q          z  t5          |Q          z  z
  t3          |Q          z            }Zt-          j        |Ut3          |Q          z  |V|Oz
  |Ut3          |Q          z  t5          |Q          z  z
  t5          |Q          z  z             }[t-          j        |X|Pz
            }\t-          j        |T|Wz
            }]t-          j        t7          |\d%z  |]d%z  z                       }^t-          j        |Tt5          |Q          z  |P|Oz
  t3          |Q          z  z
            }_t-          j        |S|^t3          t0          d%z  |Qz
  t9          |\|^z            z
            z  z             }`t-          j        |^t5          t0          d%z  |Qz
  t9          |\|^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         }d|Lj        }et?          |e|c          }ft?          |d|L                                 |c          }g|f                                }f|g                                }g|f!                                |gz  }h|h"                    g d<g d=          }itG          g d>g d?d@dA|bd%z  z  dBz
  dC|bz  dD|bz  gdEdF|bd%z  z  dGz   dH|bz  dI|bz  gg          }jdJmtI          dK          D ]L}k|j%                    |b|k          |i%                    |b|k          z
  }ltM          mfdL|lD                       sJ Md S )MNzToo slow for CI.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   r4   r8      )r   r4   r:      )r   r         ?r   )r   r   r   r=   g'^"@gltggWI?g>g{_*'տgE_p'@g%g>@gHj@gaGͭg-q=r;   c              3   >   K   | ]}t          |          k     V  d S )N)abs).0xepss     Hlib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane3.py	<genexpr>ztest_bicycle.<locals>.<genexpr>%  s-      //A3q66C<//////    )'r   r   r   r   r   	orientnewzrA   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)n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	ForkCGParr6   val_dictforcing_linMM_full	MM_full_sforcing_lin_sAmatAResierrorrB   sn                                                                                                                @rC   test_bicycler      sM    !   " $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',wrAvR/?'@'@C'+J',wCHH1DmTWX[T\T\F\1]/^'_'_D'+G'*M'*M'*L'*L',wws3xx/?}UaGacfgjckckFkAk/l'm'mK',w0Lm\_`c\d\dNdfijmfnfnMn0npstwpxpx/x'y'yK',w}s3xx/G=[gKgjwz}  B  {C  {C  kC  FI  JM  FN  FN  kN  LN  RU  VY  RZ  RZ  KZ  0Z  ([  ([J',w|m/K'L'LE',ww|/C'D'DE',wtE1HUAX4E/F/F'G'GE',wws3xx/?|A[]`ad]e]e@e/e'f'fJ',wtUSAc$uUZ{J[J[A[=\=\5\/]'^'^J',wusBqDHd5QV;FWFW;W7X7X/XYc/c'd'dI 	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\ ,,..#K !Ggx((I+r~~'7'7BBM!!I!''))M ==??]*D\\\<<<00A }}}}}}#%71%<?P%PRdefRfhz{|h|}#'8A'=@P'PTdefTfiz{|i|} @ @C C1XX 0 0A1-/////////////0 0rE   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    rE   rC   <module>r      s)   " " " " " " ! ! ! ! ! ! % % % % % % 9 9 9 9 9 9 C C C C C C C C C C ' ' ' ' ' '8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 2 2 2 2 2 2 2 2 2 2 X0 X0 X0 X0 X0rE   