feature: I implemented the u_ref = [m*g, 0, 0, 0] (reference for thru…

…st, tau_roll, tau_pitch, tau_yaw) 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
danielbinschmid · Feb 13, 2024 · dfd128b · dfd128b
1 parent 131eab4
commit dfd128b
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 4 deletions.
diff --git 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
@@ -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],