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/bullet_solver.h similarity index 82% 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 5ed6d081..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,15 +56,25 @@ struct Config resistance_coff_qd_30, g, delay, dt, timeout; }; +enum class SelectedArmor +{ + FRONT = 0, + LEFT = 1, + BACK = 2, + RIGHT = 3 +}; + 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); - 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); + // 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 { @@ -73,9 +84,8 @@ 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); + 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; @@ -89,10 +99,14 @@ 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_; + double windmill_radius_; + SelectedArmor selected_armor_; bool track_target_; + std::shared_ptr target_kinematics_; geometry_msgs::Point target_pos_{}; double fly_time_; visualization_msgs::Marker marker_desire_; 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/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 1c919927..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,11 +47,13 @@ #include #include #include -#include +#include "rm_gimbal_controllers/bullet_solver/bullet_solver.h" #include #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/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 94188c8a..5449baf0 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -35,10 +35,11 @@ // 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 +#include namespace rm_gimbal_controllers { @@ -54,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"; @@ -99,7 +101,7 @@ 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(); @@ -112,63 +114,62 @@ 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; + pos.z += dz; } - int count{}; - 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_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); - 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)); + 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_; double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * (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_ + 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); - } - 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_; @@ -183,31 +184,51 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d return true; } -void BulletSolver::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 BulletSolver::getYawVelAndAccelDes(double& vel_des, double& accel_des) { - double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_ != 0) - { - r = r2; - z = pos.z + dz; - } - if (track_target_) - { - armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_pos.z = z; - armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_vel.z = vel.z; - } - else + 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); + vel_des = yaw_vel_des; + accel_des = yaw_accel_des; +} + +void BulletSolver::getPitchVelAndAccelDes(double& vel_des, double& accel_des) +{ + double dt = 0.01; + 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; + while (std::abs(error_z)>= 1e-9) { - armor_pos = pos; - armor_pos.z = z; - armor_vel = vel; + 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) @@ -259,26 +280,16 @@ 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) + @@ -286,14 +297,9 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec } else { + double delay = config_.delay; 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_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_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/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 7208e7a9..9b3b6f5c 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -41,42 +41,30 @@ #include #include #include -#include -#include 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 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); - 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.); - - 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.); - 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)) @@ -232,40 +220,47 @@ 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()); } - target_pos.x -= odom2pitch_.transform.translation.x; - target_pos.y -= odom2pitch_.transform.translation.y; - target_pos.z -= 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(); - bool solve_success = - bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.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) { 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(); @@ -360,76 +355,33 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des); double yaw_vel_des = 0., pitch_vel_des = 0.; + double yaw_accel_des = 0., pitch_accel_des = 0.; 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) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - bullet_solver_->getSelectedArmorPosAndVel(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()); - } + 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); 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(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()

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