Commit a7240407 authored by li213's avatar li213
Browse files

creating vehicle examples for emsoft2022

parent da29e297
// #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
try:
from circular_curve_hybrid import *
except:
from examples.circular_tracking_hybrid.circular_curve_hybrid import *
\ No newline at end of file
# 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()
try:
from circular_curve_platoon import *
except:
from examples.circular_tracking_platoon.circular_curve_platoon import *
\ No newline at end of file
# 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()
try:
from circular_curve_threelanes import *
except:
from examples.circular_tracking_threelanes.circular_curve_threelanes import *
\ No newline at end of file
# 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