
    g!&h8K                         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 d Zd Zd Zd	 Zy
)    )cosMatrixsinzerostanpisymbols)simplify)trigsimp)solve)
crossdotdynamicsymbolsfind_dynamicsymbolsKanesMethodinertiainertia_of_point_massPointReferenceFrame	RigidBodyc                     t        d      \  } }}}}}t        d      \  }}}t        d      }	|	D 
cg c]  }
|
j                  |        }}
t        d      }|D cg c]  }|j                  |        }}t        t	        |dgt        |      z              }t        d      }t        t	        |dgt        |      z              }t        d      }|j                  dd	|	d
   |j                  g      }|j                  dd	|	d   |j                  g      }|j                  dd	|	d   |j                  g      }|j                  ||d
   |j                  z  |d   |j                  z  z   |d   |j                  z  z          |j                  ||j                  |      j                  | |      t        |j                  |      |j                  |            z          t        d      }|j!                  ||d
   |j                  z  |d   |j                  z  z   |d   |j                  z  z          |j#                  d|	d   |j                  z  |t%        |	d         z  |j                  z  z         }|j!                  ||d   |j                  z  |d   |j                  z  z   |d   |j                  z  z          |j'                  ||j)                  |      j                  | |      t        |j                  |      |j)                  |            z          |d
   |j                  z  |d   |j                  z  z   |d   |j                  z  z   }|j+                  |      j                  | |      t        |j                  |      |j+                  |            z   }t-        |D cg c]   }t/        ||j                  |      z
  |      " c}t/        ||j)                  |      z
  |j                        gz         }t1        ||      }t1        |j3                  |d   d
|d   d
i      |      }|j5                  |d   d
|d   d
i       ||z   D cg c]#  }|j                  |      j                  ||      % }}||z   D cg c]#  }|j)                  |      j                  ||      % }}||z   D cg c]#  }|j)                  |      j                  ||      % } }t-        t/        | |j                  z  |j                        |	d   z
  g      }!t-        |D "cg c]h  }"t/        |j)                  |      |j)                  |      t        |j                  |      |j+                  |            z   z
  |"      j7                         j c}"      }#t        |j                  |      |j+                  |            }$|$j                  | |      t        |j                  |      |$      z   }%t-        |D "cg c]   }"t/        |j9                  |      |%z
  |"      " c}"      }&t;        dd      }'t=        d      D ]1  }(t?        ||z         D ]  \  })}|#|(   j                  |      |'|(|)f<     3 |'d d d df   }*|'d d ddf   }+|'d d dd f   },|*jA                  |,      }-|+jC                          |-z  }.|.d d d df   t-        |d d       z  }/t        t	        |dd  |/            }0||z  |j                  z  }1||j                  z  ||j                  z  z   ||j                  z  z   }2t-        t	        ||       D 34cg c]  \  }3}4t/        |1|3      t/        |2|4      z     c}4}3      }5| |j9                  |      z  }6tE        ||||      }7t/        |7|jG                  |            t        |j                  |      t/        |7|j                  |                  z    }8t-        t	        ||      D 9:cg c]  \  }9}:t/        |6|9      t/        |8|:      z     c}:}9      };|5d dd d f   jI                  |5dd d d f         |.jJ                  |5ddd d f   z  z   }<|;d dd d f   jI                  |;dd d d f         |.jJ                  |;ddd d f   z  z   }=|=j3                  |      j3                  |0      j3                  |      j3                  |	d   | tM        |	d         z  i      j7                         }>|7|f}?tO        d||||?      }@|@g}A||1f}B||2f}C|B|Cg}DtQ        ||	d d |d d ||	dd  |!|dd  |#|	      }E|EjS                  |A|D      \  }F}G|Gj3                  |      j3                  |0      j3                  |      j3                  |	d   | tM        |	d         z  i      j7                         }H|EjU                         }It-        |<      j7                         |Fj7                         k(  sJ t-        |=j3                  I            j7                         Gj7                         k(  sJ tW        t-        |>      j7                               jY                  d
