diff --git a/python/IK_manipulator3D.py b/python/IK_manipulator3D.py
index 3f00d657564c21225fda270e308ab98d39a4a737..e31880c72689ac0b55e5646cfae6c5c5a22157be 100644
--- a/python/IK_manipulator3D.py
+++ b/python/IK_manipulator3D.py
@@ -117,15 +117,17 @@ def q2R(q):
 
 # Rotation matrix to unit quaternion conversion
 def R2q(R):
-    R = R.T
-    K = np.array([
-            [R[0,0]-R[1,1]-R[2,2], R[1,0]+R[0,1], R[2,0]+R[0,2], R[1,2]-R[2,1]],
-            [R[1,0]+R[0,1], R[1,1]-R[0,0]-R[2,2], R[2,1]+R[1,2], R[2,0]-R[0,2]],
-            [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]
+	R = R.T
+	K = np.array([
+			[R[0,0]-R[1,1]-R[2,2], R[1,0]+R[0,1], R[2,0]+R[0,2], R[1,2]-R[2,1]],
+			[R[1,0]+R[0,1], R[1,1]-R[0,0]-R[2,2], R[2,1]+R[1,2], R[2,0]-R[0,2]],
+			[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)
+	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]
+	return q
 
 # Plot coordinate system
 def plotCoordSys(ax, x, R, width=1):
diff --git a/python/IK_manipulator_manipulability.py b/python/IK_manipulator_manipulability.py
index 923d6f47e8a29572420f2f6bf154482bdc07543c..ba110964de82621966d665a6aa733d3dadfc7b45 100644
--- a/python/IK_manipulator_manipulability.py
+++ b/python/IK_manipulator_manipulability.py
@@ -14,7 +14,7 @@ import matplotlib
 import matplotlib.pyplot as plt
 import math as m
 import scipy
-
+import scipy.spatial
 
 # Logarithmic map for R^2 x S^1 manifold
 def logmap(f, f0):
@@ -218,6 +218,12 @@ if ellBound == True:
         A = np.diag(coeff ** 2)
         Q = J @ A @ J.T
         eigenvals, eigenvecs = np.linalg.eig(Q)
+        
+        # Sort Eigenvalues and EigenVectors
+        idx = eigenvals.argsort()[::-1]   
+        eigenvals = eigenvals[idx]
+        eigenvecs = eigenvecs[idx]
+
         # the sqrt of the eigenvalues give the length of the semi-axes
         print(f"Ellipsoid eigenvalues: {eigenvals}")
         vec1, vec2 = eigenvecs.T
diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index b770b1961d0b932758c17ac2c0662418fab3c4a3..1f34ddf09abda2097038ac7c18917008edbf0a52 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)]])
+	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]
 	return q
 
 # Plot coordinate system