Merge pull request #14 from jpanikulam/master

PROPULSION: Implement thruster mapper

gnc/sub8_thruster_mapper/CMakeLists.txt

@@ -7,4 +7,11 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package()
 include_directories(
   ${catkin_INCLUDE_DIRS}
-)
+)
+
+if(CATKIN_ENABLE_TESTING)
+  foreach(T
+      test/test_sub8_mapper.test)
+     add_rostest(${T})
+  endforeach()
+endif()

gnc/sub8_thruster_mapper/launch/thruster_mapper.launch

@@ -0,0 +1,62 @@
+<launch>
+    <rosparam>
+      busses:
+        - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMC9-if00-port0
+          thrusters:
+            FLV: {
+              node_id: 10,
+              frame_id: /base_link,
+              position: [0.1583, 0.169, 0.0142],
+              direction: [0, 0, -1]
+            }
+            FLL: {
+              node_id: 11,
+              frame_id: /base_link,
+              position: [0.2678, 0.2795, 0],
+              direction: [-0.866, 0.5, 0]
+            }
+        - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMCI-if00-port0
+          thrusters:
+            FRV: {
+              node_id: 12,
+              frame_id: /base_link,
+              position: [0.1583, -0.169, 0.0142],
+              direction: [0, 0, -1]
+            }
+            FRL: {
+              node_id: 13,
+              frame_id: /base_link,
+              position: [0.2678, -0.2795, 0],
+              direction: [-0.866, -0.5, 0]
+            }
+        - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IN02-if00-port0
+          thrusters:
+            BLV: {
+              node_id: 14,
+              frame_id: /base_link,
+              position: [-0.1583, 0.169, 0.0142], 
+              direction: [0, 0, 1]
+            }
+            BLL: {
+              node_id: 15,
+              frame_id: /base_link,
+              position: [-0.2678, 0.2795, 0], 
+              direction: [0.866, 0.5, 0]
+            }
+        - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IN03-if00-port0
+          thrusters:
+            BRV: {
+              node_id: 16,
+              frame_id: /base_link,
+              position: [-0.1583, -0.169, 0.0142],
+              direction: [0, 0, 1]
+            }
+            BRL: {
+              node_id: 17,
+              frame_id: /base_link,
+              position: [-0.2678, -0.2795, 0],
+              direction: [0.866, -0.5, 0]
+            }
+  </rosparam>
+  <node pkg="sub8_thruster_mapper" type="mapper.py" name="thruster_mapper" />
+</launch>

gnc/sub8_thruster_mapper/nodes/mapper.py

@@ -0,0 +1,166 @@
+#!/usr/bin/env python
+import rospy
+from scipy.optimize import minimize
+import numpy as np
+from sub8_ros_tools import wait_for_param, thread_lock, rosmsg_to_numpy
+import threading
+from sub8_msgs.msg import Thrust, ThrusterCmd
+from geometry_msgs.msg import WrenchStamped
+
+
+lock = threading.Lock()
+class ThrusterMapper(object):
+    def __init__(self, layout):
+        '''The layout should be a dictionary of the form used in thruster_mapper.launch
+        an excerpt is quoted here for posterity
+
+        busses:
+            - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMC9-if00-port0
+              thrusters:
+                FLV: {
+                  node_id: 10,
+                  frame_id: /base_link,
+                  position: [0.1583, 0.169, 0.0142],
+                  direction: [0, 0, -1]
+                }
+                FLL: {
+                  node_id: 11,
+                  frame_id: /base_link,
+                  position: [0.2678, 0.2795, 0],
+                  direction: [-0.866, 0.5, 0]
+                }
+
+            - port: ....
+              thrusters: ....
+
+        '''
+        self.min_thrust, self.max_thrust = self.get_ranges()
+        # Make thruster layout
+        self.B = self.generate_B(layout)
+        self.wrench_sub = rospy.Subscriber('/wrench', WrenchStamped, self.request_wrench_cb, queue_size=1)
+        self.thruster_pub = rospy.Publisher('/thrust', Thrust, queue_size=1)
+
+    @thread_lock(lock)
+    def update_layout(self, srv):
+        '''Update the physical thruster layout.
+        This should only be done in a thruster-out event
+        '''
+        raise(NotImplementedError("Layout Updates Not Implemented"))
+        self.layout = '...'
+        self.B = self.generate_B(self.layout)
+
+    def get_ranges(self):
+        '''Get upper and lower thrust limits for each thruster
+        Todo: Actually do something
+            --> Wait for a service in the thruster driver to say something
+            - We have this information in the form of calibration.json
+        '''
+        rospy.logwarn("Thruster mapper not querying thrusters for range data")
+        return np.array([-90] * 8), np.array([90] * 8)
+
+    def get_thruster_wrench(self, position, direction):
+        '''Compute a single column of B, or the wrench created by a particular thruster'''
+        forces = direction
+        torques = np.cross(position, forces)
+        wrench_column = np.hstack([forces, torques])
+        return np.transpose(wrench_column)
+
+    def generate_B(self, layout):
+        '''Construct the control-input matrix
+        Each column represents the wrench generated by a single thruster
+
+        The single letter "B" is conventionally used to refer to a matrix which converts
+         a vector of control inputs to a wrench
+
+        Meaning where u = [thrust_1, ... thrust_n],
+         B * u = [Fx, Fy, Fz, Tx, Ty, Tz]
+        '''
+        # Maintain an ordered list, tracking which column corresponds to which thruster
+        self.thruster_name_map = []
+        B = []
+        for port in layout:
+            for thruster_name, thruster_info in port['thrusters'].items():
+                # Assemble the B matrix by columns
+                self.thruster_name_map.append(thruster_name)
+                wrench_column = self.get_thruster_wrench(
+                    thruster_info['position'],
+                    thruster_info['direction']
+                )
+                B.append(wrench_column)
+
+        return np.transpose(np.array(B))
+
+    @thread_lock(lock)
+    def map(self, wrench):
+        '''TODO:
+            - Account for variable thrusters
+        '''
+        thrust_cost = np.diag([1.0] * 8)
+
+        def objective(u):
+            '''Compute a cost resembling analytical least squares
+            Minimize 
+                norm((B * u) - wrench) + (u.T * Q * u)
+            Subject to
+                min_u < u < max_u
+            Where 
+                Q defines the cost of firing the thrusters
+                u is a vector where u[n] is the thrust output by thruster_n
+
+            i.e., least squares with bounds
+            '''
+            error_cost = np.linalg.norm(self.B.dot(u) - wrench) ** 2
+            effort_cost = np.transpose(u).dot(thrust_cost).dot(u)
+            return error_cost + effort_cost
+
+        def obj_jacobian(u):
+            '''Compute the jacobian of the objective function
+
+            [1] Scalar-By-Matrix derivative identities [Online]
+                Available: https://en.wikipedia.org/wiki/Matrix_calculus#Scalar-by-vector_identities
+            '''
+            error_jacobian = 2 * self.B.T.dot(self.B.dot(u) - wrench)
+            effort_jacobian = np.transpose(u).dot(2 * thrust_cost)
+            return error_jacobian + effort_jacobian
+
+        minimization = minimize(
+            method='slsqp',
+            fun=objective,
+            jac=obj_jacobian,
+            x0=(self.min_thrust + self.max_thrust) / 2,
+            bounds=zip(self.min_thrust, self.max_thrust),
+            tol=0.1
+        )
+        return minimization.x, minimization.success
+
+    def request_wrench_cb(self, msg):
+        '''Callback for requesting a wrench'''
+        force = rosmsg_to_numpy(msg.wrench.force)
+        torque = rosmsg_to_numpy(msg.wrench.torque)
+        wrench = np.hstack([force, torque])
+
+        u, success = self.map(wrench)
+        if success:
+            thrust_cmds = []
+            # Assemble the list of thrust commands to send
+            for name, thrust in zip(self.thruster_name_map, u):
+                # > Can speed this up by avoiding appends
+                thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
+            self.thruster_pub.publish(thrust_cmds)
+        else:
+            # I expect this to happen frequently enough that we should not raise
+            rospy.logwarn("Could not achieve wrench of {}".format(wrench))
+
+
+if __name__ == '__main__':
+    '''
+        --> Deal with ranges
+        --> Deal with getting B
+        --> Figure out how to communicate with the thrusters
+        --> Do these things in TF
+            - Decided not to do this to avoid needless traffic for a lot of static transforms
+    '''
+    rospy.init_node('thruster_mapper')
+    busses = wait_for_param('busses')
+    mapper = ThrusterMapper(busses)
+    rospy.spin()

gnc/sub8_thruster_mapper/package.xml

