minor changes done; TODO: fix MPC

Robotic-Decision-Making-Lab · Aug 28, 2024 · 20f4a5b · 20f4a5b
1 parent 3d35989
commit 20f4a5b
Show file tree

Hide file tree

Showing 5 changed files with 120 additions and 156 deletions.
diff --git a/docking_control/config/mpc_bluerov2_heavy.yaml b/docking_control/config/mpc_bluerov2_heavy.yaml
@@ -32,7 +32,7 @@ penalty:
   # Q: [0., 0., 0., 0., 0., 0.]
   Q: [1, 1, 1, 1, 1, 1]
 
-  R: [0.01, 0.01, 0.1, 0.01, 0.01, 0.01]
+  R: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
   # R: [1, 0.01, 0.01, 10, 10, 0.01]
   # R: [0.01, 0.01, 0.01, 0.1, 0.1, 0.1]
   # R: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
@@ -44,19 +44,21 @@ penalty:
   # R: [100, 100, 100, 100, 100, 100]
 
 bounds:
-  xmin: [-100, -100, -100., -3.14, -3.14, -3.14, -1, -1, -1, -0.087, -0.087, -0.087]
-  xmax: [100, 100, 100, 3.14, 3.14, 3.14, 1, 1, 1, 0.087, 0.087, 0.087]
+  xmin: [-15., -15., -15., -3.14, -3.14, -3.14, -1, -1, -1, -0.087, -0.087, -0.087]
+  xmax: [15., 15., 15., 3.14, 3.14, 3.14, 1, 1, 1, 0.087, 0.087, 0.087]
 
   # umin: [-0.1, -0.1, -0.1, -0.1, -0.1, -0.1, -0.1, -0.1]
   # umax: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
   # umin: [-250.0, -250.0, -250.0, -250.0, -250.0, -250.0]
   # umax: [250.0, 250.0, 250.0, 250.0, 250.0, 250.0]
   # umin: [-20.0, -20.0, -20.0, -20.0, -20.0, -20.0]
   # umax: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0]
-  # umin: [-35.0, -20.0, -25.0, -1.0, -1.0, -15]
-  # umax: [35.0, 20.0, 25.0, 1.0, 1.0, 15]
-  umin: [-35.0, -35.0, -35.0, -35.0, -35.0, -35.0]
-  umax: [35.0, 35.0, 35.0, 35.0, 35.0, 35.0]
+  umin: [-35.0, -20.0, -25.0, -1.0, -1.0, -15]
+  umax: [35.0, 20.0, 25.0, 1.0, 1.0, 15]
+  # umin: [-35.0, -35.0, -35.0, -10.0, -10.0, -10.0]
+  # umax: [35.0, 35.0, 35.0, 10.0, 10.0, 10.0]
+  # umin: [-35.0, -35.0, -35.0, -35.0, -35.0, -35.0]
+  # umax: [35.0, 35.0, 35.0, 35.0, 35.0, 35.0]
   # umin: [-100.0, -100.0, -100.0, -100.0, -100.0, -100.0]
   # umax: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0]
 

diff --git a/docking_control/launch/mission_control.launch b/docking_control/launch/mission_control.launch
@@ -50,6 +50,7 @@
 
   <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="marker3_to_map_tf_broadcaster" args="0.0 -0.17 0.434 0 1.571 -1.571 marker_3 map" /> -->
   <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="marker3_to_map_tf_broadcaster" args="0.035 -0.17 0.334 0 1.571 -1.571 marker_3 map" /> -->
+  <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="marker3_to_map_tf_broadcaster" args="0.015 -0.24 0.334 0 1.571 -1.571 marker_3 map" /> -->
   <node pkg="tf2_ros" type="static_transform_publisher" name="marker3_to_dock_tf_broadcaster" args="0.015 -0.24 0.334 0 1.571 -1.571 marker_3 dock" />
 
   <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_base_footprint" args="0 0 0 0 0 0 base_link base_footprint" /> -->

diff --git a/docking_control/src/mission_control.py b/docking_control/src/mission_control.py
@@ -441,6 +441,8 @@ def auto_control(self, joy):
         # xr = np.array([[2.0, 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
         # xr = np.array([[x0[0, 0], 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
         xr = np.array([[-5.0, 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
+        # xr = np.array([[x0[0, 0], 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
+        # xr = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
 
         try:
             forces, wrench, converge_flag = self.mpc.run_mpc(x0, xr)
@@ -491,14 +493,12 @@ def auto_control(self, joy):
                     # pwm.append(self.neutral_pwm)
 
                 for i in range(len(pwm)):
-                    pwm[i] = max(
-                        min(pwm[i], self.max_pwm_auto), self.min_pwm_auto
-                    )
+                    pwm[i] = max(min(pwm[i], self.max_pwm_auto), self.min_pwm_auto)
 
-                if all(element == 0.0 for element in np.round(wrench[1:6],2)):
+                if all(element == 0.0 for element in np.round(wrench[1:6], 2)):
                     pwm[0:4] = [1800, 1800, 1800, 1800]
 
-                pwm[8] = 1250
+                pwm[8] = 1300
 
                 self.mpc_pwm_pub.publish(pwm)
                 self.control_pub.publish(pwm)