In [2]:

    

import numpy as np 
import matplotlib.pyplot as plt
from sympy import *


    

In [3]:

    

init_printing(use_latex=True)


    

In [4]:

    

#estos son simbolos especiales que los toma como letras griegas directamente(muuy groso)
theta_1, theta_2, theta_3, L_1, L_2, L_3 = symbols('theta_1, theta_2 theta_3 L_1 L_2 L_3')


    

In [6]:

    

#jacobiano simbolico
a11 = -L_1*sin(theta_1) - L_2*sin(theta_1+theta_2) - L_3*sin(theta_1+theta_2+theta_3)
a12 = -L_2*sin(theta_1+theta_2) - L_3*sin(theta_1+theta_2+theta_3)
a13 = -L_3*sin(theta_1+theta_2+theta_3)

a21 = L_1*cos(theta_1) + L_2*cos(theta_1+theta_2) + L_3*cos(theta_1+theta_2+theta_3)
a22 = L_2*cos(theta_1+theta_2) + L_3*cos(theta_1+theta_2+theta_3)
a23 = L_3*cos(theta_1+theta_2+theta_3)
a31 = 1
a32 = 1 
a33 = 1

J = Matrix([[a11,a12,a13],[a21,a22,a23],[a31,a32,a33]])
J


    

    
Out[6]:





$$\left[\begin{smallmatrix}{}- L_{1} \sin{\left (\theta_{1} \right )} - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )} - L_{3} \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )} - L_{3} \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - L_{3} \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )} + L_{3} \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )} + L_{3} \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & L_{3} \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\1 & 1 & 1\end{smallmatrix}\right]$$

In [7]:

    

#reemplazando las longitudes de los links

J_real = J.subs([(L_1,4),(L_2,3),(L_3,2)])
J_real


    

    
Out[7]:





$$\left[\begin{smallmatrix}{}- 4 \sin{\left (\theta_{1} \right )} - 3 \sin{\left (\theta_{1} + \theta_{2} \right )} - 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - 3 \sin{\left (\theta_{1} + \theta_{2} \right )} - 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\4 \cos{\left (\theta_{1} \right )} + 3 \cos{\left (\theta_{1} + \theta_{2} \right )} + 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & 3 \cos{\left (\theta_{1} + \theta_{2} \right )} + 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\1 & 1 & 1\end{smallmatrix}\right]$$

In [9]:

    

J_real_inv = J_real.inv()
J_real_inv.simplify()
J_real_inv


    

    
Out[9]:





