Robotic-Decision-Making-Lab · mergify · Jun 3, 2025
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -20,6 +20,7 @@
   "files.trimTrailingWhitespace": true,
   "editor.formatOnSave": true,
   "editor.tabSize": 2,
+  "editor.detectIndentation": false,
   "autoDocstring.startOnNewLine": false,
   "autoDocstring.docstringFormat": "google-notypes",
   "python.autoComplete.extraPaths": [

diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md
@@ -0,0 +1,14 @@
+# Changelog for package auv_control_demos
+
+## 0.2.1 (2025-06-03)
+
+- Updates the individual_controller and chained_controllers demos to use the
+current `hydrodynamics` API and the new adaptive integral terminal sliding mode
+controller.
+
+## 0.2.0 (2025-05-03)
+
+## 0.1.0 (2025-04-27)
+
+- Updates the individual_controller and chained_controllers demos to use the
+correct topic names
diff --git a/auv_control_demos/chained_controllers/README.md b/auv_control_demos/chained_controllers/README.md
@@ -23,16 +23,16 @@ applies a PWM command to the hardware interface.
    The output should resemble:
 
    ```bash
-   integral_sliding_mode_controller[velocity_controllers/IntegralSlidingModeController] active
-   thruster_allocation_matrix_controller[thruster_allocation_matrix_controller/ThrusterAllocationMatrixController] active
-   thruster_1_controller[thruster_controllers/PolynomialThrustCurveController] active
-   thruster_2_controller[thruster_controllers/PolynomialThrustCurveController] active
-   thruster_3_controller[thruster_controllers/PolynomialThrustCurveController] active
-   thruster_4_controller[thruster_controllers/PolynomialThrustCurveController] active
-   thruster_5_controller[thruster_controllers/PolynomialThrustCurveController] active
-   thruster_6_controller[thruster_controllers/PolynomialThrustCurveController] active
-   thruster_7_controller[thruster_controllers/PolynomialThrustCurveController] active
-   thruster_8_controller[thruster_controllers/PolynomialThrustCurveController] active
+   adaptive_integral_terminal_sliding_mode_controller velocity_controllers/AdaptiveIntegralTerminalSlidingModeController        active
+   thruster_allocation_matrix_controller              thruster_allocation_matrix_controller/ThrusterAllocationMatrixController  active
+   thruster_8_controller                              thruster_controllers/PolynomialThrustCurveController                      active
+   thruster_7_controller                              thruster_controllers/PolynomialThrustCurveController                      active
+   thruster_6_controller                              thruster_controllers/PolynomialThrustCurveController                      active
+   thruster_5_controller                              thruster_controllers/PolynomialThrustCurveController                      active
+   thruster_4_controller                              thruster_controllers/PolynomialThrustCurveController                      active
+   thruster_3_controller                              thruster_controllers/PolynomialThrustCurveController                      active
+   thruster_2_controller                              thruster_controllers/PolynomialThrustCurveController                      active
+   thruster_1_controller                              thruster_controllers/PolynomialThrustCurveController                      active
    ```
 
 3. Verify that the hardware interfaces have been properly loaded by opening
@@ -46,57 +46,55 @@ applies a PWM command to the hardware interface.
 
    ```bash
    command interfaces
-     integral_sliding_mode_controller/rx/velocity [available] [unclaimed]
-     integral_sliding_mode_controller/ry/velocity [available] [unclaimed]
-     integral_sliding_mode_controller/rz/velocity [available] [unclaimed]
-     integral_sliding_mode_controller/x/velocity [available] [unclaimed]
-     integral_sliding_mode_controller/y/velocity [available] [unclaimed]
-     integral_sliding_mode_controller/z/velocity [available] [unclaimed]
-     thruster_1_controller/thruster_1_joint/effort [available] [claimed]
-     thruster_1_joint/effort [available] [claimed]
-     thruster_2_controller/thruster_2_joint/effort [available] [claimed]
-     thruster_2_joint/effort [available] [claimed]
-     thruster_3_controller/thruster_3_joint/effort [available] [claimed]
-     thruster_3_joint/effort [available] [claimed]
-     thruster_4_controller/thruster_4_joint/effort [available] [claimed]
-     thruster_4_joint/effort [available] [claimed]
-     thruster_5_controller/thruster_5_joint/effort [available] [claimed]
-     thruster_5_joint/effort [available] [claimed]
-     thruster_6_controller/thruster_6_joint/effort [available] [claimed]
-     thruster_6_joint/effort [available] [claimed]
-     thruster_7_controller/thruster_7_joint/effort [available] [claimed]
-     thruster_7_joint/effort [available] [claimed]
-     thruster_8_controller/thruster_8_joint/effort [available] [claimed]
-     thruster_8_joint/effort [available] [claimed]
-     thruster_allocation_matrix_controller/rx/effort [available] [claimed]
-     thruster_allocation_matrix_controller/ry/effort [available] [claimed]
-     thruster_allocation_matrix_controller/rz/effort [available] [claimed]
-     thruster_allocation_matrix_controller/x/effort [available] [claimed]
-     thruster_allocation_matrix_controller/y/effort [available] [claimed]
-     thruster_allocation_matrix_controller/z/effort [available] [claimed]
+           adaptive_integral_terminal_sliding_mode_controller/rx/velocity [available] [unclaimed]
+           adaptive_integral_terminal_sliding_mode_controller/ry/velocity [available] [unclaimed]
+           adaptive_integral_terminal_sliding_mode_controller/rz/velocity [available] [unclaimed]
+           adaptive_integral_terminal_sliding_mode_controller/x/velocity [available] [unclaimed]
+           adaptive_integral_terminal_sliding_mode_controller/y/velocity [available] [unclaimed]
+           adaptive_integral_terminal_sliding_mode_controller/z/velocity [available] [unclaimed]
+           thruster_1_controller/thruster_1_joint/effort [available] [claimed]
+           thruster_1_joint/pwm [available] [claimed]
+           thruster_2_controller/thruster_2_joint/effort [available] [claimed]
+           thruster_2_joint/pwm [available] [claimed]
+           thruster_3_controller/thruster_3_joint/effort [available] [claimed]
+           thruster_3_joint/pwm [available] [claimed]
+           thruster_4_controller/thruster_4_joint/effort [available] [claimed]
+           thruster_4_joint/pwm [available] [claimed]
+           thruster_5_controller/thruster_5_joint/effort [available] [claimed]
+           thruster_5_joint/pwm [available] [claimed]
+           thruster_6_controller/thruster_6_joint/effort [available] [claimed]
+           thruster_6_joint/pwm [available] [claimed]
+           thruster_7_controller/thruster_7_joint/effort [available] [claimed]
+           thruster_7_joint/pwm [available] [claimed]
+           thruster_8_controller/thruster_8_joint/effort [available] [claimed]
+           thruster_8_joint/pwm [available] [claimed]
+           thruster_allocation_matrix_controller/rx/effort [available] [claimed]
+           thruster_allocation_matrix_controller/ry/effort [available] [claimed]
+           thruster_allocation_matrix_controller/rz/effort [available] [claimed]
+           thruster_allocation_matrix_controller/x/effort [available] [claimed]
+           thruster_allocation_matrix_controller/y/effort [available] [claimed]
+           thruster_allocation_matrix_controller/z/effort [available] [claimed]
    state interfaces
-     rx/velocity
-     ry/velocity
-     rz/velocity
-     x/velocity
-     y/velocity
-     z/velocity
    ```
 
-4. State feedback can be provided to ISMC using the controller's state
+4. State feedback can be provided to AITSMC using the controller's state
    interfaces or a topic. To demonstrate the topic-based interface, run the
    following command:
 
    ```bash
+<<<<<<< HEAD
    ros2 topic pub /integral_sliding_mode_controller/system_state geometry_msgs/msg/Twist
+=======
+   ros2 topic pub /adaptive_integral_terminal_sliding_mode_controller/system_state nav_msgs/msg/Odometry
+>>>>>>> c8d798e (Update auv_control_demos configurations (#60))
    ```
 
-5. The ISMC accepts reference commands sent over a topic or a reference
+5. The AITSMC accepts reference commands sent over a topic or a reference
    interface. Run the following command in a separate terminal to provide the
    controller with a dummy reference input:
 
    ```bash
-   ros2 topic pub /integral_sliding_mode_controller/reference geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}"
+   ros2 topic pub /adaptive_integral_terminal_sliding_mode_controller/reference geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}"
    ```
 
 6. Each of the implemented controllers publishes controller state information to
@@ -111,23 +109,23 @@ applies a PWM command to the hardware interface.
    ```bash
    header:
      stamp:
-       sec: 1710818346
-       nanosec: 252453476
+       sec: 1748936962
+       nanosec: 907678344
      frame_id: ''
    dof_state:
      name: thruster_2_joint
-     reference: -39.79632414479818
+     reference: -63.32823451272053
      feedback: 0.0
      feedback_dot: 0.0
      error: 0.0
      error_dot: 0.0
-     time_step: 0.033380572
+     time_step: 0.033281545
      output: 1134.0
    ```
 
    This output indicates that the controller chaining was successful. In
    particular, we can see that the velocity commands sent to the
-   `integral_sliding_mode_controller` are converted to thrust values, which is
+   `adaptive_integral_terminal_sliding_mode_controller` are converted to thrust values, which is
    reflected in the `thruster_controller` output.
 
 ## Files used for this demo
@@ -136,14 +134,15 @@ applies a PWM command to the hardware interface.
   - [chaining.launch.py](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/launch/chaining.launch.py)
 
 - Controllers:
-  - [Integral Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers)
+  - [Adaptive Integral Terminal Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers)
   - [Thruster Allocation Matrix Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/thruster_allocation_matrix_controller)
   - [Polynomial Thrust Curve Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/thruster_controllers)
 
 - Controller Config:
-  - [chained_controllers.yaml](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/config/chained_controllers.yaml)
+  - [chained.controllers.yaml](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/config/chained_controllers.yaml)
 
 - Xacro Files:
-  - [chained_config.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained_config.xacro)
-  - [chained_ros2_control.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro)
-  - [chained_urdf.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro)
+  - [chained.config.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.config.xacro)
+  - [chained.ros2_control.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.ros2_control.xacro)
+  - [chained.urdf.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.urdf.xacro)
+  - [chained.model.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.urdf.xacro)
diff --git a/auv_control_demos/chained_controllers/config/chained_controllers.yaml b/auv_control_demos/chained_controllers/config/chained_controllers.yaml
@@ -2,8 +2,8 @@ controller_manager:
   ros__parameters:
     update_rate: 30 # Hz
 
-    integral_sliding_mode_controller:
-      type: velocity_controllers/IntegralSlidingModeController
+    adaptive_integral_terminal_sliding_mode_controller:
+      type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController
 
     thruster_allocation_matrix_controller:
       type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController
@@ -32,24 +32,48 @@ controller_manager:
     thruster_8_controller:
       type: thruster_controllers/PolynomialThrustCurveController
 
-integral_sliding_mode_controller:
+adaptive_integral_terminal_sliding_mode_controller:
   ros__parameters:
-    use_external_measured_states: true
     reference_controller: thruster_allocation_matrix_controller
+    use_external_measured_states: true
     gains:
-      rho: 20.0
-      lambda: 200.0
-      Kp: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0]
-    hydrodynamics:
-      mass: 13.5
-      weight: 114.80
-      buoyancy: 112.80
-      moments_of_inertia: [0.16, 0.16, 0.16]
-      added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12]
-      center_of_buoyancy: [0.0, 0.0, 0.0]
-      center_of_gravity: [0.0, 0.0, 0.0]
-      linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07]
-      quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55]
+      lambda: 0.9
+      x:
+        alpha: 12.0
+        k1_min: 0.1
+        k2: 0.4
+        k_theta: 2.0
+        mu: 0.01
+      y:
+        alpha: 12.0
+        k1_min: 0.1
+        k2: 0.4
+        k_theta: 2.0
+        mu: 0.01
+      z:
+        alpha: 12.0
+        k1_min: 0.4
+        k2: 0.8
+        k_theta: 2.0
+        mu: 0.01
+      rx:
+        alpha: 0.5
+        k1_min: 0.1
+        k2: 0.4
+        k_theta: 2.0
+        mu: 0.01
+      ry:
+        alpha: 12.0
+        k1_min: 0.1
+        k2: 1.0
+        k_theta: 2.0
+        mu: 0.01
+      rz:
+        alpha: 12.0
+        k1_min: 0.1
+        k2: 0.4
+        k_theta: 2.0
+        mu: 0.01
 
 thruster_allocation_matrix_controller:
   ros__parameters:

diff --git a/auv_control_demos/chained_controllers/launch/chaining.launch.py b/auv_control_demos/chained_controllers/launch/chaining.launch.py
@@ -40,7 +40,7 @@ def generate_launch_description():
                 [
                     FindPackageShare("auv_control_demos"),
                     "xacro",
-                    "chained_config.xacro",
+                    "chained.config.xacro",
                 ]
             ),
         ]
@@ -51,7 +51,7 @@ def generate_launch_description():
         package="controller_manager",
         executable="spawner",
         arguments=[
-            "integral_sliding_mode_controller",
+            "adaptive_integral_terminal_sliding_mode_controller",
             "--controller-manager",
             ["", "controller_manager"],
         ],

diff --git a/...ed_controllers/xacro/chained_config.xacro → ...ed_controllers/xacro/chained.config.xacro b/...ed_controllers/xacro/chained_config.xacro → ...ed_controllers/xacro/chained.config.xacro
@@ -2,8 +2,8 @@
 <robot name="auv"
        xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <xacro:include filename="$(find auv_control_demos)/xacro/chained_urdf.xacro" />
-
-  <xacro:include filename="$(find auv_control_demos)/xacro/chained_ros2_control.xacro" />
+  <xacro:include filename="$(find auv_control_demos)/xacro/chained.urdf.xacro" />
+  <xacro:include filename="$(find auv_control_demos)/xacro/chained.model.xacro" />
+  <xacro:include filename="$(find auv_control_demos)/xacro/chained.ros2_control.xacro" />
 
 </robot>
