From 47a9b78216f04b7aa2b8375babbd0d44c45ecc25 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?J=C3=A9r=C3=A9my=20Maceiras?= <jeremy.maceiras@idiap.ch>
Date: Tue, 5 Nov 2024 10:11:06 +0100
Subject: [PATCH] fix: Fixed R2q method

---
 python/iLQR_manipulator3D.py | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index 7a76623..b770b19 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
 
 
-- 
GitLab