creating vehicle examples for emsoft2022

circular_controller_threelanes_3cars_explicit2.c

+// #include <stdio.h>
+
+enum modes {Straight,Switch_left,Switch_right,Stop};
+enum lanes {Lane0, Lane1, Lane2};
+
+struct State {
+double tau1;
+double yf1;
+double thetaf1;
+double v1; 
+enum lanes lane1;
+
+double tau2;
+double yf2;
+double thetaf2;
+double v2;
+enum lanes lane2;
+
+double tau3;
+double yf3;
+double thetaf3;
+double v3; 
+enum lanes lane3;
+
+enum modes mode;
+};
+
+typedef struct State State;
+    
+State P(State s) {
+    double tau1 = s.tau1;
+    double yf1 = s.yf1;
+    double thetaf1 = s.thetaf1;
+    double v1 = s.v1;
+    enum lanes lane1 = s.lane1;
+    double tau2 = s.tau2;
+    double yf2 = s.yf2;
+    double thetaf2 = s.thetaf2;
+    double v2 = s.v2;
+    enum lanes lane2 = s.lane2;
+
+    double tau3 = s.tau3;
+    double yf3 = s.yf3;
+    double thetaf3 = s.thetaf3;
+    double v3 = s.v3;
+    enum lanes vehicle_lane = s.lane3;
+    enum modes vehicle_state = s.mode;
+
+    if (vehicle_lane == Lane0){
+        if (vehicle_state==Straight) {
+            if(
+                (30*tau1-30*tau3<20 && 
+                30*tau1-30*tau3>10 &&
+                lane1 == Lane0 &&
+                (30*tau2-30*tau3>40 || tau2!=Lane1 || tau3-tau2>10/30)) ||
+                (30*tau2-30*tau3<20 && 
+                30*tau2-30*tau3>10 &&
+                lane2 == Lane0 &&
+                (30*tau1-30*tau3>40 || tau1!=Lane1 || tau3-tau1>10/30))
+            ){
+                vehicle_state = Switch_right;
+                // Resets
+            }
+            if(
+                (30*tau1-30*tau3<5 && 
+                30*tau1-30*tau3>0 &&
+                lane1 == Lane0) ||
+                (30*tau2-30*tau3<5 && 
+                30*tau2-30*tau3>0 &&
+                lane2 == Lane0)
+            ){
+                vehicle_state = Stop;
+                // Resets 
+                v3 = 0;
+            }
+        }
+        if (vehicle_state==Switch_left){
+
+        }
+        if (vehicle_state==Switch_right){
+            if(yf3<=-2.5){
+                vehicle_state = Straight; 
+                vehicle_lane = Lane1;
+                // Resets
+                yf3 = yf3+3;
+            }
+        }
+        if (vehicle_state==Stop){
+            if(
+                (30*tau1-30*tau3>40 && lane1==0) ||
+                (30*tau2-30*tau3>40 && lane2==0) ||
+                (lane1!=0 && lane2!=0)
+            ){
+                vehicle_state = Straight;
+                // Resets
+                v3 = 4;
+            }
+        }
+    }
+    if (vehicle_lane == Lane1){
+        if (vehicle_state==Straight) {
+            if(
+                (30*tau1-30*tau3<21 && 
+                30*tau1-30*tau3>10 &&
+                lane1 == Lane1 &&
+                (30*tau2-30*tau3>40 || tau2!=Lane0 || tau3-tau2>10/30)) ||
+                (30*tau2-30*tau3<21 && 
+                30*tau2-30*tau3>10 &&
+                lane2 == Lane1 &&
+                (30*tau1-30*tau3>40 || tau1!=Lane0 || tau3-tau1>10/30))
+            ){
+                vehicle_state = Switch_left;
+                // Resets
+            }
+            if(
+                (30*tau1-30*tau3<20 && 
+                30*tau1-30*tau3>10 &&
+                lane1 == Lane1 &&
+                (30*tau2-30*tau3>40 || tau2!=Lane2 || tau3-tau2>10/30)) ||
+                (30*tau2-30*tau3<20 && 
+                30*tau2-30*tau3>10 &&
+                lane2 == Lane1 &&
+                (30*tau1-30*tau3>40 || tau1!=Lane2 || tau3-tau1>10/30))    
+            ){
+                vehicle_state = Switch_right;
+                // Resets
+            }
+            if(
+                (30*tau1-30*tau3<5 && 
+                30*tau1-30*tau3>0 &&
+                lane1 == Lane1) ||
+                (30*tau2-30*tau3<5 && 
+                30*tau2-30*tau3>0 &&
+                lane2 == Lane1)
+            ){
+                vehicle_state = Stop;
+                // Resets 
+                v3 = 0;
+            }
+        }
+        if (vehicle_state==Switch_left){
+            if(yf3>=2.5){
+                vehicle_state = Straight; 
+                vehicle_lane = Lane0;
+                // Resets
+                yf3 = yf3-3;
+            }
+        }
+        if (vehicle_state==Switch_right){
+            if(yf3<=-2.5){
+                vehicle_state = Straight;
+                vehicle_lane = Lane2;
+                // Resets
+                yf3 = yf3+3;
+            }
+        }
+        if (vehicle_state==Stop){
+            if(
+                (30*tau1-30*tau3>40 && lane1==1) ||
+                (30*tau2-30*tau3>40 && lane2==1) ||
+                (lane1!=1 && lane2!=1)
+            ){
+                vehicle_state = Straight;
+                // Resets
+                v3 = 4;
+            }
+        }
+    }   
+    if (vehicle_lane == Lane2){
+        if (vehicle_state==Straight) {
+            if(
+                (30*tau1-30*tau3<20 && 
+                30*tau1-30*tau3>10 &&
+                lane1 == Lane2 &&
+                (30*tau2-30*tau3>40 || tau2!=Lane1 || tau3-tau2>10/30)) ||
+                (30*tau2-30*tau3<20 && 
+                30*tau2-30*tau3>10 &&
+                lane2 == Lane2 &&
+                (30*tau1-30*tau3>40 || tau1!=Lane1 || tau3-tau1>10/30))    
+            ){
+                vehicle_state = Switch_left;
+                // Resets
+            }                
+            if(
+                (30*tau1-30*tau3<5 && 
+                30*tau1-30*tau3>0 &&
+                lane1 == Lane2) ||
+                (30*tau2-30*tau3<5 && 
+                30*tau2-30*tau3>0 &&
+                lane2 == Lane2)
+            ){
+                vehicle_state = Stop;
+                // Resets 
+                v3 = 0;
+            }
+        }
+        if (vehicle_state==Switch_left){
+            if(yf3>=2.5){
+                vehicle_state = Straight;
+                vehicle_lane = Lane1;
+                // Resets
+                yf3 = yf3-3;
+            }
+        }
+        if (vehicle_state==Switch_right){
+            //Resets
+        }
+        if (vehicle_state==Stop){
+            if(
+                (30*tau1-30*tau3>40 && lane1==2) ||
+                (30*tau2-30*tau3>40 && lane2==2) ||
+                (lane1!=2 && lane2!=2)
+            ){
+                vehicle_state = Straight;
+                // Resets
+                v3 = 4;
+            }
+        }
+    } 
+    s.mode = vehicle_state;
+    s.lane3 = vehicle_lane;
+    s.yf3 = yf3;
+    return s;
+}
+
+// int main(){
+//     State s;
+//     s = P(s);
+//     printf("Hello, World!\n");
+//     return 0;
+// }
\ No newline at end of file

