From 371c520ec322f7fa067084385af1426ace2da416 Mon Sep 17 00:00:00 2001 From: vortex Date: Mon, 8 Jan 2024 14:32:06 +0100 Subject: [PATCH 01/20] Set up python package, launch, lqr_config.yaml --- motion/lqr_controller/LICENSE | 17 ++++ motion/lqr_controller/config/lqr_config.yaml | 11 +++ .../launch/lqr_controller.launch.py | 16 ++++ .../lqr_controller/lqr_controller/__init__.py | 0 .../lqr_controller/asv_model.py | 81 +++++++++++++++++++ .../lqr_controller/lqr_controller_dp.py | 0 .../lqr_controller/lqr_controller_node.py | 17 ++++ motion/lqr_controller/package.xml | 20 +++++ motion/lqr_controller/resource/lqr_controller | 0 motion/lqr_controller/setup.cfg | 4 + motion/lqr_controller/setup.py | 30 +++++++ 11 files changed, 196 insertions(+) create mode 100644 motion/lqr_controller/LICENSE create mode 100644 motion/lqr_controller/config/lqr_config.yaml create mode 100644 motion/lqr_controller/launch/lqr_controller.launch.py create mode 100644 motion/lqr_controller/lqr_controller/__init__.py create mode 100644 motion/lqr_controller/lqr_controller/asv_model.py create mode 100644 motion/lqr_controller/lqr_controller/lqr_controller_dp.py create mode 100644 motion/lqr_controller/lqr_controller/lqr_controller_node.py create mode 100644 motion/lqr_controller/package.xml create mode 100644 motion/lqr_controller/resource/lqr_controller create mode 100644 motion/lqr_controller/setup.cfg create mode 100644 motion/lqr_controller/setup.py diff --git a/motion/lqr_controller/LICENSE b/motion/lqr_controller/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/motion/lqr_controller/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml new file mode 100644 index 00000000..ed2c214f --- /dev/null +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + lqr_controller: + mass: 50.0 + inertia: 5.0 + damping: + x: 5.0 + y: 20.0 + Q: [10.0, 10.0, 5.0, 0.1, 1.0, 5.0] # State costs for [x, y, heading, surge, sway, yaw] + R: [1.0, 1.0, 1.0] # Control cost weight + diff --git a/motion/lqr_controller/launch/lqr_controller.launch.py b/motion/lqr_controller/launch/lqr_controller.launch.py new file mode 100644 index 00000000..b51a0ab3 --- /dev/null +++ b/motion/lqr_controller/launch/lqr_controller.launch.py @@ -0,0 +1,16 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + lqr_controller_node = Node( + package='lqr_controller', + executable='lqr_controller_node', + name='lqr_controller_node', + parameters=[os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.yaml')], + output='screen', + ) + return LaunchDescription([ + lqr_controller_node + ]) diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/lqr_controller/asv_model.py b/motion/lqr_controller/lqr_controller/asv_model.py new file mode 100644 index 00000000..821c2f56 --- /dev/null +++ b/motion/lqr_controller/lqr_controller/asv_model.py @@ -0,0 +1,81 @@ +import numpy as np + +class ASV(): + + def __init__(self, M, D): + + """ + Initialize some system matrices + """ + + #m = 50 + #d = np.array([10, 10, 5]) + + #self.M = np.diag(m*np.ones(3)) + #self.D = np.diag(d) + + + self.M = M + self.M_inv = np.linalg.inv(self.M) + self.D = D + + + def state_dot(self, state, tau_actuation, V_current = np.zeros(2)): + + """ + Derivative of the state calculated with the non-linear kinematix + """ + + heading = state[2] + + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - self.M_inv @ self.D + + B = np.zeros((6,3)) + B[3:,:] = self.M_inv + + x_dot = A @ state + B @ tau_actuation + x_dot[0:2] += V_current # add current drift term at velocity level + + return x_dot + + def linearize_model(self, heading): + """ + Get a linearization about some heading + """ + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - self.M_inv @ self.D + + B = np.zeros((6,3)) + B[3:,:] = self.M_inv + + return A, B + + def RK4_integration_step(self, x, u, dt): + + # integration scheme for simulation, implements the Runge-Kutta 4 integrator + + k1 = self.state_dot(x, u) + k2 = self.state_dot(x+dt/2*k1, u) + k3 = self.state_dot(x+dt/2*k2, u) + k4 = self.state_dot(x+dt*k3, u) + + x_next = x + dt/6*(k1+2*k2+2*k3+k4) + + return x_next \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_dp.py b/motion/lqr_controller/lqr_controller/lqr_controller_dp.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py new file mode 100644 index 00000000..a1d1649d --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -0,0 +1,17 @@ +import rclpy +from rclpy.node import Node + +class LQRControllerNode(Node): + def __init__(self): + super().__init__("lqr_controller_node") + self.get_logger().info("lqr_controller_node started") + +def main(args=None): + rclpy.init(args=args) + node = LQRControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml new file mode 100644 index 00000000..3ab4efaf --- /dev/null +++ b/motion/lqr_controller/package.xml @@ -0,0 +1,20 @@ + + + + lqr_controller + 0.0.0 + TODO: Package description + vortex + MIT + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/motion/lqr_controller/resource/lqr_controller b/motion/lqr_controller/resource/lqr_controller new file mode 100644 index 00000000..e69de29b diff --git a/motion/lqr_controller/setup.cfg b/motion/lqr_controller/setup.cfg new file mode 100644 index 00000000..e61c4ac2 --- /dev/null +++ b/motion/lqr_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lqr_controller +[install] +install_scripts=$base/lib/lqr_controller diff --git a/motion/lqr_controller/setup.py b/motion/lqr_controller/setup.py new file mode 100644 index 00000000..ada29567 --- /dev/null +++ b/motion/lqr_controller/setup.py @@ -0,0 +1,30 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'lqr_controller' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='vortex', + maintainer_email='vortex.git@vortexntnu.no', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'lqr_controller_node = lqr_controller.lqr_controller_node:main' + ], + }, +) From 059cdf1b2f0f1db1a77b198eae9fba68e9b79477 Mon Sep 17 00:00:00 2001 From: vortex Date: Mon, 8 Jan 2024 15:29:32 +0100 Subject: [PATCH 02/20] Getting params from config file working --- motion/lqr_controller/config/lqr_config.yaml | 7 +------ .../lqr_controller/lqr_controller_node.py | 19 ++++++++++++++++++- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml index ed2c214f..55abb392 100644 --- a/motion/lqr_controller/config/lqr_config.yaml +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -2,10 +2,5 @@ ros__parameters: lqr_controller: mass: 50.0 - inertia: 5.0 - damping: - x: 5.0 - y: 20.0 Q: [10.0, 10.0, 5.0, 0.1, 1.0, 5.0] # State costs for [x, y, heading, surge, sway, yaw] - R: [1.0, 1.0, 1.0] # Control cost weight - + R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index a1d1649d..db66b724 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -4,6 +4,23 @@ class LQRControllerNode(Node): def __init__(self): super().__init__("lqr_controller_node") + + # Load parameters from lqr_config.yaml + self.declare_parameters( + namespace='', + parameters=[ + ('lqr_controller.mass', 1.0), + ('lqr_controller.Q', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), + ('lqr_controller.R', [0.0, 0.0, 0.0]) + ]) + + m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value + Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value + R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value + + self.get_logger().info(f"Mass: {m}") + self.get_logger().info(f"Q: {Q}") + self.get_logger().info(f"R: {R}") self.get_logger().info("lqr_controller_node started") def main(args=None): @@ -14,4 +31,4 @@ def main(args=None): rclpy.shutdown() if __name__ == "__main__": - main() \ No newline at end of file + main() From 386e2cc08a67813bc6f0870989ab709ec8061515 Mon Sep 17 00:00:00 2001 From: vortex Date: Mon, 8 Jan 2024 15:47:02 +0100 Subject: [PATCH 03/20] Added dampening D --- motion/lqr_controller/config/lqr_config.yaml | 1 + motion/lqr_controller/lqr_controller/lqr_controller_node.py | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml index 55abb392..a46f88db 100644 --- a/motion/lqr_controller/config/lqr_config.yaml +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -2,5 +2,6 @@ ros__parameters: lqr_controller: mass: 50.0 + D: [10.0, 10.0, 10.0] # Damping coefficients Q: [10.0, 10.0, 5.0, 0.1, 1.0, 5.0] # State costs for [x, y, heading, surge, sway, yaw] R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index db66b724..6e8f9135 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -10,17 +10,21 @@ def __init__(self): namespace='', parameters=[ ('lqr_controller.mass', 1.0), + ('lqr_controller.D', [0.0, 0.0, 0.0]), ('lqr_controller.Q', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), ('lqr_controller.R', [0.0, 0.0, 0.0]) ]) m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value + D = self.get_parameter('lqr_controller.D').get_parameter_value().double_array_value Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value self.get_logger().info(f"Mass: {m}") + self.get_logger().info(f"D: {D}") self.get_logger().info(f"Q: {Q}") self.get_logger().info(f"R: {R}") + self.get_logger().info("lqr_controller_node started") def main(args=None): From cab4c391d84550686a9f4eb4c6e6da0a3f3ca518 Mon Sep 17 00:00:00 2001 From: vortex Date: Mon, 8 Jan 2024 16:56:25 +0100 Subject: [PATCH 04/20] WIP not working --- .../lqr_controller/lqr_controller.py | 87 +++++++++++++++++++ .../lqr_controller/lqr_controller_dp.py | 0 .../lqr_controller/lqr_controller_node.py | 4 + 3 files changed, 91 insertions(+) create mode 100644 motion/lqr_controller/lqr_controller/lqr_controller.py delete mode 100644 motion/lqr_controller/lqr_controller/lqr_controller_dp.py diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py new file mode 100644 index 00000000..2451acc5 --- /dev/null +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -0,0 +1,87 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.linalg import solve_continuous_are +from asv_model import ASV + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi + +def calculate_control_input(x, x_ref, K_LQR, K_r): + u = -K_LQR @ x + K_r @ x_ref + return u + +class LQRController: + + def __init__(self, m, D, Q, R): + self.heading_ref = 50*np.pi/180 # magic init number!!! + + self.M = np.diag([m, m, m]) + self.M_inv = np.linalg.inv(self.M) + self.D = np.diag(D) + + self.asv = ASV(self.M, self.D) + self.A, self.B = self.asv.linearize_model(self.heading_ref) + C = np.zeros((3,6)) + C[:3, :3] = np.eye(3) + + self.Q = np.diag(Q) + self.R = np.diag(R) + self.P = solve_continuous_are(self.A(), self.B(), self.Q, self.R) + self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) + self.K_r = np.linalg.inv(C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) + + def run_ivan_sim(self): + x_init = np.array([-10, -10, 40*np.pi/180, 0, 0, 0]) + x_ref = np.array([0, 0, self.heading_ref]) + + T = 69.0 # Total simulation time + dt = 0.01 # Time step + num_steps = int(T / dt) # Number of time steps + + # Initialize arrays to store data + time = np.linspace(0, T, num_steps+1) + x_history = np.zeros((num_steps+1, self.A.shape[0])) + u_history = np.zeros((num_steps+1, self.B.shape[1])) + x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) + + # Simulation loop + x = x_init # Initial state + for i in range(num_steps+1): + #x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step + x_ref_history[i, :] = x_ref # Update the reference at each time step + + u = calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step + x = self.asv.RK4_integration_step(x, u, dt) + + x_history[i] = x # Store state history + u_history[i] = u # Store control input history + + # Plot results + plt.figure(figsize=(12, 8)) + plt.subplot(3, 1, 1) + for j in range(3): + plt.plot(time, x_history[:, j], label=f'State (x_{j+1})') + plt.plot(time, x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') + plt.xlabel('Time') + plt.ylabel('State Value') + plt.legend() + + plt.subplot(3, 1, 2) + for j in range(B.shape[1]): + plt.plot(time, u_history[:, j], label=f'Control Input (u_{j+1})') + plt.xlabel('Time') + plt.ylabel('Control Input Value') + plt.legend() + + plt.subplot(3, 1, 3) + plt.scatter(x_history[:,0], x_history[:,1], label=f'Position') + plt.xlabel('Time') + plt.ylabel('Control Input Value') + plt.legend() + + plt.tight_layout() + plt.show() + + + + \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_dp.py b/motion/lqr_controller/lqr_controller/lqr_controller_dp.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 6e8f9135..0110110b 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -1,5 +1,6 @@ import rclpy from rclpy.node import Node +from lqr_controller import LQRController class LQRControllerNode(Node): def __init__(self): @@ -25,6 +26,9 @@ def __init__(self): self.get_logger().info(f"Q: {Q}") self.get_logger().info(f"R: {R}") + lqr_controller = LQRController(m, D, Q, R) + lqr_controller.run_ivan_sim() + self.get_logger().info("lqr_controller_node started") def main(args=None): From 46f846ad6170377e66bca6386a09cb4123095047 Mon Sep 17 00:00:00 2001 From: vortex Date: Tue, 9 Jan 2024 10:03:17 +0100 Subject: [PATCH 05/20] Ivan stuff working in ROS2 --- motion/lqr_controller/lqr_controller/lqr_controller.py | 6 +++--- motion/lqr_controller/lqr_controller/lqr_controller_node.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index 2451acc5..bbc7ab16 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -1,7 +1,7 @@ import numpy as np import matplotlib.pyplot as plt from scipy.linalg import solve_continuous_are -from asv_model import ASV +from lqr_controller.asv_model import ASV def ssa(angle): return (angle + np.pi) % (2 * np.pi) - np.pi @@ -26,7 +26,7 @@ def __init__(self, m, D, Q, R): self.Q = np.diag(Q) self.R = np.diag(R) - self.P = solve_continuous_are(self.A(), self.B(), self.Q, self.R) + self.P = solve_continuous_are(self.A, self.B, self.Q, self.R) self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) self.K_r = np.linalg.inv(C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) @@ -67,7 +67,7 @@ def run_ivan_sim(self): plt.legend() plt.subplot(3, 1, 2) - for j in range(B.shape[1]): + for j in range(self.B.shape[1]): plt.plot(time, u_history[:, j], label=f'Control Input (u_{j+1})') plt.xlabel('Time') plt.ylabel('Control Input Value') diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 0110110b..7094b003 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -1,6 +1,6 @@ import rclpy from rclpy.node import Node -from lqr_controller import LQRController +from lqr_controller.lqr_controller import LQRController class LQRControllerNode(Node): def __init__(self): From 8a1ac50cbcf98ceb38e9440299c720ac064813c1 Mon Sep 17 00:00:00 2001 From: vortex Date: Tue, 9 Jan 2024 13:34:01 +0100 Subject: [PATCH 06/20] Implemented Ivan LOS LQR in ROS2 --- motion/lqr_controller/config/lqr_config.yaml | 2 +- .../lqr_controller/asv_model.py | 53 ++++++++++++----- .../lqr_controller/lqr_controller.py | 59 +++++++++++++++++-- motion/lqr_controller/package.xml | 1 + 4 files changed, 93 insertions(+), 22 deletions(-) diff --git a/motion/lqr_controller/config/lqr_config.yaml b/motion/lqr_controller/config/lqr_config.yaml index a46f88db..50dc3463 100644 --- a/motion/lqr_controller/config/lqr_config.yaml +++ b/motion/lqr_controller/config/lqr_config.yaml @@ -2,6 +2,6 @@ ros__parameters: lqr_controller: mass: 50.0 - D: [10.0, 10.0, 10.0] # Damping coefficients + D: [10.0, 10.0, 5.0] # Damping coefficients Q: [10.0, 10.0, 5.0, 0.1, 1.0, 5.0] # State costs for [x, y, heading, surge, sway, yaw] R: [1.0, 1.0, 1.0] # Control cost weight \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/asv_model.py b/motion/lqr_controller/lqr_controller/asv_model.py index 821c2f56..88bc8ea7 100644 --- a/motion/lqr_controller/lqr_controller/asv_model.py +++ b/motion/lqr_controller/lqr_controller/asv_model.py @@ -2,30 +2,52 @@ class ASV(): - def __init__(self, M, D): - + def __init__(self, M: np.ndarray, D: np.ndarray): """ Initialize some system matrices """ - - #m = 50 - #d = np.array([10, 10, 5]) - - #self.M = np.diag(m*np.ones(3)) - #self.D = np.diag(d) - - self.M = M self.M_inv = np.linalg.inv(self.M) self.D = D + def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: + """ + Get a linearization about some heading + """ + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) - def state_dot(self, state, tau_actuation, V_current = np.zeros(2)): - + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - self.M_inv @ self.D + + B = np.zeros((6,3)) + B[3:,:] = self.M_inv + + return A, B + + def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: """ - Derivative of the state calculated with the non-linear kinematix + Perform an integration step using the Runge-Kutta 4 integrator """ + k1 = self.state_dot(x, u) + k2 = self.state_dot(x+dt/2*k1, u) + k3 = self.state_dot(x+dt/2*k2, u) + k4 = self.state_dot(x+dt*k3, u) + + x_next = x + dt/6*(k1+2*k2+2*k3+k4) + + return x_next + + def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: + """ + Calculate the derivative of the state using the non-linear kinematics + """ heading = state[2] J = np.array( @@ -46,8 +68,9 @@ def state_dot(self, state, tau_actuation, V_current = np.zeros(2)): x_dot[0:2] += V_current # add current drift term at velocity level return x_dot + - def linearize_model(self, heading): + def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: """ Get a linearization about some heading """ @@ -67,7 +90,7 @@ def linearize_model(self, heading): return A, B - def RK4_integration_step(self, x, u, dt): + def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: # integration scheme for simulation, implements the Runge-Kutta 4 integrator diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index bbc7ab16..a2701699 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -12,8 +12,8 @@ def calculate_control_input(x, x_ref, K_LQR, K_r): class LQRController: - def __init__(self, m, D, Q, R): - self.heading_ref = 50*np.pi/180 # magic init number!!! + def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): + self.heading_ref = 30*np.pi/180 # magic init number!!! self.M = np.diag([m, m, m]) self.M_inv = np.linalg.inv(self.M) @@ -30,10 +30,39 @@ def __init__(self, m, D, Q, R): self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) self.K_r = np.linalg.inv(C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) + ## Magic init Path parameters parameters + self.set_path([0, 0], [20, 20]) + self.calculate_R_Pi_p() + + def set_path(self, p0: list[float], p1: list[float]): + self.p0 = np.array(p0) + self.p1 = np.array(p1) + + def calculate_R_Pi_p(self): + self.Pi_p = np.arctan2(self.p1[1]-self.p0[1], self.p1[0]-self.p0[0]) + self.R_Pi_p = np.array( + [[np.cos(self.Pi_p), -np.sin(self.Pi_p)], + [np.sin(self.Pi_p), np.cos(self.Pi_p)]] + ) + + def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: + """ + generate reference at the look-ahead distance + """ + p_asv = np.array([x[0], x[1]]) + errors = self.R_Pi_p.T @ (p_asv - self.p0) + along_track_error = errors[0] + p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 + + x_ref = np.array([p_los_world[0], p_los_world[1], self.heading_ref]) + return x_ref + def run_ivan_sim(self): - x_init = np.array([-10, -10, 40*np.pi/180, 0, 0, 0]) + x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) x_ref = np.array([0, 0, self.heading_ref]) + look_ahead = 5 # Look ahead distance + T = 69.0 # Total simulation time dt = 0.01 # Time step num_steps = int(T / dt) # Number of time steps @@ -44,11 +73,22 @@ def run_ivan_sim(self): u_history = np.zeros((num_steps+1, self.B.shape[1])) x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) + cross_track_error_history = np.zeros(num_steps+1) + # Simulation loop x = x_init # Initial state for i in range(num_steps+1): #x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step - x_ref_history[i, :] = x_ref # Update the reference at each time step + # x_ref_history[i, :] = x_ref # Update the reference at each time step + + # generate reference at the look-ahead distance + p_asv = np.array([x[0], x[1]]) + errors = self.R_Pi_p.T @ (p_asv - self.p0) + along_track_error = errors[0] + p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 + + x_ref[:2] = p_los_world # Update the position reference at each time step + x_ref_history[i, :] = x_ref u = calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step x = self.asv.RK4_integration_step(x, u, dt) @@ -75,12 +115,19 @@ def run_ivan_sim(self): plt.subplot(3, 1, 3) plt.scatter(x_history[:,0], x_history[:,1], label=f'Position') - plt.xlabel('Time') - plt.ylabel('Control Input Value') + plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path') + plt.xlabel('x') + plt.ylabel('y') plt.legend() plt.tight_layout() plt.show() + plt.figure(1) + plt.plot(time, cross_track_error_history, label="cross track error") + plt.axis("equal") + plt.plot(time, np.zeros_like(time)) + plt.legend() + plt.show() diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml index 3ab4efaf..9238eea0 100644 --- a/motion/lqr_controller/package.xml +++ b/motion/lqr_controller/package.xml @@ -8,6 +8,7 @@ MIT rclpy + geometry_msgs ament_copyright ament_flake8 From 145a90c6e806ff9c6bab6f1a6574e26e12659b61 Mon Sep 17 00:00:00 2001 From: vortex Date: Tue, 9 Jan 2024 14:01:41 +0100 Subject: [PATCH 07/20] Added los_guidance package --- guidance/los_guidance/LICENSE | 17 +++++++++++ .../launch/los_guidance.launch.py | 16 ++++++++++ .../los_guidance/los_guidance/__init__.py | 0 .../los_guidance/los_guidance/los_guidance.py | 2 ++ .../los_guidance/los_guidance_node.py | 10 +++++++ guidance/los_guidance/package.xml | 21 ++++++++++++++ guidance/los_guidance/resource/los_guidance | 0 guidance/los_guidance/setup.cfg | 4 +++ guidance/los_guidance/setup.py | 29 +++++++++++++++++++ motion/lqr_controller/package.xml | 1 + 10 files changed, 100 insertions(+) create mode 100644 guidance/los_guidance/LICENSE create mode 100644 guidance/los_guidance/launch/los_guidance.launch.py create mode 100644 guidance/los_guidance/los_guidance/__init__.py create mode 100644 guidance/los_guidance/los_guidance/los_guidance.py create mode 100644 guidance/los_guidance/los_guidance/los_guidance_node.py create mode 100644 guidance/los_guidance/package.xml create mode 100644 guidance/los_guidance/resource/los_guidance create mode 100644 guidance/los_guidance/setup.cfg create mode 100644 guidance/los_guidance/setup.py diff --git a/guidance/los_guidance/LICENSE b/guidance/los_guidance/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/guidance/los_guidance/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py new file mode 100644 index 00000000..90c48833 --- /dev/null +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -0,0 +1,16 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + los_guidance_node = Node( + package='los_guidance', + executable='los_guidance_node', + name='los_guidance_node', + parameters=[], + output='screen', + ) + return LaunchDescription([ + los_guidance_node + ]) diff --git a/guidance/los_guidance/los_guidance/__init__.py b/guidance/los_guidance/los_guidance/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py new file mode 100644 index 00000000..3ca1f424 --- /dev/null +++ b/guidance/los_guidance/los_guidance/los_guidance.py @@ -0,0 +1,2 @@ +import numpy as np +import matplotlib.pyplot as plt \ No newline at end of file diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py new file mode 100644 index 00000000..bed1d792 --- /dev/null +++ b/guidance/los_guidance/los_guidance/los_guidance_node.py @@ -0,0 +1,10 @@ +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry + +def main(): + print('Hi from los_guidance.') + + +if __name__ == '__main__': + main() diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml new file mode 100644 index 00000000..e559fc43 --- /dev/null +++ b/guidance/los_guidance/package.xml @@ -0,0 +1,21 @@ + + + + los_guidance + 0.0.0 + TODO: Package description + vortex + MIT + + rclpy + nav_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/guidance/los_guidance/resource/los_guidance b/guidance/los_guidance/resource/los_guidance new file mode 100644 index 00000000..e69de29b diff --git a/guidance/los_guidance/setup.cfg b/guidance/los_guidance/setup.cfg new file mode 100644 index 00000000..8709d00f --- /dev/null +++ b/guidance/los_guidance/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/los_guidance +[install] +install_scripts=$base/lib/los_guidance diff --git a/guidance/los_guidance/setup.py b/guidance/los_guidance/setup.py new file mode 100644 index 00000000..1ead3e2d --- /dev/null +++ b/guidance/los_guidance/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'los_guidance' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='vortex', + maintainer_email='vortex.git@vortexntnu.no', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'los_guidance_node = los_guidance.los_guidance_node:main' + ], + }, +) diff --git a/motion/lqr_controller/package.xml b/motion/lqr_controller/package.xml index 9238eea0..b38062f7 100644 --- a/motion/lqr_controller/package.xml +++ b/motion/lqr_controller/package.xml @@ -9,6 +9,7 @@ rclpy geometry_msgs + nav_msgs ament_copyright ament_flake8 From a2d122166a3a814f4acb14d8a99b09dcea5113c0 Mon Sep 17 00:00:00 2001 From: vortex Date: Tue, 9 Jan 2024 16:26:15 +0100 Subject: [PATCH 08/20] Added x_ref LOS calculation and Odometry --- .../config/los_guidance_config.yaml | 6 ++ .../launch/los_guidance.launch.py | 2 +- .../los_guidance/los_guidance/los_guidance.py | 28 ++++++- .../los_guidance/los_guidance_node.py | 73 ++++++++++++++++++- guidance/los_guidance/package.xml | 1 + guidance/los_guidance/setup.py | 1 + 6 files changed, 106 insertions(+), 5 deletions(-) create mode 100644 guidance/los_guidance/config/los_guidance_config.yaml diff --git a/guidance/los_guidance/config/los_guidance_config.yaml b/guidance/los_guidance/config/los_guidance_config.yaml new file mode 100644 index 00000000..13a0ada6 --- /dev/null +++ b/guidance/los_guidance/config/los_guidance_config.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + los_guidance: + p0: [0.0, 0.0] + p1: [20.0, 20.0] + look_ahead_distance: 5.0 \ No newline at end of file diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 90c48833..f6875f8e 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): package='los_guidance', executable='los_guidance_node', name='los_guidance_node', - parameters=[], + parameters=[os.path.join(get_package_share_directory('los_guidance'),'config','los_guidance_config.yaml')], output='screen', ) return LaunchDescription([ diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py index 3ca1f424..2a9c81f0 100644 --- a/guidance/los_guidance/los_guidance/los_guidance.py +++ b/guidance/los_guidance/los_guidance/los_guidance.py @@ -1,2 +1,28 @@ import numpy as np -import matplotlib.pyplot as plt \ No newline at end of file +import matplotlib.pyplot as plt + +class LOSGuidance: + def __init__(self, p0: list[float], p1: list[float]): + self.set_path(p0, p1) + + def set_path(self, p0: list[float], p1: list[float]): + self.p0 = np.array(p0) + self.p1 = np.array(p1) + + def calculate_R_Pi_p(self): + self.Pi_p = np.arctan2(self.p1[1]-self.p0[1], self.p1[0]-self.p0[0]) + self.R_Pi_p = np.array( + [[np.cos(self.Pi_p), -np.sin(self.Pi_p)], + [np.sin(self.Pi_p), np.cos(self.Pi_p)]] + ) + + def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: + self.set_path(self.p0, self.p1) + self.calculate_R_Pi_p() + p_asv = np.array([x[0], x[1]]) + errors = self.R_Pi_p.T @ (p_asv - self.p0) + along_track_error = errors[0] + p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 + x_ref = np.array([p_los_world[0], p_los_world[1], self.heading_ref]) + + return x_ref diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py index bed1d792..91b7140d 100644 --- a/guidance/los_guidance/los_guidance/los_guidance_node.py +++ b/guidance/los_guidance/los_guidance/los_guidance_node.py @@ -1,10 +1,77 @@ import rclpy +import numpy as np from rclpy.node import Node from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler, euler2quat +from los_guidance.los_guidance import LOSGuidance -def main(): - print('Hi from los_guidance.') +class LOSGuidanceNode(Node): + def __init__(self): + super().__init__("los_guidance_node") + self.declare_parameters( + namespace='', + parameters=[ + ('los_guidance.p0', [0.0, 0.0]), + ('los_guidance.p1', [0.0, 0.0]), + ('los_guidance.look_ahead', 0.0) + ]) + + p0 = self.get_parameter('los_guidance.p0').get_parameter_value().double_array_value + p1 = self.get_parameter('los_guidance.p1').get_parameter_value().double_array_value + self.look_ahead = self.get_parameter('los_guidance.look_ahead').get_parameter_value().double_value + + self.get_logger().info(f"p0: {p0}") + self.get_logger().info(f"p1: {p1}") + self.get_logger().info(f"look_ahead: {self.look_ahead}") -if __name__ == '__main__': + self.los_guidance = LOSGuidance(p0, p1) + + + self.guidance_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_callback, 1) + + self.get_logger().info("los_guidance_node started") + + def state_callback(self, msg): + self.get_logger().info("state_callback") + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w + ] + # Convert quaternion to Euler angles + (roll, pitch, yaw) = quat2euler(orientation_list) + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + + x_ref = self.los_guidance.calculate_LOS_x_ref(state, self.look_ahead) + + orientation_list_ref = euler2quat(roll, pitch, x_ref[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = x_ref[0] + odometry_msg.pose.pose.position.y = x_ref[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.x = orientation_list_ref[0] + odometry_msg.pose.pose.orientation.y = orientation_list_ref[1] + odometry_msg.pose.pose.orientation.z = orientation_list_ref[2] + odometry_msg.pose.pose.orientation.w = orientation_list_ref[3] + + self.guidance_publisher_.publish(odometry_msg) + + +def main(args=None): + rclpy.init(args=args) + node = LOSGuidanceNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": main() diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index e559fc43..023e7271 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -9,6 +9,7 @@ rclpy nav_msgs + python-transforms3d-pip ament_copyright ament_flake8 diff --git a/guidance/los_guidance/setup.py b/guidance/los_guidance/setup.py index 1ead3e2d..cef8ea5a 100644 --- a/guidance/los_guidance/setup.py +++ b/guidance/los_guidance/setup.py @@ -13,6 +13,7 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), ], install_requires=['setuptools'], zip_safe=True, From 3488c7cf3985a407e3dbacb3ac5b4091a1d57297 Mon Sep 17 00:00:00 2001 From: vortex Date: Tue, 9 Jan 2024 16:26:31 +0100 Subject: [PATCH 09/20] Added Odometry --- .../lqr_controller/lqr_controller.py | 40 +++---------- .../lqr_controller/lqr_controller_node.py | 56 ++++++++++++++++++- 2 files changed, 63 insertions(+), 33 deletions(-) diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index a2701699..cfa1acd9 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -6,10 +6,6 @@ def ssa(angle): return (angle + np.pi) % (2 * np.pi) - np.pi -def calculate_control_input(x, x_ref, K_LQR, K_r): - u = -K_LQR @ x + K_r @ x_ref - return u - class LQRController: def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): @@ -20,7 +16,9 @@ def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): self.D = np.diag(D) self.asv = ASV(self.M, self.D) - self.A, self.B = self.asv.linearize_model(self.heading_ref) + + self.linearize_model(self.heading_ref) + C = np.zeros((3,6)) C[:3, :3] = np.eye(3) @@ -30,33 +28,13 @@ def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) self.K_r = np.linalg.inv(C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) - ## Magic init Path parameters parameters - self.set_path([0, 0], [20, 20]) - self.calculate_R_Pi_p() + def linearize_model(self, heading: float): + self.A, self.B = self.asv.linearize_model(heading) - def set_path(self, p0: list[float], p1: list[float]): - self.p0 = np.array(p0) - self.p1 = np.array(p1) - - def calculate_R_Pi_p(self): - self.Pi_p = np.arctan2(self.p1[1]-self.p0[1], self.p1[0]-self.p0[0]) - self.R_Pi_p = np.array( - [[np.cos(self.Pi_p), -np.sin(self.Pi_p)], - [np.sin(self.Pi_p), np.cos(self.Pi_p)]] - ) + def calculate_control_input(x, x_ref, K_LQR, K_r): + u = -K_LQR @ x + K_r @ x_ref + return u - def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: - """ - generate reference at the look-ahead distance - """ - p_asv = np.array([x[0], x[1]]) - errors = self.R_Pi_p.T @ (p_asv - self.p0) - along_track_error = errors[0] - p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 - - x_ref = np.array([p_los_world[0], p_los_world[1], self.heading_ref]) - return x_ref - def run_ivan_sim(self): x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) x_ref = np.array([0, 0, self.heading_ref]) @@ -90,7 +68,7 @@ def run_ivan_sim(self): x_ref[:2] = p_los_world # Update the position reference at each time step x_ref_history[i, :] = x_ref - u = calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step + u = self.calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step x = self.asv.RK4_integration_step(x, u, dt) x_history[i] = x # Store state history diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 7094b003..7dbae2d8 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -1,5 +1,9 @@ import rclpy +import numpy as np from rclpy.node import Node +from geometry_msgs.msg import Wrench +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler from lqr_controller.lqr_controller import LQRController class LQRControllerNode(Node): @@ -15,6 +19,10 @@ def __init__(self): ('lqr_controller.Q', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), ('lqr_controller.R', [0.0, 0.0, 0.0]) ]) + + self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_callback, 1) + self.guidance_subscriber_ = self.create_subscription(Odometry, "path/pathpath", self.guidance_callback, 1) m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value D = self.get_parameter('lqr_controller.D').get_parameter_value().double_array_value @@ -26,11 +34,55 @@ def __init__(self): self.get_logger().info(f"Q: {Q}") self.get_logger().info(f"R: {R}") - lqr_controller = LQRController(m, D, Q, R) - lqr_controller.run_ivan_sim() + self.lqr_controller = LQRController(m, D, Q, R) + #self.lqr_controller.run_ivan_sim() + + self.x_ref = [0, 0, 0, 0, 0, 0] self.get_logger().info("lqr_controller_node started") + + def guidance_callback(self, msg): + pass + + def state_callback(self, msg): + + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles + yaw = quat2euler(orientation_list)[2] + + # Gjør en relinearisering + self.lqr_controller.linearize_model(yaw) + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + + # Kjør LQR + u = self.lqr_controller.calculate_control_input(state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r) + + wrench = Wrench() + wrench.force.x = u[0] + wrench.force.y = u[1] + wrench.force.z = 0.0 + wrench.torque.x = 0.0 + wrench.torque.y = 0.0 + wrench.torque.z = u[2] + + #Publisher til thrust/wrench_input + self.wrench_publisher_.publish(wrench) + + + def main(args=None): rclpy.init(args=args) node = LQRControllerNode() From 091f6249e54f0ac6b95c88059e9ff97fbc3916c9 Mon Sep 17 00:00:00 2001 From: vortex Date: Wed, 10 Jan 2024 17:45:24 +0100 Subject: [PATCH 10/20] WIP need to fix lqr --- .../los_guidance/los_guidance/los_guidance.py | 1 + .../los_guidance/los_guidance_node.py | 18 +++++----- .../lqr_controller/lqr_controller.py | 2 +- .../lqr_controller/lqr_controller_node.py | 34 +++++++++---------- 4 files changed, 29 insertions(+), 26 deletions(-) diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py index 2a9c81f0..f0584022 100644 --- a/guidance/los_guidance/los_guidance/los_guidance.py +++ b/guidance/los_guidance/los_guidance/los_guidance.py @@ -4,6 +4,7 @@ class LOSGuidance: def __init__(self, p0: list[float], p1: list[float]): self.set_path(p0, p1) + self.heading_ref = 30*np.pi/180 # magic init number!!! def set_path(self, p0: list[float], p1: list[float]): self.p0 = np.array(p0) diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py index 91b7140d..e82a6936 100644 --- a/guidance/los_guidance/los_guidance/los_guidance_node.py +++ b/guidance/los_guidance/los_guidance/los_guidance_node.py @@ -28,19 +28,21 @@ def __init__(self): self.los_guidance = LOSGuidance(p0, p1) - self.guidance_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_callback, 1) + self.guidance_publisher_ = self.create_publisher(Odometry, "controller/lqr/reference", 1) + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1) self.get_logger().info("los_guidance_node started") - def state_callback(self, msg): + def state_cb(self, msg): self.get_logger().info("state_callback") + x = msg.pose.pose.position.x y = msg.pose.pose.position.y orientation_q = msg.pose.pose.orientation orientation_list = [ - orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z ] + # Convert quaternion to Euler angles (roll, pitch, yaw) = quat2euler(orientation_list) @@ -58,10 +60,10 @@ def state_callback(self, msg): odometry_msg.pose.pose.position.x = x_ref[0] odometry_msg.pose.pose.position.y = x_ref[1] odometry_msg.pose.pose.position.z = 0.0 - odometry_msg.pose.pose.orientation.x = orientation_list_ref[0] - odometry_msg.pose.pose.orientation.y = orientation_list_ref[1] - odometry_msg.pose.pose.orientation.z = orientation_list_ref[2] - odometry_msg.pose.pose.orientation.w = orientation_list_ref[3] + odometry_msg.pose.pose.orientation.w = orientation_list_ref[0] + odometry_msg.pose.pose.orientation.x = orientation_list_ref[1] + odometry_msg.pose.pose.orientation.y = orientation_list_ref[2] + odometry_msg.pose.pose.orientation.z = orientation_list_ref[3] self.guidance_publisher_.publish(odometry_msg) diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index cfa1acd9..7b2b2c12 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -31,7 +31,7 @@ def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): def linearize_model(self, heading: float): self.A, self.B = self.asv.linearize_model(heading) - def calculate_control_input(x, x_ref, K_LQR, K_r): + def calculate_control_input(self, x, x_ref, K_LQR, K_r): u = -K_LQR @ x + K_r @ x_ref return u diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 7dbae2d8..e21a2866 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -21,8 +21,8 @@ def __init__(self): ]) self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_callback, 1) - self.guidance_subscriber_ = self.create_subscription(Odometry, "path/pathpath", self.guidance_callback, 1) + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1) + self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value D = self.get_parameter('lqr_controller.D').get_parameter_value().double_array_value @@ -41,33 +41,34 @@ def __init__(self): self.get_logger().info("lqr_controller_node started") - - def guidance_callback(self, msg): - pass - - def state_callback(self, msg): - + def odometrymsg_to_state(self, msg): x = msg.pose.pose.position.x y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation orientation_list = [ orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z ] - - # Convert quaternion to Euler angles - yaw = quat2euler(orientation_list)[2] - # Gjør en relinearisering - self.lqr_controller.linearize_model(yaw) + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] vx = msg.twist.twist.linear.x vy = msg.twist.twist.linear.y vyaw = msg.twist.twist.angular.z state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + + def guidance_cb(self, msg): + self.x_ref = self.odometrymsg_to_state(msg) + + def state_cb(self, msg): + + state = self.odometrymsg_to_state(msg) - # Kjør LQR + self.lqr_controller.linearize_model(state[2]) + + # Run LQR u = self.lqr_controller.calculate_control_input(state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r) wrench = Wrench() @@ -78,11 +79,10 @@ def state_callback(self, msg): wrench.torque.y = 0.0 wrench.torque.z = u[2] - #Publisher til thrust/wrench_input + # Publish thrust/wrench_input self.wrench_publisher_.publish(wrench) - def main(args=None): rclpy.init(args=args) node = LQRControllerNode() From cf7a61ef14b6c575ca768579fc3d56c5b6a17f63 Mon Sep 17 00:00:00 2001 From: vortex Date: Thu, 11 Jan 2024 15:14:34 +0100 Subject: [PATCH 11/20] Fixed x_ref dimensions --- motion/lqr_controller/lqr_controller/lqr_controller.py | 4 ++++ motion/lqr_controller/lqr_controller/lqr_controller_node.py | 5 +++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index 7b2b2c12..0937dc9a 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -32,6 +32,10 @@ def linearize_model(self, heading: float): self.A, self.B = self.asv.linearize_model(heading) def calculate_control_input(self, x, x_ref, K_LQR, K_r): + # print("x:", x) + # print("x_ref:", x) + # print("K_LQR:", K_LQR) + # print("K_r:", K_r) u = -K_LQR @ x + K_r @ x_ref return u diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index e21a2866..01a79e96 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -37,7 +37,8 @@ def __init__(self): self.lqr_controller = LQRController(m, D, Q, R) #self.lqr_controller.run_ivan_sim() - self.x_ref = [0, 0, 0, 0, 0, 0] + # Using x, y, yaw as reference (1x3) + self.x_ref = [0, 0, 0] self.get_logger().info("lqr_controller_node started") @@ -60,7 +61,7 @@ def odometrymsg_to_state(self, msg): return state def guidance_cb(self, msg): - self.x_ref = self.odometrymsg_to_state(msg) + self.x_ref = self.odometrymsg_to_state(msg)[:3] def state_cb(self, msg): From 7ec8ebad8c25eaca2895afea734b7cd238a107e5 Mon Sep 17 00:00:00 2001 From: vortex Date: Thu, 11 Jan 2024 15:38:57 +0100 Subject: [PATCH 12/20] Made package --- motion/vessel_simulator/LICENSE | 17 ++++++ motion/vessel_simulator/package.xml | 20 +++++++ .../resource/vessel_simulator | 0 motion/vessel_simulator/setup.cfg | 4 ++ motion/vessel_simulator/setup.py | 26 +++++++++ .../vessel_simulator/__init__.py | 0 .../vessel_simulator/vessel_simulator_node.py | 53 +++++++++++++++++++ 7 files changed, 120 insertions(+) create mode 100644 motion/vessel_simulator/LICENSE create mode 100644 motion/vessel_simulator/package.xml create mode 100644 motion/vessel_simulator/resource/vessel_simulator create mode 100644 motion/vessel_simulator/setup.cfg create mode 100644 motion/vessel_simulator/setup.py create mode 100644 motion/vessel_simulator/vessel_simulator/__init__.py create mode 100644 motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py diff --git a/motion/vessel_simulator/LICENSE b/motion/vessel_simulator/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/motion/vessel_simulator/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/motion/vessel_simulator/package.xml b/motion/vessel_simulator/package.xml new file mode 100644 index 00000000..c9aeb96b --- /dev/null +++ b/motion/vessel_simulator/package.xml @@ -0,0 +1,20 @@ + + + + vessel_simulator + 0.0.0 + TODO: Package description + vortex + MIT + + lqr_controller + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/motion/vessel_simulator/resource/vessel_simulator b/motion/vessel_simulator/resource/vessel_simulator new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator/setup.cfg b/motion/vessel_simulator/setup.cfg new file mode 100644 index 00000000..d28c4cc5 --- /dev/null +++ b/motion/vessel_simulator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/vessel_simulator +[install] +install_scripts=$base/lib/vessel_simulator diff --git a/motion/vessel_simulator/setup.py b/motion/vessel_simulator/setup.py new file mode 100644 index 00000000..0948f249 --- /dev/null +++ b/motion/vessel_simulator/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'vessel_simulator' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='vortex', + maintainer_email='vortex.git@vortexntnu.no', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'vessel_simulator_node = vessel_simulator.vessel_simulator_node:main' + ], + }, +) diff --git a/motion/vessel_simulator/vessel_simulator/__init__.py b/motion/vessel_simulator/vessel_simulator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py new file mode 100644 index 00000000..e044be1a --- /dev/null +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -0,0 +1,53 @@ +import rclpy +from std_msgs.msg import Float64MultiArray, Wrench +from geometry_msgs.msg import Point +from nav_msgs.msg import Odometry +from transforms3d.euler import quat2euler, euler2quat +import numpy as np +import matplotlib.pyplot as plt +from scipy.linalg import solve_continuous_are +# Assuming your_module.py is in the same package as lqr_controller +from lqr_controller.lqr_controller import LQRController +from lqr_controller.asv_model import ASV + + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi + +class VesselVisualizer: + # ... (unchanged) + + def update(self, frame): + # ... (unchanged) + + # Assuming self.vessel.state is in the order: [x, y, yaw, vx, vy, vyaw] + odom = Odometry() + odom.pose.pose.position = Point(self.vessel.state[0], self.vessel.state[1], 0) + quaternion = quaternion_from_euler(0, 0, self.vessel.state[2]) + odom.pose.pose.orientation = Quaternion(*quaternion) + odom.twist.twist = Twist(Vector3(self.vessel.state[3], self.vessel.state[4], 0), + Vector3(0, 0, self.vessel.state[5])) + self.odom_pub.publish(odom) + + # ... (unchanged) + +if __name__ == '__main__': + rclpy.init() + + # Initiate the vessel + mass = 50.0 + damping_x = 5.0 + damping_y = 20.0 + damping_psi = 15.0 + inertia = 5.0 + + M = np.diag([mass, mass, inertia]) + D = np.diag([damping_x, damping_y, damping_psi]) + lqr_controller = LQRController(mass, [damping_x, damping_y, damping_psi], [10.0, 10.0, 5.0, 0.1, 1.0, 5.0], [1.0, 1.0, 1.0]) + simulated_vessel = ASV(M, D) + + visualizer = VesselVisualizer(simulated_vessel) + visualizer.animate() + + rclpy.spin() + rclpy.shutdown() From dd09e3fe8803c359a4d01a183485a15d2d75082f Mon Sep 17 00:00:00 2001 From: vortex Date: Fri, 12 Jan 2024 14:56:19 +0100 Subject: [PATCH 13/20] WIP stuff --- .../lqr_controller/asv_model.py | 12 -- .../vessel_simulator/vessel_simulator_node.py | 160 ++++++++++++++---- 2 files changed, 129 insertions(+), 43 deletions(-) diff --git a/motion/lqr_controller/lqr_controller/asv_model.py b/motion/lqr_controller/lqr_controller/asv_model.py index 88bc8ea7..f3b1cc07 100644 --- a/motion/lqr_controller/lqr_controller/asv_model.py +++ b/motion/lqr_controller/lqr_controller/asv_model.py @@ -90,15 +90,3 @@ def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: return A, B - def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: - - # integration scheme for simulation, implements the Runge-Kutta 4 integrator - - k1 = self.state_dot(x, u) - k2 = self.state_dot(x+dt/2*k1, u) - k3 = self.state_dot(x+dt/2*k2, u) - k4 = self.state_dot(x+dt*k3, u) - - x_next = x + dt/6*(k1+2*k2+2*k3+k4) - - return x_next \ No newline at end of file diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py index e044be1a..f611713d 100644 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -1,53 +1,151 @@ import rclpy +from rclpy.node import Node + from std_msgs.msg import Float64MultiArray, Wrench +from geometry_msgs.msg import Wrench from geometry_msgs.msg import Point from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler, euler2quat + import numpy as np import matplotlib.pyplot as plt from scipy.linalg import solve_continuous_are -# Assuming your_module.py is in the same package as lqr_controller -from lqr_controller.lqr_controller import LQRController -from lqr_controller.asv_model import ASV - def ssa(angle): return (angle + np.pi) % (2 * np.pi) - np.pi -class VesselVisualizer: - # ... (unchanged) +class VesselVisualizerNode(Node): + def __init__(self): + super().__init__("vessel_visualizer_node") + # add wrench_input subscriber + self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1) + # publish state (seapath) + self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) - def update(self, frame): - # ... (unchanged) + self.x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) + self.x_ref = np.array([0, 0, self.heading_ref]) + self.dt = 0.01 - # Assuming self.vessel.state is in the order: [x, y, yaw, vx, vy, vyaw] - odom = Odometry() - odom.pose.pose.position = Point(self.vessel.state[0], self.vessel.state[1], 0) - quaternion = quaternion_from_euler(0, 0, self.vessel.state[2]) - odom.pose.pose.orientation = Quaternion(*quaternion) - odom.twist.twist = Twist(Vector3(self.vessel.state[3], self.vessel.state[4], 0), - Vector3(0, 0, self.vessel.state[5])) - self.odom_pub.publish(odom) + def wrench_input_cb(self, msg): + u = np.array([msg.force.x, msg.force.y, msg.torque.z]) + x_next = self.RK4_integration_step(self, self.x_init, u, self.dt) - # ... (unchanged) + -if __name__ == '__main__': - rclpy.init() + + + def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: + """ + Calculate the derivative of the state using the non-linear kinematics + """ + heading = state[2] + + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - self.M_inv @ self.D + + B = np.zeros((6,3)) + B[3:,:] = self.M_inv + + x_dot = A @ state + B @ tau_actuation + x_dot[0:2] += V_current # add current drift term at velocity level + + return x_dot + + def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: + # integration scheme for simulation, implements the Runge-Kutta 4 integrator + k1 = self.state_dot(x, u) + k2 = self.state_dot(x+dt/2*k1, u) + k3 = self.state_dot(x+dt/2*k2, u) + k4 = self.state_dot(x+dt*k3, u) + + x_next = x + dt/6*(k1+2*k2+2*k3+k4) + + return x_next + + def run_ivan_sim(self): - # Initiate the vessel - mass = 50.0 - damping_x = 5.0 - damping_y = 20.0 - damping_psi = 15.0 - inertia = 5.0 - M = np.diag([mass, mass, inertia]) - D = np.diag([damping_x, damping_y, damping_psi]) - lqr_controller = LQRController(mass, [damping_x, damping_y, damping_psi], [10.0, 10.0, 5.0, 0.1, 1.0, 5.0], [1.0, 1.0, 1.0]) - simulated_vessel = ASV(M, D) + look_ahead = 5 # Look ahead distance + + T = 69.0 # Total simulation time + dt = 0.01 # Time step + num_steps = int(T / dt) # Number of time steps + + # Initialize arrays to store data + time = np.linspace(0, T, num_steps+1) + x_history = np.zeros((num_steps+1, self.A.shape[0])) + u_history = np.zeros((num_steps+1, self.B.shape[1])) + x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) + + cross_track_error_history = np.zeros(num_steps+1) + + # Simulation loop + x = x_init # Initial state + for i in range(num_steps+1): + # x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step + # x_ref_history[i, :] = x_ref # Update the reference at each time step + + # generate reference at the look-ahead distance + p_asv = np.array([x[0], x[1]]) + errors = self.R_Pi_p.T @ (p_asv - self.p0) + along_track_error = errors[0] + p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 + + x_ref[:2] = p_los_world # Update the position reference at each time step + x_ref_history[i, :] = x_ref + + u = self.calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step + x = self.asv.RK4_integration_step(x, u, dt) + + x_history[i] = x # Store state history + u_history[i] = u # Store control input history + + # Plot results + plt.figure(figsize=(12, 8)) + plt.subplot(3, 1, 1) + for j in range(3): + plt.plot(time, x_history[:, j], label=f'State (x_{j+1})') + plt.plot(time, x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') + plt.xlabel('Time') + plt.ylabel('State Value') + plt.legend() + + plt.subplot(3, 1, 2) + for j in range(self.B.shape[1]): + plt.plot(time, u_history[:, j], label=f'Control Input (u_{j+1})') + plt.xlabel('Time') + plt.ylabel('Control Input Value') + plt.legend() + + plt.subplot(3, 1, 3) + plt.scatter(x_history[:,0], x_history[:,1], label=f'Position') + plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path') + plt.xlabel('x') + plt.ylabel('y') + plt.legend() + + plt.tight_layout() + plt.show() + plt.figure(1) + plt.plot(time, cross_track_error_history, label="cross track error") + plt.axis("equal") + plt.plot(time, np.zeros_like(time)) + plt.legend() + plt.show() + + + +if __name__ == '__main__': + rclpy.init() - visualizer = VesselVisualizer(simulated_vessel) - visualizer.animate() rclpy.spin() rclpy.shutdown() From 46b4327a918492e3e3e821588dab9dab776c5155 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Sun, 21 Jan 2024 14:36:22 +0100 Subject: [PATCH 14/20] Vessel Simulator working, added giraffe --- asv_setup/launch/ASV_simulator.launch.py | 35 ++++ asv_setup/launch/vessel_sim.launch.py | 35 ++++ .../los_guidance/los_guidance_node.py | 8 +- .../lqr_controller/lqr_controller_node.py | 21 +- .../launch/vessel_simulator.launch.py | 17 ++ motion/vessel_simulator/package.xml | 9 + motion/vessel_simulator/setup.py | 3 + .../vessel_simulator/vessel_simulator_node.py | 184 +++++++++++------- 8 files changed, 234 insertions(+), 78 deletions(-) create mode 100644 asv_setup/launch/ASV_simulator.launch.py create mode 100644 asv_setup/launch/vessel_sim.launch.py create mode 100644 motion/vessel_simulator/launch/vessel_simulator.launch.py diff --git a/asv_setup/launch/ASV_simulator.launch.py b/asv_setup/launch/ASV_simulator.launch.py new file mode 100644 index 00000000..a68b94c7 --- /dev/null +++ b/asv_setup/launch/ASV_simulator.launch.py @@ -0,0 +1,35 @@ +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(): + # LQR_controller launch + lqr_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('lqr_controller'), 'launch/lqr_controller.launch.py') + ) + ) + + # LOS_guidance launch + los_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('los_guidance'), 'launch/los_guidance.launch.py') + ) + ) + + # Vessel_simulator launch + vessel_simulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('vessel_simulator'), 'launch/vessel_simulator.launch.py') + ) + ) + + # Return launch description + return LaunchDescription([ + lqr_controller_launch, + los_guidance_launch, + vessel_simulator_launch + ]) \ No newline at end of file diff --git a/asv_setup/launch/vessel_sim.launch.py b/asv_setup/launch/vessel_sim.launch.py new file mode 100644 index 00000000..7bea0071 --- /dev/null +++ b/asv_setup/launch/vessel_sim.launch.py @@ -0,0 +1,35 @@ +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(): + # LQR launch + lqr_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('lqr_controller'), 'launch/lqr_controller.launch.py') + ) + ) + + # LOS_guidance launch + los_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('los_guidance'), 'launch/los_guidance.launch.py') + ) + ) + + vessel_simulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('vessel_simulator'), 'launch/vessel_simulator.launch.py') + ) + ) + + + # Return launch description + return LaunchDescription([ + lqr_controller_launch, + los_guidance_launch, + vessel_simulator_launch + ]) \ No newline at end of file diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py index e82a6936..c4fc3256 100644 --- a/guidance/los_guidance/los_guidance/los_guidance_node.py +++ b/guidance/los_guidance/los_guidance/los_guidance_node.py @@ -14,16 +14,16 @@ def __init__(self): parameters=[ ('los_guidance.p0', [0.0, 0.0]), ('los_guidance.p1', [0.0, 0.0]), - ('los_guidance.look_ahead', 0.0) + ('los_guidance.look_ahead_distance', 0.0) ]) p0 = self.get_parameter('los_guidance.p0').get_parameter_value().double_array_value p1 = self.get_parameter('los_guidance.p1').get_parameter_value().double_array_value - self.look_ahead = self.get_parameter('los_guidance.look_ahead').get_parameter_value().double_value + self.look_ahead = self.get_parameter('los_guidance.look_ahead_distance').get_parameter_value().double_value self.get_logger().info(f"p0: {p0}") self.get_logger().info(f"p1: {p1}") - self.get_logger().info(f"look_ahead: {self.look_ahead}") + self.get_logger().info(f"look_ahead_distance: {self.look_ahead}") self.los_guidance = LOSGuidance(p0, p1) @@ -34,7 +34,7 @@ def __init__(self): self.get_logger().info("los_guidance_node started") def state_cb(self, msg): - self.get_logger().info("state_callback") + # self.get_logger().info("state_callback") x = msg.pose.pose.position.x y = msg.pose.pose.position.y diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 01a79e96..a3a31016 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -39,6 +39,7 @@ def __init__(self): # Using x, y, yaw as reference (1x3) self.x_ref = [0, 0, 0] + self.state = [0, 0, 0, 0, 0, 0] self.get_logger().info("lqr_controller_node started") @@ -63,14 +64,17 @@ def odometrymsg_to_state(self, msg): def guidance_cb(self, msg): self.x_ref = self.odometrymsg_to_state(msg)[:3] - def state_cb(self, msg): - - state = self.odometrymsg_to_state(msg) - - self.lqr_controller.linearize_model(state[2]) + wrench = self.run_lqr_to_wrench() + + # Publish thrust/wrench_input + self.wrench_publisher_.publish(wrench) - # Run LQR - u = self.lqr_controller.calculate_control_input(state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r) + def state_cb(self, msg): + self.state = self.odometrymsg_to_state(msg) + + def run_lqr_to_wrench(self): + self.lqr_controller.linearize_model(self.state[2]) + u = self.lqr_controller.calculate_control_input(self.state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r) wrench = Wrench() wrench.force.x = u[0] @@ -80,8 +84,7 @@ def state_cb(self, msg): wrench.torque.y = 0.0 wrench.torque.z = u[2] - # Publish thrust/wrench_input - self.wrench_publisher_.publish(wrench) + return wrench def main(args=None): diff --git a/motion/vessel_simulator/launch/vessel_simulator.launch.py b/motion/vessel_simulator/launch/vessel_simulator.launch.py new file mode 100644 index 00000000..e9c7d5cb --- /dev/null +++ b/motion/vessel_simulator/launch/vessel_simulator.launch.py @@ -0,0 +1,17 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + vessel_simulator_node = Node( + package='vessel_simulator', + executable='vessel_simulator_node', + name='vessel_simulator_node', + # parameters=[os.path.join(get_package_share_directory('vessel_simulator'),'config','lqr_config.yaml')], + output='screen', + ) + return LaunchDescription([ + vessel_simulator_node + ]) + diff --git a/motion/vessel_simulator/package.xml b/motion/vessel_simulator/package.xml index c9aeb96b..00033eb7 100644 --- a/motion/vessel_simulator/package.xml +++ b/motion/vessel_simulator/package.xml @@ -8,6 +8,15 @@ MIT lqr_controller + rclpy + std_msgs + geometry_msgs + nav_msgs + transforms3d + numpy + matplotlib + scipy + ament_copyright ament_flake8 diff --git a/motion/vessel_simulator/setup.py b/motion/vessel_simulator/setup.py index 0948f249..58c3dba8 100644 --- a/motion/vessel_simulator/setup.py +++ b/motion/vessel_simulator/setup.py @@ -1,3 +1,5 @@ +import os +from glob import glob from setuptools import find_packages, setup package_name = 'vessel_simulator' @@ -10,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py index f611713d..2a683295 100644 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -1,15 +1,13 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import Float64MultiArray, Wrench from geometry_msgs.msg import Wrench -from geometry_msgs.msg import Point from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler, euler2quat +from transforms3d.euler import euler2quat, quat2euler import numpy as np import matplotlib.pyplot as plt -from scipy.linalg import solve_continuous_are +import time def ssa(angle): return (angle + np.pi) % (2 * np.pi) - np.pi @@ -19,19 +17,110 @@ def __init__(self): super().__init__("vessel_visualizer_node") # add wrench_input subscriber self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1) + self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) + # publish state (seapath) self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) + + self.state = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) + self.x_ref = np.array([0, 0, 40*np.pi/180]) # Note to self: make x_ref have 6 states + + self.cb_count = 0 - self.x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) - self.x_ref = np.array([0, 0, self.heading_ref]) + m = 50.0 + self.M = np.diag([m, m, m]) + self.M_inv = np.linalg.inv(self.M) + self.D = np.diag([10.0, 10.0, 5.0]) + + self.T = 69.0 self.dt = 0.01 + self.num_steps = int(self.T / self.dt) + + self.time = np.linspace(0, self.T, self.num_steps+1) + self.x_history = np.zeros((self.num_steps+1, 6)) #6 rows in A + self.x_ref_history = np.zeros((self.num_steps+1, 3)) # defined as length 3 for now + self.u_history = np.zeros((self.num_steps+1, 3)) #3 columns in B + + self.get_logger().info("vessel_visualizer_node started") + self.get_logger().info("len(self.x_history): " + str(len(self.x_history)) + "self.num_steps" + str(self.num_steps)) + + # Send x_init + self.get_logger().info("Simulation starting in 3 seconds...") + time.sleep(3) + + self.get_logger().info("""\ + + ._ o o + \_`-)|_ + ,"" \ + ," ## | ಠ ಠ. + ," ## ,-\__ `. + ," / `--._;) + ," ## / + ," ## / + + Waiting for simulation to finish...:) + """) + self.state_publisher_.publish(self.state_to_odometrymsg(self.state)) + + # while(self.cb_count < self.num_steps): + # self.get_logger().info(str(self.cb_count) + " lol xD") # heh DO NOT REMOVE + + + def odometrymsg_to_state(self, msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + + def guidance_cb(self, msg): + self.x_ref = self.odometrymsg_to_state(msg)[:3] + self.x_ref_history[self.cb_count, : ] = self.x_ref + + def state_to_odometrymsg(self, state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg def wrench_input_cb(self, msg): u = np.array([msg.force.x, msg.force.y, msg.torque.z]) - x_next = self.RK4_integration_step(self, self.x_init, u, self.dt) + x_next = self.RK4_integration_step(self.state, u, self.dt) + self.x_history[self.cb_count] = x_next + self.u_history[self.cb_count] = u - + odometry_msg = self.state_to_odometrymsg(x_next) + + # Update state + self.state = x_next + + self.state_publisher_.publish(odometry_msg) + self.cb_count += 1 + # self.get_logger().info(str(self.cb_count) + " lol xD") # heh DO NOT REMOVE + + if (self.cb_count == self.num_steps): + self.plot_history() def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: @@ -70,82 +159,47 @@ def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.nd return x_next - def run_ivan_sim(self): - - - look_ahead = 5 # Look ahead distance - - T = 69.0 # Total simulation time - dt = 0.01 # Time step - num_steps = int(T / dt) # Number of time steps - - # Initialize arrays to store data - time = np.linspace(0, T, num_steps+1) - x_history = np.zeros((num_steps+1, self.A.shape[0])) - u_history = np.zeros((num_steps+1, self.B.shape[1])) - x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) - - cross_track_error_history = np.zeros(num_steps+1) - - # Simulation loop - x = x_init # Initial state - for i in range(num_steps+1): - # x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step - # x_ref_history[i, :] = x_ref # Update the reference at each time step - - # generate reference at the look-ahead distance - p_asv = np.array([x[0], x[1]]) - errors = self.R_Pi_p.T @ (p_asv - self.p0) - along_track_error = errors[0] - p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 - - x_ref[:2] = p_los_world # Update the position reference at each time step - x_ref_history[i, :] = x_ref - - u = self.calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step - x = self.asv.RK4_integration_step(x, u, dt) - - x_history[i] = x # Store state history - u_history[i] = u # Store control input history - + def plot_history(self): # Plot results + # self.get_logger().info(str(self.x_history[:,0])) + # print(self.x_history[:,0]) + # print(self.x_history[:,1]) + plt.figure(figsize=(12, 8)) plt.subplot(3, 1, 1) for j in range(3): - plt.plot(time, x_history[:, j], label=f'State (x_{j+1})') - plt.plot(time, x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') + plt.plot(self.time, self.x_history[:, j], label=f'State (x_{j+1})') + plt.plot(self.time, self.x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') plt.xlabel('Time') plt.ylabel('State Value') plt.legend() plt.subplot(3, 1, 2) - for j in range(self.B.shape[1]): - plt.plot(time, u_history[:, j], label=f'Control Input (u_{j+1})') + for j in range(3): #3 columns in B + plt.plot(self.time, self.u_history[:, j], label=f'Control Input (u_{j+1})') plt.xlabel('Time') plt.ylabel('Control Input Value') plt.legend() plt.subplot(3, 1, 3) - plt.scatter(x_history[:,0], x_history[:,1], label=f'Position') - plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path') + plt.scatter(self.x_history[:,0], self.x_history[:,1], label=f'Position') + + p0 = [0.0, 0.0] + p1 = [20.0, 20.0] + plt.plot([p0[0], p1[0]], [p0[1], p1[1]], 'r-', label='Path') plt.xlabel('x') plt.ylabel('y') plt.legend() plt.tight_layout() plt.show() - plt.figure(1) - plt.plot(time, cross_track_error_history, label="cross track error") - plt.axis("equal") - plt.plot(time, np.zeros_like(time)) - plt.legend() - plt.show() - - - -if __name__ == '__main__': - rclpy.init() +def main(args=None): + rclpy.init(args=args) + node = VesselVisualizerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() - rclpy.spin() - rclpy.shutdown() +if __name__ == "__main__": + main() From 426945a8696e8bc51b51de1d76a7eb1dd501e1b1 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Sun, 21 Jan 2024 18:45:59 +0100 Subject: [PATCH 15/20] Updated giraffe message, added write np.array to file --- .../vessel_simulator/vessel_simulator_node.py | 39 ++++++++++--------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py index 2a683295..e13ccf25 100644 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -23,16 +23,16 @@ def __init__(self): self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) self.state = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) - self.x_ref = np.array([0, 0, 40*np.pi/180]) # Note to self: make x_ref have 6 states + self.x_ref = np.array([0, 0, 50*np.pi/180]) # Note to self: make x_ref have 6 states - self.cb_count = 0 + self.num_steps_simulated = 0 m = 50.0 self.M = np.diag([m, m, m]) self.M_inv = np.linalg.inv(self.M) self.D = np.diag([10.0, 10.0, 5.0]) - self.T = 69.0 + self.T = 20.0 self.dt = 0.01 self.num_steps = int(self.T / self.dt) @@ -45,8 +45,8 @@ def __init__(self): self.get_logger().info("len(self.x_history): " + str(len(self.x_history)) + "self.num_steps" + str(self.num_steps)) # Send x_init - self.get_logger().info("Simulation starting in 3 seconds...") - time.sleep(3) + self.get_logger().info("Simulation starting in 1 second...") + time.sleep(1) self.get_logger().info("""\ @@ -56,17 +56,13 @@ def __init__(self): ," ## | ಠ ಠ. ," ## ,-\__ `. ," / `--._;) - ," ## / + ," ## / U ," ## / - Waiting for simulation to finish...:) + Waiting for simulation to finish...""" + str(self.T) + """ secs approximated waiting time :) """) self.state_publisher_.publish(self.state_to_odometrymsg(self.state)) - # while(self.cb_count < self.num_steps): - # self.get_logger().info(str(self.cb_count) + " lol xD") # heh DO NOT REMOVE - - def odometrymsg_to_state(self, msg): x = msg.pose.pose.position.x y = msg.pose.pose.position.y @@ -87,7 +83,6 @@ def odometrymsg_to_state(self, msg): def guidance_cb(self, msg): self.x_ref = self.odometrymsg_to_state(msg)[:3] - self.x_ref_history[self.cb_count, : ] = self.x_ref def state_to_odometrymsg(self, state): orientation_list_next = euler2quat(0, 0, state[2]) @@ -106,8 +101,14 @@ def state_to_odometrymsg(self, state): def wrench_input_cb(self, msg): u = np.array([msg.force.x, msg.force.y, msg.torque.z]) x_next = self.RK4_integration_step(self.state, u, self.dt) - self.x_history[self.cb_count] = x_next - self.u_history[self.cb_count] = u + self.x_history[self.num_steps_simulated] = x_next + self.x_ref_history[self.num_steps_simulated, : ] = self.x_ref + self.u_history[self.num_steps_simulated] = u + print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") + + if (self.num_steps_simulated >= self.num_steps): + self.plot_history() + return odometry_msg = self.state_to_odometrymsg(x_next) @@ -115,14 +116,10 @@ def wrench_input_cb(self, msg): self.state = x_next self.state_publisher_.publish(odometry_msg) - self.cb_count += 1 + self.num_steps_simulated += 1 # self.get_logger().info(str(self.cb_count) + " lol xD") # heh DO NOT REMOVE - if (self.cb_count == self.num_steps): - self.plot_history() - - def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: """ Calculate the derivative of the state using the non-linear kinematics @@ -165,6 +162,10 @@ def plot_history(self): # print(self.x_history[:,0]) # print(self.x_history[:,1]) + # Save the array to a text file + # file_path = '/home/martin/x_ref_hist.txt' # martin stays + # np.savetxt(file_path, self.x_ref_history, fmt='%4f', delimiter=',') + plt.figure(figsize=(12, 8)) plt.subplot(3, 1, 1) for j in range(3): From c16897b2475a6dae8000756a2faebad84d49dfc7 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Wed, 24 Jan 2024 17:26:06 +0100 Subject: [PATCH 16/20] Changed magic number to be like Ivan sim (still not work) --- guidance/los_guidance/los_guidance/los_guidance.py | 2 +- .../vessel_simulator/vessel_simulator/vessel_simulator_node.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py index f0584022..b3486e1e 100644 --- a/guidance/los_guidance/los_guidance/los_guidance.py +++ b/guidance/los_guidance/los_guidance/los_guidance.py @@ -4,7 +4,7 @@ class LOSGuidance: def __init__(self, p0: list[float], p1: list[float]): self.set_path(p0, p1) - self.heading_ref = 30*np.pi/180 # magic init number!!! + self.heading_ref = 50*np.pi/180 # magic init number!!! def set_path(self, p0: list[float], p1: list[float]): self.p0 = np.array(p0) diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py index e13ccf25..3fdfc4b1 100644 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -104,7 +104,7 @@ def wrench_input_cb(self, msg): self.x_history[self.num_steps_simulated] = x_next self.x_ref_history[self.num_steps_simulated, : ] = self.x_ref self.u_history[self.num_steps_simulated] = u - print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") + # print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") if (self.num_steps_simulated >= self.num_steps): self.plot_history() From 42f7500a592b1406fc71a74b1ef930fc587d2b41 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Thu, 25 Jan 2024 12:15:03 +0100 Subject: [PATCH 17/20] Edited points --- .../los_guidance/los_guidance/los_guidance.py | 20 +++++++++++++++++++ .../vessel_simulator/vessel_simulator_node.py | 17 +++++++++++++--- 2 files changed, 34 insertions(+), 3 deletions(-) diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py index b3486e1e..a131f7a4 100644 --- a/guidance/los_guidance/los_guidance/los_guidance.py +++ b/guidance/los_guidance/los_guidance/los_guidance.py @@ -5,6 +5,7 @@ class LOSGuidance: def __init__(self, p0: list[float], p1: list[float]): self.set_path(p0, p1) self.heading_ref = 50*np.pi/180 # magic init number!!! + self.p_next = [np.array([50, -40]), np.array([20.0, -80.0]), np.array([120.0, -60.0]), np.array([160, 0.0]), np.array([60.0, 60.0])] def set_path(self, p0: list[float], p1: list[float]): self.p0 = np.array(p0) @@ -16,6 +17,12 @@ def calculate_R_Pi_p(self): [[np.cos(self.Pi_p), -np.sin(self.Pi_p)], [np.sin(self.Pi_p), np.cos(self.Pi_p)]] ) + + def calculate_distance(self, p0, p1): + return np.sqrt((p0[0]-p1[0])**2 + (p0[1]-p1[1])**2) + + def switch_path(self): + self.p0, self.p1 = self.p1, self.p_next[0] def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: self.set_path(self.p0, self.p1) @@ -23,7 +30,20 @@ def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: p_asv = np.array([x[0], x[1]]) errors = self.R_Pi_p.T @ (p_asv - self.p0) along_track_error = errors[0] + + # Switch points to next pair if crossed orthogonal line + if (along_track_error > self.calculate_distance(self.p0, self.p1)): + self.switch_path() + self.calculate_R_Pi_p() + errors = self.R_Pi_p.T @ (p_asv - self.p0) + along_track_error = errors[0] + self.p_next.pop(0) + if (self.p_next == []): + self.p_next = [np.array([100000.0, 1000000.0])] + + p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 x_ref = np.array([p_los_world[0], p_los_world[1], self.heading_ref]) return x_ref + diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py index 3fdfc4b1..1595c00f 100644 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -32,7 +32,7 @@ def __init__(self): self.M_inv = np.linalg.inv(self.M) self.D = np.diag([10.0, 10.0, 5.0]) - self.T = 20.0 + self.T = 200.0 self.dt = 0.01 self.num_steps = int(self.T / self.dt) @@ -118,7 +118,7 @@ def wrench_input_cb(self, msg): self.state_publisher_.publish(odometry_msg) self.num_steps_simulated += 1 - # self.get_logger().info(str(self.cb_count) + " lol xD") # heh DO NOT REMOVE + self.get_logger().info(str(self.num_steps_simulated) + " lol xD") # heh DO NOT REMOVE def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: """ @@ -183,11 +183,22 @@ def plot_history(self): plt.legend() plt.subplot(3, 1, 3) - plt.scatter(self.x_history[:,0], self.x_history[:,1], label=f'Position') + plt.scatter(self.x_history[:, 0], self.x_history[:, 1], label='Position', s=5) p0 = [0.0, 0.0] p1 = [20.0, 20.0] + p2 = [50.0, -40.0] + p3 = [20.0, -80.0] + p4 = [120.0, -60.0] + p5 = [160, 0.0] + p6 = [60.0, 60.0] plt.plot([p0[0], p1[0]], [p0[1], p1[1]], 'r-', label='Path') + plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'g-', label='Path') + plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'b-', label='Path') + plt.plot([p3[0], p4[0]], [p3[1], p4[1]], 'y-', label='Path') + plt.plot([p4[0], p5[0]], [p4[1], p5[1]], 'm-', label='Path') + plt.plot([p5[0], p6[0]], [p5[1], p6[1]], 'c-', label='Path') + plt.xlabel('x') plt.ylabel('y') plt.legend() From 74370e936be143825b87e6aadc42de6ea5f8b878 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 28 Jan 2024 18:28:48 +0100 Subject: [PATCH 18/20] 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 --- .../los_guidance/los_guidance/los_guidance.py | 11 +- .../los_guidance/los_guidance_node.py | 12 +- .../lqr_controller/asv_model.py | 20 --- .../lqr_controller/lqr_controller.py | 131 +++++++++--------- .../lqr_controller/lqr_controller_node.py | 9 +- .../vessel_simulator/vessel_simulator_node.py | 22 +-- 6 files changed, 100 insertions(+), 105 deletions(-) diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py index a131f7a4..500c8059 100644 --- a/guidance/los_guidance/los_guidance/los_guidance.py +++ b/guidance/los_guidance/los_guidance/los_guidance.py @@ -1,11 +1,12 @@ import numpy as np -import matplotlib.pyplot as plt class LOSGuidance: - def __init__(self, p0: list[float], p1: list[float]): + def __init__(self, p0: list[float], p1: list[float], p_next: list[float]): self.set_path(p0, p1) self.heading_ref = 50*np.pi/180 # magic init number!!! + self.p_next = [np.array([40, 40]), np.array([90.0, -20.0]), np.array([120.0, 50.0])]#, np.array([160, 0.0]), np.array([60.0, 60.0])] self.p_next = [np.array([50, -40]), np.array([20.0, -80.0]), np.array([120.0, -60.0]), np.array([160, 0.0]), np.array([60.0, 60.0])] + self.acceptance_radius = 0.5 def set_path(self, p0: list[float], p1: list[float]): self.p0 = np.array(p0) @@ -23,7 +24,7 @@ def calculate_distance(self, p0, p1): def switch_path(self): self.p0, self.p1 = self.p1, self.p_next[0] - + def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: self.set_path(self.p0, self.p1) self.calculate_R_Pi_p() @@ -31,8 +32,8 @@ def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray: errors = self.R_Pi_p.T @ (p_asv - self.p0) along_track_error = errors[0] - # Switch points to next pair if crossed orthogonal line - if (along_track_error > self.calculate_distance(self.p0, self.p1)): + # Switch points to next pair if crossed orthogonal line or entered circle of acceptance + if ((along_track_error > 0.8*self.calculate_distance(self.p0, self.p1))): self.switch_path() self.calculate_R_Pi_p() errors = self.R_Pi_p.T @ (p_asv - self.p0) diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py index c4fc3256..0341bb3c 100644 --- a/guidance/los_guidance/los_guidance/los_guidance_node.py +++ b/guidance/los_guidance/los_guidance/los_guidance_node.py @@ -3,6 +3,7 @@ from rclpy.node import Node from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler, euler2quat +from geometry_msgs.msg import Point from los_guidance.los_guidance import LOSGuidance class LOSGuidanceNode(Node): @@ -20,16 +21,18 @@ def __init__(self): p0 = self.get_parameter('los_guidance.p0').get_parameter_value().double_array_value p1 = self.get_parameter('los_guidance.p1').get_parameter_value().double_array_value self.look_ahead = self.get_parameter('los_guidance.look_ahead_distance').get_parameter_value().double_value + + self.p_next = np.array([10000.0, 10000.0]) self.get_logger().info(f"p0: {p0}") self.get_logger().info(f"p1: {p1}") self.get_logger().info(f"look_ahead_distance: {self.look_ahead}") - self.los_guidance = LOSGuidance(p0, p1) - + self.los_guidance = LOSGuidance(p0, p1, self.p_next) self.guidance_publisher_ = self.create_publisher(Odometry, "controller/lqr/reference", 1) self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1) + self.point_subscriber_ = self.create_subscription(Point, "los/point", self.point_cb, 1) self.get_logger().info("los_guidance_node started") @@ -67,6 +70,11 @@ def state_cb(self, msg): self.guidance_publisher_.publish(odometry_msg) + #rate = rclpy.Rate(100) + + def point_cb(self, msg): + self.p_next.append(np.array([msg.x, msg.y])) + def main(args=None): rclpy.init(args=args) diff --git a/motion/lqr_controller/lqr_controller/asv_model.py b/motion/lqr_controller/lqr_controller/asv_model.py index f3b1cc07..09231ce1 100644 --- a/motion/lqr_controller/lqr_controller/asv_model.py +++ b/motion/lqr_controller/lqr_controller/asv_model.py @@ -70,23 +70,3 @@ def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np. return x_dot - def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: - """ - Get a linearization about some heading - """ - J = np.array( - [[np.cos(heading), -np.sin(heading), 0], - [np.sin(heading), np.cos(heading), 0], - [0, 0, 1]] - ) - - A = np.zeros((6,6)) - - A[:3,3:] = J - A[3:, 3:] = - self.M_inv @ self.D - - B = np.zeros((6,3)) - B[3:,:] = self.M_inv - - return A, B - diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index 0937dc9a..7c977f8a 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -9,7 +9,8 @@ def ssa(angle): class LQRController: def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): - self.heading_ref = 30*np.pi/180 # magic init number!!! + #self.heading_ref = 30*np.pi/180 # magic init number!!! + self.heading_ref = 50*np.pi/180 self.M = np.diag([m, m, m]) self.M_inv = np.linalg.inv(self.M) @@ -39,77 +40,77 @@ def calculate_control_input(self, x, x_ref, K_LQR, K_r): u = -K_LQR @ x + K_r @ x_ref return u - def run_ivan_sim(self): - x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) - x_ref = np.array([0, 0, self.heading_ref]) + # def run_ivan_sim(self): + # x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) + # x_ref = np.array([0, 0, self.heading_ref]) - look_ahead = 5 # Look ahead distance + # look_ahead = 5 # Look ahead distance - T = 69.0 # Total simulation time - dt = 0.01 # Time step - num_steps = int(T / dt) # Number of time steps + # T = 69.0 # Total simulation time + # dt = 0.01 # Time step + # num_steps = int(T / dt) # Number of time steps - # Initialize arrays to store data - time = np.linspace(0, T, num_steps+1) - x_history = np.zeros((num_steps+1, self.A.shape[0])) - u_history = np.zeros((num_steps+1, self.B.shape[1])) - x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) + # # Initialize arrays to store data + # time = np.linspace(0, T, num_steps+1) + # x_history = np.zeros((num_steps+1, self.A.shape[0])) + # u_history = np.zeros((num_steps+1, self.B.shape[1])) + # x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) - cross_track_error_history = np.zeros(num_steps+1) + # cross_track_error_history = np.zeros(num_steps+1) - # Simulation loop - x = x_init # Initial state - for i in range(num_steps+1): - #x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step - # x_ref_history[i, :] = x_ref # Update the reference at each time step + # # Simulation loop + # x = x_init # Initial state + # for i in range(num_steps+1): + # #x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step + # # x_ref_history[i, :] = x_ref # Update the reference at each time step - # generate reference at the look-ahead distance - p_asv = np.array([x[0], x[1]]) - errors = self.R_Pi_p.T @ (p_asv - self.p0) - along_track_error = errors[0] - p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 - - x_ref[:2] = p_los_world # Update the position reference at each time step - x_ref_history[i, :] = x_ref + # # generate reference at the look-ahead distance + # p_asv = np.array([x[0], x[1]]) + # errors = self.R_Pi_p.T @ (p_asv - self.p0) + # along_track_error = errors[0] + # p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 + + # x_ref[:2] = p_los_world # Update the position reference at each time step + # x_ref_history[i, :] = x_ref - u = self.calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step - x = self.asv.RK4_integration_step(x, u, dt) - - x_history[i] = x # Store state history - u_history[i] = u # Store control input history - - # Plot results - plt.figure(figsize=(12, 8)) - plt.subplot(3, 1, 1) - for j in range(3): - plt.plot(time, x_history[:, j], label=f'State (x_{j+1})') - plt.plot(time, x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') - plt.xlabel('Time') - plt.ylabel('State Value') - plt.legend() - - plt.subplot(3, 1, 2) - for j in range(self.B.shape[1]): - plt.plot(time, u_history[:, j], label=f'Control Input (u_{j+1})') - plt.xlabel('Time') - plt.ylabel('Control Input Value') - plt.legend() - - plt.subplot(3, 1, 3) - plt.scatter(x_history[:,0], x_history[:,1], label=f'Position') - plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path') - plt.xlabel('x') - plt.ylabel('y') - plt.legend() - - plt.tight_layout() - plt.show() - plt.figure(1) - plt.plot(time, cross_track_error_history, label="cross track error") - plt.axis("equal") - plt.plot(time, np.zeros_like(time)) - plt.legend() - plt.show() + # u = self.calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step + # x = self.asv.RK4_integration_step(x, u, dt) + + # x_history[i] = x # Store state history + # u_history[i] = u # Store control input history + + # # Plot results + # plt.figure(figsize=(12, 8)) + # plt.subplot(3, 1, 1) + # for j in range(3): + # plt.plot(time, x_history[:, j], label=f'State (x_{j+1})') + # plt.plot(time, x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') + # plt.xlabel('Time') + # plt.ylabel('State Value') + # plt.legend() + + # plt.subplot(3, 1, 2) + # for j in range(self.B.shape[1]): + # plt.plot(time, u_history[:, j], label=f'Control Input (u_{j+1})') + # plt.xlabel('Time') + # plt.ylabel('Control Input Value') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.scatter(x_history[:,0], x_history[:,1], label=f'Position') + # plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path') + # plt.xlabel('x') + # plt.ylabel('y') + # plt.legend() + + # plt.tight_layout() + # plt.show() + # plt.figure(1) + # plt.plot(time, cross_track_error_history, label="cross track error") + # plt.axis("equal") + # plt.plot(time, np.zeros_like(time)) + # plt.legend() + # plt.show() diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index a3a31016..d1d6246d 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -64,13 +64,14 @@ def odometrymsg_to_state(self, msg): def guidance_cb(self, msg): self.x_ref = self.odometrymsg_to_state(msg)[:3] - wrench = self.run_lqr_to_wrench() - - # Publish thrust/wrench_input - self.wrench_publisher_.publish(wrench) + #wrench = self.run_lqr_to_wrench() + #self.wrench_publisher_.publish(wrench) def state_cb(self, msg): self.state = self.odometrymsg_to_state(msg) + + wrench = self.run_lqr_to_wrench() + self.wrench_publisher_.publish(wrench) def run_lqr_to_wrench(self): self.lqr_controller.linearize_model(self.state[2]) diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py index 1595c00f..09a5c340 100644 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -26,6 +26,7 @@ def __init__(self): self.x_ref = np.array([0, 0, 50*np.pi/180]) # Note to self: make x_ref have 6 states self.num_steps_simulated = 0 + m = 50.0 self.M = np.diag([m, m, m]) @@ -59,7 +60,7 @@ def __init__(self): ," ## / U ," ## / - Waiting for simulation to finish...""" + str(self.T) + """ secs approximated waiting time :) + Waiting for simulation to finish...""" + str(self.T) + """ secs approximated waiting time on Martin's Thinkpad, will probably go faster for everybody else :) """) self.state_publisher_.publish(self.state_to_odometrymsg(self.state)) @@ -100,11 +101,14 @@ def state_to_odometrymsg(self, state): def wrench_input_cb(self, msg): u = np.array([msg.force.x, msg.force.y, msg.torque.z]) + x_next = self.RK4_integration_step(self.state, u, self.dt) + self.x_history[self.num_steps_simulated] = x_next self.x_ref_history[self.num_steps_simulated, : ] = self.x_ref self.u_history[self.num_steps_simulated] = u - # print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") + + print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") if (self.num_steps_simulated >= self.num_steps): self.plot_history() @@ -118,7 +122,7 @@ def wrench_input_cb(self, msg): self.state_publisher_.publish(odometry_msg) self.num_steps_simulated += 1 - self.get_logger().info(str(self.num_steps_simulated) + " lol xD") # heh DO NOT REMOVE + #self.get_logger().info(str(self.num_steps_simulated) + " lol xD") # heh DO NOT REMOVE def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: """ @@ -186,18 +190,18 @@ def plot_history(self): plt.scatter(self.x_history[:, 0], self.x_history[:, 1], label='Position', s=5) p0 = [0.0, 0.0] - p1 = [20.0, 20.0] - p2 = [50.0, -40.0] - p3 = [20.0, -80.0] + p1 = [40.0, 40.0] + p2 = [90.0, -20.0] + p3 = [120.0, 50.0] p4 = [120.0, -60.0] p5 = [160, 0.0] p6 = [60.0, 60.0] plt.plot([p0[0], p1[0]], [p0[1], p1[1]], 'r-', label='Path') plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'g-', label='Path') plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'b-', label='Path') - plt.plot([p3[0], p4[0]], [p3[1], p4[1]], 'y-', label='Path') - plt.plot([p4[0], p5[0]], [p4[1], p5[1]], 'm-', label='Path') - plt.plot([p5[0], p6[0]], [p5[1], p6[1]], 'c-', label='Path') + # plt.plot([p3[0], p4[0]], [p3[1], p4[1]], 'y-', label='Path') + # plt.plot([p4[0], p5[0]], [p4[1], p5[1]], 'm-', label='Path') + # plt.plot([p5[0], p6[0]], [p5[1], p6[1]], 'c-', label='Path') plt.xlabel('x') plt.ylabel('y') From 7cf90ff125d455ebd0eaf9bbdb45cfc1063e234f Mon Sep 17 00:00:00 2001 From: Benjaminas Visockis Date: Sun, 4 Feb 2024 20:56:10 +0100 Subject: [PATCH 19/20] Add functional FoxGlove realtime visualization package. --- asv_setup/launch/ASV_simulator.launch.py | 23 +- .../los_guidance/los_guidance_node.py | 2 +- motion/vessel_simulator_realtime/LICENSE | 17 ++ .../cfg/freya_sim_foxglove.json | 214 ++++++++++++++++++ .../vessel_simulator_realtime.launch.py | 17 ++ motion/vessel_simulator_realtime/package.xml | 29 +++ .../resource/vessel_simulator_realtime | 0 motion/vessel_simulator_realtime/setup.cfg | 4 + motion/vessel_simulator_realtime/setup.py | 29 +++ .../TF2Broadcaster.py | 79 +++++++ .../VesselVisualizerNode.py | 137 +++++++++++ .../vessel_simulator_realtime/__init__.py | 0 .../vessel_simulator_realtime/conversions.py | 59 +++++ .../vessel_simulator_realtime/simulation.py | 37 +++ .../vessel_simulator_realtime_node.py | 16 ++ 15 files changed, 655 insertions(+), 8 deletions(-) create mode 100644 motion/vessel_simulator_realtime/LICENSE create mode 100644 motion/vessel_simulator_realtime/cfg/freya_sim_foxglove.json create mode 100644 motion/vessel_simulator_realtime/launch/vessel_simulator_realtime.launch.py create mode 100644 motion/vessel_simulator_realtime/package.xml create mode 100644 motion/vessel_simulator_realtime/resource/vessel_simulator_realtime create mode 100644 motion/vessel_simulator_realtime/setup.cfg create mode 100644 motion/vessel_simulator_realtime/setup.py create mode 100644 motion/vessel_simulator_realtime/vessel_simulator_realtime/TF2Broadcaster.py create mode 100644 motion/vessel_simulator_realtime/vessel_simulator_realtime/VesselVisualizerNode.py create mode 100644 motion/vessel_simulator_realtime/vessel_simulator_realtime/__init__.py create mode 100644 motion/vessel_simulator_realtime/vessel_simulator_realtime/conversions.py create mode 100644 motion/vessel_simulator_realtime/vessel_simulator_realtime/simulation.py create mode 100644 motion/vessel_simulator_realtime/vessel_simulator_realtime/vessel_simulator_realtime_node.py diff --git a/asv_setup/launch/ASV_simulator.launch.py b/asv_setup/launch/ASV_simulator.launch.py index a68b94c7..5a33caf6 100644 --- a/asv_setup/launch/ASV_simulator.launch.py +++ b/asv_setup/launch/ASV_simulator.launch.py @@ -1,29 +1,37 @@ 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 launch_xml.launch_description_sources import XMLLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory def generate_launch_description(): # LQR_controller launch lqr_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( + PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('lqr_controller'), 'launch/lqr_controller.launch.py') ) ) # LOS_guidance launch los_guidance_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( + PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('los_guidance'), 'launch/los_guidance.launch.py') ) ) # Vessel_simulator launch vessel_simulator_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('vessel_simulator'), 'launch/vessel_simulator.launch.py') + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('vessel_simulator_realtime'), 'launch/vessel_simulator_realtime.launch.py') + ) + ) + + # Foxglove bridge launch + # Download using: $ sudo apt install ros-$ROS_DISTRO-foxglove-bridge + foxglove_bridge_launch = IncludeLaunchDescription( + XMLLaunchDescriptionSource( + os.path.join(get_package_share_directory('foxglove_bridge'), 'launch/foxglove_bridge_launch.xml') ) ) @@ -31,5 +39,6 @@ def generate_launch_description(): return LaunchDescription([ lqr_controller_launch, los_guidance_launch, - vessel_simulator_launch - ]) \ No newline at end of file + vessel_simulator_launch, + foxglove_bridge_launch + ]) diff --git a/guidance/los_guidance/los_guidance/los_guidance_node.py b/guidance/los_guidance/los_guidance/los_guidance_node.py index 0341bb3c..ca600a12 100644 --- a/guidance/los_guidance/los_guidance/los_guidance_node.py +++ b/guidance/los_guidance/los_guidance/los_guidance_node.py @@ -73,7 +73,7 @@ def state_cb(self, msg): #rate = rclpy.Rate(100) def point_cb(self, msg): - self.p_next.append(np.array([msg.x, msg.y])) + self.p_next.append(np.array([msg.x, msg.y])) # TODO: append does not exist for np.arrays def main(args=None): diff --git a/motion/vessel_simulator_realtime/LICENSE b/motion/vessel_simulator_realtime/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/motion/vessel_simulator_realtime/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/motion/vessel_simulator_realtime/cfg/freya_sim_foxglove.json b/motion/vessel_simulator_realtime/cfg/freya_sim_foxglove.json new file mode 100644 index 00000000..602ac170 --- /dev/null +++ b/motion/vessel_simulator_realtime/cfg/freya_sim_foxglove.json @@ -0,0 +1,214 @@ +{ + "configById": { + "3D!45bpewl": { + "cameraState": { + "perspective": true, + "distance": 250.52738551722237, + "phi": 179.99994266987844, + "thetaOffset": -89.99999770360385, + "targetOffset": [ + 73.97173196727483, + 0.23675590015567488, + 1.2349479935931187e-21 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "followTf": "world", + "scene": { + "transforms": { + "lineWidth": 0.5 + }, + "meshUpAxis": "y_up" + }, + "transforms": {}, + "topics": { + "/sensor/seapath/path/ned": { + "visible": true, + "lineWidth": 0.5, + "gradient": [ + "#ffffffff", + "#00be1080" + ] + }, + "/sensor/seapath/posestamped/ned": { + "visible": true, + "axisScale": 10.2, + "type": "axis", + "arrowScale": [ + 10, + 3.15, + 3.15 + ] + }, + "/move_base_simple/goal": { + "visible": false + }, + "/sensor/seapath/state_path/ned": { + "visible": true, + "lineWidth": 1, + "gradient": [ + "#fafafaff", + "#00ff0d80" + ] + }, + "/sensor/seapath/xref_path/ned": { + "visible": true, + "gradient": [ + "#c750cfff", + "#ae19a9ff" + ], + "lineWidth": 0.5 + } + }, + "layers": { + "6bba1d8c-b741-42b0-901c-20e59bfe5fcc": { + "visible": true, + "frameLocked": true, + "label": "Grid", + "instanceId": "6bba1d8c-b741-42b0-901c-20e59bfe5fcc", + "layerId": "foxglove.Grid", + "size": 1000, + "divisions": 100, + "color": "#248eff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ], + "order": 1, + "lineWidth": 0.3 + } + }, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": {} + }, + "Plot!41trey": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.x", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.x", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Surge" + }, + "Plot!q6f7ui": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/posestamped/ned.pose.position.y", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/controller/lqr/reference.pose.pose.position.y", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": false, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Sway" + }, + "Tab!23gzpt8": { + "activeTabIdx": 0, + "tabs": [ + { + "title": "1", + "layout": { + "first": "3D!45bpewl", + "second": { + "first": "Plot!41trey", + "second": { + "first": "Plot!q6f7ui", + "second": "Plot!4ia1dcm", + "direction": "row" + }, + "direction": "row", + "splitPercentage": 29.72972972972973 + }, + "direction": "column", + "splitPercentage": 69.86721144024514 + } + } + ] + }, + "Plot!4ia1dcm": { + "paths": [ + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw/ned.data", + "enabled": true + }, + { + "timestampMethod": "receiveTime", + "value": "/sensor/seapath/yaw_ref/ned.data", + "enabled": true + } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": true, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30, + "foxglovePanelTitle": "Yaw" + } + }, + "globalVariables": {}, + "userNodes": {}, + "playbackConfig": { + "speed": 1 + }, + "layout": "Tab!23gzpt8" +} \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/launch/vessel_simulator_realtime.launch.py b/motion/vessel_simulator_realtime/launch/vessel_simulator_realtime.launch.py new file mode 100644 index 00000000..6220e4db --- /dev/null +++ b/motion/vessel_simulator_realtime/launch/vessel_simulator_realtime.launch.py @@ -0,0 +1,17 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + vessel_simulator_realtime_node = Node( + package='vessel_simulator_realtime', + executable='vessel_simulator_realtime_node', + name='vessel_simulator_realtime_node', + # parameters=[os.path.join(get_package_share_directory('vessel_simulator_realtime'),'config','lqr_config.yaml')], + output='screen', + ) + return LaunchDescription([ + vessel_simulator_realtime_node + ]) + diff --git a/motion/vessel_simulator_realtime/package.xml b/motion/vessel_simulator_realtime/package.xml new file mode 100644 index 00000000..ccddaac6 --- /dev/null +++ b/motion/vessel_simulator_realtime/package.xml @@ -0,0 +1,29 @@ + + + + vessel_simulator_realtime + 0.0.0 + 3DOF marine vessel simulation and visualization package. + BenG + MIT + + lqr_controller + rclpy + std_msgs + geometry_msgs + nav_msgs + transforms3d + numpy + matplotlib + scipy + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/motion/vessel_simulator_realtime/resource/vessel_simulator_realtime b/motion/vessel_simulator_realtime/resource/vessel_simulator_realtime new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator_realtime/setup.cfg b/motion/vessel_simulator_realtime/setup.cfg new file mode 100644 index 00000000..8076957e --- /dev/null +++ b/motion/vessel_simulator_realtime/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/vessel_simulator_realtime +[install] +install_scripts=$base/lib/vessel_simulator_realtime diff --git a/motion/vessel_simulator_realtime/setup.py b/motion/vessel_simulator_realtime/setup.py new file mode 100644 index 00000000..3dda1e92 --- /dev/null +++ b/motion/vessel_simulator_realtime/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'vessel_simulator_realtime' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='BenG', + maintainer_email='benjamin.visockis@gmail.com', + description='3DOF marine vessel simulation and visualization package.', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'vessel_simulator_realtime_node = vessel_simulator_realtime.vessel_simulator_realtime_node:main' + ], + }, +) diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/TF2Broadcaster.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/TF2Broadcaster.py new file mode 100644 index 00000000..fe45d10e --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/TF2Broadcaster.py @@ -0,0 +1,79 @@ +from geometry_msgs.msg import TransformStamped +from rclpy.node import Node +from tf2_ros import TransformBroadcaster +import math +import numpy as np + +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = np.empty((4, )) + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + + +class TF2Broadcaster(Node): + def __init__(self, frame_id): + super().__init__('tf2_broadcaster') + self.tf_broadcaster = TransformBroadcaster(self) + + self.own_vessel_frame_id = frame_id + + def fixed_frame_broadcaster(self): + t = TransformStamped() + + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'map' + t.child_frame_id = 'world' + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 0.0 + + self.tf_broadcaster.sendTransform(t) + + def handle_pose(self, state): + t = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'world' + t.child_frame_id = self.own_vessel_frame_id + + # Turtle only exists in 2D, thus we get x and y translation + # coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = state[0] + t.transform.translation.y = state[1] + t.transform.translation.z = 0.0 + + # For the same reason, turtle can only rotate around one axis + # and this why we set rotation in x and y to 0 and obtain + # rotation in z axis from the message + q = quaternion_from_euler(0, 0, state[2]) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + # Send the transformation + self.tf_broadcaster.sendTransform(t) \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/VesselVisualizerNode.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/VesselVisualizerNode.py new file mode 100644 index 00000000..ab869fab --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/VesselVisualizerNode.py @@ -0,0 +1,137 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Int64, Float32 +from geometry_msgs.msg import Wrench, PoseStamped +from nav_msgs.msg import Odometry, Path + +import numpy as np +import time + +from vessel_simulator_realtime.conversions import * +from vessel_simulator_realtime.simulation import * +from vessel_simulator_realtime.TF2Broadcaster import TF2Broadcaster + +class VesselVisualizerNode(Node): + def __init__(self): + super().__init__("vessel_visualizer_node") + + # Init TF2 brpadcaster for frames and tfs + self.own_vessel_frame_id = "asv_pose" + self.tf_broadcaster = TF2Broadcaster(self.own_vessel_frame_id) + + # add wrench_input subscriber + self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1) + self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) + + # publish state (seapath) + self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1) + self.posestamped_publisher_ = self.create_publisher(PoseStamped, "/sensor/seapath/posestamped/ned", 1) + self.state_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/state_path/ned", 1) + self.xref_path_publisher_ = self.create_publisher(Path, "/sensor/seapath/xref_path/ned", 1) + + self.yaw_ref_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw_ref/ned", 1) + self.yaw_publisher_ = self.create_publisher(Float32, "/sensor/seapath/yaw/ned", 1) + + self.test_publisher = self.create_publisher(Int64, "/test1", 1) + + # create timer + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + + # Init state and control variables + self.state = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) + self.x_ref = np.array([0, 0, 50*np.pi/180, 0, 0, 0]) # Note to self: make x_ref have 6 states + self.u = np.array([0, 0, 0]) + + # Init historical pose (path) + self.state_path = Path() + self.xref_path = Path() + + self.N = 5000 # Example length + # for _ in range(N): + # pose = PoseStamped() + # pose.header.frame_id = "world" + # pose.header.stamp = self.get_clock().now().to_msg() + # # Initialize pose here if needed + # self.state_path.poses.append(pose) + # self.xref_path.poses.append(pose) + + # Init simulation parameters + m = 50.0 + self.M = np.diag([m, m, m]) + self.M_inv = np.linalg.inv(self.M) + self.D = np.diag([10.0, 10.0, 5.0]) + + realtime_factor = 10.0 + self.dt = timer_period*realtime_factor + self.num_steps_simulated = 0 + + # Publish initial state + self.state_publisher_.publish(state_to_odometrymsg(self.state)) + + def timer_callback(self): + self.simulate_step() + msg = Int64() + msg.data = self.num_steps_simulated + self.test_publisher.publish(msg) + + def guidance_cb(self, msg): + self.x_ref = odometrymsg_to_state(msg) + self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2])) + + def wrench_input_cb(self, msg): + self.u = np.array([msg.force.x, msg.force.y, msg.torque.z]) + + # def update_path_element(self, n, new_pose): + # if n < len(self.state_path.poses): + # self.state_path.poses[n] = new_pose + # else: + # # Shift elements and append new_pose + # self.state_path.poses.pop(0) + # self.state_path.poses.append(new_pose) + + def update_path_element(self, path, new_pose, N): + if len(path.poses) < N: + path.poses.append(new_pose) + else: + # Shift elements and append new_pose + path.poses.pop(0) + path.poses.append(new_pose) + + def simulate_step(self): + # Integrate a step forward using RK4 + x_next = RK4_integration_step(self.M_inv, self.D, self.state, self.u, self.dt) + + # Update state + self.state = x_next + + # Pub odom + odometry_msg = state_to_odometrymsg(x_next) + self.state_publisher_.publish(odometry_msg) + + # Pub Posestamped + posestamped_msg = state_to_posestamped(x_next, "world", self.get_clock().now().to_msg()) + self.posestamped_publisher_.publish(posestamped_msg) + self.yaw_publisher_.publish(Float32(data=x_next[2])) + + # Pub Paths + self.state_path.header.frame_id = "world" + self.state_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.state_path, posestamped_msg, self.N) + self.state_path_publisher_.publish(self.state_path) + + xref_msg = state_to_posestamped(self.x_ref, "world", self.get_clock().now().to_msg()) + self.xref_path.header.frame_id = "world" + self.xref_path.header.stamp = self.get_clock().now().to_msg() + + self.update_path_element(self.xref_path, xref_msg, self.N) + self.xref_path_publisher_.publish(self.xref_path) + + # Pub frame and odom tf + self.tf_broadcaster.fixed_frame_broadcaster() + self.tf_broadcaster.handle_pose(x_next) + + # Update current sim step + self.num_steps_simulated += 1 diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/__init__.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/conversions.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/conversions.py new file mode 100644 index 00000000..92f14cfb --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/conversions.py @@ -0,0 +1,59 @@ +import numpy as np + +from transforms3d.euler import euler2quat, quat2euler +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +def odometrymsg_to_state(msg): + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + orientation_q = msg.pose.pose.orientation + orientation_list = [ + orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z + ] + + # Convert quaternion to Euler angles, extract yaw + yaw = quat2euler(orientation_list)[2] + + vx = msg.twist.twist.linear.x + vy = msg.twist.twist.linear.y + vyaw = msg.twist.twist.angular.z + + state = np.array([x, y, yaw, vx, vy, vyaw]) + return state + +def state_to_odometrymsg(state): + orientation_list_next = euler2quat(0, 0, state[2]) + + odometry_msg = Odometry() + odometry_msg.pose.pose.position.x = state[0] + odometry_msg.pose.pose.position.y = state[1] + odometry_msg.pose.pose.position.z = 0.0 + odometry_msg.pose.pose.orientation.w = orientation_list_next[0] + odometry_msg.pose.pose.orientation.x = orientation_list_next[1] + odometry_msg.pose.pose.orientation.y = orientation_list_next[2] + odometry_msg.pose.pose.orientation.z = orientation_list_next[3] + + return odometry_msg + +def state_to_posestamped(state, frame_id, stamp): + orientation_list_next = euler2quat(0, 0, state[2]) + + posestamped_msg = PoseStamped() + + posestamped_msg.header.frame_id = frame_id + posestamped_msg.header.stamp = stamp + + posestamped_msg.pose.position.x = state[0] + posestamped_msg.pose.position.y = state[1] + posestamped_msg.pose.position.z = 0.0 + posestamped_msg.pose.orientation.w = orientation_list_next[0] + posestamped_msg.pose.orientation.x = orientation_list_next[1] + posestamped_msg.pose.orientation.y = orientation_list_next[2] + posestamped_msg.pose.orientation.z = orientation_list_next[3] + + return posestamped_msg + +def ssa(angle): + return (angle + np.pi) % (2 * np.pi) - np.pi \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/simulation.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/simulation.py new file mode 100644 index 00000000..2224d261 --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/simulation.py @@ -0,0 +1,37 @@ +import numpy as np + +def state_dot(M_inv: np.ndarray, D: np.ndarray, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: + """ + Calculate the derivative of the state using the non-linear kinematics + """ + heading = state[2] + + J = np.array( + [[np.cos(heading), -np.sin(heading), 0], + [np.sin(heading), np.cos(heading), 0], + [0, 0, 1]] + ) + + A = np.zeros((6,6)) + + A[:3,3:] = J + A[3:, 3:] = - M_inv @ D + + B = np.zeros((6,3)) + B[3:,:] = M_inv + + x_dot = A @ state + B @ tau_actuation + x_dot[0:2] += V_current # add current drift term at velocity level + + return x_dot + +def RK4_integration_step(M_inv: np.ndarray, D: np.ndarray, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: + # integration scheme for simulation, implements the Runge-Kutta 4 integrator + k1 = state_dot(M_inv, D, x, u) + k2 = state_dot(M_inv, D, x+dt/2*k1, u) + k3 = state_dot(M_inv, D, x+dt/2*k2, u) + k4 = state_dot(M_inv, D, x+dt*k3, u) + + x_next = x + dt/6*(k1+2*k2+2*k3+k4) + + return x_next \ No newline at end of file diff --git a/motion/vessel_simulator_realtime/vessel_simulator_realtime/vessel_simulator_realtime_node.py b/motion/vessel_simulator_realtime/vessel_simulator_realtime/vessel_simulator_realtime_node.py new file mode 100644 index 00000000..ab4d1616 --- /dev/null +++ b/motion/vessel_simulator_realtime/vessel_simulator_realtime/vessel_simulator_realtime_node.py @@ -0,0 +1,16 @@ +import rclpy +from vessel_simulator_realtime.VesselVisualizerNode import VesselVisualizerNode + + +def main(args=None): + rclpy.init(args=args) + node = VesselVisualizerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() From 974b37b6d904d7ee6ccc5040c8e2c1b8d880d0f5 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Sun, 18 Feb 2024 11:56:47 +0100 Subject: [PATCH 20/20] Remove Foxglove bridge launch and add print statement for instructions --- asv_setup/launch/ASV_simulator.launch.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/asv_setup/launch/ASV_simulator.launch.py b/asv_setup/launch/ASV_simulator.launch.py index 5a33caf6..6205e2cb 100644 --- a/asv_setup/launch/ASV_simulator.launch.py +++ b/asv_setup/launch/ASV_simulator.launch.py @@ -27,18 +27,11 @@ def generate_launch_description(): ) ) - # Foxglove bridge launch - # Download using: $ sudo apt install ros-$ROS_DISTRO-foxglove-bridge - foxglove_bridge_launch = IncludeLaunchDescription( - XMLLaunchDescriptionSource( - os.path.join(get_package_share_directory('foxglove_bridge'), 'launch/foxglove_bridge_launch.xml') - ) - ) + print("Make sure Foxglove bridge is installed and running before launching the ASV simulator. See Vortex wiki page for info.") # Return launch description return LaunchDescription([ lqr_controller_launch, los_guidance_launch, vessel_simulator_launch, - foxglove_bridge_launch ])