$$\left[\begin{smallmatrix}{}\frac{1}{8 \left(3 \sin{\left (\theta_{2} \right )} + 2 \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \left(4 \sin{\left (\theta_{1} \right )} + 3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right)^{2} \sin{\left (\theta_{2} \right )}} \left(- 8 \left(3 \sin{\left (\theta_{2} \right )} + 2 \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \left(4 \sin{\left (\theta_{1} \right )} + 3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )} + 2 \left(3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \left(4 \sin{\left (\theta_{1} \right )} + 3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \left(4 \cos{\left (\theta_{1} \right )} + 3 \cos{\left (\theta_{1} + \theta_{2} \right )} + 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )} - \left(4 \sin{\left (2 \theta_{1} \right )} + 3 \sin{\left (\theta_{2} \right )} + 3 \sin{\left (2 \theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{2} + \theta_{3} \right )} + 2 \sin{\left (2 \theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \left(- 2 \cos{\left (\theta_{1} + \theta_{2} \right )} - 4 \cos{\left (\theta_{1} - \theta_{3} \right )} + 4 \cos{\left (\theta_{1} + \theta_{3} \right )} - 3 \cos{\left (\theta_{1} + \theta_{2} - \theta_{3} \right )} + 3 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} + 2 \cos{\left (\theta_{1} + \theta_{2} + 2 \theta_{3} \right )}\right)\right) & \frac{3 \cos{\left (\theta_{1} \right )} - 3 \cos{\left (\theta_{1} + 2 \theta_{2} \right )} + 2 \cos{\left (\theta_{1} - \theta_{3} \right )} - 2 \cos{\left (\theta_{1} + 2 \theta_{2} + \theta_{3} \right )}}{- 12 \cos{\left (2 \theta_{2} \right )} + 8 \cos{\left (\theta_{3} \right )} - 8 \cos{\left (2 \theta_{2} + \theta_{3} \right )} + 12} & \frac{\sin{\left (\theta_{3} \right )}}{2 \sin{\left (\theta_{2} \right )}}\\- \frac{1}{\left(3 \sin{\left (\theta_{2} \right )} + 2 \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \left(4 \sin{\left (\theta_{1} \right )} + 3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}} \left(\frac{1}{6} \left(3 \sin{\left (\theta_{3} \right )} + 4 \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \left(2 \sin{\left (2 \theta_{1} \right )} + \frac{3}{2} \sin{\left (\theta_{2} \right )} + \frac{3}{2} \sin{\left (2 \theta_{1} + \theta_{2} \right )} + \sin{\left (\theta_{2} + \theta_{3} \right )} + \sin{\left (2 \theta_{1} + \theta_{2} + \theta_{3} \right )}\right) + \frac{1}{4} \left(4 \sin{\left (\theta_{1} \right )} + 3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \left(4 \cos{\left (\theta_{1} \right )} + 3 \cos{\left (\theta_{1} + \theta_{2} \right )} + 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}\right) & - \frac{1}{\left(3 \sin{\left (\theta_{2} \right )} + 2 \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}} \left(\frac{1}{6} \left(3 \sin{\left (\theta_{3} \right )} + 4 \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{1} \right )} + \frac{1}{4} \left(4 \sin{\left (\theta_{1} \right )} + 3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}\right) & - \frac{1}{6 \sin{\left (\theta_{2} \right )}} \left(3 \sin{\left (\theta_{3} \right )} + 4 \sin{\left (\theta_{2} + \theta_{3} \right )}\right)\\\frac{\frac{2}{3} \sin{\left (2 \theta_{1} \right )} + \frac{1}{2} \sin{\left (\theta_{2} \right )} + \frac{1}{2} \sin{\left (2 \theta_{1} + \theta_{2} \right )} + \frac{1}{3} \sin{\left (\theta_{2} + \theta_{3} \right )} + \frac{1}{3} \sin{\left (2 \theta_{1} + \theta_{2} + \theta_{3} \right )}}{\left(4 \sin{\left (\theta_{1} \right )} + 3 \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} \right )}} & \frac{\sin{\left (\theta_{1} \right )}}{3 \sin{\left (\theta_{2} \right )}} & \frac{2 \sin{\left (\theta_{3} \right )}}{3 \tan{\left (\theta_{2} \right )}} + \frac{2}{3} \cos{\left (\theta_{3} \right )} + 1\end{smallmatrix}\right]$$

In [10]:

    

#creamos una funcion numerica a partir de la simbolica
f1 = lambdify((theta_1,theta_2,theta_3),J_real,'numpy')


    

In [11]:

    

f1(.3,.2,.2)


    

    
Out[11]:





matrix([[-3.90879282, -2.72671199, -1.28843537],
        [ 7.98377802,  4.16243206,  1.52968437],
        [ 1.        ,  1.        ,  1.        ]])

In [12]:

    

f2 = lambdify((theta_1,theta_2,theta_3),J_real_inv,'numpy')


    

In [16]:

    

f2(.1,np.pi/2,1)


    

    
Out[16]:





matrix([[-0.02495835,  0.24875104,  0.42073549],
        [-0.3067097 , -0.28202885, -0.78093703],
        [ 0.33166806,  0.03327781,  1.36020154]])

In [ ]: