
    sg3                         d dl mZmZmZmZmZmZ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mZmZmZmZmZmZmZmZ d dlmZ ed        Zd Zd	 Zd
 Zd Z d Z!y)    )symbolsMatrixcossinatansqrtRational)sympify)simplify)solve)dynamicsymbolsReferenceFramePointdotcrossinertiaKanesMethodParticle	RigidBody
LagrangianLagrangesMethod)slowc                     t        d      \  } }}}}t        d      x\  }}}}}	}
}|D cg c]  }|j                  |        c}x\  }}}}}}}t        d      }t        d      x\  }}}}}}}|D cg c]  }|j                  |        c}\  }}}}} }!t        d      }"t	        d      }#|"j                  dd||"j                  g      }$|$j                  d	d||$j                  g      }%|%j                  d
d||%j                  g      }&|#j                  d||"j                  z  |	|"j                  z  z   |
|"j                  z  z         }'|&j                  |"      }(|%j                  |"      })|&j                  |"||%j                  z  ||%j                  z  z   ||%j                  z  z          |'j                  |#      j                  |"      }*|'j                  |"||&j                  z  ||&j                  z  z   ||&j                  z  z          |'j                  d||%j                  z        }+|+j                  |'|"|&       t!        |
t#        |'j                  |+      |"j                        z
  g      },t!        |&D -cg c]  }-t#        |+j%                  |"      |-       c}-      }.t!        |%D -cg c]   }-t#        |(|&j                  |"      z
  |-      " c}-|"D -cg c]   }-t#        |*|'j%                  |"      z
  |-      " c}-z         }/t'        |/|      }0|%j                  |"|)j)                  |0             |&j+                  |"|&j                  |"      j                  |%      t-        |%j                  |"      |&j                  |"            z          ||z  |$j                  z  }1||dz  z  dz  }2||dz  z  dz  }3t/        |&|2|3|2      }4t1        d|'|&||4|'f      }5|5g}6|'|1fg}7t3        |"|||||	g|||g|/|
g|,|||g|.      }8|8j5                  |6|7      \  }9}:|8j7                         };|;j8                  |,k(  sJ |;j:                  |.k(  sJ |;j<                  |.j                  |       j)                  |8j?                               k(  sJ t'        |;j@                  |;jB                  z   |      }<|0jE                         D ]  }|<|   |0|   k(  rJ  tG        |;jH                  |;jJ                  z   |9z
  |:z
        t!        g d      k(  sJ |
| tM        |      z  i}=|d|tO        |      |z  |z   |tM        |      |z  || tO        |      |z  |z   z  tM        |      z  |d|| tO        |      |z  |z   z  tO        |      z  i}>|d|| tO        |      |z  |z   z  tM        |      z  || tO        |      |z  |z   z  tO        |      z  |di}?|d|z  tO        |      z  d|z  z  tO        d|z        |dz  z  dz  z   dtM        |      z  |z  |z  dz  z   |d|d||tO        |      tO        |      z  |z  |z  tO        |      |dz  z  z   z  | |d|z  tO        |      z  d|z  z  tO        d|z        |dz  z  dz  z   dtM        |      z  |z  |z  dz  z   z  |!| tO        |      tM        |      z  |z  |z  tM        |      |dz  z  z   z  i}@|;jQ                  |=|>|?|@gdd      \  }$}%|d|d|d|d|di}At!        g dg dg dtO        |      |z  ddddtO        |       tM        |       dgtM        |       |z  ddddtM        |      tO        |       dgdtS        dd      dddddd|z  dz  gg ddddddd|z  ddgg      }Bt!        g       }C|$j)                  |A      |Bk(  sJ |%j)                  A      Ck(  sJ tU        |$j)                  A      j)                  |dtW        d      z              jY                         ddik(  sJ |8j7                  d       };|;jQ                  |=|>|?@gdd      \  }$}%|$j)                  A      Bk(  sJ |%j)                  A      Ck(  sJ y c c}w c c}w c c}-w c c}-w c c}-w )!Nz	t r m g vzq1:7zu:6zu1:7NNOAAxisBCCOP      Disc)kd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraints)r   r   r   r         Top_pointA_and_Br      )r   r   r   r   r   r   r   r/   )r   r   r   r   r   r/   r   r   )r   r   r   r   r   r   r/   r   )r   r   r   r   r   r   r   r         GJ)linear_solver)-r   r   diffr   r   	orientnewzxy	locatenew
ang_vel_inset_ang_velpos_fromdtset_velv2pt_theoryr   r   velr   subsset_ang_accr   r   r   r   kanes_equationsto_linearizerf_cf_vf_akindiffdictf_0f_1keysr   f_2f_3r   r   	linearizer	   r
   r   	eigenvals)Dtrmgvq1q2q3q4q5q6qqiq1dq2dq3dq4dq5dq6dqduu1u2u3u4u5u6uiu1du2du3du4du5du6dr   r   r   r   r   r    w_c_n_qdw_b_n_qd	v_co_n_qdr!   rF   uvrG   kindiffsqdotsF_COIJI_C_COr$   BLFLKMfrfr_star
