From fd55bdfc4a8e1967f4a15a1d9bb5a1f1b869137a Mon Sep 17 00:00:00 2001 From: Andrew Wang Date: Mon, 29 Apr 2024 19:04:23 -0700 Subject: [PATCH] Move state2vec and vec2state to a utility file, removed copy-pasted code --- ff_control/ff_control/linear_ctrl.py | 42 ++------------- ff_control/ff_control/tri_thruster_ctrl.py | 8 +-- ff_control/ff_control/utils.py | 62 ++++++++++++++++++++++ ff_control/scripts/opt_ctrl_py_node | 45 ++-------------- 4 files changed, 72 insertions(+), 85 deletions(-) create mode 100644 ff_control/ff_control/utils.py diff --git a/ff_control/ff_control/linear_ctrl.py b/ff_control/ff_control/linear_ctrl.py index c2bee07..1ef46b1 100644 --- a/ff_control/ff_control/linear_ctrl.py +++ b/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. diff --git a/ff_control/ff_control/tri_thruster_ctrl.py b/ff_control/ff_control/tri_thruster_ctrl.py index 6c48d39..2f7c33d 100644 --- a/ff_control/ff_control/tri_thruster_ctrl.py +++ b/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 diff --git a/ff_control/ff_control/utils.py b/ff_control/ff_control/utils.py new file mode 100644 index 0000000..f1f566b --- /dev/null +++ b/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 diff --git a/ff_control/scripts/opt_ctrl_py_node b/ff_control/scripts/opt_ctrl_py_node index 7d75255..b714d29 100755 --- a/ff_control/scripts/opt_ctrl_py_node +++ b/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