From d96de654c6acccbaf51ceadcbb69550b63efdc36 Mon Sep 17 00:00:00 2001 From: Kaneko Date: Thu, 16 Jan 2025 14:55:24 +0900 Subject: [PATCH] [Twin Hammer] [controller] low-pass filter for gimbal roll angle --- .../config/control/TwinHammerControl.yaml | 1 + .../twin_hammer/control/twin_hammer_controller.h | 3 +++ .../src/control/twin_hammer_controller.cpp | 14 ++++++++++++-- 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/robots/twin_hammer/config/control/TwinHammerControl.yaml b/robots/twin_hammer/config/control/TwinHammerControl.yaml index f0cbf97b7..de43d495b 100644 --- a/robots/twin_hammer/config/control/TwinHammerControl.yaml +++ b/robots/twin_hammer/config/control/TwinHammerControl.yaml @@ -6,6 +6,7 @@ controller: gimbal_roll_delta_angle: 0.1 gimbal_pitch_delta_angle: 0.1 gravity_acc: 3.0 + delay_param: 0.5 xy: p_gain: 0.5 diff --git a/robots/twin_hammer/include/twin_hammer/control/twin_hammer_controller.h b/robots/twin_hammer/include/twin_hammer/control/twin_hammer_controller.h index ffa4084c8..6f907a8c8 100644 --- a/robots/twin_hammer/include/twin_hammer/control/twin_hammer_controller.h +++ b/robots/twin_hammer/include/twin_hammer/control/twin_hammer_controller.h @@ -30,6 +30,9 @@ namespace aerial_robot_control double gimbal_roll_delta_angle_; double gimbal_pitch_delta_angle_; double gravity_acc_; + double filtered_gimbal_1_roll_; + double filtered_gimbal_2_roll_; + double delay_param_; boost::shared_ptr nl_solver_; std::vector opt_x_; diff --git a/robots/twin_hammer/src/control/twin_hammer_controller.cpp b/robots/twin_hammer/src/control/twin_hammer_controller.cpp index 85df223e8..5fa27fe90 100644 --- a/robots/twin_hammer/src/control/twin_hammer_controller.cpp +++ b/robots/twin_hammer/src/control/twin_hammer_controller.cpp @@ -31,6 +31,8 @@ void TwinHammerController::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, haptics_force_ = Eigen::Vector3d::Zero(); haptics_torque_ = Eigen::Vector3d::Zero(); + filtered_gimbal_1_roll_ = 0.0; + filtered_gimbal_2_roll_ = 0.0; rosParamInit(); } @@ -42,7 +44,7 @@ void TwinHammerController::rosParamInit() getParam(control_nh, "gimbal_roll_delta_angle", gimbal_roll_delta_angle_, 0.1); getParam(control_nh, "gimbal_pitch_delta_angle", gimbal_pitch_delta_angle_, 0.1); getParam(control_nh, "gravity_acc", gravity_acc_, 1.0); - + getParam(control_nh, "delay_param", delay_param_, 1.0); } void TwinHammerController::HapticsSwitchCallback(std_msgs::Int8 msg) @@ -185,7 +187,15 @@ void TwinHammerController::controlCore() if(gimbal_i_pitch < prev_gimbal_angles_.at(2*i+1) - gimbal_pitch_delta_angle_){ gimbal_i_pitch = prev_gimbal_angles_.at(2*i+1) - gimbal_pitch_delta_angle_; } - target_gimbal_angles_.at(2*i) = gimbal_i_roll; + if(i==0){ + filtered_gimbal_1_roll_ = (1-delay_param_) * filtered_gimbal_1_roll_ + delay_param_ * gimbal_i_roll; + target_gimbal_angles_.at(0) = filtered_gimbal_1_roll_; + } + if(i==1){ + filtered_gimbal_2_roll_ = (1-delay_param_) * filtered_gimbal_2_roll_ + delay_param_ * gimbal_i_roll; + target_gimbal_angles_.at(2) = filtered_gimbal_2_roll_; + } + // target_gimbal_angles_.at(2*i) = gimbal_i_roll; target_gimbal_angles_.at(2*i+1) = gimbal_i_pitch; // std::cout << "gimbal" << i << "roll is " << gimbal_i_roll << std::endl; // std::cout << "gimbal" << i << "pitch is " << gimbal_i_pitch << std::endl;