o
    8VaQJ                     @   s   d dl mZmZmZmZmZmZmZ d dlm	Z	m
Z
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)trigsimpsimplify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|,& t+|6& 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   J/usr/lib/python3/dist-packages/sympy/physics/mechanics/tests/test_kane2.py
<listcomp>/       z test_aux_dep.<locals>.<listcomp>zu:6c                    r   r   r   r   uir   r   r   r   1   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   V        c                    s   g | ]}  |qS r   )r.   r   r    )r'   r"   r   r   r   `       c                       g | ]}  | qS r   velr   r    )r"   r*   r   r   r   a   r2   c                    r3   r   r4   r    )r"   r)   r   r   r   b   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   i   s    c                    s    g | ]}t   |qS r   )r   accr8   )r"   r*   a_o_nr   r   r   m   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   Z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
   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_tuplerD   ZbodyListZF_oZF_pZ	forceListZkanefrZfrstarZfrstar_steadyZkddZsyms_in_forcingZqdir   )r'   r?   r@   r"   r*   r)   rB   rC   r:   r   r0   r   test_aux_dep	   s   22,20*(
 "
	
$D(&  


rh   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   rQ   r   pr#   ro   r   r   r      r2   z,test_non_central_inertia.<locals>.<listcomp>rbArbBrbC)rE   rF   rG   rH   rI   rJ   r(   r+    )r   r   r   rN   rQ   rP   rR   rO   r   rS   rT   v2pt_theoryr   r   r   r[   r   r   r   r	   rU   Zdoitr7   r   rL   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IAr`   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bodiesrg   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 ) Nri   r&   rj   zu1:4rl   rm   rn   zQ1 Q2 Q3ro   r)   rp   r#   r%   r'   rq   r   rr   rs   rt   ru   rv   rw   rx   c                    ry   r   rz   r{   r}   r   r   r   S  r2   z!test_sub_qdot.<locals>.<listcomp>r~   r   r   )rG   rJ   r(   r+   )r   r   r   rN   rQ   rP   rR   rO   r   rS   rT   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   r`   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   rg   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&   rj   zu:5r#   B_primer$   r   r%   r(   r'   r*   r   r+   r,   rx   rv   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   )rH   rI   )r   r   r   rN   rO   r   rP   r   rS   rT   rQ   r6   r   r   rL   r   r[   r   setr   r   r   r   r	   r7   )r^   r]   ZPxZPyZPzr   r   ra   rb   rc   r   r%   ZpOZpRZpCsZR_C_hatZR_Csr   Zu_exprr   Zkm1Zfr1_Zu_indeprf   r   Zkm2r   Zfr1_expectedZfr2_expectedr   )r#   r'   r   r   test_sub_qdot2~  sb    $
0""r   N)Zsympy.core.backendr   r   r   r   r   r   r   Zsympyr	   r
   r   Zsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   rh   r   r   r   r   r   r   r   <module>   s   $ 0 ;cX