Testing changes (#1062)

* Added changes from recent testings
- Changed the thruster ID values to correspond to the current thruster
  connection setup.
- Changed FRV thruster direction from -1Z to +1Z.
- Changed PID values for the sub. Values are still not ideal and will be
  adjusted over the next few testing sessions.
- Changed the identifier for the USB to CAN board's USB to serial
  converter.
- Changed tcp address for the depth sensor, imu, and dvl connection
  scripts to their ip addresses instead of their DNS names (there is an
issue with the DNS server in the network box, or in the lab, or both).
- Made changes to the sub8.urdf.xacro for gazebo to avoid URDF inertia
  warning.
- Added bag_debugging_controller.launch file which is a launch file to
  record bags for debugging the PID controller on the sub.

* Add documentation for bag_debugging_controller.launch

* pre-commit and CI fixes...probably

* Added documentation about PID controller
- Added documentation page about PID controller containing information
about the bag_debugging_controller.launch file and some tips for how to
tune the PID controller.
- Reverted subjugator_launch/readme.md changes to reflect moving to a
new documentation page.
- Modified adaptive controller values to remove commented out kd values.

* Modify URDF fix to fix /mission_server crash
- Another URDF xacro fix has been applied to the URDF xacro file using
these ROS forum posts as reference:
https://answers.ros.org/question/393006/urdf-link-not-properly-fixed-to-world/,
https://answers.ros.org/question/192817/error-msg-the-root-link_base-has-an-inertia-specified-in-the-urdf-but-kdl/.
NOTE: This is not a complete fix yet. I'm getting a tf republisher error
that 'base_link' and 'map' do not have a connection with TF having 2+
unconnected trees. Additionally, gazebogui does not work since
the /gzclient/set_physics_properties service is not advertised (even
though /gazebo/set_physics_properties is).

* Add support for simulation vs physical PID gains

---------

Co-authored-by: Cameron Brown <[email protected]>
Co-authored-by: cameron brown <[email protected]>

SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml

@@ -1,6 +1,6 @@
 ---
-kp: 1000, 1000, 1000, 5000, 5000, 5000
-kd: 150, 150, 150, 50, 100, 25
-kg: 5,5,5,5,5,5
-ki: 5,5,5,5,10,5
-use_learned: false
+kp: 120, 150, 200, 100, 50, 50
+kd: 25, 25, 25, 25, 25, 25
+kg: 2.5,2.5,2.5,2.5,2.5,2.5
+ki: 2.5,2.5,2.5,2.5,2.5,2.5
+use_learned: true

SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml

@@ -0,0 +1,6 @@
+---
+kp: 1000, 1000, 1000, 5000, 5000, 5000
+kd: 150, 150, 150, 50, 100, 25
+kg: 5,5,5,5,5,5
+ki: 5,5,5,5,10,5
+use_learned: false

SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="prefix_name" default="controller_debug" />
+  <arg name="bag_name" default="$(find subjugator_controller)/debug_bags/$(arg prefix_name)" />
+  <node name="record" pkg="rosbag" type="record" args="-o $(arg bag_name) /adaptive_controller/pose_error /adaptive_controller/twist_error /adaptive_controller/adaptation /adaptive_controller/dist /adaptive_controller/drag /wrench /trajectory /odom /imu/data_raw /dvl/range">
+  </node>
+</launch>

SubjuGator/command/subjugator_launch/launch/can.launch

@@ -5,7 +5,7 @@
     <rosparam command="delete" />
     <rosparam>
     # Path of serial device
-    port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0
+    port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0
     # Baudrate of device, should leave as is
     baudrate: 115200
     # List of python device handle classes

SubjuGator/command/subjugator_launch/launch/sub8.launch

@@ -39,7 +39,9 @@
   <include if="$(eval environment != 'dynsim')" file="$(find subjugator_launch)/launch/subsystems/odometry.launch"/>
   <include file="$(find subjugator_launch)/launch/subsystems/thruster_mapper.launch"/>
 
-  <include if="$(arg use_adaptive_controller)" file="$(find subjugator_launch)/launch/subsystems/adaptive_controller.launch"/>
+  <include if="$(arg use_adaptive_controller)" file="$(find subjugator_launch)/launch/subsystems/adaptive_controller.launch">
+    <arg name="environment" value="$(arg environment)" />
+  </include>
   <include unless="$(arg use_adaptive_controller)" file="$(find subjugator_launch)/launch/subsystems/rise.launch"/>
 
   <include file="$(find subjugator_launch)/launch/subsystems/path_planner.launch" />

SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch

@@ -1,8 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-  <arg name="debug" default="True" />
+  <arg name="debug" default="True"/>
+  <arg name="environment" default="real"/>
   <node pkg="subjugator_controller" type="adaptive_controller" name="adaptive_controller">
-    <rosparam param="" file="$(find subjugator_launch)/config/adaptive_controller.yaml" />
-    <param name="debug" value="$(arg debug)" />
+    <rosparam if="$(eval environment == 'real')" param="" file="$(find subjugator_launch)/config/adaptive_controller.yaml"/>
+    <rosparam if="$(eval environment == 'gazebo')" param="" file="$(find subjugator_launch)/config/adaptive_controller_gazebo.yaml"/>
+    <param name="debug" value="$(arg debug)"/>
   </node>
 </launch>

SubjuGator/command/subjugator_launch/package.xml

@@ -5,8 +5,10 @@
   <description>The subjugator_launch package</description>
   <maintainer email="[email protected]">Jacob Panikulam</maintainer>
   <license>MIT</license>
+  <build_depend>rosbag</build_depend>
   <build_depend>roslaunch</build_depend>
   <run_depend>roslaunch</run_depend>
+  <run_depend>rosbag</run_depend>
   <run_depend>robot_state_publisher</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>c3_trajectory_generator</run_depend>

SubjuGator/command/subjugator_launch/scripts/depth_conn

@@ -1,2 +1,2 @@
 #!/bin/sh
-exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:33056
+exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:192.168.37.61:33056

SubjuGator/command/subjugator_launch/scripts/dvl_conn

@@ -1,2 +1,2 @@
 #!/bin/sh
-exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:349
+exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:192.168.37.61:349

SubjuGator/command/subjugator_launch/scripts/imu_conn

@@ -1,2 +1,2 @@
 #!/bin/sh
-exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:mil-sub-gumstix.ad.mil.ufl.edu:1382
+exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:192.168.37.61:1382

SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml

@@ -30,7 +30,7 @@ thruster_ports:
 #     mapping from thrust in Newtons to effort (e = t[0]x^4 + t[1]x^3 + t[2]x^2 + t[3]x)
 thrusters:
   FLH: {
-    node_id: 3,
+    node_id: 4,
     position: [0.2678, 0.2795, 0.0],
     direction: [0.866, -0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
@@ -42,7 +42,7 @@ thrusters:
     }
   }
   FLV: {
-    node_id: 1,
+    node_id: 5,
     position: [0.1583, 0.169, 0.0142],
     direction: [0.0, 0.0, 1.0],
     thrust_bounds: [-81.85, 99.7],
@@ -54,7 +54,7 @@ thrusters:
     }
   }
   FRH: {
-    node_id: 2,
+    node_id: 0,
     position: [0.2678, -0.2795, 0.0],
     direction: [0.866, 0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
@@ -66,9 +66,9 @@ thrusters:
     }
   }
   FRV: {
-    node_id: 0,
+    node_id: 1,
     position: [0.1583, -0.169, 0.0142],
-    direction: [0.0, 0.0, -1.0],
+    direction: [0.0, 0.0, 1.0],
     thrust_bounds: [-81.85, 99.7],
     calib: {
       # yamllint disable-line rule:line-length
@@ -78,7 +78,7 @@ thrusters:
     }
   }
   BLH: {
-    node_id: 4,
+    node_id: 3,
     position: [-0.2678, 0.2795, 0.0],
     direction: [0.866, 0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
@@ -90,7 +90,7 @@ thrusters:
     }
   }
   BLV: {
-    node_id: 5,
+    node_id: 6,
     position: [-0.1583, 0.169, 0.0142],
     direction: [0.0, 0.0, 1.0],
     thrust_bounds: [-81.85, 99.7],
@@ -102,7 +102,7 @@ thrusters:
     }
   }
   BRH: {
-    node_id: 6,
+    node_id: 7,
     position: [-0.2678, -0.2795, 0.0],
     direction: [0.866, -0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
@@ -114,7 +114,7 @@ thrusters:
     }
   }
   BRV: {
-    node_id: 7,
+    node_id: 2,
     position: [-0.1583, -0.169, 0.0142],
     direction: [0.0, 0.0, 1.0],
     thrust_bounds: [-81.85, 99.7],

SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro

@@ -11,6 +11,8 @@
   <xacro:include filename="$(find mil_gazebo)/xacro/passive_sonar.xacro"/>
   <xacro:include filename="$(find mil_gazebo)/xacro/inclinometer.xacro"/>
 
+  <link name="world"/>
+
   <!-- Base link of sub -->
   <link name="base_link">
     <inertial>
@@ -31,6 +33,11 @@
     </collision>
   </link>
 
+  <joint name="dummy_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+  </joint>
+
   <!-- Sensors -->
   <xacro:mil_fixed_link name="front_stereo" parent="base_link" xyz="0.2559 0 0.1707" rpy="0 0 0"/>
   <xacro:mil_camera name="front_left_cam" parent="front_stereo" namespace="/camera/front/left" xyz="0 0.0445 0" rpy="0 0 0"/>

docs/subjugator/index.rst

@@ -15,6 +15,7 @@ SubjuGator 8
     Simulating <simulating>
     Enabling <enabling>
     Cameras <cameras>
+    PID Controller <pid>
 
     electrical
 

docs/subjugator/pid.rst

@@ -0,0 +1,26 @@
+PID Controller
+--------------
+
+SubjuGator 8 uses 6 PID controllers for its 6 degrees of freedom (x, y, z, roll, pitch, yaw). The gain for these PID controllers can be adjusted in ``adaptive_controller.yaml`` found in the ``subjugator_launch/config`` directory. Since the weight of the sub shifts with each component modified, the PID controller values have to be adjusted from time to time. There are two approaches to adjust PID values in the water:
+
+1. Have someone with experience in tuning PID controllers swim with the sub and use the sub's response to movement commands to adjust the gains.
+2. Eliminate the error in each axis by adjusting the gains and evaluating the RMS error in each axis after sending the same movement commands.
+
+PID Controller Tuning Tips
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+* The learning gains (``ki`` and ``kd``) provide very little input to the wrenches applied to the sub, so it is better to treat it as a PD controller to start with. You can disable the learning gains using ``use_learned: false``.
+* While you are tuning the PID values, keep a history of the values you have tried for further analysis during and after testing. You can use ``scp`` for this, but it may just be easier to take a screenshot or a photo. You can also take videos of the sub from the side of the pool to reference later.
+
+PID Debugging Data Collection
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. code-block:: bash
+
+    $ roslaunch subjugator_launch bag_debugging_controller.launch prefix_name:="my_prefix"
+
+This launch file records a bag file containing adaptive controller ``pose_error``, ``twist_error``, ``adaptation``, ``dist``, and ``drag`` data along with ``wrench``, ``trajectory``, ``odom``, ``imu/data_raw``, and ``dvl/range`` data. This data is useful in determining if the PID values are getting better or worse using simulations. The resulting bag file will be located in ``gnc/subjugator_controller/debug_bags/``. This launch file was written by Andres Pulido.
+
+.. warning::
+
+    This launch file needs the sub launch file (``sub8.launch``) running in order to function properly.