Merge pull request #85 in SOF/subcode from pooltest to master

Author: CUAUV

cave/gui/#cave.glade#

@@ -1,12 +1,18 @@
 {
     // The below are highly dependent on trim
     "center_of_buoyancy": [0.00012155, -0.000034256, -0.00089525],
-    "buoyancy_force": 215, // FUDGED to assume postively buoyant sub - 187 with black batteries
+    //"buoyancy_force": 208, // green: 215, black: 187 FUDGED to assume postively buoyant sub
+    "buoyancy_force": 198, // green: 215, black: 187 FUDGED to assume postively buoyant sub
     "gravity_force": 189.38,
 
     // only used for partially submerged buoyancy force calculation, non critical
     "sub_height": 0.50,
 
+    // distance from dvl beam to axis of rotation used to offset velocity
+    // to account for misaligned reference point and dvl position
+    // for minisub, probably doesn't matter
+    "dvl_offset": 0,
+
     // Moments of inertia (kg * m^2) from solidworks.
     // In the model frame.
     "I": [[0.6149, -0.0054, -0.0662],
@@ -42,22 +48,22 @@
             "type": "T200",
             "pos": [-0.271, -0.201, -0.0204],
             "heading_pitch": [0, 90],
-            "reversed": false
+            "reversed": true
         },
         {
             "name": "sway_aft",
             "type": "T200",
             "pos": [-0.271, -0.006, 0.031],
             "heading_pitch": [90, 0],
             "min_neg_pwm": -2,
-            "reversed": true
+            "reversed": false
         },
         {
             "name": "aft_starboard",
             "type": "T200",
             "pos": [-0.271, 0.201, -0.0204],
             "heading_pitch": [0, 90],
-            "reversed": false
+            "reversed": true
         },
         {
             "name": "starboard",
@@ -79,20 +85,20 @@
             "pos": [0.280, -0.006, 0.031],
             "heading_pitch": [90, 0],
             "min_neg_pwm": -2,
-            "reversed": true
+            "reversed": false
         }
 
         //     "vector": [ [0, -1, 0], 360, "vector_status.position_port", "vector_desires.port", 61, false ] },
         //    "vector": [ [0, -1, 0], 360, "vector_status.position_starboard", "vector_desires.starboard", 793, false ] },
     ],
 
     "control_settings": {
-        "depth":    { "kP": 6.00, "kI": 0.15, "kD": 15.00, "rD": 0.5, "ramp_speed": 0.5 },
-        "pitch":    { "kP": 0.10, "kI": 0.10, "kD": 11.00, "rD": 50.0 },
+        "depth":    { "kP": 7.00, "kI": 0.15, "kD": 15.00, "rD": 0.3, "ramp_speed": 0.5 },
+        "pitch":    { "kP": 0.10, "kI": 0.10, "kD": 11.00, "rD": 30.0 },
         "heading":  { "kP": 0.24, "kI": 0.02, "kD": 13.50, "rD": 20.0 },
         "velx":     { "kP": 5.00, "kI": 0.00, "kD": 0.00, "rD": 0.1547 },
         "vely":     { "kP": 5.00, "kI": 0.00, "kD": 0.00, "rD": 0.5 },
-        "roll":     { "kP": 0.14, "kI": 0.00, "kD": 10.00, "rD": 50.0 },
+        "roll":     { "kP": 0.14, "kI": 0.01, "kD": 10.00, "rD": 30.0 },
         "quat":     { "kP": 30.0, "kI": 0.00, "kD": 4.00 }
     },
     "quaternion_filtering": 1,
