test: Tested MPC Controller with the following weighting matrices:

# Don't care about the velocity of the drone
Q = diagcat(1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0)
R = diagcat(0.3, 0.3, 0.3, 0.8)

Objective function
J += (st - st_ref).T @ Q @ (st - st_ref) + (cont - cont_ref).T @ R @ (cont - cont_ref)

Drone completely goes off the rails and crazyflie flies crazily and fails

gym_pybullet_drones/control/MPCCasADiControl.py

@@ -184,9 +184,15 @@ def __init__(self,
 
         '''
 
-        # Only care greedily about the position
+        '''# Only care greedily about the position 
         Q = diagcat(1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0)
         R = diagcat(0, 0, 0, 0)
+        # -> Test result: Drone completely goes off the rails and crazyflie flies crazily and fails'''
+
+        # Don't care about the velocity of the drone
+        Q = diagcat(1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0)
+        R = diagcat(0.3, 0.3, 0.3, 0.8)
+        # -> Test result: Drone completely goes off the rails and crazyflie flies crazily and fails
 
         x_init = P[0:Nx]
         g = X[:, 0] - P[0:Nx]  # initial condition constraints
@@ -207,9 +213,7 @@ def __init__(self,
             st = X[:, k]
             cont = U[:, k]
             cont_ref = u_ref
-            #J += (st - st_ref).T @ Q @ (st - st_ref) + (cont - cont_ref).T @ R @ (cont - cont_ref)
-            # Only care greedily about the position
-            J += (st - st_ref).T @ Q @ (st - st_ref)
+            J += (st - st_ref).T @ Q @ (st - st_ref) + (cont - cont_ref).T @ R @ (cont - cont_ref)
             st_next = X[:, k + 1]
             k1 = f(st, cont)
             k2 = f(st + h / 2 * k1, cont)