examples/circular_tracking_hybrid/__init__.py

+try:
+    from circular_curve_hybrid import *
+except:
+    from examples.circular_tracking_hybrid.circular_curve_hybrid import *
\ No newline at end of file

examples/circular_tracking_hybrid/circular_curve_hybrid.py

+# from socket import TIPC_SUBSCR_TIMEOUT
+import numpy as np
+import matplotlib.pyplot as plt
+# from pyrsistent import T
+from scipy.integrate import ode
+from numpy.polynomial import Polynomial
+from scipy.optimize import minimize_scalar
+
+
+WHEEL_BASE = 1.75  # meter
+# K_ST = 1.5  # Proportional Gain for Stanley
+K_ST = 0.45
+SPEED = 2.8  # meter/second
+STEER_LIM = 0.61  # radian
+
+#################################
+# Vehicle following circular curve
+#################################
+# R = 15
+def CURVE_X(tau):
+    return R*np.sin(tau)
+
+
+def CURVE_Y(tau):
+    return R*(-np.cos(tau)+1)
+
+
+def CURVE_X_DER(tau):
+    return R*np.cos(tau)
+
+
+def CURVE_Y_DER(tau):
+    return R*np.sin(tau)
+
+def error_dynamic_circular(t, state, u):
+    tau, yf, thetaf = state
+    steering = u
+
+    tau_dot = SPEED*np.cos(thetaf+steering)/(-1*yf +
+                                             np.linalg.norm([CURVE_X_DER(tau), CURVE_Y_DER(tau)]))
+    yf_dot = SPEED*np.sin(thetaf+steering)
+    thetaf_dot = SPEED*np.sin(steering)/WHEEL_BASE-tau_dot
+
+    return [tau_dot, yf_dot, thetaf_dot]
+
+def R_circular(tau):
+    return np.array([[R*np.cos(tau), -R*np.sin(tau)], [R*np.sin(tau), R*np.cos(tau)]])
+
+def TC_Simulate(Mode, initialCondition, time_bound):
+    print(initialCondition, Mode)
+    time_step = 0.01
+    time_bound = float(time_bound)
+    number_points = int(np.ceil(time_bound/time_step))
+    t = [i*time_step for i in range(0, number_points)]
+
+    init = initialCondition
+    trace = [[0]+init]
+    if Mode == "Normal":
+        for j in range(len(t)):
+            tau, yf, thetaf = init
+            d = -yf
+            psi = -thetaf
+            steering = psi + np.arctan2(K_ST*d, SPEED)
+
+            r = ode(error_dynamic_circular)
+            r.set_initial_value(init).set_f_params(steering)
+            res = r.integrate(r.t+time_step)
+            init = [res[0], res[1], res[2]]
+            trace.append([t[j]+time_step]+init)
+    elif Mode == "Sat_low":
+        for j in range(min(500,len(t))):
+            tau, yf, thetaf = init 
+            d = -yf 
+            psi = -thetaf 
+            steering = -STEER_LIM
+            r = ode(error_dynamic_circular)
+            r.set_initial_value(init).set_f_params(steering)
+            res = r.integrate(r.t+time_step)
+            init = [res[0], res[1], res[2]]
+            trace.append([t[j]+time_step]+init)
+            # print(j, init)
+            if init[1]>5 or init[1]<-5:
+                break
+    elif Mode == "Sat_high":
+        for j in range(min(500,len(t))):
+            tau, yf, thetaf = init 
+            d = -yf 
+            psi = -thetaf 
+            steering = STEER_LIM
+            r = ode(error_dynamic_circular)
+            r.set_initial_value(init).set_f_params(steering)
+            res = r.integrate(r.t+time_step)
+            init = [res[0], res[1], res[2]]
+            trace.append([t[j]+time_step]+init)
+            if init[1]>5 or init[1]<-5:
+                break
+
+    return np.array(trace)
+
+
+def world_to_bishop_poly(state, curve_y, curve_y_der):
+    # v_diff = state[0:2] - np.array(ARC_CENTER)
+    # y_err = LANE_ARC_RADIUS - np.linalg.norm(v_diff, ord=2)
+    # yaw_err = state[2] - (np.arctan2(v_diff[1], v_diff[0]) + np.pi/2)
+    x, y, yaw = state
+    dist_sq = Polynomial([-x, 1])**2 + (curve_y - y)**2
+    min_res = minimize_scalar(dist_sq, bounds=(x-10, x+10), method='bounded')
+    x_min = min_res.x.squeeze().item()
+    y_min = curve_y(x_min).squeeze()
+    tan_vec = np.array([1.0, curve_y_der(x_min)])
+    v_diff = np.array([x - x_min, y - y_min], dtype=float)
+    assert np.isclose(tan_vec @ v_diff, 0.0, atol=10**-
+                      4), str(tan_vec @ v_diff)
+    sgn = np.sign(tan_vec[0] * v_diff[1] - tan_vec[1]*v_diff[0])
+    y_err = sgn * np.linalg.norm(v_diff)
+    yaw_err = yaw - (np.arctan2(tan_vec[1], tan_vec[0]))
+    return 0.0, y_err, yaw_err, x_min, y_min
+
+def bishop_to_world_circular(traj, Rot, radius=15):
+    
+    # Convert x-y-theta trajectory from bishop frame to world frame
+    # Input:
+    #   traj: Nx3 ndarray. The first column holds tau values, second column holds yf values and third column holding theta value
+    #   R: The 2x2 rotation matrix function obtained by T, N1, N2
+    # Output:
+    #   Nx3 ndarray. The first and second column holds x and y values and third column holds theta value
+    global R
+    R = radius
+    tau = traj[:, 0]
+    yf = traj[:, 1]
+    theta = traj[:, 2]
+
+    res = []
+    for i in range(traj.shape[0]):
+        tmp = (Rot(tau[i])@np.array([0, yf[i]]) +
+               np.array([CURVE_X(tau[i]), CURVE_Y(tau[i])])).flatten()
+        res.append(tmp)
+
+    res = np.array(res)
+    res = np.concatenate((res, theta[:, np.newaxis]), axis=1)
+    return res
+
+if __name__ == "__main__":
+    mode = "Sat_low"
+    for i in range(10):
+        y = np.random.uniform(-0.5,0.5)
+        res = TC_Simulate(mode, [3.086960894574077, -0.02147590301783911, 0.656805926619499], 20)
+
+        # state = bishop_to_world_poly(
+        #     np.array([[0, y, 0]]), R_poly).flatten().tolist()
+        # res2 = TC_simulate("poly_curve", state, 20)
+        # print(res)
+        # plt.plot(res[:,1], res[:,2])
+        # res_ina = bishop_to_world_poly(res[:, 1:], R_poly)
+        plt.plot(res[:, 1], res[:, 2], 'r')
+        # plt.plot(res2[:,1], res2[:,2], 'g')
+        # traj = apply_traj_poly(res[:, 1:])
+        # plt.plot(traj[:, 0], traj[:, 1], 'b')
+    ax = plt.gca()
+    ax.tick_params(axis='both', which='major', labelsize=28)
+    plt.xlabel('x', fontsize=30)
+    plt.ylabel('y', fontsize=30)
+    plt.show()

