o
    GZh8K                     @   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).0Zqitr   W/var/www/auris/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   Z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   r2   c                    r3   r   r4   r    )r"   r)   r   r   r   d   r2   c              
      s@   g | ]}t t   | qS r   )r   r5   r   r.   pos_fromexpandr   Zai)r'   r"   r*   r)   r   r   r   k   s    c                    s    g | ]}t   |qS r   )r   accr8   )r"   r*   a_o_nr   r   r   o   r1   	      c                    $   g | ]\}}t  |t | qS r   r   )r   Zpv_oZpv_p)F_OF_Pr   r   r         $ c                    r=   r   r>   )r   pvZpav)R_star_OT_star_Cr   r   r      rA   disc)q_indu_indkd_eqsZq_dependentZconfiguration_constraintsu_dependentvelocity_constraintsu_auxiliary)/r   r   dictziplenr   	orientnewzxyset_ang_velZset_ang_accr.   r   r   r   set_vel	locatenewr   Zset_accr5   r6   r   r   r   subsupdater   range	enumerateZrow_joininvr9   r   Z
ang_acc_inZcol_joinTr   r7   r   r   kanes_equationsZkindiffdictr	   Zxreplacer   Zforcing):rmgIJZFxZFyZFzqqduZudZud_zeroZuaZua_zeror#   r%   Zv_o_n_qdZkindiffsZqd_kdZsteady_conditionsZpartial_w_CZpartial_v_OZpartial_v_PZf_cZf_vZv_o_nZf_aZM_vijr!   ZM_v_iZM_v_dZM_v_auxZ	M_v_i_auxZA_rsu_depZ
u_dep_dictZFr_uZI_C_OZ	Fr_star_uZFr_cZ	Fr_star_cZFr_star_steadyZ
iner_tuplerE   ZbodyListZF_oZF_pZ	forceListZkanefrZfrstarZfrstar_steadyZkddZsyms_in_forcingZqdir   )r'   r?   r@   r"   r*   r)   rC   rD   r:   r   r0   r   test_aux_dep   s   22,20*(
 "
	
$D(&  

ri   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   r5   rR   r   pr#   rp   r   r   r      r2   z,test_non_central_inertia.<locals>.<listcomp>rbArbBrbC)rF   rG   rH   rI   rJ   rK   r(   r+    )r   r   r   rO   rR   rQ   rS   rP   r   rT   rU   v2pt_theoryr   r   r   r\   r   r   r   r
   rV   Zdoitr7   r   rM   r   ZmassZ
masscenterr6   frameappend)Aq1q2q3q1dq2dq3du1u2u3Zu4Zu5u_primeRMr_   efthetaabmAmBIAra   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bodiesrh   fr_starZvc_mapfr_star_expectedZbodies2rbZI_starr`   fr2Z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 ) Nrj   r&   rk   zu1:4rm   rn   ro   zQ1 Q2 Q3rp   r)   rq   r#   r%   r'   rr   r   rs   rt   ru   rv   rw   rx   ry   c                    rz   r   r{   r|   r~   r   r   r   W  r2   z!test_sub_qdot.<locals>.<listcomp>r   r   r   )rH   rK   r(   r+   )r   r   r   rO   rR   rQ   rS   rP   r   rT   rU   r   r   listitemsr   r   r   r   r   r   r   r\   r7   r
   r   );r   r   r   r   r   r   r   r   r   r   r   r   r_   r   r   r   r   r   r   r   r   ra   r   r   r   r   r   r   r   r   r)   r%   r'   r   r   r   r   r   r   r}   r   r   r   Zkde_mapkvr   r   r   r   r   r   r   r   r   Zfr_expectedr   rh   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&   rk   zu:5r#   B_primer$   r   r%   r(   r'   r*   r   r+   r,   ry   rw   c                       g | ]	}  |@ qS r   )r.   r/   )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   )r5   r/   )r#   r   r   r   r     r   )rI   rJ   )r   r   r   rO   rP   r   rQ   r   rT   rU   rR   r6   r   r   rM   r   r\   r   setr   r   r   r   r
   r7   )r_   r^   ZPxZPyZPzr   r   rb   rc   rd   r   r%   ZpOZpRZpCsZR_C_hatZR_Csr   Zu_exprr   Zkm1Zfr1_Zu_indeprg   r   Zkm2r   Zfr1_expectedZfr2_expectedr   )r#   r'   r   r   test_sub_qdot2  sb    $
0""r   N)Zsympyr   r   r   r   r   r   r   Zsympy.simplify.simplifyr	   Zsympy.simplify.trigsimpr
   Zsympy.solvers.solversr   Zsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   ri   r   r   r   r   r   r   r   <module>   s   $ 0 =cX