
    g8K                         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d
S )    )cosMatrixsinzerostanpisymbols)simplify)trigsimp)solve)
crossdotdynamicsymbolsfind_dynamicsymbolsKanesMethodinertiainertia_of_point_massPointReferenceFrame	RigidBodyc                    :;<=>?@ABCD t          d          \  C} }}}}t          d          \  }}}t          d          }Cfd|D             }	t          d          }
Cfd|
D             }t          t          |dgt	          |          z                      }t          d          }t          t          |dgt	          |          z                      }t          d	          ==                    d
d|d         =j        g          }|                    dd|d         |j        g          }|                    dd|d         |j	        g          ::
                    =|
d         |j        z  |
d         |j	        z  z   |
d         |j        z  z              :                    =:                    =                              C|          t          |                    =          :                    =                    z              t          d          ??                    =|d         |j        z  |d         |j	        z  z   |d         |j        z  z              ?                    d|d         |j        z  | t%          |d                   z  |j	        z  z             >>                    =|
d         |j        z  |
d         |j	        z  z   |
d         |j        z  z              >                    =>                    =                              C|          t          |                    =          >                    =                    z              |	d         |j        z  |	d         |j        z  z   |	d         |j	        z  z   D>                    ?                              C|          t          |                    =          >                    ?                    z   }t-          :=Dfd|D             t/          |>                    =          z
  |j                  gz             }t1          ||	          }t1          |                    |	d         d|	d         di          |
          }|                    |	d         d|	d         di           :=fd|
|z   D             }=>fd|
|z   D             }=?fd|
|z   D             }t-          t/          |  |j        z  |j                  |d         z
  g          }t-          :=>?fd|D                       }t          :                    =          >                    ?                    }|                    C|          t          |                    =          |          z   Bt-          =>Bfd|D                       }t7          dd          }t9          d          D ]:}t;          |
|z             D ]%\  }}||                             |          |||f<   &;|d d d df         } |d d ddf         }!|d d dd f         }"|                     |"          }#|!                                 |#z  }$|$d d d df         t-          |
d d                   z  }%t          t          |
dd          |%                    }&||z  |j        z  ;||j        z  ||j	        z  z   ||j        z  z   <t-          ;<fdt          ||          D                       }'| >                     =          z  @tC          ||||          }(t/          |(:"                    =                    t          :                    =          t/          |(:                    =                              z    At-          @Afdt          ||          D                       })|'d dd d f         #                    |'dd d d f                   |$j$        |'ddd d f         z  z   }*|)d dd d f         #                    |)dd d d f                   |$j$        |)ddd d f         z  z   }+|+                    |                              |&                              |                              |d         |  tK          |d                   z  i          &                                },|(>f}-tO          d >:||-          }.|.g}/>;f}0?<f}1|0|1g}2tQ          =|d d         |
d d         ||dd          ||
dd          ||!	  	        }3|3)                    |/|2          \  }4}5|5                    |                              |&                              |                              |d         |  tK          |d                   z  i          &                                }6|3*                                }7t-          |*          &                                |4&                                k    sJ t-          |+                    |7                    &                                |5&                                k    sJ tW          t-          |,          &                                          ,                    ddi          tW          |6&                                          ,                    ddi          k    sJ t[          |3j.                  }8|	D ]}9|9|8vsJ 	d S )"Nzt r m g I JzFx Fy Fzzq:4c                 :    g | ]}|                               S  diff).0qits     d/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane2.py
<listcomp>z test_aux_dep.<locals>.<listcomp>1   #    	!	!	!"''!**	!	!	!    zu:6c                 :    g | ]}|                               S r   r   )r   uir   s     r   r    z test_aux_dep.<locals>.<listcomp>3   r!   r"   g        zua:3NAAxisr   B   C   PO         c                 \    g | ](}t                                        z
  |          )S r   )r   
