diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index e4f8bd33..98732896 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -24,11 +24,10 @@ find_package(catkin REQUIRED visualization_msgs dynamic_reconfigure angles -) + ) generate_dynamic_reconfigure_options( cfg/BulletSolver.cfg - cfg/GimbalBase.cfg ) ################################### diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 628c46f3..8f2b4f4a 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,7 +14,5 @@ gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) -gen.add("angle1", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0) -gen.add("angle2", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg deleted file mode 100755 index 0dd3e3e2..00000000 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "rm_gimbal_controllers" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("yaw_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) -gen.add("pitch_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) - -exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index eef3783b..5ed6d081 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,14 +46,13 @@ #include #include #include -#include namespace rm_gimbal_controllers { struct Config { double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, dt, timeout, angle1, angle2; + resistance_coff_qd_30, g, delay, dt, timeout; }; class BulletSolver @@ -77,7 +76,6 @@ class BulletSolver void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); - void isShootAfterDelay(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -85,7 +83,6 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> is_shoot_after_delay_pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; @@ -95,7 +92,6 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; int selected_armor_; bool track_target_; - double is_shoot_after_delay_ = 1.; geometry_msgs::Point target_pos_{}; double fly_time_; diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 7001757e..1c919927 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -38,32 +38,22 @@ #pragma once #include -#include #include #include -#include #include -#include +#include +#include #include #include #include -#include +#include #include #include #include -#include -#include -#include -#include +#include namespace rm_gimbal_controllers { -struct GimbalConfig -{ - // Input feedforward - double yaw_k_v_, pitch_k_v_; -}; - class ChassisVel { public: @@ -149,42 +139,40 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; // ROS Interface ros::Time last_publish_time_{}; - std::unique_ptr> pid_yaw_pos_state_pub_, - pid_pitch_pos_state_pub_; std::shared_ptr> error_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - urdf::JointConstSharedPtr pitch_joint_urdf_, yaw_joint_urdf_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; std::string gimbal_des_frame_id_{}, imu_name_{}; double publish_rate_{}; bool state_changed_{}; - int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_, last_odom2gimbal_des_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; double gravity_; bool enable_gravity_compensation_; + // Input feedforward + double yaw_k_v_; + double pitch_k_v_; + // Resistance compensation double yaw_resistance_; double velocity_saturation_point_, effort_saturation_point_; @@ -193,11 +181,6 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; - bool dynamic_reconfig_initialized_{}; - GimbalConfig config_{}; - realtime_tools::RealtimeBuffer config_rt_buffer_; - dynamic_reconfigure::Server* d_srv_{}; - enum { RATE, diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e6764d6a..94188c8a 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -52,9 +52,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), - .timeout = getParam(controller_nh, "timeout", 0.), - .angle1 = getParam(controller_nh, "angle1", 15.), - .angle2 = getParam(controller_nh, "angle2", 15.) }; + .timeout = getParam(controller_nh, "timeout", 0.) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -82,7 +80,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - is_shoot_after_delay_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -119,17 +116,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double r = r1; double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - config_.angle1 = config_.angle1 * M_PI / 180; - config_.angle2 = config_.angle2 * M_PI / 180; - double switch_armor_angle = track_target_ ? acos(r / target_rho) - config_.angle1 + - (-acos(r / target_rho) + config_.angle1 + config_.angle2) * - std::abs(v_yaw) / max_track_target_vel_ : - config_.angle2; - is_shoot_after_delay_ = 1.; - if (((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && - track_target_) - is_shoot_after_delay_ = 0.; + double switch_armor_angle = track_target_ ? + acos(r / target_rho) - M_PI / 12 + + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : + M_PI / 12; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { @@ -220,16 +210,6 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge } } -void BulletSolver::isShootAfterDelay(const ros::Time& time) -{ - if (is_shoot_after_delay_pub_->trylock()) - { - is_shoot_after_delay_pub_->msg_.stamp = time; - is_shoot_after_delay_pub_->msg_.error = is_shoot_after_delay_; - is_shoot_after_delay_pub_->unlockAndPublish(); - } -} - void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) { marker_desire_.points.clear(); @@ -336,8 +316,6 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.delay = init_config.delay; config.dt = init_config.dt; config.timeout = init_config.timeout; - config.angle1 = init_config.angle1; - config.angle2 = init_config.angle2; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -348,9 +326,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .g = config.g, .delay = config.delay, .dt = config.dt, - .timeout = config.timeout, - .angle1 = config.angle1, - .angle2 = config.angle2 }; + .timeout = config.timeout }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 8c5d5da2..7208e7a9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -75,22 +75,12 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); - ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); - ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); - - config_ = { .yaw_k_v_ = getParam(nh_pitch, "k_v", 0.), .pitch_k_v_ = getParam(nh_yaw, "k_v", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - + yaw_k_v_ = getParam(nh_yaw, "k_v", 0.); + pitch_k_v_ = getParam(nh_pitch, "k_v", 0.); hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); - if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch) || - !pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) + if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) return false; - robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); if (!controller_nh.hasParam("imu_name")) has_imu_ = false; @@ -106,45 +96,22 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - // Get URDF info about joint - urdf::Model urdf; - if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } - pitch_joint_urdf_ = urdf.getJoint(ctrl_pitch_.getJointName()); - yaw_joint_urdf_ = urdf.getJoint(ctrl_yaw_.getJointName()); - if (!pitch_joint_urdf_) - { - ROS_ERROR("Could not find joint pitch in urdf"); - return false; - } - if (!yaw_joint_urdf_) - { - ROS_ERROR("Could not find joint yaw in urdf"); - return false; - } - - gimbal_des_frame_id_ = pitch_joint_urdf_->child_link_name + "_des"; + gimbal_des_frame_id_ = ctrl_pitch_.joint_urdf_->child_link_name + "_des"; odom2gimbal_des_.header.frame_id = "odom"; odom2gimbal_des_.child_frame_id = gimbal_des_frame_id_; odom2gimbal_des_.transform.rotation.w = 1.; odom2pitch_.header.frame_id = "odom"; - odom2pitch_.child_frame_id = pitch_joint_urdf_->child_link_name; + odom2pitch_.child_frame_id = ctrl_pitch_.joint_urdf_->child_link_name; odom2pitch_.transform.rotation.w = 1.; odom2base_.header.frame_id = "odom"; - odom2base_.child_frame_id = yaw_joint_urdf_->parent_link_name; + odom2base_.child_frame_id = ctrl_yaw_.joint_urdf_->parent_link_name; odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - pid_yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher( - nh_pid_yaw_pos, "pid_yaw_pos_state", 1)); - pid_pitch_pos_state_pub_.reset(new realtime_tools::RealtimePublisher( - nh_pid_pitch_pos, "pid_pitch_pos_state", 1)); + return true; } @@ -158,11 +125,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); - config_ = *config_rt_buffer_.readFromRT(); try { - odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time); - odom2base_ = robot_state_handle_.lookupTransform("odom", yaw_joint_urdf_->parent_link_name, time); + odom2pitch_ = robot_state_handle_.lookupTransform("odom", ctrl_pitch_.joint_urdf_->child_link_name, time); + odom2base_ = robot_state_handle_.lookupTransform("odom", ctrl_yaw_.joint_urdf_->parent_link_name, time); } catch (tf2::TransformException& ex) { @@ -201,13 +167,13 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(base2gimbal_des), roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw); double pitch_real_des, yaw_real_des; - if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pitch_joint_urdf_)) + if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, ctrl_pitch_.joint_urdf_)) { double yaw_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->upper : 1e16; - lower_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->lower : -1e16; + upper_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->upper : 1e16; + lower_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, lower_limit)) ? @@ -217,13 +183,13 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_temp); } - if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, yaw_joint_urdf_)) + if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, ctrl_yaw_.joint_urdf_)) { double pitch_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->upper : 1e16; - lower_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->lower : -1e16; + upper_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->upper : 1e16; + lower_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, base2gimbal_current_des_pitch, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, lower_limit)) ? @@ -305,7 +271,6 @@ void Controller::track(const ros::Time& time) error_pub_->unlockAndPublish(); } bullet_solver_->bulletModelPub(odom2pitch_, time); - bullet_solver_->isShootAfterDelay(time); last_publish_time_ = time; } @@ -371,10 +336,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { tf2::doTransform(gyro, angular_vel_pitch, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); tf2::doTransform(gyro, angular_vel_yaw, - robot_state_handle_.lookupTransform(yaw_joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); } catch (tf2::TransformException& ex) @@ -388,13 +353,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_yaw.z = ctrl_yaw_.joint_.getVelocity(); angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); } - double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; - quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); - double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); - double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); - pid_pitch_pos_.computeCommand(pitch_angle_error, period); - pid_yaw_pos_.computeCommand(yaw_angle_error, period); + geometry_msgs::TransformStamped base_frame2des; + base_frame2des = + robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); + double roll_des, pitch_des, yaw_des; // desired position + quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des); double yaw_vel_des = 0., pitch_vel_des = 0.; if (state_ == RATE) @@ -410,18 +373,19 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) data_track_.yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); tf2::Vector3 target_pos_tf, target_vel_tf; + try { geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - yaw_joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->parent_link_name, data_track_.header.frame_id, - data_track_.header.stamp); + transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, + data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -434,67 +398,35 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } } - // publish state - if (loop_count_ % 10 == 0) - { - if (pid_yaw_pos_state_pub_ && pid_yaw_pos_state_pub_->trylock()) - { - pid_yaw_pos_state_pub_->msg_.header.stamp = time; - pid_yaw_pos_state_pub_->msg_.set_point = yaw_des; - pid_yaw_pos_state_pub_->msg_.process_value = yaw_real; - pid_yaw_pos_state_pub_->msg_.process_value_dot = ctrl_yaw_.joint_.getVelocity(); - pid_yaw_pos_state_pub_->msg_.error = yaw_angle_error; - pid_yaw_pos_state_pub_->msg_.time_step = period.toSec(); - pid_yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); - double dummy; - bool antiwindup; - pid_yaw_pos_.getGains(pid_yaw_pos_state_pub_->msg_.p, pid_yaw_pos_state_pub_->msg_.i, - pid_yaw_pos_state_pub_->msg_.d, pid_yaw_pos_state_pub_->msg_.i_clamp, dummy, antiwindup); - pid_yaw_pos_state_pub_->msg_.antiwindup = static_cast(antiwindup); - pid_yaw_pos_state_pub_->unlockAndPublish(); - } - if (pid_pitch_pos_state_pub_ && pid_pitch_pos_state_pub_->trylock()) - { - pid_pitch_pos_state_pub_->msg_.header.stamp = time; - pid_pitch_pos_state_pub_->msg_.set_point = pitch_des; - pid_pitch_pos_state_pub_->msg_.process_value = pitch_real; - pid_pitch_pos_state_pub_->msg_.process_value_dot = ctrl_pitch_.joint_.getVelocity(); - pid_pitch_pos_state_pub_->msg_.error = pitch_angle_error; - pid_pitch_pos_state_pub_->msg_.time_step = period.toSec(); - pid_pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); - double dummy; - bool antiwindup; - pid_pitch_pos_.getGains(pid_pitch_pos_state_pub_->msg_.p, pid_pitch_pos_state_pub_->msg_.i, - pid_pitch_pos_state_pub_->msg_.d, pid_pitch_pos_state_pub_->msg_.i_clamp, dummy, - antiwindup); - pid_pitch_pos_state_pub_->msg_.antiwindup = static_cast(antiwindup); - pid_pitch_pos_state_pub_->unlockAndPublish(); - } - } - loop_count_++; - - ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + - ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); + ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); + ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); + double resistance_compensation = 0.; + if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_saturation_point_) + resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; + else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_saturation_point_) + resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; + else + resistance_compensation = ctrl_yaw_.joint_.getCommand() * yaw_resistance_ / effort_saturation_point_; + ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + + yaw_k_v_ * yaw_vel_des + resistance_compensation); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des); } double Controller::feedForward(const ros::Time& time) { Eigen::Vector3d gravity(0, 0, -gravity_); tf2::doTransform(gravity, gravity, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, "odom", time)); + robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, "odom", time)); Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); double feedforward = -mass_origin.cross(gravity).y(); if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); tf2::doTransform(gravity_compensation, gravity_compensation, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, - pitch_joint_urdf_->parent_link_name, time)); + robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, + ctrl_pitch_.joint_urdf_->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -532,23 +464,6 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } -void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) -{ - ROS_INFO("[Gimbal Base] Dynamic params change"); - if (!dynamic_reconfig_initialized_) - { - GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml - config.yaw_k_v_ = init_config.yaw_k_v_; - config.pitch_k_v_ = init_config.pitch_k_v_; - dynamic_reconfig_initialized_ = true; - } - GimbalConfig config_non_rt{ - .yaw_k_v_ = config.yaw_k_v_, - .pitch_k_v_ = config.pitch_k_v_, - }; - config_rt_buffer_.writeFromNonRT(config_non_rt); -} - } // namespace rm_gimbal_controllers PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::Controller, controller_interface::ControllerBase)