
    MZd`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y
)    )evalf)pi)symbols)sqrt)acossincos)Matrix)ReferenceFramedynamicsymbolsKanesMethodinertiamsubsPoint	RigidBodydot)slowON_CIskipc                    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      }/|/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|G"      }L|Lj+                  |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|Lj;                         d   }d|Lj<                  }et?        |e|c      }ft?        |d|LjA                         |c      }g|fj-                         }f|gj-                         }g|fjC                         |gz  }h|hjE                  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 ]=  }kjjK                  b|k      ijK                  |b|k      z
  }ltM        mfdL|lD              r=J  y )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    y w)N)abs).0xepss     J/usr/lib/python3/dist-packages/sympy/physics/mechanics/tests/test_kane3.py	<genexpr>ztest_bicycle.<locals>.<genexpr>%  s     /A3q6C</s   )'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      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',wwrAvR/?'@C'+J',wwCH1DmTWX[T\F\1]/^'_D'+G'*M'*M'*L'*L',wwws3x/?}UaGacfgjckFkAk/l'mK',ww0Lm\_`c\dNdfijmfnMn0npstwpx/x'yK',ww}s3x/G=[gKgjwz}  B  {C  kC  FI  JM  FN  kN  LN  RU  VY  RZ  KZ  0Z  ([J',ww|m/K'LE',www|/C'DE',wwtE1HUAX4E/F'GE',wwws3x/?|A[]`ad]e@e/e'fJ',wwtUSAc$uUZ{J[A[=\5\/]'^J',wwusBqDHd5QV;FW;W7X/XYc/c'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BM!I!'')M ==?]*D\<0A }}#%71%<?P%PRdefRfhz{|h|}#'8A'=@P'PTdefTfiz{|i|} @C C1X 0A1-/////0    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    r   rC   <module>r      sE    " ! % 9 C C '8 8 8 2 2 X0 X0r   