diff --git a/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h b/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h
index 14fcde50f..b34fba7af 100644
--- a/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h
+++ b/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h
@@ -15,7 +15,6 @@ class ThrusterManager : public rclcpp::Node
 
   private:
     rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr wrench_subscription_;
-    Eigen::VectorXd reference_wrench_;  // TODO: change to Vector6d
     Eigen::MatrixXd tam_;
     int const dof_ = 6;
     int const thruster_count_ = 8;
@@ -25,6 +24,7 @@ class ThrusterManager : public rclcpp::Node
 
     rclcpp::TimerBase::SharedPtr timer_;
     rclcpp::Publisher<subjugator_msgs::msg::ThrusterEfforts>::SharedPtr thrust_publisher_;
+    subjugator_msgs::msg::ThrusterEfforts efforts_;
 
     void wrench_callback(geometry_msgs::msg::Wrench::SharedPtr msg);
     void timer_callback();
diff --git a/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp b/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp
index b50188488..a26492bef 100644
--- a/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp
+++ b/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp
@@ -15,7 +15,6 @@
 // Construct node class
 ThrusterManager::ThrusterManager() : Node("thruster_manager")
 {
-    reference_wrench_ = Eigen::VectorXd::Zero(6);
     this->declare_parameter("thruster_cap", 0.0);
     thruster_cap_ = this->get_parameter("thruster_cap").as_double();
 
@@ -41,6 +40,7 @@ ThrusterManager::ThrusterManager() : Node("thruster_manager")
                                          "with "
                                          "ros2 launch, and that config values are set.");
     }
+    tam_ = tam_.completeOrthogonalDecomposition().pseudoInverse();
 
     wrench_subscription_ = this->create_subscription<geometry_msgs::msg::Wrench>(
         "cmd_wrench", 1,
@@ -57,13 +57,10 @@ void ThrusterManager::wrench_callback(geometry_msgs::msg::Wrench::SharedPtr msg)
     // RCLCPP_INFO(this->get_logger(), "Heard wrench: [%.2f, %.2f, %.2f], [%.2f, %.2f, %.2f]", msg->force.x,
     // msg->force.y,
     //             msg->force.z, msg->torque.x, msg->torque.y, msg->torque.z);
-    this->reference_wrench_ << msg->force.x, msg->force.y, msg->force.z, msg->torque.x, msg->torque.y, msg->torque.z;
-}
+    Eigen::VectorXd wrench;
+    wrench << msg->force.x, msg->force.y, msg->force.z, msg->torque.x, msg->torque.y, msg->torque.z;
 
-// Compute and publish thruster efforts
-void ThrusterManager::timer_callback()
-{
-    Eigen::VectorXd thrust_values(tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_wrench_);
+    Eigen::VectorXd thrust_values(tam_ * wrench);
 
     // check that the allocated thrust is not over the thruster cap (typically 1.0), and if it is, rescale all thrusters
     double biggest_thrust = 0;
@@ -89,17 +86,20 @@ void ThrusterManager::timer_callback()
         thrust_values = thrust_values * (thruster_cap_ / biggest_thrust);
     }
 
-    auto msg = subjugator_msgs::msg::ThrusterEfforts();
-    msg.thrust_frh = thrust_values[0];
-    msg.thrust_flh = thrust_values[1];
-    msg.thrust_brh = thrust_values[2];
-    msg.thrust_blh = thrust_values[3];
-    msg.thrust_frv = thrust_values[4];
-    msg.thrust_flv = thrust_values[5];
-    msg.thrust_brv = thrust_values[6];
-    msg.thrust_blv = thrust_values[7];
+    efforts_.thrust_frh = thrust_values[0];
+    efforts_.thrust_flh = thrust_values[1];
+    efforts_.thrust_brh = thrust_values[2];
+    efforts_.thrust_blh = thrust_values[3];
+    efforts_.thrust_frv = thrust_values[4];
+    efforts_.thrust_flv = thrust_values[5];
+    efforts_.thrust_brv = thrust_values[6];
+    efforts_.thrust_blv = thrust_values[7];
+}
 
-    this->thrust_publisher_->publish(msg);
+// Compute and publish thruster efforts
+void ThrusterManager::timer_callback()
+{
+    this->thrust_publisher_->publish(efforts_);
 }
 
 int main(int argc, char **argv)