From 4259a8f0387ba66294cfba99e5f27c8286e5f2e0 Mon Sep 17 00:00:00 2001
From: Philip Abbet <philip.abbet@idiap.ch>
Date: Fri, 23 Feb 2024 14:28:18 +0100
Subject: [PATCH] Fix some issues in 'iLQR_manipulator3D.py'
---
python/iLQR_manipulator3D.py | 38 +++++++++++++++++++++++-------------
1 file changed, 24 insertions(+), 14 deletions(-)
diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index 60e66de..d16fb83 100644
--- a/python/iLQR_manipulator3D.py
+++ b/python/iLQR_manipulator3D.py
@@ -79,19 +79,20 @@ def logmap(f, f0):
# Logarithmic map for S^3 manifold (with e in ambient space)
def logmap_S3(x, x0):
+ x0 = x0.reshape((4, 1))
+ x = x.reshape((4, 1))
+
th = acoslog(x0.T @ x)
- if math.isnan(th):
- th = 1.0
u = x - (x0.T @ x) * x0
- u = np.multiply(th, u) / np.linalg.norm(u)
+ if np.linalg.norm(u) > 1e-9:
+ u = np.multiply(th, u) / np.linalg.norm(u)
- # import sys; sys.exit(0)
- return u
+ return u[:, 0]
# Arcosine redefinition to make sure the distance between antipodal quaternions is zero
def acoslog(x):
- y = np.arccos(x)
+ y = np.arccos(x)[0][0]
if (x>=-1.0) and (x<0):
y = y - np.pi
return y
@@ -175,7 +176,7 @@ Q = np.eye((param.nbVarF-1) * param.nbPoints) * param.q
R = np.eye((param.nbData-1) * param.nbVarU) * param.r
# Time occurrence of viapoints
-tl = np.linspace(0, param.nbData, param.nbPoints+1)
+tl = np.linspace(1, param.nbData, param.nbPoints+1)
tl = np.rint(tl[1:]).astype(np.int64) - 1
idx = np.array([i + np.arange(0,param.nbVarX,1) for i in (tl*param.nbVarX)]).flatten()
@@ -191,28 +192,37 @@ Su = Su0[idx,:] # We remove the lines that are out of interest
# iLQR
# ===============================
-u = np.zeros(param.nbVarU * (param.nbData-1)) # Initial control command
+u = np.zeros((param.nbVarU * (param.nbData-1), 1)) # Initial control command
x0 = np.array([0, 0, 0, -np.pi/2, -0, np.pi/2, 0]) # Initial robot pose
+x0 = x0.reshape((-1, 1))
+
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
+ f = f.reshape((-1,1), order='F')
+
+ du = np.linalg.pinv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # 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
+ cost0 = f.T @ Q @ f + np.linalg.norm(u)**2 * param.r # Cost
while True:
utmp = u + du * alpha
xtmp = Su0 @ utmp + Sx0 @ x0 # System evolution
xtmp = xtmp.reshape([param.nbVarX, param.nbData], order='F')
- ftmp,_ = f_reach(xtmp[:,tl], param) # Residuals
- cost = ftmp.flatten('F').T @ Q @ ftmp.flatten('F') + np.linalg.norm(utmp)**2 * param.r # Cost
+ ftmp,_ = f_reach(xtmp[:,tl], param) # Residuals
+ ftmp = ftmp.reshape((-1,1), order='F')
+ cost = ftmp.T @ Q @ ftmp + np.linalg.norm(utmp)**2 * param.r # Cost
if cost < cost0 or alpha < 1e-3:
- u = utmp
- print("Iteration {}, cost: {}".format(i,cost))
+ print("Iteration {}, cost: {}".format(i,cost[0][0]))
break
alpha /= 2
+
+ u = u + du * alpha
+
if np.linalg.norm(du * alpha) < 1E-2:
break # Stop iLQR iterations when solution is reached
--
GitLab