12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix nosim demo not updating state correctly

master
mcarfagno 2022-02-05 10:16:38 +00:00
parent a2eb20e7be
commit 89e5796fbc
5 changed files with 262 additions and 214 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

299
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -3,6 +3,7 @@
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from matplotlib import animation from matplotlib import animation
from scipy.integrate import odeint
from mpcpy.utils import compute_path_from_wp from mpcpy.utils import compute_path_from_wp
import mpcpy import mpcpy
@ -11,154 +12,195 @@ import sys
import time import time
# Robot Starting position # Robot Starting position
SIM_START_X=0. SIM_START_X = 0.0
SIM_START_Y=0.5 SIM_START_Y = 0.5
SIM_START_V=0. SIM_START_V = 0.0
SIM_START_H=0. SIM_START_H = 0.0
L=0.3 L = 0.3
P=mpcpy.Params() P = mpcpy.Params()
# Params
VEL = 1.0 # m/s
# Classes # Classes
class MPCSim(): class MPCSim:
def __init__(self): def __init__(self):
# State for the robot mathematical model [x,y,heading] # 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 = np.zeros(P.M)
self.action[0] = P.MAX_ACC/2 #a self.action[0] = P.MAX_ACC / 2 # a
self.action[1] = 0.0 #delta 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 # Cost Matrices
Q = np.diag([20,20,10,20]) #state error cost Q = np.diag([20, 20, 10, 20]) # state error cost
Qf = np.diag([30,30,30,30]) #state final 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 cost
R_ = np.diag([10,10]) #input rate of change 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 # Interpolated Path to follow given waypoints
self.path = compute_path_from_wp([0,3,4,6,10,12,13,13,6,1,0], self.path = compute_path_from_wp(
[0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick) [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 # Sim help vars
self.sim_time=0 self.sim_time = 0
self.x_history=[] self.x_history = []
self.y_history=[] self.y_history = []
self.v_history=[] self.v_history = []
self.h_history=[] self.h_history = []
self.a_history=[] self.a_history = []
self.d_history=[] self.d_history = []
self.predicted=None self.predicted = None
#Initialise plot # Initialise plot
plt.style.use("ggplot") plt.style.use("ggplot")
self.fig = plt.figure() self.fig = plt.figure()
plt.ion() plt.ion()
plt.show() plt.show()
def predict_motion(self): def preview(self, mpc_out):
''' """
''' [TODO:summary]
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
[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 self.predicted = predicted
def run(self): def run(self):
''' """
''' [TODO:summary]
[TODO:description]
"""
self.plot_sim() self.plot_sim()
input("Press Enter to continue...") input("Press Enter to continue...")
while 1: while 1:
if (
if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<0.1: np.sqrt(
(self.state[0] - self.path[0, -1]) ** 2
+ (self.state[1] - self.path[1, -1]) ** 2
)
< 0.5
):
print("Success! Goal Reached") print("Success! Goal Reached")
input("Press Enter to continue...") input("Press Enter to continue...")
return return
# optimization loop
#optimization loop # start=time.time()
#start=time.time() # dynamycs w.r.t robot frame
curr_state = np.array(
[0, 0, self.state[2], 0]
)
# State Matrices # State Matrices
A,B,C = mpcpy.get_linear_model_matrices(self.state, self.action) A, B, C = mpcpy.get_linear_model_matrices(curr_state, self.action)
# Get Reference_traj -> inputs are in worldframe
#TODO: check why taget does not update? target, _ = mpcpy.get_ref_trajectory(
self.state, self.path, VEL, dl=P.path_tick
#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]]
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)) # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
self.predict([self.action[0], self.action[1]])
self.update_sim(self.action[0],self.action[1]) self.preview(x_mpc.value)
self.predict_motion()
self.plot_sim() 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): # solve ODE
''' tspan = [0, P.DT]
''' self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
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
def plot_sim(self): def plot_sim(self):
''' """
''' [TODO:summary]
self.sim_time = self.sim_time+P.dt
[TODO:description]
"""
self.sim_time = self.sim_time + P.DT
self.x_history.append(self.state[0]) self.x_history.append(self.state[0])
self.y_history.append(self.state[1]) self.y_history.append(self.state[1])
self.v_history.append(self.state[2]) self.v_history.append(self.state[2])
self.h_history.append(self.state[3]) self.h_history.append(self.state[3])
self.a_history.append(self.opt_u[0,1]) self.a_history.append(self.opt_u[0, 1])
self.d_history.append(self.opt_u[1,1]) self.d_history.append(self.opt_u[1, 1])
plt.clf() plt.clf()
grid = plt.GridSpec(2, 3) grid = plt.GridSpec(2, 3)
plt.subplot(grid[0:2, 0:2]) 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', plt.plot(
marker=".", self.path[0, :],
label="reference track") self.path[1, :],
c="tab:orange",
marker=".",
label="reference track",
)
plt.plot(self.x_history, self.y_history, c='tab:blue', plt.plot(
marker=".", self.x_history,
alpha=0.5, self.y_history,
label="vehicle trajectory") c="tab:blue",
marker=".",
alpha=0.5,
label="vehicle trajectory",
)
if self.predicted is not None: if self.predicted is not None:
plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green', plt.plot(
marker="+", self.predicted[0, :],
alpha=0.5, self.predicted[1, :],
label="mpc opt trajectory") c="tab:green",
marker="+",
alpha=0.5,
label="mpc opt trajectory",
)
# plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue', # plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue',
# marker=".", # marker=".",
@ -175,28 +217,32 @@ class MPCSim():
plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1]) plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1])
plt.ylabel('map y') plt.ylabel("map y")
plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0)) plt.yticks(
plt.xlabel('map x') np.arange(min(self.path[1, :]) - 1.0, max(self.path[1, :] + 1.0) + 1, 1.0)
plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+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.axis("equal")
#plt.legend() # plt.legend()
plt.subplot(grid[0, 2]) plt.subplot(grid[0, 2])
#plt.title("Linear Velocity {} m/s".format(self.v_history[-1])) # plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
plt.plot(self.a_history,c='tab:orange') plt.plot(self.a_history, c="tab:orange")
locs, _ = plt.xticks() locs, _ = plt.xticks()
plt.xticks(locs[1:], locs[1:]*P.dt) plt.xticks(locs[1:], locs[1:] * P.DT)
plt.ylabel('a(t) [m/ss]') plt.ylabel("a(t) [m/ss]")
plt.xlabel('t [s]') plt.xlabel("t [s]")
plt.subplot(grid[1, 2]) plt.subplot(grid[1, 2])
#plt.title("Angular Velocity {} m/s".format(self.w_history[-1])) # plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
plt.plot(np.degrees(self.d_history),c='tab:orange') plt.plot(np.degrees(self.d_history), c="tab:orange")
plt.ylabel('gamma(t) [deg]') plt.ylabel("gamma(t) [deg]")
locs, _ = plt.xticks() locs, _ = plt.xticks()
plt.xticks(locs[1:], locs[1:]*P.dt) plt.xticks(locs[1:], locs[1:] * P.DT)
plt.xlabel('t [s]') plt.xlabel("t [s]")
plt.tight_layout() plt.tight_layout()
@ -205,23 +251,41 @@ class MPCSim():
def plot_car(x, y, yaw): 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] LENGTH = 0.5 # [m]
WIDTH = 0.25 # [m] WIDTH = 0.25 # [m]
OFFSET = LENGTH # [m] OFFSET = LENGTH # [m]
outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET], outline = np.array(
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) [
[-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)], Rotm = np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]])
[-np.sin(yaw), np.cos(yaw)]])
outline = (outline.T.dot(Rotm)).T outline = (outline.T.dot(Rotm)).T
outline[0, :] += x outline[0, :] += x
outline[1, :] += y 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(): def do_sim():
@ -231,5 +295,6 @@ def do_sim():
except Exception as e: except Exception as e:
sys.exit(e) sys.exit(e)
if __name__ == '__main__':
if __name__ == "__main__":
do_sim() do_sim()

14
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -31,7 +31,7 @@ def set_ctrl(robotId,currVel,acceleration,steeringAngle):
wheels = [8,15] wheels = [8,15]
maxForce = 50 maxForce = 50
targetVelocity = currVel + acceleration*P.dt targetVelocity = currVel + acceleration*P.DT
#targetVelocity=lastVel #targetVelocity=lastVel
#print(targetVelocity) #print(targetVelocity)
@ -158,10 +158,10 @@ def run_sim():
state[3] = 0.0 state[3] = 0.0
#add 1 timestep delay to input #add 1 timestep delay to input
state[0]=state[0]+state[2]*np.cos(state[3])*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[1]=state[1]+state[2]*np.sin(state[3])*P.DT
state[2]=state[2]+action[0]*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[3]=state[3]+action[0]*np.tan(action[1])/P.L*P.DT
#optimization loop #optimization loop
@ -186,8 +186,8 @@ def run_sim():
set_ctrl(car,state[2],action[0],action[1]) set_ctrl(car,state[2],action[0],action[1])
if P.dt-elapsed>0: if P.DT-elapsed>0:
time.sleep(P.dt-elapsed) time.sleep(P.DT-elapsed)
if __name__ == '__main__': if __name__ == '__main__':
run_sim() run_sim()

10
mpc_pybullet_demo/mpcpy/cvxpy_mpc.py
View File

@ -34,15 +34,15 @@ def get_linear_model_matrices(x_bar,u_bar):
A[1,2] = st A[1,2] = st
A[1,3] = v*ct A[1,3] = v*ct
A[3,2] = v*td/P.L 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 = np.zeros((P.N,P.M))
B[2,0]=1 B[2,0]=1
B[3,1]=v/(P.L*cd**2) 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) 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 np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)
return A_lin, B_lin, C_lin return A_lin, B_lin, C_lin
@ -98,8 +98,8 @@ class MPC():
# Actuation rate of change # Actuation rate of change
if t < (time_horizon - 1): if t < (time_horizon - 1):
_cost += opt.quad_form(u[:,t + 1] - u[:,t], R * 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[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[1, t + 1] - u[1, t])/P.DT <= P.MAX_D_STEER]
if t == 0: if t == 0:

23
mpc_pybullet_demo/mpcpy/mpc_config.py
View File

@ -1,15 +1,16 @@
import numpy as np import numpy as np
class Params():
class Params:
def __init__(self): def __init__(self):
self.N = 4 #number of state variables self.N = 4 # number of state variables
self.M = 2 #number of control variables self.M = 2 # number of control variables
self.T = 10 #Prediction Horizon self.T = 10 # Prediction Horizon
self.dt = 0.2 #discretization step self.DT = 0.2 # discretization step
self.path_tick = 0.05 self.path_tick = 0.05
self.L = 0.3 #vehicle wheelbase self.L = 0.3 # vehicle wheelbase
self.MAX_SPEED = 1.5 #m/s self.MAX_SPEED = 1.5 # m/s
self.MAX_ACC = 1.0 #m/ss self.MAX_ACC = 1.0 # m/ss
self.MAX_D_ACC = 1.0 #m/sss self.MAX_D_ACC = 1.0 # m/sss
self.MAX_STEER = np.radians(30) #rad self.MAX_STEER = np.radians(30) # rad
self.MAX_D_STEER = np.radians(30) #rad/s self.MAX_D_STEER = np.radians(30) # rad/s

126
mpc_pybullet_demo/mpcpy/utils.py
View File

@ -1,124 +1,106 @@
import numpy as np import numpy as np
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
from .mpc_config import Params 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 Computes a reference path given a set of waypoints
""" """
final_xp = []
final_xp=[] final_yp = []
final_yp=[] delta = step # [m]
delta = step #[m] for idx in range(len(start_xp) - 1):
section_len = np.sum(
for idx in range(len(start_xp)-1): np.sqrt(
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))) 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) interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))
fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1) 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! # watch out to duplicate points!
final_xp=np.append(final_xp,fx(interp_range)[1:]) final_xp = np.append(final_xp, fx(interp_range)[1:])
final_yp=np.append(final_yp,fy(interp_range)[1:]) final_yp = np.append(final_yp, fy(interp_range)[1:])
dx = np.append(0, np.diff(final_xp)) dx = np.append(0, np.diff(final_xp))
dy = np.append(0, np.diff(final_yp)) dy = np.append(0, np.diff(final_yp))
theta = np.arctan2(dy, dx) 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 Computes the index of the waypoint closest to vehicle
""" """
dx = state[0] - path[0, :]
dx = state[0]-path[0,:] dy = state[1] - path[1, :]
dy = state[1]-path[1,:] dist = np.hypot(dx, dy)
dist = np.hypot(dx,dy)
nn_idx = np.argmin(dist) nn_idx = np.argmin(dist)
try: try:
v = [path[0,nn_idx+1] - path[0,nn_idx], v = [
path[1,nn_idx+1] - path[1,nn_idx]] path[0, nn_idx + 1] - path[0, nn_idx],
path[1, nn_idx + 1] - path[1, nn_idx],
]
v /= np.linalg.norm(v) v /= np.linalg.norm(v)
d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]
d = [path[0,nn_idx] - state[0], if np.dot(d, v) > 0:
path[1,nn_idx] - state[1]]
if np.dot(d,v) > 0:
target_idx = nn_idx target_idx = nn_idx
else: else:
target_idx = nn_idx+1 target_idx = nn_idx + 1
except IndexError as e: except IndexError as e:
target_idx = nn_idx target_idx = nn_idx
return target_idx return target_idx
def normalize_angle(angle): def normalize_angle(angle):
""" """
Normalize an angle to [-pi, pi] Normalize an angle to [-pi, pi]
""" """
while angle > np.pi: while angle > np.pi:
angle -= 2.0 * np.pi angle -= 2.0 * np.pi
while angle < -np.pi: while angle < -np.pi:
angle += 2.0 * np.pi angle += 2.0 * np.pi
return angle 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 For each step in the time horizon
modified reference in robot frame modified reference in robot frame
""" """
xref = np.zeros((P.N, P.T+1)) xref = np.zeros((P.N, P.T + 1))
dref = np.zeros((1, P.T+1)) dref = np.zeros((1, P.T + 1))
# sp = np.ones((1,T +1))*target_v #speed profile
#sp = np.ones((1,T +1))*target_v #speed profile
ncourse = path.shape[1] ncourse = path.shape[1]
ind = get_nn_idx(state, path) ind = get_nn_idx(state, path)
dx=path[0,ind] - state[0] dx = path[0, ind] - state[0]
dy=path[1,ind] - state[1] 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[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X xref[2, 0] = target_v # V
xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta
xref[2, 0] = target_v #V dref[0, 0] = 0.0 # Steer operational point should be 0
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]
travel = 0.0 travel = 0.0
for i in range(1, P.T + 1):
for i in range(1, P.T+1): travel += abs(target_v) * P.DT
travel += abs(target_v) * P.dt #current V or target V?
dind = int(round(travel / dl)) dind = int(round(travel / dl))
if (ind + dind) < ncourse: if (ind + dind) < ncourse:
dx=path[0,ind + dind] - state[0] dx = path[0, ind + dind] - state[0]
dy=path[1,ind + dind] - state[1] dy = path[1, ind + dind] - state[1]
xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) 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[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
xref[2, i] = target_v #sp[ind + dind] xref[2, i] = target_v # sp[ind + dind]
xref[3, i] = normalize_angle(path[2,ind + dind] - state[3]) xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])
dref[0, i] = 0.0 dref[0, i] = 0.0
else: else:
dx=path[0,ncourse - 1] - state[0] dx = path[0, ncourse - 1] - state[0]
dy=path[1,ncourse - 1] - state[1] dy = path[1, ncourse - 1] - state[1]
xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) 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[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
xref[2, i] = 0.0 #stop? #sp[ncourse - 1] xref[2, i] = 0.0 # stop? #sp[ncourse - 1]
xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3]) xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])
dref[0, i] = 0.0 dref[0, i] = 0.0
return xref, dref return xref, dref