Merge pull request #23 from StanfordASL/andrew/optimization_controller

Implement new fuel-efficient Optimization Controller

.gitignore

@@ -13,13 +13,15 @@ __pycache__/
 # Distribution / packaging
 .Python
 build/
+install/
 develop-eggs/
 dist/
 downloads/
 eggs/
 .eggs/
 lib/
 lib64/
+log/
 parts/
 sdist/
 var/
@@ -131,3 +133,6 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
+
+# Data from Free-Flyer low-level MPC characterization experiments (Andrew Wang, 2024)
+testing_files/experiment_data

ff_control/CMakeLists.txt

@@ -70,6 +70,7 @@ ament_python_install_package(${PROJECT_NAME})
 
 # install Python nodes
 install(PROGRAMS
+  scripts/opt_ctrl_py_node
   scripts/pd_ctrl_py_node
   scripts/safety_filter
   DESTINATION lib/${PROJECT_NAME}

ff_control/ff_control/linear_ctrl.py

@@ -28,6 +28,7 @@
 from ff_msgs.msg import FreeFlyerState
 from ff_msgs.msg import FreeFlyerStateStamped
 from ff_msgs.msg import Wrench2D
+from ff_control.utils import state2vec
 
 import numpy as np
 
@@ -88,9 +89,9 @@ def send_control(self, state_des: T.Union[FreeFlyerState, np.ndarray], K: np.nda
 
         # convert desired state to vector form
         if isinstance(state_des, FreeFlyerState):
-            state_des = self.state2vec(state_des)
+            state_des = state2vec(state_des)
 
-        state_vector = self.state2vec(self.get_state())
+        state_vector = state2vec(self.get_state())
         state_delta = state_des - state_vector
         # wrap angle delta to [-pi, pi]
         state_delta[2] = (state_delta[2] + np.pi) % (2 * np.pi) - np.pi
@@ -111,43 +112,6 @@ def state_ready_callback(self) -> None:
         """
         pass
 
-    @staticmethod
-    def state2vec(state: FreeFlyerState) -> np.ndarray:
-        """
-        Convert state message to state vector.
-
-        :param state: state message
-        :return: state vector
-        """
-        return np.array(
-            [
-                state.pose.x,
-                state.pose.y,
-                state.pose.theta,
-                state.twist.vx,
-                state.twist.vy,
-                state.twist.wz,
-            ]
-        )
-
-    @staticmethod
-    def vec2state(vec: np.ndarray) -> FreeFlyerState:
-        """
-        Convert state vector to state message.
-
-        :param vec: state vector
-        :return: state message
-        """
-        state = FreeFlyerState()
-        state.pose.x = vec[0]
-        state.pose.y = vec[1]
-        state.pose.theta = vec[2]
-        state.twist.vx = vec[3]
-        state.twist.vy = vec[4]
-        state.twist.wz = vec[5]
-
-        return state
-
     def state_is_ready(self) -> bool:
         """
         Check if state is ready.

ff_control/ff_control/ll_ctrl.py

@@ -34,7 +34,7 @@ def __init__(self, node_name: str = "ll_ctrl_node") -> None:
         super().__init__(node_name)
 
         # robot parameters that can be accessed by sub-classes
-        self.p = RobotParams(self)
+        self.p = RobotParams(self, self.param_update_callback)
 
         # low level thruster control publishers
         self._thruster_binary_pub = self.create_publisher(ThrusterCommand, "ctrl/binary_thrust", 10)
@@ -76,11 +76,18 @@ def set_wheel_velocity(self, velocity: float) -> None:
         """
         Send command to set the inertial wheel velocity.
 
-        TODO(alvin): suppor this or remove?
+        TODO(alvin): support this or remove?
 
         :param velocity: angular velocity in [rad/s]
         """
         msg = WheelVelCommand()
         msg.header.stamp = self.get_clock().now().to_msg()
         msg.velocity = velocity
         self._wheel_pub.publish(msg)
+
+    def param_update_callback(self):
+        """
+        Callback when parameters are initialized
+        Override in subclass
+        """
+        pass

ff_control/ff_control/tri_thruster_ctrl.py

@@ -0,0 +1,76 @@
+# MIT License
+#
+# Copyright (c) 2024 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+from ff_control.ll_ctrl import LowLevelController
+from ff_msgs.msg import ThrusterCommand
+
+import numpy as np
+import typing as T
+
+
+class TrinaryThrusterController(LowLevelController):
+    def __init__(self, node_name: str = "tri_thruster_ctrl_node") -> None:
+        super().__init__(node_name)
+
+    def set_tri_thrusters(self, tri_switches: T.Sequence[int], use_wheel: bool = False) -> None:
+        """
+        Convert trinary thruster commands into binary thruster commands
+        This formulation represents each thruster pair (eg thruster 1 and 2 below) as a single
+        "trinary" thruster, which can either take value -1 (1 on 2 off), 0 (both off), or 1 (1 off 2 on)
+        This reduces the search space for the optimization, and implicitly removes consideration of the
+        undesirable case where both thrusters are on (0 net force or moment, only wasted fuel)
+        tri_switches[0] = Thruster Pair [1,2]
+        tri_switches[1] = Thruster Pair [3,4]
+        tri_switches[2] = Thruster Pair [5,6]
+        tri_switches[3] = Thruster Pair [7,0]
+
+        
+        Thrusters Configuration
+             (2) e_y (1)        ___
+            <--   ^   -->      /   \
+           ^  |   |   |  ^     v M  )
+        (3)|--o-------o--|(0)    __/
+              | free- |
+              | flyer |   ---> e_x
+              | robot |
+        (4)|--o-------o--|(7)
+           v  |       |  v
+            <--       -->
+             (5)     (6)
+        """
+
+        if len(tri_switches) != len(ThrusterCommand().switches) / 2:
+            self.get_logger().error("Incompatible thruster length sent." + str(len(tri_switches)))
+            return
+
+        switches = []
+        for i in range(len(tri_switches)):
+            if tri_switches[i] > 0:
+                switches.extend([True, False])
+            elif tri_switches[i] == 0:
+                switches.extend([False, False])
+            else:
+                switches.extend([False, True])
+        lastVal = switches.pop(-1)
+        switches = [lastVal] + switches
+
+        self.set_thrust_binary(switches)

ff_control/ff_control/utils.py

@@ -0,0 +1,62 @@
+# MIT License
+#
+# Copyright (c) 2024 Stanford Autonomous Systems Lab
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import numpy as np
+from ff_msgs.msg import FreeFlyerState
+
+
+##################### Helper Functions to unpack FreeFlyerState #####################
+def state2vec(state: FreeFlyerState) -> np.ndarray:
+    """
+    Convert state message to state vector.
+
+    :param state: state message
+    :return: state vector
+    """
+    return np.array(
+        [
+            state.pose.x,
+            state.pose.y,
+            state.pose.theta,
+            state.twist.vx,
+            state.twist.vy,
+            state.twist.wz,
+        ]
+    )
+
+
+def vec2state(vec: np.ndarray) -> FreeFlyerState:
+    """
+    Convert state vector to state message.
+
+    :param vec: state vector
+    :return: state message
+    """
+    state = FreeFlyerState()
+    state.pose.x = vec[0]
+    state.pose.y = vec[1]
+    state.pose.theta = vec[2]
+    state.twist.vx = vec[3]
+    state.twist.vy = vec[4]
+    state.twist.wz = vec[5]
+
+    return state

ff_control/ff_control/wrench_ctrl.py

@@ -35,7 +35,7 @@ def set_body_wrench(self, wrench_body: Wrench2D, use_wheel: bool = False) -> Non
         Set wrench in body frame.
 
         :param wrench_body: wrench in body frame
-        :param use_wheel: set to ture to use the inertial wheel (TODO(alvin): unsupported)
+        :param use_wheel: set to true to use the inertial wheel (TODO(alvin): unsupported)
         """
         if use_wheel:
             self.get_logger().error("set_wrench failed: use_wheel not implemented")

ff_control/package.xml

@@ -17,6 +17,8 @@
   <depend>rclpy</depend>
   <depend>geometry_msgs</depend>
 
+  <exec_depend>casadi-pip</exec_depend>
+
   <depend>ff_estimate</depend>
   <depend>ff_msgs</depend>
   <depend>ff_params</depend>