Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Hybridpath guidance fix #185

Merged
merged 5 commits into from
Apr 28, 2024
Merged

Hybridpath guidance fix #185

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
afa4f2e
Fixed issue with u_desired in get_vs, get_vs_derivative methods.
Andeshog Apr 22, 2024
b140a79
Removed u_desired as a class variable
Andeshog Apr 22, 2024
1dd7035
Added default value for u_desired to be chosen by the mission_planner
Andeshog Apr 22, 2024
aaf9cd9
Removed unnecessary int conversion of waypoints.
Andeshog Apr 25, 2024
e3bc3e3
formatting
alekskl01 Apr 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
157 changes: 78 additions & 79 deletions guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]])
Expand Down Expand Up @@ -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:
"""
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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:
"""
Expand All @@ -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]:
"""
Expand Down Expand Up @@ -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.

Expand All @@ -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.

Expand All @@ -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:
Expand Down
3 changes: 2 additions & 1 deletion guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
Loading