diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index 73a2dd1d7cc5af7598950259a9ead0488027d6bf..a49b92c18370c0ebf7d0b152b06acaec55ba4fc6 100644
--- a/python/iLQR_manipulator3D.py
+++ b/python/iLQR_manipulator3D.py
@@ -143,7 +143,6 @@ 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 = [1,1,1,1,1,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])
@@ -169,7 +168,8 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm
 # ===============================
 
 # Precision matrix
-Q = np.diag( param.q * param.nbPoints) 
+# Q = np.identity((param.nbVarF-1) * param.nbPoints) # Full precision matrix
+Qr = np.diag( [1.0,1.0,1.0,1.0,1.0,0.0] * param.nbPoints) # Precision matrix in relative coordinate frame (tool frame) (by removing orientation constraint on 3rd axis)
 
 
 # Control weight matrix (at trajectory level)
@@ -204,7 +204,19 @@ 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.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # Gauss-Newton update
+	Ra = np.zeros_like(Qr)
+
+	for i in range(param.nbPoints):
+		Rtmp = q2R(param.Mu[-4:,i]) # Orientation matrix for target
+		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
+
+		nbVarQ = param.nbVarF - 1
+		Ra[i*nbVarQ:(i+1)*nbVarQ,i*nbVarQ:(i+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
 
 	# Estimate step size with backtracking line search method
 	alpha = 1