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

WIP: remove Params class

master
mcarfagno 2023-10-23 20:58:21 +01:00
parent 82623d2f81
commit 82a4b6ab2d
7 changed files with 79 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mpc_pybullet_demo/cvxpy_mpc/__init__.py
View File

@ -1,2 +1,2 @@
from .cvxpy_mpc import MPC
from .mpc_config import Params
from .vehicle_model import VehicleModel

44
mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py
View File

@ -4,22 +4,23 @@ np.seterr(divide="ignore", invalid="ignore")
import cvxpy as opt
from .mpc_config import Params
P = Params()
class MPC:
def __init__(self, state_cost, final_state_cost, input_cost, input_rate_cost):
def __init__(
self, vehicle, T, DT, state_cost, final_state_cost, input_cost, input_rate_cost
):
"""
:param vehicle:
:param T:
:param DT:
:param state_cost:
:param final_state_cost:
:param input_cost:
:param input_rate_cost:
"""
self.nx = P.N # number of state vars
self.nu = P.M # umber of input/control vars
self.nx = 4 # number of state vars
self.nu = 2 # umber of input/control vars
if len(state_cost) != self.nx:
raise ValueError(f"State Error cost matrix shuld be of size {self.nx}")
@ -32,14 +33,13 @@ class MPC:
f"Control Effort Difference cost matrix shuld be of size {self.nu}"
)
self.dt = P.DT
self.control_horizon = int(P.T / P.DT)
self.vehicle = vehicle
self.dt = DT
self.control_horizon = int(T / DT)
self.Q = np.diag(state_cost)
self.Qf = np.diag(final_state_cost)
self.R = np.diag(input_cost)
self.P = np.diag(input_rate_cost)
self.u_bounds = np.array([P.MAX_ACC, P.MAX_STEER])
self.du_bounds = np.array([P.MAX_D_ACC, P.MAX_D_STEER])
def get_linear_model_matrices(self, x_bar, u_bar):
"""
@ -67,15 +67,17 @@ class MPC:
A[0, 3] = -v * st
A[1, 2] = st
A[1, 3] = v * ct
A[3, 2] = v * td / P.L
A[3, 2] = v * td / self.vehicle.wheelbase
A_lin = np.eye(self.nx) + self.dt * A
B = np.zeros((P.N, P.M))
B = np.zeros((self.nx, self.nu))
B[2, 0] = 1
B[3, 1] = v / (P.L * cd**2)
B[3, 1] = v / (self.vehicle.wheelbase * cd**2)
B_lin = self.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 / self.vehicle.wheelbase]).reshape(
self.nx, 1
)
C_lin = (
self.dt
* (
@ -122,13 +124,13 @@ class MPC:
# Kinematics Constrains
constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
# Actuation rate of change limit
# Actuation rate of change bounds
if k < (self.control_horizon - 1):
constr += [
opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.du_bounds[0]
opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.vehicle.max_d_acc
]
constr += [
opt.abs(u[1, k + 1] - u[1, k]) / self.dt <= self.du_bounds[1]
opt.abs(u[1, k + 1] - u[1, k]) / self.dt <= self.vehicle.max_d_steer
]
# Final Point tracking
@ -137,9 +139,9 @@ class MPC:
# initial state
constr += [x[:, 0] == initial_state]
# actuation magnitude
constr += [opt.abs(u[:, 0]) <= self.u_bounds[0]]
constr += [opt.abs(u[:, 1]) <= self.u_bounds[1]]
# actuation bounds
constr += [opt.abs(u[:, 0]) <= self.vehicle.max_acc]
constr += [opt.abs(u[:, 1]) <= self.vehicle.max_steer]
prob = opt.Problem(opt.Minimize(cost), constr)
solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)

16
mpc_pybullet_demo/cvxpy_mpc/mpc_config.py
View File

@ -1,16 +0,0 @@
import numpy as np
class Params:
def __init__(self):
self.N = 4 # number of state variables
self.M = 2 # number of control variables
self.T = 5 # Prediction Horizon [s]
self.DT = 0.2 # discretization step [s]
self.L = 0.3 # vehicle wheelbase [m]
self.TARGET_SPEED = 1.0 # [m/s
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]

9
mpc_pybullet_demo/cvxpy_mpc/utils.py
View File

@ -1,8 +1,5 @@
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):
@ -62,7 +59,7 @@ def get_nn_idx(state, path):
return target_idx
def get_ref_trajectory(state, path, target_v):
def get_ref_trajectory(state, path, target_v, double T, double DT):
"""
Reinterpolate the trajectory to get a set N desired target states
:param state:
@ -70,9 +67,9 @@ def get_ref_trajectory(state, path, target_v):
:param target_v:
:return:
"""
K = int(P.T / P.DT)
K = int(T / DT)
xref = np.zeros((P.N, K))
xref = np.zeros((4, K))
ind = get_nn_idx(state, path)
cdist = np.append(

15
mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py Normal file
View File

@ -0,0 +1,15 @@
import numpy as np
class VehicleModel:
"""
Helper class to hold vehicle info
"""
def __init__(self):
self.wheelbase = 0.3 # vehicle wheelbase [m]
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]

36
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -5,21 +5,23 @@ import matplotlib.pyplot as plt
from scipy.integrate import odeint
from cvxpy_mpc.utils import compute_path_from_wp, get_ref_trajectory
from cvxpy_mpc import MPC, Params
from cvxpy_mpc import MPC, VehicleModel
import sys
# Robot Starting position
SIM_START_X = 0.0
SIM_START_Y = 0.5
SIM_START_V = 0.0
SIM_START_H = 0.0
L = 0.3
params = Params()
# Params
VEL = 1.0 # m/s
TARGET_VEL = 1.0 # m/s
T = 5 # Prediction Horizon [s]
DT = 0.2 # discretization step [s]
L = 0.3 # vehicle wheelbase [m]
# Classes
@ -29,12 +31,12 @@ class MPCSim:
self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
# starting guess
self.action = np.zeros(params.M)
self.action[0] = params.MAX_ACC / 2 # a
self.action = np.zeros(2)
self.action[0] = 1.0 # a
self.action[1] = 0.0 # delta
self.K = int(params.T / params.DT)
self.opt_u = np.zeros((params.M, self.K))
self.K = int(T / DT)
self.opt_u = np.zeros((2, self.K))
# Weights for Cost Matrices
Q = [20, 20, 10, 20] # state error cost
@ -42,7 +44,7 @@ class MPCSim:
R = [10, 10] # input cost
P = [10, 10] # input rate of change cost
self.mpc = MPC(Q, Qf, R, P)
self.mpc = MPC(VehicleModel(), T, DT, Q, Qf, R, P)
# Interpolated Path to follow given waypoints
self.path = compute_path_from_wp(
@ -110,7 +112,7 @@ class MPCSim:
# dynamycs w.r.t robot frame
curr_state = np.array([0, 0, self.state[2], 0])
# Get Reference_traj -> inputs are in worldframe
target = get_ref_trajectory(self.state, self.path, VEL)
target = get_ref_trajectory(self.state, self.path, TARGET_VEL)
x_mpc, u_mpc = self.mpc.step(
curr_state,
@ -127,21 +129,21 @@ class MPCSim:
)
self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
self.predict([self.action[0], self.action[1]])
self.predict([self.action[0], self.action[1]], DT)
self.preview(x_mpc.value)
self.plot_sim()
def predict(self, u):
def predict(self, u, dt):
def kinematics_model(x, t, u):
dxdt = x[2] * np.cos(x[3])
dydt = x[2] * np.sin(x[3])
dvdt = u[0]
dthetadt = x[2] * np.tan(u[1]) / params.L
dthetadt = x[2] * np.tan(u[1]) / L
dqdt = [dxdt, dydt, dvdt, dthetadt]
return dqdt
# solve ODE
tspan = [0, params.DT]
tspan = [0, dt]
self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
def plot_sim(self):
@ -150,7 +152,7 @@ class MPCSim:
[TODO:description]
"""
self.sim_time = self.sim_time + params.DT
self.sim_time = self.sim_time + DT
self.x_history.append(self.state[0])
self.y_history.append(self.state[1])
self.v_history.append(self.state[2])
@ -224,7 +226,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:] * params.DT)
plt.xticks(locs[1:], locs[1:] * DT)
plt.ylabel("a(t) [m/ss]")
plt.xlabel("t [s]")
@ -233,7 +235,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:] * params.DT)
plt.xticks(locs[1:], locs[1:] * DT)
plt.xlabel("t [s]")
plt.tight_layout()

