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

View File

@ -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()

View File

@ -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()

View File

@ -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:

View File

@ -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

View File

@ -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