From c3d92cc4bdda0818017caded282e4eb2574b103d Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Thu, 12 Oct 2023 20:17:55 +0100 Subject: [PATCH] update usage in visualization only demo --- mpc_pybullet_demo/mpc_demo_nosim.py | 39 +++++++++++++-------------- mpc_pybullet_demo/mpcpy/cvxpy_mpc.py | 30 ++++++++++----------- mpc_pybullet_demo/mpcpy/mpc_config.py | 3 +-- mpc_pybullet_demo/mpcpy/utils.py | 11 ++++---- 4 files changed, 41 insertions(+), 42 deletions(-) diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index 7d42b56..0af8a06 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -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() diff --git a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py index 723ba28..bfa03de 100755 --- a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py +++ b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py @@ -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 diff --git a/mpc_pybullet_demo/mpcpy/mpc_config.py b/mpc_pybullet_demo/mpcpy/mpc_config.py index af32e4a..c031086 100644 --- a/mpc_pybullet_demo/mpcpy/mpc_config.py +++ b/mpc_pybullet_demo/mpcpy/mpc_config.py @@ -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 diff --git a/mpc_pybullet_demo/mpcpy/utils.py b/mpc_pybullet_demo/mpcpy/utils.py index 5d23f8f..a12dad7 100755 --- a/mpc_pybullet_demo/mpcpy/utils.py +++ b/mpc_pybullet_demo/mpcpy/utils.py @@ -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: