From f85dcb76f916872da60cb98ff5af432377e1d134 Mon Sep 17 00:00:00 2001 From: Rjkzavr Date: Tue, 17 Dec 2024 15:30:27 +0300 Subject: [PATCH] fix zero force and torque bug --- src/thrusters_controller_node.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/thrusters_controller_node.cpp b/src/thrusters_controller_node.cpp index 72d16a0..61859db 100755 --- a/src/thrusters_controller_node.cpp +++ b/src/thrusters_controller_node.cpp @@ -289,10 +289,12 @@ private: t[1] = t[1] * d[4]; t[2] = t[2] * d[5]; - // serach min force and torque - force_norm = force_norm < f.norm() ? force_norm : f.norm(); - torque_norm = torque_norm < t.norm() ? torque_norm : t.norm(); + // serach min non zero force and torque + force_norm = f.norm() < force_norm && f.norm() != 0 ? f.norm(): force_norm; + torque_norm = t.norm() < torque_norm && t.norm() != 0 ? t.norm(): torque_norm; } + // force_norm = 1; + // torque_norm = 0; RCLCPP_INFO_STREAM(this->get_logger(), "force norm: " << force_norm << " torque norm: " << torque_norm); for (size_t i = 0; i < thrusters_vec_.size(); ++i) -- GitLab