di      tW        Hj7                               jY                  d
di      k(  sJ t[        Ej\                        }J|D ]  }K|KJvrJ  y c c}
w c c}w c c}w c c}w c c}w c c}w c c}"w c c}"w c c}4}3w c c}:}9w )Nzt r m g I JzFx Fy Fzzq:4zu:6g        zua:3NAAxisr   B   C   PO         	      disc)q_indu_indkd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraintsu_auxiliary)/r	   r   diffdictziplenr   	orientnewzxyset_ang_velset_ang_acc
ang_vel_inr   r   set_vel	locatenewr   set_accvelpos_fromr   r   r   subsupdateexpandaccr   range	enumeraterow_joininvr   
ang_acc_incol_joinTr   r   r   kanes_equationskindiffdictr
   xreplacer   forcing)LtrmgIJFxFyFzqqiqduuiudud_zerouaua_zeror   r   r   r   r   r    w_c_n_qdv_o_n_qduvkindiffsqd_kdsteady_conditionspartial_w_Cpartial_v_Opartial_v_Pf_caif_vv_o_na_o_nf_aM_vijM_v_iM_v_dM_v_aux	M_v_i_auxA_rsu_dep
u_dep_dictF_OF_Ppv_opv_pFr_uR_star_OI_C_OT_star_Cpvpav	Fr_star_uFr_c	Fr_star_cFr_star_steady
iner_tupler&   bodyListF_oF_p	forceListkanefrfrstarfrstar_steadykddsyms_in_forcingqdisL                                                                               ~/var/www/pru.catia.catastroantioquia-mas.com/valormas/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane2.pytest_aux_depr      s	   " }-Aq!Q1$JBB& 	uA 	!"''!*	!B	!uA 	!"''!*	!B	!3rB4B<()G		B3rB4B<()G 	sA	C!A$-A	C!A$-A	C!A$-A MM!QqT!##X!QSS(1Q4834MM!Q\\!_))!Q/1<<?ALLO<= > 	c
AIIaAqssRU133Y&Aqss23	C1accAc!A$iKO34AIIa1accAaDH$qtACCx/0IIaqq!$uQ\\!_aeeAh'GGH !uQSSy2a59$r!uQSSy0Hzz!}!!!Q'%QA*OOHQGrs8all1o5r:G8aeeAh.456 7H(BE hmmRUQ1,BCQGbeaA34 :;R@21<<?''A.@K@23b&9B1558==Q'9K923b&9B1558==Q'9K9 #qbfacc"QqT)*
+C
013*, aeeAh!%%(U1<<?	

1. # #VX& 3 4C!,,q/1::a=1EJJq!uQ\\!_e<<E
a8#aeeAh&+8
9C 1+C1X (q2v& 	(EArABC1I	(( 2A2JE1Q3KE!QR%jGw'IYY[=9$DBQBK&2A-'Ec!AB%'(J A#acc'C
qss(R!##X
QSS
(C[)+ztT3sD>CTN2 + ,D r!%%({HAq!QEUALLO,a#eQ\\!_*EFG HHK5772sHb)C#,>> 7 8I
 A;QRU,tvvQqS!V/DDD"1"a% )))ABE*:;&&9QqS!V,,-I^^G,11*=T#$TT1Q4!C!I*>%?  JVQ1j1DvH c(Cc(CsI 	!BQ%"1habEsabE	
D &&x;LRKK(--j9>>?PQT1Q4!C!I./ 



C$< BIIK///)..%&--/6==?BBB VN+2245>>#wG]))+,55qg>? @ ? *$,,7O */)))*e 
"	!J H A993 96+7s<   jj%j(j 7(j%((j*A-j/$%j4(#j9
#j?
c                  <   t        d      \  } }}t        dd      \  }}}t        d      \  }}}}	}
t        d      \  }}}}}}}t        d      \  }}}}}}}}t        d      \  }}}t        d      \  }}}t        d	      } | j                  d
d| | j                  g      }!|!j                  dd| |!j
                  g      }"|"j                  | ||"j
                  z  ||"j                  z  z          |"j                  dd||"j                  g      }#|"j                  dd||"j                  g      }$|#j                  |"|	|"j                  z         |$j                  |"|
