fix nosim demo not updating state correctly
mcarfagno
parent
a2eb20e7be
commit
89e5796fbc
|
|
@ -3,6 +3,7 @@
|
|||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib import animation
|
||||
from scipy.integrate import odeint
|
||||
|
||||
from mpcpy.utils import compute_path_from_wp
|
||||
import mpcpy
|
||||
|
|
@ -11,154 +12,195 @@ import sys
|
|||
import time
|
||||
|
||||
# Robot Starting position
|
||||
SIM_START_X=0.
|
||||
SIM_START_Y=0.5
|
||||
SIM_START_V=0.
|
||||
SIM_START_H=0.
|
||||
L=0.3
|
||||
SIM_START_X = 0.0
|
||||
SIM_START_Y = 0.5
|
||||
SIM_START_V = 0.0
|
||||
SIM_START_H = 0.0
|
||||
L = 0.3
|
||||
|
||||
P=mpcpy.Params()
|
||||
P = mpcpy.Params()
|
||||
|
||||
# Params
|
||||
VEL = 1.0 # m/s
|
||||
|
||||
# Classes
|
||||
class MPCSim():
|
||||
|
||||
class MPCSim:
|
||||
def __init__(self):
|
||||
|
||||
# State for the robot mathematical model [x,y,heading]
|
||||
self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
|
||||
self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
|
||||
|
||||
#starting guess
|
||||
# starting guess
|
||||
self.action = np.zeros(P.M)
|
||||
self.action[0] = P.MAX_ACC/2 #a
|
||||
self.action[1] = 0.0 #delta
|
||||
self.action[0] = P.MAX_ACC / 2 # a
|
||||
self.action[1] = 0.0 # delta
|
||||
|
||||
self.opt_u = np.zeros((P.M,P.T))
|
||||
self.opt_u = np.zeros((P.M, P.T))
|
||||
|
||||
# Cost Matrices
|
||||
Q = np.diag([20,20,10,20]) #state error cost
|
||||
Qf = np.diag([30,30,30,30]) #state final error cost
|
||||
R = np.diag([10,10]) #input cost
|
||||
R_ = np.diag([10,10]) #input rate of change cost
|
||||
Q = np.diag([20, 20, 10, 20]) # state error cost
|
||||
Qf = np.diag([30, 30, 30, 30]) # state final error cost
|
||||
R = np.diag([10, 10]) # input cost
|
||||
R_ = np.diag([10, 10]) # input rate of change cost
|
||||
|
||||
self.mpc = mpcpy.MPC(P.N,P.M,Q,R)
|
||||
self.mpc = mpcpy.MPC(P.N, P.M, Q, R)
|
||||
|
||||
# Interpolated Path to follow given waypoints
|
||||
self.path = compute_path_from_wp([0,3,4,6,10,12,13,13,6,1,0],
|
||||
[0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick)
|
||||
self.path = compute_path_from_wp(
|
||||
[0, 3, 4, 6, 10, 12, 13, 13, 6, 1, 0],
|
||||
[0, 0, 2, 4, 3, 3, -1, -2, -6, -2, -2],
|
||||
P.path_tick,
|
||||
)
|
||||
|
||||
# Sim help vars
|
||||
self.sim_time=0
|
||||
self.x_history=[]
|
||||
self.y_history=[]
|
||||
self.v_history=[]
|
||||
self.h_history=[]
|
||||
self.a_history=[]
|
||||
self.d_history=[]
|
||||
self.predicted=None
|
||||
self.sim_time = 0
|
||||
self.x_history = []
|
||||
self.y_history = []
|
||||
self.v_history = []
|
||||
self.h_history = []
|
||||
self.a_history = []
|
||||
self.d_history = []
|
||||
self.predicted = None
|
||||
|
||||
#Initialise plot
|
||||
# Initialise plot
|
||||
plt.style.use("ggplot")
|
||||
self.fig = plt.figure()
|
||||
plt.ion()
|
||||
plt.show()
|
||||
|
||||
def predict_motion(self):
|
||||
'''
|
||||
'''
|
||||
predicted=np.zeros(self.opt_u.shape)
|
||||
x=self.state[0]
|
||||
y=self.state[1]
|
||||
v=self.state[2]
|
||||
th=self.state[3]
|
||||
|
||||
for idx,a,delta in zip(range(len(self.opt_u[0,:])),self.opt_u[0,:],self.opt_u[1,:]):
|
||||
x = x+v*np.cos(th)*P.dt
|
||||
y = y+v*np.sin(th)*P.dt
|
||||
v = v+a*P.dt
|
||||
th = th + v*np.tan(delta)/L*P.dt
|
||||
|
||||
predicted[0,idx]=x
|
||||
predicted[1,idx]=y
|
||||
def preview(self, mpc_out):
|
||||
"""
|
||||
[TODO:summary]
|
||||
|
||||
[TODO:description]
|
||||
"""
|
||||
predicted = np.zeros(self.opt_u.shape)
|
||||
predicted[:, :] = mpc_out[0:2, 1:]
|
||||
Rotm = np.array(
|
||||
[
|
||||
[np.cos(self.state[3]), np.sin(self.state[3])],
|
||||
[-np.sin(self.state[3]), np.cos(self.state[3])],
|
||||
]
|
||||
)
|
||||
predicted = (predicted.T.dot(Rotm)).T
|
||||
predicted[0, :] += self.state[0]
|
||||
predicted[1, :] += self.state[1]
|
||||
self.predicted = predicted
|
||||
|
||||
def run(self):
|
||||
'''
|
||||
'''
|
||||
"""
|
||||
[TODO:summary]
|
||||
|
||||
[TODO:description]
|
||||
"""
|
||||
self.plot_sim()
|
||||
input("Press Enter to continue...")
|
||||
while 1:
|
||||
|
||||
if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<0.1:
|
||||
if (
|
||||
np.sqrt(
|
||||
(self.state[0] - self.path[0, -1]) ** 2
|
||||
+ (self.state[1] - self.path[1, -1]) ** 2
|
||||
)
|
||||
< 0.5
|
||||
):
|
||||
print("Success! Goal Reached")
|
||||
input("Press Enter to continue...")
|
||||
return
|
||||
|
||||
#optimization loop
|
||||
#start=time.time()
|
||||
|
||||
# optimization loop
|
||||
# start=time.time()
|
||||
# dynamycs w.r.t robot frame
|
||||
curr_state = np.array(
|
||||
[0, 0, self.state[2], 0]
|
||||
)
|
||||
# State Matrices
|
||||
A,B,C = mpcpy.get_linear_model_matrices(self.state, self.action)
|
||||
|
||||
#TODO: check why taget does not update?
|
||||
|
||||
#Get Reference_traj -> inputs are in worldframe
|
||||
target, _ = mpcpy.get_ref_trajectory(self.state,
|
||||
self.path, 1.0)
|
||||
|
||||
x_mpc, u_mpc = self.mpc.optimize_linearized_model(A, B, C, self.state, target, time_horizon=P.T, verbose=False)
|
||||
|
||||
self.opt_u = np.vstack((np.array(u_mpc.value[0,:]).flatten(),
|
||||
(np.array(u_mpc.value[1,:]).flatten())))
|
||||
|
||||
self.action[:] = [u_mpc.value[0,1],u_mpc.value[1,1]]
|
||||
A, B, C = mpcpy.get_linear_model_matrices(curr_state, self.action)
|
||||
# Get Reference_traj -> inputs are in worldframe
|
||||
target, _ = mpcpy.get_ref_trajectory(
|
||||
self.state, self.path, VEL, dl=P.path_tick
|
||||
)
|
||||
|
||||
x_mpc, u_mpc = self.mpc.optimize_linearized_model(
|
||||
A,
|
||||
B,
|
||||
C,
|
||||
curr_state,
|
||||
target,
|
||||
time_horizon=P.T,
|
||||
verbose=False,
|
||||
)
|
||||
self.opt_u = np.vstack(
|
||||
(
|
||||
np.array(u_mpc.value[0, :]).flatten(),
|
||||
(np.array(u_mpc.value[1, :]).flatten()),
|
||||
)
|
||||
)
|
||||
self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
|
||||
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
|
||||
|
||||
self.update_sim(self.action[0],self.action[1])
|
||||
self.predict_motion()
|
||||
self.predict([self.action[0], self.action[1]])
|
||||
self.preview(x_mpc.value)
|
||||
self.plot_sim()
|
||||
def predict(self, u):
|
||||
def kinematics_model(x, t, u):
|
||||
dxdt = x[2] * np.cos(x[3])
|
||||
dydt = x[2] * np.sin(x[3])
|
||||
dvdt = u[0]
|
||||
dtheta0dt = x[2] * np.tan(u[1]) / P.L
|
||||
dqdt = [dxdt, dydt, dvdt, dtheta0dt]
|
||||
return dqdt
|
||||
|
||||
def update_sim(self,acc,steer):
|
||||
'''
|
||||
'''
|
||||
self.state[0] = self.state[0] + self.state[2]*np.cos(self.state[3])*P.dt
|
||||
self.state[1] = self.state[1] + self.state[2]*np.sin(self.state[3])*P.dt
|
||||
self.state[2] = self.state[2] + acc*P.dt
|
||||
self.state[3] = self.state[3] + self.state[2]*np.tan(steer)/L*P.dt
|
||||
# solve ODE
|
||||
tspan = [0, P.DT]
|
||||
self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
|
||||
|
||||
def plot_sim(self):
|
||||
'''
|
||||
'''
|
||||
self.sim_time = self.sim_time+P.dt
|
||||
"""
|
||||
[TODO:summary]
|
||||
|
||||
[TODO:description]
|
||||
"""
|
||||
self.sim_time = self.sim_time + P.DT
|
||||
self.x_history.append(self.state[0])
|
||||
self.y_history.append(self.state[1])
|
||||
self.v_history.append(self.state[2])
|
||||
self.h_history.append(self.state[3])
|
||||
self.a_history.append(self.opt_u[0,1])
|
||||
self.d_history.append(self.opt_u[1,1])
|
||||
self.a_history.append(self.opt_u[0, 1])
|
||||
self.d_history.append(self.opt_u[1, 1])
|
||||
|
||||
plt.clf()
|
||||
|
||||
grid = plt.GridSpec(2, 3)
|
||||
|
||||
plt.subplot(grid[0:2, 0:2])
|
||||
plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time))
|
||||
plt.title(
|
||||
"MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time)
|
||||
)
|
||||
|
||||
plt.plot(self.path[0,:],self.path[1,:], c='tab:orange',
|
||||
marker=".",
|
||||
label="reference track")
|
||||
plt.plot(
|
||||
self.path[0, :],
|
||||
self.path[1, :],
|
||||
c="tab:orange",
|
||||
marker=".",
|
||||
label="reference track",
|
||||
)
|
||||
|
||||
plt.plot(self.x_history, self.y_history, c='tab:blue',
|
||||
marker=".",
|
||||
alpha=0.5,
|
||||
label="vehicle trajectory")
|
||||
plt.plot(
|
||||
self.x_history,
|
||||
self.y_history,
|
||||
c="tab:blue",
|
||||
marker=".",
|
||||
alpha=0.5,
|
||||
label="vehicle trajectory",
|
||||
)
|
||||
|
||||
if self.predicted is not None:
|
||||
plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green',
|
||||
marker="+",
|
||||
alpha=0.5,
|
||||
label="mpc opt trajectory")
|
||||
plt.plot(
|
||||
self.predicted[0, :],
|
||||
self.predicted[1, :],
|
||||
c="tab:green",
|
||||
marker="+",
|
||||
alpha=0.5,
|
||||
label="mpc opt trajectory",
|
||||
)
|
||||
|
||||
# plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue',
|
||||
# marker=".",
|
||||
|
|
@ -175,28 +217,32 @@ class MPCSim():
|
|||
|
||||
plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1])
|
||||
|
||||
plt.ylabel('map y')
|
||||
plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0))
|
||||
plt.xlabel('map x')
|
||||
plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0))
|
||||
plt.ylabel("map y")
|
||||
plt.yticks(
|
||||
np.arange(min(self.path[1, :]) - 1.0, max(self.path[1, :] + 1.0) + 1, 1.0)
|
||||
)
|
||||
plt.xlabel("map x")
|
||||
plt.xticks(
|
||||
np.arange(min(self.path[0, :]) - 1.0, max(self.path[0, :] + 1.0) + 1, 1.0)
|
||||
)
|
||||
plt.axis("equal")
|
||||
#plt.legend()
|
||||
# plt.legend()
|
||||
|
||||
plt.subplot(grid[0, 2])
|
||||
#plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
|
||||
plt.plot(self.a_history,c='tab:orange')
|
||||
# plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
|
||||
plt.plot(self.a_history, c="tab:orange")
|
||||
locs, _ = plt.xticks()
|
||||
plt.xticks(locs[1:], locs[1:]*P.dt)
|
||||
plt.ylabel('a(t) [m/ss]')
|
||||
plt.xlabel('t [s]')
|
||||
plt.xticks(locs[1:], locs[1:] * P.DT)
|
||||
plt.ylabel("a(t) [m/ss]")
|
||||
plt.xlabel("t [s]")
|
||||
|
||||
plt.subplot(grid[1, 2])
|
||||
#plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
|
||||
plt.plot(np.degrees(self.d_history),c='tab:orange')
|
||||
plt.ylabel('gamma(t) [deg]')
|
||||
# plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
|
||||
plt.plot(np.degrees(self.d_history), c="tab:orange")
|
||||
plt.ylabel("gamma(t) [deg]")
|
||||
locs, _ = plt.xticks()
|
||||
plt.xticks(locs[1:], locs[1:]*P.dt)
|
||||
plt.xlabel('t [s]')
|
||||
plt.xticks(locs[1:], locs[1:] * P.DT)
|
||||
plt.xlabel("t [s]")
|
||||
|
||||
plt.tight_layout()
|
||||
|
||||
|
|
@ -205,23 +251,41 @@ class MPCSim():
|
|||
|
||||
|
||||
def plot_car(x, y, yaw):
|
||||
"""
|
||||
[TODO:summary]
|
||||
|
||||
[TODO:description]
|
||||
|
||||
Parameters
|
||||
----------
|
||||
x : [TODO:type]
|
||||
[TODO:description]
|
||||
y : [TODO:type]
|
||||
[TODO:description]
|
||||
yaw : [TODO:type]
|
||||
[TODO:description]
|
||||
"""
|
||||
LENGTH = 0.5 # [m]
|
||||
WIDTH = 0.25 # [m]
|
||||
OFFSET = LENGTH # [m]
|
||||
|
||||
outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],
|
||||
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
|
||||
outline = np.array(
|
||||
[
|
||||
[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],
|
||||
[WIDTH / 2, WIDTH / 2, -WIDTH / 2, -WIDTH / 2, WIDTH / 2],
|
||||
]
|
||||
)
|
||||
|
||||
Rotm = np.array([[np.cos(yaw), np.sin(yaw)],
|
||||
[-np.sin(yaw), np.cos(yaw)]])
|
||||
Rotm = np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]])
|
||||
|
||||
outline = (outline.T.dot(Rotm)).T
|
||||
|
||||
outline[0, :] += x
|
||||
outline[1, :] += y
|
||||
|
||||
plt.plot(np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), 'tab:blue')
|
||||
|
||||
plt.plot(
|
||||
np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), "tab:blue"
|
||||
)
|
||||
|
||||
|
||||
def do_sim():
|
||||
|
|
@ -231,5 +295,6 @@ def do_sim():
|
|||
except Exception as e:
|
||||
sys.exit(e)
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
if __name__ == "__main__":
|
||||
do_sim()
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ def set_ctrl(robotId,currVel,acceleration,steeringAngle):
|
|||
wheels = [8,15]
|
||||
maxForce = 50
|
||||
|
||||
targetVelocity = currVel + acceleration*P.dt
|
||||
targetVelocity = currVel + acceleration*P.DT
|
||||
#targetVelocity=lastVel
|
||||
#print(targetVelocity)
|
||||
|
||||
|
|
@ -158,10 +158,10 @@ def run_sim():
|
|||
state[3] = 0.0
|
||||
|
||||
#add 1 timestep delay to input
|
||||
state[0]=state[0]+state[2]*np.cos(state[3])*P.dt
|
||||
state[1]=state[1]+state[2]*np.sin(state[3])*P.dt
|
||||
state[2]=state[2]+action[0]*P.dt
|
||||
state[3]=state[3]+action[0]*np.tan(action[1])/P.L*P.dt
|
||||
state[0]=state[0]+state[2]*np.cos(state[3])*P.DT
|
||||
state[1]=state[1]+state[2]*np.sin(state[3])*P.DT
|
||||
state[2]=state[2]+action[0]*P.DT
|
||||
state[3]=state[3]+action[0]*np.tan(action[1])/P.L*P.DT
|
||||
|
||||
|
||||
#optimization loop
|
||||
|
|
@ -186,8 +186,8 @@ def run_sim():
|
|||
|
||||
set_ctrl(car,state[2],action[0],action[1])
|
||||
|
||||
if P.dt-elapsed>0:
|
||||
time.sleep(P.dt-elapsed)
|
||||
if P.DT-elapsed>0:
|
||||
time.sleep(P.DT-elapsed)
|
||||
|
||||
if __name__ == '__main__':
|
||||
run_sim()
|
||||
|
|
|
|||
|
|
@ -34,15 +34,15 @@ def get_linear_model_matrices(x_bar,u_bar):
|
|||
A[1,2] = st
|
||||
A[1,3] = v*ct
|
||||
A[3,2] = v*td/P.L
|
||||
A_lin = np.eye(P.N)+P.dt*A
|
||||
A_lin = np.eye(P.N)+P.DT*A
|
||||
|
||||
B = np.zeros((P.N,P.M))
|
||||
B[2,0]=1
|
||||
B[3,1]=v/(P.L*cd**2)
|
||||
B_lin=P.dt*B
|
||||
B_lin=P.DT*B
|
||||
|
||||
f_xu=np.array([v*ct, v*st, a, v*td/P.L]).reshape(P.N,1)
|
||||
C_lin = P.dt*(f_xu - np.dot(A, x_bar.reshape(P.N,1)) - np.dot(B, u_bar.reshape(P.M,1))).flatten()
|
||||
C_lin = P.DT*(f_xu - np.dot(A, x_bar.reshape(P.N,1)) - np.dot(B, u_bar.reshape(P.M,1))).flatten()
|
||||
|
||||
#return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)
|
||||
return A_lin, B_lin, C_lin
|
||||
|
|
@ -98,8 +98,8 @@ class MPC():
|
|||
# Actuation rate of change
|
||||
if t < (time_horizon - 1):
|
||||
_cost += opt.quad_form(u[:,t + 1] - u[:,t], R * 1)
|
||||
_constraints += [opt.abs(u[0, t + 1] - u[0, t])/P.dt <= P.MAX_D_ACC]
|
||||
_constraints += [opt.abs(u[1, t + 1] - u[1, t])/P.dt <= P.MAX_D_STEER]
|
||||
_constraints += [opt.abs(u[0, t + 1] - u[0, t])/P.DT <= P.MAX_D_ACC]
|
||||
_constraints += [opt.abs(u[1, t + 1] - u[1, t])/P.DT <= P.MAX_D_STEER]
|
||||
|
||||
|
||||
if t == 0:
|
||||
|
|
|
|||
|
|
@ -1,15 +1,16 @@
|
|||
import numpy as np
|
||||
|
||||
class Params():
|
||||
|
||||
class Params:
|
||||
def __init__(self):
|
||||
self.N = 4 #number of state variables
|
||||
self.M = 2 #number of control variables
|
||||
self.T = 10 #Prediction Horizon
|
||||
self.dt = 0.2 #discretization step
|
||||
self.N = 4 # number of state variables
|
||||
self.M = 2 # number of control variables
|
||||
self.T = 10 # Prediction Horizon
|
||||
self.DT = 0.2 # discretization step
|
||||
self.path_tick = 0.05
|
||||
self.L = 0.3 #vehicle wheelbase
|
||||
self.MAX_SPEED = 1.5 #m/s
|
||||
self.MAX_ACC = 1.0 #m/ss
|
||||
self.MAX_D_ACC = 1.0 #m/sss
|
||||
self.MAX_STEER = np.radians(30) #rad
|
||||
self.MAX_D_STEER = np.radians(30) #rad/s
|
||||
self.L = 0.3 # vehicle wheelbase
|
||||
self.MAX_SPEED = 1.5 # m/s
|
||||
self.MAX_ACC = 1.0 # m/ss
|
||||
self.MAX_D_ACC = 1.0 # m/sss
|
||||
self.MAX_STEER = np.radians(30) # rad
|
||||
self.MAX_D_STEER = np.radians(30) # rad/s
|
||||
|
|
|
|||
|
|
@ -1,124 +1,106 @@
|
|||
import numpy as np
|
||||
from scipy.interpolate import interp1d
|
||||
|
||||
from .mpc_config import Params
|
||||
P=Params()
|
||||
|
||||
def compute_path_from_wp(start_xp, start_yp, step = 0.1):
|
||||
P = Params()
|
||||
|
||||
|
||||
def compute_path_from_wp(start_xp, start_yp, step=0.1):
|
||||
"""
|
||||
Computes a reference path given a set of waypoints
|
||||
"""
|
||||
|
||||
final_xp=[]
|
||||
final_yp=[]
|
||||
delta = step #[m]
|
||||
|
||||
for idx in range(len(start_xp)-1):
|
||||
section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
|
||||
|
||||
interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))
|
||||
|
||||
fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
|
||||
fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)
|
||||
|
||||
final_xp = []
|
||||
final_yp = []
|
||||
delta = step # [m]
|
||||
for idx in range(len(start_xp) - 1):
|
||||
section_len = np.sum(
|
||||
np.sqrt(
|
||||
np.power(np.diff(start_xp[idx : idx + 2]), 2)
|
||||
+ np.power(np.diff(start_yp[idx : idx + 2]), 2)
|
||||
)
|
||||
)
|
||||
interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))
|
||||
fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)
|
||||
fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)
|
||||
# watch out to duplicate points!
|
||||
final_xp=np.append(final_xp,fx(interp_range)[1:])
|
||||
final_yp=np.append(final_yp,fy(interp_range)[1:])
|
||||
|
||||
final_xp = np.append(final_xp, fx(interp_range)[1:])
|
||||
final_yp = np.append(final_yp, fy(interp_range)[1:])
|
||||
dx = np.append(0, np.diff(final_xp))
|
||||
dy = np.append(0, np.diff(final_yp))
|
||||
theta = np.arctan2(dy, dx)
|
||||
|
||||
return np.vstack((final_xp,final_yp,theta))
|
||||
return np.vstack((final_xp, final_yp, theta))
|
||||
|
||||
|
||||
def get_nn_idx(state,path):
|
||||
def get_nn_idx(state, path):
|
||||
"""
|
||||
Computes the index of the waypoint closest to vehicle
|
||||
"""
|
||||
|
||||
dx = state[0]-path[0,:]
|
||||
dy = state[1]-path[1,:]
|
||||
dist = np.hypot(dx,dy)
|
||||
dx = state[0] - path[0, :]
|
||||
dy = state[1] - path[1, :]
|
||||
dist = np.hypot(dx, dy)
|
||||
nn_idx = np.argmin(dist)
|
||||
|
||||
try:
|
||||
v = [path[0,nn_idx+1] - path[0,nn_idx],
|
||||
path[1,nn_idx+1] - path[1,nn_idx]]
|
||||
v = [
|
||||
path[0, nn_idx + 1] - path[0, nn_idx],
|
||||
path[1, nn_idx + 1] - path[1, nn_idx],
|
||||
]
|
||||
v /= np.linalg.norm(v)
|
||||
|
||||
d = [path[0,nn_idx] - state[0],
|
||||
path[1,nn_idx] - state[1]]
|
||||
|
||||
if np.dot(d,v) > 0:
|
||||
d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]
|
||||
if np.dot(d, v) > 0:
|
||||
target_idx = nn_idx
|
||||
else:
|
||||
target_idx = nn_idx+1
|
||||
|
||||
target_idx = nn_idx + 1
|
||||
except IndexError as e:
|
||||
target_idx = nn_idx
|
||||
|
||||
return target_idx
|
||||
|
||||
|
||||
def normalize_angle(angle):
|
||||
"""
|
||||
Normalize an angle to [-pi, pi]
|
||||
"""
|
||||
while angle > np.pi:
|
||||
angle -= 2.0 * np.pi
|
||||
|
||||
while angle < -np.pi:
|
||||
angle += 2.0 * np.pi
|
||||
|
||||
return angle
|
||||
|
||||
def get_ref_trajectory(state, path, target_v):
|
||||
|
||||
def get_ref_trajectory(state, path, target_v, dl=0.1):
|
||||
"""
|
||||
For each step in the time horizon
|
||||
modified reference in robot frame
|
||||
"""
|
||||
xref = np.zeros((P.N, P.T+1))
|
||||
dref = np.zeros((1, P.T+1))
|
||||
|
||||
#sp = np.ones((1,T +1))*target_v #speed profile
|
||||
|
||||
xref = np.zeros((P.N, P.T + 1))
|
||||
dref = np.zeros((1, P.T + 1))
|
||||
# sp = np.ones((1,T +1))*target_v #speed profile
|
||||
ncourse = path.shape[1]
|
||||
|
||||
ind = get_nn_idx(state, path)
|
||||
dx=path[0,ind] - state[0]
|
||||
dy=path[1,ind] - state[1]
|
||||
|
||||
|
||||
xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X
|
||||
xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y
|
||||
xref[2, 0] = target_v #V
|
||||
xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta
|
||||
dref[0, 0] = 0.0 # steer operational point should be 0
|
||||
|
||||
dl = 0.05 # Waypoints spacing [m]
|
||||
dx = path[0, ind] - state[0]
|
||||
dy = path[1, ind] - state[1]
|
||||
xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X
|
||||
xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y
|
||||
xref[2, 0] = target_v # V
|
||||
xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta
|
||||
dref[0, 0] = 0.0 # Steer operational point should be 0
|
||||
travel = 0.0
|
||||
|
||||
for i in range(1, P.T+1):
|
||||
travel += abs(target_v) * P.dt #current V or target V?
|
||||
for i in range(1, P.T + 1):
|
||||
travel += abs(target_v) * P.DT
|
||||
dind = int(round(travel / dl))
|
||||
|
||||
if (ind + dind) < ncourse:
|
||||
dx=path[0,ind + dind] - state[0]
|
||||
dy=path[1,ind + dind] - state[1]
|
||||
|
||||
dx = path[0, ind + dind] - state[0]
|
||||
dy = path[1, ind + dind] - state[1]
|
||||
xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
|
||||
xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
|
||||
xref[2, i] = target_v #sp[ind + dind]
|
||||
xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])
|
||||
xref[2, i] = target_v # sp[ind + dind]
|
||||
xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])
|
||||
dref[0, i] = 0.0
|
||||
else:
|
||||
dx=path[0,ncourse - 1] - state[0]
|
||||
dy=path[1,ncourse - 1] - state[1]
|
||||
|
||||
dx = path[0, ncourse - 1] - state[0]
|
||||
dy = path[1, ncourse - 1] - state[1]
|
||||
xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
|
||||
xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
|
||||
xref[2, i] = 0.0 #stop? #sp[ncourse - 1]
|
||||
xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])
|
||||
xref[2, i] = 0.0 # stop? #sp[ncourse - 1]
|
||||
xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])
|
||||
dref[0, i] = 0.0
|
||||
|
||||
return xref, dref
|
||||
|
|
|
|||
Loading…
Reference in New Issue