Move state2vec and vec2state to a utility file, removed copy-pasted code

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/tri_thruster_ctrl.py

@@ -38,10 +38,10 @@ def set_tri_thrusters(self, tri_switches: T.Sequence[int], use_wheel: bool = Fal
         "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_swtiches[0] = Thruster Pair [1,2]
-        tri_swtiches[1] = Thruster Pair [3,4]
-        tri_swtiches[2] = Thruster Pair [5,6]
-        tri_swtiches[3] = Thruster Pair [7,0]
+        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

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/scripts/opt_ctrl_py_node

@@ -25,14 +25,13 @@
 
 import copy
 import rclpy
-
 import numpy as np
 from casadi import *
 import typing as T
 import matplotlib.pyplot as plt
 import time
-
 from ff_control.tri_thruster_ctrl import TrinaryThrusterController
+from ff_control.utils import state2vec
 from ff_msgs.msg import FreeFlyerState
 from ff_msgs.msg import FreeFlyerStateStamped
 from geometry_msgs.msg import PoseStamped
@@ -121,10 +120,10 @@ class ThrusterOptControlNode(TrinaryThrusterController):
 
         # convert desired state to vector form
         if isinstance(state_des, FreeFlyerState):
-            state_des = self.state2vec(state_des)
+            state_des = state2vec(state_des)
 
         if isinstance(curr_state, FreeFlyerState):
-            curr_state = self.state2vec(curr_state)
+            curr_state = state2vec(curr_state)
 
         if np.all(np.abs(state_des - curr_state) < self._deadband):
             self._u = np.zeros(4)
@@ -356,44 +355,6 @@ class ThrusterOptControlNode(TrinaryThrusterController):
 
         return w0
 
-    ############################### Helper Functions to unpack FreeFlyerState ###############################
-    @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
-
     ############################### Helper Functions to access ROS parameters ###############################
 
     @property