From 3c509ee09e4b1c0ec0538dcdae56e30c4a16c7d4 Mon Sep 17 00:00:00 2001
From: Shotaro Itahara <itahara-shotaro@ynl.t.u-tokyo.ac.jp>
Date: Thu, 2 Nov 2023 15:57:59 +0900
Subject: [PATCH] [Control] enabled switching of control methods form config

---
 .../fully_actuated_nobend_controller.h        |   2 +
 .../fully_actuated_nobend_controller.cpp      | 112 ++++++++++++------
 2 files changed, 81 insertions(+), 33 deletions(-)

diff --git a/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_nobend_controller.h b/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_nobend_controller.h
index 1a87bcd7b..8e7ed2cf2 100644
--- a/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_nobend_controller.h
+++ b/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_nobend_controller.h
@@ -95,6 +95,8 @@ namespace aerial_robot_control
     double wrench_allocation_matrix_pub_interval_;
     void rosParamInit();
 
+    int control_method;
+
 
   };
 } //namespace aerial_robot_control
diff --git a/aerial_robot_control/src/control/fully_actuated_nobend_controller.cpp b/aerial_robot_control/src/control/fully_actuated_nobend_controller.cpp
index 95a153da8..e4dae591e 100644
--- a/aerial_robot_control/src/control/fully_actuated_nobend_controller.cpp
+++ b/aerial_robot_control/src/control/fully_actuated_nobend_controller.cpp
@@ -115,48 +115,93 @@ namespace aerial_robot_control
     //Step0.5: obtain the normal vectors of thrusts
     std::vector<Eigen::Vector3d> rotors_normal = robot_model_->getRotorsNormalFromCog<Eigen::Vector3d>();
 
-    // Step1: create new Q-matrix
-  
-    Eigen::MatrixXd Q_new(7,8);
-    Q_new<<q_mat,q_mat.row(2);
+    Eigen::VectorXd target_thrust_x_term;
+    Eigen::VectorXd target_thrust_y_term;
+    Eigen::VectorXd target_thrust_z_term;
+    Eigen::VectorXd target_thrust_soft_term;
 
+    if(control_method == 0){
+      // Step1: create new Q-matrix
     
-    Eigen::MatrixXd Q_new_test(7,8);
-    Eigen::VectorXd Q_row6 = Eigen::VectorXd::Zero(8), Q_row7 = Eigen::VectorXd::Zero(8);
-    
-    for(int i=0;i<4;i++){
-      Q_row6(i) = (q_mat.row(4))(i);
-      Q_row7(i+4) = (q_mat.row(4))(i+4);
+      Eigen::MatrixXd Q_new(7,8);
+      Q_new<<q_mat,q_mat.row(2);
+
+      
+      Eigen::MatrixXd Q_new_test(7,8);
+      Eigen::VectorXd Q_row6 = Eigen::VectorXd::Zero(8), Q_row7 = Eigen::VectorXd::Zero(8);
+      
+      for(int i=0;i<4;i++){
+        Q_row6(i) = (q_mat.row(4))(i);
+        Q_row7(i+4) = (q_mat.row(4))(i+4);
+      }
+      Q_new_test<<q_mat.row(0), q_mat.row(1), q_mat.row(2), q_mat.row(3) ,q_mat.row(5), Q_row6.transpose(),Q_row7.transpose();
+
+      // Step2: calculate SR-inverse of the new Q
+
+      double sr_inverse_sigma = 0.1;
+      Eigen::MatrixXd q = Q_new_test;
+      Eigen::MatrixXd q_q_t = q * q.transpose();
+      Eigen::MatrixXd sr_inv = q.transpose() * (q_q_t + sr_inverse_sigma* Eigen::MatrixXd::Identity(q_q_t.cols(), q_q_t.rows())).inverse();
+
+      q_mat_inv_=sr_inv;
+      
+      //step3: calculate thrust accounting for softness & linear acc
+
+      tf::Vector3 gravity_world(0,
+                                0,
+                                -9.8);
+      //gravity_world(3) = -9.8;
+      tf::Vector3 gravity_cog = uav_rot.inverse() * gravity_world;
+      Eigen::Vector3d gravity_CoG(gravity_cog.x(), gravity_cog.y(), gravity_cog.z() );
+
+      Eigen::VectorXd thrust_constant = Eigen::VectorXd::Zero(8);
+      thrust_constant << 0,0,0,0,0,(mass/2)*(pCoG_1.cross(gravity_CoG)).y(),(mass/2)*(pCoG_2.cross(gravity_CoG)).y();
+      
+      target_thrust_x_term = q_mat_inv_.col(X) * target_acc_cog.x();
+      target_thrust_y_term = q_mat_inv_.col(Y) * target_acc_cog.y();
+      target_thrust_z_term = q_mat_inv_.col(Z) * target_acc_cog.z();
+      target_thrust_soft_term = -1*(q_mat_inv_*thrust_constant);
     }
-    Q_new_test<<q_mat.row(0), q_mat.row(1), q_mat.row(2), q_mat.row(3) ,q_mat.row(5), Q_row6.transpose(),Q_row7.transpose();
 
-    // Step2: calculate SR-inverse of the new Q
+    else{
+      Eigen::MatrixXd WrenchMatrixOnCoG = robot_model_->calcWrenchMatrixOnCoG();
+      Eigen::MatrixXd q_bottom_q1(3,8), q_bottom_q2(3,8), q1_bottom(3,8), q2_bottom(3,8);
+
+      q_bottom_q1 = WrenchMatrixOnCoG.bottomRows(3);
+      q_bottom_q2 = WrenchMatrixOnCoG.bottomRows(3);
+      
+      Eigen::VectorXd Q_row6 = Eigen::VectorXd::Zero(8), Q_row7 = Eigen::VectorXd::Zero(8);
+      
+      for(int i=0;i<4;i++){
+        Q_row6(i) = ( (rotors_origin[i]-pCoG_1).cross(rotors_normal[i]) ).y();
+        Q_row7(i+4) = ( (rotors_origin[i+4]-pCoG_2).cross(rotors_normal[i+4]) ).y();
+      }
 
-    double sr_inverse_sigma = 0.1;
-    Eigen::MatrixXd q = Q_new_test;
-    Eigen::MatrixXd q_q_t = q * q.transpose();
-    Eigen::MatrixXd sr_inv = q.transpose() * (q_q_t + sr_inverse_sigma* Eigen::MatrixXd::Identity(q_q_t.cols(), q_q_t.rows())).inverse();
+      q_bottom_q1.row(1)=Q_row6;
+      q_bottom_q2.row(1)=Q_row7;
 
-    q_mat_inv_=sr_inv;
-    
-    //step3: calculate thrust accounting for softness & linear acc
+      q1_bottom = inertia_inv * q_bottom_q1;
+      q2_bottom = inertia_inv * q_bottom_q2;
 
-    tf::Vector3 gravity_world(0,
-                              0,
-                              -9.8);
-    //gravity_world(3) = -9.8;
-    tf::Vector3 gravity_cog = uav_rot.inverse() * gravity_world;
-    Eigen::Vector3d gravity_CoG(gravity_cog.x(), gravity_cog.y(), gravity_cog.z() );
 
-    Eigen::VectorXd thrust_constant = Eigen::VectorXd::Zero(8);
-    thrust_constant << 0,0,0,0,0,(mass/2)*(pCoG_1.cross(gravity_CoG)).y(),(mass/2)*(pCoG_2.cross(gravity_CoG)).y();
+      //ROS_INFO_STREAM(rotors_normal[0]);
+      Eigen::MatrixXd Q_new_test(7,8);
+      Q_new_test<<q_mat_.row(0), q_mat_.row(1), q_mat_.row(2), q_mat_.row(3) ,q_mat_.row(5), q1_bottom.row(1), q2_bottom.row(1);
     
-    Eigen::VectorXd target_thrust_x_term = q_mat_inv_.col(X) * target_acc_cog.x();
-    Eigen::VectorXd target_thrust_y_term = q_mat_inv_.col(Y) * target_acc_cog.y();
-    Eigen::VectorXd target_thrust_z_term = q_mat_inv_.col(Z) * target_acc_cog.z();
-    Eigen::VectorXd target_thrust_soft_term = -1*(q_mat_inv_*thrust_constant);
-    // ROS_INFO_STREAM(target_thrust_soft_term);
-
+      // Step2: calculate SR-inverse of the new Q
+      double sr_inverse_sigma = 0.1;
+      Eigen::MatrixXd q = Q_new_test;
+      Eigen::MatrixXd q_q_t = q * q.transpose();
+      Eigen::MatrixXd sr_inv = q.transpose() * (q_q_t + sr_inverse_sigma* Eigen::MatrixXd::Identity(q_q_t.cols(), q_q_t.rows())).inverse();
+
+      q_mat_inv_=sr_inv;
+
+      //step3: calculate thrust accounting for softness & linear acc
+      target_thrust_x_term = q_mat_inv_.col(X) * target_acc_cog.x();
+      target_thrust_y_term = q_mat_inv_.col(Y) * target_acc_cog.y();
+      target_thrust_z_term = q_mat_inv_.col(Z) * target_acc_cog.z();
+      target_thrust_soft_term = Eigen::VectorXd::Zero(8).transpose(); 
+    }
     // constraint x and y
     int index;
     double max_term = target_thrust_x_term.cwiseAbs().maxCoeff(&index);
@@ -279,6 +324,7 @@ namespace aerial_robot_control
     ros::NodeHandle control_nh(nh_, "controller");
     getParam<double>(control_nh, "torque_allocation_matrix_inv_pub_interval", torque_allocation_matrix_inv_pub_interval_, 0.05);
     getParam<double>(control_nh, "wrench_allocation_matrix_pub_interval", wrench_allocation_matrix_pub_interval_, 0.1);
+    getParam<int>(control_nh, "control_method", control_method, 0);
   }
 
   void FullyActuatedNobendController::setAttitudeGains()