From 6fc2ba168ff07592df183151fe0506aa9489f52c Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: 2024年1月11日 23:32:26 +0800 Subject: [PATCH 1/7] Compute gimbal yaw vel desire and accel desire. --- rm_gimbal_controllers/CMakeLists.txt | 2 +- .../rm_gimbal_controllers/bullet_solver.h | 9 ++-- .../rm_gimbal_controllers/gimbal_base.h | 4 +- rm_gimbal_controllers/src/bullet_solver.cpp | 52 ++++++++++--------- rm_gimbal_controllers/src/gimbal_base.cpp | 38 +++----------- 5 files changed, 44 insertions(+), 61 deletions(-) diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98732896..56bd01d7 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -67,7 +67,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/feedforward.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 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 5ed6d081..7079c794 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -73,9 +73,10 @@ class BulletSolver { return -output_pitch_; } - 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); + double getYawVelDes(); + double getYawAccelDes(); + double getPitchVelDes(); + double getPitchAccelDes(); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -94,6 +95,8 @@ class BulletSolver bool track_target_; geometry_msgs::Point target_pos_{}; + geometry_msgs::Vector3 target_vel_{}; + geometry_msgs::Vector3 target_accel_{}; double fly_time_; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; 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 e09d968f..068fe406 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -170,8 +170,8 @@ class Controller : public controller_interface::MultiInterfaceControllergetSelectedArmorPosAndVel(target_pos, target_vel, data_track_.position, data_track_.velocity, - 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( - 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(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); - tf2::fromMsg(target_vel, target_vel_tf); - pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - } + yaw_vel_des = bullet_solver_->getYawVelDes(); + yaw_accel_des = bullet_solver_->getYawAccelDes(); + pitch_vel_des = bullet_solver_->getPitchVelDes(); + pitch_accel_des = bullet_solver_->getPitchAccelDes(); } ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); @@ -408,8 +384,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) else if (std::abs(ctrl_yaw_.joint_.getCommand())> effort_dead_zone_) resistance_compensation = (ctrl_yaw_.joint_.getCommand()> 0 ? 1 : -1) * yaw_resistance_; 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); + yaw_k_v_ * yaw_vel_des + yaw_k_a_ * yaw_accel_des +resistance_compensation); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des + pitch_k_a_ * pitch_accel_des); } double Controller::feedForward(const ros::Time& time) From 84a7a9236fbabb8c4b8bf0b68f958c01520c26a5 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: 2024年1月12日 14:13:27 +0800 Subject: [PATCH 2/7] Use accelaration desire from topic under rate mode. --- .../rm_gimbal_controllers/bullet_solver.h | 4 +-- .../rm_gimbal_controllers/gimbal_base.h | 4 +-- rm_gimbal_controllers/src/bullet_solver.cpp | 25 +++++++++++-------- rm_gimbal_controllers/src/gimbal_base.cpp | 9 +++++-- 4 files changed, 25 insertions(+), 17 deletions(-) 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 7079c794..73ead6a8 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -73,8 +73,8 @@ class BulletSolver { return -output_pitch_; } - double getYawVelDes(); - double getYawAccelDes(); + double getYawVelDes() const; + double getYawAccelDes() const; double getPitchVelDes(); double getPitchAccelDes(); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& 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 068fe406..c4190814 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -170,8 +170,8 @@ class Controller : public controller_interface::MultiInterfaceControllerget(); if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) @@ -364,7 +366,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (state_ == RATE) { yaw_vel_des = cmd_gimbal_.rate_yaw; + yaw_accel_des = cmd_gimbal_.accel_yaw; pitch_vel_des = cmd_gimbal_.rate_pitch; + pitch_accel_des = cmd_gimbal_.accel_pitch; } else if (state_ == TRACK) { @@ -384,8 +388,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) else if (std::abs(ctrl_yaw_.joint_.getCommand())> effort_dead_zone_) resistance_compensation = (ctrl_yaw_.joint_.getCommand()> 0 ? 1 : -1) * yaw_resistance_; ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + - yaw_k_v_ * yaw_vel_des + yaw_k_a_ * yaw_accel_des +resistance_compensation); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des + pitch_k_a_ * pitch_accel_des); + yaw_k_v_ * yaw_vel_des + yaw_k_a_ * yaw_accel_des + resistance_compensation); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des + + pitch_k_a_ * pitch_accel_des); } double Controller::feedForward(const ros::Time& time) From 668d8528de25ce6cca906613c928d763d1336b46 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: 2024年1月14日 16:45:34 +0800 Subject: [PATCH 3/7] Compute gimbal pitch vel desire and accel desire. --- .../rm_gimbal_controllers/bullet_solver.h | 19 ++++-- rm_gimbal_controllers/src/bullet_solver.cpp | 62 ++++++++++++++----- rm_gimbal_controllers/src/gimbal_base.cpp | 16 ++--- 3 files changed, 70 insertions(+), 27 deletions(-) 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 73ead6a8..67f734c9 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -55,6 +55,16 @@ struct Config resistance_coff_qd_30, g, delay, dt, timeout; }; +struct TargetState +{ + geometry_msgs::Point current_target_center_pos; + geometry_msgs::Vector3 current_target_center_vel; + double yaw; + double v_yaw; + double r; + int armors_num; +}; + class BulletSolver { public: @@ -73,10 +83,8 @@ class BulletSolver { return -output_pitch_; } - double getYawVelDes() const; - double getYawAccelDes() const; - double getPitchVelDes(); - double getPitchAccelDes(); + void getYawVelAndAccelDes(double& vel_des, double& accel_des); + void getPitchVelAndAccelDes(double& vel_des, double& accel_des); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -90,6 +98,8 @@ class BulletSolver double max_track_target_vel_; bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; + double last_pitch_vel_des_{}; + ros::Time last_pitch_vel_des_solve_time_{ 0 }; double bullet_speed_{}, resistance_coff_{}; int selected_armor_; bool track_target_; @@ -97,6 +107,7 @@ class BulletSolver geometry_msgs::Point target_pos_{}; geometry_msgs::Vector3 target_vel_{}; geometry_msgs::Vector3 target_accel_{}; + TargetState target_state_{}; double fly_time_; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 13c236ec..484cec87 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -105,6 +105,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; + target_state_ = TargetState{ .current_target_center_pos = pos, + .current_target_center_vel = vel, + .yaw = yaw, + .v_yaw = v_yaw, + .armors_num = armors_num }; double temp_z = pos.z; double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); @@ -127,6 +132,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; } + target_state_.r = r; + target_state_.current_target_center_pos.z = z; int count{}; double error = 999; if (track_target_) @@ -143,8 +150,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d while (error>= 0.001) { output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2))); target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + output_pitch_ = std::atan2(temp_z, target_rho); fly_time_ = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * @@ -187,15 +194,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d return true; } -double BulletSolver::getYawVelDes() const +void BulletSolver::getYawVelAndAccelDes(double& vel_des, double& accel_des) { double yaw_vel_des = (target_pos_.x * target_vel_.y - target_pos_.y * target_vel_.x) / (pow(target_pos_.x, 2) + pow(target_pos_.y, 2)); - return yaw_vel_des; -} - -double BulletSolver::getYawAccelDes() const -{ double yaw_accel_des = (pow(target_pos_.x, 3) * target_accel_.y - pow(target_pos_.y, 3) * target_accel_.x + 2 * target_pos_.x * target_pos_.y * pow(target_vel_.x, 2) - 2 * target_pos_.x * target_pos_.y * pow(target_vel_.y, 2) - @@ -204,17 +206,47 @@ double BulletSolver::getYawAccelDes() const 2 * pow(target_pos_.x, 2) * target_vel_.x * target_vel_.y + 2 * pow(target_pos_.y, 2) * target_vel_.x * target_vel_.y) / pow((pow(target_pos_.x, 2) + pow(target_pos_.y, 2)), 2); - return yaw_accel_des; -} - -double BulletSolver::getPitchVelDes() -{ - return 0; + vel_des = yaw_vel_des; + accel_des = yaw_accel_des; } -double BulletSolver::getPitchAccelDes() +void BulletSolver::getPitchVelAndAccelDes(double& vel_des, double& accel_des) { - return 0; + double dt = 0.01; + double r = target_state_.r; + double pos_x = target_state_.current_target_center_pos.x + + target_state_.current_target_center_vel.x * (fly_time_ + dt) - + r * cos(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) + + selected_armor_ * 2 * M_PI / target_state_.armors_num); + double pos_y = target_state_.current_target_center_pos.y + + target_state_.current_target_center_vel.y * (fly_time_ + dt) - + r * sin(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) + + selected_armor_ * 2 * M_PI / target_state_.armors_num); + double pos_z = + target_state_.current_target_center_pos.z + (fly_time_ + dt) * target_state_.current_target_center_vel.z; + double target_rho = std::sqrt(std::pow(pos_x, 2) + std::pow(pos_y, 2)); + double temp_z = target_rho * tan(output_pitch_); + double output_pitch_next = output_pitch_; + double error_z = 999; + while (std::abs(error_z)>= 1e-9) + { + output_pitch_next = std::atan2(temp_z, target_rho); + double fly_time = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_next)))) / + resistance_coff_; + double real_z = (bullet_speed_ * std::sin(output_pitch_next) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - + config_.g * fly_time / resistance_coff_; + error_z = pos_z - real_z; + temp_z += error_z; + } + double pitch_vel_des, pitch_accel_des; + pitch_vel_des = (output_pitch_next - output_pitch_) / dt; + ros::Time now = ros::Time::now(); + pitch_accel_des = (pitch_vel_des - last_pitch_vel_des_) / (now - last_pitch_vel_des_solve_time_).toSec(); + last_pitch_vel_des_ = pitch_vel_des; + last_pitch_vel_des_solve_time_ = now; + vel_des = pitch_vel_des; + accel_des = pitch_accel_des; } void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index c15ac5de..6e41fe6e 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -250,14 +250,16 @@ void Controller::track(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - target_pos.x -= odom2pitch_.transform.translation.x; - target_pos.y -= odom2pitch_.transform.translation.y; - target_pos.z -= odom2pitch_.transform.translation.z; + ros::Time now = ros::Time::now(); + target_pos.x += target_vel.x * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; + target_pos.y += target_vel.y * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; + target_pos.z += target_vel.z * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); + double yaw = data_track_.yaw + data_track_.v_yaw * (now - data_track_.header.stamp).toSec(); bool solve_success = - bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, data_track_.v_yaw, + bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); if (publish_rate_> 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) @@ -372,10 +374,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - yaw_vel_des = bullet_solver_->getYawVelDes(); - yaw_accel_des = bullet_solver_->getYawAccelDes(); - pitch_vel_des = bullet_solver_->getPitchVelDes(); - pitch_accel_des = bullet_solver_->getPitchAccelDes(); + bullet_solver_->getYawVelAndAccelDes(yaw_vel_des, yaw_accel_des); + bullet_solver_->getPitchVelAndAccelDes(pitch_vel_des, pitch_accel_des); } ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); From ade76df194223701411ee8f1aad53d57941edb40 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: 2024年1月14日 18:42:38 +0800 Subject: [PATCH 4/7] Simplify getGimbalError(). --- .../rm_gimbal_controllers/bullet_solver.h | 3 +- rm_gimbal_controllers/src/bullet_solver.cpp | 28 ++++++++----------- rm_gimbal_controllers/src/gimbal_base.cpp | 5 +--- 3 files changed, 13 insertions(+), 23 deletions(-) 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 67f734c9..bfcf77ac 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -72,8 +72,7 @@ class BulletSolver bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); - double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); + double getGimbalError(double yaw_real, double pitch_real); double getResistanceCoefficient(double bullet_speed) const; double getYaw() const { diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 484cec87..745071bd 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -298,26 +298,17 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } } -double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, - double bullet_speed) +double BulletSolver::getGimbalError(double yaw_real, double pitch_real) { double delay = track_target_ ? 0 : config_.delay; - double r = r1; - double z = pos.z; - if (selected_armor_ != 0) - { - r = armors_num == 4 ? r2 : r1; - z = armors_num == 4 ? pos.z + dz : pos.z; - } double error; if (track_target_) { double bullet_rho = - bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + bullet_speed_ * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; double bullet_x = bullet_rho * std::cos(yaw_real); double bullet_y = bullet_rho * std::sin(yaw_real); - double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) * + double bullet_z = (bullet_speed_ * std::sin(pitch_real) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - config_.g * fly_time_ / resistance_coff_; error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) + @@ -327,12 +318,15 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec { geometry_msgs::Point target_pos_after_fly_time_and_delay{}; target_pos_after_fly_time_and_delay.x = - pos.x + vel.x * (fly_time_ + delay) - - r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_state_.current_target_center_pos.x + target_state_.current_target_center_vel.x * (fly_time_ + delay) - + target_state_.r * cos(target_state_.yaw + target_state_.v_yaw * (fly_time_ + delay) + + selected_armor_ * 2 * M_PI / target_state_.armors_num); target_pos_after_fly_time_and_delay.y = - pos.y + vel.y * (fly_time_ + delay) - - r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); + target_state_.current_target_center_pos.y + target_state_.current_target_center_vel.y * (fly_time_ + delay) - + target_state_.r * sin(target_state_.yaw + target_state_.v_yaw * (fly_time_ + delay) + + selected_armor_ * 2 * M_PI / target_state_.armors_num); + target_pos_after_fly_time_and_delay.z = + target_state_.current_target_center_pos.z + target_state_.current_target_center_vel.z * (fly_time_ + delay); error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 5fb5b292..9549d3d3 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -266,10 +266,7 @@ void Controller::track(const ros::Time& time) { if (error_pub_->trylock()) { - double error = - bullet_solver_->getGimbalError(target_pos, target_vel, data_track_.yaw, data_track_.v_yaw, - data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, yaw_compute, pitch_compute, cmd_gimbal_.bullet_speed); + double error = bullet_solver_->getGimbalError(yaw_compute, pitch_compute); error_pub_->msg_.stamp = time; error_pub_->msg_.error = solve_success ? error : 1.0; error_pub_->unlockAndPublish(); From ccb0515820a05a54d143ea42657895de83a0d064 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: 2024年1月15日 02:37:59 +0800 Subject: [PATCH 5/7] Make selected_armor to enum variable select armor in better way. --- .../rm_gimbal_controllers/bullet_solver.h | 10 +++- rm_gimbal_controllers/src/bullet_solver.cpp | 49 +++++++++++-------- 2 files changed, 38 insertions(+), 21 deletions(-) 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 bfcf77ac..fdb8df74 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -65,6 +65,14 @@ struct TargetState int armors_num; }; +enum class SelectedArmor +{ + FRONT = 0, + LEFT = 1, + BACK = 2, + RIGHT = 3 +}; + class BulletSolver { public: @@ -100,7 +108,7 @@ class BulletSolver double last_pitch_vel_des_{}; ros::Time last_pitch_vel_des_solve_time_{ 0 }; double bullet_speed_{}, resistance_coff_{}; - int selected_armor_; + SelectedArmor selected_armor_; bool track_target_; geometry_msgs::Point target_pos_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 745071bd..4a9b5450 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -117,20 +118,24 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); double rough_fly_time = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - selected_armor_ = 0; double r = r1; double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; + double aim_range_front; 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.)) + aim_range_front = output_yaw_ + (v_yaw> 0 ? switch_armor_angle - 2 * M_PI / armors_num : -switch_armor_angle); + double shortest_angular_distance = angles::shortest_angular_distance(yaw + v_yaw * rough_fly_time, aim_range_front); + if (shortest_angular_distance < 0) + shortest_angular_distance += 2 * M_PI; + selected_armor_ = + static_cast(fmod((shortest_angular_distance / (2 * M_PI / armors_num) + 1), armors_num)); + if (armors_num == 4 && selected_armor_ != SelectedArmor::FRONT && selected_armor_ != SelectedArmor::BACK) { - selected_armor_ = v_yaw> 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = armors_num == 4 ? pos.z + dz : pos.z; + r = r2; + z = pos.z + dz; } target_state_.r = r; target_state_.current_target_center_pos.z = z; @@ -138,8 +143,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double error = 999; if (track_target_) { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.x = pos.x - r * cos(yaw + static_cast(selected_armor_) * 2 * M_PI / armors_num); + target_pos_.y = pos.y - r * sin(yaw + static_cast(selected_armor_) * 2 * M_PI / armors_num); } else { @@ -160,14 +165,18 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (track_target_) { - target_pos_.x = - pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = - pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_vel_.x = vel.x + v_yaw * r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_vel_.y = vel.y - v_yaw * r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_accel_.x = pow(v_yaw, 2) * r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_accel_.y = pow(v_yaw, 2) * r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.x = pos.x + vel.x * fly_time_ - + r * cos(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); + target_pos_.y = pos.y + vel.y * fly_time_ - + r * sin(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); + target_vel_.x = + vel.x + v_yaw * r * sin(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); + target_vel_.y = + vel.y - v_yaw * r * cos(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); + target_accel_.x = + pow(v_yaw, 2) * r * cos(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); + target_accel_.y = + pow(v_yaw, 2) * r * sin(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); } else { @@ -217,11 +226,11 @@ void BulletSolver::getPitchVelAndAccelDes(double& vel_des, double& accel_des) double pos_x = target_state_.current_target_center_pos.x + target_state_.current_target_center_vel.x * (fly_time_ + dt) - r * cos(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) + - selected_armor_ * 2 * M_PI / target_state_.armors_num); + static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); double pos_y = target_state_.current_target_center_pos.y + target_state_.current_target_center_vel.y * (fly_time_ + dt) - r * sin(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) + - selected_armor_ * 2 * M_PI / target_state_.armors_num); + static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); double pos_z = target_state_.current_target_center_pos.z + (fly_time_ + dt) * target_state_.current_target_center_vel.z; double target_rho = std::sqrt(std::pow(pos_x, 2) + std::pow(pos_y, 2)); @@ -320,11 +329,11 @@ double BulletSolver::getGimbalError(double yaw_real, double pitch_real) target_pos_after_fly_time_and_delay.x = target_state_.current_target_center_pos.x + target_state_.current_target_center_vel.x * (fly_time_ + delay) - target_state_.r * cos(target_state_.yaw + target_state_.v_yaw * (fly_time_ + delay) + - selected_armor_ * 2 * M_PI / target_state_.armors_num); + static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); target_pos_after_fly_time_and_delay.y = target_state_.current_target_center_pos.y + target_state_.current_target_center_vel.y * (fly_time_ + delay) - target_state_.r * sin(target_state_.yaw + target_state_.v_yaw * (fly_time_ + delay) + - selected_armor_ * 2 * M_PI / target_state_.armors_num); + static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); target_pos_after_fly_time_and_delay.z = target_state_.current_target_center_pos.z + target_state_.current_target_center_vel.z * (fly_time_ + delay); error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + From 1f1857c390cf356dcee66b49bbadc864be071b40 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: 2024年1月23日 23:31:47 +0800 Subject: [PATCH 6/7] Encapsulate feedforward. --- .../rm_gimbal_controllers/feedforward.h | 54 ++++++++++++ .../rm_gimbal_controllers/gimbal_base.h | 15 ++-- rm_gimbal_controllers/src/feedforward.cpp | 86 +++++++++++++++++++ rm_gimbal_controllers/src/gimbal_base.cpp | 70 ++++----------- 4 files changed, 165 insertions(+), 60 deletions(-) create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h create mode 100644 rm_gimbal_controllers/src/feedforward.cpp diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h new file mode 100644 index 00000000..f9a649ab --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h @@ -0,0 +1,54 @@ +// +// Created by yezi on 24-1-11. +// + +#pragma once + +#include +#include +#include +#include + +class FrictionCompensation +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(double vel_act, double effort_cmd) const; + +private: + double resistance_{ 0. }; + double velocity_saturation_point_{ 0. }, effort_saturation_point_{ 0. }; +}; + +class InputFeedforward +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(double vel_desire, double accel_desire) const; + +private: + double k_v_{ 0. }, k_a_{ 0. }; +}; + +class GravityCompensation +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(rm_control::RobotStateHandle* robot_state_handle, const urdf::JointConstSharedPtr& joint_urdf, + ros::Time time) const; + +private: + geometry_msgs::Vector3 mass_origin_{}; + double gravity_{}; + bool enable_gravity_compensation_ = false; +}; + +class BaseVelCompensation +{ +public: + void init(XmlRpc::XmlRpcValue config); + double output(double base_angular_vel_z) const; + +private: + double k_chassis_vel_{ 0. }; +}; 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 b8d9f3f9..46ac66df 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -52,6 +52,8 @@ #include #include +#include "rm_gimbal_controllers/feedforward.h" + namespace rm_gimbal_controllers { class ChassisVel @@ -135,7 +137,6 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; enum diff --git a/rm_gimbal_controllers/src/feedforward.cpp b/rm_gimbal_controllers/src/feedforward.cpp new file mode 100644 index 00000000..6fb71645 --- /dev/null +++ b/rm_gimbal_controllers/src/feedforward.cpp @@ -0,0 +1,86 @@ +// +// Created by yezi on 24-1-11. +// + +#include +#include +#include "rm_gimbal_controllers/feedforward.h" + +void FrictionCompensation::init(XmlRpc::XmlRpcValue config) +{ + ROS_ASSERT(config.getType() == XmlRpc::XmlRpcValue::TypeStruct); + resistance_ = config.hasMember("resistance") ? static_cast(config["resistance"]) : 0.; + velocity_saturation_point_ = + config.hasMember("velocity_saturation_point") ? static_cast(config["velocity_saturation_point"]) : 0.; + effort_saturation_point_ = + config.hasMember("effort_saturation_point") ? static_cast(config["effort_saturation_point"]) : 0.; +} + +double FrictionCompensation::output(double vel_act, double effort_cmd) const +{ + double resistance_compensation = 0.; + if (std::abs(vel_act)> velocity_saturation_point_) + resistance_compensation = (vel_act> 0 ? 1 : -1) * resistance_; + else if (std::abs(effort_cmd)> effort_saturation_point_) + resistance_compensation = (effort_cmd> 0 ? 1 : -1) * resistance_; + else + resistance_compensation = effort_cmd * resistance_ / effort_saturation_point_; + return resistance_compensation; +} + +void InputFeedforward::init(XmlRpc::XmlRpcValue config) +{ + ROS_ASSERT(config.getType() == XmlRpc::XmlRpcValue::TypeStruct); + k_v_ = config.hasMember("k_v") ? static_cast(config["k_v"]) : 0.; + k_a_ = config.hasMember("k_a") ? static_cast(config["k_a"]) : 0.; +} + +double InputFeedforward::output(double vel_desire, double accel_desire) const +{ + return k_v_ * vel_desire + k_a_ * accel_desire; +} + +void GravityCompensation::init(XmlRpc::XmlRpcValue config) +{ + bool enable_feedforward; + enable_feedforward = config.hasMember("gravity_compensation"); + if (enable_feedforward) + { + ROS_ASSERT(config["gravity_compensation"].hasMember("mass_origin")); + ROS_ASSERT(config["gravity_compensation"].hasMember("gravity")); + ROS_ASSERT(config["gravity_compensation"].hasMember("enable_gravity_compensation")); + } + mass_origin_.x = enable_feedforward ? static_cast(config["gravity_compensation"]["mass_origin"][0]) : 0.; + mass_origin_.z = enable_feedforward ? static_cast(config["gravity_compensation"]["mass_origin"][2]) : 0.; + gravity_ = enable_feedforward ? static_cast(config["gravity_compensation"]["gravity"]) : 0.; + enable_gravity_compensation_ = + enable_feedforward && static_cast(config["gravity_compensation"]["enable_gravity_compensation"]); +} + +double GravityCompensation::output(rm_control::RobotStateHandle* robot_state_handle, + const urdf::JointConstSharedPtr& joint_urdf, ros::Time time) const +{ + Eigen::Vector3d gravity(0, 0, -gravity_); + tf2::doTransform(gravity, gravity, robot_state_handle->lookupTransform(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(joint_urdf->child_link_name, joint_urdf->parent_link_name, + time)); + feedforward -= mass_origin.cross(gravity_compensation).y(); + } + return feedforward; +} + +void BaseVelCompensation::init(XmlRpc::XmlRpcValue config) +{ + k_chassis_vel_ = config.hasMember("k_chassis_vel") ? static_cast(config["k_chassis_vel"]) : 0.; +} + +double BaseVelCompensation::output(double base_angular_vel_z) const +{ + return -k_chassis_vel_ * base_angular_vel_z; +} diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 9549d3d3..2b68dc37 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -49,36 +49,24 @@ namespace rm_gimbal_controllers bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { XmlRpc::XmlRpcValue xml_rpc_value; - bool enable_feedforward; - enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); - if (enable_feedforward) - { - ROS_ASSERT(xml_rpc_value.hasMember("mass_origin")); - ROS_ASSERT(xml_rpc_value.hasMember("gravity")); - ROS_ASSERT(xml_rpc_value.hasMember("enable_gravity_compensation")); - } - mass_origin_.x = enable_feedforward ? (double)xml_rpc_value["mass_origin"][0] : 0.; - mass_origin_.z = enable_feedforward ? (double)xml_rpc_value["mass_origin"][2] : 0.; - gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; - enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - - ros::NodeHandle resistance_compensation_nh(controller_nh, "yaw/resistance_compensation"); - yaw_resistance_ = getParam(resistance_compensation_nh, "resistance", 0.); - velocity_saturation_point_ = getParam(resistance_compensation_nh, "velocity_saturation_point", 0.); - effort_saturation_point_ = getParam(resistance_compensation_nh, "effort_saturation_point", 0.); + ros::NodeHandle feedforward_nh(controller_nh, "feedforward"); + feedforward_nh.getParam("yaw", xml_rpc_value); + yaw_friction_compensation_.init(xml_rpc_value); + yaw_input_feedforward_.init(xml_rpc_value); + base_vel_compensation_.init(xml_rpc_value); + feedforward_nh.getParam("pitch", xml_rpc_value); + pitch_friction_compensation_.init(xml_rpc_value); + pitch_input_feedforward_.init(xml_rpc_value); + gravity_compensation_.init(xml_rpc_value); - k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.); ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); chassis_vel_ = std::make_shared(chassis_vel_nh); + ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); bullet_solver_ = std::make_shared(nh_bullet_solver); ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); - yaw_k_v_ = getParam(nh_yaw, "k_v", 0.); - yaw_k_a_ = getParam(nh_yaw, "k_a", 0.); - pitch_k_v_ = getParam(nh_pitch, "k_v", 0.); - pitch_k_a_ = getParam(nh_pitch, "k_a", 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)) @@ -379,35 +367,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) 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 + yaw_k_a_ * yaw_accel_des + resistance_compensation); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des + - pitch_k_a_ * pitch_accel_des); -} - -double Controller::feedForward(const ros::Time& time) -{ - Eigen::Vector3d gravity(0, 0, -gravity_); - tf2::doTransform(gravity, gravity, - 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(ctrl_pitch_.joint_urdf_->child_link_name, - ctrl_pitch_.joint_urdf_->parent_link_name, time)); - feedforward -= mass_origin.cross(gravity_compensation).y(); - } - return feedforward; + ctrl_yaw_.joint_.setCommand( + ctrl_yaw_.joint_.getCommand() + base_vel_compensation_.output(chassis_vel_->angular_->z()) + + yaw_input_feedforward_.output(yaw_vel_des, yaw_accel_des) + + yaw_friction_compensation_.output(ctrl_yaw_.joint_.getVelocity(), ctrl_yaw_.joint_.getCommand())); + ctrl_pitch_.joint_.setCommand( + ctrl_pitch_.joint_.getCommand() + + gravity_compensation_.output(&robot_state_handle_, ctrl_pitch_.joint_urdf_, time) + + pitch_input_feedforward_.output(pitch_vel_des, pitch_accel_des) + + pitch_friction_compensation_.output(ctrl_pitch_.joint_.getVelocity(), ctrl_pitch_.joint_.getCommand())); } void Controller::updateChassisVel() From b0708c4899303a6c37ef28277722ef29e544f864 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: 2024年1月27日 03:52:33 +0800 Subject: [PATCH 7/7] Adopt windmill kinematics model. --- .../{ => bullet_solver}/bullet_solver.h | 23 +-- .../bullet_solver/target_kinematics_model.h | 143 ++++++++++++++++++ .../rm_gimbal_controllers/gimbal_base.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 128 ++++++---------- rm_gimbal_controllers/src/gimbal_base.cpp | 44 +++--- 5 files changed, 223 insertions(+), 117 deletions(-) rename rm_gimbal_controllers/include/rm_gimbal_controllers/{ => bullet_solver}/bullet_solver.h (90%) create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/target_kinematics_model.h diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/bullet_solver.h similarity index 90% rename from rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h rename to rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/bullet_solver.h index fdb8df74..fe39f2fb 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/bullet_solver.h @@ -46,6 +46,7 @@ #include #include #include +#include "rm_gimbal_controllers/bullet_solver/target_kinematics_model.h" namespace rm_gimbal_controllers { @@ -55,16 +56,6 @@ struct Config resistance_coff_qd_30, g, delay, dt, timeout; }; -struct TargetState -{ - geometry_msgs::Point current_target_center_pos; - geometry_msgs::Vector3 current_target_center_vel; - double yaw; - double v_yaw; - double r; - int armors_num; -}; - enum class SelectedArmor { FRONT = 0, @@ -77,9 +68,12 @@ class BulletSolver { public: explicit BulletSolver(ros::NodeHandle& controller_nh); - - bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, + // normal target(including robots and buildings) + void input(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); + // windmill + void input(double theta, double theta_dot, double bullet_speed, geometry_msgs::TransformStamped windmill2odom); + bool solve(); double getGimbalError(double yaw_real, double pitch_real); double getResistanceCoefficient(double bullet_speed) const; double getYaw() const @@ -108,13 +102,12 @@ class BulletSolver double last_pitch_vel_des_{}; ros::Time last_pitch_vel_des_solve_time_{ 0 }; double bullet_speed_{}, resistance_coff_{}; + double windmill_radius_; SelectedArmor selected_armor_; bool track_target_; + std::shared_ptr target_kinematics_; geometry_msgs::Point target_pos_{}; - geometry_msgs::Vector3 target_vel_{}; - geometry_msgs::Vector3 target_accel_{}; - TargetState target_state_{}; double fly_time_; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/target_kinematics_model.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/target_kinematics_model.h new file mode 100644 index 00000000..c4f15275 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/target_kinematics_model.h @@ -0,0 +1,143 @@ +// +// Created by yezi on 24-1-26. +// + +#pragma once + +#include +#include + +class TargetKinematicsBase +{ +public: + virtual ~TargetKinematicsBase() = default; + virtual geometry_msgs::Point position(double time) = 0; + virtual geometry_msgs::Vector3 velocity(double time) = 0; + virtual geometry_msgs::Vector3 acceleration(double time) = 0; +}; + +class NormalTargetKinematics : public TargetKinematicsBase +{ +public: + NormalTargetKinematics(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r) + { + pos_ = pos; + vel_ = vel; + yaw_ = yaw; + v_yaw_ = v_yaw; + r_ = r; + } + +protected: + geometry_msgs::Point pos_; + geometry_msgs::Vector3 vel_; + double yaw_; + double v_yaw_; + double r_; +}; + +class TrackedTargetKinematics : public NormalTargetKinematics +{ +public: + TrackedTargetKinematics(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r) + : NormalTargetKinematics(pos, vel, yaw, v_yaw, r) + { + } + geometry_msgs::Point position(double time) override + { + geometry_msgs::Point target_pos; + target_pos.x = pos_.x + vel_.x * time - r_ * cos(yaw_ + v_yaw_ * time); + target_pos.y = pos_.y + vel_.y * time - r_ * sin(yaw_ + v_yaw_ * time); + target_pos.z = pos_.z + vel_.z * time; + return target_pos; + } + geometry_msgs::Vector3 velocity(double time) override + { + geometry_msgs::Vector3 target_vel; + target_vel.x = vel_.x + v_yaw_ * r_ * sin(yaw_ + v_yaw_ * time); + target_vel.y = vel_.y - v_yaw_ * r_ * cos(yaw_ + v_yaw_ * time); + return target_vel; + } + geometry_msgs::Vector3 acceleration(double time) override + { + geometry_msgs::Vector3 target_accel; + target_accel.x = pow(v_yaw_, 2) * r_ * cos(yaw_ + v_yaw_ * time); + target_accel.y = pow(v_yaw_, 2) * r_ * sin(yaw_ + v_yaw_ * time); + return target_accel; + } +}; + +class UntrackedTargetKinematic : public NormalTargetKinematics +{ +public: + UntrackedTargetKinematic(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r) + : NormalTargetKinematics(pos, vel, yaw, v_yaw, r) + { + } + geometry_msgs::Point position(double time) override + { + geometry_msgs::Point target_pos; + double target_center_pos[2]; + target_center_pos[0] = pos_.x + vel_.x * time; + target_center_pos[1] = pos_.y + vel_.y * time; + target_pos.x = target_center_pos[0] - r_ * cos(atan2(target_center_pos[1], target_center_pos[0])); + target_pos.y = target_center_pos[1] - r_ * sin(atan2(target_center_pos[1], target_center_pos[0])); + target_pos.z = pos_.z + vel_.z * time; + return target_pos; + } + geometry_msgs::Vector3 velocity(double time) override + { + geometry_msgs::Vector3 target_vel; + return target_vel; + } + geometry_msgs::Vector3 acceleration(double time) override + { + geometry_msgs::Vector3 target_accel; + return target_accel; + } +}; + +class WindmillKinematics : public TargetKinematicsBase +{ +public: + WindmillKinematics(double theta, double theta_dot, double radius, geometry_msgs::TransformStamped windmill2odom) + { + theta_ = theta; + theta_dot_ = theta_dot; + radius_ = radius; + windmill2odom_ = windmill2odom; + } + geometry_msgs::Point position(double time) override + { + geometry_msgs::Point target_pos; + target_pos.x = 0.; + target_pos.y = -radius_ * sin(theta_ + theta_dot_ * time); + target_pos.z = radius_ * cos(theta_ + theta_dot_ * time); + tf2::doTransform(target_pos, target_pos, windmill2odom_); + return target_pos; + } + geometry_msgs::Vector3 velocity(double time) override + { + geometry_msgs::Vector3 target_vel; + target_vel.x = 0.; + target_vel.y = -theta_dot_ * radius_ * cos(theta_ + theta_dot_ * time); + target_vel.z = -theta_dot_ * radius_ * sin(theta_ + theta_dot_ * time); + tf2::doTransform(target_vel, target_vel, windmill2odom_); + return target_vel; + } + geometry_msgs::Vector3 acceleration(double time) override + { + geometry_msgs::Vector3 target_accel; + target_accel.x = 0.; + target_accel.y = pow(theta_dot_, 2) * radius_ * sin(theta_ + theta_dot_ * time); + target_accel.z = -pow(theta_dot_, 2) * radius_ * cos(theta_ + theta_dot_ * time); + tf2::doTransform(target_accel, target_accel, windmill2odom_); + return target_accel; + } + +private: + double theta_; + double theta_dot_; + double radius_; + geometry_msgs::TransformStamped windmill2odom_; +}; 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 46ac66df..210a0e22 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -47,7 +47,7 @@ #include #include #include -#include +#include "rm_gimbal_controllers/bullet_solver/bullet_solver.h" #include #include #include diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 4a9b5450..5449baf0 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -35,7 +35,7 @@ // Created by qiayuan on 8/14/20. // -#include "rm_gimbal_controllers/bullet_solver.h" +#include "rm_gimbal_controllers/bullet_solver/bullet_solver.h" #include #include #include @@ -55,6 +55,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); + windmill_radius_ = getParam(controller_nh, "windmill_radius", 0.2); config_rt_buffer_.initRT(config_); marker_desire_.header.frame_id = "odom"; @@ -100,17 +101,12 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const return resistance_coff; } -bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, +void BulletSolver::input(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; - target_state_ = TargetState{ .current_target_center_pos = pos, - .current_target_center_vel = vel, - .yaw = yaw, - .v_yaw = v_yaw, - .armors_num = armors_num }; double temp_z = pos.z; double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); @@ -119,7 +115,6 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double rough_fly_time = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; double r = r1; - double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; double aim_range_front; double switch_armor_angle = track_target_ ? @@ -135,27 +130,38 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (armors_num == 4 && selected_armor_ != SelectedArmor::FRONT && selected_armor_ != SelectedArmor::BACK) { r = r2; - z = pos.z + dz; + pos.z += dz; } - target_state_.r = r; - target_state_.current_target_center_pos.z = z; - int count{}; - double error = 999; if (track_target_) - { - target_pos_.x = pos.x - r * cos(yaw + static_cast(selected_armor_) * 2 * M_PI / armors_num); - target_pos_.y = pos.y - r * sin(yaw + static_cast(selected_armor_) * 2 * M_PI / armors_num); - } + target_kinematics_.reset(new TrackedTargetKinematics( + pos, vel, yaw + static_cast(selected_armor_) * 2 * M_PI / armors_num, v_yaw, r)); else - { - target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); - target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - } - target_pos_.z = z; + target_kinematics_.reset(new UntrackedTargetKinematic( + pos, vel, yaw + static_cast(selected_armor_) * 2 * M_PI / armors_num, v_yaw, r)); +} + +void BulletSolver::input(double theta, double theta_dot, double bullet_speed, + geometry_msgs::TransformStamped windmill2odom) +{ + bullet_speed_ = bullet_speed; + resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; + track_target_ = true; + target_kinematics_.reset(new WindmillKinematics(theta, theta_dot, windmill_radius_, windmill2odom)); +} + +bool BulletSolver::solve() +{ + config_ = *config_rt_buffer_.readFromRT(); + int count{}; + double error = 999; + + target_pos_ = target_kinematics_->position(0.); + + double temp_z = target_pos_.z; while (error>= 0.001) { output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); - target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); output_pitch_ = std::atan2(temp_z, target_rho); fly_time_ = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; @@ -163,32 +169,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - config_.g * fly_time_ / resistance_coff_; - if (track_target_) - { - target_pos_.x = pos.x + vel.x * fly_time_ - - r * cos(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); - target_pos_.y = pos.y + vel.y * fly_time_ - - r * sin(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); - target_vel_.x = - vel.x + v_yaw * r * sin(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); - target_vel_.y = - vel.y - v_yaw * r * cos(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); - target_accel_.x = - pow(v_yaw, 2) * r * cos(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); - target_accel_.y = - pow(v_yaw, 2) * r * sin(yaw + v_yaw * fly_time_ + static_cast(selected_armor_) * 2 * M_PI / armors_num); - } - else - { - double target_pos_after_fly_time[2]; - target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_; - target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_; - target_pos_.x = - target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - target_pos_.y = - target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - } - target_pos_.z = z + vel.z * fly_time_; + target_pos_ = target_kinematics_->position(fly_time_); double target_yaw = std::atan2(target_pos_.y, target_pos_.x); double error_theta = target_yaw - output_yaw_; @@ -205,16 +186,18 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d void BulletSolver::getYawVelAndAccelDes(double& vel_des, double& accel_des) { + geometry_msgs::Vector3 target_vel = target_kinematics_->velocity(fly_time_); + geometry_msgs::Vector3 target_accel = target_kinematics_->acceleration(fly_time_); double yaw_vel_des = - (target_pos_.x * target_vel_.y - target_pos_.y * target_vel_.x) / (pow(target_pos_.x, 2) + pow(target_pos_.y, 2)); - double yaw_accel_des = (pow(target_pos_.x, 3) * target_accel_.y - pow(target_pos_.y, 3) * target_accel_.x + - 2 * target_pos_.x * target_pos_.y * pow(target_vel_.x, 2) - - 2 * target_pos_.x * target_pos_.y * pow(target_vel_.y, 2) - - pow(target_pos_.x, 2) * target_pos_.y * target_accel_.x + - target_pos_.x * pow(target_pos_.y, 2) * target_accel_.y - - 2 * pow(target_pos_.x, 2) * target_vel_.x * target_vel_.y + - 2 * pow(target_pos_.y, 2) * target_vel_.x * target_vel_.y) / - pow((pow(target_pos_.x, 2) + pow(target_pos_.y, 2)), 2); + (target_pos_.x * target_vel.y - target_pos_.y * target_vel.x) / (pow(target_pos_.x, 2) + pow(target_pos_.y, 2)); + double yaw_accel_des = + (pow(target_pos_.x, 3) * target_accel.y - pow(target_pos_.y, 3) * target_accel.x + + 2 * target_pos_.x * target_pos_.y * pow(target_vel.x, 2) - + 2 * target_pos_.x * target_pos_.y * pow(target_vel.y, 2) - + pow(target_pos_.x, 2) * target_pos_.y * target_accel.x + target_pos_.x * pow(target_pos_.y, 2) * target_accel.y - + 2 * pow(target_pos_.x, 2) * target_vel.x * target_vel.y + + 2 * pow(target_pos_.y, 2) * target_vel.x * target_vel.y) / + pow((pow(target_pos_.x, 2) + pow(target_pos_.y, 2)), 2); vel_des = yaw_vel_des; accel_des = yaw_accel_des; } @@ -222,18 +205,8 @@ void BulletSolver::getYawVelAndAccelDes(double& vel_des, double& accel_des) void BulletSolver::getPitchVelAndAccelDes(double& vel_des, double& accel_des) { double dt = 0.01; - double r = target_state_.r; - double pos_x = target_state_.current_target_center_pos.x + - target_state_.current_target_center_vel.x * (fly_time_ + dt) - - r * cos(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) + - static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); - double pos_y = target_state_.current_target_center_pos.y + - target_state_.current_target_center_vel.y * (fly_time_ + dt) - - r * sin(target_state_.yaw + target_state_.v_yaw * (fly_time_ + dt) + - static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); - double pos_z = - target_state_.current_target_center_pos.z + (fly_time_ + dt) * target_state_.current_target_center_vel.z; - double target_rho = std::sqrt(std::pow(pos_x, 2) + std::pow(pos_y, 2)); + geometry_msgs::Point pos = target_kinematics_->position(fly_time_ + dt); + double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); double temp_z = target_rho * tan(output_pitch_); double output_pitch_next = output_pitch_; double error_z = 999; @@ -245,7 +218,7 @@ void BulletSolver::getPitchVelAndAccelDes(double& vel_des, double& accel_des) double real_z = (bullet_speed_ * std::sin(output_pitch_next) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - error_z = pos_z - real_z; + error_z = pos.z - real_z; temp_z += error_z; } double pitch_vel_des, pitch_accel_des; @@ -309,7 +282,6 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi double BulletSolver::getGimbalError(double yaw_real, double pitch_real) { - double delay = track_target_ ? 0 : config_.delay; double error; if (track_target_) { @@ -325,17 +297,9 @@ double BulletSolver::getGimbalError(double yaw_real, double pitch_real) } else { + double delay = config_.delay; geometry_msgs::Point target_pos_after_fly_time_and_delay{}; - target_pos_after_fly_time_and_delay.x = - target_state_.current_target_center_pos.x + target_state_.current_target_center_vel.x * (fly_time_ + delay) - - target_state_.r * cos(target_state_.yaw + target_state_.v_yaw * (fly_time_ + delay) + - static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); - target_pos_after_fly_time_and_delay.y = - target_state_.current_target_center_pos.y + target_state_.current_target_center_vel.y * (fly_time_ + delay) - - target_state_.r * sin(target_state_.yaw + target_state_.v_yaw * (fly_time_ + delay) + - static_cast(selected_armor_) * 2 * M_PI / target_state_.armors_num); - target_pos_after_fly_time_and_delay.z = - target_state_.current_target_center_pos.z + target_state_.current_target_center_vel.z * (fly_time_ + delay); + target_pos_after_fly_time_and_delay = target_kinematics_->position(fly_time_ + delay); error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 2b68dc37..9b3b6f5c 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -41,8 +41,6 @@ #include #include #include -#include -#include namespace rm_gimbal_controllers { @@ -222,33 +220,41 @@ void Controller::track(const ros::Time& time) quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_compute = yaw_real; double pitch_compute = -pitch_real; - geometry_msgs::Point target_pos = data_track_.position; - geometry_msgs::Vector3 target_vel = data_track_.velocity; + + ros::Time now = ros::Time::now(); + double yaw = data_track_.yaw + data_track_.v_yaw * (now - data_track_.header.stamp).toSec(); + geometry_msgs::TransformStamped transform; try { if (!data_track_.header.frame_id.empty()) { - geometry_msgs::TransformStamped transform = - robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); + transform = robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); } } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); } - ros::Time now = ros::Time::now(); - target_pos.x += target_vel.x * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; - target_pos.y += target_vel.y * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; - target_pos.z += target_vel.z * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - double yaw = data_track_.yaw + data_track_.v_yaw * (now - data_track_.header.stamp).toSec(); - bool solve_success = - bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, - data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); + if (data_track_.id != 12) + { + geometry_msgs::Point target_pos = data_track_.position; + geometry_msgs::Vector3 target_vel = data_track_.velocity; + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + target_pos.x += target_vel.x * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; + target_pos.y += target_vel.y * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; + target_pos.z += target_vel.z * (now - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + bullet_solver_->input(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, + data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); + } + else + { + bullet_solver_->input(yaw, data_track_.v_yaw, cmd_gimbal_.bullet_speed, transform); + } + bool solve_success = bullet_solver_->solve(); if (publish_rate_> 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) {

AltStyle によって変換されたページ (->オリジナル) /