cost fix

Author: FedeClaudi

proj/control/control.py

@@ -18,6 +18,8 @@ def __init__(self, model):
         self.R = model.R
         self.Sf = model.Sf
 
+        self.angle_idx = model.ANGLE_IDX
+
         # Params
         self.max_iter = model.iLQR["max_iter"]
         self.init_mu = model.iLQR["init_mu"]

proj/control/cost.py

@@ -3,8 +3,7 @@
 
 
 class Cost:
-    @staticmethod
-    def fit_diff_in_range(diff_x):
+    def fit_diff_in_range(self, diff_x):
         """ fit difference state in range(angle)
 
         Args:
@@ -17,11 +16,15 @@ def fit_diff_in_range(diff_x):
         """
 
         if len(diff_x.shape) == 3:
-            diff_x[:, :, -2] = fit_angle_in_range(diff_x[:, :, -2])
+            diff_x[:, :, self.angle_idx] = fit_angle_in_range(
+                diff_x[:, :, self.angle_idx]
+            )
         elif len(diff_x.shape) == 2:
-            diff_x[:, -2] = fit_angle_in_range(diff_x[:, -2])
+            diff_x[:, self.angle_idx] = fit_angle_in_range(
+                diff_x[:, self.angle_idx]
+            )
         elif len(diff_x.shape) == 1:
-            diff_x[-2] = fit_angle_in_range(diff_x[-2])
+            diff_x[self.angle_idx] = fit_angle_in_range(diff_x[self.angle_idx])
 
         return diff_x
 
@@ -53,7 +56,7 @@ def state_cost_fn(self, x, g_x):
         """
         diff = self.fit_diff_in_range(x - g_x)
         # diff = x - g_x
-        return ((diff) ** 2) * np.diag(self.Q)
+        return (diff ** 2) * np.diag(self.Q)
 
     def terminal_state_cost_fn(self, terminal_x, terminal_g_x):
         """

proj/model/config.py

@@ -7,14 +7,15 @@ class Config:
     SPAWN_TYPE = "trajectory"
 
     # ----------------------------- Simulation params ---------------------------- #
-    dt = 0.01
+    dt = 0.005
 
     # -------------------------------- Cost params ------------------------------- #
     STATE_SIZE = 5
     INPUT_SIZE = 2
+    ANGLE_IDX = 2  # state vector index which is angle, used to fit diff in
 
-    R = np.diag([0.1, 0.1])  # control cost
-    Q = np.diag([5, 5, 1, 5, 0])  # state cost | x, y, theta, v, omega
+    R = np.diag([0.01, 0.01])  # control cost
+    Q = np.diag([1, 1, 1, 5, 0])  # state cost | x, y, theta, v, omega
     Sf = np.diag([0, 0, 0, 0, 0])  # final state cost
 
     # STATE_SIZE = 4
@@ -64,7 +65,7 @@ class Config:
 
     # ------------------------------ Planning params ----------------------------- #
     planning = dict(  # params used to compute goal states to be used for control
-        prediction_length=80,
+        prediction_length=150,
         n_ahead=5,  # start prediction states from N steps ahead
     )
 

proj/run/runner.py

@@ -98,7 +98,7 @@ def run_experiment(
                 environment.itern = itern
                 environment.update_world(g_xs, elapsed=itern * model.dt)
 
-                # log status once a second
+                # log status once a (simulation) second
                 if itern % int(1 / model.dt) == 0:
                     log.info(
                         f"Iteration {itern}/{n_steps}. Current cost: {environment.curr_cost}."