diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py index 899ccca2def20f7c57c2cda2af3818ec91694df9..ed5362d32bfbcf811fd36645e52c4cef047a6996 100644 --- a/python/iLQR_manipulator3D.py +++ b/python/iLQR_manipulator3D.py @@ -122,6 +122,7 @@ def R2q(R): ]) / 3.0 e_val, e_vec = np.linalg.eig(K) + # /!\ With numpy eigenvalues are not sorted! 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