add mpc, still need some adjustment to make it more fault tolerant

Author: Patrick8894

GEMstack/knowledge/defaults/current.yaml

@@ -26,6 +26,11 @@ control:
         softening_gain: 0.2
         desired_speed: trajectory        # Speed reference source: can be "trajectory", "path", or a constant value
 
+    # MPC controller parameters
+    mpc:
+        dt: 0.2                          # Time step for the MPC controller (seconds)
+        horizon: 30                      # Prediction horizon for the MPC controller (number of time steps)
+
     # Shared longitudinal control parameters
     longitudinal_control:
         pid_p: 1.0                       # Proportional gain for speed PID controller

GEMstack/knowledge/routes/backward_15m.csv

@@ -0,0 +1,16 @@
+0.0,0,0
+-1.0,0,0
+-2.0,0,0
+-3.0,0,0
+-4,0,0
+-5,0,0
+-6,0,0
+-7,0,0
+-8,0,0
+-9,0,0
+-10,0,0
+-11,0,0
+-12,0,0
+-13,0,0
+-14,0,0
+-15,0,0

GEMstack/knowledge/routes/backward_curve.csv

@@ -0,0 +1,6 @@
+0.0,0,0
+-1.0,0,0
+-2.0,0.2,0
+-3.0,0.4,0
+-4,0.6,0
+-5,0.8,0

GEMstack/knowledge/routes/forward_curve.csv

@@ -0,0 +1,33 @@
+0.0,0,0
+1.0,0,0
+2.0,0,0
+3.0,0,0
+4.0,0.4,0
+5.0,0.8,0
+6.0,1.2,0
+7.0,1.6,0
+8.0,2.0,0
+9.0,2.4,0
+10.0,2.8,0
+11.0,3.2,0
+12.0,3.6,0
+13.0,4.0,0
+14.0,4.4,0
+15.0,4.8,0
+16.0,5.2,0
+17.0,5.6,0
+18.0,6.0,0
+19.0,6.4,0
+20.0,6.8,0
+21.0,7.2,0
+22.0,7.6,0
+23.0,8.0,0
+24.0,8.4,0
+25.0,8.8,0
+26.0,9.2,0
+27.0,9.6,0
+28.0,10.0,0
+29.0,10.4,0
+30.0,10.8,0
+31.0,11.2,0
+32.0,11.6,0

GEMstack/knowledge/routes/offset.csv

@@ -0,0 +1,31 @@
+0.0,0.5,0
+1.0,0.7,0
+2.0,0.9,0
+3.0,1.1,0
+4.0,1.3,0
+5.0,1.5,0
+6.0,1.7,0
+7.0,1.9,0
+8.0,2.1,0
+9.0,2.3,0
+10.0,2.5,0
+11.0,2.7,0
+12.0,2.9,0
+13.0,3.1,0
+14.0,3.3,0
+15.0,3.5,0
+16.0,3.7,0
+17.0,3.9,0
+18.0,4.1,0
+19.0,4.3,0
+20.0,4.5,0
+21.0,4.7,0
+22.0,4.9,0
+23.0,5.1,0
+24.0,5.3,0
+25.0,5.5,0
+26.0,5.7,0
+27.0,5.9,0
+28.0,6.1,0
+29.0,6.3,0
+30.0,6.5,0