From 9b83c90dc17c0fbc71a0ad9bf823ee6e0535a023 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:34:02 +0100
Subject: [PATCH] fix: sorted eigenvalues and eigenvectors when required
---
python/IK_manipulator3D.py | 20 +++++++++++---------
python/IK_manipulator_manipulability.py | 8 +++++++-
python/iLQR_manipulator3D.py | 2 +-
3 files changed, 19 insertions(+), 11 deletions(-)
diff --git a/python/IK_manipulator3D.py b/python/IK_manipulator3D.py
index 3f00d65..e31880c 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 923d6f4..ba11096 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 b770b19..1f34ddf 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
--
GitLab