@@ -5,7 +5,7 @@
   <description>The sub8_thruster_mapper package</description>
 
   <maintainer email="[email protected]">Jacob Panikulam</maintainer>
-  <license>TODO</license>
+  <license>MIT</license>
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>rospy</build_depend>
   <run_depend>rospy</run_depend>

gnc/sub8_thruster_mapper/test/test_sub8_mapper.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+
+PKG = 'sub8_thruster_mapper'
+NAME = 'test_map'
+
+import sys
+import unittest
+import numpy as np
+from sub8_msgs.msg import Thrust, ThrusterCmd
+from geometry_msgs.msg import WrenchStamped, Wrench, Vector3
+import rospy
+import rostest
+import time
+
+
+class TestMapThrusters(unittest.TestCase):
+    def setUp(self, *args):
+        '''TODO:
+            - Assert that wrenches within bounds are close to the unbounded least-squares estimate
+            - Make waiting for node functionality into a test_helpers util
+            - Test random wrenches and assert valid behavior
+        '''
+        self.got_msg = False
+        self.test_data = []
+
+    def thrust_callback(self, msg):
+        self.got_msg = True
+        self.test_data.append(msg)
+
+    def test_map_good(self):
+        '''Test desired wrenches that are known to be achievable
+        '''
+        # wait at most 5 seconds for the thruster_mapper to be registered
+        timeout_t = time.time() + 5.0
+        while not (rostest.is_subscriber(
+            rospy.resolve_name('/wrench'),
+            rospy.resolve_name('thruster_mapper')) and time.time() < timeout_t):
+            time.sleep(0.1)
+
+        self.assert_(
+            rostest.is_subscriber(
+                rospy.resolve_name('/wrench'),
+                rospy.resolve_name('thruster_mapper')
+            ), 
+            "{} is not up".format('thruster_mapper')
+        )
+
+        thrust_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1, latch=True)
+        wrenches = [
+            np.zeros(6),
+            np.arange(6) * 5,
+            np.arange(6)[::-1] * 5,
+            np.arange(6) * -5,
+            np.arange(6)[::-1] * -5,
+        ]
+        rospy.Subscriber("/thrust", Thrust, self.thrust_callback)
+
+        for num, wrench in enumerate(wrenches):
+            wrench_msg = WrenchStamped(
+                wrench=Wrench(
+                    force=Vector3(*wrench[:3]),
+                    torque=Vector3(*wrench[3:])
+                )
+            )
+
+            thrust_pub.publish(wrench_msg)
+            timeout_t = time.time() + 0.2
+            while not rospy.is_shutdown() and time.time() < timeout_t and not self.got_msg:
+                time.sleep(0.01)
+
+            self.assertEqual(len(self.test_data) - 1, num, msg="Could not compute wrench for " + str(wrench) + " within timeout")
+            self.got_msg = False
+
+
+if __name__ == '__main__':
+    rospy.init_node(NAME, anonymous=True)
+    rostest.rosrun(PKG, NAME, TestMapThrusters, sys.argv)

gnc/sub8_thruster_mapper/test/test_sub8_mapper.test

@@ -0,0 +1,5 @@
+<launch>
+  <!-- <node name="mapper_server" pkg="sub8_thruster_mapper" type="mapper.py" /> -->
+  <include file="$(find sub8_thruster_mapper)/launch/thruster_mapper.launch" />
+  <test test-name="sub8_mapper_test" pkg="sub8_thruster_mapper" type="test_sub8_mapper.py" />
+</launch>

utils/sub8_msgs/CMakeLists.txt

@@ -9,26 +9,26 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
 )
 
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_message_files(
+  FILES
+  Alarm.msg
+  Thrust.msg
+  ThrusterCmd.msg
+)
+add_service_files(
+  FILES
+  ThrusterInfo.srv
+)
+
 # add_action_files(
 #   FILES
 #   Action1.action
-#   Action2.action
-# )
-# generate_messages(
-#   DEPENDENCIES
-#   geometry_msgs
-#   std_msgs
 # )
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+  std_msgs
+)
 
 catkin_package(
     LIBRARIES sub8_msgs

utils/sub8_msgs/msg/Thrust.msg

@@ -0,0 +1,3 @@
+# Control thrust
+
+sub8_msgs/ThrusterCmd[] thruster_commands

utils/sub8_msgs/msg/ThrusterCmd.msg

@@ -0,0 +1,4 @@
+# Thruster Command
+
+string name
+float32 thrust