@@ -106,10 +112,10 @@
             "area": 0.0
         },
         {
-            "pos": [-0.25, 0, 0],
+            "pos": [-1, 0, 0],
             "normal": [0, 0, 1],
             "cD": 2.0,
-            "area": 0.00
+            "area": 0.0
         },
         // fore sway, produces drag when swaying and rolling
         {
@@ -218,16 +224,23 @@
     },
 
     "vision_modules": [
-        {"name": "bicolor_gate", "capture_sources": ["forward"]},
+        {"name": "gate", "capture_sources": ["forward"]},
+        {"name": "vamp_buoy", "capture_sources": ["forward"]},
         // {"name": "torpedoes", "capture_sources": ["forward"]},
         {"name": "path", "capture_sources": ["downward"]},
+        {"name": "bins_cover", "capture_sources": ["downward"]},
+        {"name": "bins_image", "capture_sources": ["downward"]},
+        {"name": "bins_lever", "capture_sources": ["downward"]},
         {"name": "record", "capture_sources": ["forward", "downward"]},
-        {"name": "poster", "capture_sources": ["forward", "downward"]}
+        {"name": "poster", "capture_sources": ["forward", "downward"]},
+        {"name": "calibrate", "capture_sources": ["default"]}
     ],
 
     "actuators": {
-        "top_torpedo": 5,
-        "bottom_torpedo":4
+        "left_marker": 2,
+        "right_marker": 1
+        //"top_torpedo": 5,
+        //"bottom_torpedo":4
         // "left_marker": 4,
         // "left_torpedo" : 5,
         // "right_marker": 6,

conf/ajax.conf

@@ -1,12 +1,17 @@
 {
     // The below are highly dependent on trim
     "center_of_buoyancy": [-0.0005, -0.0025, -0.001],
-    "buoyancy_force": 332.5,  // green: 340.5 FUDGED to assume postively buoyant sub
+    "buoyancy_force": 342.5,  // black: 330, green: 342.5 FUDGED to assume postively buoyant sub
     "gravity_force": 314.809539,
 
     // only used for partially submerged buoyancy force calculation, non critical
     "sub_height": 0.50,
 
+    // distance from dvl beam to axis of rotation used to offset velocity
+    // to account for misaligned reference point and dvl position
+    // for minisub, probably doesn't matter
+    "dvl_offset": -0.2286,
+
     // Moments of inertia (kg * m^2) from solidworks.
     // In the model frame.
     "I": [[ 0.7570, -0.0193, -0.0157],
@@ -28,15 +33,15 @@
             "type": "T200",
             "pos": [0.3682922, -0.202912, -0.0352978],
             "heading_pitch": [0, 90],
-            "reversed": true,
+            "reversed": false,
             "broken": false
         },
         {
             "name": "port",
             "type": "T200",
             "pos": [0.062654, -0.3188122, -0.036898],
             "heading_pitch": [0, 0],
-            "reversed": false,
+            "reversed": true,
             "broken": false
         },
         {
@@ -53,7 +58,7 @@
             "type": "T200",
             "pos": [-0.3055542, 0.001685, -0.0289478],
             "heading_pitch": [90, 0],
-            "reversed": true,
+            "reversed": false,
             "broken": false
         },
         {
@@ -85,21 +90,31 @@
             "type": "T200",
             "pos": [0.5155614, 0.001685, -0.0289478],
             "heading_pitch": [90, 0],
-            "reversed": false,
+            "reversed": true,
             "broken": false
         }
 
         //    "vector": [ [0, -1, 0], 360, "vector_status.position_port", "vector_desires.port", 61, false ] },
         //    "vector": [ [0, -1, 0], 360, "vector_status.position_starboard", "vector_desires.starboard", 793, false ] },
     ],
 
+    // "control_settings": {
+    //     "depth":    { "kP": 2.50, "kI": 0.03, "kD": 4.00, "rD": 0.30, "ramp_speed": 0.5 },
+    //     "pitch":    { "kP": 0.16, "kI": 0.02, "kD": 5.0, "rD": 30.0 },
+    //     "heading":  { "kP": 0.20, "kI": 0.004, "kD": 15.00, "rD": 20.0 }, // Who would ever set an I term on heading?! .25, 0, 1.5
+    //     "velx":     { "kP": 9.50, "kI": 0.12, "kD": 0.50, "rD": 0.1547 },
+    //     "vely":     { "kP": 10.00, "kI": 0.20, "kD": 0.50, "rD": 0.5 },
+    //     "roll":     { "kP": 0.17, "kI": 0.02, "kD": 10.00, "rD": 30.0 },
+    //     "quat":     { "kP": 30.0, "kI": 0.00, "kD": 4.00 }
+    // },
+
     "control_settings": {
-        "depth":    { "kP": 2.50, "kI": 0.03, "kD": 3.00, "rD": 0.30, "ramp_speed": 0.5 },
-        "pitch":    { "kP": 0.16, "kI": 0.02, "kD": 5.0, "rD": 30.0 },
-        "heading":  { "kP": 0.20, "kI": 0.002, "kD": 15.00, "rD": 20.0 }, // Who would ever set an I term on heading?! .25, 0, 1.5
+        "depth":    { "kP": 2.50, "kI": 0.03, "kD": 4.00, "rD": 0.30, "ramp_speed": 0.5 },
+        "pitch":    { "kP": 0.16, "kI": 0.02, "kD": 3.50, "rD": 30.0 },
+        "heading":  { "kP": 0.40, "kI": 0.004, "kD": 20.00, "rD": 20.0 }, // Who would ever set an I term on heading?! .25, 0, 1.5
         "velx":     { "kP": 9.50, "kI": 0.12, "kD": 0.50, "rD": 0.1547 },
         "vely":     { "kP": 10.00, "kI": 0.20, "kD": 0.50, "rD": 0.5 },
-        "roll":     { "kP": 0.17, "kI": 0.02, "kD": 10.00, "rD": 30.0 },
+        "roll":     { "kP": 0.2, "kI": 0.05, "kD": 7.00, "rD": 30.0 },
         "quat":     { "kP": 30.0, "kI": 0.00, "kD": 4.00 }
     },
     "quaternion_filtering": 1,
@@ -264,28 +279,25 @@
     },
 
     "vision_modules": [
-        {"name": "bicolor_gate", "capture_sources": ["forward"]},
+        {"name": "gate", "capture_sources": ["forward"]},
+        {"name": "stake", "capture_sources": ["forward"]},
         //{"name": "dice", "capture_sources": ["forward"]},
-        // {"name": "torpedoes", "capture_sources": ["forward"]},
-        {"name": "path", "capture_sources": ["downward"]},
+        // {"name": "paul_path", "capture_sources": ["downward"]},
+        {"name": "vampire", "capture_sources": ["downward"]},
+        {"name": "crucifix", "capture_sources": ["downward"]},
         //{"name": "roulette", "capture_sources": ["downward"]},
         //{"name": "cash_in_forward", "capture_sources": ["forward"]},
         //{"name": "cash_in_downward", "capture_sources": ["downward"]},
         {"name": "record", "capture_sources": ["forward", "downward"]},
-        {"name": "poster", "capture_sources": ["forward", "downward"]}
+        {"name": "poster", "capture_sources": ["forward", "downward"]},
+        {"name": "calibrate", "capture_sources": ["default"]}
     ],
 
     "actuators": {
-        "bottom_torpedo": 4,
-        "top_torpedo": 5
-        // "left_marker": 4,
-        // "left_torpedo" : 5,
-        // "right_marker": 6,
-        // "right_torpedo" : 7,
-        // "left_piston_extend" : 10,
-        // "left_piston_retract" : 11,
-        // "right_piston_extend" : 8,
-        // "right_piston_retract" : 9
+        "bottom_torpedo": 8,
+        "top_torpedo": 4,
+        "manipulator_grab": 9,
+        "manipulator_release": 2
     }
 }
 // vim: set filetype=javascript expandtab :