ang_vel_in)r   uvr*   r%   w_c_n_qds     r   r    z test_aux_dep.<locals>.<listcomp>X   s2    GGGrs8all1oo5r::GGGr"   c                 b    g | ]+}                                                   |          ,S r   )r2   r   )r   r$   r*   r%   s     r   r    z test_aux_dep.<locals>.<listcomp>b   s3    @@@21<<??''A..@@@r"   c                 b    g | ]+}                                                   |          ,S r   velr   )r   r$   r%   r-   s     r   r    z test_aux_dep.<locals>.<listcomp>c   1    999B15588==Q''999r"   c                 b    g | ]+}                                                   |          ,S r   r7   )r   r$   r%   r,   s     r   r    z test_aux_dep.<locals>.<listcomp>d   r9   r"   c                    g | ]}t                                                                      t                                                                                z   z
  |                                          S r   )r   r8   r   r2   pos_fromexpand)r   air*   r%   r-   r,   s     r   r    z test_aux_dep.<locals>.<listcomp>k   s     3 3 3*, aeeAhh!%%((U1<<??	

1. . #  #VXX3 3 3r"   c                 \    g | ](}t                                        z
  |          )S r   )r   acc)r   r>   r%   r-   a_o_ns     r   r    z test_aux_dep.<locals>.<listcomp>o   s2    888#aeeAhh&++888r"   	      c                 X    g | ]&\  }}t          |          t          |          z   'S r   r   )r   pv_opv_pF_OF_Ps      r   r    z test_aux_dep.<locals>.<listcomp>   s>     + + +ztT3sD>>CTNN2 + + +r"   c                 X    g | ]&\  }}t          |          t          |          z   'S r   rE   )r   pvpavR_star_OT_star_Cs      r   r    z test_aux_dep.<locals>.<listcomp>   sB     7 7 772sHb))C#,>,>> 7 7 7r"   disc)q_indu_indkd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraintsu_auxiliary)/r	   r   dictziplenr   	orientnewzxyset_ang_velset_ang_accr2   r   r   r   set_vel	locatenewr   set_accr8   r<   r   r   r   subsupdater   range	enumeraterow_joininvr@   r   
ang_acc_incol_joinTr   r=   r   r   kanes_equationskindiffdictr
   xreplacer   forcing)ErmgIJFxFyFzqqduudud_zerouaua_zeror&   r(   v_o_n_qdkindiffsqd_kdsteady_conditionspartial_w_Cpartial_v_Opartial_v_Pf_cf_vv_o_nf_aM_vijr$   M_v_iM_v_dM_v_aux	M_v_i_auxA_rsu_dep
u_dep_dictFr_uI_C_O	Fr_star_uFr_c	Fr_star_cFr_star_steady
iner_tuplerO   bodyListF_oF_p	forceListkanefrfrstarfrstar_steadykddsyms_in_forcingqdir*   rH   rI   r%   r-   r,   rM   rN   rA   r   r4   sE                                                             @@@@@@@@@@@r   test_aux_depr      s
   " }--Aq!Q1$$JBB& 	uA	!	!	!	!q	!	!	!BuA	!	!	!	!q	!	!	!B3rB4B<(())G			B3rB4B<(())G 	sA	C!A$--A	C!A$--A	C!A$--A MM!QqT!#X!QS(1Q483444MM!Q\\!__))!Q//1<<??ALLOO<<= > > > 	c

AIIaAqsRU13Y&Aqs2333	C1acAc!A$iiKO344AIIa1acAaDH$qtACx/000IIaqq!$$uQ\\!__aeeAhh'G'GGHHH !uQSy2a59$r!uQSy0Hzz!}}!!!Q''%QA*O*OOHGGGGGGQGGG8aeeAhh.4456 7 7H(BE hmmRUQ1,BCCQGGbeaA3444 A@@@@R@@@K99999!b&999K99999!b&999K #qbfac""QqT)*
+
+C
 3 3 3 3 3 3 3013 3 3 4 4C!,,q//1::a==11EJJq!uQ\\!__e<<<E
888888a888
9
9C 1++C1XX ( (q2v&& 	( 	(EArABC1II	( 2A2JE1Q3KE!!!QRR%jGw''IYY[[=9$DBQBK&2A2--'Ec!ABB%''((J A#ac'C
qs(R!#X
QS
(C + + + + +[))+ + + , ,D r!%%(({HAq!QEUALLOO,,a#eQ\\!__*E*EFFG HH 7 7 7 7 7K557 7 7 8 8I
 AAA;QRRU,,tvQqS!!!V/DDD"1"aaa% )))ABBE*:;;&9QqS!!!V,,-I^^G,,11*==T#$$TT1Q4!C!II*>%?%?  JVQ1j11DvH c(Cc(CsI 	!BQB%"1"habbEsabbE	
 
 
D &&x;;LRKK((--j99>>?PQQT1Q4!C!II.// 




C$<<  BIIKK////)..%%&&--//6==??BBBB VN++224455>>#wGG]))++,,55qg>>? ? ? ? *$,77O * */)))))* *r"   c                    AB t          d          \  } }}t          dd          \  }}}t          d          \  }}}}	}
t          d          \  }}}}}}}t          d          \  }}}}}}}}t          d          \  }}}t          d          \  }}}t          d	          BB                    d
d| Bj        g          } |                     dd| | j        g          AA                    B|Aj        z  |Aj        z  z              A                    dd|Aj        g          }!A                    dd|Aj        g          }"|!                    A|	Aj        z             |"                    A|
Aj        z             t          d          }#|#	                    Ad           |#	                    B|Aj        z             |#
                    d|Aj        z            }$|#
                    d|Aj        z  |Aj        z  z
            }%|$|%fD ]}&|&                    |#BA           |#
                    d|Aj        z            }'|#
                    d|Aj        z            }(|#
                    d| Aj        z            })|'|(|)fD ]}&|&                    |#BA           |(
                    d| Aj        z            }*|)
                    d| Aj        z            }+|*                    |(B|!           |+                    |)B|"           ||z
  ||	z
  ||
