fix: fixed least square computation in iLQR_manipulator3d.py

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