<feat> implemented Liner Quadratic Regulator #500
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Andeshog
requested changes
Nov 10, 2024
| endif() | ||
|
|
||
| # find dependencies | ||
| find_package(ament_cmake REQUIRED) |
There was a problem hiding this comment.
Are you sure you need ament_cmake here?
| find_package(rclpy REQUIRED) | ||
| find_package(nav_msgs REQUIRED) | ||
| find_package(geometry_msgs REQUIRED) | ||
| find_package(rclcpp REQUIRED) |
There was a problem hiding this comment.
No cpp code, so no need to find rclcpp
Comment on lines
+4
to
+6
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-Wall -Wextra -Wpedantic) | ||
| endif() |
There was a problem hiding this comment.
This is not necessary for python code, only for cpp
Comment on lines
+4
to
+7
| #Path parameters | ||
| odom_path: /nucleus/odom | ||
| guidance_path: /guidance/los | ||
| thrust_path: /thrust/wrench_input |
| # Define the node | ||
| velocity_controller_node = Node( | ||
| package="velocity_controller_lqr", | ||
| executable="velocity_controller_lqr_node.py", # Ensure this matches your Python file name |
Comment on lines
+104
to
+108
| if angle > np.pi: | ||
| angle -= 2 * np.pi | ||
| elif angle < -np.pi: | ||
| angle += 2 * np.pi | ||
| return angle |
There was a problem hiding this comment.
why not use Fossen's ssa?
return (angle + np.pi) % (2 * np.pi) - np.pi
| angle += 2 * np.pi | ||
| return angle | ||
|
|
||
| def saturate(self, value: float, windup: bool, limit: float) -> float: |
Comment on lines
+118
to
+119
| Returns: | ||
| value: float: saturated value |
Comment on lines
+212
to
+218
| surge_error = ( | ||
| guidance_values[0] - states[0] | ||
| ) # Surge error no need for angle wrapping | ||
| pitch_error = self.ssa( | ||
| guidance_values[1] - states[1] | ||
| ) # Apply ssa to pitch error | ||
| yaw_error = self.ssa(guidance_values[2] - states[2]) # Apply ssa to yaw error |
Comment on lines
+233
to
+235
| -surge_error, | ||
| -pitch_error, | ||
| -yaw_error, |
There was a problem hiding this comment.
... these can be positive
There was a problem hiding this comment.
if i flip, then the integral values become negative. Pick your poison.
Talhanc
requested changes
Nov 10, 2024
|
|
||
| ament_python_install_package(${PROJECT_NAME}) | ||
|
|
||
| #install python scripts |
| if __name__ == "__main__": | ||
| main() | ||
|
|
||
| # Anders er goated |
| controller is not None | ||
| ) # Simple test to ensure the controller initializes | ||
|
|
||
| def test_ssa(self): |
There was a problem hiding this comment.
I think you should have doc-string to explain the tests
Comment on lines
+2
to
+7
| import math as math | ||
|
|
||
| import numpy as np | ||
|
|
||
| import control as ct | ||
|
|
Comment on lines
+259
to
+289
|
|
||
|
|
||
| # .--------------. | ||
| # .---' o . `---. | ||
| # .-' . O . . `-. | ||
| # .-' @@@@@@ . `-. | ||
| # .'@@ @@@@@@@@@@@ @@@@@@@ . `. | ||
| # .'@@@ @@@@@@@@@@@@@@ @@@@@@@@@ `. | ||
| # /@@@ o @@@@@@@@@@@@@@ @@@@@@@@@ O \ | ||
| # / @@@@@@@@@@@@@@ @ @@@@@@@@@ @@ . \ | ||
| # /@ o @@@@@@@@@@@ . @@ @@@@@@@@@@@ @@ \ | ||
| # /@@@ . @@@@@@ o @ @@@@@@@@@@@@@ o @@@@ \ | ||
| # /@@@@@ @ . @@@@@@@@@@@@@@ @@@@@ \ | ||
| # |@@@@@ O `.-./ . . @@@@@@@@@@@@@ @@@ | | ||
| # / @@@@@ --`-' o @@@@@@@@@@@ @@@ . \ | ||
| # |@ @@@@ . @ @ ` @ @@ . @@@@@@ | | ||
| # | @@ o @@ . @@@@@@ | | ||
| # | . @ @ @ o @@ o @@@@@@. | | ||
| # \ @ @ @ .-. @@@@ @@@ / | ||
| # | @ @ @ `-' . @@@@ . . | | ||
| # \ . o @ @@@@ . @@ . . / | ||
| # \ @@@ @@@@@@ . o / | ||
| # \ @@@@@ @@\@@ / O . / | ||
| # \ o @@@ \ \ / __ . . .--. / | ||
| # \ . . \.-.--- `--' / | ||
| # `. `-' . .' | ||
| # `. o / | ` O . .' | ||
| # `-. / | o .-' | ||
| # `-. . . .-' | ||
| # `---. . .---' | ||
| # `--------------' |
There was a problem hiding this comment.
I want a millennium falcon
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣠⢴⣤⣠⣤⢤⣂⣔⣲⣒⣖⡺⢯⣝⡿⣿⣿⣿⣷⣶⣶⣢⢦⣤⣄⣀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣯⣿⣾⣿⣶⣺⣯⡿⣓⣽⢞⡸⣻⢏⣋⠌⣛⣭⣿⢟⣿⣛⣿⢷⣿⣿⣿⡟⣿⣻⣵⣲⢢⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⢀⣀⣤⡴⠲⠶⢦⠤⢤⡤⠿⠿⠿⠿⣿⣽⣿⣽⣷⣿⢽⣾⣷⣭⡞⣩⡐⠏⡋⣽⡬⣭⠏⢍⣞⢿⣽⣿⣷⢿⣿⣿⡿⠾⣿⢶⡶⣤⣀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⣤⣖⠯⡙⢢⡁⠄⢢⡤⢠⢀⣸⠀⣄⡠⣀⣀⣦⡄⣠⢀⡃⠽⣽⠬⠍⣿⣿⣤⣥⣷⣿⣿⣿⣾⡍⠛⢌⠈⢫⣍⢋⣍⡁⠹⢍⠈⣳⢎⠴⠟⠻⢧⣄⠀⠀⠀⠀⠀
⠀⠀⣠⣾⣿⣿⣿⣯⡔⠆⠠⠈⣿⣿⠾⡾⠿⣶⠿⡟⣻⡛⣭⢙⠍⢩⠭⢩⡳⢮⡿⡿⢿⣿⣿⣧⣿⣽⣮⣤⣥⣩⣶⣟⣻⠧⣻⠶⢤⢰⡱⣬⣤⣌⣑⠞⠲⠓⠭⡀⠀⠀⠀
⠀⠐⣿⣿⣿⢟⡋⢈⢤⣤⣷⢿⣿⠒⢜⣁⡱⡧⢿⣹⣷⣿⡿⣷⠌⣢⣟⢱⢽⡨⢊⠴⡉⢉⡿⣯⢿⣏⢹⠏⣯⣩⣙⠾⢿⣳⣶⢻⣟⣿⠧⢀⠋⠟⣿⡧⠠⠄⡤⠈⢠⠀⠀
⠀⠀⠘⠻⠿⠶⠶⠿⠛⣹⣟⡞⠸⣬⠐⠙⢍⢉⣔⠪⠟⡛⠫⢵⣾⣣⣼⣽⢈⠔⡅⣜⡽⢯⢞⡕⡠⠓⣡⢚⣷⣷⣿⣳⡄⠢⠉⠛⢿⣲⢿⣶⢿⣬⣾⣛⠳⣼⡮⠳⡂⠒⠀
⠀⠀⠀⠀⠀⠀⠀⠀⢠⠏⡁⢉⣀⣑⣆⡐⠊⣅⡕⢦⣀⣱⡶⢫⣨⢟⠽⠕⣇⢶⣵⣋⢝⣉⣋⠜⠉⠉⡯⠛⣿⣿⣿⣾⣳⠠⠤⠪⠁⠊⠉⠻⣟⣾⣿⣿⣟⣧⣧⢸⠂⠠⢠
⠀⠀⠀⠀⠀⠀⠀⣠⣾⢞⢉⠿⢿⣟⡓⠖⢾⡏⢠⣾⣿⠛⣰⠾⢓⡵⣺⢺⣼⡫⢪⡱⣉⠷⢗⡀⠤⠆⡻⣛⠿⣻⣿⢶⣊⡄⠀⠀⠀⠀⠄⢀⠀⠉⠿⣿⡿⣿⣛⡁⢍⣀⡌
⠀⠀⠀⠀⠀⠀⣠⣛⢓⠗⠀⠀⠠⣈⠉⢀⢬⣵⡿⢋⣴⣞⣵⣼⣭⢁⠕⢿⢋⣞⢟⣕⡩⣔⠃⠀⠀⡀⣛⣤⢿⣷⢻⣿⣿⣽⣮⡙⠆⠀⠤⢓⡙⣆⠀⠀⠘⠙⢯⣛⣶⠋⠁
⠀⠀⠀⠀⠀⢠⢋⢿⣼⣶⣶⣤⠒⢉⠠⣪⣮⠟⣡⡾⠹⡿⣿⣿⠝⢊⣐⢺⡜⣫⡞⢭⡕⠋⣐⠒⠀⠡⠏⠉⠘⠛⠚⡻⣯⠋⠛⢅⠐⢄⠀⣸⡃⠛⠀⡀⡀⠀⠈⠙⡟⠀⠀
⠀⠀⠀⠀⣠⢫⠎⠙⠿⣿⠛⡡⠔⠕⣴⡿⡁⡮⡷⡶⢟⣿⢎⡵⡠⢞⠱⢈⣼⠁⠄⠇⡄⣢⠒⠀⡎⠀⡇⠒⠐⠐⠐⢚⠐⢷⣔⢖⢊⡈⠒⠗⠠⠘⠈⡁⢈⣠⣤⠾⠀⠀⠀
⠀⠀⠀⣰⢳⠏⢀⢔⢤⠶⠪⣠⣭⣾⣿⠗⢘⣷⣼⠛⠛⢛⡝⣜⢑⣤⣾⠿⣿⣿⢽⣿⠿⠶⢴⣯⣄⡄⣇⣀⣀⡀⠄⠠⠆⣀⡨⢽⣵⣕⣄⣀⣰⣥⣶⣾⡿⠟⠉⠀⠀⠀⠀
⠀⠀⡰⣱⢋⠴⣩⠔⠻⣴⡾⢷⣿⡿⠃⠰⢿⣿⣿⣿⣶⣬⣧⣼⡿⠛⠁⡢⠒⠈⢈⡉⠿⠚⢼⣿⣿⣿⡆⠋⠉⠢⠀⢀⣀⣡⣴⡶⣿⣿⣿⠿⠻⠛⠋⠁⠀⠀⠀⠀⠀⠀⠀
⠀⡼⣳⣯⣱⣪⣲⣫⠻⣯⢟⠽⠋⠀⠀⠀⠀⠈⠙⠻⢻⡳⡩⢇⢀⠸⢢⣤⣴⣁⡀⠊⡀⠠⠂⢉⣫⡭⣁⣀⣠⣴⣶⢿⣿⣿⣿⡿⠞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⣼⣟⡿⣿⣿⢝⣫⣶⡿⠣⠉⠀⠀⠀⠀⠀⠀⠀⣠⣖⠕⡩⢂⢕⠡⠒⠸⣿⣿⣿⡿⢂⢀⠀⣜⡿⣵⣶⣾⣿⡿⠯⠟⠋⠉⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⢸⢿⣷⣷⣷⣿⠛⠋⠁⠀⠀⠀⠀⠀⠀⢀⣴⡺⠟⣑⣿⡺⢑⡴⠂⠊⠀⢀⡁⣍⣢⣼⣺⣽⣶⡿⠿⠏⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠈⠙⠻⠝⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⡿⡋⢰⠕⠋⡿⠉⢁⡈⢕⣲⣼⣒⣯⣷⣿⠿⠟⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⣴⣿⣷⣧⡎⣠⡤⠥⣰⢬⣵⣮⣽⣿⡿⠟⠛⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣷⣮⣟⡯⣓⣦⣿⣮⣿⣿⣿⠿⠛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠛⠿⣿⣿⣿⣿⡿⠟⠛⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠛⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
kluge7
force-pushed
the
435-task-velocity-controller-pog
branch
from
8ee813a
to
9a171f1
Compare
kluge7
requested changes
Nov 10, 2024
Comment on lines
+15
to
+18
| <depend>nav_msgs</depend> | ||
| <depend>vortex_msgs</depend> | ||
|
|
||
| <exec_depend>control</exec_depend> |
There was a problem hiding this comment.
You need to replace <exec_depend>control</exec_depend> with the correct rosdep key for control. I think the correct one is <depend>python-control-pip</depend>
There was a problem hiding this comment.
najs python-control-pip implemented chief
3 tasks
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Fixed everything, AUV works flawlessly now.