z
  g},ABfd|*|+fD             }-t          A|||d|d          }.t          |!|||          }/t          |"|||          }0t          d|'A||.|'f          }1t          d|(|!||/|(f          }2t          d|)|"||0|)f          }3t          B| ||g||g|,|	|
g|-|g          }4|$| |z  Bj        z  f|%|Aj        z  |Aj        z  z   |Aj        z  z   fg}5|1|2|3g}6|4                    |6|5          \  }7}8t!          |-|	|
g          }9t#          |d|z  |dz  z  |dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |                    |          z  ||z  |z  |z  z
  |d|z  z   d|z  |dz  z  z    |                    |          z  ||z  |dz  z  z   dg          }:t'          |8                    |9                              |di                                                                                    }|:|z
                                  t/          dd          k    sJ g };t1          |1|2|3g|.|/|0g          D ]r\  }<}=|=t3          |<j        |<j                            |#          |<j                  z   }>|;                    t          d|<j        |<j        |<j        |>|#f                     s|4                    |;|5          \  }?}@t'          |@                    |9                              |di                                                    }|:|z
                                  t/          dd          k    sJ d S ) 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^c                 `    g | ]*}t          |                              j                  +S r   r   r8   r^   r   pr&   r   s     r   r    z,test_non_central_inertia.<locals>.<listcomp>   s/    	7	7	7#aeeAhh

	7	7	7r"   rbArbBrbC)rP   rQ   rR   rU   rV   rW   r+   r.    )r   r	   r   r[   r^   r]   r_   r\   r   ra   rb   v2pt_theoryr   r   r   rm   r   r   r   r   rd   doitr=   r   rY   r   mass
masscenterr<   frameappend)Cq1q2q3q1dq2dq3du1u2u3u4u5u_primeRMrs   efthetaabmAmBIAru   Kr   Q1Q2Q3IA22IA23IA33r,   r(   r*   pDpS_starpQr   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_starrt   fr2fr_star2r&   r   sC                                                                    @@r   test_non_central_inertiar      s     ''JBB"6333MCc'//BBB$+,F$G$G!GQ1aE '(< = =Aq"b"aA%%JBB/00D$ 	sA	C5&!#//A	C"ac++AMM!RVbf_%%%	C"ac++A	C"ac++AMM!R!#XMM!R!#X 
sBJJq!JJq"qs(ll413''G	c1QS51QS5=	)	)Br]    	b!Q ll413''Gll413''Gll4!AC((Gw(    	b!Q taRV,,FtaRV,,F
w1%%%
w1%%% 8S2XsRx
(C	7	7	7	7	7vv&6	7	7	7B 2tT1dA66I1a##I1a##I E7ArIw+?
@
@C
E7ArIw+?
@
@C
E7ArIw+?
@
@C	Qr2rl2r(3"$b"$
' 
' 
'B 1QS!B13AC"QS&(@#ABF3_F$$VV44KB2Bx  F 1Q3q!t8AqD= 1Q3&Ag"QT	" #%'WWQZZ024Q$r'"*=1R4i1QT!"RWWQZZ/"Q$r1u*<	  
 	f%%**B7334499;;BBDDA!))++uQ{{:::: G3S/Iy)+LMM + +
F*27+-=+A+A"+E+E+-85 5 5 	yR]BHbg"#R* * 	+ 	+ 	+ 	+&&w77MCv&&++RG4455::<<Aq ((**eAqkk999999r"   c            	      4  ;< t          d          \  } }}t          dd          \  }}}t          d          \  }}}t          d          \  }	}
}}}}}t          d          \  }}}}}}}}t          d          \  }}}t          d          \  }}}t          d	          <<                    d
d| <j        g          }|                    dd| |j        g          ;;                    <|;j        z  |;j        z  z              ;                    dd|;j        g          };                    dd|;j        g          } t          d          }!|!	                    ;d           |!	                    <|;j        z             |!
                    d|;j        z            }"|!
                    d|;j        z  |
;j        z  z
            }#|!
                    d|;j        z            }$|!
                    d|;j        z            }%|!
                    d| ;j        z            }&|"|#|$|%|&fD ]}'|'                    |!<;           |%
                    d|
 ;j        z            }(|&
                    d|
 ;j        z            })|(                    |%<|           |)                    |&<|            ;<fd|(|)fD             }*|*||z
  gz  }*t          |*|||g          }+t          |+                                          D ]0\  },}-|-                    |          |+|,                    |          <   1t!          ;|||d|d          }.t!          ||||          }/t!          | |||          }0t#          d|$;||.|$f          }1t#          d|%|||/|%f          }2t#          d|&| ||0|&f          }3t%          <| ||g||g|*|g          }4|"| |z  <j        z  f|#|;j        z  |;j        z  z   |;j        z  z   fg}5|1|2|3g}6t'          ||z  ||z  |z  t)          |          z  t+          |           z  z   |||z  t)          |          z  t)          |           z  z   ||z  |z  t+          |          z  ||z  z
  ||
z  z
  g          }7t'          |d|z  |dz  z  |
dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |                    |          z  ||z  |z  |z  z
  |d|z  z   d|z  |
dz  z  z    |                    |          z  ||z  |dz  z  z   dg          }8|4                    |6|5          \  }9}:|9                                |7                                k    sJ |8t1          |:          z
                                  t3          dd          k    sJ d S )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   c                 `    g | ]*}t          |                              j                  +S r   r   r   s     r   r    z!test_sub_qdot.<locals>.<listcomp>W  s/    
