feat: allow custom Q matrix

python/iLQR_manipulator3D.py

@@ -144,7 +144,7 @@ param.nbPoints = 2 # Number of viapoints
 param.nbVarX = 7 # State space dimension (x1,x2,x3,...)
 param.nbVarU = param.nbVarX # Control space dimension (dx1,dx2,dx3,...)
 param.nbVarF = 7 # Task space dimension (f1,f2,f3 for position, f4,f5,f6,f7 for unit quaternion)
-param.q = 1e0 # Tracking weighting term
+param.q = [1,1,1,0.1,0.1,0.1] # Tracking weighting term for each parameter of F (3 first position, 3 last orientation)
 param.r = 1e-6 # Control weighting term
 
 Rtmp = q2R([np.cos(np.pi/3), np.sin(np.pi/3), 0.0, 0.0])
@@ -170,7 +170,8 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm
 # ===============================
 
 # Precision matrix
-Q = np.eye((param.nbVarF-1) * param.nbPoints) * param.q
+Q = np.diag( param.q * param.nbPoints) 
+
 
 # Control weight matrix (at trajectory level)
 R = np.eye((param.nbData-1) * param.nbVarU) * param.r
@@ -204,7 +205,7 @@ for i in range(param.nbIter):
 	f, J = f_reach(x[:,tl], param) # Residuals and Jacobians
 	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
+	du = np.linalg.inv(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