|"j                  z         t        d      }%|%j                  |"d       |%j                  | ||"j                  z         |%j                  d||"j                  z        }&|%j                  d||"j                  z  ||"j
                  z  z
        }'|&|'fD ]  }(|(j                  |%| |"        |%j                  d||"j                  z        })|%j                  d||"j                  z        }*|%j                  d| |"j                  z        }+|)|*|+fD ]  }(|(j                  |%| |"        |*j                  d| |"j
                  z        },|+j                  d| |"j
                  z        }-|,j                  |*| |#       |-j                  |+| |$       ||z
  ||	z
  ||
z
  g}.|,|-fD (cg c]'  }(t        |(j                  |       |"j                        ) }/}(t        |"|||d|d      }0t        |#|||      }1t        |$|||      }2t        d|)|"||0|)f      }3t        d|*|#||1|*f      }4t        d|+|$||2|+f      }5t!        | | ||g||g|.|	|
g|/|g      }6|&| |z  | j
                  z  f|'||"j
                  z  ||"j                  z  z   ||"j                  z  z   fg}7|3|4|5g}8|6j#                  |8|7      \  }9}:t%        |/|	|
g      };t'        |d|z  |dz  z  |dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |j)                  |      z  ||z  |z  |z  z
  |d|z  z   d|z  |dz  z  z    |j)                  |      z  ||z  |dz  z  z   dg      }<t+        |:j-                  |;      j-                  |di            j/                         j1                         }|<|z
  j1                         t3        dd      k(  sJ g }=t5        |3|4|5g|0|1|2g      D ]  \  }>}?|?t7        |>j8                  |>j:                  j=                  |%      |>j>                        z   }@|=jA                  t        d|>j:                  |>j>                  |>j8                  |@|%f              |6j#                  |=|7      \  }A}Bt+        |Bj-                  |;      j-                  |di            j/                         }|<|z
  j1                         t3        dd      k(  sJ y c c}(w )Nq1:4r   levelzu1:6u' R, M, g, e, f, thetaa b mA mB IA J K tz	Q1, Q2 Q3IA22 IA23 IA33Fr   axisr   r   r   Dr   S*QA*B*C*B^C^rbArbBrbC)r'   r(   r)   r,   r-   r.   r   r!    )!r   r	   r   r3   r6   r5   r7   r4   r   r:   r;   v2pt_theoryr   r=   r   r   r   rJ   r   r   r/   r   r?   doitrA   r   r1   r   mass
masscenterr>   frameappend)Cq1q2q3q1dq2dq3du1u2u3u4u5u_primeRMrQ   efthetaabmAmBIArS   KrN   Q1Q2Q3IA22IA23IA33r   r   r   r   r   pDpS_starpQppA_starpB_starpC_starpB_hatpC_hatkdevc	inertia_A	inertia_B	inertia_Cr   r   r   kmforcesbodiesr   fr_starvc_mapfr_star_expectedbodies2rbI_starrR   fr2fr_star2sC                                                                      r   test_non_central_inertiar      s/     'JBB"63MCc'/BBB$+,F$G!GQ1aE '(< =Aq"b"aA%JBB/0D$ 	sA	C5&!##/A	C"acc+AMM!RVbf_%	C"acc+A	C"acc+AMM!R!##XMM!R!##X 
sBJJq!JJq"qss(ll4133'G	c1QSS51QSS5=	)Br]  	b!Q  ll4133'Gll4133'Gll4!ACC(Gw(  	b!Q  taRV,FtaRV,F
w1%
w1% 8S2XsRx
(C'-v&6	7#aeeAh
	7B	7 2tT1dA6I1a#I1a#I E7ArIw+?
@C
E7ArIw+?
@C
E7ArIw+?
@C	Qr2rl2r(3"$b"$
'B 1QSS!B133ACC"QSS&(@#ABF3_F$$VV4KB2Bx F 1Q3q!t8AqD= 1Q3&Ag"QT	" #%'WWQZ024Q$r'"*=1R4i1QT!"RWWQZ/"Q$r1u*<	 
 	f%**B73499;BBDA!))+uQ{:;: G3S/Iy)+LM +
