From c157495aceb70c12168bcb59815e286711188b59 Mon Sep 17 00:00:00 2001 From: Daniel He Date: Fri, 16 Feb 2024 10:15:43 +0100 Subject: [PATCH] test: Tested MPC Controller with the following weighting matrices: # Care 3x less about the reference input and velocities of the drone Q = diagcat(1, 1, 1, 0.1, 0.1, 0.1666, 0, 0, 0, 0, 0, 0) R = diagcat(0, 0, 0, 0) Objective function J += (st - st_ref).T @ Q @ (st - st_ref) + (cont - cont_ref).T @ R @ (cont - cont_ref) (always the same now, because R being 0-matrix cancels out the term anyway) Test result: # DEFAULT_DISCR_LEVEL = 10 -> goes off track after 1 point # DEFAULT_DISCR_LEVEL = 20 -> goes off track after 2 points # DEFAULT_DISCR_LEVEL = 30 -> goes off track after 7-8 points # DEFAULT_DISCR_LEVEL = 40 -> goes off track after 4 points # DEFAULT_DISCR_LEVEL = 50 -> goes off track after 4 points --- .../control/MPCCasADiControl.py | 21 ++++++++++++++----- runnables/test_suite_eval/pid.py | 2 +- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/gym_pybullet_drones/control/MPCCasADiControl.py b/gym_pybullet_drones/control/MPCCasADiControl.py index 325d6fe7..3755782c 100644 --- a/gym_pybullet_drones/control/MPCCasADiControl.py +++ b/gym_pybullet_drones/control/MPCCasADiControl.py @@ -195,10 +195,23 @@ def __init__(self, R = diagcat(0.3, 0.3, 0.3, 0.8) # -> Test result: Drone completely goes off the rails and crazyflie flies crazily and fails''' - # Don't care about the reference input of the drone + '''# Don't care about the reference input of the drone Q = diagcat(1, 1, 1, 0.6, 0.6, 1, 0, 0, 0, 0, 0, 0) R = diagcat(0, 0, 0, 0) - # -> Test result: Drone circles around a reference point indefinitely and doesn't reach it + # -> Test result: Drone circles around a reference point indefinitely and doesn't reach it''' + + # Care 3x less about and velocities and do not care about the reference input of the drone + Q = diagcat(1, 1, 1, 0.1, 0.1, 0.1666, 0, 0, 0, 0, 0, 0) + R = diagcat(0, 0, 0, 0) + # -> Test result: + # DEFAULT_DISCR_LEVEL = 10 -> goes off track after 1 point + # DEFAULT_DISCR_LEVEL = 20 -> goes off track after 2 points + # DEFAULT_DISCR_LEVEL = 30 -> goes off track after 7-8 points + # DEFAULT_DISCR_LEVEL = 40 -> goes off track after 4 points + # DEFAULT_DISCR_LEVEL = 50 -> goes off track after 4 points + + + #R = diagcat(0.1, 0.1, 0.1, 0.2666) x_init = P[0:Nx] g = X[:, 0] - P[0:Nx] # initial condition constraints @@ -219,9 +232,7 @@ def __init__(self, st = X[:, k] cont = U[:, k] cont_ref = u_ref - # Don't care about the reference input of the drone - #J += (st - st_ref).T @ Q @ (st - st_ref) + (cont - cont_ref).T @ R @ (cont - cont_ref) - 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) diff --git a/runnables/test_suite_eval/pid.py b/runnables/test_suite_eval/pid.py index 1cafe35b..be9960c5 100644 --- a/runnables/test_suite_eval/pid.py +++ b/runnables/test_suite_eval/pid.py @@ -49,7 +49,7 @@ DEFAULT_DURATION_SEC = 50 DEFAULT_OUTPUT_FOLDER = 'results' DEFAULT_COLAB = False -DEFAULT_DISCR_LEVEL = 30 +DEFAULT_DISCR_LEVEL = 50 DEFAULT_EVAL_SET = "./eval-v0_n-ctrl-points-3_n-tracks-20_2024-02-11_22:18:28_46929077-0248-4c6e-b2b1-da2afb13b2e2" from runnables.test_suite_eval.utils import compute_metrics_single