linearizersolq_opu_opqd_opud_opupright_nominalA_solB_solsD                                                                       _/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_linearize.py test_linearize_rolling_disc_kaner      s    K(MAq!Q "0!77BBBQ>?(@(@@ Cc3S2 	uA!/!77BBBQ9:#;2BGGAJ#; Cc3S 	sA	tB	C"acc+A	C"acc+A	C"acc+A	dBqssFRVObf4	5B ||AH||AH MM!RVbf_r!##v-. B""1%I JJq"QSS&2acc6/BqssF*+ 	S!ACC% AMM"a "s2;;q>133//0
1C a0#aeeAh#0
1C QGrs8all1o5r:GBCDBY2B7DE FH(BE MM!X]]5)*MM!Q\\!_''*U1<<?ALLQRO-TTU Q3qss7D 
QTQA	
QTQAQ1a FVRA|4D
Bt*B	QRR,r2rl8R3
@B &&r2.MR !!#J>>S   >>S   >>SXXa[--bnn.>????

/
4Cjjl $2w%)###$JNNZ^^3b87BCviGXXXX 3r7
DBc!BCGCK#%&s2w.CGCK#%&s2w.0D !1"c"gckC'(R01"c"gckC'(R0!E !A#c"g+qs#c!B$iQ&6q&881SW9S=;LQ;NN!!!SWSW_S(,s2wsAv~=>!QqSR[!A#&QrT36)9!);;aBimC>OPQ>QQR1"c"gc"goc)#-BQ>?AE $eU)CT\`aDAqAr1aAq!Q7O ,,,WS[!Q1s2whR!D"gXc\1aAs2wR!DA1aAquQw?,1aBsFAq13 4E 2JE 66/"e+++66/"e+++ 166/*//QtAwY?@JJLQRTUPVVVV !!!5J$eU)C(,t   =DAq66/"e+++66/"e+++s )A
 $<> 1 HDs   a1a2"a$%a%a
c                     t        d      } t        d      }t        dd      }t        d      \  }}}d}t        d      }t        d      }|j	                  |d       |j                  d	d
| |j                  g      }	|	j                  |||j                  z         |j                  d||	j                  z        }
|
j                  |||	       t        d|
|      }t        ||z
  g      }||z  |j                  z  }t        || g|g|      }|j                  |g|
