Tune mpc in sim

stack/main/src/executor/executor/mpc_initializer_node.py

@@ -31,48 +31,34 @@ def __init__(self):
         self.data_dir = os.getenv('TRUNK_DATA', '/home/trunk/Documents/trunk-stack/stack/main/data')
 
         # Generate reference trajectory
-        z_ref, t = self._generate_ref_trajectory(10, 0.01, 'figure_eight', 0.1)
+        z_ref, t = self._generate_ref_trajectory(10, 0.01, 'circle', 0.075)
 
         # Load the model
         self._load_model()
 
         # MPC configuration
-        # Qz = 1 * jnp.eye(self.model.n_z)
-        # Qz = Qz.at[1, 1].set(0)
-        # Qzf = 10 * jnp.eye(self.model.n_z)
-        # Qzf = Qzf.at[1, 1].set(0)
-
-        # R_tip, R_mid, R_top = 0.006, 0.008, 0.007
+        U = HyperRectangle([0.4]*6, [-0.4]*6)
+        dU = HyperRectangle([0.1]*6, [-0.1]*6)
+        # U = None
+        dU = None
 
-        # TODO: John edits
-        Qz = 5. * jnp.eye(self.model.n_z)
+        Qz = 5 * jnp.eye(self.model.n_z)
         Qz = Qz.at[1, 1].set(0)
-        Qzf = 0. * jnp.eye(self.model.n_z)
+        Qzf = 10 * jnp.eye(self.model.n_z)
         Qzf = Qzf.at[1, 1].set(0)
-
-        # TODO: John edits
-        R_tip, R_mid, R_top = 0.0055, 0.008, 0.01
-
+        R_tip, R_mid, R_top = 0.001, 0.0075, 0.01
         R = jnp.diag(jnp.array([R_tip, R_mid, R_top, R_mid, R_top, R_tip]))
+
         gusto_config = GuSTOConfig(
             Qz=Qz,
             Qzf=Qzf,
             R=R,
             x_char=0.05*jnp.ones(self.model.n_x),
             f_char=0.5*jnp.ones(self.model.n_x),
-            N=5
+            N=6
         )
-        U = HyperRectangle([0.4]*6, [-0.4]*6)
-        # dU = HyperRectangle([0.1]*6, [-0.1]*6)
-        dU = None
-
-        # TODO: John edits
-        # dU = HyperRectangle([0.03]*6, [-0.03]*6)
 
         x0 = jnp.zeros(self.model.n_x)
-        # self.mpc_solver_node = run_mpc_solver_node(self.model, gusto_config, x0, t=t, z=z_ref, U=U, dU=dU)
-
-        # TODO: John edits
         self.mpc_solver_node = run_mpc_solver_node(self.model, gusto_config, x0, t=t, z=z_ref, U=U, dU=dU, solver="GUROBI")