34
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -2,9 +2,7 @@ import numpy as np
import matplotlib.pyplot as plt
from cvxpy_mpc.utils import compute_path_from_wp, get_ref_trajectory
from cvxpy_mpc import MPC, Params
params = Params()
from cvxpy_mpc import MPC, VehicleModel
import sys
import time
@ -12,6 +10,12 @@ import pathlib
import pybullet as p
import time
# Params
TARGET_VEL = 1.0 # m/s
L = 0.3 # vehicle wheelbase [m]
T = 5 # Prediction Horizon [s]
DT = 0.2 # discretization step [s]
def get_state(robotId):
""" """
@ -34,7 +38,7 @@ def set_ctrl(robotId, currVel, acceleration, steeringAngle):
wheels = [8, 15]
maxForce = 50
targetVelocity = currVel + acceleration * params.DT
targetVelocity = currVel + acceleration * DT
for wheel in wheels:
p.setJointMotorControl2(
@ -210,8 +214,8 @@ def run_sim():
p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1])
# starting guess
action = np.zeros(params.M)
action[0] = params.MAX_ACC / 2 # a
action = np.zeros(2)
action[0] = 1.0 # a
action[1] = 0.0 # delta
# Cost Matrices
@ -220,7 +224,7 @@ def run_sim():
R = [10, 10] # input cost [acc ,steer]
P = [10, 10] # input rate of change cost [acc ,steer]
mpc = MPC(Q, Qf, R, P)
mpc = MPC(VehicleModel(), Q, Qf, R, P)
x_history = []
y_history = []
@ -246,7 +250,7 @@ def run_sim():
# Get Reference_traj
# NOTE: inputs are in world frame
target = get_ref_trajectory(state, path, params.TARGET_SPEED)
target = get_ref_trajectory(state, path, TARGET_VEL)
# for MPC base link frame is used:
# so x, y, yaw are 0.0, but speed is the same
@ -254,12 +258,10 @@ def run_sim():
# to account for MPC latency
# simulate one timestep actuation delay
ego_state[0] = ego_state[0] + ego_state[2] * np.cos(ego_state[3]) * params.DT
ego_state[1] = ego_state[1] + ego_state[2] * np.sin(ego_state[3]) * params.DT
ego_state[2] = ego_state[2] + action[0] * params.DT
ego_state[3] = (
ego_state[3] + action[0] * np.tan(action[1]) / params.L * params.DT
)
ego_state[0] = ego_state[0] + ego_state[2] * np.cos(ego_state[3]) * DT
ego_state[1] = ego_state[1] + ego_state[2] * np.sin(ego_state[3]) * DT
ego_state[2] = ego_state[2] + action[0] * DT
ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT
# optimization loop
start = time.time()
@ -274,8 +276,8 @@ def run_sim():
set_ctrl(car, state[2], action[0], action[1])
if params.DT - elapsed > 0:
time.sleep(params.DT - elapsed)
if DT - elapsed > 0:
time.sleep(DT - elapsed)
if __name__ == "__main__":