[Hydrus][controller] workaround activating wrench estimation in hydrus

robots/hydrus/config/quad/default_mode_201907/FlightControl.yaml

@@ -1,6 +1,7 @@
 aerial_robot_control_name: aerial_robot_control/hydrus_tilted_lqi_controller
 
 controller:
+  wrench_estimate_flag: true
   xy:
     p_gain: 2.3
     i_gain: 0.02

robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h

@@ -55,5 +55,6 @@ namespace aerial_robot_control
 
   protected:
     bool checkRobotModel() override;
+    void controlCore() override;
   };
 };

robots/hydrus/src/hydrus_tilted_lqi_controller.cpp

@@ -34,6 +34,26 @@ bool HydrusTiltedLQIController::checkRobotModel()
   return true;
 }
 
+void HydrusTiltedLQIController::controlCore()
+{
+  UnderActuatedTiltedLQIController::controlCore();
+  tf::Matrix3x3 uav_rot = estimator_->getOrientation(Frame::COG, estimate_mode_);
+  tf::Vector3 target_acc_w(pid_controllers_.at(X).result(),
+                           pid_controllers_.at(Y).result(),
+                           pid_controllers_.at(Z).result());
+  tf::Vector3 target_acc_cog = uav_rot.inverse() * target_acc_w;
+  Eigen::VectorXd target_wrench_acc_cog = Eigen::VectorXd::Zero(6);
+  target_wrench_acc_cog.head(3) = Eigen::Vector3d(target_acc_cog.x(), target_acc_cog.y(), target_acc_cog.z());
+
+  double target_ang_acc_x = pid_controllers_.at(ROLL).result();
+  double target_ang_acc_y = pid_controllers_.at(PITCH).result();
+  double target_ang_acc_z = pid_controllers_.at(YAW).result();
+  target_wrench_acc_cog.tail(3) = Eigen::Vector3d(target_ang_acc_x, target_ang_acc_y, target_ang_acc_z);
+
+  setTargetWrenchAccCog(target_wrench_acc_cog);
+}
+
+
 /* plugin registration */
 #include <pluginlib/class_list_macros.h>
 PLUGINLIB_EXPORT_CLASS(aerial_robot_control::HydrusTiltedLQIController, aerial_robot_control::ControlBase);