R2q() fixed in all python examples

python/IK_manipulator3D.py

@@ -125,7 +125,7 @@ def R2q(R):
 			[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
 
-	e_val, e_vec = np.linalg.eig(K)
+	e_val, e_vec = np.linalg.eig(K) # unsorted eigenvalues
 	q = np.real([e_vec[3, np.argmax(e_val)], *e_vec[0:3, np.argmax(e_val)]]) # for quaternions as [w,x,y,z]
 	return q
 

python/iLQR_manipulator3D.py

@@ -120,9 +120,7 @@ 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
-
-	e_val, e_vec = np.linalg.eig(K)
-	# /!\ With numpy eigenvalues are not sorted!
+	e_val, e_vec = np.linalg.eig(K) # unsorted eigenvalues
 	q = np.real([e_vec[3, np.argmax(e_val)], *e_vec[0:3, np.argmax(e_val)]]) # for quaternions as [w,x,y,z]
 	return q
 
@@ -172,7 +170,7 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm
 
 # Precision matrix
 # Q = np.identity((param.nbVarF-1) * param.nbPoints) # Full precision matrix
-Qr = np.diag( [1.0,1.0,1.0,1.0,1.0,0.0] * param.nbPoints) # Precision matrix in relative coordinate frame (tool frame) (by removing orientation constraint on 3rd axis)
+Qr = np.diag([1.0,1.0,1.0,1.0,1.0,0.0] * param.nbPoints) # Precision matrix in relative coordinate frame (tool frame) (by removing orientation constraint on 3rd axis)
 
 # Control weight matrix (at trajectory level)
 R = np.eye((param.nbData-1) * param.nbVarU) * param.r
@@ -218,10 +216,10 @@ for i in range(param.nbIter):
 
 	Q = Ra @ Qr @ Ra.T # Precision matrix in absolute coordinate frame (base frame)
 	
-	#du = np.linalg.pinv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # Gauss-Newton update
+	#du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # Gauss-Newton update
 	du = np.linalg.lstsq(
 		Su.T @ J.T @ Q @ J @ Su + R,
-		-Su.T @ J.T @ Q @ f - u * param.r ,
+		-Su.T @ J.T @ Q @ f - u * param.r,
 		rcond=-1
 	)[0] # Gauss-Newton update