diff --git a/auv_control_demos/chained_controllers/xacro/chained.model.xacro b/auv_control_demos/chained_controllers/xacro/chained.model.xacro
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <hydrodynamics>
+    <inertial>
+      <mass value="16.61" />
+      <inertia ixx="0.24"
+               iyy="0.2"
+               izz="0.4" />
+      <center_of_gravity x="0.0"
+                         y="0.0"
+                         z="0.0" />
+      <added_mass Xdu="-12.7"
+                  Ydv="-10.5"
+                  Zdw="-14.57"
+                  Kdp="-0.12"
+                  Mdq="-0.12"
+                  Ndr="-0.12" />
+    </inertial>
+
+    <damping>
+      <linear Xu="-28.23"
+              Yv="-18.29"
+              Zw="-23.51"
+              Kp="-0.31"
+              Mq="-0.31"
+              Nr="-0.31" />
+      <quadratic Xuu="-93.47"
+                 Yvv="-74.28"
+                 Zww="-168.17"
+                 Kpp="-1.55"
+                 Mqq="-1.55"
+                 Nrr="-1.55" />
+    </damping>
+
+    <hydrostatics>
+      <buoyancy value="159.91" />
+      <center_of_buoyancy x="0.0"
+                          y="0.0"
+                          z="0.0" />
+    </hydrostatics>
+  </hydrodynamics>
+</robot>
diff --git a/...trollers/xacro/chained_ros2_control.xacro → ...trollers/xacro/chained.ros2_control.xacro b/...trollers/xacro/chained_ros2_control.xacro → ...trollers/xacro/chained.ros2_control.xacro
@@ -7,42 +7,6 @@
       <param name="calculate_dynamics">false</param>
     </hardware>
 
-    <joint name="x">
-      <state_interface name="velocity">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-    </joint>
-
-    <joint name="y">
-      <state_interface name="velocity">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-    </joint>
-
-    <joint name="z">
-      <state_interface name="velocity">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-    </joint>
-
-    <joint name="rx">
-      <state_interface name="velocity">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-    </joint>
-
-    <joint name="ry">
-      <state_interface name="velocity">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-    </joint>
-
-    <joint name="rz">
-      <state_interface name="velocity">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-    </joint>
-
     <joint name="thruster_1_joint">
       <command_interface name="pwm" />
     </joint>