conf/odysseus.conf

@@ -216,13 +216,16 @@
     },
 
     "vision_modules": [
-        {"name": "bicolor_gate", "capture_sources": ["forward"]},
-        {"name": "dice", "capture_sources": ["forward"]},
+        {"name": "gate", "capture_sources": ["forward"]},
+        {"name": "vamp_buoy", "capture_sources": ["forward"]},
         // {"name": "torpedoes", "capture_sources": ["forward"]},
-        {"name": "path", "capture_sources": ["downward"]},
-        {"name": "roulette", "capture_sources": ["downward"]},
-        {"name": "cash_in_forward", "capture_sources": ["forward"]},
-        {"name": "cash_in_downward", "capture_sources": ["downward"]},
+        {"name": "paul_path", "capture_sources": ["downward"]},
+        {"name": "bins_cover", "capture_sources": ["downward"]},
+        {"name": "bins_image", "capture_sources": ["downward"]},
+        {"name": "bins_lever", "capture_sources": ["downward"]},
+        {"name": "garlic", "capture_sources": ["downward"]},
+        // {"name": "cash_in_forward", "capture_sources": ["forward"]},
+        // {"name": "cash_in_downward", "capture_sources": ["downward"]},
         {"name": "record", "capture_sources": ["forward", "downward"]},
         {"name": "poster", "capture_sources": ["forward", "downward"]}
     ],

conf/pollux.conf

@@ -32,6 +32,7 @@
 buoyancy_force = d['buoyancy_force']
 gravity_force = d['gravity_force']
 sub_height = d['sub_height']
+dvl_offset = d['dvl_offset']  # TODO
 I = np.array(d['I'])
 thrusters = d['thrusters']
 sensors = d['sensors']

conf/vehicle.py

@@ -46,17 +46,15 @@ def get_thrusters(self):
         # bounds is an array of min, max pairs for each thruster
         self.bounds = []
         for i, t in enumerate(got_thrusters):
-            current_max_thrust, current_max_neg_thrust = t.current_max_thrusts()
-            self.constraints.append(lambda x, m=current_max_thrust, i=i: m-x[i])
-            self.constraints.append(lambda x, mn=current_max_neg_thrust, i=i: x[i]-mn)
-            self.bounds.append((current_max_neg_thrust, current_max_thrust))
+            self.constraints.append(lambda x, m=t.max_thrust, i=i: m-x[i])
+            self.constraints.append(lambda x, mn=t.max_neg_thrust, i=i: x[i]-mn)
+            self.bounds.append((t.max_neg_thrust, t.max_thrust))
 
         # calculate some "starting step" guesses for the optimization algorithm
         # by using 1/4 of the average max thrust
         # good start guesses are important and make the algorithm run faster
-        self.rhobeg = 0.25 * sum([current_max_thrust - current_max_neg_thrust \
-                          for (current_max_thrust, current_max_neg_thrust) in \
-                          [t.current_max_thrusts() for t in got_thrusters]]) / (len(got_thrusters) * 2)
+        self.rhobeg = 0.25 * sum([t.max_thrust - t.max_neg_thrust \
+                          for t in got_thrusters]) / (len(got_thrusters) * 2)
 
         # initial guess for optimizing function, currently static; 0 on all
         self.initial_guess = np.array((0,) * len(got_thrusters))
@@ -237,11 +235,10 @@ def set_motors(self, tm, ft_passive_s):
             # this only happens when desires are wild and slsqp fails
             # at that point we don't really care about accuracy and just
             # truncate for safety.
-            current_max_thrust, current_max_neg_thrust = t.current_max_thrusts()
-            if x[i] < current_max_neg_thrust:
-                x[i] = current_max_neg_thrust
-            elif x[i] > current_max_thrust:
-                x[i] = current_max_thrust
+            if x[i] < t.max_neg_thrust:
+                x[i] = t.max_neg_thrust
+            elif x[i] > t.max_thrust:
+                x[i] = t.max_thrust
 
             out[i] = t.thrust_to_pwm(x[i])
             if t.reversed_polarity:

control/optimizer.py

@@ -64,9 +64,8 @@ def max_forces(self, axis, torque=False):
         forces = [0, 0]
         f = lambda t: t.torque_about if torque else t.thrust_in
         for thruster in self.get_thrusters():
-            current_max_thrust, current_max_neg_thrust = thruster.current_max_thrusts()
-            max_t = (f(thruster))(axis, current_max_thrust)
-            min_t = (f(thruster))(axis, current_max_neg_thrust)
+            max_t = (f(thruster))(axis, thruster.max_thrust)
+            min_t = (f(thruster))(axis, thruster.max_neg_thrust)
             forces[0] += min(max_t, min_t)
             forces[1] += max(max_t, min_t)
 

