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

update usage in visualization only demo

master
mcarfagno 2023-10-12 20:17:55 +01:00
parent 279625b4c1
commit c3d92cc4bd
4 changed files with 41 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -18,7 +18,7 @@ SIM_START_V = 0.0
SIM_START_H = 0.0
L = 0.3
P = mpcpy.Params()
params = mpcpy.Params()
# Params
VEL = 1.0 # m/s
@ -31,25 +31,26 @@ class MPCSim:
self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
# starting guess
self.action = np.zeros(P.M)
self.action[0] = P.MAX_ACC / 2 # a
self.action = np.zeros(params.M)
self.action[0] = params.MAX_ACC / 2 # a
self.action[1] = 0.0 # delta
self.opt_u = np.zeros((P.M, P.T))
self.K = int(params.T / params.DT)
self.opt_u = np.zeros((params.M, self.K))
# 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 = [20, 20, 10, 20] # state error cost
Qf = [30, 30, 30, 30] # state final error cost
R = [10, 10] # input cost
P = [10, 10] # input rate of change cost
self.mpc = mpcpy.MPC(P.N, P.M, Q, R)
self.mpc = mpcpy.MPC(Q, Qf, R, P)
# 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,
0.05,
)
# Sim help vars
@ -113,9 +114,7 @@ class MPCSim:
# State Matrices
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
)
target, _ = mpcpy.get_ref_trajectory(self.state, self.path, VEL)
x_mpc, u_mpc = self.mpc.optimize_linearized_model(
A,
@ -123,13 +122,13 @@ class MPCSim:
C,
curr_state,
target,
time_horizon=P.T,
verbose=False,
)
# NOTE: used only for preview purposes
self.opt_u = np.vstack(
(
np.array(u_mpc.value[0, :]).flatten(),
(np.array(u_mpc.value[1, :]).flatten()),
np.array(u_mpc.value[1, :]).flatten(),
)
)
self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
@ -143,12 +142,12 @@ class MPCSim:
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
dtheta0dt = x[2] * np.tan(u[1]) / params.L
dqdt = [dxdt, dydt, dvdt, dtheta0dt]
return dqdt
# solve ODE
tspan = [0, P.DT]
tspan = [0, params.DT]
self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
def plot_sim(self):
@ -157,7 +156,7 @@ class MPCSim:
[TODO:description]
"""
self.sim_time = self.sim_time + P.DT
self.sim_time = self.sim_time + params.DT
self.x_history.append(self.state[0])
self.y_history.append(self.state[1])
self.v_history.append(self.state[2])
@ -231,7 +230,7 @@ class MPCSim:
# 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.xticks(locs[1:], locs[1:] * params.DT)
plt.ylabel("a(t) [m/ss]")
plt.xlabel("t [s]")
@ -240,7 +239,7 @@ class MPCSim:
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.xticks(locs[1:], locs[1:] * params.DT)
plt.xlabel("t [s]")
plt.tight_layout()

30
mpc_pybullet_demo/mpcpy/cvxpy_mpc.py
View File

@ -60,22 +60,22 @@ class MPC:
def __init__(self, state_cost, final_state_cost, input_cost, input_rate_cost):
""" """
nx = P.N # number of state vars
nu = P.M # umber of input/control vars
self.nx = P.N # number of state vars
self.nu = P.M # umber of input/control vars
if len(state_cost) != nx:
raise ValueError(f"State Error cost matrix shuld be of size {nx}")
if len(final_state_cost) != nx:
raise ValueError(f"End State Error cost matrix shuld be of size {nx}")
if len(input_cost) != nu:
raise ValueError(f"Control Effort cost matrix shuld be of size {nu}")
if len(input_rate_cost) != nu:
if len(state_cost) != self.nx:
raise ValueError(f"State Error cost matrix shuld be of size {self.nx}")
if len(final_state_cost) != self.nx:
raise ValueError(f"End State Error cost matrix shuld be of size {self.nx}")
if len(input_cost) != self.nu:
raise ValueError(f"Control Effort cost matrix shuld be of size {self.nu}")
if len(input_rate_cost) != self.nu:
raise ValueError(
f"Control Effort Difference cost matrix shuld be of size {nu}"
f"Control Effort Difference cost matrix shuld be of size {self.nu}"
)
self.dt = P.DT
self.control_horizon = P.T / P.DT
self.control_horizon = int(P.T / P.DT)
self.Q = np.diag(state_cost)
self.Qf = np.diag(final_state_cost)
self.R = np.diag(input_cost)
@ -103,11 +103,11 @@ class MPC:
:return:
"""
assert len(initial_state) == self.state_len
assert len(initial_state) == self.nx
# Create variables
x = opt.Variable((self.state_len, control_horizon + 1), name="states")
u = opt.Variable((self.action_len, control_horizon), name="actions")
x = opt.Variable((self.nx, self.control_horizon + 1), name="states")
u = opt.Variable((self.nu, self.control_horizon), name="actions")
cost = 0
constr = []
@ -143,4 +143,4 @@ class MPC:
prob = opt.Problem(opt.Minimize(cost), constr)
solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
return u[:, 0].value
return x, u

3
mpc_pybullet_demo/mpcpy/mpc_config.py
View File

@ -5,9 +5,8 @@ class Params:
def __init__(self):
self.N = 4 # number of state variables
self.M = 2 # number of control variables
self.T = 10 # Prediction Horizon [s]
self.T = 5 # Prediction Horizon [s]
self.DT = 0.2 # discretization step [s]
self.path_tick = 0.05 # [m]
self.L = 0.3 # vehicle wheelbase [m]
self.TARGET_SPEED = 1.0 # [m/s
self.MAX_SPEED = 1.5 # [m/s

11
mpc_pybullet_demo/mpcpy/utils.py
View File

@ -66,14 +66,14 @@ def normalize_angle(angle):
return angle
def get_ref_trajectory(state, path, target_v, dl=0.1):
def get_ref_trajectory(state, path, target_v):
"""
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
K = int(P.T / P.DT)
xref = np.zeros((P.N, K + 1))
dref = np.zeros((1, K + 1))
ncourse = path.shape[1]
ind = get_nn_idx(state, path)
dx = path[0, ind] - state[0]
@ -84,7 +84,8 @@ def get_ref_trajectory(state, path, target_v, dl=0.1):
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):
dl = np.hypot(path[0, 1] - path[0, 0], path[1, 1] - path[1, 0])
for i in range(1, K + 1):
travel += abs(target_v) * P.DT
dind = int(round(travel / dl))
if (ind + dind) < ncourse: