a
    {d2.                     @   s   d dl mZ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mZ d dlmZ edd Zd	d
 Zdd Zdd Z dd Z!dd Z"dS )    )symbolsMatrixcossinatansqrtRational_simplify_matrix)sympify)simplify)solve)dynamicsymbolsReferenceFramePointdotcrossinertiaKanesMethodParticle	RigidBody
LagrangianLagrangesMethod)slowc            ;         sv  t d\} }}}td \}}}}}}	}
fdd|
D  \}}}}}}}td}td \}}}}}}}fdd|D \}}}}}}tdtd	}d
d|jg} | dd|| jg}!|!dd||!jg |d|j |j  |	j   	|!	}" 
||!j ||!j  ||!j   || j | j  | j   d| |!j   t|	tj g}#tfdd D }$t fdd|!D fddD  }%t|%|}&|!
|"|&   	|!t|!	 	  || | j }'|| d  d }(|| d  d })t |(|)|(}*td ||*f}+|+g},|'fg}-t|||||g|||g|%|	g|#|||g|$d}.|.|,|-\}/}0|. }1|1j|#ksJ |1j|$ksJ |1j|$|. ks"J t|1j|1j  |}2|&! D ]}3|2|3 |&|3 ks<J q<t"|1j#|1j$ |/ |0 tg dksJ |	|  t%| i}4|d|t&|| | |t%|| ||  t&|| |  t%| |d||  t&|| |  t&| i}5|d||  t&|| |  t%| ||  t&|| |  t&| |di}6|d| t&| d|   t&d| |d  d  dt%| | | d  |d|d|| t&|t&| | | t&||d    || d| t&| d|   t&d| |d  d  dt%| | | d   ||  t&|t%| | | t%||d    i}7|1j'|4|5|6|7gddd\} }!|d|d|d| d|di}8tg dg dg d t&|| ddddt&| t%| dgt%| | ddddt%|t&| dgdt(dddddddd| d gg d!dddddd"| ddgg}9tg }:| |8|9ks.J |!|8|:ksBJ t)| |8|dt*d# + dd$iksrJ d S )%Nz	t r m g vzq1:7c                    s   g | ]}|  qS  diff).0qitr   u/var/www/html/stable-diffusion-webui/venv/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_linearize.py
<listcomp>       z4test_linearize_rolling_disc_kane.<locals>.<listcomp>zu:6zu1:7c                    s   g | ]}|  qS r   r   )r   Zuir   r   r    r!      r"   NNOAAxisBCCOPc                    s   g | ]}t  |qS r   r   velr   Zuv)r#   r*   r   r    r!   7   r"   c                    s    g | ]}t   |qS r   )r   
ang_vel_inr-   )r(   r#   w_c_n_qdr   r    r!   :   r"   c                    s    g | ]}t   |qS r   r+   r-   )r)   r#   	v_co_n_qdr   r    r!   ;   r"         Disc)kd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraints)r   r   r   r         Top_pointA_and_Br      )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   	orientnewzxy	locatenewr.   set_ang_velpos_fromdtset_velv2pt_theoryr   r   r   subsZset_ang_accr   r   r   r   kanes_equationsZto_linearizerf_cf_vf_ar   ZkindiffdictZf_0Zf_1keysr   Zf_2Zf_3r   r   	linearizer   r
   r   Z	eigenvals);rmgvq1q2q3Zq4Zq5Zq6qq1dq2dq3dZq4dZq5dZq6dqduu1u2u3Zu4Zu5Zu6u1du2dZu3dZu4dZu5dZu6dr$   r%   r'   Zw_b_n_qdrN   rO   ZkindiffsZqdotsZF_COIJZI_C_COr3   ZBLZFLKMfrZfr_starZ
linearizersolr   q_opu_opZqd_opud_opZupright_nominalZA_solZB_solr   )r(   r)   r#   r*   r   r0   r/   r     test_linearize_rolling_disc_kane   s    "&

&&
.

 *H,L.&& rm   c                  C   s6  t d} t d}t dd}td\}}}d}td}td}||d |d	d
| |jg}	|	|||j  |d||	j	 }
|

