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

improve naming

master
mcarfagno 2023-11-10 09:40:55 +00:00
parent f397acdbda
commit fbd5f792d7
2 changed files with 27 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -32,7 +32,7 @@ class MPCSim:
# helper variable to keep track of mpc output # helper variable to keep track of mpc output
# starting condition is 0,0 # starting condition is 0,0
self.action = np.zeros(2) self.control = np.zeros(2)
self.K = int(T / DT) self.K = int(T / DT)
@ -57,7 +57,7 @@ class MPCSim:
self.h_history = [] self.h_history = []
self.a_history = [] self.a_history = []
self.d_history = [] self.d_history = []
self.predicted = None self.optimized_trajectory = None
# Initialise plot # Initialise plot
plt.style.use("ggplot") plt.style.use("ggplot")
@ -65,24 +65,26 @@ class MPCSim:
plt.ion() plt.ion()
plt.show() plt.show()
def ego_to_world(self, mpc_out): def ego_to_global(self, mpc_out):
""" """
transforms optimized trajectory XY points from ego(car) reference
into global(map) frame
Args: Args:
mpc_out (): mpc_out ():
""" """
predicted = np.zeros((2, self.K)) trajectory = np.zeros((2, self.K))
predicted[:, :] = mpc_out[0:2, 1:] trajectory[:, :] = mpc_out[0:2, 1:]
Rotm = np.array( Rotm = np.array(
[ [
[np.cos(self.state[3]), np.sin(self.state[3])], [np.cos(self.state[3]), np.sin(self.state[3])],
[-np.sin(self.state[3]), np.cos(self.state[3])], [-np.sin(self.state[3]), np.cos(self.state[3])],
] ]
) )
predicted = (predicted.T.dot(Rotm)).T trajectory = (trajectory.T.dot(Rotm)).T
predicted[0, :] += self.state[0] trajectory[0, :] += self.state[0]
predicted[1, :] += self.state[1] trajectory[1, :] += self.state[1]
return predicted return trajectory
def run(self): def run(self):
""" """
@ -114,19 +116,19 @@ class MPCSim:
x_mpc, u_mpc = self.mpc.step( x_mpc, u_mpc = self.mpc.step(
curr_state, curr_state,
target, target,
self.action, self.control,
verbose=False, verbose=False,
) )
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
# only the first one is used to advance the simulation # only the first one is used to advance the simulation
self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]] self.control[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
self.state = self.predict_next_state( self.state = self.predict_next_state(
self.state, [self.action[0], self.action[1]], DT self.state, [self.control[0], self.control[1]], DT
) )
# use the optimizer output to preview the predicted state trajectory # use the optimizer output to preview the predicted state trajectory
self.predicted = self.ego_to_world(x_mpc.value) self.optimized_trajectory = self.ego_to_global(x_mpc.value)
self.plot_sim() self.plot_sim()
def predict_next_state(self, state, u, dt): def predict_next_state(self, state, u, dt):
@ -149,8 +151,8 @@ class MPCSim:
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])
self.h_history.append(self.state[3]) self.h_history.append(self.state[3])
self.a_history.append(self.action[0]) self.a_history.append(self.control[0])
self.d_history.append(self.action[1]) self.d_history.append(self.control[1])
plt.clf() plt.clf()
@ -178,10 +180,10 @@ class MPCSim:
label="vehicle trajectory", label="vehicle trajectory",
) )
if self.predicted is not None: if self.optimized_trajectory is not None:
plt.plot( plt.plot(
self.predicted[0, :], self.optimized_trajectory[0, :],
self.predicted[1, :], self.optimized_trajectory[1, :],
c="tab:green", c="tab:green",
marker="+", marker="+",
alpha=0.5, alpha=0.5,

14
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -227,7 +227,7 @@ 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 conditions # starting conditions
action = np.zeros(2) control = np.zeros(2)
# Cost Matrices # Cost Matrices
Q = [20, 20, 10, 20] # state error cost [x,y,v,yaw] Q = [20, 20, 10, 20] # state error cost [x,y,v,yaw]
@ -271,21 +271,21 @@ def run_sim():
# simulate one timestep actuation delay # simulate one timestep actuation delay
ego_state[0] = ego_state[0] + ego_state[2] * np.cos(ego_state[3]) * 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[1] = ego_state[1] + ego_state[2] * np.sin(ego_state[3]) * DT
ego_state[2] = ego_state[2] + action[0] * DT ego_state[2] = ego_state[2] + control[0] * DT
ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT ego_state[3] = ego_state[3] + control[0] * np.tan(control[1]) / L * DT
# optimization loop # optimization loop
start = time.time() start = time.time()
# MPC step # MPC step
_, u_mpc = mpc.step(ego_state, target, action, verbose=False) _, u_mpc = mpc.step(ego_state, target, control, verbose=False)
action[0] = u_mpc.value[0, 0] control[0] = u_mpc.value[0, 0]
action[1] = u_mpc.value[1, 0] control[1] = u_mpc.value[1, 0]
elapsed = time.time() - start elapsed = time.time() - start
print("CVXPY Optimization Time: {:.4f}s".format(elapsed)) print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
set_ctrl(car, state[2], action[0], action[1]) set_ctrl(car, state[2], control[0], control[1])
if DT - elapsed > 0: if DT - elapsed > 0:
time.sleep(DT - elapsed) time.sleep(DT - elapsed)