From 279a54315740bf695ae688faabefe61ebd257461 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?J=C3=A9r=C3=A9my=20Maceiras?= <jeremy.maceiras@idiap.ch>
Date: Mon, 4 Nov 2024 15:41:46 +0100
Subject: [PATCH] fix: fix indexes in iLQRManipulator3d.py

---
 python/iLQR_manipulator3D.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index a49b92c..6d34847 100644
--- a/python/iLQR_manipulator3D.py
+++ b/python/iLQR_manipulator3D.py
@@ -206,14 +206,13 @@ for i in range(param.nbIter):
 
 	Ra = np.zeros_like(Qr)
 
-	for i in range(param.nbPoints):
-		Rtmp = q2R(param.Mu[-4:,i]) # Orientation matrix for target
+	for j in range(param.nbPoints):
 		Rkp = np.zeros((param.nbVarF-1,param.nbVarF-1)) # Transformation matrix with both translation and rotation
 		Rkp[:3,:3] = np.identity(3) # For translation
-		Rkp[-3:,-3:] = Rtmp # For rotation
+		Rkp[-3:,-3:] = q2R(param.Mu[-4:,j]) # Orientation matrix for target
 
 		nbVarQ = param.nbVarF - 1
-		Ra[i*nbVarQ:(i+1)*nbVarQ,i*nbVarQ:(i+1)*nbVarQ] = Rkp
+		Ra[j*nbVarQ:(j+1)*nbVarQ,j*nbVarQ:(j+1)*nbVarQ] = Rkp
 
 	Q = Ra @ Qr @ Ra.T # Precision matrix in absolute coordinate frame (base frame)
 	du = np.linalg.pinv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # Gauss-Newton update
-- 
GitLab