MZd"ddlmZddlmZddlmZmZddlmZddl m Z ddl m Z m Z mZmZmZmZmZddlmZmZddlmZdd lmZdd lmZdd lmZej<Zd Z d Z!dZ"dZ#dZ$dZ%dZ&y))expand)symbols)cossin)Matrix)trigsimp)PinJoint JointsMethodBody KanesMethodPrismaticJointLagrangesMethodinertia)dynamicsymbolsReferenceFrame)raises)zeros)lambdify)solvec|td}td}td||}td\}}td\}}|j ||j zt ||}|j|jk(sJ|j||gk(sJ|j|j||jj zfgk(sJ|jt|gk(sJ|jt|gk(sJ|jt||jz gk(sJ|j!}|t| |jzggk(sJ|j"t|gdggk(sJ|j$tddgd|ggk(sJt'|j(t*sJy)NPCP1zC_ixx gz q_P1, u_P1r)r r rr apply_forceyr framebodiesloads masscenterqrukdesdiff form_eoms forcing_fullmass_matrix_full isinstancemethodr ) rrPinC_ixxgr!r"r)solns Q/usr/lib/python3/dist-packages/sympy/physics/mechanics/tests/test_jointsmethod.pytest_jointsmethodr/s S A S A 4A Cy!HE1 , 'DAqMM!ACC% !S !F <<177 "" " ==QF "" " <z?test_jointmethod_duplicate_coordinates_speeds..-|Ar26r0)speedsctSr5r6r7sr.r8z?test_jointmethod_duplicate_coordinates_speeds..1r9r0ctSr5r6r7sr.r8z?test_jointmethod_duplicate_coordinates_speeds..5r9r0)r rr r r ValueError)rr2r!r"rrr3s @@@r.-test_jointmethod_duplicate_coordinates_speedsr>&s S A S A S A % DAq $1a B aA &B :67 $1Q 'B a1 -B :67 $1a #B aAq )B :67r0c td\}}td\}}td\}}}td}td|}td|} td||||| |jz|j } td || ||| | jz|j } |j ||z|jz| j ||z|jzt|| | } | jt| jtgd gd d d d|dzz|zt|zd|dzz|zz|dz|zt|z|dz|zzgd d |dz|zt|z|dz|zz|dz|zggk(sJt| jtt|g|g| |z|zt||zt|zz||z|zt|zz |dz|zd|z|zz|zt|zzg| |z|zt||zz|dz|z|dzzt|zz ggk(sJy)Nzq1 q2zu1 u2zm l grrmassRJ1r: coordinates child_point joint_axisJ2)rrrr)rrrrr)rrr r xzrr r%rr'rrrr&r) q1q2u1u2mlr,rPartPPartRrCrHr)s r.$test_complete_simple_double_pendulumrU7s G $FB G $FBgGAq! S A 1 E 1 E $5 bjQSS :B $uRR bjUWW >B ac!##g ac!##g !R $F  &)) *fl6B78!Qq!tVAXc"g=MPQRSUVRVPVWXPX=XZ[]^Z^_`Z`adegahZhklnokopqkqZq6r78!QT!VCG^aQRdSTf=TVWYZVZ[\V\6]6_/` `` ` F'' (HVbTB41"Q$q&RUVX[]V]R^adegahRhJi+,Q3q5R=K9;AaAcRTgAU+U*V=X6Y-Z ZZ Zr0cRtd\}}}}td\}}}}}td} td|} td|} td| | ||} td | | ||} | j ||z| j z| | j ||z| j z| | j ||z| j z| | j ||z| j z| t | | | }|j|j}|j}|j|}t|d t| |z||zz ||zz||zz|z k(sJt|d t||z||zzd |z|zz d |z|zz |z k(sJy)Nz q1 q2 u1 u2z m c1 c2 k1 k2WB1r@B2rC)rEr:rH) reaction_bodyrrrI) rrr r rrKr r% mass_matrixforcingLUsolver)rMrNrOrPrQc1c2k1k2rWrXrYrCrHr)MMr\rhss r.test_two_dof_jointsrdQs#M2NBB0Ar2r2 S A d B d B aB ?B b""R @BMM"R%)2M.MM"R%)2M.NN2b59BN/NN2b59BN/ !R $F    BnnG **W C #a&>VbS2XR%7"r'%ABG%KQ$NO OO O #a&>VR"WrBw%6R"%Dq$&H')+H,&,/0%12 22 2r0c td\}}}td}td|}td}td|||j t || |j z|j}| |z|zt|z|_ t||}|jt|j}|d| t|z|z k(sJy) Nzl m grbr@r!rrDr)rr rr r$trKrLrpotential_energyr r%rrcr) rRrQr,rrfr!rr)rcs r.test_simple_pedulumrifsgGAq! S A SqAsAa166!9!R!##X!## 7Aq1s1v-A !Q F _% **,C q6aRAYq[  r0c td\ }}}}}}}}}td\} } } } td} td}td|| t | ||d}td||t ||||}td }t d ||| | | |j z|j }t d ||| | ||z |j z|j }|j||z|j z|j||z|j zt|||}|j|j}|j}|j|}d|z| z| zt| zt| zd|z| z| zt| zt| zz||z|zt| zz ||z|zt| zz ||t| dzzz|t| dzzz|dz|zz|dz|zzz }|d|z j!dk(sJ||z | dzzt| zt| z|z }|d|z j!dk(sJy)Nz)mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, gztheta phi omega alphaABrodr)rArcentral_inertiaplaterrC)rEr:rFrGrH)rEr: parent_pointrGrIr)rrrr rr rLrrr r%r[r\r]rrsimplify)mAmBlAlBIAxxIBxxIByyIBzzr,thetaphiomegaalpharkrlrmrorrCrHr)rbr\rcxds r.test_chaos_pendulumrss078c0d-BBD$dA-.EFE3usAsA u2Q4q8Q RC rGAtTSW@BA PK LB FRK ! ! #q (( ( + !CH ,s3x 7$ >B FRK ! ! #q (( (r0c )td\}}}}}}td\}}}} } td} |||| fD cgc]} t| dd| | dzzdz } } t d| | |z| d}t d | |z| d  }t d | |z| d  }t d| | z| d }t d|||||j |d z |jz| d z |jz}t d|||||j |d z |jz| d z |jz}t d|||||j |d z |jz| d z |jz}|jj|j|d z |jzz| d z |jzz}t|j|j|j|jg}t||||}tj}t|j |j#||j#||j#|g}|j#|j%|}t'|j(|g|g||g||g|j |||j*|j, }|j/\}}|t1d k(sJt|||| | g}t|||g}t|||g} t3||f|j4}!t3|| |f|j6}"t3|| |f|}#gd}$gd}%gd}&tgdgdgdg}'tdgdgdgg}(d)t9)fd|#|%|&|$DsJt9)fdt|!|%|$|'z DsJt9)fd t|"|%|&|$|(z DsJycc} w)!Nz q1:4, u1:4z l1:5, rhoNrrJ Link1)rrArnLink2r)rArnLink3rILink4rC)rEr:rGrprFrHJ3) q_indu_ind q_dependent u_dependentkd_eqsconfiguration_constraintsvelocity_constraints forcelistr)gp= ף?gQ?gzG?g(\?i)g@gtM)L?gF!8@)g?g68[ƿgൄ?)gDwCA@g_' @gY>)ggTB?gv1?)g_r= ףg0Sg;?gLgd`Egop]?g|=c3:K|]}t|kywr5abs.0rKepss r. z@test_four_bar_linkage_with_manual_constraints..sFs1v|Fc3:K|]}t|kywr5rrs r.rz@test_four_bar_linkage_with_manual_constraints..s>s1v|>rc3:K|]}t|kywr5rrs r.rz@test_four_bar_linkage_with_manual_constraints..sIs1v|Ir)rrrrr r rLrKr pos_fromrdotrr _trr#r$subsr rrrkanes_equationsrrr[r\all)*rMrNq3rOrPu3l1l2l3l4rhorrRinertiaslink1link2link3link4joint1joint2joint3loopfhr)rgqdotsfhdkanefrfrspr!r"eval_meval_feval_fhdp_valsq_valsu_vals mass_check forcing_checkrs* @r.-test_four_bar_linkage_with_manual_constraintsrs+L9BBB!+.BBCsA>@"b"=MN1aqAv!23NHN b(1+ NE sRx! EE sRx! EE sRx! EE  eU2%''!Vegg%B37UWW3DFF eU2%''!Vegg%B37UWW3DFF eU2%''!Vegg%B37UWW3DFF    $ $U%5%5 6 !Vegg   "Q 0 1D %''"DHHUWW$56 7B % 8FA &++ BGGAJ C DE ''!*//% C v||B4t$&8"b$kkR,/6<<$mm  -D ""$GB q>> BB$%AB|AB|A q!fd.. /F q!Qi .FAq 3'H *F 9F vff-.;> >> > Ivfff56FI II IwOsO(N)'sympy.core.functionrsympy.core.symbolr(sympy.functions.elementary.trigonometricrrsympy.matrices.densersympy.simplify.trigsimprsympy.physics.mechanicsr r r r r rrsympy.physics.vectorrrsympy.testing.pytestrsympy.core.backendrsympy.utilities.lambdifyrsympy.solvers.solversrrrgr/r>rUrdrirrr0r.rsi&%?',NNN?'$-'2(8"Z42* !)@AIr0