diff --git a/matlab/iLQR_manipulator3D.m b/matlab/iLQR_manipulator3D.m
index c9570dc16d6d098691651acb521f20e005f17635..a14ac70c78d66bd987a3e879499de5b998fa8d9b 100644
--- a/matlab/iLQR_manipulator3D.m
+++ b/matlab/iLQR_manipulator3D.m
@@ -35,7 +35,7 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0]; %Length of the common norm
 %Weight matrices for iLQR cost
 %Qr = speye((param.nbVarF-1) * param.nbPoints) * param.q; %Precision matrix in relative coordinate frame (tool frame)
 %Qr = kron(eye(param.nbPoints), diag([0, 0, 0, 1E0, 1E0, 1E0])); %Precision matrix in relative coordinate frame (tool frame), by removing position constraint
-Qr = kron(eye(param.nbPoints), diag([1, 1, 1, 1, 1, 0])); %Precision matrix in relative coordinate frame (tool frame), by removing orientation constraint on 3rd axis
+Qr = kron(eye(param.nbPoints), diag([1, 1, 1, 1, 1, 0])) * param.q; %Precision matrix in relative coordinate frame (tool frame), by removing orientation constraint on 3rd axis
 
 R = speye(param.nbVarU * (param.nbData-1)) * param.r; %Control weight matrix (at trajectory level)
 
diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py
index a49b92c18370c0ebf7d0b152b06acaec55ba4fc6..45b255f3c3f88447553937c179a83b30d550af27 100644
--- a/python/iLQR_manipulator3D.py
+++ b/python/iLQR_manipulator3D.py
@@ -171,7 +171,6 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm
 # 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)
 R = np.eye((param.nbData-1) * param.nbVarU) * param.r
 
@@ -193,7 +192,7 @@ Su = Su0[idx,:] # We remove the lines that are out of interest
 # ===============================
 
 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 = np.array([0, 0, 0, -np.pi/2, 0, np.pi/2, 0]) # Initial robot pose
 
 x0 = x0.reshape((-1, 1))