|fg      \  }}|j                  dd      \  }	}}|	t        ddgdt!        |       z  |z  dgg      k(  sJ |t        g       k(  sJ y )NrV   rf   r/   L, m, t皙#@r   N*r   r   axisr!   pP)q_indu_indr%   T)r.   r   皙#)r   r   r   r   r?   r6   r7   r<   r:   r8   r@   r   r   r   rD   rO   r   )rV   rf   r^   LrS   rQ   rT   r   pNr   r!   r   kdeRr   r   frstarr   inp_vecs                      r   $test_linearize_pendulum_kane_minimalr      st   		B		B
q
!Ci GAq!A 	sA	tBJJq! 	
C"acc+AMM!RV 	S!ACC% AMM"a	$1	B #(
C 	
!ACCA 
Qrd2$s	;B%%rdaVH5LR LLL=MAq'Ac"ga 345555r
??    c                  \   t        d      \  } }t        dd      \  }}t        d      \  }}t        dd      \  }}t        d      \  }}	}
d}t        d      }t        d      }|j	                  |d	       t        || z        }|j                  d
d||j                  g      }|j                  d| |j                  z  ||j                  z  z         }t        d||	      }t        ||z
  ||z
  g      }t        |||g      }|j	                  ||j                  |      j                  |      j!                  |             t        |j                  |      j#                         |z
  g      }t        |j%                  |      j'                  |      j)                  |j                        g      }|j+                          |j-                  |
      }|j+                          |	|z  |j                  z  }t/        ||g|g| g|g||||	      }|j1                  |g||fg      \  }}| ||d	i}|d	|d	i}|d	|d	i}|j3                  |||gdd      \  }}}|j5                         t        d	dgd|z  d	gg      k(  sJ |t        g       k(  sJ |j3                  |||gddd      \  }}}|j5                         t        d	dgd|z  d	gg      k(  sJ |t        g       k(  sJ |j3                  |||gddd       \  }}}|j5                         t        d	dgd|z  d	gg      k(  sJ |t        g       k(  sJ y )Nq1:3r/   levelzu1:3r   r   r   r   r   r   r   P1r   )r   r   r&   r(   r'   r)   acceleration_constraintsr%   Tr,   r   r3   )r-   r.   r   r4   c                 $    | j                  |      S NLUsolver   bs     r   <lambda>z9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>   s    AIIaL r   )r   r   r   r   r?   r   r6   r7   r:   r8   r9   r   r   r   r=   r>   rB   	magnituderA   expressr   r   r5   r   rD   rO   expand) rV   rW   r^   r_   rf   rg   rm   rn   r   rS   rQ   rT   r   r   theta1r   r!   r   r   dq_dictrF   rG   rH   r   r   r   r   r   r   r   r   r   s                                    r   'test_linearize_pendulum_kane_nonminimalr      sa    F#FBfA.HCF#FBfA.HCi GAq!A 	sA	tBJJq! "R%[F	C&!##/A 	T2acc6BqssF?+A	$1	B #(( CC#s$G IIaB""1%**734 !**R.**,q01
2C !%%(""1%))!##./
0CLLN ((1+CLLN 	
!ACCA 
Qrd2$RD!$s3
PB %%rdaVH5LR 2q>D2q>D!S!ELL4u*=t*. ! 0MAq' 88:!Q$q&! 56666r
?? LL4u*=t)-T ! CMAq' 88:!Q$q&! 56666r
??LL4u*=)-*./H ! JMAq'
 88:!Q$q&! 56666r
??r   c                  ~   t        d      } t        dd      }t        d      \  }}}d}t        d      }t        d      }|j	                  |d       |j                  dd	| |j                  g      }|j                  |||j                  z         |j                  d
||j                  z        }	|	j                  |||       t        d|	|      }
t        ||
      }t        || g|	||z  |j                  z  fg|      }|j                          |j                  | g|gd      \  }}}t!        |      t#        ddgdt%        |       z  |z  dgg      k(  sJ |t#        g       k(  sJ |j                  | g|gdd      \  }}}t!        |      t#        ddgdt%        |       z  |z  dgg      k(  sJ |t#        g       k(  sJ y )NrV   r/   r   r   r   r   r   r   r   r!   r   )	forcelistframeT)r.   r   r3   )r.   r4   )r   r   r   r   r?   r6   r7   r<   r:   r8   r@   r   r   r   form_lagranges_equationsrO   r   r   r   )rV   r^   r   rS   rQ   rT   r   r   r   r!   r   LagLMr   r   s                  r   (test_linearize_pendulum_lagrange_minimalr     s   		B
