U
    ZÊ¹j8K  ã                   @   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�sÂt+‚t|+ |7¡ƒ &¡ |5 &¡ k�sât+‚t,t|,ƒ &¡ ƒ -ddi¡t,|6 &¡ ƒ -ddi¡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/bwave/python/venv/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)0r   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	   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%   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�sÂt‚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   W  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�sòt‚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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