[gimbalrotor][control] consider counter torque

jsk-ros-pkg · May 27, 2024 · 6dfe0f4 · 6dfe0f4
1 parent d74f5be
commit 6dfe0f4
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
@@ -114,14 +114,16 @@ 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);
     int last_col = 0;
 
     /* 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;
     }