diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index 7a766233c9e4114ad7cfb1d1dd3f93345eee63f7..b770b1961d0b932758c17ac2c0662418fab3c4a3 100644
--- a/python/iLQR_manipulator3D.py
+++ b/python/iLQR_manipulator3D.py
@@ -120,8 +120,10 @@ def R2q(R):
 			[R[2,0]+R[0,2], R[2,1]+R[1,2], R[2,2]-R[0,0]-R[1,1], R[0,1]-R[1,0]],
 			[R[1,2]-R[2,1], R[2,0]-R[0,2], R[0,1]-R[1,0], R[0,0]+R[1,1]+R[2,2]],
 	]) / 3.0
-	_, V = np.linalg.eig(K)
-	return np.real([V[3, 0], V[0, 0], V[1, 0], V[2, 0]]) # for quaternions as [w,x,y,z]
+
+	e_val, e_vec = np.linalg.eig(K)
+	q = np.array([e_vec[3, np.argmax(e_val)], *e_vec[0:3, np.argmax(e_val)]])
+	return q
 
 # Plot coordinate system
 def plotCoordSys(ax, x, R, width=1):
@@ -240,7 +242,7 @@ for i in range(param.nbIter):
 
 	u = u + du * alpha
 
-	if np.linalg.norm(du * alpha) < 1E-2 or cost < 1.5e-3:
+	if np.linalg.norm(du * alpha) < 1E-2:
 		break # Stop iLQR iterations when solution is reached