o
    g3                     @   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syJ |1j|$ksJ |1j|$|. ksJ t|1j|1j  |}2|&! D ]}3|2|3 |&|3 ksJ qt"|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s!J t)| |8|dt*d# + dd$iks9J |.jd%d&}1|1j'|4|5|6|7gddd\} }!| |8|9ksWJ |!|8|:ksaJ d S )'Nz	t r m g vzq1:7c                       g | ]}|  qS  diff).0qitr   c/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_linearize.py
<listcomp>       z4test_linearize_rolling_disc_kane.<locals>.<listcomp>zu:6zu1:7c                    r   r   r   )r   uir   r   r    r!      r"   NNOAAxisBCCOPc                    s   g | ]
}t  |qS r   r   velr   uv)r$   r+   r   r    r!   6   s    c                        g | ]}t   |qS r   )r   
ang_vel_inr.   )r)   r$   w_c_n_qdr   r    r!   9        c                    r0   r   r,   r.   )r*   r$   	v_co_n_qdr   r    r!   :   r3         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   rB   )r   r   r   r   r   rB   r   r   )r   r   r   r   r   r   rB   r   )r   r   r   r   r   r   r   r         GJ)linear_solver),r   r   r   r   	orientnewzxy	locatenewr1   set_ang_velpos_fromdtset_velv2pt_theoryr   r   r   subsset_ang_accr   r   r   r   kanes_equationsto_linearizerf_cf_vf_ar   kindiffdictf_0f_1keysr
   f_2f_3r   r   	linearizer   r	   r   	eigenvals);rmgvq1q2q3q4q5q6qq1dq2dq3dq4dq5dq6dqduu1u2u3u4u5u6u1du2du3du4du5du6dr%   r&   r(   w_b_n_qdrV   rW   kindiffsqdotsF_COIJI_C_COr7   BLFLKMfrfr_star
linearizersolr   q_opu_opqd_opud_opupright_nominalA_solB_solr   )r)   r*   r$   r+   r   r4   r2   r     test_linearize_rolling_disc_kane   s   "&

&&
.

 *H,L.&& 0
r   c                  C   s2  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sJ |tg ksJ d S )Nre   rt   rB   L, m, t皙#@r$   N*r   r&   axisr+   pP)q_indu_indr8   T)rA   r
   皙#)r   r   r   r   rP   rH   rI   rM   rL   rJ   rQ   r   r   r   rT   r_   r   )re   rt   rl   Lrb   r   rc   r$   pNr&   r+   r   kdeRr   r   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 |j|||gdddd\}}}| td	dgd| d	ggksJ |tg ks(J |j|||gdddd d\}}}| td	dgd| d	ggksLJ |tg ksUJ d S )Nq1:3rB   levelzu1:3r   r   r$   r   r   r&   r   P1r   )r   r   r9   r;   r:   r<   acceleration_constraintsr8   Tr?   r   rF   )r@   rA   r
   rG   c                 S   
   |  |S NLUsolver&   br   r   r    <lambda>   s   
 z9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>)r   r   r   r   rP   r   rH   rI   rL   rJ   rK   r   r   r   rN   rO   rR   	magnituder-   expressr   r
   r   r   rT   r_   expand) re   rf   rl   rm   rt   ru   rz   r{   r   rb   r   rc   r$   r   theta1r&   r+   r   r   dq_dictrV   rW   rX   r   r   r   r   r   r   r   r(   r   r   r   r    'test_linearize_pendulum_kane_nonminimal   sb   
$&&r   c                  C   sx  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sJ |tg ksJ |j| g|gddd\}}}t|tddgdt|  | dggksJ |tg ksJ d S )Nre   rB   r   r   r$   r   r   r&   r   r+   r   )	forcelistframeT)rA   r   rF   )rA   rG   )r   r   r   r   rP   rH   rI   rM   rL   rJ   rQ   r   r   r   form_lagranges_equationsr_   r
   r   r   )re   rl   r   rb   r   rc   r$   r   r&   r+   r   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 |j|g|g| g|g|ddd d\}}}t|tddgd| dggksJ |tg ksJ d S )Nr   rB   r   r   r   r$   r   r   r&   r   r   r   r5   )
hol_coneqsr   r   )r@   T)r@   rA   r   c                 S   r   r   r   r   r   r   r    r   L  s   
z=test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>)r@   rA   rG   )r   r   r   r   rP   r   rH   rI   rL   rJ   rK   rN   rO   r   r   r   r   r   r   solve_multipliersupdater_   r
   )re   rf   rl   rm   r   rb   r   rc   r$   r   r   r&   r+   r   rV   r   r   r@   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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 q3rB   zr m gr$   Yr'   r   r   r)   r   Dmcr6   r5   BodyDT)r   qd_indr@   rA   )r   r   r   rB   r   r   )r   r   r   r   rB   r   )r   r   r   r   r   rB   ir=   r>   )r   r   r   r   r   r   )r   r   r   rH   rI   rJ   rK   r   rP   rL   rQ   r   r   r   potential_energyr   r   r   r   r_   r   )re   rf   rg   rk   rl   rm   rn   rr   ra   rb   rc   r$   r   r   r   r)   r   r   r   r   lr@   r&   r   r   r   r    $test_linearize_rolling_disc_lagrangeQ  s<   2

"r   N)"sympyr   r   r   r   r   r   r   sympy.core.sympifyr	   sympy.simplify.simplifyr
   sympy.solvers.solversr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r   r   r   r   r   r   r   r   r   r    <module>   s   $ 4
 %Q&(