From 1b1fd740c5d26ad381f2d94ef0e69bbec7b1861e Mon Sep 17 00:00:00 2001 From: AndrewHWang1605 <70298553+AndrewHWang1605@users.noreply.github.com> Date: Mon, 29 Apr 2024 19:08:06 -0700 Subject: [PATCH 1/3] Update ff_sim/ff_sim/controller_metrics.py Co-authored-by: Alvin Sun --- ff_sim/ff_sim/controller_metrics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ff_sim/ff_sim/controller_metrics.py b/ff_sim/ff_sim/controller_metrics.py index ca58307..e725d3c 100755 --- a/ff_sim/ff_sim/controller_metrics.py +++ b/ff_sim/ff_sim/controller_metrics.py @@ -62,7 +62,7 @@ class ControllerMetricsPublisher(Node): def __init__(self): super().__init__("ff_ctrl_metrics") - self.curr_time = self.get_clock().now().to_msg() + self.curr_time = self.get_clock().now() self.steps = 0 self.running_total_gas = 0 self.prev_thruster_sum = 0 From 3c20d70c767eb2f6d72b8a6600bef9c8130b652d Mon Sep 17 00:00:00 2001 From: AndrewHWang1605 <70298553+AndrewHWang1605@users.noreply.github.com> Date: Mon, 29 Apr 2024 19:08:28 -0700 Subject: [PATCH 2/3] Update ff_sim/ff_sim/controller_metrics.py Change incorrect usage of self.get_clock().now() function Co-authored-by: Alvin Sun --- ff_sim/ff_sim/controller_metrics.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ff_sim/ff_sim/controller_metrics.py b/ff_sim/ff_sim/controller_metrics.py index e725d3c..83a8550 100755 --- a/ff_sim/ff_sim/controller_metrics.py +++ b/ff_sim/ff_sim/controller_metrics.py @@ -88,10 +88,8 @@ def process_new_wheel_cmd(self, msg: WheelVelCommand) -> None: def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None: """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 + 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) From 8fc30aafe060b564655b2879b55a41532c80382f Mon Sep 17 00:00:00 2001 From: AndrewHWang1605 <70298553+AndrewHWang1605@users.noreply.github.com> Date: Mon, 29 Apr 2024 19:09:26 -0700 Subject: [PATCH 3/3] Update ff_control/scripts/opt_ctrl_py_node Add comment for helper function Co-authored-by: Alvin Sun --- ff_control/scripts/opt_ctrl_py_node | 1 + 1 file changed, 1 insertion(+) diff --git a/ff_control/scripts/opt_ctrl_py_node b/ff_control/scripts/opt_ctrl_py_node index c8868f9..8fdd174 100755 --- a/ff_control/scripts/opt_ctrl_py_node +++ b/ff_control/scripts/opt_ctrl_py_node @@ -322,6 +322,7 @@ class ThrusterOptControlNode(TrinaryThrusterController): 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