config working px to cm realistic

Author: FedeClaudi

analysis_workspace.py

@@ -71,14 +71,19 @@
     zip(loaded["trajectory"], loaded["history"]),
     total=len(loaded["trajectory"]),
 ):
+    last_idx = hist.trajectory_idx.values[-1]
     metad = compute_trajectory_stats(
-        traj, 1, config["trajectory"], config["planning"], mute=True
+        traj[:last_idx, :],
+        1,
+        config["trajectory"],
+        config["planning"],
+        mute=True,
     )[-1]
+
     traj_dist.append(metad["distance_travelled"])
 
     actual_dist.append(
         np.sum(calc_distance_between_points_in_a_vector_2d(hist.x, hist.y))
-        + config["trajectory"]["dist_th"]
     )
 
 # %%
@@ -92,7 +97,7 @@
     lw=1,
     ec=[0.3, 0.3, 0.3],
 )
-ax.plot([0, 400], [0, 400], lw=2, color=[0.6, 0.6, 0.6], zorder=-1)
+ax.plot([25, 130], [25, 130], lw=2, color=[0.6, 0.6, 0.6], zorder=-1)
 
 ax.set(xlabel="Trajectory length", ylabel="Distance travelled")
 # %%

proj/model/config.py

@@ -5,7 +5,7 @@
     STATE_SIZE=5,
     INPUT_SIZE=2,
     ANGLE_IDX=2,  # state vector index which is angle, used to fit diff in
-    R=np.diag([1.0e-8, 1.0e-8]),  # control cost
+    R=np.diag([1.0e-7, 1.0e-7]),  # 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
 )
@@ -64,7 +64,7 @@ class Config:
     trajectory = dict(  # parameters of the goals trajectory
         name="tracking",
         # ? For artificial trajectories
-        nsteps=1000,
+        nsteps=500,
         distance=150,
         max_speed=100,
         min_speed=80,