fix: fix indexes in iLQRManipulator3d.py

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