From e0aa22d75fef266949e9c8711d675a7b07c6018f 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 16:47:27 +0100
Subject: [PATCH] fix: fixed least square computation in iLQR_manipulator3d.py

---
 python/iLQR_manipulator3D.py | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index 6d34847..7a76623 100644
--- a/python/iLQR_manipulator3D.py
+++ b/python/iLQR_manipulator3D.py
@@ -215,7 +215,13 @@ for i in range(param.nbIter):
 		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
+	
+	#du = np.linalg.pinv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # Gauss-Newton update
+	du = np.linalg.lstsq(
+		Su.T @ J.T @ Q @ J @ Su + R,
+		-Su.T @ J.T @ Q @ f - u * param.r ,
+		rcond=-1
+	)[0] # Gauss-Newton update
 
 	# Estimate step size with backtracking line search method
 	alpha = 1
@@ -234,7 +240,7 @@ for i in range(param.nbIter):
 
 	u = u + du * alpha
 
-	if np.linalg.norm(du * alpha) < 1E-2:
+	if np.linalg.norm(du * alpha) < 1E-2 or cost < 1.5e-3:
 		break # Stop iLQR iterations when solution is reached
 
 
-- 
GitLab