From 97a27f3698c7763ca3ba5def4fcd414054316941 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 18 Nov 2023 19:23:25 +0900 Subject: [PATCH] [Beetle] add pd and wrench comp mode for yaw --- robots/beetle/config/BeetleControl.yaml | 2 ++ robots/beetle/config/BeetleControl_sim.yaml | 3 ++- .../beetle/control/beetle_controller.h | 5 +++- .../beetle/src/control/beetle_controller.cpp | 26 ++++++++++++------- .../src/control/gimbalrotor_controller.cpp | 21 +++++++-------- 5 files changed, 34 insertions(+), 23 deletions(-) diff --git a/robots/beetle/config/BeetleControl.yaml b/robots/beetle/config/BeetleControl.yaml index da24a9306..627395a94 100644 --- a/robots/beetle/config/BeetleControl.yaml +++ b/robots/beetle/config/BeetleControl.yaml @@ -8,6 +8,8 @@ controller: momentum_observer_force_weight: 3 momentum_observer_torque_weight: 2.5 + pd_wrench_comp_mode: true + external_force_upper_limit: 0.0 external_force_lower_limit: 0.0 external_torque_lower_limit: 0.0 diff --git a/robots/beetle/config/BeetleControl_sim.yaml b/robots/beetle/config/BeetleControl_sim.yaml index 75a00e09f..8a3f21412 100644 --- a/robots/beetle/config/BeetleControl_sim.yaml +++ b/robots/beetle/config/BeetleControl_sim.yaml @@ -8,11 +8,12 @@ controller: momentum_observer_force_weight: 3 momentum_observer_torque_weight: 2.5 + pd_wrench_comp_mode: true + external_force_upper_limit: 0.0 external_force_lower_limit: 0.0 external_torque_lower_limit: 0.0 external_torque_lower_limit: 0.0 - cutoff_freq: 1.0 sample_freq: 40.0 diff --git a/robots/beetle/include/beetle/control/beetle_controller.h b/robots/beetle/include/beetle/control/beetle_controller.h index 56cbd54a2..b92865637 100644 --- a/robots/beetle/include/beetle/control/beetle_controller.h +++ b/robots/beetle/include/beetle/control/beetle_controller.h @@ -44,6 +44,7 @@ namespace aerial_robot_control Eigen::VectorXd filterd_est_external_wrench_; /* external wrench compensation */ + bool pd_wrench_comp_mode_; Eigen::VectorXd external_wrench_upper_limit_; Eigen::VectorXd external_wrench_lower_limit_; @@ -51,7 +52,9 @@ namespace aerial_robot_control double ErrI_X_; double ErrI_Y_; double ErrI_Z_; - double ErrI_Yaw_; + double ErrI_ROLL_; + double ErrI_PITCH_; + double ErrI_YAW_; void controlCore() override; void externalWrenchEstimate(); diff --git a/robots/beetle/src/control/beetle_controller.cpp b/robots/beetle/src/control/beetle_controller.cpp index a559887a6..cc0a1e06f 100644 --- a/robots/beetle/src/control/beetle_controller.cpp +++ b/robots/beetle/src/control/beetle_controller.cpp @@ -7,6 +7,7 @@ namespace aerial_robot_control BeetleController::BeetleController(): GimbalrotorController(), lpf_init_flag_(false), + pd_wrench_comp_mode_(false), pre_module_state_(SEPARATED) { } @@ -24,6 +25,7 @@ namespace aerial_robot_control external_wrench_lower_limit_ = Eigen::VectorXd::Zero(6); external_wrench_upper_limit_ = Eigen::VectorXd::Zero(6); rosParamInit(); + if(pd_wrench_comp_mode_) ROS_ERROR("PD & Wrench comp mode"); lpf_est_external_wrench_ = IirFilter(sample_freq_, cutoff_freq_, 6); estimate_external_wrench_pub_ = nh_.advertise("estimated_external_wrench", 1); external_wrench_compensation_pub_ = nh_.advertise("external_wrench_compensation", 1); @@ -63,9 +65,14 @@ namespace aerial_robot_control Eigen::Matrix3d inertia_inv = (beetle_robot_model_->getInertia()).inverse(); int module_state = beetle_robot_model_-> getModuleState(); - if(module_state == FOLLOWER && navigator_->getNaviState() == aerial_robot_navigation::HOVER_STATE){ + if(module_state == FOLLOWER && navigator_->getNaviState() == aerial_robot_navigation::HOVER_STATE && pd_wrench_comp_mode_){ feedforward_wrench_acc_cog_term_.head(3) = mass_inv * wrench_compensation_term.head(3); feedforward_wrench_acc_cog_term_.tail(3) = -inertia_inv * wrench_compensation_term.tail(3); + + /*regarding to torque, only use yaw term */ + feedforward_wrench_acc_cog_term_(3) = 0; + feedforward_wrench_acc_cog_term_(4) = 0; + geometry_msgs::WrenchStamped wrench_msg; wrench_msg.header.stamp.fromSec(estimator_->getImuLatestTimeStamp()); wrench_msg.wrench.force.x = feedforward_wrench_acc_cog_term_(0); @@ -74,12 +81,6 @@ namespace aerial_robot_control wrench_msg.wrench.torque.x = feedforward_wrench_acc_cog_term_(3); wrench_msg.wrench.torque.y = feedforward_wrench_acc_cog_term_(4); wrench_msg.wrench.torque.z = feedforward_wrench_acc_cog_term_(5); - // wrench_msg.wrench.force.x = filterd_est_external_wrench_(0); - // wrench_msg.wrench.force.y = filterd_est_external_wrench_(1); - // wrench_msg.wrench.force.z = filterd_est_external_wrench_(2); - // wrench_msg.wrench.torque.x = filterd_est_external_wrench_(3); - // wrench_msg.wrench.torque.y = filterd_est_external_wrench_(4); - // wrench_msg.wrench.torque.z = filterd_est_external_wrench_(5); external_wrench_compensation_pub_.publish(wrench_msg); GimbalrotorController::controlCore(); @@ -89,14 +90,18 @@ namespace aerial_robot_control ErrI_X_ = pid_controllers_.at(X).getErrI(); ErrI_Y_ = pid_controllers_.at(Y).getErrI(); ErrI_Z_ = pid_controllers_.at(Z).getErrI(); - ErrI_Yaw_ = pid_controllers_.at(YAW).getErrI(); + // ErrI_ROLL_ = pid_controllers_.at(ROLL).getErrI(); + // ErrI_PITCH_ = pid_controllers_.at(PITCH).getErrI(); + ErrI_YAW_ = pid_controllers_.at(YAW).getErrI(); pre_module_state_ = FOLLOWER; } //fix i_term - pid_controllers_.at(X).setErrI(ErrI_X_); + pid_controllers_.at(X).setErrI(ErrI_X_); pid_controllers_.at(Y).setErrI(ErrI_Y_); pid_controllers_.at(Z).setErrI(ErrI_Z_); - pid_controllers_.at(YAW).setErrI(ErrI_Yaw_); + // pid_controllers_.at(ROLL).setErrI(ErrI_ROLL_); + // pid_controllers_.at(PITCH).setErrI(ErrI_PITCH_); + pid_controllers_.at(YAW).setErrI(ErrI_YAW_); }else{ feedforward_wrench_acc_cog_term_ = Eigen::VectorXd::Zero(6); @@ -110,6 +115,7 @@ namespace aerial_robot_control ros::NodeHandle control_nh(nh_, "controller"); momentum_observer_matrix_ = Eigen::MatrixXd::Identity(6,6); double force_weight, torque_weight; + getParam(control_nh, "pd_wrench_comp_mode", pd_wrench_comp_mode_, false); getParam(control_nh, "momentum_observer_force_weight", force_weight, 10.0); getParam(control_nh, "momentum_observer_torque_weight", torque_weight, 10.0); momentum_observer_matrix_.topRows(3) *= force_weight; diff --git a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp index fa91126bb..03d0b0d4c 100644 --- a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp +++ b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp @@ -52,7 +52,7 @@ namespace aerial_robot_control bool GimbalrotorController::update() { - sendGimbalCommand(); + // sendGimbalCommand(); if(gimbal_calc_in_fc_){ std_msgs::UInt8 msg; msg.data = gimbal_dof_; @@ -162,7 +162,7 @@ namespace aerial_robot_control if(integrated_map_inv(i, YAW) > max_yaw_scale) max_yaw_scale = integrated_map_inv(i, YAW); last_col += 2; } - candidate_yaw_term_ = pid_controllers_.at(YAW).result() * max_yaw_scale; + candidate_yaw_term_ = pid_controllers_.at(YAW).result() * max_yaw_scale + feedforward_wrench_acc_cog_term_(5); /* calculate target full thrusts and gimbal angles (considering full components)*/ last_col = 0; @@ -190,16 +190,15 @@ namespace aerial_robot_control for(int i = 0; i < motor_num_; i++){ gimbal_control_msg.position.push_back(target_gimbal_angles_.at(i)); } - gimbal_control_pub_.publish(gimbal_control_msg); - - std_msgs::Float32MultiArray target_vectoring_force_msg; - for(int i = 0; i < target_vectoring_f_.size(); i++){ - target_vectoring_f_ = target_vectoring_f_trans_ + target_vectoring_f_rot_; - target_vectoring_force_msg.data.push_back(target_vectoring_f_(i)); - } - target_vectoring_force_pub_.publish(target_vectoring_force_msg); - + gimbal_control_pub_.publish(gimbal_control_msg); } + std_msgs::Float32MultiArray target_vectoring_force_msg; + target_vectoring_f_ = target_vectoring_f_trans_ + target_vectoring_f_rot_; + for(int i = 0; i < target_vectoring_f_.size(); i++){ + target_vectoring_force_msg.data.push_back(target_vectoring_f_(i)); + } + target_vectoring_force_pub_.publish(target_vectoring_force_msg); + } void GimbalrotorController::sendFourAxisCommand()