SeriesElasticJacobian

"""
This cacluclates the jacobian for the seires elastic arm. 
The link parameters are taken from https://github.com/nag92/SeriesElasticActuator/blob/master/walkingSEA.xml
"""
import numpy as np

def get_jacobian_matricies(theta_1,theta_2,theta_3):
    """
    :param robot:
    :return:
    """
    theta_3 -= 0.5*np.pi
    l = [90,200,200]

    r = [0,0,0]
    J_1 =  np.zeros(shape=(6,3))
    J_1[5,0] = 1


    J_2 = np.matrix(  [ [-r[1] * np.cos(theta_2), 0, 0],
                        [  0,                              0,                   0],
                        [0,                                              -r[1], 0],
                        [  0,                                -1,                0],
                        [  -np.sin(theta_2),                    0,                 0],
                        [np.cos(theta_2),                       0,                 0] ] )

    J_3 = np.matrix( [ [-l[2] * np.cos(theta_2) - r[2] * np.cos(theta_2 + theta_3), 0,              0],
                       [ 0,                           l[1] * np.sin(theta_3),                      0],
                       [ 0,                   -r[2] - l[1] * np.cos(theta_3),                  -r[2]],
                       [ 0,                                          -1,                    -1 ],
                       [-np.sin(theta_2+theta_3),                          0,                     0 ],
                       [np.cos(theta_2+theta_3),                           0,                     0 ] ])

    return (J_1, J_2, J_3)



j1, j2, j3 = get_jacobian_matricies(0,0,0)
print j1