From 55f560ca7ecb4a1868581d919b2c7cf39abfc326 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Wed, 25 Oct 2023 17:04:35 +0100 Subject: [PATCH] set rate of change limit on u_0 --- mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py | 41 ++++++++++++++---------- mpc_pybullet_demo/mpc_demo_nosim.py | 14 ++++---- mpc_pybullet_demo/mpc_demo_pybullet.py | 4 +-- 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py b/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py index 7c554dd..b75b51c 100755 --- a/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py +++ b/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py @@ -125,29 +125,25 @@ class MPC: # Ak, Bk, Ck = self.get_linear_model_matrices(x_prev[:,k], u_prev[:,k]) A, B, C = self.get_linear_model_matrices(initial_state, prev_cmd) + # Tracking error cost + for k in range(self.control_horizon): + cost += opt.quad_form(x[:, k + 1] - target[:, k], self.Q) + + # Final point tracking cost + cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf) + + # Actuation magnitude cost for k in range(self.control_horizon): - cost += opt.quad_form(target[:, k] - x[:, k + 1], self.Q) cost += opt.quad_form(u[:, k], self.R) - # Actuation rate of change - if k < (self.control_horizon - 1): - cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P) + # Actuation rate of change cost + for k in range(1, self.control_horizon): + cost += opt.quad_form(u[:, k] - u[:, k - 1], self.P) - # Kinematics Constrains + # Kinematics Constrains + for k in range(self.control_horizon): constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C] - # Actuation rate of change bounds - if k < (self.control_horizon - 1): - constr += [ - 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.vehicle.max_d_steer - ] - - # Final Point tracking - cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf) - # initial state constr += [x[:, 0] == initial_state] @@ -155,6 +151,17 @@ class MPC: constr += [opt.abs(u[:, 0]) <= self.vehicle.max_acc] constr += [opt.abs(u[:, 1]) <= self.vehicle.max_steer] + # Actuation rate of change bounds + constr += [opt.abs(u[0, 0] - prev_cmd[0]) / self.dt <= self.vehicle.max_d_acc] + constr += [opt.abs(u[1, 0] - prev_cmd[1]) / self.dt <= self.vehicle.max_d_steer] + for k in range(1, self.control_horizon): + constr += [ + opt.abs(u[0, k] - u[0, k - 1]) / self.dt <= self.vehicle.max_d_acc + ] + constr += [ + opt.abs(u[1, k] - u[1, k - 1]) / self.dt <= self.vehicle.max_d_steer + ] + prob = opt.Problem(opt.Minimize(cost), constr) solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False) return x, u diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index c54147f..eee22b3 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -31,12 +31,9 @@ class MPCSim: self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H]) # helper variable to keep track of mpc output + # starting condition is 0,0 self.action = np.zeros(2) - # starting guess - self.action[0] = 1.0 # a - self.action[1] = 0.0 # delta - self.K = int(T / DT) Q = [20, 20, 10, 20] # state error cost @@ -121,16 +118,16 @@ class MPCSim: verbose=False, ) # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) - # only the first one is used to advance the simulation + self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]] - self.state = self.predict([self.action[0], self.action[1]], DT) + self.state = self.predict(self.state, [self.action[0], self.action[1]], DT) # use the state trajectory to preview the optimizer output self.predicted = self.preview(x_mpc.value) self.plot_sim() - def predict(self, u, dt): + def predict(self, state, u, dt): def kinematics_model(x, t, u): dxdt = x[2] * np.cos(x[3]) dydt = x[2] * np.sin(x[3]) @@ -141,7 +138,8 @@ class MPCSim: # solve ODE tspan = [0, dt] - return odeint(kinematics_model, self.state, tspan, args=(u[:],))[1] + new_state = odeint(kinematics_model, state, tspan, args=(u[:],))[1] + return new_state def plot_sim(self): self.sim_time = self.sim_time + DT diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 8696163..4ac94c5 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -226,10 +226,8 @@ def run_sim(): for x_, y_ in zip(path[0, :], path[1, :]): p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1]) - # starting guess + # starting conditions action = np.zeros(2) - action[0] = 1.0 # a - action[1] = 0.0 # delta # Cost Matrices Q = [20, 20, 10, 20] # state error cost [x,y,v,yaw]