diff --git a/.gitignore b/.gitignore index c54510b..4d0e6fe 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,7 @@ __pycache__/ # Distribution / packaging .Python build/ +install/ develop-eggs/ dist/ downloads/ @@ -20,6 +21,7 @@ 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 diff --git a/ff_control/CMakeLists.txt b/ff_control/CMakeLists.txt index 0213d44..252b10d 100644 --- a/ff_control/CMakeLists.txt +++ b/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} 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/ll_ctrl.py b/ff_control/ff_control/ll_ctrl.py index 0700f42..bde8420 100644 --- a/ff_control/ff_control/ll_ctrl.py +++ b/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,7 +76,7 @@ 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] """ @@ -84,3 +84,10 @@ def set_wheel_velocity(self, velocity: float) -> None: 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 diff --git a/ff_control/ff_control/tri_thruster_ctrl.py b/ff_control/ff_control/tri_thruster_ctrl.py new file mode 100644 index 0000000..2f7c33d --- /dev/null +++ b/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) 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/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index 55c4143..3208cee 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/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") diff --git a/ff_control/package.xml b/ff_control/package.xml index 680f795..99d9624 100644 --- a/ff_control/package.xml +++ b/ff_control/package.xml @@ -17,6 +17,8 @@ rclpy geometry_msgs + casadi-pip + ff_estimate ff_msgs ff_params diff --git a/ff_control/scripts/opt_ctrl_py_node b/ff_control/scripts/opt_ctrl_py_node new file mode 100755 index 0000000..0e41079 --- /dev/null +++ b/ff_control/scripts/opt_ctrl_py_node @@ -0,0 +1,460 @@ +#!/usr/bin/env python3 + +# 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 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 +from std_msgs.msg import Float32 + + +class ThrusterOptControlNode(TrinaryThrusterController): + """ + Class for a CasADi-based optimization controller (based on linear_ctrl.py. + + state definition: [x, y, theta, vx, vy, wz] + control definition: [u0, u1, u2, u3] Trinary (-1, 0, 1) thruster representation of 8-thruster config + + Note: the current implementation is not thread safe + """ + STATE_DIM = 6 + CONTROL_DIM = 4 + + def __init__(self): + super().__init__('thrust_opt_control_node') + # State estimator + self.declare_parameter("state_channel", "est/state") + self._state_sub = self.create_subscription( + FreeFlyerStateStamped, + self.get_parameter("state_channel").get_parameter_value().string_value, + self._state_callback, + 10, + ) + self._state_ready = False + self._state_stamped = FreeFlyerStateStamped() + self._state_desired = FreeFlyerStateStamped() + self._params_ready = False + self._deadband = np.array([0.03, 0.03, 0.05, 0.001, 0.001, 0.02]) + self._readjust_controller_bounds = np.array([0.4, 0.4]) + self._def_controller = True + + # Goal State command + self.state_sp_sub = self.create_subscription(FreeFlyerStateStamped, + 'ctrl/state', self.state_setpoint_callback, 10) + self.rviz_sp_sub = self.create_subscription(PoseStamped, + '/goal_pose', self.rviz_setpoint_callback, 10) + + self.goal_repeater_pub = self.create_publisher(FreeFlyerStateStamped, "goal", 10) + self.opt_cost_pub = self.create_publisher(Float32, "cost", 10) + + # Optimization far hyperparameters/gains + self.get_logger().info("*** Reset to default hyperparams ***") + self.declare_parameter('def_opt_horizon_secs', 2.667) + self.declare_parameter('def_opt_freq', 3.0) + self.declare_parameter('def_opt_gain_kth', 1.0) + self.declare_parameter('def_opt_gain_kpos', 2.5) + self.declare_parameter('def_opt_gain_kinput', 20.0) + self.declare_parameter('def_opt_gain_kvelo', 40.0) + + self.declare_parameter('close_opt_horizon_secs', 0.4) + self.declare_parameter('close_opt_freq', 10.0) + self.declare_parameter('close_opt_gain_kth', 5.5) + self.declare_parameter('close_opt_gain_kpos', 90.0) + self.declare_parameter('close_opt_gain_kinput', 5.5) + self.declare_parameter('close_opt_gain_kvelo', 125.0) + + # To start, Frequency based on default control frequency in the optimizer (3Hz) + self.timer = self.create_timer(1.0/self.freq, self.control_loop) + + def param_update_callback(self): + self.w0, self.cont_nlp_solver, self.lbw, self.ubw, self.lbg, self.ubg = self.init_solver() + self._params_ready = True + + def send_control(self, curr_state: T.Union[FreeFlyerState, np.ndarray], state_des: T.Union[FreeFlyerState, np.ndarray]) -> None: + """ + Send desirable target state for optimization-based control. + + :param state_des: desired state + """ + t = time.time() + # self.get_logger().info("Goal"+str(state_des) + "\n") + if not self._state_ready: + self.get_logger().warn("send_control ignored, current state not yet ready") + return + + if not self._params_ready: + self.get_logger().warn("send_control ignored, parameters not yet ready") + return + + self.goal_repeater_pub.publish(self._state_desired) + + # convert desired state to vector form + if isinstance(state_des, FreeFlyerState): + state_des = state2vec(state_des) + + if isinstance(curr_state, FreeFlyerState): + curr_state = state2vec(curr_state) + + if np.all(np.abs(state_des - curr_state) < self._deadband): + self._u = np.zeros(4) + else: + if self._def_controller: + pos_err = np.abs(state_des[:2] - curr_state[:2]) + if np.all(pos_err < self._readjust_controller_bounds): + self.set_close_hyperparameters() + else: + pos_err = np.abs(state_des[:2] - curr_state[:2]) + if np.any(pos_err > self._readjust_controller_bounds): + self.set_default_hyperparameters() + + + self.lbw[:len(curr_state)] = curr_state + self.ubw[:len(curr_state)] = curr_state + self.w0[0] = curr_state + + sol = self.cont_nlp_solver(x0=vertcat(*self.w0), p=DM(list(state_des)), lbx=self.lbw, ubx=self.ubw, lbg=self.lbg, ubg=self.ubg) + output = sol['x'] + + cost = Float32() + cost.data = float(sol['f']) + self.opt_cost_pub.publish(cost) + + u0_opt, u1_opt, u2_opt, u3_opt = self.unpack_wopt(output) + cont_thrust = np.array([u0_opt[0], u1_opt[0], u2_opt[0], u3_opt[0]]).reshape((4,1)) / self.Fmax + self._u = np.round(cont_thrust) + + # Warm Start next run + self.w0 = self.get_next_warm_start(output) + + self.set_tri_thrusters(self._u) + self.get_logger().info("Control Computation Time: " + str(time.time()-t)) + + def get_state(self) -> T.Optional[FreeFlyerState]: + """Get the current latest state.""" + if not self._state_ready: + self.get_logger().error("get_state failed: state not yet ready") + return None + + return self._state_stamped.state + + def state_ready_callback(self) -> None: + # copy current position as goal position + self._state_desired.header.stamp = self.get_clock().now().to_msg() + self._state_desired.state = self.get_state() + + def state_setpoint_callback(self, msg: FreeFlyerStateStamped) -> None: + self.set_default_hyperparameters() + self._state_desired = copy.deepcopy(msg) + + def rviz_setpoint_callback(self, msg: PoseStamped) -> None: + self.set_default_hyperparameters() + self._state_desired.header.stamp = msg.header.stamp + + self._state_desired.state.pose.x = msg.pose.position.x + self._state_desired.state.pose.y = msg.pose.position.y + z = msg.pose.orientation.z + w = msg.pose.orientation.w + self._state_desired.state.pose.theta = np.arctan2(2 * w * z, w * w - z * z) + + self._state_desired.state.twist.vx = 0. + self._state_desired.state.twist.vy = 0. + self._state_desired.state.twist.wz = 0. + + def control_loop(self) -> None: + # state not yet ready + if not self.state_is_ready(): + self.get_logger().error("control_loop not started: state not yet ready") + return + + self.send_control(self.get_state(), self._state_desired.state) + + def init_solver(self): + # Initialize constraints on x (x,y,th,xdot,ydot,thdot) + lbx = [0., 0., -np.pi, -0.5, -0.5, -0.5] + ubx = [4., 4., np.pi, 0.5, 0.5, 0.5] + lbu = [-self.Fmax] * 4 + ubu = [self.Fmax] * 4 + + N = int(self.T * self.freq) + x0 = [0]*6 # Just initialize to zeros, will revise once first state measurement comes in + + # Declare model variables + x = MX.sym('x', self.STATE_DIM) # x,y,th,xdot,ydot,thdot + u = MX.sym('u', self.CONTROL_DIM) # 4 trinary thrusters (-1, 0, 1) + goal = MX.sym("goal", self.STATE_DIM) + + body_Fx, body_Fy, M = self._map_to_force(u) + + th = x[2] + world_Fx = body_Fx*cos(th) - body_Fy*sin(th) + world_Fy = body_Fx*sin(th) + body_Fy*cos(th) + + # Model equations + xdot = vertcat(x[3], + x[4], + x[5], + world_Fx / self.m, + world_Fy / self.m, + M / self.Ixx) + + # Stepwise Cost + L = self.k_th*(1-cos(goal[2]-x[2])) + self.k_pos*(self.normsq(goal[0:2]-x[0:2])) + self.k_input*(self.normsq(u)) + self.k_velo*self.normsq(goal[3:5]-x[3:5]) + + # Fixed step Runge-Kutta 4 integrator + M = 4 # RK4 steps per interval + DT = self.T/N/M + f = Function('f', [x, u, goal], [xdot, L]) # Define function to take in current state/input and output xdot and cost + X0 = MX.sym('X0', 6) + U = MX.sym('U', 4) + X = X0 + Q = 0 + for j in range(M): + k1, k1_q = f(X, U, goal) + k2, k2_q = f(X + DT/2 * k1, U, goal) + k3, k3_q = f(X + DT/2 * k2, U, goal) + k4, k4_q = f(X + DT * k3, U, goal) + X=X+DT/6*(k1 +2*k2 +2*k3 +k4) + Q = Q + DT/6*(k1_q + 2*k2_q + 2*k3_q + k4_q) + F = Function('F', [X0, U, goal], [X, Q],['x0','p', 'param_goal'],['xf','qf']) # Take in initial state and current input and outputs final state and cost after one step (Runge-Kutta integrated) + + # Initial guess for u + u_start = [DM([0.,0.,0.,0.])] * N + + # Get a feasible trajectory as an initial guess + xk = DM(x0) + x_start = [xk] + for k in range(N): + xk = F(x0=xk, p=u_start[k])['xf'] + x_start += [xk] + + # Start with an empty NLP + w=[] + w0 = [] + lbw = [] + ubw = [] + discrete = [] + J = 0 + g=[] + lbg = [] + ubg = [] + + # "Lift" initial conditions + X0 = MX.sym('X0', self.STATE_DIM) + w += [X0] + lbw += x0 + ubw += x0 + w0 += [x_start[0]] + + # Formulate the NLP + Xk = X0 + for k in range(N): + # New NLP variable for the control + Uk = MX.sym('U_' + str(k), 4) + w += [Uk] + lbw += lbu + ubw += ubu + w0 += [u_start[k]] + + # Integrate till the end of the interval + Fk = F(x0=Xk, p=Uk) + Xk_end = Fk['xf'] + J=J+Fk['qf'] + + # New NLP variable for state at end of interval + Xk = MX.sym('X_' + str(k+1), 6) + w += [Xk] + lbw += lbx + ubw += ubx + w0 += [x_start[k+1]] + # Add equality dynamics constraint + g += [Xk_end-Xk] + lbg += [0, 0, 0, 0, 0, 0] + ubg += [0, 0, 0, 0, 0, 0] + + J = J + 10*self.k_pos*(self.normsq(Xk_end[0:2]-goal[0:2])) + 10*self.k_th*(1-cos(goal[2]-Xk_end[2])) + 10*self.k_velo*self.normsq(Xk_end[3:]-goal[3:]) + + # Concatenate decision variables and constraint terms + w = vertcat(*w) + g = vertcat(*g) + + # Create an NLP solver + nlp_prob = {'f': J, 'p':goal, 'x': w, 'g': g} + cont_nlp_solver = nlpsol('nlp_solver', 'ipopt', nlp_prob); # Solve relaxed problem + return w0, cont_nlp_solver, lbw, ubw, lbg, ubg + + ############################### Helper Functions for Opt ############################### + def _map_to_force(self, u): + # Compute body-frame force from thrusters + Fx = -u[0] + u[2] + Fy = -u[1] + u[3] + M = self.r * (u[0]+u[1]+u[2]+u[3]) + return Fx, Fy, M + + def normsq(self, x): + """ norm square operation for casadi variables. """ + sum = 0 + for i in range(x.shape[0]): + sum += x[i]**2 + return sum + + def unpack_wopt(self, w_opt): + w_opt = w_opt.full().flatten() + u0_opt = w_opt[6::10] + u1_opt = w_opt[7::10] + u2_opt = w_opt[8::10] + u3_opt = w_opt[9::10] + return u0_opt, u1_opt, u2_opt, u3_opt + + def get_next_warm_start(self, w_opt): + output = w_opt.full().flatten() + x = [output[10*i:6+10*i] for i in range(w_opt.size()[0]//10+1)] + u = [output[6+10*i:10+10*i] for i in range(w_opt.size()[0]//10)] + + w0 = [DM(x[1])] + for i in range(len(u)-1): + w0 += [DM(u[i+1])] + w0 += [DM(x[i+2])] + w0 += [DM(u[-1])] + w0 += [DM(x[-1])] + + return w0 + + ############################### Helper Functions to access ROS parameters ############################### + + @property + def r(self): + return self.p.actuators["thrusters_lever_arm"] + + @property + def Fmax(self): + return self.p.actuators["F_max_per_thruster"] + + @property + def m(self): + return self.p.dynamics["mass"] + + @property + def Ixx(self): + return self.p.dynamics["inertia"] + + @property + def k_th(self): + if (self._def_controller): + return self.get_parameter('def_opt_gain_kth').value + else: + return self.get_parameter('close_opt_gain_kth').value + + @property + def k_pos(self): + if (self._def_controller): + return self.get_parameter('def_opt_gain_kpos').value + else: + return self.get_parameter('close_opt_gain_kpos').value + + @property + def k_input(self): + if (self._def_controller): + return self.get_parameter('def_opt_gain_kinput').value + else: + return self.get_parameter('close_opt_gain_kinput').value + + @property + def k_velo(self): + if (self._def_controller): + return self.get_parameter('def_opt_gain_kvelo').value + else: + return self.get_parameter('close_opt_gain_kvelo').value + + + @property + def T(self): + if (self._def_controller): + return self.get_parameter('def_opt_horizon_secs').value + else: + return self.get_parameter('close_opt_horizon_secs').value + + @property + def freq(self): + if (self._def_controller): + return self.get_parameter('def_opt_freq').value + else: + return self.get_parameter('close_opt_freq').value + + def state_is_ready(self) -> bool: + """ + Check if state is ready. + + :return: True if state is ready, False otherwise + """ + return self._state_ready + + def _state_callback(self, msg: FreeFlyerStateStamped) -> None: + """ + Get called when the first current state measurement comes in. + """ + self._state_stamped = copy.deepcopy(msg) + + if not self._state_ready: + self._state_ready = True + self.state_ready_callback() + + def set_default_hyperparameters(self) -> None: + # Optimization for default longer trajectories hyperparameters/gains + self.get_logger().info("*** Reset to default hyperparams ***") + self._def_controller = True + + self.w0, self.cont_nlp_solver, self.lbw, self.ubw, self.lbg, self.ubg = self.init_solver() + + self.timer.cancel() + self.timer = self.create_timer(1.0/self.freq, self.control_loop) + + + def set_close_hyperparameters(self) -> None: + # Optimization close-range fine-tuning trajectories hyperparameters/gains + self.get_logger().info("*** Set to close hyperparams ***") + self._def_controller = False + self.w0, self.cont_nlp_solver, self.lbw, self.ubw, self.lbg, self.ubg = self.init_solver() + + self.timer.cancel() + self.timer = self.create_timer(1.0/self.freq, self.control_loop) + + +def main(args=None): + rclpy.init(args=args) + opt_ctrl = ThrusterOptControlNode() + rclpy.spin(opt_ctrl) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ff_msgs/CMakeLists.txt b/ff_msgs/CMakeLists.txt index a889b37..f1bdcb4 100644 --- a/ff_msgs/CMakeLists.txt +++ b/ff_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/WheelVelCommand.msg" "msg/ThrusterCommand.msg" "msg/ThrusterPWMCommand.msg" + "msg/ControllerMetrics.msg" DEPENDENCIES std_msgs ) diff --git a/ff_msgs/msg/ControllerMetrics.msg b/ff_msgs/msg/ControllerMetrics.msg new file mode 100644 index 0000000..aa921c2 --- /dev/null +++ b/ff_msgs/msg/ControllerMetrics.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +float64[8] running_duty_cycles +float64 total_gas_time diff --git a/ff_sim/ff_sim/controller_metrics.py b/ff_sim/ff_sim/controller_metrics.py new file mode 100755 index 0000000..2f6fce3 --- /dev/null +++ b/ff_sim/ff_sim/controller_metrics.py @@ -0,0 +1,146 @@ +# 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. + + +""" +Simulates the freeflyer. + +Maps thrusters + wheel control input +to a state evolution over time +----------------------------------------------- +""" + +import math +import sys +import numpy as np +import typing as T + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +from queue import Queue + +from ff_msgs.msg import ( + FreeFlyerStateStamped, + Wrench2DStamped, + WheelVelCommand, + ThrusterCommand, + ControllerMetrics, +) +from ff_params import RobotParams + + +class ControllerMetricsPublisher(Node): + """ + Class to listen to free flyer commands and calculate metrics + Calculates two key metrics: + 1. total_gas_time: Measures total time that thrusters are on (summed over each thruster). Provides + a proxy for total gas expenditure over time + 2. running_duty_cycles: Measures average duty cycle for each thruster over a time window specified by + self.duty_cycle_window. + + """ + + def __init__(self): + super().__init__("ff_ctrl_metrics") + self.curr_time = self.get_clock().now() + self.steps = 0 + self.running_total_gas = 0 + self.prev_thruster_sum = 0 + self.duty_cycle_window = 6 + self.thrust_hist = [Queue(maxsize=self.duty_cycle_window) for i in range(8)] + self.time_hist = Queue(maxsize=self.duty_cycle_window) + self.thrust_duty_cycles = [0] * 8 + self.total_time_window = 0 + + self.sub_wheel_cmd_vel = self.create_subscription( + WheelVelCommand, "commands/velocity", self.process_new_wheel_cmd, 10 + ) + self.sub_thrusters_cmd_binary = self.create_subscription( + ThrusterCommand, "commands/binary_thrust", self.process_new_binary_thrust_cmd, 10 + ) + self.pub_controller_metrics = self.create_publisher( + ControllerMetrics, "metrics/controller", 10 + ) + + def process_new_wheel_cmd(self, msg: WheelVelCommand) -> None: + """Placeholder for now""" + pass + + def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None: + """Process binary thrusters""" + now = self.get_clock().now() + dt = (now - self.curr_time).nanoseconds / 1e9 + + # Perform Euler integration for how long each thruster was on + thrusters = np.array(msg.switches, dtype=float) + self.running_total_gas += self.prev_thruster_sum * dt + self.prev_thruster_sum = np.sum(thrusters) + + # Calculate rolling average of duty cycle for each thruster + if not self.time_hist.full(): # Build up full queue valid duty cycles at the beginning + # Update time variables + self.time_hist.put_nowait(dt) + prev_time_window = self.total_time_window + self.total_time_window += dt + # Update rolling thrust averages + for i in range(8): + self.thrust_hist[i].put_nowait(thrusters[i]) + weighted_avg_thrust = ( + self.thrust_duty_cycles[i] * prev_time_window + thrusters[i] * dt + ) + self.thrust_duty_cycles[i] = weighted_avg_thrust / self.total_time_window + else: # Once queue is filled up, we need to pop a value off and append the new one, and update running averages + # Update time variables + dt_0 = self.time_hist.get_nowait() + self.time_hist.put_nowait(dt) + prev_time_window = self.total_time_window + self.total_time_window = self.total_time_window - dt_0 + dt + # Update rolling thrust averages + for i in range(8): + thrust_0 = self.thrust_hist[i].get_nowait() + self.thrust_hist[i].put_nowait(thrusters[i]) + weighted_avg_thrust = ( + self.thrust_duty_cycles[i] * prev_time_window + - thrust_0 * dt_0 + + thrusters[i] * dt + ) + self.thrust_duty_cycles[i] = weighted_avg_thrust / self.total_time_window + + metrics = ControllerMetrics() + metrics.header.stamp = now.to_msg() + metrics.total_gas_time = self.running_total_gas + metrics.running_duty_cycles = self.thrust_duty_cycles + self.pub_controller_metrics.publish(metrics) + + self.curr_time = now + + +def main(): + rclpy.init() + ff_metrics = ControllerMetricsPublisher() + rclpy.spin(ff_metrics) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py old mode 100644 new mode 100755 index 018e5c7..29e451f --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -1,6 +1,6 @@ # MIT License # -# Copyright (c) 2023 Stanford Autonomous Systems Lab +# 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 @@ -402,6 +402,9 @@ def compute_dynamics_dt(self, x_k, u_k, dt, discretization="Euler"): else: print("[FreeFlyerSimulator::compute_dynamics_dt]: Unknown Discretization Scheme.") + # Wrap theta to [-pi, pi] + x_next[2] = (x_next[2] + np.pi) % (2 * np.pi) - np.pi + return x_next diff --git a/ff_sim/launch/single.launch.py b/ff_sim/launch/single.launch.py index 3d8c900..14b0e08 100644 --- a/ff_sim/launch/single.launch.py +++ b/ff_sim/launch/single.launch.py @@ -45,6 +45,12 @@ def generate_launch_description(): name="simulator_node", namespace=robot_name, ), + Node( + package="ff_sim", + executable="controller_metrics", + name="controller_metrics", + namespace=robot_name, + ), Node( package="ff_control", executable="safety_filter", diff --git a/ff_sim/setup.py b/ff_sim/setup.py index 8c8f284..dd5226d 100644 --- a/ff_sim/setup.py +++ b/ff_sim/setup.py @@ -20,6 +20,9 @@ license="TODO: License declaration", tests_require=["pytest"], entry_points={ - "console_scripts": ["simulator_node = ff_sim.simulator_node:main"], + "console_scripts": [ + "simulator_node = ff_sim.simulator_node:main", + "controller_metrics = ff_sim.controller_metrics:main", + ], }, ) diff --git a/freeflyer/CMakeLists.txt b/freeflyer/CMakeLists.txt index e4627fe..6b4bc2c 100644 --- a/freeflyer/CMakeLists.txt +++ b/freeflyer/CMakeLists.txt @@ -27,9 +27,10 @@ if(BUILD_TESTING) # check Python style with black ament_add_pytest_test(black_formatting test/black_formatting.py) - # test both implementations of PD controller + # test both implementations of PD controller and Opt controller add_launch_test(test/pd_ctrl_launch_test.py TARGET pd_ctrl_cpp_test ARGS "impl:=cpp") add_launch_test(test/pd_ctrl_launch_test.py TARGET pd_ctrl_py_test ARGS "impl:=py") + add_launch_test(test/opt_ctrl_launch_test.py TARGET opt_ctrl_py_test ARGS "impl:=py") endif() ament_package() diff --git a/freeflyer/launch/hardware_opt.launch.py b/freeflyer/launch/hardware_opt.launch.py new file mode 100644 index 0000000..ddb593e --- /dev/null +++ b/freeflyer/launch/hardware_opt.launch.py @@ -0,0 +1,85 @@ +# 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 launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_name = LaunchConfiguration("robot_name") + impl = LaunchConfiguration("impl") + + return LaunchDescription( + [ + DeclareLaunchArgument("robot_name", default_value="robot"), + DeclareLaunchArgument( + "impl", + default_value="py", + description="Optimization controller implementation", + choices=["cpp", "py"], + ), + IncludeLaunchDescription( + PathJoinSubstitution( + [ + FindPackageShare("ff_viz"), + "launch", + "ff_viz.launch.py", + ] + ), + launch_arguments={"robot_name": robot_name}.items(), + ), + Node( + package="ff_sim", + executable=["controller_metrics"], + name="controller_metrics", + namespace=robot_name, + ), + Node( + package="ff_control", + executable=["opt_ctrl_", impl, "_node"], + name="opt_ctrl_node", + namespace=robot_name, + ), + Node( + package="ff_estimate", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", + namespace=robot_name, + parameters=[ + { + "pose_channel": PathJoinSubstitution( + [ + "mocap", + robot_name, + "pose", + ] + ), + } + ], + ), + ] + ) diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index 6e4f0a4..408e392 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -60,6 +60,12 @@ def generate_launch_description(): launch_arguments={"robot_name": robot_name}.items(), condition=IfCondition(rviz), ), + Node( + package="ff_sim", + executable=["controller_metrics"], + name="controller_metrics", + namespace=robot_name, + ), Node( package="ff_control", executable=["pd_ctrl_", impl, "_node"], diff --git a/freeflyer/launch/sim_opt.launch.py b/freeflyer/launch/sim_opt.launch.py new file mode 100644 index 0000000..f216179 --- /dev/null +++ b/freeflyer/launch/sim_opt.launch.py @@ -0,0 +1,78 @@ +# MIT License +# +# Copyright (c) 2023 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 launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_name = LaunchConfiguration("robot_name") + impl = LaunchConfiguration("impl") + + return LaunchDescription( + [ + DeclareLaunchArgument("robot_name", default_value="robot"), + DeclareLaunchArgument( + "impl", + default_value="py", + description="Optimization controller implementation", + choices=["cpp", "py"], + ), + IncludeLaunchDescription( + PathJoinSubstitution( + [ + FindPackageShare("ff_sim"), + "launch", + "single.launch.py", + ] + ), + launch_arguments={"robot_name": robot_name}.items(), + ), + IncludeLaunchDescription( + PathJoinSubstitution( + [ + FindPackageShare("ff_viz"), + "launch", + "ff_viz.launch.py", + ] + ), + launch_arguments={"robot_name": robot_name}.items(), + ), + Node( + package="ff_control", + executable=["opt_ctrl_", impl, "_node"], + name="opt_ctrl_node", + namespace=robot_name, + ), + Node( + package="ff_estimate", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", + namespace=robot_name, + ), + ] + ) diff --git a/freeflyer/test/opt_ctrl_launch_test.py b/freeflyer/test/opt_ctrl_launch_test.py new file mode 100644 index 0000000..418d2b4 --- /dev/null +++ b/freeflyer/test/opt_ctrl_launch_test.py @@ -0,0 +1,149 @@ +# MIT License +# +# Copyright (c) 2023 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 time +import unittest + +from ff_msgs.msg import FreeFlyerStateStamped + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest + +import rclpy + + +ROBOT_NAME = "freeflyer" + + +def generate_test_description(): + impl = LaunchConfiguration("impl") + + sim_launch = IncludeLaunchDescription( + PathJoinSubstitution( + [ + FindPackageShare("ff_sim"), + "launch", + "single.launch.py", + ] + ), + launch_arguments={"robot_name": ROBOT_NAME}.items(), + ) + opt_ctrl_node = Node( + package="ff_control", + executable=["opt_ctrl_", impl, "_node"], + name="opt_ctrl_node", + namespace=ROBOT_NAME, + ) + estimator = Node( + package="ff_estimate", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", + namespace=ROBOT_NAME, + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("impl", default_value="py", choices=["cpp", "py"]), + sim_launch, + opt_ctrl_node, + estimator, + ReadyToTest(), + ] + ) + + +class TestOptControlNode(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_opt_ctrl_node") + + def tearDown(self): + self.node.destroy_node() + + def test_set_target_state(self): + current_state = FreeFlyerStateStamped() + + def state_callback(msg): + current_state.header = msg.header + current_state.state = msg.state + + sub = self.node.create_subscription( + FreeFlyerStateStamped, + f"/{ROBOT_NAME}/sim/state", + state_callback, + 10, + ) + pub = self.node.create_publisher( + FreeFlyerStateStamped, + f"/{ROBOT_NAME}/ctrl/state", + 10, + ) + + try: + # wait for nodes to start up (with 5 seconds timeout) + end_time = time.time() + 5.0 + node_flag = False + while time.time() < end_time and not node_flag: + node_flag = ( + "opt_ctrl_node" in self.node.get_node_names() + and "simulator_node" in self.node.get_node_names() + ) + time.sleep(0.1) + assert node_flag, "opt_ctrl_node or simulator_node launch failure" + + # wait for node to initialize + time.sleep(3.0) + + # publish target state + target_state = FreeFlyerStateStamped() + target_state.header.stamp = self.node.get_clock().now().to_msg() + target_state.state.pose.x = 1.0 + target_state.state.pose.y = 1.0 + target_state.state.pose.theta = 1.0 + pub.publish(target_state) + + # wait for 15 seconds and check results + end_time = time.time() + 25.0 + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + + # current state should be close to the target state + self.assertAlmostEquals(current_state.state.pose.x, 1.0, delta=1e-1) + self.assertAlmostEquals(current_state.state.pose.y, 1.0, delta=1e-1) + self.assertAlmostEquals(current_state.state.pose.theta, 1.0, delta=1e-1) + + finally: + self.node.destroy_subscription(sub) + self.node.destroy_publisher(pub) diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py index bcf9134..9e6903f 100644 --- a/freeflyer/test/pd_ctrl_launch_test.py +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -129,7 +129,7 @@ def state_callback(msg): and "simulator_node" in self.node.get_node_names() ) time.sleep(0.1) - assert node_flag, "pd_ctrl_node or simualtor_node launch failure" + assert node_flag, "pd_ctrl_node or simulator_node launch failure" # wait for node to initialize time.sleep(3.0) diff --git a/testing_files/inject_goal_pose.py b/testing_files/inject_goal_pose.py new file mode 100755 index 0000000..d1036b3 --- /dev/null +++ b/testing_files/inject_goal_pose.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python +# 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. + + +""" +Injects goal position into ROS stack for testing +----------------------------------------------- +""" +from ff_msgs.msg import FreeFlyerStateStamped +from rclpy.node import Node +import rclpy +import math +import sys + +goal_positions = [ + [0.5, 0.5, -math.pi / 2], + [0.5, 0.5, math.pi / 2], + [1.0, 1.0, math.pi / 2], + [2.0, 1.5, math.pi / 2], +] +goal_velos = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + + +class GoalPublisherNode(Node): + def __init__(self, index): + super().__init__("goal_publisher") + self.pub_goal = self.create_publisher(FreeFlyerStateStamped, f"robot/ctrl/state", 10) + self.create_timer(0.5, self.publish_goal_callback) + self.index = index + + def publish_goal_callback(self): + self.publish_goal(goal_positions[self.index], goal_velos[self.index]) + + def publish_goal(self, pos, velo): + goal_pose = FreeFlyerStateStamped() + goal_pose.header.stamp = self.get_clock().now().to_msg() + goal_pose.header.frame_id = "world" + goal_pose.state.pose.x = pos[0] + goal_pose.state.pose.y = pos[1] + goal_pose.state.pose.theta = pos[2] + goal_pose.state.twist.vx = velo[0] + goal_pose.state.twist.vy = velo[1] + goal_pose.state.twist.wz = velo[2] + + self.pub_goal.publish(goal_pose) + + +def main(): + index = int(sys.argv[1]) + rclpy.init() + publisher = GoalPublisherNode(index) + # publisher.publish_goal(goal_positions[index], goal_velos[index]) + rclpy.spin(publisher) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/testing_files/plotjuggler/freeflyerstatescost.xml b/testing_files/plotjuggler/freeflyerstatescost.xml new file mode 100644 index 0000000..24173b9 --- /dev/null +++ b/testing_files/plotjuggler/freeflyerstatescost.xml @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/testing_files/plotjuggler/hardwarefreeflyerstatescost.xml b/testing_files/plotjuggler/hardwarefreeflyerstatescost.xml new file mode 100644 index 0000000..d5d69de --- /dev/null +++ b/testing_files/plotjuggler/hardwarefreeflyerstatescost.xml @@ -0,0 +1,116 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +