From a244c447518d3f864898141f7b83f393689ee9e6 Mon Sep 17 00:00:00 2001
From: hashaam <hashaam1217@gmail.com>
Date: Mon, 5 Aug 2024 18:16:14 -0700
Subject: [PATCH] thrusters contrib

---
 .../config/thruster_config.yaml               | 53 ++++++++++++-------
 src/triton_gazebo/models/triton_auv/model.sdf |  2 +-
 2 files changed, 36 insertions(+), 19 deletions(-)

diff --git a/src/triton_controls/config/thruster_config.yaml b/src/triton_controls/config/thruster_config.yaml
index 7a58a91c..3cdd48e4 100644
--- a/src/triton_controls/config/thruster_config.yaml
+++ b/src/triton_controls/config/thruster_config.yaml
@@ -1,39 +1,56 @@
 /triton/controls/thrust_allocator:
   ros__parameters:
   # Old values are commented, we should think about remodelling the Gazebo Sim to match
+  #
+  #       # Thrusters will follow the following naming convention: 
+
+      #               ___      Thruster 1
+      #    Thruster 3 ___________|__|________
+      #                 |                    |
+      #                 |        -x          |
+      #            Thruster 5 |||            |_
+      #                 |     |||___+y       |_) AUV front 
+      #                 |     +z             |
+      #                 |   (pointing up)    |
+      #               __|____________________|   
+      #    Thruster 4 ___        |  |        
+      #                        Thruster 2
+
     num_thrusters: 6
     bits_per_thruster: 5
     max_fwd: 3.71 # kgf
     max_rev: 2.92 # kgf
     t1:
       contrib: 
-        x: -0.7071
-        y: 0.7071
-      lx: -0.5
-      ly: -0.5
+        x: 0 #1
+      lx: 0.01
+      ly: 0.36
     t2:
       contrib: 
-        x: -0.7071
-        y: -0.7071
-      lx: -0.5
-      ly: 0.5
+        z: 1
+      ly: 0.22
+      lz: -0.07
     t3:
       contrib:
         x: 0.7071
-        y: -0.7071
-      lx: 0.5
-      ly: 0.5
+        z: 0.7071
+      lx: 0.21
+      ly: -0.33
+      lz: 0.0075
     t4:
       contrib: 
         x: 0.7071
-        y: 0.7071
-      lx: 0.5
-      ly: -0.5
+        z: 0.7071
+      lx: 0.21
+      ly: -0.33
+      lz: 0.0075
     t5:
       contrib:
-        z: 1.0
-      ly: -0.5
+        y: -1.0
+      lx: 0.325
+      ly: 0.075
     t6:
       contrib:
-        z: 1.0
-      ly: 0.5
+        y: -1.0
+      lx: -0.325
+      ly: 0.075
diff --git a/src/triton_gazebo/models/triton_auv/model.sdf b/src/triton_gazebo/models/triton_auv/model.sdf
index 6b501362..ab21ec9f 100644
--- a/src/triton_gazebo/models/triton_auv/model.sdf
+++ b/src/triton_gazebo/models/triton_auv/model.sdf
@@ -24,7 +24,7 @@
     <!-- Frame -->
     <include>
       <uri>model://triton_frame</uri>
-      <pose>0 0 0 0 0 0</pose>
+      <pose>0 0 0 0 3.1415 0</pose>
       <name>frame</name>
     </include>