From abfc222908cec2a1eeb63c4fbd2720f0e5b34ac9 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Mon, 23 Oct 2023 21:02:41 +0100 Subject: [PATCH] remove Params class --- mpc_pybullet_demo/cvxpy_mpc/utils.py | 4 ++-- mpc_pybullet_demo/mpc_demo_nosim.py | 2 +- mpc_pybullet_demo/mpc_demo_pybullet.py | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/mpc_pybullet_demo/cvxpy_mpc/utils.py b/mpc_pybullet_demo/cvxpy_mpc/utils.py index 90ed926..928bb1a 100755 --- a/mpc_pybullet_demo/cvxpy_mpc/utils.py +++ b/mpc_pybullet_demo/cvxpy_mpc/utils.py @@ -59,7 +59,7 @@ def get_nn_idx(state, path): 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 :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]) 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[1, :] = np.interp(interp_points, cdist, path[1, :]) xref[2, :] = target_v diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index 6f263ab..a1ea3af 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -112,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, TARGET_VEL) + target = get_ref_trajectory(self.state, self.path, TARGET_VEL, T, DT) x_mpc, u_mpc = self.mpc.step( curr_state, diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 58d74c8..aa73cd2 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -224,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(VehicleModel(), Q, Qf, R, P) + mpc = MPC(VehicleModel(), T, DT, Q, Qf, R, P) x_history = [] y_history = [] @@ -250,7 +250,7 @@ def run_sim(): # Get Reference_traj # 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: # so x, y, yaw are 0.0, but speed is the same