From c1030e251c7b8b6428587b45aec4691abfdbc720 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 17:12:20 +0100
Subject: [PATCH] fix: changed call to pinv by lstsq
---
python/iLQR_bicopter.py | 8 ++++++--
python/iLQR_bimanual.py | 9 ++++++---
python/iLQR_bimanual_manipulability.py | 8 +++++++-
python/iLQR_car.py | 6 +++++-
python/iLQR_distMaintenance.py | 8 ++++++--
python/iLQR_manipulator.py | 8 +++++++-
python/iLQR_manipulator_CP.py | 6 +++++-
python/iLQR_manipulator_CoM.py | 7 +++++--
python/iLQR_manipulator_boundary.py | 8 +++++---
python/iLQR_manipulator_dynamics.py | 6 +++++-
python/iLQR_manipulator_initStateOptim.py | 7 ++++++-
python/iLQR_manipulator_object_affordance.py | 6 +++++-
python/iLQR_manipulator_obstacle.py | 15 +++++++++++----
python/iLQR_obstacle.py | 14 ++++++++++----
python/iLQR_spacetime.py | 7 ++++++-
15 files changed, 95 insertions(+), 28 deletions(-)
diff --git a/python/iLQR_bicopter.py b/python/iLQR_bicopter.py
index cce4181..0892b93 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 0983da0..14c5357 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 cab68cd..7875d09 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 cc32c35..26b4e13 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 b0b8bfa..24e31f1 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 be03c02..759470a 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 ec1c37a..44afe88 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 98dfa0e..6114c09 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 d35cdb8..40b30ad 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 e02caba..e1cafac 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 d020483..b55f910 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 6499109..81fae54 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 f35e3f1..2d3218f 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 0392599..517c333 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 13475cb..82b9f19 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
--
GitLab