8
8
8!3quuQxx
8
8
8r"   r   r   r   )rR   rW   r+   r.   )r   r	   r   r[   r^   r]   r_   r\   r   ra   rb   r   r   listitemsr   r   r   r   r   r   r   rm   r=   r   r   )=r   r   r   r   r   r   r   r   r   r   r   r   rs   r   r   r   r   r   r   r   r   ru   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   r&   r   s=                                                              @@r   test_sub_qdotr  *  s   
  ''JBB"6333MCc''JBB$+,F$G$G!GQ1aE '(< = =Aq"b"aA/00D$$$JBB 	sA	C5&!#//A	C"ac++AMM!RVbf_%%%	C"ac++A	C"ac++A 
sBJJq!JJq"qs(ll413''G	c1QS51QS5=	)	)Bll413''Gll413''Gll4!AC((Gr7GW5    	b!Q taRV,,FtaRV,,F
w1%%%
w1%%% 9
8
8
8
8'7
8
8
8CBH:CC#sC))GW]]__%% ' '1VVAYYq		 2tT1dA66I1a##I1a##I E7ArIw+?
@
@C
E7ArIw+?
@
@C
E7ArIw+?
@
@C 
QRr2hs	M	M	MB1QS!B13AC"QS&(@#ABF3_F bD1Q3q5U#CGG++1SZZB''aCE#e**r!t#bd*, - -K
 1Q3q!t8AqD= 1Q3&Ag"QT	" #%'WWQZZ024Q$r'"*=1R4i1QT!"RWWQZZ/"Q$r1u*<	   $$VV44KBIIKK;--////// 1 1199;;uQ{{JJJJJJr"   c                  @	    t          d          \  } }}}}}}t          d          }t          dd          }t          d          }	t          d                              dd|d	         j        g          }
