From 02ffe1dfdf1849ee4bcc936cf136648573332af5 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:48:57 +0100
Subject: [PATCH] fix: cast quaternion to real

---
 python/IK_manipulator3D.py   | 2 +-
 python/iLQR_manipulator3D.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/python/IK_manipulator3D.py b/python/IK_manipulator3D.py
index e31880c..ed85b97 100644
--- a/python/IK_manipulator3D.py
+++ b/python/IK_manipulator3D.py
@@ -126,7 +126,7 @@ def R2q(R):
 	]) / 3.0
 
 	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)]]) # for quaternions as [w,x,y,z]
+	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
 
 # Plot coordinate system
diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index 1f34ddf..899ccca 100644
--- a/python/iLQR_manipulator3D.py
+++ b/python/iLQR_manipulator3D.py
@@ -122,7 +122,7 @@ def R2q(R):
 	]) / 3.0
 
 	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)]]) # for quaternions as [w,x,y,z]
+	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
 
 # Plot coordinate system
-- 
GitLab