Hybridpath controller (#135)

* Set up python package, launch, lqr_config.yaml * Getting params from config file working * Added dampening D * WIP not working * Ivan stuff working in ROS2 * Implemented Ivan LOS LQR in ROS2 * Added los_guidance package * Added x_ref LOS calculation and Odometry * Added Odometry * WIP need to fix lqr * Fixed x_ref dimensions * Made package * WIP stuff * Vessel Simulator working, added giraffe * Updated giraffe message, added write np.array to file * Changed magic number to be like Ivan sim (still not work) * Edited points * Updated switching logic with line in front of endpoint, point subscriber in guidance, removed redundant linearization from asv_model, moved lqr control to state_cb in lqr_controller, fixed heading_ref in lqr * Fixed plotting points, removed ivansim * Add hybridpath_controller package files * Added adaptive backstepping controller and plotting * Added simulation function * Added hybridpath_guidance, updated hybridpath controller node * Seemingly working hybridpath_controller * Finished hybridpath simulator * Plotting works almost perfectly * Removed duplicate launch file, added READMEs, added docs and typehints, code quality stuff * Added default kappa value to AdaptiveBackstep * Added WIP unit test for hybridpath_controller control_law * Added working unit test for control law * Added s_ reset when updating path in guidance * Removed testing stuff and comments * Cleanup and made ready for testing. * Removed plotting function and example usage * Cleanup * Added return type for every function * Added type annotations * Added theory to README, and fixes to hybridpath * Updated README * Update README * Fixed README * Fix README * Fix README * Minor fixes * Minor name changes * Added theory in README * Fix link * Fix link * typo * Fix hybridpath guidance callback and readme * Removed plotting * Moved K1 and K2 to yaml * Merge branch 'hybridpath_to_CMakeLists' * Removed both simulators (to be readded at a later time) * Removed launch file for old LQR LOS simulator * Fixed asv_setup hybridpath nitpick * Fixed hp controller import parameters * Removed los_guidance * Removed old lqr_controller * Added otter inertia and damping matrices * Added coriolis matrix * Added source * Rewrite hybridpath * Fix node * Added update method for s * Fixes to hybridpath node * Initial test file * Fix node to publish vs and vss * Docstring for update_s method * Typo * Deleted lqr_controller package * Added source * Fixes to hybridpath * Added staticmethod odom_to_state * Fix odom_to_eta * Moved odom_to_eta to node * Added fixes to node * Fix update_s * Fixed import params from freya.yaml * Added mu tuning parameter to hp guidance * Minor fix * Added tests * Improvement * Fix ssa * Docstrings and minor fixes * Fixes * Space * Remove printing of s after final waypoint reached * Removed time_to_max_speed * Fix topic name * Added comments * Minor change * Added comments in the HybridPathGenerator class * Made u_desired an argument instead of yaml parameter * Added comments in HybrodPathSignals class * formatting --------- Co-authored-by: vortex <[email protected]> Co-authored-by: Aldokan <[email protected]> Co-authored-by: Andeshog <[email protected]> Co-authored-by: Alekskl <[email protected]>
vortexntnu · Apr 21, 2024 · 5f53f85 · 5f53f85
1 parent 5e5974d
commit 5f53f85
Show file tree

Hide file tree

Showing 20 changed files with 1,555 additions and 0 deletions.
diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml
@@ -21,6 +21,8 @@
       buoyancy: 0   # kg
       center_of_mass: [0, 0, 0]        # mm (x,y,z)
       center_of_buoyancy: [0, 0, 0]   # mm (x,y,z)
+      inertia_matrix: [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65] # from otter
+      damping_matrix_diag: [77.55, 162.5, 42.65] # from otter
 
 
     propulsion:

diff --git a/asv_setup/launch/hybridpath.launch.py b/asv_setup/launch/hybridpath.launch.py
@@ -0,0 +1,25 @@
+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    hybridpath_controller_launch = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('hybridpath_controller'), 'launch', 'hybridpath_controller.launch.py')
+        )
+    )
+
+    hybridpath_guidance_launch = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('hybridpath_guidance'), 'launch', 'hybridpath_guidance.launch.py')
+        )
+    )
+
+    # Return launch description
+    return LaunchDescription([
+        hybridpath_controller_launch,
+        hybridpath_guidance_launch
+    ])
diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(hybridpath_guidance)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  hybridpath_guidance/hybridpath_guidance_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(python_tests tests)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+endif()
+
+ament_package()
diff --git a/guidance/hybridpath_guidance/README.md b/guidance/hybridpath_guidance/README.md
@@ -0,0 +1,131 @@
+# Hybrid Path Guidance
+
+This package provides the implementation of hybrid path guidance for the Vortex ASV.
+
+## Usage
+
+To use the hybrid path guidance launch it using: `ros2 launch hybridpath_guidance hybridpath_guidance.launch`
+Or alternatively, run it together with the hybridpath controller using the launch file `hybridpath.launch.py` in asv_setup 
+
+To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered):
+
+`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"`
+
+## Configuration
+
+You can configure the behavior of the hybrid path guidance by modifying the parameters in the `config` directory. 
+
+## Theory
+### Path generation
+Given a set of $n + 1$ waypoints, the hybridpath generator will generate a $C^r$ path that goes through the waypoints. That is, the path is continuous in $r$ derivatives. Towards constructing the overall desired path $p_d(s)$, it is first divided into $n$ subpaths $p_{d,i}(s)$ for $i = 1, ... , n$ between the waypoints. Each of these is expressed as a polynomial in $s$ of a certain order. Then the expressions for the subpaths are concatenated at the waypoints to assemble the full path. The order of the polynomials must be chosen sufficiently high to ensure the overall path is sufficiently differentiable at the waypoints. Increasing the order of the polynomial increases the number of coefficients giving more degrees-of-freedom to satisfy these continuity constraints.
+
+The variable $s \in [0,n)$ is called the path variable, and it tells us exactly where we are on the path. For example, $s$ will be $0$ at the first waypoint, $1$ at the second, and so on. However, using the path parameter to take values $s \in [0,n)$ is unnecessary and can be numerically problematic. Instead, we can identify each path segment $p_i$ and parametrize it by $\theta \in [0,1)$. We define 
+```math
+i := \lfloor s \rfloor + 1
+``` 
+```math
+\theta = s - \lfloor s \rfloor \in [0,1)
+```
+
+to get the continuous map 
+```math 
+s \mapsto p_d(s)
+```
+
+Consider polynomials of order $k$, 
+```math
+x_{d,i}(\theta) = a_{k,i} \theta^k + ... + a_{1,i} \theta + a_{0,i}
+```
+```math
+ y_{d,i}(\theta) = b_{k,i} \theta^k + ... + b_{1,i} \theta + b_{0,i}
+```
+where the coefficients {$`a_{j,i}`$, $`b_{j,i}`$} must be determined. For each subpath there are $(k + 1)$ $\cdot$ $2$ unknowns so that there are $(k + 1)$ $\cdot$ $2n$ unknown coefficients in total to be determined for the full path. This is done by setting up and solving the linear system 
+```math
+A\phi = b \; \; \; \; \; \; \phi^T = [a^T, b^T]
+```
+for each path segment and solve them in a single operation as $` \phi = A^{-1}b`$.
+
+Assuming that the subpaths is to be connected at the waypoints, we use the following procedure to calculate the coefficients of each subpath:
+
+$C^0$ : Continuity at the waypoints gives for i: 
+```math
+x_{d,i}(0) = x_{i-1} \; \; \; \; \; \; y_{d,i}(0) = y_{i-1}
+```
+```math
+ x_{d,i}(1) = x_i  \; \; \; \; \; \; y_{d,i}(1) = y_i
+```
+
+$C^1$ : The slopes at the first and last waypoints are chosen as: 
+```math
+x^\theta_{d,i}(0) = x_1 - x_0 \; \; \; \; \; \; y^\theta_{d,1}(0) = y_1 - y_0
+```
+```math
+ x^\theta_{d,n}(1) = x_n - x_{n-1} \; \; \; \; \; \; y^\theta_{d,n}(1) = y_n - y_{n-1}
+``` 
+
+Here, $x^\theta$ represents the derivative of $x$ with respect to $\theta$.
+
+The slopes at the intermediate waypoints are chosen as: 
+```math
+ x^\theta_{d,i}(0) = \lambda (x_i - x_{i-2}) \; \; \; \; \; \; i = 2, ... , n
+```
+```math
+ y^\theta_{d,i}(0) = \lambda (y_i - y_{i-2}) \; \; \; \; \; \; i = 2, ... , n
+```
+```math
+ x^\theta_{d,i}(1) = \lambda (x_{i+1} - x_{i-1}) \; \; \; \; \; \; i = 1, ... , n-1
+```
+```math
+ y^\theta_{d,1}(1) = \lambda (y_{i+1} - y_{i-1}) \; \; \; \; \; \; i = 1, ... , n-1
+```
+where $\lambda > 0$ is a design constant. Choosing $\lambda > 0.5$ gives more rounded corners, choosing $\lambda < 0.5$ gives sharper corners and choosing $\lambda = 0.5$ makes the slope at waypoint $p_i$ be the average of pointing to $p_{i-1}$ and $p_{i+1}$
+
+$C^j$ : Setting derivatives of order $j = 2, 3, ... , k$ to zero for i:
+```math
+ x^{\theta^j}_{d,i}(0) = 0 \; \; \; \; \; \; y^{\theta^j}_{d,i}(0) = 0
+```
+```math
+x^{\theta^j}_{d,i}(1) = 0 \; \; \; \; \; \; y^{\theta^j}_{d,i}(1) = 0
+```
+This results in the hybrid parametrization of the path,
+```math
+\bar{p_d}(i,\theta) = \begin{bmatrix} x_{d,i}(\theta) \\ y_{d,i}(\theta) \end{bmatrix}
+```
+
+where $\theta \in [0,1)$. 
+
+If the differentiability requirement of the path is $C^r$, then the above equations up to $j=r$ gives $2(r+1) \cdot 2n$ equations to solve for $(k + 1) \cdot 2n$ unknown coefficients. As a result, the order $k$ of the polynomials must be 
+```math
+k = 2r + 1
+```
+
+### Guidance system
+So far we have defined the desired path 
+```math
+p_d(i,\theta) = \begin{bmatrix} x_d(i,\theta) \\ y_d(i,\theta) \end{bmatrix},\; \theta \in [0,1)
+```
+. Now we define the desired heading 
+```math
+\psi_d = atan2(y^\theta_d(i,\theta), x^\theta_d(i,\theta))
+```
+where $atan2(y,x)$ is the four-quadrant version of $arctan(y/x)$. As mentioned previously, the pair $(i,\theta)$ tells us which path segment $i$ we are on and where on the segment we are (given by $\theta$). Since we also need the first and second derivatives of the heading for the controller, we define them as:
+```math
+\psi^\theta_d(i,\theta) = \frac{x^\theta_d(i,\theta)y^{\theta^2}_d(i,\theta)-x^{\theta^2}_d(i,\theta)y^\theta_d(i,\theta)}{x^\theta_d(i,\theta)^2+y^\theta_d(i,\theta)^2}
+```
+
+Differentiating one more time yields:
+
+```math
+\psi^{\theta^2}_d(i,\theta) = \frac{x^\theta_d(i,\theta)y^{\theta^3}_d(i,\theta)-x^{\theta^3}_d(i,\theta)y^\theta_d(i,\theta)}{x^\theta_d(i,\theta)^2+y^\theta_d(i,\theta)^2} \\-2\frac{(x^\theta_d(i,\theta)y^{\theta^2}_d(i,\theta)-x^{\theta^2}_d(i,\theta)y^\theta_d(i,\theta))(x^\theta_d(i,\theta)x^{\theta^2}_d(i,\theta)-y^{\theta^2}_d(i,\theta)y^\theta_d(i,\theta))}{(x^\theta_d(i,\theta)^2+y^\theta_d(i,\theta)^2)^2}
+```
+
+Source: https://folk.ntnu.no/rskjetne/Publications/SkjetnePhDthesis_B5_compressed.pdf
+
+### Example plots
+
+These plots showcases the what the $r$ and $\lambda$ parameters does to the path.
+
+![Path Plots](https://drive.google.com/uc?export=download&id=1CE8kN1yJSjEgiCdWGQYgjdIfSkE40V1f)
+
+![Lambda Plots](https://drive.google.com/uc?export=download&id=1TtpIAwBRKcZhpuXAUEinpuqUhN-WKoX-)
+
diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml
@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    hybridpath_guidance:
+      lambda_val: 0.15 # Curvature constant
+      path_generator_order: 1 # Differentiability order
+      dt: 0.1 # Time step
+      mu: 0.03 # Tuning parameter 
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/__init__.py b/guidance/hybridpath_guidance/hybridpath_guidance/__init__.py