working REALISTIC config

Author: FedeClaudi

proj/model/config.py

@@ -5,8 +5,8 @@
     STATE_SIZE=5,
     INPUT_SIZE=2,
     ANGLE_IDX=2,  # state vector index which is angle, used to fit diff in
-    R=np.diag([0.0001, 0.0001]),  # control cost
-    Q=np.diag([1, 1, 1, 1, 0]),  # state cost | x, y, theta, v, omega
+    R=np.diag([1.0e-6, 1.0e-6]),  # control cost
+    Q=np.diag([30, 30, 30, 10, 0]),  # state cost | x, y, theta, v, omega
     Sf=np.diag([0, 0, 0, 0, 0]),  # final state cost
 )
 
@@ -48,13 +48,13 @@
 
 class Config:
     # ----------------------------- Simulation params ---------------------------- #
-    SIMULATION_NAME = "horix"
+    SIMULATION_NAME = "analysis"
 
     USE_FAST = True  # if true use cumba's methods
     SPAWN_TYPE = "trajectory"
     LIVE_PLOT = True
 
-    mouse_type = "easy"
+    mouse_type = "realistic"
     model_type = "cart"
 
     dt = 0.005
@@ -80,7 +80,7 @@ class Config:
 
     # ------------------------------ Planning params ----------------------------- #
     planning = dict(  # params used to compute goal states to be used for control
-        prediction_length=40,
+        prediction_length=80,
         n_ahead=5,  # start prediction states from N steps ahead
     )
 

proj/model/config.py.save.2

@@ -0,0 +1,136 @@
+import numpy as np
+
+# ------------------------------- Model specific params ------------------------------ #
+_cart_params = dict(
+    STATE_SIZE=5,
+    INPUT_SIZE=2,
+    ANGLE_IDX=2,  # state vector index which is angle, used to fit diff in
+    R=np.diag([0.0001, 0.00001]),  # control cost
+    Q=np.diag([30, 30, 30, 1, 0]),  # state cost | x, y, theta, v, omega
+    Sf=np.diag([0, 0, 0, 0, 0]),  # final state cost
+)
+
+_polar_params = dict(
+    STATE_SIZE=4,
+    INPUT_SIZE=2,
+    ANGLE_IDX=2,  # state vector index which is angle, used to fit diff in
+    R=np.diag([0.05, 0.05]),  # control cost
+    Q=np.diag([2.5, 2.5, 0, 0]),  # state cost | r, omega, v, omega
+    Sf=np.diag([2.5, 2.5, 0, 0]),  # final state cost
+)
+
+# -------------------------------- Mouse types ------------------------------- #
+
+_easy_mouse = dict(
+    L=1.5,  # half body width | cm
+    R=1,  # radius of wheels | cm
+    d=0.1,  # distance between axel and CoM | cm
+    length=3,  # cm
+    m=round(20 / 9.81, 2),  # mass | g
+    m_w=round(2 / 9.81, 2),  # mass of wheels/legs |g
+    mouse_type="easy",
+)
+
+_realistic_mouse = dict(
+    L=2,  # half body width | cm
+    R=1.5,  # radius of wheels | cm
+    d=2,  # distance between axel and CoM | cm
+    length=1,  # 8.6,  # cm
+    m=round(23 / 9.81, 2),  # mass | g
+    m_w=0.8,  # mass of wheels/legs |g
+    mouse_type="realistic",
+)
+
+# ---------------------------------------------------------------------------- #
+#                                    CONFIG                                    #
+# ---------------------------------------------------------------------------- #
+
+
+class Config:
+    # ----------------------------- Simulation params ---------------------------- #
+    SIMULATION_NAME = "even_moreR_moreQ"
+
+    USE_FAST = True  # if true use cumba's methods
+    SPAWN_TYPE = "trajectory"
+    LIVE_PLOT = False
+
+    mouse_type = "realistic"
+    model_type = "cart"
+
+    dt = 0.005
+
+    # ------------------------------ Goal trajectory ----------------------------- #
+
+    trajectory = dict(  # parameters of the goals trajectory
+        name="tracking",
+        # ? For artificial trajectories
+        nsteps=1000,
+        distance=150,
+        max_speed=100,
+        min_speed=80,
+        min_dist=0,  # if agent is within this distance from trajectory end the goal is considered achieved
+        # ? for trajectories from data
+        px_to_cm=1,  # 1 / 8,  # convert px values to cm
+        # dist_th=60,  # keep frames only after moved away from start location
+        dist_th=50,
+        resample=True,  # if True when using tracking trajectory resamples it
+        max_deg_interpol=8,  # if using track fit a N degree polynomial to daa to smoothen
+        randomize=True,  # if true when using tracking it pulls a random trial
+    )
+
+    # ------------------------------ Planning params ----------------------------- #
+    planning = dict(  # params used to compute goal states to be used for control
+        prediction_length=80,
+        n_ahead=5,  # start prediction states from N steps ahead
+    )
+
+    # --------------------------------- Plotting --------------------------------- #
+    traj_plot_every = 15
+
+    # ------------------------------ Control params ------------------------------ #
+    iLQR = dict(
+        max_iter=500,
+        init_mu=1.0,
+        mu_min=1e-6,
+        mu_max=1e10,
+        init_delta=2.0,
+        threshold=1e-6,
+    )
+
+    def __init__(self,):
+        # get mouse params
+        self.mouse = (
+            _easy_mouse if self.mouse_type == "easy" else _realistic_mouse
+        )
+
+        # set model params
+        if self.model_type == "cart":
+            params = _cart_params
+        else:
+            params = _polar_params
+
+        self.STATE_SIZE = params["STATE_SIZE"]
+        self.INPUT_SIZE = params["INPUT_SIZE"]
+        self.ANGLE_IDX = params["ANGLE_IDX"]
+        self.R = params["R"]
+        self.Q = params["Q"]
+        self.Sf = params["Sf"]
+
+        # Adjust mouse length for plotting
+        self.mouse["length"] = (
+            self.mouse["length"] * self.trajectory["px_to_cm"]
+        )
+
+    def config_dict(self):
+        return dict(
+            dt=self.dt,
+            STATE_SIZE=self.STATE_SIZE,
+            INPUT_SIZE=self.INPUT_SIZE,
+            R=list(np.diag(self.R).tolist()),
+            Q=list(np.diag(self.Q).tolist()),
+            Sf=list(np.diag(self.Sf).tolist()),
+            mouse=self.mouse,
+            trajectory=self.trajectory,
+            planning=self.planning,
+            iLQR=self.iLQR,
+        )