Merge branch 'hybridpath_controller' of github.com:vortexntnu/vortex-…

…asv into hybridpath_controller
vortexntnu · Apr 14, 2024 · d361e0a · d361e0a
2 parents a75e8a3 + 833642d
commit d361e0a
Show file tree

Hide file tree

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

diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml
@@ -5,4 +5,5 @@
       path_generator_order: 1 # Differentiability order
       time_to_max_speed: 10.0 # Time to reach maximum speed
       dt: 0.1 # Time step
-      u_desired: 0.5 # Desired speed
+      u_desired: 0.5 # Desired speed
+      mu: 0.03 # Tuning parameter 
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py
@@ -20,7 +20,8 @@ def __init__(self):
                 ('hybridpath_guidance.path_generator_order', 1),
                 ('hybridpath_guidance.time_to_max_speed', 10.0),
                 ('hybridpath_guidance.dt', 0.1),
-                ('hybridpath_guidance.u_desired', 0.5)
+                ('hybridpath_guidance.u_desired', 0.5),
+                ('hybridpath_guidance.mu', 0.03)
             ])
 
         self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback)
@@ -32,7 +33,7 @@ def __init__(self):
         self.path_generator_order = self.get_parameter('hybridpath_guidance.path_generator_order').get_parameter_value().integer_value
         self.dt = self.get_parameter('hybridpath_guidance.dt').get_parameter_value().double_value
         self.u_desired = self.get_parameter('hybridpath_guidance.u_desired').get_parameter_value().double_value
-        self.mu = 0.03
+        self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value
         self.eta = np.zeros(3)
 
         # Flags for logging

diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
@@ -16,7 +16,7 @@ def __init__(self):
             parameters=[
                 ('hybridpath_controller.K1_diag', [10.0, 10.0, 10.0]),
                 ('hybridpath_controller.K2_diag', [60.0, 60.0, 60.0]),
-                ('physical.inertia_matrix_diag', [50.0, 50.0, 50.0]),
+                ('physical.inertia_matrix', [50.0, 50.0, 50.0]),
                 ('physical.damping_matrix_diag', [10.0, 10.0, 5.0])
             ])
 
@@ -27,20 +27,17 @@ def __init__(self):
         # Get parameters
         K1_diag = self.get_parameter('hybridpath_controller.K1_diag').get_parameter_value().double_array_value
         K2_diag = self.get_parameter('hybridpath_controller.K2_diag').get_parameter_value().double_array_value
-        M_diag = self.get_parameter('physical.inertia_matrix_diag').get_parameter_value().double_array_value
         D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value
-
-        K1_diag = [10.0, 10.0, 10.0]
-        K2_diag = [60.0, 60.0, 60.0]
-        M_diag = [50.0, 50.0, 50.0]
-        D_diag = [10.0, 10.0, 5.0]
+        M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value
 
         # Transform parameters to diagonal matrices
         K1 = np.diag(K1_diag)
         K2 = np.diag(K2_diag)
-        M = np.diag(M_diag)
         D = np.diag(D_diag)
 
+        # Reshape M to a 3x3 matrix
+        M = np.reshape(M, (3, 3))
+
         # Create controller object
         self.AB_controller_ = AdaptiveBackstep(K1, K2, M, D)
 

diff --git a/motion/hybridpath_controller/launch/hybridpath_controller.launch.py b/motion/hybridpath_controller/launch/hybridpath_controller.launch.py
@@ -8,7 +8,10 @@ def generate_launch_description():
             package='hybridpath_controller',
             executable='hybridpath_controller_node.py',
             name='hybridpath_controller_node',
-            parameters=[os.path.join(get_package_share_directory('hybridpath_controller'),'config','hybridpath_controller_config.yaml')],
+            parameters=[
+                os.path.join(get_package_share_directory('hybridpath_controller'), 'config', 'hybridpath_controller_config.yaml'),
+                os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml')
+            ],
             output='screen',
         )
     return LaunchDescription([