get ref trajectory purely from guide path, instead of trying to use velocities

guided_mrmp/controllers/utils.py

@@ -99,50 +99,27 @@ def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
     """
     K = int(T / DT)
 
-    xref = np.zeros((3, K))  # Reference trajectory for [x, y, theta]
-
-    path_distances = [0]
-    for i in range(1, len(path)):
-        dist = np.linalg.norm(np.array(path[i]) - np.array(path[i-1]))
-        path_distances.append(path_distances[-1] + dist)
-    
     # Find the last visited point
     last_visited_idx = 0 if path_visited_points == [] else path_visited_points[-1]
-    
+
     # Find the spatially closest point after the last visited point
     next_ind = last_visited_idx + 1
+    path_visited_points.append(next_ind)
 
+    K = min(K, len(path[0]) - next_ind)
+    xref = np.zeros((3, K))  # Reference trajectory for [x, y, theta]
 
-    ind = next_ind
-    # ind = get_nn_idx(state, path, path_visited_points)
-    # min_dist = float('inf')
-    # for i in range(last_visited_idx+2, len(path)):
-    #     dist = np.linalg.norm(np.array(path[i][:2]) - np.array(state[:2]))
-    #     if dist < min_dist:
-    #         min_dist = dist
-    #         ind = i 
-
-    path_visited_points.append(ind)
-
-    # calculate the cumulative distance along the path
-    cdist = np.append([0.0], np.cumsum(np.hypot(np.diff(path[0, :]), np.diff(path[1, :]))))
-    cdist = np.clip(cdist, cdist[0], cdist[-1])
-
-    # determine where we want the robot to be at each time step 
-    start_dist = cdist[ind]
-    interp_points = [d * DT * target_v + start_dist for d in range(1, K + 1)]
+    # return the next k points on the path
+    xref[0,:] = path[0, next_ind:next_ind+K]
+    xref[1,:] = path[1, next_ind:next_ind+K]
+    xref[2,:] = path[2, next_ind:next_ind+K]
 
-    # interpolate between these points to get the reference trajectory
-    xref[0, :] = np.interp(interp_points, cdist, path[0, :])
-    xref[1, :] = np.interp(interp_points, cdist, path[1, :])
-    xref[2, :] = np.interp(interp_points, cdist, path[2, :])
-    
     # Transform to ego frame
     dx = xref[0, :] - state[0]
     dy = xref[1, :] - state[1]
     xref[0, :] = dx * np.cos(-state[2]) - dy * np.sin(-state[2])  # X
     xref[1, :] = dy * np.cos(-state[2]) + dx * np.sin(-state[2])  # Y
-    xref[2, :] = path[2, ind] - state[2]  # Theta
+    xref[2, :] = path[2, next_ind] - state[2]  # Theta
 
     # Normalize the angles
     xref[2, :] = (xref[2, :] + np.pi) % (2.0 * np.pi) - np.pi
@@ -150,53 +127,3 @@ def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
 
     return xref, path_visited_points
 
-
-# def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
-#     """
-#     Generates a reference trajectory for the Roomba.
-
-#     Args:
-#         state (array-like): Current state [x, y, theta]
-#         path (ndarray): Path points [x, y, theta] in the global frame
-#         path_visited_points (array-like): Visited path points [[x, y], [x, y], ...]
-#         target_v (float): Desired speed
-#         T (float): Control horizon duration
-#         DT (float): Control horizon time-step
-
-#     Returns:
-#         ndarray: Reference trajectory [x_k, y_k, theta_k] in the ego frame
-#     """
-#     K = int(T / DT)
-
-#     xref = np.zeros((3, K))  # Reference trajectory for [x, y, theta]
-
-#     # find the nearest path point to the current state
-#     ind = get_nn_idx(state, path, path_visited_points)
-
-#     path_visited_points.append([path[0, ind], path[1, ind]])
-
-#     # calculate the cumulative distance along the path
-#     cdist = np.append([0.0], np.cumsum(np.hypot(np.diff(path[0, :]), np.diff(path[1, :]))))
-#     cdist = np.clip(cdist, cdist[0], cdist[-1])
-
-#     # determine where we want the robot to be at each time step 
-#     start_dist = cdist[ind]
-#     interp_points = [d * DT * target_v + start_dist for d in range(1, K + 1)]
-
-#     # interpolate between these points to get the reference trajectory
-#     xref[0, :] = np.interp(interp_points, cdist, path[0, :])
-#     xref[1, :] = np.interp(interp_points, cdist, path[1, :])
-#     xref[2, :] = np.interp(interp_points, cdist, path[2, :])
-    
-#     # Transform to ego frame
-#     dx = xref[0, :] - state[0]
-#     dy = xref[1, :] - state[1]
-#     xref[0, :] = dx * np.cos(-state[2]) - dy * np.sin(-state[2])  # X
-#     xref[1, :] = dy * np.cos(-state[2]) + dx * np.sin(-state[2])  # Y
-#     xref[2, :] = path[2, ind] - state[2]  # Theta
-
-#     # Normalize the angles
-#     xref[2, :] = (xref[2, :] + np.pi) % (2.0 * np.pi) - np.pi
-#     xref[2, :] = fix_angle_reference(xref[2, :], xref[2, 0])
-
-#     return xref, path_visited_points