q
!Ci GAq!A 	sA	tBJJq! 	
C"acc+AMM!SW 	S!ACC% AMM"a	$1	B Q
C	rd1Q3qss7|nA	FB! LL"udL;MAq'A;&1a&4B<>1*=!>????r
?? LL"ud$LOMAq'A;&1a&4B<>1*=!>????r
??r   c                     t        d      \  } }t        dd      \  }}t        d      \  }}}d}t        d      }t        d      }	|	j	                  |d       t        || z        }
|j                  d	d
|
|j                  g      }|	j                  d| |j                  z  ||j                  z  z         }|j	                  ||j                  |	      j                  |             t        d||      }t        | dz  |dz  z   |dz  z
  g      }t        ||      }t!        || |g||||z  |j                  z  fg|      }|j#                          | ||d|d|d|j%                  |      d|j%                  |      di}|j'                  |      }|j)                  |       |j+                  |g|g| g|g|d      \  }}}t-        |      t        ddgd|z  dgg      k(  sJ |t        g       k(  sJ |j+                  |g|g| g|g|dd       \  }}}t-        |      t        ddgd|z  dgg      k(  sJ |t        g       k(  sJ y )Nr   r/   r   r   r   r   r   r   r   r   r   r   r"   )
hol_coneqsr   r   )r-   T)r-   r.   r   c                 $    | j                  |      S r   r   r   s     r   r   z=test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>L  s    !"1 r   )r-   r.   r4   )r   r   r   r   r?   r   r6   r7   r:   r8   r9   r=   r>   r   r   r   r   r   r5   solve_multipliersupdaterO   r   )rV   rW   r^   r_   r   rS   rQ   rT   r   r   r   r   r!   r   rF   r   r   r-   lam_opr   r   s                        r   +test_linearize_pendulum_lagrange_nonminimalr   )  sU   F#FBfA.HCi GAq!AsA	tBJJq!"R%[F	C&!##/A
T2acc6BqssF?+AIIaB""1%&	$1	B
"a%"a%-!Q$&'
(C
Q
C	r2h3Aqs133w<.XY	ZB!Ar1c1c1chhqk1chhqk1MH!!8!4FOOFLL"urdSEt ! -MAq'A;&1a&461+!67777r
?? LL"urdSEH)- >. ! /MAq' A;&1a&461+!67777r
??r   c                     t        d      x\  } }}}t        dd      x\  }}}}t        d      \  }}	}
t        d      }|j                  dd| |j                  g      }|j                  dd||j
                  g      }|j                  dd||j                  g      }t        d	      }|j                  |d
       |j                  d||j                  z        }|j                  |||       t        ||	dz  |dz  z  |	dz  |dz  z  |	dz  |dz  z        }t        d|||	||f      }|	 |
z  |z  t        |      z  |_        t        ||      }t!        ||      }|j#                          | d
|d
|d
|d
|d
|j%                         d
|j%                         d
|j%                         d
i}|j'                  |||d      d
   }t)        g dg dg dd
d
d
d
d|z  d
gd
d|
z  d|z  z  d
d|z  dz  d
d
gg dg      }||k(  sJ y )Nzq1 q2 q3r/   zr m gr   Yr   r   r   r   r   Dmcr#   r"   BodyDT)r   qd_indr-   r.   )r   r   r   r/   r   r   )r   r   r   r   r/   r   )r   r   r   r   r   r/   ir*   r+   )r   r   r   r   r   r   )r   r   r   r6   r7   r8   r9   r   r?   r:   r@   r   r   r   potential_energyr   r   r   r5   rO   r   )rV   rW   rX   r\   r^   r_   r`   rd   rR   rS   rT   r   r   r   r   r   r   rz   r   r   lr-   r   r   s                           r   $test_linearize_rolling_disc_lagranger   Q  s   #J//JBB'
A66MCcBgGAq!sA	C"acc+A	C"acc+A	C"acc+Ac
AIIaO
++eQW
%COOAq!1q51a4<QAq1uq!t|<AgsAq1c(3E S1Wq[3r72E
Q
CQA  Ar1b!QQ
Asxxz1chhj!=H 	
!B4HKA
$$$aAr#vq)bdAaCj!QsU1Wa3$& 'C 8O8r   N)"sympyr   r   r   r   r   r   r	   sympy.core.sympifyr
   sympy.simplify.simplifyr   sympy.solvers.solversr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r   r   r   r   r   r    r   r   <module>r      se    A A A & , '    & , ,B#JNb#L&P#r   