o
    g8K                     @   s   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d Zdd	 Zd
d Zd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            :         s@  t d\	} }}}}t d\}}}td}	fdd|D }	td}
	fdd|
D }tt|dgt| }td	}tt|dgt| }td
dd|d jg}|dd|d |jg}|dd|d |j	g  
|
d |j |
d |j	  |
d |j     	|t|   td|d |j |d |j	  |d |j   d|d |j | t|d  |j	  |
d |j |
d |j	  |
d |j   	|t|  |	d |j |	d |j  |	d |j	  
	|t| }t 
fdd|D t| |jg }t||	}t||	d d|	d di|
}||	d d|	d di  fdd|
| D }fdd|
| D }fdd|
| D }tt|  |j |j|d  g}t fdd|D }t }|	|t|| tfdd|D }tdd}tdD ]}t|
| D ]\}}|| ||||f< qq|d d d df } |d d ddf }!|d d dd f }"| |"}#|!  |# }$|$d d d df t|
d d  }%tt|
dd  |%}&|| |j ||j ||j	  ||j  tfddt||D }'|   t!||||}(t|( "t t|(   tfd dt||D })|'d dd d f #|'dd d d f |$j$|'ddd d f   }*|)d dd d f #|)dd d d f |$j$|)ddd d f   }+|+||&||d |  t%|d  i& },|(f}-t'd! ||-}.|.g}/f}0f}1|0|1g}2t(|d d |
d d ||dd  ||
dd  ||d"	}3|3)|/|2\}4}5|5||&||d |  t%|d  i& }6|3* }7t|*& |4& ksaJ t|+|7& |5& ksqJ t+t|,& ,ddit+|6& ,ddiksJ t-|3j.}8|	D ]
}9|9|8vsJ qd S )#Nzt r m g I JzFx Fy Fzzq:4c                       g | ]}|  qS  diff).0qitr   _/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_kane2.py
<listcomp>1       z test_aux_dep.<locals>.<listcomp>zu:6c                    r   r   r   r   uir   r   r   r   3   r    g        zua:3NAAxisr   B   C   PO         c                    s    g | ]}t   |qS r   )r   
ang_vel_inr   uv)r(   r#   w_c_n_qdr   r   r   X        c                    s   g | ]}  |qS r   )r/   r   r!   )r(   r#   r   r   r   b       c                       g | ]}  | qS r   velr   r!   )r#   r+   r   r   r   c   r4   c                    r5   r   r6   r!   )r#   r*   r   r   r   d   r4   c              
      s@   g | ]}t t   | qS r   )r   r7   r   r/   pos_fromexpandr   ai)r(   r#   r+   r*   r   r   r   k   s    c                    s    g | ]}t   |qS r   )r   accr:   )r#   r+   a_o_nr   r   r   o   r3   	      c                    $   g | ]\}}t  |t | qS r   r   )r   pv_opv_p)F_OF_Pr   r   r         $ c                    r@   r   rA   )r   pvpav)R_star_OT_star_Cr   r   r      rF   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_accr/   r   r   r   set_vel	locatenewr   set_accr7   r8   r   r   r   subsupdater   range	enumeraterow_joininvr<   r   
ang_acc_incol_joinTr   r9   r   r   kanes_equationskindiffdictr	   xreplacer   forcing):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_tuplerK   bodyListF_oF_p	forceListkanefrfrstarfrstar_steadykddsyms_in_forcingqdir   )r(   rD   rE   r#   r+   r*   rI   rJ   r=   r   r2   r   test_aux_dep   s   22,20*(
 "
	
$D(&  

r   c            A   
      s  t d\} }}t ddd\}}}t d\}}}}	}
td\}}}}}}}td\}}}}}}}}td\}}}td\}}}td	d
d| jg} | dd| | jg  | j | j    dd| jg}! dd| jg}"|! |	 j  |" |
 j  td}#|#	 d |#	| j  |#
d| j }$|#
d| j | j  }%|$|%fD ]	}&|&|#  q|#
d| j }'|#
d| j }(|#
d|  j })|'|(|)fD ]	}&|&|#  q|(
d|  j }*|)
d|  j }+|*|(|! |+|)|" || ||	 ||
 g}, fdd|*|+fD }-t |||d|d}.t|!|||}/t|"|||}0td|' ||.|'f}1td|(|!||/|(f}2td|)|"||0|)f}3t| ||g||g|,|	|