control/thruster_manager.py

@@ -20,7 +20,12 @@
 
 MODEL_DIR = os.path.join(os.environ['CUAUV_SOFTWARE'], 'control', 'bollard',
                          '2015-06-03')
-NEAR_SURFACE_THRESHOLD = 0.5 # Meters depth
+# NEAR_SURFACE_THRESHOLD = 0.0 # Meters depth
+# DEPTH_THRUSTER_NAMES = ['fore_port', 'fore_starboard', 'aft_port', 'aft_starboard']
+# SURGE_THRUSTER_NAMES = ['port', 'starboard']
+# SWAY_THRUSTER_NAMES = ['sway_aft', 'sway_fore']
+# THRUSTER_NAMES = DEPTH_THRUSTER_NAMES + SURGE_THRUSTER_NAMES + SWAY_THRUSTER_NAMES
+NEAR_SURFACE_THRESHOLD = 0.1 # Meters depth
 DEPTH_THRUSTER_NAMES = ['fore_port', 'fore_starboard', 'aft_port', 'aft_starboard']
 SURGE_THRUSTER_NAMES = ['port', 'starboard']
 SWAY_THRUSTER_NAMES = ['sway_aft', 'sway_fore']
@@ -200,14 +205,8 @@ def __init__(self, yaw, pitch, position, min_neg_pwm, min_pos_pwm, drag=1.0, lin
         self.min_pos_pwm = min_pos_pwm
         self.min_neg_pwm = min_neg_pwm
 
-        if self.name in DEPTH_THRUSTER_NAMES:
-            self.max_thrust = self.pwm_to_thrust(self.max_pwm * 3 / 4)
-            self.max_neg_thrust = self.pwm_to_thrust(-self.max_pwm * 3 / 4)
-        else:
-            self.max_thrust = self.pwm_to_thrust(self.max_pwm)
-            self.max_neg_thrust = self.pwm_to_thrust(-self.max_pwm)
-        self.max_thrust_near_surface = self.pwm_to_thrust(self.max_pwm / 2)
-        self.max_neg_thrust_near_surface = self.pwm_to_thrust(-self.max_pwm / 2)
+        self.max_thrust = self.pwm_to_thrust(self.max_pwm)
+        self.max_neg_thrust = self.pwm_to_thrust(-self.max_pwm)
 
         self.min_thrust = self.pwm_to_thrust(self.min_pos_pwm)
         self.min_neg_thrust = self.pwm_to_thrust(self.min_neg_pwm)
@@ -366,15 +365,6 @@ def parse_model(self, filename):
     #    else:
     #        log("No model for %s thruster, defaulting to VideoRay!" % self.name)
 
-    def current_max_thrusts(self):
-        """
-            Returns the maximum positive and negative thrusts given the current sub position
-        """
-        if shm.kalman.depth.get() > NEAR_SURFACE_THRESHOLD:
-            return self.max_thrust, self.max_neg_thrust
-        else:
-            return self.max_thrust_near_surface, self.max_neg_thrust_near_surface
-
 class VideoRay(GenericThruster):
     max_pwm = 255
     min_pwm = 26

control/thrusters.py

@@ -7,7 +7,6 @@
 import math
 import numpy as np
 from control.thruster_manager import ThrusterManager
-import shm
 
 # average magnitude
 def amag(pair):
@@ -34,10 +33,6 @@ def calculate():
     # axis as [min, max]. We average |min| and |max| so we can treat it as
     # the major axis length in that direction.
 
-    # Set depth to well below surface of the water so that thruster maximums are computed
-    # without any artificial throttling based on depth
-    shm.kalman.depth.set(2.0)
-
     fx = tm.max_forces(np.array([1, 0, 0]), torque=False)
     fy = tm.max_forces(np.array([0, 1, 0]), torque=False)
     fz = tm.max_forces(np.array([0, 0, 1]), torque=False)