diff --git a/python/IK_manipulator3D.py b/python/IK_manipulator3D.py index e31880c72689ac0b55e5646cfae6c5c5a22157be..ed85b97e0fce91279f06c16592633c1e989bc48a 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 1f34ddf09abda2097038ac7c18917008edbf0a52..899ccca2def20f7c57c2cda2af3818ec91694df9 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