diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index 80739e9..03978be 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -23,10 +23,10 @@ params = mpcpy.Params() # Params VEL = 1.0 # m/s + # Classes class MPCSim: def __init__(self): - # State for the robot mathematical model [x,y,heading] self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H]) @@ -114,7 +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) + target = mpcpy.get_ref_trajectory(self.state, self.path, VEL) x_mpc, u_mpc = self.mpc.optimize_linearized_model( A, diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 01ba2e5..b3df682 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -247,7 +247,7 @@ def run_sim(): # Get Reference_traj # NOTE: inputs are in world frame - target, _ = mpcpy.get_ref_trajectory(state, path, params.TARGET_SPEED) + target = mpcpy.get_ref_trajectory(state, path, params.TARGET_SPEED) # for MPC base link frame is used: # so x, y, yaw are 0.0, but speed is the same diff --git a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py index bfa03de..ca19801 100755 --- a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py +++ b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py @@ -16,6 +16,9 @@ P = Params() def get_linear_model_matrices(x_bar, u_bar): """ Computes the LTI approximated state space model x' = Ax + Bu + C + :param x_bar: + :param u_bar: + :return: """ x = x_bar[0] @@ -58,7 +61,12 @@ def get_linear_model_matrices(x_bar, u_bar): class MPC: def __init__(self, state_cost, final_state_cost, input_cost, input_rate_cost): - """ """ + """ + :param state_cost: + :param final_state_cost: + :param input_cost: + :param input_rate_cost: + """ self.nx = P.N # number of state vars self.nu = P.M # umber of input/control vars diff --git a/mpc_pybullet_demo/mpcpy/utils.py b/mpc_pybullet_demo/mpcpy/utils.py index a12dad7..bc5c697 100755 --- a/mpc_pybullet_demo/mpcpy/utils.py +++ b/mpc_pybullet_demo/mpcpy/utils.py @@ -8,6 +8,10 @@ P = Params() def compute_path_from_wp(start_xp, start_yp, step=0.1): """ Computes a reference path given a set of waypoints + :param start_xp: + :param start_yp: + :param step: + :return: """ final_xp = [] final_yp = [] @@ -34,6 +38,9 @@ def compute_path_from_wp(start_xp, start_yp, step=0.1): def get_nn_idx(state, path): """ Computes the index of the waypoint closest to vehicle + :param state: + :param path: + :return: """ dx = state[0] - path[0, :] dy = state[1] - path[1, :] @@ -58,6 +65,8 @@ def get_nn_idx(state, path): def normalize_angle(angle): """ Normalize an angle to [-pi, pi] + :param angle: + :return: """ while angle > np.pi: angle -= 2.0 * np.pi @@ -68,40 +77,38 @@ def normalize_angle(angle): def get_ref_trajectory(state, path, target_v): """ - For each step in the time horizon - modified reference in robot frame + Reinterpolate the trajectory to get a set N desired target states + :param state: + :param path: + :param target_v: + :return: """ K = int(P.T / P.DT) - xref = np.zeros((P.N, K + 1)) - dref = np.zeros((1, K + 1)) - ncourse = path.shape[1] + + xref = np.zeros((P.N, K)) ind = get_nn_idx(state, path) - dx = path[0, ind] - state[0] - dy = path[1, ind] - state[1] - xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X - xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y - xref[2, 0] = target_v # V - 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 - 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: - dx = path[0, ind + dind] - state[0] - dy = path[1, ind + dind] - state[1] - xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) - xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) - xref[2, i] = target_v # sp[ind + dind] - xref[3, i] = normalize_angle(path[2, ind + dind] - state[3]) - dref[0, i] = 0.0 - else: - dx = path[0, ncourse - 1] - state[0] - dy = path[1, ncourse - 1] - state[1] - xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) - xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) - xref[2, i] = 0.0 # stop? #sp[ncourse - 1] - xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3]) - dref[0, i] = 0.0 - return xref, dref + + cdist = np.append( + [0.0], np.cumsum(np.hypot(np.diff(path[0, :].T), np.diff(path[1, :]).T)) + ) + 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)] + xref[0, :] = np.interp(interp_points, cdist, path[0, :]) + xref[1, :] = np.interp(interp_points, cdist, path[1, :]) + xref[2, :] = target_v + xref[3, :] = np.interp(interp_points, cdist, path[2, :]) + + # points where the vehicle is at the end of trajectory + xref_cdist = np.interp(interp_points, cdist, cdist) + stop_idx = np.where(xref_cdist == cdist[-1]) + xref[2, stop_idx] = 0.0 + + # transform in car ego frame + dx = xref[0, :] - state[0] + dy = xref[1, :] - state[1] + xref[0, :] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X + xref[1, :] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y + xref[3, :] = normalize_angle(path[2, ind] - state[3]) # Theta + return xref