a
    kh8K                     @   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 ]4}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sJ t|+|7& |5& ksJ t+t|,& ,ddit+|6& ,ddiksJ t-|3j.}8|	D ]}9|9|8vs&J q&d S )#Nzt r m g I JzFx Fy Fzzq:4c                    s   g | ]}|  qS  diff).0Zqitr   V/var/www/auris/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_kane2.py
<listcomp>1       z test_aux_dep.<locals>.<listcomp>zu:6c                    s   g | ]}|  qS 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   r   c                    s   g | ]}  |qS r   )r-   r   r   )r&   r!   r   r   r   b   r   c                    s   g | ]}  | qS r   velr   r   )r!   r)   r   r   r   c   r   c                    s   g | ]}  | qS r   r0   r   )r!   r(   r   r   r   d   r   c              
      s@   g | ]8}t t   | qS r   )r   r1   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   accr4   )r!   r)   a_o_nr   r   r   o   r   	      c                    s$   g | ]\}}t  |t | qS r   r   )r   Zpv_oZpv_p)F_OF_Pr   r   r      r   c                    s$   g | ]\}}t  |t | qS r   r9   )r   pvZpav)R_star_OT_star_Cr   r   r      r   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_accr1   r2   r   r   r   subsupdater   range	enumerateZrow_joininvr5   r   Z
ang_acc_inZcol_joinTr   r3   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_tupler?   ZbodyListZF_oZF_pZ	forceListZkanefrZfrstarZfrstar_steadyZkddZsyms_in_forcingZqdir   )r&   r:   r;   r!   r)   r(   r=   r>   r6   r   r/   r   test_aux_dep   s    22,20*(
 "	

$D( 

rc   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 ]}&|&|#  qr|#
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 ]F\}<}=|=t|<j|<j|#|<j }>|;td |<j|<j|<j|>|#f q2|4|;|5\}?}@t|@|9|di }|:|  tddksJ 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                    s   g | ]}t | jqS r   r   r1   rL   r   pr"   rj   r   r   r      r   z,test_non_central_inertia.<locals>.<listcomp>rbArbBrbC)r@   rA   rB   rC   rD   rE   r'   r*    )r   r   r   rI   rL   rK   rM   rJ   r   rN   rO   v2pt_theoryr   r   r   rV   r   r   r   r
   rP   Zdoitr3   r   rG   r   ZmassZ
masscenterr2   frameappend)Aq1q2q3q1dq2dq3du1u2u3Zu4Zu5u_primeRMrY   efthetaabmAmBIAr[   Kr   Q1Q2Q3IA22IA23IA33r(   r$   r&   pDpS_starpQrv   pA_starpB_starpC_starpB_hatpC_hatkdevc	inertia_A	inertia_B	inertia_Crx   ry   rz   kmforcesbodiesrb   fr_starZvc_mapfr_star_expectedZbodies2rbZI_starrZ   fr2Zfr_star2r   rw   r   test_non_central_inertia   s    6
"
6 

r   c            ;         sX  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s4J |8t|:  tddksTJ d S ) Nrd   r%   re   zu1:4rg   rh   ri   zQ1 Q2 Q3rj   r(   rk   r"   r$   r&   rl   r   rm   rn   ro   rp   rq   rr   rs   c                    s   g | ]}t | jqS r   rt   ru   rw   r   r   r   W  r   z!test_sub_qdot.<locals>.<listcomp>rx   ry   rz   )rB   rE   r'   r*   )r   r   r   rI   rL   rK   rM   rJ   r   rN   rO   r|   r   listitemsr   r   r   r   r   r   r   rV   r3   r
   r   );r   r   r   r   r   r   r   r   r   r   r   r   rY   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   rv   r   r   r   Zkde_mapkvr   r   r   rx   ry   rz   r   r   r   Zfr_expectedr   rb   r   r   rw   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sJ t| t| ksJ d S )Nzg m Px Py Pz R tzq:5r%   re   zu:5r"   B_primer#   r   r$   r'   r&   r)   r   r*   r+   rs   rq   c                    s   g | ]}  |@ qS r   )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     r   c                    s   g | ]}  |@ qS r   )r1   r.   )r"   r   r   r   r     r   )rC   rD   )r   r   r   rI   rJ   r   rK   r   rN   rO   rL   r2   r   r|   rG   r   rV   r   setr   r   r   r   r
   r3   )rY   rX   ZPxZPyZPzr   r   r\   r]   r^   r   r$   ZpOZpRZpCsZR_C_hatZR_Csr   Zu_exprr   Zkm1Zfr1_Zu_indepra   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   rc   r   r   r   r   r   r   r   <module>   s   $0 =cX