Compute gimbal pitch vel desire and accel desire.

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,13 +98,16 @@ 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_;
 
   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_;

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)

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);