Evo ako neko ima problema s taylorom. Inace neke stvari ne radi kao recimo zbrajanje pa treba koristit np.add i tako neke sitnice ( la.norm isto meni nije radio, pa treba drugacije slozit ). Ali rekurzija i izlaz da su sve točke lijepo u istoj matrici bi trebalo radit. Razlika je sto ova funkcija vraca bas točke, a za pokretanje robota trebaju zakreti zglobova. U pripremi pise da mora vratit točke pa zato i vrača točke. Kasnije ce trebat s funkcijom def_ik to pretvorit u zakrete zglobova
import numpy.linalg as la
def taylor_swift(self, w0, w1, q0, tol=0.01):
q0 = self.get_ik(w0,q0)
q1 = self.get_ik(w1,q0)
q_m = (q0+q1)/2.0
w_m = self.get_dk(q_m)
w_M = (w0+w1)/2.0
# L2 norma
if la.norm(w_m, w_M) <= tol:
return np.array([w0, w1])
else:
left = taylor_swift(w0, w_M)
right = taylor_swift(w_M, w1)
return np.vstack((left, right))
