From dfd128b38eab2aec9bb21db4a4828a280f9cab7f Mon Sep 17 00:00:00 2001 From: Daniel He Date: Tue, 13 Feb 2024 14:36:21 +0100 Subject: [PATCH] feature: I implemented the u_ref = [m*g, 0, 0, 0] (reference for thrust, tau_roll, tau_pitch, tau_yaw) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit For target_position = np.array([0.1, 0.1, 0.8] and starting position INIT_XYZS = np.array([[0., 0., 0.7]]) I get something around x +00.12, y +00.13, z +00.83 ——— velocity +00.02, +00.05, +00.01 --- gym_pybullet_drones/control/MPCCasADiControl.py | 14 ++++++++++++-- runnables/MPC_CasADi.py | 4 ++-- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/gym_pybullet_drones/control/MPCCasADiControl.py b/gym_pybullet_drones/control/MPCCasADiControl.py index f8de684e..0a305759 100644 --- a/gym_pybullet_drones/control/MPCCasADiControl.py +++ b/gym_pybullet_drones/control/MPCCasADiControl.py @@ -179,11 +179,21 @@ def __init__(self, h = 0.2 J = 0 + # Calculate the hovering thrust as a scalar + hovering_thrust = 0.28449 # 0.029 * 9.81 m and g should be scalar values (floats or ints) + tau_phi_ref = 0 + tau_theta_ref = 0 + tau_psi_ref = 0 + + # Create a DM vector for the reference control input + u_ref = DM([hovering_thrust, tau_phi_ref, tau_theta_ref, tau_psi_ref]) + for k in range(Nhoriz - 1): st_ref = P[Nx:2 * Nx] st = X[:, k] cont = U[:, k] - J += (st - st_ref).T @ Q @ (st - st_ref) + cont.T @ R @ cont + cont_ref = u_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) @@ -534,7 +544,7 @@ def computeControl(self, # Thrust calculation # Original scalar thrust normalized - thrust_normalized_scalar = thrust_step_0 * 1.2 + thrust_normalized_scalar = thrust_step_0 # Convert to a 3D vector with the thrust applied along the z-axis thrust_normalized = np.array([0, 0, thrust_normalized_scalar]) diff --git a/runnables/MPC_CasADi.py b/runnables/MPC_CasADi.py index 83507098..2f810618 100644 --- a/runnables/MPC_CasADi.py +++ b/runnables/MPC_CasADi.py @@ -60,7 +60,7 @@ def run( colab=DEFAULT_COLAB ): #### Initialize the simulation ############################# - INIT_XYZS = np.array([[0., 0., 0.5]]) + INIT_XYZS = np.array([[0., 0., 0.7]]) INIT_RPYS = np.array([[0., 0., 0.]]) #### Initialize a circular trajectory ###################### @@ -113,7 +113,7 @@ def run( #### Step the simulation ################################### obs, reward, terminated, truncated, info = env.step(action) #target_position = TARGET_TRAJECTORY.get_waypoint(current_step).coordinate - target_position = np.array([0.1, 0.1, 0.1]) + target_position = np.array([0.1, 0.1, 0.8]) #### Compute control for the current way point ############# action, _, _ = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, state=obs[0],