g|-|gd}4|$| | j f|%| j | j  | j  fg}5|1|2|3g}6|4|6|5\}7}8t|-|	|
g}9t|d| |d  |d   d|  ||d   d| |d    || || | |  |d|  d| |d    || || |d   dg}:t|8|9|di  }|:|  tddks
J g };t|1|2|3g|.|/|0gD ]#\}<}=|=t|<j|<j|#|<j }>|;td |<j|<j|<j|>|#f q|4|;|5\}?}@t|@|9|di }|:|  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qS r   r   r7   rZ   r   pr$   r   r   r   r      r4   z,test_non_central_inertia.<locals>.<listcomp>rbArbBrbC)rL   rM   rN   rQ   rR   rS   r)   r,    )r   r   r   rW   rZ   rY   r[   rX   r   r]   r^   v2pt_theoryr   r   r   ri   r   r   r   r
   r`   doitr9   r   rU   r   mass
masscenterr8   frameappend)Aq1q2q3q1dq2dq3du1u2u3u4u5u_primeRMro   efthetaabmAmBIArq   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_starrp   fr2fr_star2r   r   r   test_non_central_inertia   s   6
"
6 

 r   c            ;         sV  t d\} }}t ddd\}}}t d\}}}td\}	}
}}}}}td\}}}}}}}}td\}}}td\}}}td	d
d| jg}|dd| |jg  | j | j    dd| jg} dd| jg} td}!|!	 d |!	| j  |!
d| j }"|!
d| j |
 j  }#|!
d| j }$|!
d| j }%|!
d|  j }&|"|#|$|%|&fD ]	}'|'|!  q|%
d|
  j }(|&
d|
  j })|(|%| |)|&|   fdd|(|)fD }*|*|| g7 }*t|*|||g}+t|+ D ]\},}-|-||+|,|< qt |||d|d}.t||||}/t| |||}0td|$ ||.|$f}1td|%|||/|%f}2td|&| ||0|&f}3t| ||g||g|*|gd}4|"| | j f|#| j | j  | j  fg}5|1|2|3g}6t|| || | t| t|   ||| t| t|   || | t| ||  ||
  g}7t|d| |d  |
d   d|  ||d   d| |d    || || | |  |d|  d| |
d    || || |d   dg}8|4|6|5\}9}:|9 |7 ksJ |8t|:  t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                    r   r   r   r   r   r   r   r   W  r4   z!test_sub_qdot.<locals>.<listcomp>r   r   r   )rN   rS   r)   r,   )r   r   r   rW   rZ   rY   r[   rX   r   r]   r^   r   r   listitemsr   r   r   r   r   r   r   ri   r9   r
   r   );r   r   r   r   r   r   r   r   r   r   r   r   ro   r   r   r   r   r   r   r   r   rq   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   r   test_sub_qdot*  s|   6
"""
6$r   c               	      s  t d\} }}}}}}td}tddd}td}	td  dd|d	  jg}
|
d
dtd |d  |
jg}|dd|d |jgtd}| d	 |	d|d  j |d  j
  }| |||  ||d	 |	dd	d	 	d||j
 }|d	 ||d	 || | |  | j | j
  | j  }| |   j }|f||fg} fdd|D }||dd  7 }dd t|	|D }t ||	|}|g |\}}|	d d }tt|	t| } fdd j j
fD }t |||||d}|g |\}}t| |  | t|d  | |t|d	  |t|d	    t|d  ||t|d	  |t|d	    ||g}t| |  | t|d  d	d	g}t| t| ksyJ t| t| ksJ 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 | ]	}  |@ qS r   )r/   r0   )r$   r(   r   r   r         z"test_sub_qdot2.<locals>.<listcomp>c                 S   s   g | ]\}}|| qS r   r   )r   r"   r   r   r   r   r     s    c                    r  r   )r7   r0   )r$   r   r   r   r     r  )rQ   rR   )r   r   r   rW   rX   r   rY   r   r]   r^   rZ   r8   r   r   rU   r   ri   r   setr   r   r   r   r
   r9   )ro   rn   PxPyPzr   r   ru   rv   rw   r   r&   pOpRpCsR_C_hatR_Csr   u_exprr   km1fr1_u_indepr   r   km2r   fr1_expectedfr2_expectedr   )r$   r(   r   r   test_sub_qdot2  sb    $
0""r  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   r   <module>   s   $ 0 =cX