|||	 td|
|}t|| g}|| |j	 }t|| g|g|d}||g|
|fg\}}|jddd\}	}}|	tddgdt|  | dggks J |tg ks2J d S )NrW   r`   r>   L, m, t皙#@r#   N*r   r%   axisr*   pP)q_indu_indr4   T)r=   r   皙#)r   r   r   r   rJ   rB   rC   rG   rF   rD   rK   r   r   r   rM   rR   r   )rW   r`   r[   LrT   r   rU   r#   pNr%   r*   rr   kdeRrg   rh   frstarr'   inp_vecr   r   r    $test_linearize_pendulum_kane_minimal   s(    
*r|   c                   C   s  t d\} }t ddd\}}t d\}}t ddd\}}td\}}	}
d}td}td}||d	 t||  }|d
d||jg}|d| |j	 ||j
  }td||	}t|| || g}t|||g}|||||| t|| | g}t||||j	g}|  ||
}|  |	| |j	 }t||g|g| g|g||||d	}||g||fg\}}| ||d	i}|d	|d	i}|d	|d	i}|j|||gddd\}}}| td	dgd| d	ggksJ |tg ksJ d S )Nq1:3r>   levelzu1:3rn   ro   r#   rp   r   r%   rq   P1rr   )rs   rt   r5   r7   r6   r8   Zacceleration_constraintsr4   Tr;   ru   )r   r   r   r   rJ   r   rB   rC   rF   rD   rE   r   r   r   rH   rI   rL   Z	magnituder,   Zexpressr   r   r   r   rM   rR   expand) rW   rX   r[   r\   r`   ra   rc   rd   rv   rT   r   rU   r#   rw   theta1r%   r*   rr   rx   Zdq_dictrN   rO   rP   ry   rg   rh   rz   rj   rk   rl   r'   r{   r   r   r    'test_linearize_pendulum_kane_nonminimal   sJ    
&r   c                  C   s$  t d} t dd}td\}}}d}td}td}||d |dd	| |jg}||||j  |d
||j	 }	|	
||| td|	|}
t||
}t|| g|	|| |j	 fg|d}|  |j| g|gdd\}}}t|tddgdt|  | dggksJ |tg ks J d S )NrW   r>   rn   ro   r#   rp   r   r%   rq   r*   rr   )	forcelistframeT)r=   ru   )r   r   r   r   rJ   rB   rC   rG   rF   rD   rK   r   r   r   form_lagranges_equationsrR   r	   r   r   )rW   r[   rv   rT   r   rU   r#   rw   r%   r*   rr   LagLMr'   r{   r   r   r    (test_linearize_pendulum_lagrange_minimal   s$    

".r   c                  C   s  t d\} }t ddd\}}td\}}}d}td}td}	|	|d t||  }
|d	d
|
|jg}|	d| |j	 ||j
  }||||	| td||}t| d |d  |d  g}t||}t|| |g|||| |j	 fg|d}|  | ||d|d|d||d||di}|j|d}|| |j|g|g| g|g|dd\}}}t|tddgd| dggksJ |tg ksJ d S )Nr}   r>   r~   rn   ro   r#   rp   r   r%   rq   r   rr   r1   )Z
hol_coneqsr   r   )r<   T)r<   r=   ru   )r   r   r   r   rJ   r   rB   rC   rF   rD   rE   rH   rI   r   r   r   r   r   r   Zsolve_multipliersupdaterR   r	   )rW   rX   r[   r\   rv   rT   r   rU   r#   rw   r   r%   r*   rr   rN   r   r   r<   Zlam_opr'   r{   r   r   r    +test_linearize_pendulum_lagrange_nonminimal
  s0    
&(
&r   c                  C   s  t d \} }}}t dd \}}}}td\}}	}
td}|dd| |jg}|dd||jg}|dd||jg}td	}||d
 |	d||j }|
||| t||	d |d  |	d |d  |	d |d  }td|||	||f}|	 |
 | t| |_t||}t||}|  | d
|d
|d
|d
|d
| d
| d
| d
i}|j|||ddd
 }tg dg dg dd
d
d
d
d| d
gd
d|
 d|  d
d| d d
d
gg dg}||ksJ d S )Nzq1 q2 q3r>   zr m gr#   Yr&   rv   ry   r(   r   Dmcr2   r1   BodyDT)rs   Zqd_indr<   r=   )r   r   r   r>   r   r   )r   r   r   r   r>   r   )r   r   r   r   r   r>   ir9   r:   )r   r   r   r   r   r   )r   r   r   rB   rC   rD   rE   r   rJ   rF   rK   r   r   r   Zpotential_energyr   r   r   r   rR   r   )rW   rX   rY   rZ   r[   r\   r]   r^   rS   rT   rU   r#   r   rv   ry   r(   r   re   r   r   lr<   r%   ri   r   r   r    $test_linearize_rolling_disc_lagrange+  s<    2

"r   N)#Zsympy.core.backendr   r   r   r   r   r   r   r	   Zsympy.core.sympifyr
   Zsympy.simplify.simplifyr   Zsympy.solvers.solversr   Zsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   Zsympy.testing.pytestr   rm   r|   r   r   r   r   r   r   r   r    <module>   s   (4
y%@!