examples/circular_tracking_platoon/__init__.py

+try:
+    from circular_curve_platoon import *
+except:
+    from examples.circular_tracking_platoon.circular_curve_platoon import *
\ No newline at end of file

examples/circular_tracking_platoon/circular_curve_platoon.py

+# from socket import TIPC_SUBSCR_TIMEOUT
+from re import A
+import numpy as np
+import matplotlib.pyplot as plt
+# from pyrsistent import T
+from scipy.integrate import ode
+from numpy.polynomial import Polynomial
+from scipy.optimize import minimize_scalar
+
+
+WHEEL_BASE = 1.75  # meter
+# K_ST = 1.5  # Proportional Gain for Stanley
+K_ST = 0.45
+SPEED = 2.8  # meter/second
+STEER_LIM = 0.61  # radian
+
+#################################
+# Vehicle following circular curve
+#################################
+R = 15
+def CURVE_X(tau):
+    return R*np.sin(tau)
+
+def CURVE_Y(tau):
+    return R*(-np.cos(tau)+1)
+
+def CURVE_X_DER(tau):
+    return R*np.cos(tau)
+
+def CURVE_Y_DER(tau):
+    return R*np.sin(tau)
+
+def error_dynamic_circular(t, state, u):
+    tau, yf, thetaf, v, a = state
+    steering = u
+
+    tau_dot = v*np.cos(thetaf+steering)/(-1*yf +
+                                             np.linalg.norm([CURVE_X_DER(tau), CURVE_Y_DER(tau)]))
+    yf_dot = v*np.sin(thetaf+steering)
+    thetaf_dot = v*np.sin(steering)/WHEEL_BASE-tau_dot
+    v_dot = a 
+    a_dot = 0
+
+    return [tau_dot, yf_dot, thetaf_dot, v_dot, a_dot]
+
+def error_dynamic_platoon(t, state, u):
+    tau1, yf1, thetaf1, v1, a1, tau2, yf2, thetaf2, v2, a2 = state 
+    steering1, steering2 = u 
+    tau1_dot, yf1_dot, thetaf1_dot, v1_dot, a1_dot = error_dynamic_circular(t, [tau1, yf1, thetaf1, v1, a1], steering1)
+    tau2_dot, yf2_dot, thetaf2_dot, v2_dot, a2_dot = error_dynamic_circular(t, [tau2, yf2, thetaf2, v2, a2], steering2)
+
+    return [
+        tau1_dot, yf1_dot, thetaf1_dot, v1_dot, a1_dot, 
+        tau2_dot, yf2_dot, thetaf2_dot, v2_dot, a2_dot
+    ]
+
+def R_circular(tau):
+    return np.array([[R*np.cos(tau), -R*np.sin(tau)], [R*np.sin(tau), R*np.cos(tau)]])
+
+
+def TC_Simulate(Mode, initialCondition, time_bound):
+    print(initialCondition, Mode)
+    time_step = 0.01
+    time_bound = float(time_bound)
+    number_points = int(np.ceil(time_bound/time_step))
+    t = [i*time_step for i in range(0, number_points)]
+
+    init = initialCondition
+    trace = [[0]+init]
+    for j in range(len(t)):
+        tau1, yf1, thetaf1, v1, a1, tau2, yf2, thetaf2, v2, a2 = init
+        d1 = -yf1
+        psi1 = -thetaf1
+        steering1 = psi1 + np.arctan2(K_ST*d1, v1)
+        steering1 = np.clip(steering1, -STEER_LIM, STEER_LIM)
+        d2 = -yf2
+        psi2 = -thetaf2
+        steering2 = psi2 + np.arctan2(K_ST*d2, v2)
+        steering2 = np.clip(steering2, -STEER_LIM, STEER_LIM)
+        
+        r = ode(error_dynamic_platoon)
+        r.set_initial_value(init).set_f_params([steering1, steering2])
+        res = r.integrate(r.t+time_step)
+        init = [res[0], res[1], res[2], res[3], res[4], res[5], res[6], res[7], res[8], res[9]]
+        trace.append([t[j]+time_step]+init)
+
+    return np.array(trace)
+
+def world_to_bishop_poly(state, curve_y, curve_y_der):
+    x, y, yaw = state
+    dist_sq = Polynomial([-x, 1])**2 + (curve_y - y)**2
+    min_res = minimize_scalar(dist_sq, bounds=(x-10, x+10), method='bounded')
+    x_min = min_res.x.squeeze().item()
+    y_min = curve_y(x_min).squeeze()
+    tan_vec = np.array([1.0, curve_y_der(x_min)])
+    v_diff = np.array([x - x_min, y - y_min], dtype=float)
+    assert np.isclose(tan_vec @ v_diff, 0.0, atol=10**-
+                      4), str(tan_vec @ v_diff)
+    sgn = np.sign(tan_vec[0] * v_diff[1] - tan_vec[1]*v_diff[0])
+    y_err = sgn * np.linalg.norm(v_diff)
+    yaw_err = yaw - (np.arctan2(tan_vec[1], tan_vec[0]))
+    return 0.0, y_err, yaw_err, x_min, y_min
+
+def bishop_to_world_circular(traj, R):
+    # Convert x-y-theta trajectory from bishop frame to world frame
+    # Input:
+    #   traj: Nx3 ndarray. The first column holds tau values, second column holds yf values and third column holding theta value
+    #   R: The 2x2 rotation matrix function obtained by T, N1, N2
+    # Output:
+    #   Nx3 ndarray. The first and second column holds x and y values and third column holds theta value
+    tau = traj[:, 0]
+    yf = traj[:, 1]
+    theta = traj[:, 2]
+
+    res = []
+    for i in range(traj.shape[0]):
+        tmp = (R(tau[i])@np.array([0, yf[i]]) +
+               np.array([CURVE_X(tau[i]), CURVE_Y(tau[i])])).flatten()
+        res.append(tmp)
+
+    res = np.array(res)
+    res = np.concatenate((res, theta[:, np.newaxis]), axis=1)
+    return res
+
+if __name__ == "__main__":
+    mode = "Sat_low"
+    # for i in range(10):
+    y = np.random.uniform(-0.5,0.5)
+    res = TC_Simulate(mode, [0.5, 0, 0, SPEED, 0, 0,0,0,SPEED,0], 10)
+
+    # state = bishop_to_world_poly(
+    #     np.array([[0, y, 0]]), R_poly).flatten().tolist()
+    # res2 = TC_simulate("poly_curve", state, 20)
+    # print(res)
+    # plt.plot(res[:,1], res[:,2])
+    res_ina1 = bishop_to_world_circular(res[:, 1:4], R_circular)
+    res_ina2 = bishop_to_world_circular(res[:, 6:9], R_circular)
+    # plt.plot(res[:, 0], res[:, 2], 'r')
+    # plt.plot(res[:, 0], res[:, 7], 'b')
+    plt.plot(res_ina1[:,0], res_ina1[:,1], 'r')
+    plt.plot(res_ina2[:,0], res_ina2[:,1], 'b')
+
+    # traj = apply_traj_poly(res[:, 1:])
+    # plt.plot(traj[:, 0], traj[:, 1], 'b')
+    ax = plt.gca()
+    ax.tick_params(axis='both', which='major', labelsize=28)
+    plt.xlabel('x', fontsize=30)
+    plt.ylabel('y', fontsize=30)
+    plt.show()

examples/circular_tracking_threelanes/__init__.py

+try:
+    from circular_curve_threelanes import *
+except:
+    from examples.circular_tracking_threelanes.circular_curve_threelanes import *
\ No newline at end of file

examples/circular_tracking_threelanes/circular_curve_threelanes.py

+# from socket import TIPC_SUBSCR_TIMEOUT
+from re import A
+import numpy as np
+import matplotlib.pyplot as plt
+# from pyrsistent import T
+from scipy.integrate import ode
+from numpy.polynomial import Polynomial
+from scipy.optimize import minimize_scalar
+