python/iLQR_manipulator3D.py

@@ -206,17 +206,22 @@ for i in range(param.nbIter):
 
 	Ra = np.zeros_like(Qr)
 
-	for i in range(param.nbPoints):
-		Rtmp = q2R(param.Mu[-4:,i]) # Orientation matrix for target
+	for j in range(param.nbPoints):
 		Rkp = np.zeros((param.nbVarF-1,param.nbVarF-1)) # Transformation matrix with both translation and rotation
 		Rkp[:3,:3] = np.identity(3) # For translation
-		Rkp[-3:,-3:] = Rtmp # For rotation
+		Rkp[-3:,-3:] = q2R(param.Mu[-4:,j]) # Orientation matrix for target
 
 		nbVarQ = param.nbVarF - 1
-		Ra[i*nbVarQ:(i+1)*nbVarQ,i*nbVarQ:(i+1)*nbVarQ] = Rkp
+		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
@@ -235,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