U
    L?h3                     @   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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)sympify)simplify)solve)dynamicsymbolsReferenceFramePointdotcrossinertiaKanesMethodParticle	RigidBody
LagrangianLagrangesMethod)slowc            ;         s  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st|1j|$kst|1j|$|. ks"tt|1j |1j! |}2|&" D ]}3|2|3 |&|3 ks<tq<t#|1j$|1j% |/ |0 tdddgkst|	|  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ddddddddgddddddddgddddddddg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ddddddddgdddddd| ddgg}9tg }:| |8|9ks`t|!|8|:ksttt*| |8|dt+d , ddikst|.jd d!}1|1j(|4|5|6|7gddd\} }!| |8|9kst|!|8|:kstd S )"Nz	t r m g vzq1:7c                    s   g | ]}|  qS  diff).0qitr   ^/var/www/html/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_linearize.py
<listcomp>   s     z4test_linearize_rolling_disc_kane.<locals>.<listcomp>zu:6zu1:7c                    s   g | ]}|  qS r   r   )r   Zuir   r   r   r       s     NNOAAxisBCCOPc                    s   g | ]}t  |qS r   r   velr   Zuv)r!   r(   r   r   r    6   s     c                    s    g | ]}t   |qS r   )r   
ang_vel_inr+   )r&   r!   w_c_n_qdr   r   r    9   s     c                    s    g | ]}t   |qS r   r)   r+   )r'   r!   	v_co_n_qdr   r   r    :   s           Disc)kd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraintsr         Top_pointA_and_Br
            GJ)linear_solver)-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AssertionError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   rP   ZkindiffsZqdotsZF_COIJZI_C_COr1   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   r.   r-   r    test_linearize_rolling_disc_kane   s    "&

&&
.
   ,  
      H   * J ,&& 0 
rn   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 t|tg ks2td S )NrX   ra   r<   L, m, t皙#@r!   N*r   r#   axisr(   pP)q_indu_indr2   T)r;   r
   皙#)r   r   r   r   rJ   rB   rC   rG   rF   rD   rK   r   r   r   rM   rS   r   rO   )rX   ra   r\   LrU   r   rV   r!   pNr#   r(   rs   kdeRrh   ri   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st|tg kst|j|||gdddd\}}}| td	dgd| d	ggksBt|tg ksTt|j|||gdddd d\}}}| td	dgd| d	ggkst|tg kstd S )Nq1:3r<   levelzu1:3ro   rp   r!   rq   r   r#   rr   P1rs   )rt   ru   r3   r5   r4   r6   Zacceleration_constraintsr2   Tr9   rv   r@   )r:   r;   r
   rA   c                 S   s
   |  |S NZLUsolver#   br   r   r   <lambda>       z9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>)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   rS   expandrO   ) rX   rY   r\   r]   ra   rb   rd   re   rw   rU   r   rV   r!   rx   theta1r#   r(   rs   ry   Zdq_dictrN   rP   rQ   rz   rh   ri   r{   rk   rl   rm   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t|tg ks t|j| g|gddd\}}}t|tddgdt|  | dggksjt|tg ks|td S )NrX   r<   ro   rp   r!   rq   r   r#   rr   r(   rs   )	forcelistframeT)r;   rv   r@   )r;   rA   )r   r   r   r   rJ   rB   rC   rG   rF   rD   rK   r   r   r   form_lagranges_equationsrS   r
   r   r   rO   )rX   r\   rw   rU   r   rV   r!   rx   r#   r(   rs   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st|tg kst|j|g|g| g|g|ddd d\}}}t|tddgd| dggkst|tg kstd S )Nr~   r<   r   ro   rp   r!   rq   r   r#   rr   r   rs   r/   )Z
hol_coneqsr   r   )r:   T)r:   r;   rv   c                 S   s
   |  |S r   r   r   r   r   r   r   L  s    z=test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>)r:   r;   rA   )r   r   r   r   rJ   r   rB   rC   rF   rD   rE   rH   rI   r   r   r   r   r   r   Zsolve_multipliersupdaterS   r
   rO   )rX   rY   r\   r]   rw   rU   r   rV   r!   rx   r   r#   r(   rs   rN   r   r   r:   Zlam_opr%   r|   r   r   r   +test_linearize_pendulum_lagrange_nonminimal)  s>    
&(
 & &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d
d
d
dd
d
gd
d
d
d
dd
gd
d
d
d
d
dgd
d
d
d
d| d
gd
d|
 d|  d
d| d d
d
gd
d
d
d
d
d
gg}||kstd S )Nzq1 q2 q3r<   zr m gr!   Yr$   rw   rz   r&   r   Dmcr0   r/   BodyDT)rt   Zqd_indr:   r;   ir7   r8   )r   r   r   rB   rC   rD   rE   r   rJ   rF   rK   r   r   r   Zpotential_energyr   r   r   r   rS   r   rO   )rX   rY   rZ   r[   r\   r]   r^   r_   rT   rU   rV   r!   r   rw   rz   r&   r   rf   r   r   lr:   r#   rj   r   r   r   $test_linearize_rolling_disc_lagrangeQ  sL    2

        "r   N)"Zsympyr   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   rn   r}   r   r   r   r   r   r   r   r   <module>   s   $4
 %Q&(