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

remove Params class

master
mcarfagno 2023-10-23 21:02:41 +01:00
parent 82a4b6ab2d
commit abfc222908
3 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
mpc_pybullet_demo/cvxpy_mpc/utils.py
View File

@ -59,7 +59,7 @@ def get_nn_idx(state, path):
return target_idx return target_idx
def get_ref_trajectory(state, path, target_v, double T, double DT): def get_ref_trajectory(state, path, target_v, T, 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:
@ -78,7 +78,7 @@ def get_ref_trajectory(state, path, target_v, double T, double DT):
cdist = np.clip(cdist, cdist[0], cdist[-1]) cdist = np.clip(cdist, cdist[0], cdist[-1])
start_dist = cdist[ind] start_dist = cdist[ind]
interp_points = [d * P.DT * target_v + start_dist for d in range(1, K + 1)] interp_points = [d * DT * target_v + start_dist for d in range(1, K + 1)]
xref[0, :] = np.interp(interp_points, cdist, path[0, :]) xref[0, :] = np.interp(interp_points, cdist, path[0, :])
xref[1, :] = np.interp(interp_points, cdist, path[1, :]) xref[1, :] = np.interp(interp_points, cdist, path[1, :])
xref[2, :] = target_v xref[2, :] = target_v

2
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -112,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, TARGET_VEL) target = get_ref_trajectory(self.state, self.path, TARGET_VEL, T, DT)
x_mpc, u_mpc = self.mpc.step( x_mpc, u_mpc = self.mpc.step(
curr_state, curr_state,

4
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -224,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(VehicleModel(), Q, Qf, R, P) mpc = MPC(VehicleModel(), T, DT, Q, Qf, R, P)
x_history = [] x_history = []
y_history = [] y_history = []
@ -250,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, TARGET_VEL) target = get_ref_trajectory(state, path, TARGET_VEL, T, DT)
# 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