diff --git a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
index 4807d6a2e..72fa1e31d 100644
--- a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
+++ b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
@@ -114,6 +114,8 @@ namespace aerial_robot_control
     double t = ros::Time::now().toSec();
 
     std::vector<Eigen::Vector3d> rotors_origin_from_cog = gimbalrotor_robot_model_->getRotorsOriginFromCog<Eigen::Vector3d>();
+    std::map<int, int> rotor_direction = robot_model_->getRotorDirection();
+    double m_f_rate = robot_model_->getMFRate();
 
     Eigen::MatrixXd wrench_map = Eigen::MatrixXd::Zero(6, 3);
     wrench_map.block(0, 0, 3, 3) =  Eigen::MatrixXd::Identity(3, 3);
@@ -121,7 +123,7 @@ namespace aerial_robot_control
 
     /* calculate normal allocation */
     for(int i = 0; i < motor_num_; i++){
-      wrench_map.block(3, 0, 3, 3) = aerial_robot_model::skew(rotors_origin_from_cog.at(i));
+      wrench_map.block(3, 0, 3, 3) = aerial_robot_model::skew(rotors_origin_from_cog.at(i)) + rotor_direction.at(i + 1) * m_f_rate * Eigen::MatrixXd::Identity(3, 3);
       full_q_mat.middleCols(last_col, 3) = wrench_map;
       last_col += 3;
     }