diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index d469577b..9f2a6155 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -67,7 +67,7 @@ class HybridPathGenerator: """ def __init__(self, WP: list[Point], r: int, lambda_val: float): # Convert the waypoints to a numpy array - WP_arr = np.array([[int(wp.x), int(wp.y)] for wp in WP]) + WP_arr = np.array([[wp.x, wp.y] for wp in WP]) if len(WP_arr) == 2: # The generator must have at least 3 waypoints to work self.WP = np.array([WP_arr[0], [(WP_arr[0][0] + WP_arr[1][0])/2, (WP_arr[0][1] + WP_arr[1][1])/2], WP_arr[1]]) @@ -124,30 +124,30 @@ def _construct_A_matrix(self) -> np.ndarray: return A def _calculate_subpath_coeffs(self, j: int) -> tuple[np.ndarray, np.ndarray]: - """ - Calculate the subpath coefficients for a given index. + """ + Calculate the subpath coefficients for a given index. - Parameters: - j (int): The index of the subpath. + Parameters: + j (int): The index of the subpath. - Returns: - tuple[np.ndarray, np.ndarray]: A tuple containing two numpy arrays representing the subpath coefficients. - """ - order_plus_one = self.order + 1 - N = self.path.NumSubpaths + Returns: + tuple[np.ndarray, np.ndarray]: A tuple containing two numpy arrays representing the subpath coefficients. + """ + order_plus_one = self.order + 1 + N = self.path.NumSubpaths - # Initialize the coefficent arrays - ax, bx = np.zeros(order_plus_one), np.zeros(order_plus_one) + # Initialize the coefficent arrays + ax, bx = np.zeros(order_plus_one), np.zeros(order_plus_one) - # Set the first two coefficients for each dimension directly from waypoints. See README under C^0 continuity. - ax[:2] = self.WP[j:j+2, 0] - bx[:2] = self.WP[j:j+2, 1] + # Set the first two coefficients for each dimension directly from waypoints. See README under C^0 continuity. + ax[:2] = self.WP[j:j+2, 0] + bx[:2] = self.WP[j:j+2, 1] - # Calculate the coefficients for subpaths when the order is greater than 2 i.e. C^1 continuity. - if self.order > 2: - self._calculate_subpaths_coeffs_if_ord_greater_than_2(ax, bx, j, N) + # Calculate the coefficients for subpaths when the order is greater than 2 i.e. C^1 continuity. + if self.order > 2: + self._calculate_subpaths_coeffs_if_ord_greater_than_2(ax, bx, j, N) - return ax, bx + return ax, bx def _calculate_subpaths_coeffs_if_ord_greater_than_2(self, ax: np.ndarray, bx: np.ndarray, j: int, N: int) -> None: """ @@ -243,42 +243,42 @@ def _append_derivatives(self, k: int, a: np.ndarray, b: np.ndarray) -> None: self.path.coeff.b_der.append([b]) def _calculate_subpaths(self) -> None: - """ - Calculates the subpaths of the hybrid path. - - This method constructs the A matrix, sets it as the A matrix of the path's linear system, - and calculates the coefficients for each subpath. It then solves the linear system for each - subpath to obtain the coefficients a and b. The derivatives of a and b are also calculated - and appended to the path's derivatives. - - Returns: - None - """ - - # Construct the A matrix - A = self._construct_A_matrix() - self.path.LinSys.A = A - - # Loop through each subpath - N = self.path.NumSubpaths - for j in range(N): - # Calculate the subpath coefficients - ax, bx = self._calculate_subpath_coeffs(j) - self.path.LinSys.bx.append(ax) - self.path.LinSys.by.append(bx) - - # Solve the linear system for the subpath - a_vec, b_vec = self._solve_linear_system(A, ax, bx) - self.path.coeff.a.append(a_vec) - self.path.coeff.b.append(b_vec) - - # Calculate the derivatives of the coefficients - a_derivatives = self._calculate_derivatives(a_vec) - b_derivatives = self._calculate_derivatives(b_vec) - - # Append the derivatives to the path object - for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)): - self._append_derivatives(k, a, b) + """ + Calculates the subpaths of the hybrid path. + + This method constructs the A matrix, sets it as the A matrix of the path's linear system, + and calculates the coefficients for each subpath. It then solves the linear system for each + subpath to obtain the coefficients a and b. The derivatives of a and b are also calculated + and appended to the path's derivatives. + + Returns: + None + """ + + # Construct the A matrix + A = self._construct_A_matrix() + self.path.LinSys.A = A + + # Loop through each subpath + N = self.path.NumSubpaths + for j in range(N): + # Calculate the subpath coefficients + ax, bx = self._calculate_subpath_coeffs(j) + self.path.LinSys.bx.append(ax) + self.path.LinSys.by.append(bx) + + # Solve the linear system for the subpath + a_vec, b_vec = self._solve_linear_system(A, ax, bx) + self.path.coeff.a.append(a_vec) + self.path.coeff.b.append(b_vec) + + # Calculate the derivatives of the coefficients + a_derivatives = self._calculate_derivatives(a_vec) + b_derivatives = self._calculate_derivatives(b_vec) + + # Append the derivatives to the path object + for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)): + self._append_derivatives(k, a, b) @staticmethod def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> float: @@ -314,12 +314,11 @@ class HybridPathSignals: Path (Path): The path object. s (float): The path parameter. """ - def __init__(self, path: Path, s: float, u_desired: float = 0.5): + def __init__(self, path: Path, s: float): if not isinstance(path, Path): raise TypeError("path must be an instance of Path") self.path = path self.s = self._clamp_s(s, self.path.NumSubpaths) - self.u_desired = u_desired def _clamp_s(self, s: float, num_subpaths: int) -> float: """ @@ -340,24 +339,24 @@ def _clamp_s(self, s: float, num_subpaths: int) -> float: def get_position(self) -> list[float]: - """ - Get the position of the object along the path. - - Returns: - list[float]: The x and y coordinates of the object's position. - """ - # Get the index and theta. See README for what they represent. - index = int(self.s) + 1 - theta = self.s - (index - 1) - - # Calculate the position using the coefficients for the polynomial for the given subpath - theta_pow = theta ** np.arange(self.path.Order + 1) - a = self.path.coeff.a[index - 1] - b = self.path.coeff.b[index - 1] - x_pos = np.dot(a, theta_pow) - y_pos = np.dot(b, theta_pow) - - return [x_pos, y_pos] + """ + Get the position of the object along the path. + + Returns: + list[float]: The x and y coordinates of the object's position. + """ + # Get the index and theta. See README for what they represent. + index = int(self.s) + 1 + theta = self.s - (index - 1) + + # Calculate the position using the coefficients for the polynomial for the given subpath + theta_pow = theta ** np.arange(self.path.Order + 1) + a = self.path.coeff.a[index - 1] + b = self.path.coeff.b[index - 1] + x_pos = np.dot(a, theta_pow) + y_pos = np.dot(b, theta_pow) + + return [x_pos, y_pos] def _compute_derivatives(self, theta: float, a_vec: np.ndarray, b_vec: np.ndarray) -> list[float]: """ @@ -445,7 +444,7 @@ def get_heading_second_derivative(self) -> float: psi_dder = (p_der[0] * p_ddder[1] - p_der[1] * p_ddder[0]) / (p_der[0]**2 + p_der[1]**2) - 2 * (p_der[0] * p_dder[1] - p_dder[0] * p_der[1]) * (p_der[0] * p_dder[0] - p_dder[1] * p_der[0]) / ((p_der[0]**2 + p_der[1]**2)**2) return psi_dder - def get_vs(self) -> float: + def get_vs(self, u_desired) -> float: """ Calculate the reference velocity. @@ -459,10 +458,10 @@ def get_vs(self) -> float: p_der = self.get_derivatives()[0] # Calculate the reference velocity - v_s = self.u_desired / np.linalg.norm(p_der) + v_s = u_desired / np.linalg.norm(p_der) return v_s - def get_vs_derivative(self) -> float: + def get_vs_derivative(self, u_desired) -> float: """ Calculate the derivative of the reference velocity. @@ -477,7 +476,7 @@ def get_vs_derivative(self) -> float: p_dder = self.get_derivatives()[1] # Calculate the derivative of the reference velocity - v_s_s = -self.u_desired * (np.dot(p_der, p_dder)) / (np.sqrt(p_der[0]**2 + p_der[1]**2)**3) + v_s_s = -u_desired * (np.dot(p_der, p_dder)) / (np.sqrt(p_der[0]**2 + p_der[1]**2)**3) return v_s_s def get_w(self, mu: float, eta: np.ndarray) -> float: diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index 94ceb66b..36255f0d 100755 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py @@ -33,6 +33,8 @@ def __init__(self): self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value self.eta = np.zeros(3) + self.u_desired = 0.25 # Desired velocity + # Flags for logging self.waypoints_received = False self.waiting_message_printed = False @@ -53,7 +55,6 @@ def waypoint_callback(self, request, response): self.s = 0 signals = HybridPathSignals(self.path, self.s) - self.u_desired = signals.u_desired self.w = signals.get_w(self.mu, self.eta) response.success = True