Fix some issues in 'iLQR_manipulator3D.py'

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