F*277+-==+A+A"+E+-885 5 	yR]]BHHbgg"#R* 	+	+ &&w7MCv&++RG45::<Aq ((*eAqk999] 
8s   ,Xc            	      
   t        d      \  } }}t        dd      \  }}}t        d      \  }}}t        d      \  }	}
}}}}}t        d      \  }}}}}}}}t        d      \  }}}t        d      \  }}}t        d	      }|j                  d
d| |j                  g      }|j                  dd| |j
                  g      } | j                  ||| j
                  z  || j                  z  z          | j                  dd|| j                  g      }!| j                  dd|| j                  g      }"t        d      }#|#j                  | d       |#j                  ||| j                  z         |#j                  d|| j                  z        }$|#j                  d|| j                  z  |
| j
                  z  z
        }%|#j                  d|| j                  z        }&|#j                  d|| j                  z        }'|#j                  d| | j                  z        }(|$|%|&|'|(fD ]  })|)j                  |#||         |'j                  d|
 | j
                  z        }*|(j                  d|
 | j
                  z        }+|*j                  |'||!       |+j                  |(||"       |*|+fD )cg c]'  })t        |)j                  |      | j                        ) },})|,||z
  gz  },t        |,|||g      }-t        |-j!                               D ](  \  }.}/|/j#                  |      |-|.j#                  |      <   * t%        | |||d|d      }0t%        |!|||      }1t%        |"|||      }2t'        d|&| ||0|&f      }3t'        d|'|!||1|'f      }4t'        d|(|"||2|(f      }5t)        || ||g||g|,|g      }6|$| |z  |j
                  z  f|%|| j
                  z  || j                  z  z   || j                  z  z   fg}7|3|4|5g}8t+        ||z  ||z  |z  t-        |      z  t/        |       z  z   |||z  t-        |      z  t-        |       z  z   ||z  |z  t/        |      z  ||z  z
  ||
z  z
  g      }9t+        |d|z  |dz  z  |
dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |j#                  |      z  ||z  |z  |z  z
  |d|z  z   d|z  |
dz  z  z    |j#                  |      z  ||z  |dz  z  z   dg      }:|6j1                  |8|7      \  };}<|;j3                         |9j3                         k(  sJ |:t5        |<      z
  j3                         t7        dd      k(  sJ y c c})w )Nr   r   r   zu1:4r   r   r   zQ1 Q2 Q3r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   )r)   r.   r   r!   )r   r	   r   r3   r6   r5   r7   r4   r   r:   r;   r   r   r=   r   listitemsr/   r   r   r   r   r   r   rJ   rA   r   r   )=r   r   r   r   r   r   r   r   r   r   r   r   rQ   r   r   r   r   r   r   r   r   rS   r   rN   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   kde_mapkvr   r   r   r   r   r   r   r   r   fr_expectedr   r   r   s=                                                                r   test_sub_qdotr   *  sw   
  'JBB"63MCc'JBB$+,F$G!GQ1aE '(< =Aq"b"aA/0D$$JBB 	sA	C5&!##/A	C"acc+AMM!RVbf_%	C"acc+A	C"acc+A 
