U
    9%eJ                     @   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st+t|+|7& |5& kst+t,t|,& t,|6& kst+t-|3j.}8|	D ]}9|9|8kst+qd S )#Nzt r m g I JzFx Fy Fzzq:4c                    s   g | ]}|  qS  diff).0Zqitr   g/var/www/html/Darija-Ai-API/env/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_kane2.py
<listcomp>1   s     z test_aux_dep.<locals>.<listcomp>zu:6c                    s   g | ]}|  qS r   r   r   uir   r   r   r   3   s     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   s     c                    s   g | ]}  |qS r   )r,   r   r   )r%   r    r   r   r   b   s     c                    s   g | ]}  | qS r   velr   r   )r    r(   r   r   r   c   s     c                    s   g | ]}  | qS r   r/   r   )r    r'   r   r   r   d   s     c              
      s@   g | ]8}t t   | qS r   )r   r0   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   accr3   )r    r(   a_o_nr   r   r   o   s     	      c                    s$   g | ]\}}t  |t | qS r   r   )r   Zpv_oZpv_p)F_OF_Pr   r   r      s     c                    s$   g | ]\}}t  |t | qS r   r8   )r   pvZpav)R_star_OT_star_Cr   r   r      s     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_accr0   r1   r   r   r   subsupdater   range	enumerateZrow_joininvr4   r   Z
ang_acc_inZcol_joinTr   r2   r   r   kanes_equationsZkindiffdictAssertionErrorr	   r   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%   r9   r:   r    r(   r'   r<   r=   r5   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t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st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   r0   rK   r   pr!   rj   r   r   r      s     z,test_non_central_inertia.<locals>.<listcomp>rbArbBrbC)r?   r@   rA   rB   rC   rD   r&   r)    ) r   r   r   rH   rK   rJ   rL   rI   r   rM   rN   v2pt_theoryr   r   r   rU   r   r   r   r
   rO   Zdoitr2   r   rV   rF   r   ZmassZ
masscenterr1   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s4t|8t|:  tddksTt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   U  s     z!test_sub_qdot.<locals>.<listcomp>rx   ry   rz   )rA   rD   r&   r)   )r   r   r   rH   rK   rJ   rL   rI   r   rM   rN   r|   r   listitemsr   r   r   r   r   r   r   rU   r2   rV   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stt| t| kst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     s     z"test_sub_qdot2.<locals>.<listcomp>c                 S   s   g | ]\}}|| qS r   r   )r   r   r   r   r   r   r     s     c                    s   g | ]}  |@ qS r   )r0   r-   )r!   r   r   r   r     s     )rB   rC   )r   r   r   rH   rI   r   rJ   r   rM   rN   rK   r1   r   r|   rF   r   rU   r   setr   r   r   r   r
   r2   rV   )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  sd     $
 0"r   N)Zsympy.core.backendr   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