From 110cfb96d228c64e3860f210a17296dd16847bda Mon Sep 17 00:00:00 2001 From: Andrew Wang Date: Fri, 16 Feb 2024 15:31:40 -0800 Subject: [PATCH] Reformat code --- ff_control/ff_control/wrench_ctrl.py | 2 +- ff_sim/ff_sim/controller_metrics.py | 20 ++++++++++++-------- ff_sim/ff_sim/simulator_node.py | 4 +++- ff_sim/launch/single.launch.py | 2 +- ff_sim/setup.py | 6 +++--- 5 files changed, 20 insertions(+), 14 deletions(-) diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index f58b3ee..68f0754 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -96,7 +96,7 @@ def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: def clip_wrench(self, wrench: Wrench2D) -> Wrench2D: wrench_clipped = Wrench2D() - force = np.sqrt(wrench.fx**2 + wrench.fy**2) + force = np.sqrt(wrench.fx ** 2 + wrench.fy ** 2) force_scale = max(force / self.p.actuators["F_body_max"], 1.0) torque_scale = max(abs(wrench.tz) / self.p.actuators["M_body_max"], 1.0) diff --git a/ff_sim/ff_sim/controller_metrics.py b/ff_sim/ff_sim/controller_metrics.py index eaf0e6a..ffbd185 100755 --- a/ff_sim/ff_sim/controller_metrics.py +++ b/ff_sim/ff_sim/controller_metrics.py @@ -48,8 +48,9 @@ from ff_params import RobotParams + class ControllerMetricsPublisher(Node): - """ Class to listen to free flyer commands and calculate metrics """ + """Class to listen to free flyer commands and calculate metrics""" def __init__(self): super().__init__("ff_ctrl_metrics") @@ -74,15 +75,15 @@ def __init__(self): ) def process_new_wheel_cmd(self, msg: WheelVelCommand) -> None: - """ Doesn't process wheel command """ + """Doesn't process wheel command""" pass def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None: - """ Process binary thrusters """ + """Process binary thrusters""" now = self.get_clock().now().to_msg() dtsec = now.sec - self.curr_time.sec dtnsec = now.nanosec - self.curr_time.nanosec - dt = dtsec + dtnsec/1e9 + dt = dtsec + dtnsec / 1e9 thrusters = np.array(msg.switches, dtype=float) self.running_total_gas += self.prev_thruster_sum * dt @@ -93,7 +94,9 @@ def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None: self.time_hist.append(dt) for i in range(8): self.thrust_hist[i].append(thrusters[i]) - self.thrust_duty_cycles[i] = np.dot(self.thrust_hist[i],self.time_hist) / np.sum(self.time_hist) + self.thrust_duty_cycles[i] = np.dot(self.thrust_hist[i], self.time_hist) / np.sum( + self.time_hist + ) if self.steps >= self.duty_cycle_window: self.rolled_up = True else: @@ -102,8 +105,10 @@ def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None: for i in range(8): self.thrust_hist[i].pop(0) self.thrust_hist[i].append(thrusters[i]) - self.thrust_duty_cycles[i] = np.dot(self.thrust_hist[i],self.time_hist) / np.sum(self.time_hist) - + self.thrust_duty_cycles[i] = np.dot(self.thrust_hist[i], self.time_hist) / np.sum( + self.time_hist + ) + metrics = ControllerMetrics() metrics.header.stamp = now metrics.total_gas_time = self.running_total_gas @@ -116,7 +121,6 @@ def process_new_pwm_thrust_cmd(self): pass - def main(): rclpy.init() ff_metrics = ControllerMetricsPublisher() diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index d8d4af7..3c200fb 100755 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -388,7 +388,9 @@ def f_dynamics_continuous_time(self, x, u): f[0:2] = v f[2] = thetadot thetaddot = (M - F[1] * p0[0] + F[0] * p0[1]) / J - f[3:5] = np.matmul(R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0))) + f[3:5] = np.matmul( + R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot ** 2 * p0)) + ) f[5] = thetaddot # add constant force due to table tilt diff --git a/ff_sim/launch/single.launch.py b/ff_sim/launch/single.launch.py index b2adfd4..14b0e08 100644 --- a/ff_sim/launch/single.launch.py +++ b/ff_sim/launch/single.launch.py @@ -56,6 +56,6 @@ def generate_launch_description(): executable="safety_filter", name="safety_filter", namespace=robot_name, - ) + ), ] ) diff --git a/ff_sim/setup.py b/ff_sim/setup.py index 1f89919..dd5226d 100644 --- a/ff_sim/setup.py +++ b/ff_sim/setup.py @@ -21,8 +21,8 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "simulator_node = ff_sim.simulator_node:main", - "controller_metrics = ff_sim.controller_metrics:main" - ], + "simulator_node = ff_sim.simulator_node:main", + "controller_metrics = ff_sim.controller_metrics:main", + ], }, )