sBJJq!JJq"qss(ll4133'G	c1QSS51QSS5=	)Bll4133'Gll4133'Gll4!ACC(Gr7GW5  	b!Q  taRV,FtaRV,F
w1%
w1% )/'7
8!3quuQx
8C
8BH:CC#sC)GW]]_% '1VVAYq	' 2tT1dA6I1a#I1a#I E7ArIw+?
@C
E7ArIw+?
@C
E7ArIw+?
@C 
QRr2hs	MB1QSS!B133ACC"QSS&(@#ABF3_F bD1Q3q5U#CG++1SZB''aCE#e*r!t#bd*, -K
 1Q3q!t8AqD= 1Q3&Ag"QT	" #%'WWQZ024Q$r'"*=1R4i1QT!"RWWQZ/"Q$r1u*<	  $$VV4KBIIK;--//0/ 1199;uQ{JKJS 9s   ,Uc                  T   t        d      \  } }}}}}}t        d      }t        dd      }t        d      }	t        d      }
|
j                  dd|d	   |
j                  g      }|j                  d
dt
        dz  |d   z
  |j                  g      }|j                  dd|d   |j                  g      }t        d      }|j                  |
d	       |j                  d|d   |
j                  z  |d   |
j                  z  z         }|j                  |
|j                  |      j                  ||
             |j                  |d	       |j                  dd	      }|j                  |d	       |j                  d||j                  z        }|j                  |d	       |j                  |d	       |j                  ||
|       |j                  ||
|       ||
j                  z  ||
j                  z  z   ||
j                  z  z   }| | z  |
j                  z  }||f||fg}|D cg c]  }|j                  |
      |z   }}||dd  z  }t        |	|      D cg c]
  \  }}||z
   }}}t!        |
||	|      }|j#                  g |      \  }}|	d d }t%        t'        |	      t'        |      z
        }|
j                  |
j                  fD cg c]  }|j)                  |
      |z   }}t!        |
|||||      } | j#                  g |      \  }!}t+        | | z  |z  t-        |d         z  | |t/        |d	         z  |t-        |d	         z  z   z  t1        |d         z  ||t/        |d	         z  |t-        |d	         z  z   z  ||g      }"t+        | | z  |z  t-        |d         z  d	d	g      }#t3        |j5                               t3        |"j5                               k(  sJ t3        |!j5                               t3        |#j5                               k(  sJ y c c}w c c}}w c c}w )Nzg m Px Py Pz R tzq:5r   r   zu:5r   B_primer   r   r   r   r   r    r   r!   r"   r   r   )r,   r-   )r	   r   r   r3   r4   r   r5   r   r:   r;   r6   r>   r/   r   r9   r1   r   rJ   r   setr=   r   r   r   r   r   rA   )$rQ   rP   PxPyPzr   rN   rW   rY   rZ   r   r   r   r   pOpRr   pCsR_C_hatR_Csr   rb   u_exprr[   r   r   km1fr1_u_indeprw   r   km2r   fr1_expectedfr2_expecteds$                                       r   test_sub_qdot2r    s    %%78Aq"b"auA	Q	'BuA 	sAkk)VadACC[9G#v1qtWYY'?@A	C!A$-A 
sBJJq! 
c1Q48ad133h.	/BJJq"++b/&&q!,-JJq! \\$"F
NN1a 

4133
'CKK1KK1 OOB1
sAq! fr!##vo133&G2a48Dw#t-F
 .//rall1o"/F/
bfF"1f~
.eb!26
.C
.
aAs
#C  V,FC eGQ#g,&'E()QSSz	2&**Q-"
	2B	2
aGS"'bBC  V,FC	
1Qs1Q4y	
Bs1Q4yL2c!A$i<'(QqT2	2c!A$i<"S1Y,&'

 L 	
1Qs1Q4y		 L SZZ\"h|/B/B/D&EEFESZZ\"h|/B/B/D&EEFE5 0
. 
3s   "PP
P%N)sympyr   r   r   r   r   r   r	   sympy.simplify.simplifyr
   sympy.simplify.trigsimpr   sympy.solvers.solversr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   r   r   r       r   <module>r
     sF    ; ; ; , , '@ @ @y*xa:FVLpNGr	  