Skip to content

Commit

Permalink
Hybridpath guidance fix (#185)
Browse files Browse the repository at this point in the history 
* Fixed issue with u_desired in get_vs, get_vs_derivative methods.

* Removed u_desired as a class variable

* Added default value for u_desired to be chosen by the mission_planner

* Removed unnecessary int conversion of waypoints.

* formatting

---------

Co-authored-by: Alekskl <[email protected]>
  • Loading branch information
@Andeshog @alekskl01
Andeshog and alekskl01 authored Apr 28, 2024
1 parent dbb8004 commit 539deef
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 80 deletions.
157 changes: 78 additions & 79 deletions guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py
View file
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
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

0 comments on commit 539deef

Please sign in to comment.