diff --git a/python/iLQR_bicopter.py b/python/iLQR_bicopter.py
index cce4181b58c3827fffa6e913e12db0e1864f825d..0892b93fbacb8d23433604fc82e9d555a207bc7a 100644
--- a/python/iLQR_bicopter.py
+++ b/python/iLQR_bicopter.py
@@ -118,8 +118,12 @@ for i in range(param.nbIter):
# Gauss-Newton update
e = x[:,tl].flatten() - param.Mu
- du = np.linalg.inv(Su.T @ Q @ Su + R) @ (-Su.T @ Q @ e - R @ u)
-
+ du = np.linalg.lstsq(
+ Su.T @ Q @ Su + R,
+ -Su.T @ Q @ e - R @ u,
+ rcond=-1
+ )[0] # Gauss-Newton update
+
# Estimate step size with backtracking line search method
alpha = 1
cost0 = e.T @ Q @ e + u.T @ R @ u
diff --git a/python/iLQR_bimanual.py b/python/iLQR_bimanual.py
index 0983da0c5ab714cfad35d5baeacf498feac90f36..14c5357745183327704b166bd32603560045263a 100644
--- a/python/iLQR_bimanual.py
+++ b/python/iLQR_bimanual.py
@@ -149,10 +149,13 @@ for i in range(param.nbIter):
# print(Qc.shape)
# print(fc.flatten('F'))
- du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Su0.T @ Jc.T @ Qc @ Jc @ Su0 + R) @ \
- (-Su.T @ J.T @ Q @ f.flatten('F') - \
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ Q @ J @ Su + Su0.T @ Jc.T @ Qc @ Jc @ Su0 + R,
+ (-Su.T @ J.T @ Q @ f.flatten('F') - \
Su0.T @ Jc.T @ Qc @ fc.flatten('F') - \
- u * param.r)
+ u * param.r),
+ rcond=-1
+ )[0] # Gauss-Newton update
# Estimate step size with line search method
alpha = 1
diff --git a/python/iLQR_bimanual_manipulability.py b/python/iLQR_bimanual_manipulability.py
index cab68cddcbf7116e1b9f840a7d43ac30a74d7dc1..7875d096ec9a7924d1de6c75237c99323c36747e 100644
--- a/python/iLQR_bimanual_manipulability.py
+++ b/python/iLQR_bimanual_manipulability.py
@@ -157,7 +157,13 @@ for i in range(param.nbIter):
x = Su0 @ u + Sx0 @ x0 # System evolution
x = x.reshape([param.nbVarX, param.nbData], order='F')
f, J = f_manipulability(x[:,tl], param) # Residuals and Jacobians
- du = np.linalg.inv(Su.T @ J.T @ J @ Su + R) @ (-Su.T @ J.T @ f.flatten('F') - u * param.r) # Gauss-Newton update
+
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ J @ Su + R,
+ -Su.T @ J.T @ f.flatten('F') - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
+
# Estimate step size with backtracking line search method
alpha = 1
cost0 = f.flatten('F').T @ f.flatten('F') + np.linalg.norm(u)**2 * param.r # Cost
diff --git a/python/iLQR_car.py b/python/iLQR_car.py
index cc32c3554ecb9ef76c9eea6153c78c06b04870d2..26b4e13d7b16bce246a209521fe7db53a4d08eec 100644
--- a/python/iLQR_car.py
+++ b/python/iLQR_car.py
@@ -111,7 +111,11 @@ for i in range(param.nbIter):
# Gauss-Newton update
e = x[:,tl].flatten('F') - param.Mu.flatten('F')
- du = np.linalg.inv(Su.T @ Q @ Su + R) @ (-Su.T @ Q @ e - R @ u)
+ du = np.linalg.lstsq(
+ Su.T @ Q @ Su + R,
+ -Su.T @ Q @ e - R @ u,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Estimate step size with backtracking line search method
alpha = 1
diff --git a/python/iLQR_distMaintenance.py b/python/iLQR_distMaintenance.py
index b0b8bfa8705007fd5d5de0f058b3766126fba2ae..24e31f1373407fd32319db762d226833126aa07a 100644
--- a/python/iLQR_distMaintenance.py
+++ b/python/iLQR_distMaintenance.py
@@ -62,8 +62,12 @@ for i in range( param.nbIter ):
x = Su @ u + Sx @ x0
x = x.reshape( ( param.nbVarX, param.nbData ),order="F" )
f, J = f_dist(x,param)# Avoidance objective
-
- du = np.linalg.inv(Su.T @ J.T @ J @ Su * param.q + R) @ (-Su.T @ J.T @ f * param.q - u * param.r)
+
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ J @ Su * param.q + R,
+ -Su.T @ J.T @ f * param.q - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Perform line search
alpha = 1
diff --git a/python/iLQR_manipulator.py b/python/iLQR_manipulator.py
index be03c02e3f43dd2f82c591a964019d69d0b949fc..759470a21d831a668d738a9f7763237f4b963d8e 100644
--- a/python/iLQR_manipulator.py
+++ b/python/iLQR_manipulator.py
@@ -133,7 +133,13 @@ for i in range(param.nbIter):
x = Su0 @ u + Sx0 @ x0 # System evolution
x = x.reshape([param.nbVarX, param.nbData], order='F')
f, J = f_reach(x[:,tl], param) # Residuals and Jacobians
- du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten('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.flatten('F') - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
+
# Estimate step size with backtracking line search method
alpha = 1
cost0 = f.flatten('F').T @ Q @ f.flatten('F') + np.linalg.norm(u)**2 * param.r # Cost
diff --git a/python/iLQR_manipulator_CP.py b/python/iLQR_manipulator_CP.py
index ec1c37a1439bd97faff1e0663e93e3c469176326..44afe88d46dc510e5d837f0e5743b3b0d3aa159e 100644
--- a/python/iLQR_manipulator_CP.py
+++ b/python/iLQR_manipulator_CP.py
@@ -189,7 +189,11 @@ for i in range( param.nbIter ):
x = x.reshape( ( param.nbData , param.nbVarX) )
f, J = f_reach(param,x[tl])
- dw = np.linalg.inv(PSI.T @ Su.T @ J.T @ Q @ J @ Su @ PSI + PSI.T @ R @ PSI) @ (-PSI.T @ Su.T @ J.T @ Q @ f.flatten() - PSI.T @ u * param.r)
+ dw = np.linalg.lstsq(
+ PSI.T @ Su.T @ J.T @ Q @ J @ Su @ PSI + PSI.T @ R @ PSI,
+ -PSI.T @ Su.T @ J.T @ Q @ f.flatten() - PSI.T @ u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
du = PSI @ dw
# Perform line search
alpha = 1
diff --git a/python/iLQR_manipulator_CoM.py b/python/iLQR_manipulator_CoM.py
index 98dfa0eba9cb37edb2ec867aaef35c3ba091300f..6114c090730d50ce101493f8dd01cbb7e3911154 100644
--- a/python/iLQR_manipulator_CoM.py
+++ b/python/iLQR_manipulator_CoM.py
@@ -143,8 +143,11 @@ for i in range(param.nbIter):
x = x.reshape([param.nbVarX, param.nbData], order='F')
f, J = f_reach(x[:,tl], param)
fc, Jc = f_reach_CoM(x, param)
- du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Su0.T @ Jc.T @ Qc @ Jc @ Su0 + R) @ \
- (-Su.T @ J.T @ Q @ f.flatten('F') - Su0.T @ Jc.T @ Qc @ fc.flatten('F') - u * param.r)
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ Q @ J @ Su + Su0.T @ Jc.T @ Qc @ Jc @ Su0 + R,
+ -Su.T @ J.T @ Q @ f.flatten('F') - Su0.T @ Jc.T @ Qc @ fc.flatten('F') - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Perform line search
alpha = 1
diff --git a/python/iLQR_manipulator_boundary.py b/python/iLQR_manipulator_boundary.py
index d35cdb8d81662fb12b55c2ba99efe58f4e10b8e1..40b30adf543df358784d69c35b50bbf59b6a5fd4 100644
--- a/python/iLQR_manipulator_boundary.py
+++ b/python/iLQR_manipulator_boundary.py
@@ -128,9 +128,11 @@ for i in range(param.nbIter):
Sv = Su0[idv, :]
- du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Sv.T @ Jv.T @ Jv @ Sv * param.rv + R) @ (
- -Su.T @ J.T @ Q @ f.flatten("F") - Sv.T @ Jv.T @ v * param.rv - u * param.r
- ) # Gauss-Newton update
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ Q @ J @ Su + Sv.T @ Jv.T @ Jv @ Sv * param.rv + R,
+ -Su.T @ J.T @ Q @ f.flatten("F") - Sv.T @ Jv.T @ v * param.rv - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Estimate step size with backtracking line search method
alpha = 1
diff --git a/python/iLQR_manipulator_dynamics.py b/python/iLQR_manipulator_dynamics.py
index e02caba6bf6410be734d64f6155d21d30e0c84d6..e1cafac82599d91b30b9d0b1b692b1c4cef61cff 100644
--- a/python/iLQR_manipulator_dynamics.py
+++ b/python/iLQR_manipulator_dynamics.py
@@ -201,7 +201,11 @@ for i in range( param.nbIter ):
Su = Su0[idx.flatten()]
f, J = f_reach(x[tl,:param.nbVarX],param)
- du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten() - u * param.r) # Gauss-Newton update
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ Q @ J @ Su + R,
+ -Su.T @ J.T @ Q @ f.flatten() - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Perform line search
alpha = 1
diff --git a/python/iLQR_manipulator_initStateOptim.py b/python/iLQR_manipulator_initStateOptim.py
index d020483e95c99a3f179c536a1d73dc7de46d8f5c..b55f910fabc82f174d9628275baf88be37dfb949 100644
--- a/python/iLQR_manipulator_initStateOptim.py
+++ b/python/iLQR_manipulator_initStateOptim.py
@@ -118,7 +118,12 @@ for i in range(param.nbIter):
x = x.reshape([param.nbVarX, param.nbData], order='F')
f, J = f_reach(x[:,tl], param) # Residuals and Jacobians
fua = ua - np.concatenate((x0_hat, np.zeros(param.nbVarU * (param.nbData-1)))) # Control command evolution
- dua = np.linalg.inv(Sa.T @ J.T @ Q @ J @ Sa + Ra) @ (-Sa.T @ J.T @ Q @ f.flatten('F') - Ra @ fua) # Gauss-Newton update
+ dua = np.linalg.lstsq(
+ Sa.T @ J.T @ Q @ J @ Sa + Ra,
+ -Sa.T @ J.T @ Q @ f.flatten('F') - Ra @ fua,
+ rcond=-1
+ )[0] # Gauss-Newton update
+
# Estimate step size with backtracking line search method
alpha = 1
cost0 = f.flatten('F').T @ Q @ f.flatten('F') + fua.T @ Ra @ fua # Cost
diff --git a/python/iLQR_manipulator_object_affordance.py b/python/iLQR_manipulator_object_affordance.py
index 6499109203e0a4298befc7383fdea24e2399a80d..81fae5461750d2d1d4536e35ada9a435aa21e742 100644
--- a/python/iLQR_manipulator_object_affordance.py
+++ b/python/iLQR_manipulator_object_affordance.py
@@ -150,7 +150,11 @@ for i in range( param.nbIter ):
f, J = f_reach(param,x[tl])
fc , Jc = f_reach(param,x[tl],False)
- du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Su.T @ Jc.T @ Qc @ Jc @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten() - Su.T @ Jc.T @ Qc @ fc.flatten() - u * param.r)
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ Q @ J @ Su + Su.T @ Jc.T @ Qc @ Jc @ Su + R,
+ -Su.T @ J.T @ Q @ f.flatten() - Su.T @ Jc.T @ Qc @ fc.flatten() - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Perform line search
alpha = 1
diff --git a/python/iLQR_manipulator_obstacle.py b/python/iLQR_manipulator_obstacle.py
index f35e3f182cd8c4dbd67191802f8585b1f151b3d0..2d3218fb733467391236440ff5e75f1add7f59f4 100644
--- a/python/iLQR_manipulator_obstacle.py
+++ b/python/iLQR_manipulator_obstacle.py
@@ -229,11 +229,18 @@ for i in range( param.nbIter ):
if len(id2) > 0: # Numpy does not allow zero sized array as Indices
Su2 = Su0[id2.flatten()]
- du = np.linalg.inv(Su.T @ J.T @ J @ Su * param.Q_track + Su2.T @ J2.T @ J2 @ Su2 * param.Q_avoid + R) @ \
- (-Su.T @ J.T @ f.flatten() * param.Q_track - Su2.T @ J2.T @ f2.flatten() * param.Q_avoid - u * param.r)
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ J @ Su * param.Q_track + Su2.T @ J2.T @ J2 @ Su2 * param.Q_avoid + R,
+ -Su.T @ J.T @ f.flatten() * param.Q_track - Su2.T @ J2.T @ f2.flatten() * param.Q_avoid - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
else: # It means that we have a collision free path
- du = np.linalg.inv(Su.T @ J.T @ J @ Su * param.Q_track + R) @ \
- (-Su.T @ J.T @ f.flatten() * param.Q_track - u * param.r)
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ J @ Su * param.Q_track + R,
+ -Su.T @ J.T @ f.flatten() * param.Q_track - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
+
# Perform line search
alpha = 1
cost0 = np.linalg.norm(f.flatten())**2 * param.Q_track + np.linalg.norm(f2.flatten())**2 * param.Q_avoid + np.linalg.norm(u) * param.r
diff --git a/python/iLQR_obstacle.py b/python/iLQR_obstacle.py
index 03925991fd61c8387d2ccec56881670027bc842d..517c333eefad34cbbc4c492cf6e8fbeadaba5aff 100644
--- a/python/iLQR_obstacle.py
+++ b/python/iLQR_obstacle.py
@@ -124,11 +124,17 @@ for i in range( param.nbIter ):
if len(id2) > 0: # Numpy does not allow zero sized array as Indices
Su2 = Su0[id2.flatten()]
- du = np.linalg.inv(Su.T @ J.T @ J @ Su * param.Q_track + Su2.T @ J2.T @ J2 @ Su2 * param.Q_avoid + R) @ \
- (-Su.T @ J.T @ f.flatten() * param.Q_track - Su2.T @ J2.T @ f2.flatten() * param.Q_avoid - u * param.r)
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ J @ Su * param.Q_track + Su2.T @ J2.T @ J2 @ Su2 * param.Q_avoid + R,
+ -Su.T @ J.T @ f.flatten() * param.Q_track - Su2.T @ J2.T @ f2.flatten() * param.Q_avoid - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
else: # It means that we have a collision free path
- du = np.linalg.inv(Su.T @ J.T @ J @ Su * param.Q_track + R) @ \
- (-Su.T @ J.T @ f.flatten() * param.Q_track - u * param.r)
+ du = np.linalg.lstsq(
+ Su.T @ J.T @ J @ Su * param.Q_track + R,
+ -Su.T @ J.T @ f.flatten() * param.Q_track - u * param.r,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Perform line search
alpha = 1
diff --git a/python/iLQR_spacetime.py b/python/iLQR_spacetime.py
index 13475cbd87769c4c6c29302a3675832a41bb2fd6..82b9f19f51c87ca27aa673ade436862e3ddcac4d 100644
--- a/python/iLQR_spacetime.py
+++ b/python/iLQR_spacetime.py
@@ -102,7 +102,12 @@ for i in range(param.nbIter):
# Increment of control input
e = x[:, tl] - param.Mu
e = e.T.reshape((-1, 1))
- du = np.linalg.inv(Su.T @ Q @ Su + R) @ (-Su.T @ Q @ e - R @ u)
+
+ du = np.linalg.lstsq(
+ Su.T @ Q @ Su + R,
+ -Su.T @ Q @ e - R @ u,
+ rcond=-1
+ )[0] # Gauss-Newton update
# Perform line search
alpha = 1