from sympy import *
import numpy as np


def jacobian(th2, th3):
    Jv = Matrix([[0, -L3*sin(th2)*cos(th3)-L2*sin(th2), L3*cos(th2)*sin(th3)], 
                [0, 0, L3*cos(th3)],
                [1, L3*cos(th2)*cos(th3)+L2*cos(th2), -L3*sin(th2)*sin(th3)]])

    return Jv

L2, L3 = symbols("L2 L3")
th2, th3 = symbols("th2 th3")

Jv = jacobian(th2, th3)
d = Jv.det()

print("J_v is:")
pretty_print(Jv)
print()
print("Determinant of a matrix is:")
pretty_print(d)
print()


# Setting the L's to their actual values from our task and getting the determinant.

L2 = 100
L3 = 207 

print("Filling the Jv-matrix with the known link lengths:")
filled = jacobian(th2, th3)
pretty_print(filled)
filled_det = filled.det()
print()

sings = solve(filled_det)
print("The joint variables that lead to singular configurations:")
print(sings)
print()

# PS: This solves the equation without the use of atan2!