diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py index cd8cace8..d469577b 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py +++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py @@ -346,8 +346,11 @@ def get_position(self) -> list[float]: 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] @@ -369,6 +372,7 @@ def _compute_derivatives(self, theta: float, a_vec: np.ndarray, b_vec: np.ndarra list[float]: A list containing the derivatives of x and y. """ + # Calculate the derivatives of x and y using the coefficients for the polynomial for the given subpath theta_pow = theta ** np.arange(len(a_vec)) x_der = np.dot(a_vec, theta_pow) y_der = np.dot(b_vec, theta_pow) @@ -381,9 +385,14 @@ def get_derivatives(self) -> list[list[float]]: Returns: list: A list of computed derivatives. """ + # Initialize the list to store the derivatives derivatives = [] + + # Get the index and theta. See README for what they represent. index = int(self.s) + 1 theta = self.s - (index - 1) + + # Loop through the order of the polynomial and calculate the derivatives for each order for k in range(1, self.path.Order + 1): a_vec = self.path.coeff.a_der[k - 1][index - 1] b_vec = self.path.coeff.b_der[k - 1][index - 1] @@ -398,7 +407,10 @@ def get_heading(self) -> float: Returns: float: The heading of the object. """ + # Get the first derivative of the position p_der = self.get_derivatives()[0] + + # Calculate the heading psi = np.arctan2(p_der[1], p_der[0]) return psi @@ -409,8 +421,11 @@ def get_heading_derivative(self) -> float: Returns: float: The derivative of the heading. """ + # Get the first and second derivatives of the position p_der = self.get_derivatives()[0] p_dder = self.get_derivatives()[1] + + # Calculate the derivative of the heading psi_der = (p_der[0] * p_dder[1] - p_der[1] * p_dder[0]) / (p_der[0]**2 + p_der[1]**2) return psi_der @@ -421,9 +436,12 @@ def get_heading_second_derivative(self) -> float: Returns: float: The second derivative of the heading. """ + # Get the first, second and third derivatives of the position p_der = self.get_derivatives()[0] p_dder = self.get_derivatives()[1] p_ddder = self.get_derivatives()[2] + + # Calculate the second derivative of the heading 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 @@ -437,7 +455,10 @@ def get_vs(self) -> float: Returns: float: The reference velocity. """ + # Get the first derivative of the position p_der = self.get_derivatives()[0] + + # Calculate the reference velocity v_s = self.u_desired / np.linalg.norm(p_der) return v_s @@ -451,8 +472,11 @@ def get_vs_derivative(self) -> float: Returns: float: The derivative of the reference velocity. """ + # Get the first and second derivatives of the position p_der = self.get_derivatives()[0] 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) return v_s_s @@ -468,11 +492,19 @@ def get_w(self, mu: float, eta: np.ndarray) -> float: Returns: float: The calculated value of w. """ + # Get the desired position and heading eta_d = np.array([self.get_position()[0], self.get_position()[1], self.get_heading()]) + + # Compute the error error = eta - eta_d + + # Get the first derivative of the position and the heading p_der = self.get_derivatives()[0] psi_der = self.get_heading_derivative() eta_d_s = np.array([p_der[0], p_der[1], psi_der]) + + # Compute the unit-tangent gradient w = (mu / np.linalg.norm(eta_d_s)) * np.dot(eta_d_s, error) + return w \ No newline at end of file