fix eigen cross issue

uri-ocean-robotics · Jan 30, 2024 · 2d56c73 · 2d56c73
1 parent 769db8e
commit 2d56c73
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/mvp_control/mvp_control_ros.cpp b/src/mvp_control/mvp_control_ros.cpp
@@ -461,8 +461,8 @@ void MvpControlROS::f_generate_control_allocation_from_tf() {
             return;
         }
 
-        auto torque_rpy = ang_vel_tranform * trans_xyz.cross(Eigen::Vector3d{Fx, Fy, Fz});
         auto torque_pqr = trans_xyz.cross(Eigen::Vector3d{Fx, Fy, Fz});
+        auto torque_rpy = ang_vel_tranform * torque_pqr;
 
         //
         contribution_vector(DOF::SURGE) = Fx;