Práctica 3 - Dinámica de manipuladores

In [ ]:

    

def f(t, x):
    # Se importan funciones matematicas necesarias
    from numpy import matrix, sin, cos
    # Se desenvuelven las variables que componen al estado
    q1, q2, q3, q̇1, q̇2, q̇3 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, m2, m3, J1, J2, J3 = 0.3, 0.2, 0.2, 0.0005, 0.0002, 0.0002
    l1, l2, l3 = 0.5, 0.4, 0.3
    τ1, τ2, τ3 = 0, 0, 0
    # Se agrupan terminos en vectores
    q̇ = matrix([[q̇1], [q̇2], [q̇3]])
    τ = matrix([[τ1], [τ2], [τ3]])
    # Se calculan terminos comúnes
    μ1 = (m2 + m3)*l2**2
    μ2 = m3*l3**2
    μ3 = m3*l2*l3
    c2 = cos(q2)
    c3 = cos(q3)
    s2 = sin(q2)
    s3 = sin(q3)
    c23 = cos(q2 + q3)
    cris1 = μ3*(sin(2*q2+q3)+s3) + μ2*sin(2*(q2+q3))
    cris2 = μ1*sin(2*q2) + 2*μ3*sin(2*q2+q3) + μ2*sin(2*(q2+q3))
    cris3 = 2*μ3*sin(q2+q3)*cos(q2) + μ2*sin(2*(q2+q3))
    # Se calculan las matrices de la ecuación de movimiento
    # ESCRIBE TU CODIGO AQUI
    raise NotImplementedError
    # Se calculan las variables a devolver por el sistema
    # ESCRIBE TU CODIGO AQUI
    raise NotImplementedError
    q1pp = qpp.item(0)
    q2pp = qpp.item(1)
    q3pp = qpp.item(2)
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q2p, q3p, q1pp, q2pp, q3pp]


    

In [ ]:

    

from numpy.testing import assert_almost_equal
assert_almost_equal(f(0, [0,0,0,0,0,0]), [0, 0, 0, 0.0, -24.37, 24.17], 2)
assert_almost_equal(f(0, [1,1,1,0,0,0]), [0, 0, 0, 0.0, -18.60, 45.32], 2)
print("Sin errores")


    

In [ ]:

    

from robots.simuladores import simulador


    

In [ ]:

    

%matplotlib widget
ts, xs = simulador(puerto_zmq="5551", f=f, x0=[0, 0, 0, 0.1, 0, 0], dt=0.04)