Skip to content
Snippets Groups Projects
Commit f73f460e authored by rachelmoan's avatar rachelmoan
Browse files

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

parent fcc10444
No related branches found
No related tags found
No related merge requests found
......@@ -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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment