From f113845e5651285d1c87905d103b376bf1295448 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 21 Sep 2024 23:31:40 +0900 Subject: [PATCH] [Beetle][Control] fix bug in compensation wrench calculation --- .../beetle/control/beetle_controller.h | 22 ++++---- .../beetle/src/control/beetle_controller.cpp | 54 +++++++++++-------- 2 files changed, 43 insertions(+), 33 deletions(-) diff --git a/robots/beetle/include/beetle/control/beetle_controller.h b/robots/beetle/include/beetle/control/beetle_controller.h index a101fe994..56800a550 100644 --- a/robots/beetle/include/beetle/control/beetle_controller.h +++ b/robots/beetle/include/beetle/control/beetle_controller.h @@ -37,11 +37,6 @@ namespace aerial_robot_control boost::shared_ptr beetle_robot_model_; boost::shared_ptr beetle_navigator_; - ros::Publisher external_wrench_compensation_pub_; - ros::Publisher whole_external_wrench_pub_; - ros::Publisher internal_wrench_pub_; - ros::Publisher wrench_comp_pid_pub_; - ros::Publisher des_inter_wrench_pub_; map ff_inter_wrench_subs_; aerial_robot_msgs::PoseControlPid wrench_pid_msg_; @@ -55,10 +50,7 @@ namespace aerial_robot_control int pre_module_state_; - std::map est_wrench_list_; - std::map inter_wrench_list_; - std::map wrench_comp_list_; - std::map ff_inter_wrench_list_; + bool des_wrench_pub_flag_; double comp_term_update_freq_; double prev_comp_update_time_; @@ -72,11 +64,21 @@ namespace aerial_robot_control double I_comp_Ty_; double I_comp_Tz_; - void calcInteractionWrench(); void estExternalWrenchCallback(const beetle::TaggedWrench & msg); protected: + std::map est_wrench_list_; + std::map inter_wrench_list_; + std::map wrench_comp_list_; + std::map ff_inter_wrench_list_; + + virtual void calcInteractionWrench(); ros::Publisher tagged_external_wrench_pub_; + ros::Publisher external_wrench_compensation_pub_; + ros::Publisher whole_external_wrench_pub_; + ros::Publisher internal_wrench_pub_; + ros::Publisher wrench_comp_pid_pub_; + ros::Publisher des_inter_wrench_pub_; void controlCore() override; virtual void ffInterWrenchCallback(const beetle::TaggedWrench & msg); diff --git a/robots/beetle/src/control/beetle_controller.cpp b/robots/beetle/src/control/beetle_controller.cpp index 4fafc04b2..337542ef8 100644 --- a/robots/beetle/src/control/beetle_controller.cpp +++ b/robots/beetle/src/control/beetle_controller.cpp @@ -7,7 +7,8 @@ namespace aerial_robot_control BeetleController::BeetleController(): GimbalrotorController(), pd_wrench_comp_mode_(false), - pre_module_state_(SEPARATED) + pre_module_state_(SEPARATED), + des_wrench_pub_flag_(false) { } @@ -124,7 +125,11 @@ namespace aerial_robot_control pid_controllers_.at(index).setDGain(wrench_comp_d_gain_ / std::pow(2, module_num -2) ); pid_controllers_.at(index).setIGain(wrench_comp_i_gain_ / std::pow(2, module_num -2) ); } - Eigen::VectorXd wrench_comp_term = wrench_comp_list_[my_id]; + Eigen::VectorXd wrench_comp_term_cog = wrench_comp_list_[my_id]; // regarding cog + Eigen::Matrix3d cog_rot; + tf::matrixTFToEigen(estimator_->getOrientation(Frame::COG, estimate_mode_), cog_rot); + Eigen::VectorXd wrench_comp_term = wrench_comp_term_cog; + wrench_comp_term.head(3) = cog_rot * wrench_comp_term.head(3); // regarding world /* current version: I term reconfig mehod */ Eigen::VectorXd I_reconfig_acc_cog_term = Eigen::VectorXd::Zero(6); @@ -213,29 +218,32 @@ namespace aerial_robot_control wrench_comp_pid_pub_.publish(wrench_pid_msg_); /*publish desire internal wrench*/ - beetle::TaggedWrenches all_tagged_des_wrenche_msg; - std::vector assembled_ids = beetle_navigator_->getAssemblyIds(); - all_tagged_des_wrenche_msg.tagged_wrenches.resize(assembled_ids.size()); - int cnt =0; - for(const auto id: assembled_ids) + if(des_wrench_pub_flag_) { - beetle::TaggedWrench tagged_des_wrench_msg; - geometry_msgs::WrenchStamped des_wrench_msg; - Eigen::VectorXd des_wrench = ff_inter_wrench_list_[id]; - des_wrench_msg.header.stamp.fromSec(estimator_->getImuLatestTimeStamp()); - des_wrench_msg.wrench.force.x = des_wrench(0); - des_wrench_msg.wrench.force.y = des_wrench(1); - des_wrench_msg.wrench.force.z = des_wrench(2); - des_wrench_msg.wrench.torque.x = des_wrench(3); - des_wrench_msg.wrench.torque.y = des_wrench(4); - des_wrench_msg.wrench.torque.z = des_wrench(5); - - tagged_des_wrench_msg.index = id; - tagged_des_wrench_msg.wrench = des_wrench_msg; - all_tagged_des_wrenche_msg.tagged_wrenches[cnt] = tagged_des_wrench_msg; - cnt ++; + beetle::TaggedWrenches all_tagged_des_wrenche_msg; + std::vector assembled_ids = beetle_navigator_->getAssemblyIds(); + all_tagged_des_wrenche_msg.tagged_wrenches.resize(assembled_ids.size()); + int cnt =0; + for(const auto id: assembled_ids) + { + beetle::TaggedWrench tagged_des_wrench_msg; + geometry_msgs::WrenchStamped des_wrench_msg; + Eigen::VectorXd des_wrench = ff_inter_wrench_list_[id]; + des_wrench_msg.header.stamp.fromSec(estimator_->getImuLatestTimeStamp()); + des_wrench_msg.wrench.force.x = des_wrench(0); + des_wrench_msg.wrench.force.y = des_wrench(1); + des_wrench_msg.wrench.force.z = des_wrench(2); + des_wrench_msg.wrench.torque.x = des_wrench(3); + des_wrench_msg.wrench.torque.y = des_wrench(4); + des_wrench_msg.wrench.torque.z = des_wrench(5); + + tagged_des_wrench_msg.index = id; + tagged_des_wrench_msg.wrench = des_wrench_msg; + all_tagged_des_wrenche_msg.tagged_wrenches[cnt] = tagged_des_wrench_msg; + cnt ++; + } + des_inter_wrench_pub_.publish(all_tagged_des_wrenche_msg); } - des_inter_wrench_pub_.publish(all_tagged_des_wrenche_msg); }else{ pid_controllers_.at(FX).reset(); pid_controllers_.at(FY).reset();