From 44a758c824391b4d5bb229bd9aa58be3cd46374b Mon Sep 17 00:00:00 2001 From: Daniel He Date: Fri, 16 Feb 2024 17:30:35 +0100 Subject: [PATCH] test: Tested the MPC controller with the following if statemets: if distance < 0.2: if current_step == len(TARGET_TRAJECTORY) - 1: and 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) # -> Test result: Drone completely goes off the rails and crazyflie flies crazily and fails --- gym_pybullet_drones/control/MPCCasADiControl.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gym_pybullet_drones/control/MPCCasADiControl.py b/gym_pybullet_drones/control/MPCCasADiControl.py index dc065295..52350d10 100644 --- a/gym_pybullet_drones/control/MPCCasADiControl.py +++ b/gym_pybullet_drones/control/MPCCasADiControl.py @@ -410,6 +410,11 @@ def __init__(self, # DEFAULT_DISCR_LEVEL = 10 -> causes it to fail on at least two tracks midways''' + # 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 h = 0.15