|
                    d
dt
          dz  |d         z
  |
j        g          }|                    dd|d         |j        g          t          d          }|                    d	           |	                    d|d         j        z  |d         j
        z  z             }|                    |                    |                              |                     |                    |d	           |	                    dd	                                d	            	                    d||j
        z            }|                    d	           |                    |d	           |                    ||                                |           |j        z  |j
        z  z   |j        z  z   }| | z  j        z  } |f||fg}fd|D             }||dd          z  }d t          |	|          D             }t          ||	|          }|                    g |          \  }}|	d d         }t#          t%          |	          t%          |          z
            } fdj        j
        fD             }t          |||||          }|                    g |          \  }}t'          | | z  |z  t)          |d                   z  | |t+          |d	                   z  |t)          |d	                   z  z   z  t-          |d                   z  ||t+          |d	                   z  |t)          |d	                   z  z   z  ||g          }t'          | | z  |z  t)          |d                   z  d	d	g          }t/          |                                          t/          |                                          k    sJ t/          |                                          t/          |                                          k    sJ d S )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   c                 @    g | ]}                               |z  S r   )r2   )r   r3   r&   r*   s     r   r    z"test_sub_qdot2.<locals>.<listcomp>  s(    ///rall1oo"///r"   c                     g | ]
\  }}||z
  S r   r   )r   r$   r   s      r   r    z"test_sub_qdot2.<locals>.<listcomp>  s     
.
.
.eb!26
.
.
.r"   c                 @    g | ]}                               |z  S r   )r8   )r   r3   r&   r   s     r   r    z"test_sub_qdot2.<locals>.<listcomp>  s(    	2	2	2&**Q--"
	2	2	2r"   )rU   rV   )r	   r   r   r[   r\   r   r]   r   ra   rb   r^   r<   r   r   rY   r   rm   r   setr   r   r   r   r   r=   )!rs   rr   PxPyPzr   r   ry   rz   r{   r  r(   pOpRpCsR_C_hatR_Csr   u_exprr   km1fr1_u_indepr   r   km2r   fr1_expectedfr2_expectedr&   r*   r   s!                                 @@@r   test_sub_qdot2r    s    %%788Aq"b"auA	Q	'	'	'BuA 	sAkk)VadAC[99G#v1qtWY'?@@A	C!A$--A 
sBJJq! 
c1Q48ad13h.	/	/BJJq"++b//&&q!,,---JJq! \\$""F
NN1a 

413
'
'CKK1KK1 OOB1
sAq!!! fr!#vo13&G2a48Dw#t-F
 0////Q///F
bfF
.
.s1f~~
.
.
.C
aAs
#
#C  V,,FC eGQ#g,,&''E	2	2	2	2	2QSz	2	2	2B
aGS"'bB B BC  V,,FC	
1Qs1Q4yy	
Bs1Q4yyL2c!A$ii<'(QqT2	2c!A$ii<"S1YY,&'

  L 	
1Qs1Q4yy		  L SZZ\\""h|/B/B/D/D&E&EEEEESZZ\\""h|/B/B/D/D&E&EEEEEEEr"   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   r"   r   <module>r"     s`   ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; , , , , , , , , , , , , ' ' ' ' ' '@ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @y* y* y*xa: a: a:FVL VL VLpNG NG NG NG NGr"   