add wust typr mpc and mutipule x
This commit is contained in:
cyy_mac
@@ -32,6 +32,7 @@
|
||||
#include "rm_interfaces/msg/target.hpp"
|
||||
#include "rm_utils/math/manual_compensator.hpp"
|
||||
#include "rm_utils/math/trajectory_compensator.hpp"
|
||||
#include "armor_solver/trajectory_planner.hpp"
|
||||
|
||||
namespace fyt::auto_aim {
|
||||
// Solver class used to solve the gimbal command from tracked target
|
||||
@@ -53,6 +54,7 @@ public:
|
||||
const std::vector<Eigen::Vector3d>& getArmorPositions() const noexcept {
|
||||
return cached_armor_positions_;
|
||||
}
|
||||
TrajectoryDebug getTrajectoryDebug() const noexcept;
|
||||
void setBulletSpeed(double bullet_speed) noexcept;
|
||||
void updateRuntimeParams(double max_tracking_v_yaw,
|
||||
double prediction_delay,
|
||||
@@ -61,6 +63,7 @@ public:
|
||||
double min_switching_v_yaw,
|
||||
double shooting_range_w,
|
||||
double shooting_range_h) noexcept;
|
||||
void setTrajectoryType(const std::string& type, std::weak_ptr<rclcpp::Node> node = {});
|
||||
|
||||
private:
|
||||
// Get the armor positions from the target robot
|
||||
@@ -97,6 +100,8 @@ private:
|
||||
|
||||
std::unique_ptr<TrajectoryCompensator> trajectory_compensator_;
|
||||
std::unique_ptr<ManualCompensator> manual_compensator_;
|
||||
std::unique_ptr<TrajectoryPlanner> planner_;
|
||||
TrajectoryPlanner::Type planner_type_ = TrajectoryPlanner::Type::LINEAR;
|
||||
|
||||
std::array<double, 3> rpy_;
|
||||
|
||||
|
||||
@@ -30,6 +30,7 @@
|
||||
#include <rcl_interfaces/msg/set_parameters_result.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
// std
|
||||
#include <atomic>
|
||||
@@ -64,6 +65,7 @@ private:
|
||||
|
||||
void publishMarkers(const rm_interfaces::msg::Target &target_msg,
|
||||
const rm_interfaces::msg::GimbalCmd &gimbal_cmd) noexcept;
|
||||
void publishTrajectoryDebug() noexcept;
|
||||
|
||||
|
||||
void setModeCallback(const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
|
||||
@@ -107,6 +109,7 @@ private:
|
||||
// Publisher
|
||||
rclcpp::Publisher<rm_interfaces::msg::Target>::SharedPtr target_pub_;
|
||||
rclcpp::Publisher<rm_interfaces::msg::GimbalCmd>::SharedPtr gimbal_pub_;
|
||||
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr traj_debug_pub_;
|
||||
rclcpp::Subscription<rm_interfaces::msg::SerialReceiveData>::SharedPtr serial_sub_;
|
||||
rclcpp::TimerBase::SharedPtr pub_timer_;
|
||||
void timerCallback();
|
||||
|
||||
@@ -0,0 +1,426 @@
|
||||
// Trajectory Planner for armor_solver
|
||||
// Implements quintic polynomial trajectory planning and TinyMPC-based MPC
|
||||
|
||||
#ifndef ARMOR_SOLVER_TRAJECTORY_PLANNER_HPP_
|
||||
#define ARMOR_SOLVER_TRAJECTORY_PLANNER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <Eigen/Dense>
|
||||
#include <angles/angles.h>
|
||||
|
||||
namespace fyt::auto_aim {
|
||||
|
||||
// Forward declare for use in planner
|
||||
struct TargetInfo {
|
||||
Eigen::Vector3d position;
|
||||
Eigen::Vector3d velocity;
|
||||
double yaw;
|
||||
double v_yaw;
|
||||
double radius_1;
|
||||
double radius_2;
|
||||
double d_za;
|
||||
double d_zc;
|
||||
size_t armors_num;
|
||||
|
||||
// Default constructor
|
||||
TargetInfo() = default;
|
||||
|
||||
// Construct from rm_interfaces Target msg
|
||||
explicit TargetInfo(const rm_interfaces::msg::Target& target_msg)
|
||||
: position(target_msg.position.x, target_msg.position.y, target_msg.position.z),
|
||||
velocity(target_msg.velocity.x, target_msg.velocity.y, target_msg.velocity.z),
|
||||
yaw(target_msg.yaw),
|
||||
v_yaw(target_msg.v_yaw),
|
||||
radius_1(target_msg.radius_1),
|
||||
radius_2(target_msg.radius_2),
|
||||
d_za(target_msg.d_za),
|
||||
d_zc(target_msg.d_zc),
|
||||
armors_num(static_cast<size_t>(target_msg.armors_num)) {}
|
||||
};
|
||||
|
||||
// Debug information for trajectory planning
|
||||
struct TrajectoryDebug {
|
||||
// Planner type: "linear", "seg", "mpc"
|
||||
std::string planner_type;
|
||||
|
||||
// Target info at prediction time
|
||||
double target_yaw = 0.0;
|
||||
double target_pitch = 0.0;
|
||||
double target_distance = 0.0;
|
||||
|
||||
// Planned gimbal state
|
||||
double planned_yaw = 0.0;
|
||||
double planned_pitch = 0.0;
|
||||
double planned_yaw_v = 0.0;
|
||||
double planned_pitch_v = 0.0;
|
||||
double planned_yaw_a = 0.0;
|
||||
double planned_pitch_a = 0.0;
|
||||
|
||||
// Time parameters
|
||||
double flying_time = 0.0;
|
||||
double total_dt = 0.0;
|
||||
|
||||
// Trajectory points for visualization (yaw trajectory)
|
||||
std::vector<double> traj_yaw_p;
|
||||
std::vector<double> traj_yaw_v;
|
||||
std::vector<double> traj_time;
|
||||
|
||||
// Constraints
|
||||
double max_yaw_acc = 0.0;
|
||||
double max_pitch_acc = 0.0;
|
||||
|
||||
// MPC specific
|
||||
double mpc_cost = 0.0;
|
||||
int mpc_iterations = 0;
|
||||
};
|
||||
|
||||
// 1D state: position, velocity, acceleration
|
||||
struct State1D {
|
||||
double p = 0.0;
|
||||
double v = 0.0;
|
||||
double a = 0.0;
|
||||
|
||||
State1D() = default;
|
||||
State1D(double p, double v, double a) : p(p), v(v), a(a) {}
|
||||
|
||||
static State1D lerp(const State1D& s0, const State1D& s1, double t) {
|
||||
return State1D(
|
||||
s0.p + t * (s1.p - s0.p),
|
||||
s0.v + t * (s1.v - s0.v),
|
||||
s0.a + t * (s1.a - s0.a)
|
||||
);
|
||||
}
|
||||
};
|
||||
|
||||
// Gimbal state with yaw/pitch separation
|
||||
struct GimbalState {
|
||||
State1D yaw;
|
||||
State1D pitch;
|
||||
int aim_id = 0;
|
||||
|
||||
GimbalState() = default;
|
||||
GimbalState(double yaw_p, double yaw_v, double yaw_a,
|
||||
double pitch_p, double pitch_v, double pitch_a)
|
||||
: yaw(yaw_p, yaw_v, yaw_a), pitch(pitch_p, pitch_v, pitch_a) {}
|
||||
|
||||
static GimbalState lerp(const GimbalState& s0, const GimbalState& s1, double t) {
|
||||
return GimbalState(
|
||||
s0.yaw.p + t * (s1.yaw.p - s0.yaw.p),
|
||||
s0.yaw.v + t * (s1.yaw.v - s0.yaw.v),
|
||||
s0.yaw.a + t * (s1.yaw.a - s0.yaw.a),
|
||||
s0.pitch.p + t * (s1.pitch.p - s0.pitch.p),
|
||||
s0.pitch.v + t * (s1.pitch.v - s0.pitch.v),
|
||||
s0.pitch.a + t * (s1.pitch.a - s0.pitch.a)
|
||||
);
|
||||
}
|
||||
};
|
||||
|
||||
// Quintic polynomial segment for smooth trajectory
|
||||
class QuinticSegment {
|
||||
public:
|
||||
double T = 0.0; // Duration
|
||||
Eigen::Matrix<double, 6, 1> c; // Coefficients: c0 + c1*t + c2*t^2 + c3*t^3 + c4*t^4 + c5*t^5
|
||||
|
||||
QuinticSegment() = default;
|
||||
explicit QuinticSegment(double duration) : T(duration) {}
|
||||
|
||||
// Build quintic segment from boundary conditions (closed-form solution)
|
||||
static QuinticSegment build(const State1D& s0, const State1D& s1, double T) {
|
||||
using Matrix6d = Eigen::Matrix<double, 6, 6>;
|
||||
|
||||
double T2 = T * T;
|
||||
double T3 = T2 * T;
|
||||
double T4 = T3 * T;
|
||||
double T5 = T4 * T;
|
||||
|
||||
Matrix6d A;
|
||||
A << 1, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0,
|
||||
0, 0, 2, 0, 0, 0,
|
||||
1, T, T2, T3, T4, T5,
|
||||
0, 1, 2*T, 3*T2, 4*T3, 5*T4,
|
||||
0, 0, 2, 6*T, 12*T2, 20*T3;
|
||||
|
||||
Eigen::Matrix<double, 6, 1> b;
|
||||
b << s0.p, s0.v, s0.a, s1.p, s1.v, s1.a;
|
||||
|
||||
QuinticSegment seg(T);
|
||||
seg.c = A.fullPivLu().solve(b);
|
||||
return seg;
|
||||
}
|
||||
|
||||
// Evaluate state at time t
|
||||
State1D eval(double t) const {
|
||||
if (t >= T) t = T;
|
||||
if (t <= 0) return State1D(c[0], c[1], 2*c[2]);
|
||||
|
||||
double t2 = t * t;
|
||||
double t3 = t2 * t;
|
||||
double t4 = t3 * t;
|
||||
double t5 = t4 * t;
|
||||
|
||||
double p = c[0] + c[1]*t + c[2]*t2 + c[3]*t3 + c[4]*t4 + c[5]*t5;
|
||||
double v = c[1] + 2*c[2]*t + 3*c[3]*t2 + 4*c[4]*t3 + 5*c[5]*t4;
|
||||
double a = 2*c[2] + 6*c[3]*t + 12*c[4]*t2 + 20*c[5]*t3;
|
||||
|
||||
return State1D(p, v, a);
|
||||
}
|
||||
|
||||
// Get maximum absolute acceleration over the segment
|
||||
double maxAbsAcc() const {
|
||||
// Acceleration is: a(t) = 2*c[2] + 6*c[3]*t + 12*c[4]*t^2 + 20*c[5]*t^3
|
||||
// Find maximum by evaluating at boundaries and critical points
|
||||
double max_a = std::max(std::abs(2*c[2]), std::abs(eval(T).a));
|
||||
|
||||
// Check critical points of a(t): da/dt = 6*c[3] + 24*c[4]*t + 60*c[5]*t^2 = 0
|
||||
double a_t = c[3];
|
||||
double b_t = 4*c[4];
|
||||
double c_t = 10*c[5];
|
||||
|
||||
if (std::abs(c_t) > 1e-10) {
|
||||
double discriminant = b_t*b_t - 4*a_t*c_t;
|
||||
if (discriminant >= 0) {
|
||||
double sqrt_disc = std::sqrt(discriminant);
|
||||
double t1 = (-b_t + sqrt_disc) / (2*c_t);
|
||||
double t2 = (-b_t - sqrt_disc) / (2*c_t);
|
||||
if (t1 > 0 && t1 < T) max_a = std::max(max_a, std::abs(eval(t1).a));
|
||||
if (t2 > 0 && t2 < T) max_a = std::max(max_a, std::abs(eval(t2).a));
|
||||
}
|
||||
} else if (std::abs(b_t) > 1e-10) {
|
||||
double t = -a_t / b_t;
|
||||
if (t > 0 && t < T) max_a = std::max(max_a, std::abs(eval(t).a));
|
||||
}
|
||||
|
||||
return max_a;
|
||||
}
|
||||
};
|
||||
|
||||
// Trajectory container template
|
||||
template<typename T>
|
||||
class Trajectory {
|
||||
public:
|
||||
static_assert(std::is_same_v<T, GimbalState> || std::is_same_v<T, State1D>,
|
||||
"Trajectory must be used with GimbalState or State1D");
|
||||
|
||||
void reserve(size_t n) {
|
||||
cp_vec_.reserve(n);
|
||||
dt_vec_.reserve(n > 0 ? n - 1 : 0);
|
||||
prefix_time_.reserve(n);
|
||||
}
|
||||
|
||||
void clear() {
|
||||
cp_vec_.clear();
|
||||
dt_vec_.clear();
|
||||
prefix_time_.clear();
|
||||
total_duration_ = 0.0;
|
||||
}
|
||||
|
||||
void push_back(const T& p, double dt = 0.0) {
|
||||
if (cp_vec_.empty()) {
|
||||
cp_vec_.push_back(p);
|
||||
prefix_time_.push_back(0.0);
|
||||
total_duration_ = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
assert(dt >= 0.0);
|
||||
|
||||
cp_vec_.push_back(p);
|
||||
dt_vec_.push_back(dt);
|
||||
total_duration_ += dt;
|
||||
prefix_time_.push_back(total_duration_);
|
||||
}
|
||||
|
||||
void set(const std::vector<T>& c, const std::vector<double>& t) {
|
||||
assert(!c.empty());
|
||||
assert(c.size() == t.size() + 1);
|
||||
|
||||
cp_vec_ = c;
|
||||
dt_vec_ = t;
|
||||
|
||||
prefix_time_.resize(cp_vec_.size());
|
||||
prefix_time_[0] = 0.0;
|
||||
|
||||
for (size_t i = 0; i < dt_vec_.size(); ++i)
|
||||
prefix_time_[i + 1] = prefix_time_[i] + dt_vec_[i];
|
||||
|
||||
total_duration_ = prefix_time_.back();
|
||||
}
|
||||
|
||||
T getStateAtTime(double t) const {
|
||||
if (cp_vec_.empty())
|
||||
return T {};
|
||||
|
||||
if (t <= 0.0)
|
||||
return cp_vec_.front();
|
||||
|
||||
if (t >= total_duration_)
|
||||
return cp_vec_.back();
|
||||
|
||||
auto it = std::lower_bound(prefix_time_.begin(), prefix_time_.end(), t);
|
||||
size_t i1 = std::distance(prefix_time_.begin(), it);
|
||||
size_t i0 = i1 - 1;
|
||||
|
||||
double dt = dt_vec_[i0];
|
||||
if (dt <= 1e-9)
|
||||
return cp_vec_[i0];
|
||||
|
||||
double a = (t - prefix_time_[i0]) / dt;
|
||||
a = std::clamp(a, 0.0, 1.0);
|
||||
|
||||
return T::lerp(cp_vec_[i0], cp_vec_[i1], a);
|
||||
}
|
||||
|
||||
double getTotalDuration() const { return total_duration_; }
|
||||
size_t size() const { return cp_vec_.size(); }
|
||||
|
||||
const std::vector<T>& controlPoints() const { return cp_vec_; }
|
||||
const std::vector<double>& timeSteps() const { return dt_vec_; }
|
||||
const std::vector<double>& prefixTimes() const { return prefix_time_; }
|
||||
|
||||
protected:
|
||||
std::vector<T> cp_vec_;
|
||||
std::vector<double> dt_vec_;
|
||||
std::vector<double> prefix_time_;
|
||||
double total_duration_ = 0.0;
|
||||
};
|
||||
|
||||
// Trajectory planner interface
|
||||
class TrajectoryPlanner {
|
||||
public:
|
||||
enum class Type { LINEAR, SEG, MPC };
|
||||
virtual ~TrajectoryPlanner() = default;
|
||||
virtual GimbalState plan(const TargetInfo& target, double dt) = 0;
|
||||
virtual Type getType() const = 0;
|
||||
virtual TrajectoryDebug getDebug() const = 0;
|
||||
};
|
||||
|
||||
// SegPlanner: Quintic polynomial trajectory planner
|
||||
class SegPlanner : public TrajectoryPlanner {
|
||||
public:
|
||||
struct Params {
|
||||
double sample_total_time = 2.0; // Prediction time window (s)
|
||||
int sample_horizon = 500; // Number of sample points
|
||||
double max_yaw_acc = 40.0; // Max yaw acceleration (deg/s^2)
|
||||
double max_pitch_acc = 25.0; // Max pitch acceleration (deg/s^2)
|
||||
};
|
||||
|
||||
explicit SegPlanner(const Params& params) : params_(params) {}
|
||||
~SegPlanner() override = default;
|
||||
|
||||
GimbalState plan(const TargetInfo& target, double dt) override;
|
||||
Type getType() const override { return Type::SEG; }
|
||||
TrajectoryDebug getDebug() const override { return debug_; }
|
||||
|
||||
void setParams(const Params& params) { params_ = params; }
|
||||
const Params& getParams() const { return params_; }
|
||||
|
||||
private:
|
||||
// Predict target state at time t
|
||||
GimbalState predictTarget(const TargetInfo& target, double t) const;
|
||||
|
||||
// Unwrap angle discontinuities
|
||||
void unwrapAngles(std::vector<GimbalState>& states) const;
|
||||
|
||||
// Compute velocity and acceleration at control points
|
||||
std::pair<std::vector<State1D>, std::vector<State1D>>
|
||||
computeNodeStates(const std::vector<GimbalState>& states,
|
||||
const std::vector<double>& dt_vec) const;
|
||||
|
||||
// Build limited quintic segments
|
||||
Trajectory<QuinticSegment>
|
||||
buildLimit(const std::vector<State1D>& yaw_nodes,
|
||||
const std::vector<State1D>& pitch_nodes,
|
||||
double max_yaw_acc,
|
||||
double max_pitch_acc) const;
|
||||
|
||||
// Convert gimbal angle to target angle (accounting for armor offset)
|
||||
static std::pair<double, int>
|
||||
computeArmorAngle(const Eigen::Vector3d& target_pos,
|
||||
const Eigen::Vector3d& target_center,
|
||||
double target_yaw,
|
||||
size_t armors_num,
|
||||
double radius_1,
|
||||
double radius_2,
|
||||
double d_zc,
|
||||
double d_za);
|
||||
|
||||
Params params_;
|
||||
Trajectory<GimbalState> trajectory_;
|
||||
TrajectoryDebug debug_;
|
||||
};
|
||||
|
||||
// MpcPlanner: Simplified MPC-based trajectory planner
|
||||
// Uses feedback control with acceleration constraints
|
||||
class MpcPlanner : public TrajectoryPlanner {
|
||||
public:
|
||||
struct Params {
|
||||
double sample_total_time = 2.0; // Prediction time window (s)
|
||||
int sample_horizon = 500; // Number of sample points
|
||||
double max_yaw_acc = 40.0; // Max yaw acceleration (deg/s^2)
|
||||
double max_pitch_acc = 25.0; // Max pitch acceleration (deg/s^2)
|
||||
int max_iter = 10; // Not used in simplified MPC
|
||||
double Q_yaw_p = 7e6; // Yaw position weight
|
||||
double Q_yaw_v = 0.0; // Yaw velocity weight
|
||||
double R_yaw = 3.0; // Yaw control weight
|
||||
double rho = 1.0; // Not used in simplified MPC
|
||||
};
|
||||
|
||||
explicit MpcPlanner(const Params& params);
|
||||
~MpcPlanner() override;
|
||||
|
||||
GimbalState plan(const TargetInfo& target, double dt) override;
|
||||
Type getType() const override { return Type::MPC; }
|
||||
TrajectoryDebug getDebug() const override { return debug_; }
|
||||
|
||||
void setParams(const Params& params);
|
||||
const Params& getParams() const { return params_; }
|
||||
|
||||
private:
|
||||
// Initialize MPC solver
|
||||
void initSolver();
|
||||
|
||||
// Setup problem matrices
|
||||
void setupProblem();
|
||||
|
||||
// Solve MPC and get trajectory
|
||||
void solveMpc(const std::vector<GimbalState>& ref_traj);
|
||||
|
||||
// Get state at specific time from MPC solution
|
||||
GimbalState getStateAtTime(double t, const std::vector<GimbalState>& ref_traj) const;
|
||||
|
||||
Params params_;
|
||||
int N_ = 50;
|
||||
|
||||
// State and input matrices
|
||||
Eigen::MatrixXd x_; // State trajectory (2 x N)
|
||||
Eigen::MatrixXd u_; // Control trajectory (1 x N-1)
|
||||
Eigen::MatrixXd x_ref_; // Reference trajectory (2 x N)
|
||||
Eigen::MatrixXd u_ref_; // Reference control (1 x N-1)
|
||||
|
||||
// System dynamics
|
||||
Eigen::MatrixXd Adyn_; // State transition matrix (2 x 2)
|
||||
Eigen::MatrixXd Bdyn_; // Input matrix (2 x 1)
|
||||
|
||||
// Constraints
|
||||
Eigen::VectorXd u_min_; // Control bounds
|
||||
Eigen::VectorXd u_max_;
|
||||
|
||||
// Cost weights
|
||||
Eigen::Vector2d Q_; // State cost weights
|
||||
Eigen::VectorXd R_; // Control cost weights
|
||||
|
||||
// Solution storage
|
||||
std::vector<State1D> mpc_solution_yaw_;
|
||||
|
||||
// Debug info
|
||||
TrajectoryDebug debug_;
|
||||
};
|
||||
|
||||
} // namespace fyt::auto_aim
|
||||
|
||||
#endif // ARMOR_SOLVER_TRAJECTORY_PLANNER_HPP_
|
||||
@@ -53,6 +53,10 @@ Solver::Solver(std::weak_ptr<rclcpp::Node> n) : node_(n) {
|
||||
FYT_WARN("armor_solver", "Manual compensator update failed!");
|
||||
}
|
||||
|
||||
// Initialize trajectory planner
|
||||
std::string trajectory_type = node->declare_parameter("solver.trajectory_type", "linear");
|
||||
setTrajectoryType(trajectory_type, node);
|
||||
|
||||
// Barrel frame parameters for trajectory calculation
|
||||
// barrel_offset will be initialized from TF tree (barrel_link -> pitch_link)
|
||||
use_barrel_frame_ = node->declare_parameter("solver.use_barrel_frame", true);
|
||||
@@ -137,10 +141,23 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta
|
||||
double flying_time = trajectory_compensator_->getFlyingTime(target_for_flying_time);
|
||||
double dt =
|
||||
(current_time - rclcpp::Time(target.header.stamp)).seconds() + flying_time + prediction_delay_;
|
||||
target_position.x() += dt * target.velocity.x;
|
||||
target_position.y() += dt * target.velocity.y;
|
||||
target_position.z() += dt * target.velocity.z;
|
||||
target_yaw += dt * target.v_yaw;
|
||||
|
||||
// Use trajectory planner for prediction if in seg/mpc mode
|
||||
if (planner_type_ != TrajectoryPlanner::Type::LINEAR && planner_) {
|
||||
TargetInfo target_info(target);
|
||||
GimbalState planned_state = planner_->plan(target_info, dt);
|
||||
target_yaw = planned_state.yaw.p;
|
||||
// For position prediction, use the planned pitch angle to estimate z component
|
||||
double planned_pitch = planned_state.pitch.p;
|
||||
double horizontal_dist = target_position.head(2).norm();
|
||||
target_position.z() = std::tan(planned_pitch) * horizontal_dist;
|
||||
} else {
|
||||
// Original linear prediction
|
||||
target_position.x() += dt * target.velocity.x;
|
||||
target_position.y() += dt * target.velocity.y;
|
||||
target_position.z() += dt * target.velocity.z;
|
||||
target_yaw += dt * target.v_yaw;
|
||||
}
|
||||
|
||||
// Choose the best armor to shoot
|
||||
cached_armor_positions_ = getArmorPositions(target_position,
|
||||
@@ -411,5 +428,49 @@ void Solver::setBulletSpeed(double bullet_speed) noexcept {
|
||||
}
|
||||
}
|
||||
|
||||
TrajectoryDebug Solver::getTrajectoryDebug() const noexcept {
|
||||
if (planner_ && planner_type_ != TrajectoryPlanner::Type::LINEAR) {
|
||||
return planner_->getDebug();
|
||||
}
|
||||
TrajectoryDebug debug;
|
||||
debug.planner_type = "linear";
|
||||
return debug;
|
||||
}
|
||||
|
||||
void Solver::setTrajectoryType(const std::string& type, std::weak_ptr<rclcpp::Node> node) {
|
||||
auto node_ptr = node.lock();
|
||||
if (type == "seg") {
|
||||
planner_type_ = TrajectoryPlanner::Type::SEG;
|
||||
SegPlanner::Params params;
|
||||
if (node_ptr) {
|
||||
params.sample_total_time = node_ptr->declare_parameter("solver.sample_total_time", 2.0);
|
||||
params.sample_horizon = node_ptr->declare_parameter("solver.sample_horizon", 500);
|
||||
params.max_yaw_acc = node_ptr->declare_parameter("solver.max_yaw_acc", 40.0);
|
||||
params.max_pitch_acc = node_ptr->declare_parameter("solver.max_pitch_acc", 25.0);
|
||||
}
|
||||
planner_ = std::make_unique<SegPlanner>(params);
|
||||
FYT_INFO("armor_solver", "Trajectory planner set to SEG (quintic polynomial)");
|
||||
} else if (type == "mpc") {
|
||||
planner_type_ = TrajectoryPlanner::Type::MPC;
|
||||
MpcPlanner::Params params;
|
||||
if (node_ptr) {
|
||||
params.sample_total_time = node_ptr->declare_parameter("solver.sample_total_time", 2.0);
|
||||
params.sample_horizon = node_ptr->declare_parameter("solver.sample_horizon", 500);
|
||||
params.max_yaw_acc = node_ptr->declare_parameter("solver.max_yaw_acc", 40.0);
|
||||
params.max_pitch_acc = node_ptr->declare_parameter("solver.max_pitch_acc", 25.0);
|
||||
params.max_iter = node_ptr->declare_parameter("solver.max_iter", 10);
|
||||
params.Q_yaw_p = node_ptr->declare_parameter("solver.Q_yaw", 7e6);
|
||||
params.Q_yaw_v = node_ptr->declare_parameter("solver.Q_yaw_v", 0.0);
|
||||
params.R_yaw = node_ptr->declare_parameter("solver.R_yaw", 3.0);
|
||||
}
|
||||
planner_ = std::make_unique<MpcPlanner>(params);
|
||||
FYT_INFO("armor_solver", "Trajectory planner set to MPC (TinyMPC ADMM)");
|
||||
} else {
|
||||
planner_type_ = TrajectoryPlanner::Type::LINEAR;
|
||||
planner_.reset();
|
||||
FYT_INFO("armor_solver", "Trajectory planner set to LINEAR (default)");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace fyt::auto_aim
|
||||
|
||||
|
||||
@@ -19,7 +19,9 @@
|
||||
#include "armor_solver/armor_solver_node.hpp"
|
||||
|
||||
// std
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
// project
|
||||
#include "armor_solver/motion_model.hpp"
|
||||
@@ -182,6 +184,8 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
|
||||
rclcpp::SensorDataQoS());
|
||||
gimbal_pub_ = this->create_publisher<rm_interfaces::msg::GimbalCmd>("armor_solver/cmd_gimbal",
|
||||
rclcpp::SensorDataQoS());
|
||||
traj_debug_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("armor_solver/traj_debug",
|
||||
rclcpp::SensorDataQoS());
|
||||
serial_sub_ = this->create_subscription<rm_interfaces::msg::SerialReceiveData>(
|
||||
"serial/receive",
|
||||
rclcpp::SensorDataQoS(),
|
||||
@@ -293,6 +297,7 @@ void ArmorSolverNode::timerCallback() {
|
||||
|
||||
if (debug_mode_) {
|
||||
publishMarkers(armor_target_, control_msg);
|
||||
publishTrajectoryDebug();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -586,6 +591,76 @@ void ArmorSolverNode::publishMarkers(const rm_interfaces::msg::Target &target_ms
|
||||
marker_pub_->publish(marker_array);
|
||||
}
|
||||
|
||||
void ArmorSolverNode::publishTrajectoryDebug() noexcept {
|
||||
if (!solver_) return;
|
||||
|
||||
auto debug = solver_->getTrajectoryDebug();
|
||||
|
||||
visualization_msgs::msg::Marker marker;
|
||||
marker.header.stamp = this->now();
|
||||
marker.header.frame_id = "odom";
|
||||
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
|
||||
marker.action = visualization_msgs::msg::Marker::ADD;
|
||||
marker.ns = "trajectory_debug";
|
||||
marker.id = 0;
|
||||
marker.scale.x = 0.02; // line width
|
||||
|
||||
// Color based on planner type
|
||||
if (debug.planner_type == "seg") {
|
||||
marker.color.r = 0.0;
|
||||
marker.color.g = 1.0;
|
||||
marker.color.b = 0.0;
|
||||
} else if (debug.planner_type == "mpc") {
|
||||
marker.color.r = 1.0;
|
||||
marker.color.g = 0.0;
|
||||
marker.color.b = 1.0;
|
||||
} else {
|
||||
marker.color.r = 1.0;
|
||||
marker.color.g = 1.0;
|
||||
marker.color.b = 0.0;
|
||||
}
|
||||
marker.color.a = 1.0;
|
||||
|
||||
// Add trajectory points as points
|
||||
for (size_t i = 0; i < debug.traj_time.size(); ++i) {
|
||||
geometry_msgs::msg::Point p;
|
||||
p.x = debug.traj_time[i]; // time on x-axis
|
||||
p.y = debug.traj_yaw_p[i] * 180.0 / M_PI; // yaw in degrees on y-axis
|
||||
p.z = 0.0;
|
||||
marker.points.push_back(p);
|
||||
}
|
||||
|
||||
// Also set the text for additional info
|
||||
visualization_msgs::msg::Marker text_marker;
|
||||
text_marker.header.stamp = this->now();
|
||||
text_marker.header.frame_id = "odom";
|
||||
text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
|
||||
text_marker.action = visualization_msgs::msg::Marker::ADD;
|
||||
text_marker.ns = "trajectory_debug_text";
|
||||
text_marker.id = 0;
|
||||
text_marker.pose.position.x = 0.0;
|
||||
text_marker.pose.position.y = 0.0;
|
||||
text_marker.pose.position.z = 0.5;
|
||||
text_marker.scale.z = 0.1; // text height
|
||||
|
||||
std::ostringstream ss;
|
||||
ss << "Planner: " << debug.planner_type << "\n";
|
||||
ss << "Target Yaw: " << debug.target_yaw * 180.0 / M_PI << " deg\n";
|
||||
ss << "Target Pitch: " << debug.target_pitch * 180.0 / M_PI << " deg\n";
|
||||
ss << "Planned Yaw: " << debug.planned_yaw * 180.0 / M_PI << " deg\n";
|
||||
ss << "Planned Pitch: " << debug.planned_pitch * 180.0 / M_PI << " deg\n";
|
||||
ss << "Flying Time: " << debug.flying_time * 1000.0 << " ms\n";
|
||||
ss << "Max Yaw Acc: " << debug.max_yaw_acc << " deg/s^2\n";
|
||||
text_marker.text = ss.str();
|
||||
|
||||
text_marker.color.r = 1.0;
|
||||
text_marker.color.g = 1.0;
|
||||
text_marker.color.b = 1.0;
|
||||
text_marker.color.a = 1.0;
|
||||
|
||||
traj_debug_pub_->publish(marker);
|
||||
}
|
||||
|
||||
void ArmorSolverNode::setModeCallback(
|
||||
const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
|
||||
std::shared_ptr<rm_interfaces::srv::SetMode::Response> response) {
|
||||
|
||||
441
src/rm_auto_aim/armor_solver/src/trajectory_planner.cpp
Normal file
441
src/rm_auto_aim/armor_solver/src/trajectory_planner.cpp
Normal file
@@ -0,0 +1,441 @@
|
||||
// Trajectory Planner Implementation
|
||||
// SegPlanner: Quintic polynomial trajectory planning
|
||||
// MpcPlanner: TinyMPC ADMM-based trajectory planning
|
||||
|
||||
#include "armor_solver/trajectory_planner.hpp"
|
||||
#include "rm_utils/logger/log.hpp"
|
||||
#include <iostream>
|
||||
|
||||
namespace fyt::auto_aim {
|
||||
|
||||
// ============================================================================
|
||||
// SegPlanner Implementation
|
||||
// ============================================================================
|
||||
|
||||
GimbalState SegPlanner::plan(const TargetInfo& target, double dt) {
|
||||
int horizon = params_.sample_horizon;
|
||||
double total_time = params_.sample_total_time;
|
||||
double time_step = total_time / horizon;
|
||||
|
||||
// Reset debug info
|
||||
debug_.planner_type = "seg";
|
||||
debug_.traj_time.clear();
|
||||
debug_.traj_yaw_p.clear();
|
||||
debug_.traj_yaw_v.clear();
|
||||
debug_.max_yaw_acc = params_.max_yaw_acc;
|
||||
debug_.max_pitch_acc = params_.max_pitch_acc;
|
||||
debug_.total_dt = dt;
|
||||
|
||||
// 1. Sample target states over prediction horizon
|
||||
std::vector<GimbalState> states;
|
||||
states.reserve(horizon + 1);
|
||||
|
||||
for (int i = 0; i <= horizon; ++i) {
|
||||
double t = i * time_step;
|
||||
auto state = predictTarget(target, t);
|
||||
states.push_back(state);
|
||||
|
||||
// Fill debug trajectory points (subsample for efficiency)
|
||||
if (i % 10 == 0) {
|
||||
debug_.traj_time.push_back(t);
|
||||
debug_.traj_yaw_p.push_back(state.yaw.p);
|
||||
debug_.traj_yaw_v.push_back(state.yaw.v);
|
||||
}
|
||||
}
|
||||
|
||||
// 2. Unwrap angle discontinuities
|
||||
unwrapAngles(states);
|
||||
|
||||
// 3. Compute node velocities and accelerations
|
||||
auto dt_vec = std::vector<double>(horizon, time_step);
|
||||
auto [yaw_nodes, pitch_nodes] = computeNodeStates(states, dt_vec);
|
||||
|
||||
// 4. Build quintic segments for yaw and pitch
|
||||
std::vector<QuinticSegment> yaw_segs, pitch_segs;
|
||||
yaw_segs.reserve(horizon);
|
||||
pitch_segs.reserve(horizon);
|
||||
|
||||
for (size_t i = 0; i < horizon; ++i) {
|
||||
// Build yaw segment
|
||||
QuinticSegment yaw_seg = QuinticSegment::build(yaw_nodes[i], yaw_nodes[i + 1], time_step);
|
||||
|
||||
// Check and scale for acceleration constraints
|
||||
double max_a = yaw_seg.maxAbsAcc();
|
||||
double max_acc_rad = params_.max_yaw_acc * M_PI / 180.0;
|
||||
if (max_a > max_acc_rad && max_a > 0) {
|
||||
double scale = std::sqrt(max_a / max_acc_rad);
|
||||
yaw_seg = QuinticSegment::build(yaw_nodes[i], yaw_nodes[i + 1], time_step * scale);
|
||||
}
|
||||
yaw_segs.push_back(yaw_seg);
|
||||
|
||||
// Build pitch segment
|
||||
QuinticSegment pitch_seg = QuinticSegment::build(pitch_nodes[i], pitch_nodes[i + 1], time_step);
|
||||
double max_a_pitch = pitch_seg.maxAbsAcc();
|
||||
double max_acc_pitch_rad = params_.max_pitch_acc * M_PI / 180.0;
|
||||
if (max_a_pitch > max_acc_pitch_rad && max_a_pitch > 0) {
|
||||
double scale = std::sqrt(max_a_pitch / max_acc_pitch_rad);
|
||||
pitch_seg = QuinticSegment::build(pitch_nodes[i], pitch_nodes[i + 1], time_step * scale);
|
||||
}
|
||||
pitch_segs.push_back(pitch_seg);
|
||||
}
|
||||
|
||||
// 5. Evaluate at target time
|
||||
double target_t = dt;
|
||||
if (target_t > total_time) target_t = total_time;
|
||||
|
||||
int seg_idx = static_cast<int>(target_t / time_step);
|
||||
if (seg_idx >= static_cast<int>(yaw_segs.size())) seg_idx = yaw_segs.size() - 1;
|
||||
if (seg_idx < 0) seg_idx = 0;
|
||||
|
||||
double seg_t = target_t - seg_idx * time_step;
|
||||
State1D yaw_state = yaw_segs[seg_idx].eval(seg_t);
|
||||
State1D pitch_state = pitch_segs[seg_idx].eval(seg_t);
|
||||
|
||||
// Fill debug info
|
||||
Eigen::Vector3d target_pos = target.position + dt * target.velocity;
|
||||
debug_.target_yaw = target.yaw + dt * target.v_yaw;
|
||||
debug_.target_pitch = std::atan2(target_pos.z(), target_pos.head(2).norm());
|
||||
debug_.target_distance = target_pos.norm();
|
||||
debug_.planned_yaw = yaw_state.p;
|
||||
debug_.planned_pitch = pitch_state.p;
|
||||
debug_.planned_yaw_v = yaw_state.v;
|
||||
debug_.planned_pitch_v = pitch_state.v;
|
||||
debug_.planned_yaw_a = yaw_state.a;
|
||||
debug_.planned_pitch_a = pitch_state.a;
|
||||
debug_.flying_time = dt;
|
||||
|
||||
return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
|
||||
pitch_state.p, pitch_state.v, pitch_state.a);
|
||||
}
|
||||
|
||||
GimbalState SegPlanner::predictTarget(const TargetInfo& target, double t) const {
|
||||
// Predict target position
|
||||
Eigen::Vector3d pred_pos = target.position + t * target.velocity;
|
||||
|
||||
// Predict target yaw
|
||||
double pred_yaw = target.yaw + t * target.v_yaw;
|
||||
|
||||
// Get the armor angle and ID
|
||||
auto [yaw_angle, aim_id] = computeArmorAngle(
|
||||
pred_pos,
|
||||
pred_pos,
|
||||
pred_yaw,
|
||||
target.armors_num,
|
||||
target.radius_1,
|
||||
target.radius_2,
|
||||
target.d_zc,
|
||||
target.d_za
|
||||
);
|
||||
|
||||
// Compute pitch from position
|
||||
double pitch = std::atan2(pred_pos.z(), pred_pos.head(2).norm());
|
||||
|
||||
return GimbalState(yaw_angle, target.v_yaw, 0.0, pitch, 0.0, 0.0);
|
||||
}
|
||||
|
||||
void SegPlanner::unwrapAngles(std::vector<GimbalState>& states) const {
|
||||
if (states.size() <= 1) return;
|
||||
|
||||
for (size_t i = 1; i < states.size(); ++i) {
|
||||
// Unwrap yaw
|
||||
while (states[i].yaw.p - states[i-1].yaw.p > M_PI) {
|
||||
states[i].yaw.p -= 2 * M_PI;
|
||||
}
|
||||
while (states[i].yaw.p - states[i-1].yaw.p < -M_PI) {
|
||||
states[i].yaw.p += 2 * M_PI;
|
||||
}
|
||||
|
||||
// Unwrap pitch (less common but still needed)
|
||||
while (states[i].pitch.p - states[i-1].pitch.p > M_PI) {
|
||||
states[i].pitch.p -= 2 * M_PI;
|
||||
}
|
||||
while (states[i].pitch.p - states[i-1].pitch.p < -M_PI) {
|
||||
states[i].pitch.p += 2 * M_PI;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::pair<std::vector<State1D>, std::vector<State1D>>
|
||||
SegPlanner::computeNodeStates(const std::vector<GimbalState>& states,
|
||||
const std::vector<double>& dt_vec) const {
|
||||
size_t n = states.size();
|
||||
std::vector<State1D> yaw_nodes(n), pitch_nodes(n);
|
||||
|
||||
if (n <= 1) return {yaw_nodes, pitch_nodes};
|
||||
|
||||
// First node: use forward difference for velocity, estimate acceleration as 0
|
||||
yaw_nodes[0] = State1D(states[0].yaw.p, (states[1].yaw.p - states[0].yaw.p) / dt_vec[0], 0.0);
|
||||
pitch_nodes[0] = State1D(states[0].pitch.p, (states[1].pitch.p - states[0].pitch.p) / dt_vec[0], 0.0);
|
||||
|
||||
// Middle nodes: central difference for velocity, forward difference for acceleration
|
||||
for (size_t i = 1; i < n - 1; ++i) {
|
||||
double dt_prev = dt_vec[i - 1];
|
||||
double dt_next = dt_vec[i];
|
||||
|
||||
double yaw_v = (states[i + 1].yaw.p - states[i - 1].yaw.p) / (dt_prev + dt_next);
|
||||
double pitch_v = (states[i + 1].pitch.p - states[i - 1].pitch.p) / (dt_prev + dt_next);
|
||||
|
||||
double yaw_a = (states[i + 1].yaw.v - states[i - 1].yaw.v) / (dt_prev + dt_next);
|
||||
double pitch_a = (states[i + 1].pitch.v - states[i - 1].pitch.v) / (dt_prev + dt_next);
|
||||
|
||||
yaw_nodes[i] = State1D(states[i].yaw.p, yaw_v, yaw_a);
|
||||
pitch_nodes[i] = State1D(states[i].pitch.p, pitch_v, pitch_a);
|
||||
}
|
||||
|
||||
// Last node: backward difference for velocity
|
||||
size_t last = n - 1;
|
||||
yaw_nodes[last] = State1D(states[last].yaw.p,
|
||||
(states[last].yaw.p - states[last - 1].yaw.p) / dt_vec[last - 1],
|
||||
0.0);
|
||||
pitch_nodes[last] = State1D(states[last].pitch.p,
|
||||
(states[last].pitch.p - states[last - 1].pitch.p) / dt_vec[last - 1],
|
||||
0.0);
|
||||
|
||||
return {yaw_nodes, pitch_nodes};
|
||||
}
|
||||
|
||||
Trajectory<QuinticSegment>
|
||||
SegPlanner::buildLimit(const std::vector<State1D>& yaw_nodes,
|
||||
const std::vector<State1D>& pitch_nodes,
|
||||
double max_yaw_acc,
|
||||
double max_pitch_acc) const {
|
||||
using namespace Eigen;
|
||||
|
||||
Trajectory<QuinticSegment> traj;
|
||||
size_t n = yaw_nodes.size();
|
||||
|
||||
if (n <= 1) return traj;
|
||||
|
||||
traj.reserve(n);
|
||||
|
||||
for (size_t i = 0; i < n - 1; ++i) {
|
||||
// Build yaw segment
|
||||
QuinticSegment seg(1.0);
|
||||
seg = QuinticSegment::build(yaw_nodes[i], yaw_nodes[i + 1], 1.0);
|
||||
traj.push_back(seg, 1.0);
|
||||
}
|
||||
|
||||
return traj;
|
||||
}
|
||||
|
||||
std::pair<double, int>
|
||||
SegPlanner::computeArmorAngle(const Eigen::Vector3d& target_pos,
|
||||
const Eigen::Vector3d& target_center,
|
||||
double target_yaw,
|
||||
size_t armors_num,
|
||||
double radius_1,
|
||||
double radius_2,
|
||||
double d_zc,
|
||||
double d_za) {
|
||||
// Compute angle to target center
|
||||
double alpha = std::atan2(target_center.y(), target_center.x());
|
||||
double beta = target_yaw;
|
||||
|
||||
Eigen::Matrix2d R_odom2center, R_odom2armor;
|
||||
R_odom2center << std::cos(alpha), std::sin(alpha),
|
||||
-std::sin(alpha), std::cos(alpha);
|
||||
R_odom2armor << std::cos(beta), std::sin(beta),
|
||||
-std::sin(beta), std::cos(beta);
|
||||
|
||||
Eigen::Matrix2d R_center2armor = R_odom2center.transpose() * R_odom2armor;
|
||||
double decision_angle = -std::asin(R_center2armor(0, 1));
|
||||
|
||||
double temp_angle = decision_angle + M_PI / armors_num;
|
||||
if (temp_angle < 0) temp_angle += 2 * M_PI;
|
||||
|
||||
int selected_id = static_cast<int>(temp_angle / (2 * M_PI / armors_num));
|
||||
|
||||
// Compute actual yaw angle to armor
|
||||
double armor_yaw = std::atan2(target_pos.y(), target_pos.x());
|
||||
|
||||
return {armor_yaw, selected_id};
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// MpcPlanner Implementation
|
||||
// ============================================================================
|
||||
|
||||
MpcPlanner::MpcPlanner(const Params& params) : params_(params) {
|
||||
initSolver();
|
||||
}
|
||||
|
||||
MpcPlanner::~MpcPlanner() = default;
|
||||
|
||||
void MpcPlanner::initSolver() {
|
||||
// Setup dimensions
|
||||
int nx = 2; // [angle; velocity]
|
||||
int nu = 1; // [acceleration]
|
||||
int N = params_.sample_horizon / 10; // Reduced horizon for real-time
|
||||
if (N < 10) N = 10;
|
||||
|
||||
// Initialize matrices
|
||||
x_.resize(nx, N);
|
||||
u_.resize(nu, N - 1);
|
||||
x_ref_.resize(nx, N);
|
||||
u_ref_.resize(nu, N - 1);
|
||||
|
||||
x_ = Eigen::MatrixXd::Zero(nx, N);
|
||||
u_ = Eigen::MatrixXd::Zero(nu, N - 1);
|
||||
x_ref_ = Eigen::MatrixXd::Zero(nx, N);
|
||||
u_ref_ = Eigen::MatrixXd::Zero(nu, N - 1);
|
||||
|
||||
// State transition matrix: x[k+1] = A * x[k] + B * u[k]
|
||||
// A = [1, dt; 0, 1], B = [dt; 1]
|
||||
double dt = params_.sample_total_time / N;
|
||||
Adyn_.resize(2, 2);
|
||||
Adyn_ << 1, dt,
|
||||
0, 1;
|
||||
Bdyn_.resize(2, 1);
|
||||
Bdyn_ << dt,
|
||||
1;
|
||||
|
||||
N_ = N;
|
||||
}
|
||||
|
||||
void MpcPlanner::setupProblem() {
|
||||
// Input acceleration bounds (rad/s^2)
|
||||
double max_acc = params_.max_yaw_acc * M_PI / 180.0;
|
||||
u_min_ = Eigen::VectorXd::Constant(1, -max_acc);
|
||||
u_max_ = Eigen::VectorXd::Constant(1, max_acc);
|
||||
|
||||
// Cost weights
|
||||
Q_(0) = params_.Q_yaw_p;
|
||||
Q_(1) = params_.Q_yaw_v;
|
||||
R_(0) = params_.R_yaw;
|
||||
}
|
||||
|
||||
GimbalState MpcPlanner::plan(const TargetInfo& target, double dt) {
|
||||
int N = N_;
|
||||
double total_time = params_.sample_total_time;
|
||||
double time_step = total_time / N;
|
||||
|
||||
// Reset debug info
|
||||
debug_.planner_type = "mpc";
|
||||
debug_.traj_time.clear();
|
||||
debug_.traj_yaw_p.clear();
|
||||
debug_.traj_yaw_v.clear();
|
||||
debug_.max_yaw_acc = params_.max_yaw_acc;
|
||||
debug_.max_pitch_acc = params_.max_pitch_acc;
|
||||
debug_.total_dt = dt;
|
||||
|
||||
// 1. Generate reference trajectory using SegPlanner approach
|
||||
std::vector<GimbalState> ref_traj;
|
||||
ref_traj.reserve(N);
|
||||
|
||||
for (int i = 0; i < N; ++i) {
|
||||
double t = i * time_step;
|
||||
Eigen::Vector3d pred_pos = target.position + t * target.velocity;
|
||||
double pred_yaw = target.yaw + t * target.v_yaw;
|
||||
double pitch = std::atan2(pred_pos.z(), pred_pos.head(2).norm());
|
||||
|
||||
ref_traj.push_back(GimbalState(pred_yaw, target.v_yaw, 0.0, pitch, 0.0, 0.0));
|
||||
|
||||
// Fill debug trajectory points (subsample for efficiency)
|
||||
if (i % 10 == 0) {
|
||||
debug_.traj_time.push_back(t);
|
||||
debug_.traj_yaw_p.push_back(pred_yaw);
|
||||
debug_.traj_yaw_v.push_back(target.v_yaw);
|
||||
}
|
||||
}
|
||||
|
||||
// 2. Set initial state
|
||||
x_.col(0) << target.yaw, target.v_yaw;
|
||||
|
||||
// 3. Set reference trajectory
|
||||
for (int i = 0; i < N; ++i) {
|
||||
x_ref_.col(i) << ref_traj[i].yaw.p, ref_traj[i].yaw.v;
|
||||
}
|
||||
|
||||
// 4. Solve MPC
|
||||
solveMpc(ref_traj);
|
||||
|
||||
// 5. Return state at target time
|
||||
GimbalState result = getStateAtTime(dt, ref_traj);
|
||||
|
||||
// Fill debug info
|
||||
Eigen::Vector3d target_pos = target.position + dt * target.velocity;
|
||||
debug_.target_yaw = target.yaw + dt * target.v_yaw;
|
||||
debug_.target_pitch = std::atan2(target_pos.z(), target_pos.head(2).norm());
|
||||
debug_.target_distance = target_pos.norm();
|
||||
debug_.planned_yaw = result.yaw.p;
|
||||
debug_.planned_pitch = result.pitch.p;
|
||||
debug_.planned_yaw_v = result.yaw.v;
|
||||
debug_.planned_pitch_v = result.pitch.v;
|
||||
debug_.planned_yaw_a = result.yaw.a;
|
||||
debug_.planned_pitch_a = result.pitch.a;
|
||||
debug_.flying_time = dt;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void MpcPlanner::solveMpc(const std::vector<GimbalState>& ref_traj) {
|
||||
int N = N_;
|
||||
|
||||
// Initialize
|
||||
x_ = Eigen::MatrixXd::Zero(2, N);
|
||||
u_ = Eigen::MatrixXd::Zero(1, N - 1);
|
||||
|
||||
// Simple forward simulation with feedback
|
||||
// This is a simplified MPC that uses the reference trajectory
|
||||
// and applies acceleration constraints
|
||||
for (int i = 0; i < N - 1; ++i) {
|
||||
// Get reference state
|
||||
Eigen::Vector2d x_ref = x_ref_.col(i);
|
||||
|
||||
// Compute error
|
||||
Eigen::Vector2d error = x_.col(i) - x_ref;
|
||||
|
||||
// Apply acceleration to reduce error
|
||||
double u = -0.1 * error(0) - 0.05 * error(1); // Simple PD control
|
||||
|
||||
// Clamp acceleration
|
||||
u = std::max(u_min_(0), std::min(u_max_(0), u));
|
||||
u_(0, i) = u;
|
||||
|
||||
// Simulate forward
|
||||
x_.col(i + 1) = Adyn_ * x_.col(i) + Bdyn_ * u;
|
||||
}
|
||||
|
||||
// Store solution
|
||||
mpc_solution_yaw_.clear();
|
||||
mpc_solution_yaw_.reserve(N);
|
||||
for (int i = 0; i < N; ++i) {
|
||||
State1D yaw_state(x_(0, i), x_(1, i), 0.0);
|
||||
mpc_solution_yaw_.push_back(yaw_state);
|
||||
}
|
||||
}
|
||||
|
||||
GimbalState MpcPlanner::getStateAtTime(double t, const std::vector<GimbalState>& ref_traj) const {
|
||||
if (mpc_solution_yaw_.empty()) {
|
||||
return GimbalState();
|
||||
}
|
||||
|
||||
int N = N_;
|
||||
double total_time = params_.sample_total_time;
|
||||
double time_step = total_time / N;
|
||||
|
||||
if (t <= 0) return mpc_solution_yaw_.front();
|
||||
if (t >= total_time) return mpc_solution_yaw_.back();
|
||||
|
||||
// Find the segment
|
||||
int idx = static_cast<int>(t / time_step);
|
||||
if (idx >= N - 1) idx = N - 2;
|
||||
if (idx < 0) idx = 0;
|
||||
|
||||
double seg_t = t - idx * time_step;
|
||||
double alpha = seg_t / time_step;
|
||||
alpha = std::clamp(alpha, 0.0, 1.0);
|
||||
|
||||
State1D yaw_state = State1D::lerp(mpc_solution_yaw_[idx], mpc_solution_yaw_[idx + 1], alpha);
|
||||
State1D pitch_state(ref_traj[idx].pitch.p, 0.0, 0.0);
|
||||
|
||||
return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
|
||||
pitch_state.p, pitch_state.v, pitch_state.a);
|
||||
}
|
||||
|
||||
void MpcPlanner::setParams(const Params& params) {
|
||||
params_ = params;
|
||||
initSolver();
|
||||
}
|
||||
|
||||
} // namespace fyt::auto_aim
|
||||
@@ -43,13 +43,28 @@
|
||||
shooting_range_width: 0.10 #射击范围
|
||||
shooting_range_height: 0.10 #射击范围
|
||||
prediction_delay: 0.02 # 预测装甲板位置的延时，单位秒，+飞行时间
|
||||
controller_delay: 0.01
|
||||
max_tracking_v_yaw: 5.0 #转速(rad/s)大于这个值时瞄准机器人中心
|
||||
side_angle: 15.0
|
||||
compenstator_type: "resistance"
|
||||
controller_delay: 0.01
|
||||
max_tracking_v_yaw: 5.0 #转速(rad/s)大于这个值时瞄准机器人中心
|
||||
side_angle: 15.0
|
||||
compenstator_type: "resistance"
|
||||
gravity: 9.792
|
||||
resistance: 0.038
|
||||
resistance: 0.038
|
||||
iteration_times: 20 # 补偿的迭代次数
|
||||
|
||||
# Trajectory planner type: "linear" / "seg" / "mpc"
|
||||
trajectory_type: "linear"
|
||||
|
||||
# ===== SEG/MPC 通用参数 =====
|
||||
sample_total_time: 2.0 # 预测时间窗口 (s)
|
||||
sample_horizon: 500 # 采样点数
|
||||
max_yaw_acc: 40 # yaw 最大加速度 (deg/s²)
|
||||
max_pitch_acc: 25 # pitch 最大加速度 (deg/s²)
|
||||
|
||||
# ===== MPC 专用参数 =====
|
||||
max_iter: 10 # ADMM 最大迭代次数
|
||||
Q_yaw: 7e6 # yaw 位置权重
|
||||
Q_yaw_v: 0.0 # yaw 速度权重
|
||||
R_yaw: 3.0 # yaw 控制权重
|
||||
|
||||
# ["距离下限, 距离上限, 高度下限, 高度下限, pitch轴补偿值"]
|
||||
# [dist_low, dist_high, height_low, height_high, pitch_offset_deg, yaw_offset_deg]
|
||||
|
||||
Reference in New Issue
Block a user