Merge pull request #7 from mcarfagno/use-numpy-interpolation

rework get_reference_trajectory

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,

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

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

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
+    Reinterpolate the trajectory to get a set N desired target states
-    modified reference in robot frame
+    :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))
+    xref = np.zeros((P.N, K))
-    ncourse = path.shape[1]
     ind = get_nn_idx(state, path)
-    dx = path[0, ind] - state[0]
+
-    dy = path[1, ind] - state[1]
+    cdist = np.append(
-    xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])  # X
+        [0.0], np.cumsum(np.hypot(np.diff(path[0, :].T), np.diff(path[1, :]).T))
-    xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])  # Y
+    )
-    xref[2, 0] = target_v  # V
+    cdist = np.clip(cdist, cdist[0], cdist[-1])
-    xref[3, 0] = normalize_angle(path[2, ind] - state[3])  # Theta
+
-    dref[0, 0] = 0.0  # Steer operational point should be 0
+    start_dist = cdist[ind]
-    travel = 0.0
+    interp_points = [d * P.DT * target_v + start_dist for d in range(1, K + 1)]
-    dl = np.hypot(path[0, 1] - path[0, 0], path[1, 1] - path[1, 0])
+    xref[0, :] = np.interp(interp_points, cdist, path[0, :])
-    for i in range(1, K + 1):
+    xref[1, :] = np.interp(interp_points, cdist, path[1, :])
-        travel += abs(target_v) * P.DT
+    xref[2, :] = target_v
-        dind = int(round(travel / dl))
+    xref[3, :] = np.interp(interp_points, cdist, path[2, :])
-        if (ind + dind) < ncourse:
+
-            dx = path[0, ind + dind] - state[0]
+    # points where the vehicle is at the end of trajectory
-            dy = path[1, ind + dind] - state[1]
+    xref_cdist = np.interp(interp_points, cdist, cdist)
-            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
+    stop_idx = np.where(xref_cdist == cdist[-1])
-            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
+    xref[2, stop_idx] = 0.0
-            xref[2, i] = target_v  # sp[ind + dind]
+
-            xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])
+    # transform in car ego frame
-            dref[0, i] = 0.0
+    dx = xref[0, :] - state[0]
-        else:
+    dy = xref[1, :] - state[1]
-            dx = path[0, ncourse - 1] - state[0]
+    xref[0, :] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])  # X
-            dy = path[1, ncourse - 1] - state[1]
+    xref[1, :] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])  # Y
-            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
+    xref[3, :] = normalize_angle(path[2, ind] - state[3])  # Theta
-            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
+    return xref
-            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