diff --git a/mpc_pybullet_demo/cvxpy_mpc/__init__.py b/mpc_pybullet_demo/cvxpy_mpc/__init__.py index 52216af..e538b43 100644 --- a/mpc_pybullet_demo/cvxpy_mpc/__init__.py +++ b/mpc_pybullet_demo/cvxpy_mpc/__init__.py @@ -1,2 +1,2 @@ from .cvxpy_mpc import MPC -from .mpc_config import Params +from .vehicle_model import VehicleModel diff --git a/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py b/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py index 95fff9c..50f4416 100755 --- a/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py +++ b/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py @@ -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) diff --git a/mpc_pybullet_demo/cvxpy_mpc/mpc_config.py b/mpc_pybullet_demo/cvxpy_mpc/mpc_config.py deleted file mode 100644 index c031086..0000000 --- a/mpc_pybullet_demo/cvxpy_mpc/mpc_config.py +++ /dev/null @@ -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] diff --git a/mpc_pybullet_demo/cvxpy_mpc/utils.py b/mpc_pybullet_demo/cvxpy_mpc/utils.py index 4fa3197..90ed926 100755 --- a/mpc_pybullet_demo/cvxpy_mpc/utils.py +++ b/mpc_pybullet_demo/cvxpy_mpc/utils.py @@ -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( diff --git a/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py b/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py new file mode 100644 index 0000000..44c042e --- /dev/null +++ b/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py @@ -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] diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index e9cad9b..6f263ab 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -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() diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 2af6b78..58d74c8 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -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__":