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

View File

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

View File

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

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]

View File

@ -1,8 +1,5 @@
import numpy as np import numpy as np
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
from .mpc_config import Params
P = Params()
def compute_path_from_wp(start_xp, start_yp, step=0.1): 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 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 Reinterpolate the trajectory to get a set N desired target states
:param state: :param state:
@ -70,9 +67,9 @@ def get_ref_trajectory(state, path, target_v):
:param target_v: :param target_v:
:return: :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) ind = get_nn_idx(state, path)
cdist = np.append( cdist = np.append(

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]

View File

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

View File

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