hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial Commit (tested training, testing, and TRT conversion)

Browse Source
This commit is contained in:
Lu Junjie
2024-10-20 17:01:07 +08:00
parent 86d2f311f8
commit 5738088bae
221 changed files with 59249 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

375
flightlib/src/grad_traj_optimization/grad_traj_optimizer.cpp Normal file
View File

@@ -0,0 +1,375 @@
#include "flightlib/grad_traj_optimization/grad_traj_optimizer.h"
GradTrajOptimizer::GradTrajOptimizer(const YAML::Node &cfg) {
//-------------------------get parameter from server--------------------
this->w_smooth = cfg["ws"].as<double>();
this->w_goal = cfg["wg"].as<double>();
this->w_long = cfg["wl"].as<double>();
this->w_vel = cfg["wv"].as<double>();
this->w_acc = cfg["wa"].as<double>();
this->w_collision = cfg["wc"].as<double>();
this->alpha = cfg["alpha"].as<double>();
this->d0 = cfg["d0"].as<double>();
this->r = cfg["r"].as<double>();
this->alphav = cfg["alphav"].as<double>();
this->v0 = cfg["v0"].as<double>();
this->rv = cfg["rv"].as<double>();
this->alphaa = cfg["alphaa"].as<double>();
this->a0 = cfg["a0"].as<double>();
this->ra = cfg["ra"].as<double>();
this->sgm_time = 2 * cfg["radio_range"].as<double>() / cfg["vel_max"].as<double>();
//------------------------generate optimization dependency------------------
Time = Eigen::VectorXd::Zero(1);
Time(0) = sgm_time;
TrajectoryGenerator generator;
generator.QPGeneration(Time);
R = generator.getR();
Rff = generator.getRff();
Rpp = generator.getRpp();
Rpf = generator.getRpf();
Rfp = generator.getRfp();
L = generator.getL();
A = generator.getA();
C = generator.getC();
int m = Time.size(); // number of segments in the trajectory
Dp = Eigen::MatrixXd::Zero(3, m * 3); // optimized x_pva, y_pva, z_pva (end state)
Df = Eigen::MatrixXd::Zero(3, m * 3); // fixed x_pva, y_pva, z_pva (init state)
V = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 5; ++i)
V(i, i + 1) = i + 1;
num_dp = Dp.cols();
num_df = Df.cols();
num_point = Time.rows() + 1;
boundary = Eigen::VectorXd::Zero(6);
}
void GradTrajOptimizer::setSignedDistanceField(std::shared_ptr<sdf_tools::SignedDistanceField> s, double res) {
this->sdf = s;
this->resolution = res;
}
void GradTrajOptimizer::constrains(double &n, double min, double max) const {
if (n > max)
n = max;
else if (n < min)
n = min;
}
void GradTrajOptimizer::setGoal(Eigen::Vector3d goal) { this->goal = goal; }
void GradTrajOptimizer::setBoundary(Eigen::Vector3d min, Eigen::Vector3d max) {
this->map_boundary_min = min;
this->map_boundary_max = max;
boundary(0) = map_boundary_min(0);
boundary(1) = map_boundary_max(0);
boundary(2) = map_boundary_min(1);
boundary(3) = map_boundary_max(1);
boundary(4) = map_boundary_min(2);
boundary(5) = map_boundary_max(2);
}
void GradTrajOptimizer::getCoefficientFromDerivative(Eigen::MatrixXd &coefficient, const std::vector<double> &_dp) const {
coefficient.resize(num_point - 1, 18);
for (int i = 0; i < 3; ++i) {
//-----------------------merge df and dp -> d(df,dp)-----------------------
Eigen::VectorXd df(num_df);
Eigen::VectorXd dp(num_dp);
Eigen::VectorXd d(num_df + num_dp);
df = Df.row(i);
for (int j = 0; j < num_dp; j++) {
dp(j) = _dp[j + num_dp * i];
}
d.segment(0, 3) = df;
d.segment(3, num_dp) = dp;
// ----------------------convert derivative to coefficient------------------
Eigen::VectorXd coe(6 * (num_point - 1));
coe = L * d;
for (int j = 0; j < (num_point - 1); j++) {
coefficient.block(j, 6 * i, 1, 6) = coe.segment(6 * j, 6).transpose();
}
}
}
void GradTrajOptimizer::getCostAndGradient(const std::vector<double> &df, const std::vector<double> &dp, double &cost,
std::vector<double> &_grad) const {
cost = 0;
double cost_smooth = 0;
double cost_colli = 0;
double cost_goal = 0;
double cost_long = 0;
double cost_vel = 0; // deprecated
double cost_acc = 0; // deprecated
Eigen::MatrixXd gradient = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_smooth = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_colli = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_goal = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_long = Eigen::MatrixXd::Zero(3, num_dp);
Eigen::MatrixXd g_vel = Eigen::MatrixXd::Zero(3, num_dp); // deprecated
Eigen::MatrixXd g_acc = Eigen::MatrixXd::Zero(3, num_dp); // deprecated
for (int i = 0; i < num_df; ++i) {
Df(0, i) = df[i];
Df(1, i) = df[i + num_df];
Df(2, i) = df[i + 2 * num_df];
}
// ------------------------- 1. get smoothness cost -----------------------------
{
// 平滑度的Cost，基于MinimalSnap，有：Js = d * R * d，其中d = [dF,dP]
Eigen::VectorXd dfx = Df.block(0, 0, 1, 3).transpose();
Eigen::VectorXd dfy = Df.block(1, 0, 1, 3).transpose();
Eigen::VectorXd dfz = Df.block(2, 0, 1, 3).transpose();
Eigen::VectorXd dpx = Eigen::VectorXd::Zero(num_dp);
Eigen::VectorXd dpy = Eigen::VectorXd::Zero(num_dp);
Eigen::VectorXd dpz = Eigen::VectorXd::Zero(num_dp);
for (int i = 0; i < num_dp; ++i) {
dpx(i) = dp[i];
dpy(i) = dp[i + num_dp];
dpz(i) = dp[i + 2 * num_dp];
}
Eigen::VectorXd dx = Eigen::VectorXd::Zero(num_dp + num_df);
Eigen::VectorXd dy = Eigen::VectorXd::Zero(num_dp + num_df);
Eigen::VectorXd dz = Eigen::VectorXd::Zero(num_dp + num_df);
dx.segment(0, 3) = dfx;
dx.segment(3, num_dp) = dpx;
dy.segment(0, 3) = dfy;
dy.segment(3, num_dp) = dpy;
dz.segment(0, 3) = dfz;
dz.segment(3, num_dp) = dpz;
// ------------------- 1.1 get smoothness cost,fs= d'Rd ---------------------
cost_smooth = double(dx.transpose() * R * dx) + double(dy.transpose() * R * dy) + (dz.transpose() * R * dz);
//-------------------- 1.2 get smoothness gradient --------------------------
Eigen::MatrixXd gx_smooth = 2 * Rfp.transpose() * dfx + 2 * Rpp * dpx;
Eigen::MatrixXd gy_smooth = 2 * Rfp.transpose() * dfy + 2 * Rpp * dpy;
Eigen::MatrixXd gz_smooth = 2 * Rfp.transpose() * dfz + 2 * Rpp * dpz;
g_smooth.row(0) = gx_smooth.transpose();
g_smooth.row(1) = gy_smooth.transpose();
g_smooth.row(2) = gz_smooth.transpose();
}
// -------------------------- 2. get collision cost -----------------------------
{
Eigen::MatrixXd coe;
getCoefficientFromDerivative(coe, dp);
Eigen::MatrixXd Ldp(6, num_dp);
// only single-segment polynomial here
for (int s = 0; s < Time.size(); s++) {
Ldp = L.block(6 * s, 3, 6, num_dp);
// discrete time step
double dt = Time(s) / 30.0;
for (double t = 1e-3; t < Time(s); t += dt) {
// get position, velocity
Eigen::Vector3d pos, vel;
getPositionFromCoeff(pos, coe, s, t);
getVelocityFromCoeff(vel, coe, s, t);
// get information from signed distance field
double dist = 0, gd = 0, cd = 0;
Eigen::Vector3d grad;
getDistanceAndGradient(pos, dist, grad); // 在sdf地图中的距离障碍物dist和grad（梯度方向）
getDistancePenalty(dist, cd); // 计算障碍物距离惩罚cost
getDistancePenaltyGradient(dist, gd); // 计算障碍物距离惩罚的梯度值
// time Matrix T
Eigen::MatrixXd T(1, 6);
getTimeMatrix(t, T);
// ------------------------ 2.1 collision cost-------------------------
cost_colli += cd * dt; // 碰撞cost = 障碍物距离惩罚c * 速度norm * 时间间隔
// ------------------ 2.2 gradient of collision cost-------------------
for (int k = 0; k < 3; k++) { // 每一行对应xyz三个轴，一行的各列对应具体轴上对p,v,a的梯度
g_colli.row(k) = g_colli.row(k) + (gd * grad(k) * T * Ldp) * dt;
}
// ---------- 3. Deprecated: get velocity and accleration cost --------
if (0) {
double cv = 0, ca = 0, gv = 0, ga = 0;
Eigen::Vector3d acc;
getAccelerationFromCoeff(acc, coe, s, t);
for (int k = 0; k < 3; k++) {
getVelocityPenalty(vel(k), cv);
cost_vel += cv * dt;
getAccelerationPenalty(acc(k), ca);
cost_acc += ca * dt;
}
for (int k = 0; k < 3; k++) {
getVelocityPenaltyGradient(vel(k), gv);
g_vel.row(k) = g_vel.row(k) + (gv * T * V * Ldp) * dt;
getAccelerationPenaltyGradient(acc(k), ga);
g_acc.row(k) = g_acc.row(k) + (ga * T * V * V * Ldp) * dt;
}
}
}
}
}
// ---------------------------- 4. get goal cost ---------------------------------
// 4.1 make the trajectry longer
Eigen::Vector3d start_pos(df[0], df[num_dp], df[2 * num_dp]);
Eigen::Vector3d end_pos(dp[0], dp[num_dp], dp[2 * num_dp]);
Eigen::Vector3d delta_pos = end_pos - start_pos;
double goal_r = 100; // param can be moved to config
cost_long = exp(-(delta_pos(0) * delta_pos(0) + delta_pos(1) * delta_pos(1)) / goal_r);
g_long(0, 0) = -2 / goal_r * delta_pos(0) * cost_long;
g_long(1, 0) = -2 / goal_r * delta_pos(1) * cost_long;
// 4.2 make the trajectry approach the goal
cost_goal = (end_pos - this->goal).norm() * (end_pos - this->goal).norm();
g_goal(0, 0) = dp[0] - this->goal(0);
g_goal(1, 0) = dp[num_dp] - this->goal(1);
g_goal(2, 0) = dp[2 * num_dp] - this->goal(2);
g_goal = 2 * g_goal;
//------------------------ sum up all cost and gradient -----------------------------
double ws = this->w_smooth, wc = this->w_collision, wg = this->w_goal, wv = this->w_vel, wa = this->w_acc, wl = this->w_long;
cost = ws * cost_smooth + wc * cost_colli + wv * cost_vel + wa * cost_acc + wg * cost_goal + wl * cost_long + 1e-3;
gradient = ws * g_smooth + wc * g_colli + wg * g_goal + wv * g_vel + wa * g_acc + wl * g_long;
// gradient: 3x3 每一行对应xyz三个轴，一行的各列对应具体轴上对p,v,a的梯度
_grad.resize(num_dp * 3);
for (int i = 0; i < num_dp; ++i) {
_grad[i] = gradient(0, i);
_grad[i + num_dp] = gradient(1, i);
_grad[i + 2 * num_dp] = gradient(2, i);
}
// cout << "smooth cost:" << ws * cost_smooth << " collision cost:" << wc * cost_colli << " goal cost:" << wg * cost_goal << endl;
// cout << "smooth grad:" << ws * g_smooth(0) << " collision grad:" << wc * g_colli(0) << " goal grad:" << wg * g_goal(0) << endl;
}
// get position from coefficient
void GradTrajOptimizer::getPositionFromCoeff(Eigen::Vector3d &pos, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
int s = index;
double t = time;
float x = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) + coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
float y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) + coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
float z =
coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) + coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);
pos(0) = x;
pos(1) = y;
pos(2) = z;
}
// get velocity from cofficient
void GradTrajOptimizer::getVelocityFromCoeff(Eigen::Vector3d &vel, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
int s = index;
double t = time;
float vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) + 4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
float vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) + 4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
float vz =
coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) + 4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);
vel(0) = vx;
vel(1) = vy;
vel(2) = vz;
}
// get acceleration from coefficient
void GradTrajOptimizer::getAccelerationFromCoeff(Eigen::Vector3d &acc, const Eigen::MatrixXd &coeff, const int &index, const double &time) const {
int s = index;
double t = time;
float ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) + 20 * coeff(s, 5) * pow(t, 3);
float ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) + 20 * coeff(s, 11) * pow(t, 3);
float az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) + 20 * coeff(s, 17) * pow(t, 3);
acc(0) = ax;
acc(1) = ay;
acc(2) = az;
}
inline void GradTrajOptimizer::getDistancePenalty(const double &d, double &cost) const { cost = this->alpha * exp(-(d - this->d0) / this->r); }
inline void GradTrajOptimizer::getDistancePenaltyGradient(const double &d, double &grad) const {
grad = -(this->alpha / this->r) * exp(-(d - this->d0) / this->r);
}
inline void GradTrajOptimizer::getVelocityPenalty(const double &v, double &cost) const { cost = alphav * exp((abs(v) - v0) / rv); }
inline void GradTrajOptimizer::getVelocityPenaltyGradient(const double &v, double &grad) const { grad = (alphav / rv) * exp((abs(v) - v0) / rv); }
inline void GradTrajOptimizer::getAccelerationPenalty(const double &a, double &cost) const { cost = alphaa * exp((abs(a) - a0) / ra); }
inline void GradTrajOptimizer::getAccelerationPenaltyGradient(const double &a, double &grad) const { grad = (alphaa / ra) * exp((abs(a) - a0) / ra); }
// get distance in signed distance field ,by position query
void GradTrajOptimizer::getDistanceAndGradient(Eigen::Vector3d &pos, double &dist, Eigen::Vector3d &grad) const {
// get sdf directly from sdf_tools
Eigen::Vector3d ori_pos = pos;
// 1、限定在地图边界内 2、后面越界的惩罚回来
constrains(pos(0), map_boundary_min(0), map_boundary_max(0));
constrains(pos(1), map_boundary_min(1), map_boundary_max(1));
constrains(pos(2), map_boundary_min(2), map_boundary_max(2));
std::vector<double> location_gradient_query = this->sdf->GetGradient(pos(0), pos(1), pos(2), true);
grad(0) = location_gradient_query[0];
grad(1) = location_gradient_query[1];
grad(2) = location_gradient_query[2];
std::pair<float, bool> location_sdf_query = this->sdf->GetSafe(pos(0), pos(1), pos(2));
dist = location_sdf_query.first;
// update distance and gradient using boundary
double dtb = getDistanceToBoundary(ori_pos(0), ori_pos(1), ori_pos(2));
// 1. 点在边界内时：把点推向内部; 2. 如果在Boundary外: (dtb<0)梯度是指向Boundary的,同样推向内部
if (dtb < dist) {
dist = dtb;
recaluculateGradient(ori_pos(0), ori_pos(1), ori_pos(2), grad);
}
}
double GradTrajOptimizer::getDistanceToBoundary(const double &x, const double &y, const double &z) const {
double dist_x = std::min(x - boundary(0), boundary(1) - x);
double dist_y = std::min(y - boundary(2), boundary(3) - y);
double dist_z = std::min(z - boundary(4), boundary(5) - z);
double dtb = std::min(dist_x, dist_y);
dtb = std::min(dtb, dist_z);
return dtb;
}
void GradTrajOptimizer::recaluculateGradient(const double &x, const double &y, const double &z, Eigen::Vector3d &grad) const {
double r = this->resolution;
grad(0) = (10 * (GDTB(x + r, y, z) - GDTB(x - r, y, z)) + 3 * (GDTB(x + r, y + r, z) - GDTB(x - r, y + r, z)) +
3 * (GDTB(x + r, y - r, z) - GDTB(x - r, y - r, z))) /
(32 * r);
grad(1) = (10 * (GDTB(x, y + r, z) - GDTB(x, y - r, z)) + 3 * (GDTB(x + r, y + r, z) - GDTB(x + r, y - r, z)) +
3 * (GDTB(x - r, y + r, z) - GDTB(x - r, y - r, z))) /
(32 * r);
grad(2) = (10 * (GDTB(x, y, z + r) - GDTB(x, y, z - r)) + 3 * (GDTB(x, y + r, z + r) - GDTB(x, y + r, z - r)) +
3 * (GDTB(x, y - r, z + r) - GDTB(x, y - r, z - r))) /
(32 * r);
}
void GradTrajOptimizer::getTimeMatrix(const double &t, Eigen::MatrixXd &T) const {
T.resize(1, 6);
T.setZero();
for (int i = 0; i < 6; ++i) {
T(0, i) = pow(t, i);
}
}

145
flightlib/src/grad_traj_optimization/opt_utile.cpp Normal file
View File

@@ -0,0 +1,145 @@
#include "flightlib/grad_traj_optimization/opt_utile.h"
/*
Front-End Guiding Path:
We evenly sample vertical_num * horizon_num * radio_num * vel_num primitives here with different position, length, and velocity direction.
But in practical, only vertical_num * horizon_num primitives are sampled (radio_num = vel_num = 1).
*/
void getLatticeGuiding(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &lattice_nodes, int horizon_num, int vertical_num, int radio_num,
int vel_num, double horizon_fov, double vertical_fov, double radio_range, double vel_fov, double vel_prefile) {
double direction_diff, altitude_diff, radio_diff, vel_dir_diff;
if (horizon_num == 1)
direction_diff = 0;
else
direction_diff = (horizon_fov / 180.0 * M_PI) / (horizon_num - 1);
if (vertical_num == 1)
altitude_diff = 0;
else
altitude_diff = (vertical_fov / 180.0 * M_PI) / (vertical_num - 1);
radio_diff = radio_range / radio_num;
if (vel_num == 1)
vel_dir_diff = 0;
else
vel_dir_diff = (vel_fov / 180.0f * M_PI) / (vel_num - 1);
// if (vel_num == 1) // be 0 looks like better
// vel_prefile = 0.0;
lattice_nodes.clear();
for (int h = 0; h < radio_num; h++) {
for (int i = 0; i < vertical_num; i++) {
for (int j = 0; j < horizon_num; j++) {
for (int k = 0; k < vel_num; k++) {
double search_radio = (h + 1) * radio_diff;
double alpha = -direction_diff * (horizon_num - 1) / 2 + j * direction_diff; // 位置偏航角（从右往左）
double beta = -altitude_diff * (vertical_num - 1) / 2 + i * altitude_diff; // 高度偏移角（从下往上）
double gamma = -vel_dir_diff * (vel_num - 1) / 2 + k * vel_dir_diff; // 速度偏航
Eigen::Vector3d lattice_node_pos(cos(beta) * cos(alpha) * search_radio, cos(beta) * sin(alpha) * search_radio,
sin(beta) * search_radio);
Eigen::Vector3d lattice_node_vel(cos(alpha + gamma) * vel_prefile, sin(alpha + gamma) * vel_prefile, 0.0);
std::pair<Eigen::Vector3d, Eigen::Vector3d> lattice_node(lattice_node_pos, lattice_node_vel);
lattice_nodes.push_back(lattice_node);
}
}
}
}
}
// TODO: 改为硬编码
Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, const Eigen::Vector3d &Vel_init, const Eigen::Vector3d &Acc_init,
const Eigen::Vector3d &Pos_end, const Eigen::Vector3d &Vel_end, const Eigen::Vector3d &Acc_end,
double Time) {
Eigen::MatrixXd PolyCoeff(1, 3 * 6);
Eigen::VectorXd Px(6), Py(6), Pz(6);
const static auto Factorial = [](int x) {
int fac = 1;
for (int i = x; i > 0; i--)
fac = fac * i;
return fac;
};
/* Produce Mapping Matrix A to the entire trajectory. */
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 3; i++) {
A(2 * i, i) = Factorial(i);
for (int j = i; j < 6; j++)
A(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time, j - i);
}
/* Produce the dereivatives in X, Y and Z axis directly. */
Eigen::VectorXd Dx = Eigen::VectorXd::Zero(6);
Eigen::VectorXd Dy = Eigen::VectorXd::Zero(6);
Eigen::VectorXd Dz = Eigen::VectorXd::Zero(6);
Dx(0) = Pos_init(0);
Dy(0) = Pos_init(1);
Dz(0) = Pos_init(2);
Dx(1) = Pos_end(0);
Dy(1) = Pos_end(1);
Dz(1) = Pos_end(2);
Dx(2) = Vel_init(0);
Dy(2) = Vel_init(1);
Dz(2) = Vel_init(2);
Dx(3) = Vel_end(0);
Dy(3) = Vel_end(1);
Dz(3) = Vel_end(2);
Dx(4) = Acc_init(0);
Dy(4) = Acc_init(1);
Dz(4) = Acc_init(2);
Dx(5) = Acc_end(0);
Dy(5) = Acc_end(1);
Dz(5) = Acc_end(2);
Px = A.inverse() * Dx;
Py = A.inverse() * Dy;
Pz = A.inverse() * Dz;
PolyCoeff.block(0, 0, 1, 6) = Px.segment(0, 6).transpose();
PolyCoeff.block(0, 6, 1, 6) = Py.segment(0, 6).transpose();
PolyCoeff.block(0, 12, 1, 6) = Pz.segment(0, 6).transpose();
return PolyCoeff;
}
void getPositionFromCoeff(Eigen::Vector3d &pos, Eigen::MatrixXd coeff, int index, double time) {
int s = index;
double t = time;
float x = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) + coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
float y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) + coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
float z =
coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) + coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);
pos(0) = x;
pos(1) = y;
pos(2) = z;
}
void getVelocityFromCoeff(Eigen::Vector3d &vel, Eigen::MatrixXd coeff, int index, double time) {
int s = index;
double t = time;
float vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) + 4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
float vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) + 4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
float vz =
coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) + 4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);
vel(0) = vx;
vel(1) = vy;
vel(2) = vz;
}
void getAccelerationFromCoeff(Eigen::Vector3d &acc, Eigen::MatrixXd coeff, int index, double time) {
int s = index;
double t = time;
float ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) + 20 * coeff(s, 5) * pow(t, 3);
float ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) + 20 * coeff(s, 11) * pow(t, 3);
float az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) + 20 * coeff(s, 17) * pow(t, 3);
acc(0) = ax;
acc(1) = ay;
acc(2) = az;
}

125
flightlib/src/grad_traj_optimization/qp_generator.cpp Normal file
View File

@@ -0,0 +1,125 @@
/*
Currently, only 5-order single-segment polynomial is used,
but the functionality for piecewise polynomials is retained (i.e. m, num_f, num_p and other variables).
*/
#include "flightlib/grad_traj_optimization/qp_generator.h"
#include <stdio.h>
#include <fstream>
#include <iostream>
#include <string>
using namespace std;
using namespace Eigen;
TrajectoryGenerator::TrajectoryGenerator() {}
TrajectoryGenerator::~TrajectoryGenerator() {}
void TrajectoryGenerator::QPGeneration(const Eigen::VectorXd &Time) {
m = Time.size();
// 阶乘
const static auto Factorial = [](int x) {
int fac = 1;
for (int i = x; i > 0; i--)
fac = fac * i;
return fac;
};
/* Produce Mapping Matrix A to the entire trajectory. */
MatrixXd Ab; // 每一段矩阵的A（论文中M）
MatrixXd A = MatrixXd::Zero(m * 6, m * 6); // m是段数
// Ab 的组成为6行，第1行Tk位置，第二行Tk+1位置，第三行Tk速度，第四行Tk+1速度，五六为加速度
// 采用5次多项式，所以每段轨迹有6个多项式系数（列）
for (int k = 0; k < m; k++) {
Ab = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 3; i++) {
Ab(2 * i, i) = Factorial(i);
for (int j = i; j < 6; j++)
Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
}
A.block(k * 6, k * 6, 6, 6) = Ab;
}
_A = A;
/* Produce the Minimum Snap cost function, the Hessian Matrix */
MatrixXd H = MatrixXd::Zero(m * 6, m * 6);
// min snap 的cost function（四阶导数的积分 和 系数的关系 的矩阵），论文中间的矩阵Q
for (int k = 0; k < m; k++) {
for (int i = 3; i < 6; i++) {
for (int j = 3; j < 6; j++) {
H(k * 6 + i, k * 6 + j) = i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
}
}
}
_Q = H; // Only minumum snap term is considered here inf the Hessian matrix
StackOptiDep();
}
void TrajectoryGenerator::StackOptiDep() {
double num_f = 3; // 4 + 4 : only start and target position has fixed derivatives
double num_p = 3 * m; // All other derivatives are free
double num_d = 6 * m;
MatrixXd Ct;
Ct = MatrixXd::Zero(num_d, num_f + num_p);
Ct(0, 0) = 1;
Ct(2, 1) = 1;
Ct(4, 2) = 1; // Stack the start point
Ct(6 * (m - 1) + 1, 3) = 1; // Stack the end point
Ct(6 * (m - 1) + 3, 4) = 1;
Ct(6 * (m - 1) + 5, 5) = 1;
if (m > 1) {
Ct(1, 6) = 1;
Ct(3, 7) = 1;
Ct(5, 8) = 1;
Ct(6 * (m - 1) + 0, 3 * m + 0) = 1;
Ct(6 * (m - 1) + 2, 3 * m + 1) = 1;
Ct(6 * (m - 1) + 4, 3 * m + 2) = 1;
for (int j = 2; j < m; j++) {
Ct(6 * (j - 1) + 0, 6 + 3 * (j - 2) + 0) = 1;
Ct(6 * (j - 1) + 1, 6 + 3 * (j - 1) + 0) = 1;
Ct(6 * (j - 1) + 2, 6 + 3 * (j - 2) + 1) = 1;
Ct(6 * (j - 1) + 3, 6 + 3 * (j - 1) + 1) = 1;
Ct(6 * (j - 1) + 4, 6 + 3 * (j - 2) + 2) = 1;
Ct(6 * (j - 1) + 5, 6 + 3 * (j - 1) + 2) = 1;
}
}
_C = Ct.transpose();
_L = _A.inverse() * Ct;
MatrixXd B = _A.inverse();
_R = _C * B.transpose() * _Q * (_A.inverse()) * Ct;
_Rff.resize(3, 3);
_Rfp.resize(3, 3 * m);
_Rpf.resize(3 * m, 3);
_Rpp.resize(3 * m, 3 * m);
_Rff = _R.block(0, 0, 3, 3);
_Rfp = _R.block(0, 3, 3, 3 * m);
_Rpf = _R.block(3, 0, 3 * m, 3);
_Rpp = _R.block(3, 3, 3 * m, 3 * m);
}
Eigen::MatrixXd TrajectoryGenerator::getA() { return _A; }
Eigen::MatrixXd TrajectoryGenerator::getQ() { return _Q; }
Eigen::MatrixXd TrajectoryGenerator::getC() { return _C; }
Eigen::MatrixXd TrajectoryGenerator::getL() { return _L; }
Eigen::MatrixXd TrajectoryGenerator::getR() { return _R; }
Eigen::MatrixXd TrajectoryGenerator::getRff() { return _Rff; }
Eigen::MatrixXd TrajectoryGenerator::getRpp() { return _Rpp; }
Eigen::MatrixXd TrajectoryGenerator::getRpf() { return _Rpf; }
Eigen::MatrixXd TrajectoryGenerator::getRfp() { return _Rfp; }

235
flightlib/src/grad_traj_optimization/traj_optimization_bridge.cpp Normal file
View File

@@ -0,0 +1,235 @@
#include "flightlib/grad_traj_optimization/traj_optimization_bridge.h"
namespace traj_opt {
std::shared_ptr<sdf_tools::SignedDistanceField> SdfConstruction(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector3d &map_boundary_min_sdf,
Eigen::Vector3d &map_boundary_max_sdf) {
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
// sdf collision map parameter
const double x_size = max.x - min.x;
const double z_size = max.z - min.z;
const double y_size = max.y - min.y;
Eigen::Translation3d origin_translation(min.x, min.y, min.z);
Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
const Eigen::Isometry3d origin_transform = origin_translation * origin_rotation;
const std ::string frame = "world";
map_boundary_min_sdf = Eigen::Vector3d(min.x, min.y, min.z);
map_boundary_max_sdf = Eigen::Vector3d(max.x, max.y, max.z);
// create map
sdf_tools ::COLLISION_CELL cell;
cell.occupancy = 0.0;
cell.component = 0;
const sdf_tools::COLLISION_CELL oob_cell = cell;
double resolution_sdf = 0.2;
sdf_tools::CollisionMapGrid collision_map(origin_transform, frame, resolution_sdf, x_size, y_size, z_size, oob_cell);
// add obstacles set in launch file
std::cout << "Generate map..." << std::endl;
sdf_tools::COLLISION_CELL obstacle_cell(1.0);
// add the generated obstacles into collision map （flightmare点云直接建图行列偶尔有全空的情况, 但不影响）
// 点云分辨率改为0.1地图分辨率0.2，就不存在空行的问题; 地图分辨率0.2时，用pt.x + 0.001, pt.y + 0.001, pt.z + 0.001可避免空行且不会造成地图偏移
for (int i = 0; i < cloud->points.size(); i++) {
pcl::PointXYZ pt = cloud->points[i];
collision_map.Set(pt.x, pt.y, pt.z, obstacle_cell);
}
// Build the signed distance field
float oob_value = INFINITY;
std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> sdf_with_extrema = collision_map.ExtractSignedDistanceField(oob_value);
sdf_tools::SignedDistanceField sdf_for_traj_optimization = sdf_with_extrema.first;
cout << "Signed Distance Field Build!" << endl;
return std::make_shared<sdf_tools::SignedDistanceField>(sdf_for_traj_optimization);
}
} // namespace traj_opt
TrajOptimizationBridge::TrajOptimizationBridge() {
std::string cfg_path = getenv("FLIGHTMARE_PATH") + std::string("/flightlib/configs/traj_opt.yaml");
cfg_ = YAML::LoadFile(cfg_path);
loadParam(cfg_);
resolution = 0.2; // Must be the same as the map in SdfConstruction()
df_.resize(9);
dp_.resize(9);
getLatticeGuiding(lattice_nodes, horizon_num, vertical_num, radio_num, vel_num, horizon_fov, vertical_fov, radio_range, vel_fov, vel_prefile);
}
TrajOptimizationBridge::~TrajOptimizationBridge() {}
// Explanation: In grad_traj_optimization of the current project, dp refers to the end_state, and df refers to the initial_state
void TrajOptimizationBridge::getCostAndGradient(const std::vector<double> &dp, int id, double &cost, std::vector<double> &grad) {
// dp: the predicted X_pva, Y_pva, Z_pva in Body Frame
if (dp_.size() != dp.size()) {
std::cout << "Error: size of dp dose not match !" << std::endl;
return;
}
std::vector<double> dp_b = dp;
// Transform to world frame.
Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
for (int i = 0; i < 3; i++) {
Pb(i) = dp_b[3 * i];
Vb(i) = dp_b[3 * i + 1];
Ab(i) = dp_b[3 * i + 2];
}
Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
Pw = Rwb * Pb + pos_;
Vw = Rwb * Vb;
Aw = Rwb * Ab;
for (int i = 0; i < 3; i++) {
dp_[3 * i] = Pw(i);
dp_[3 * i + 1] = Vw(i);
dp_[3 * i + 2] = Aw(i);
}
// ----------------------------main optimization procedure--------------------------
GradTrajOptimizer grad_traj_opt(cfg_);
grad_traj_opt.setSignedDistanceField(sdf_, resolution);
grad_traj_opt.setBoundary(map_boundary_min_, map_boundary_max_);
grad_traj_opt.setGoal(goal_);
double cost_;
std::vector<double> grad_w, grad_b;
grad_traj_opt.getCostAndGradient(df_, dp_, cost_, grad_w);
Eigen::Vector3d grad_pb, grad_vb, grad_ab, grad_pw, grad_vw, grad_aw;
for (int i = 0; i < 3; i++) {
grad_pw(i) = grad_w[3 * i];
grad_vw(i) = grad_w[3 * i + 1];
grad_aw(i) = grad_w[3 * i + 2];
}
grad_pb = Rwb.transpose() * grad_pw;
grad_vb = Rwb.transpose() * grad_vw;
grad_ab = Rwb.transpose() * grad_aw;
grad_b.resize(grad_w.size());
for (int i = 0; i < 3; i++) {
grad_b[3 * i] = grad_pb(i);
grad_b[3 * i + 1] = grad_vb(i);
grad_b[3 * i + 2] = grad_ab(i);
}
cost = cost_;
grad = grad_b; // x_pva, y_pva, z_pva
}
void TrajOptimizationBridge::setMap(std::shared_ptr<sdf_tools::SignedDistanceField> sdf_for_traj_optimization, Eigen::Vector3d &map_boundary_min,
Eigen::Vector3d &map_boundary_max) {
map_boundary_min_ = map_boundary_min;
map_boundary_max_ = map_boundary_max;
sdf_ = sdf_for_traj_optimization;
}
void TrajOptimizationBridge::setState(Eigen::Vector3d pos, Eigen::Quaterniond q, Eigen::Vector3d vel, Eigen::Vector3d acc) {
pos_ = pos;
q_wb_ = q;
vel_ = vel;
acc_ = acc;
for (int i = 0; i < 3; i++) {
df_[3 * i] = pos(i);
df_[3 * i + 1] = vel(i);
df_[3 * i + 2] = acc(i);
}
}
// Explanation: In grad_traj_optimization of the current project, dp refers to the end_state, and df refers to the initial_state
void TrajOptimizationBridge::getNextStateAndCost(const std::vector<double> &dp, double &cost, Eigen::Vector3d &pos, Eigen::Vector3d &vel,
Eigen::Vector3d &acc, double sim_t) {
// dp: xyz_pva (in Body Frame)
if (dp_.size() != dp.size()) {
std::cout << "Error: size of dp dose not match !" << std::endl;
return;
}
std::vector<double> dp_b = dp;
Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
for (int i = 0; i < 3; i++) {
Pb(i) = dp_b[3 * i];
Vb(i) = dp_b[3 * i + 1];
Ab(i) = dp_b[3 * i + 2];
}
Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
Pw = Rwb * Pb + pos_;
Vw = Rwb * Vb;
Aw = Rwb * Ab;
for (int i = 0; i < 3; i++) {
dp_[3 * i] = Pw(i);
dp_[3 * i + 1] = Vw(i);
dp_[3 * i + 2] = Aw(i);
}
GradTrajOptimizer grad_traj_opt(cfg_);
grad_traj_opt.setSignedDistanceField(sdf_, resolution);
grad_traj_opt.setBoundary(map_boundary_min_, map_boundary_max_);
grad_traj_opt.setGoal(goal_);
double cost_;
std::vector<double> grad_w;
grad_traj_opt.getCostAndGradient(df_, dp_, cost_, grad_w); // Df is set here
grad_traj_opt.getCoefficientFromDerivative(pred_coeff_, dp_); // get coefficient by Dp and Df
cost = cost_;
getPositionFromCoeff(pos, pred_coeff_, 0, sim_t);
getVelocityFromCoeff(vel, pred_coeff_, 0, sim_t);
getAccelerationFromCoeff(acc, pred_coeff_, 0, sim_t);
}
/**
set dp and get the coeffs for getNextState() function.
dp is in the world frame because this func only used in real flight and
the prediction must be tramsformed to world frame in python to avoid the attitude inconsistency caused by latency
*/
void TrajOptimizationBridge::solveBVP(const std::vector<double> &dp) {
// dp: xyz_pva given by python（in World Frame, 除了位置没加机身的偏移）
if (dp_.size() != dp.size()) {
std::cout << "Error: size of dp dose not match !" << std::endl;
return;
}
std::vector<double> dp_w = dp;
Eigen::Vector3d Pb, Vb, Ab, Pw, Vw, Aw;
for (int i = 0; i < 3; i++) {
Pw(i) = dp_w[3 * i];
Vw(i) = dp_w[3 * i + 1];
Aw(i) = dp_w[3 * i + 2];
}
Pw = Pw + pos_;
// Eigen::Matrix3d Rwb = q_wb_.toRotationMatrix();
// Pw = Rwb * Pb + pos_;
// Vw = Rwb * Vb;
// Aw = Rwb * Ab;
double traj_time = 2 * cfg_["radio_range"].as<double>() / cfg_["vel_max"].as<double>();
pred_coeff_ = solveCoeffFromBoundaryState(pos_, vel_, acc_, Pw, Vw, Aw, traj_time);
}
// get the next state in world frame
void TrajOptimizationBridge::getNextState(Eigen::Vector3d &pos, Eigen::Vector3d &vel, Eigen::Vector3d &acc, double sim_t) {
getPositionFromCoeff(pos, pred_coeff_, 0, sim_t);
getVelocityFromCoeff(vel, pred_coeff_, 0, sim_t);
getAccelerationFromCoeff(acc, pred_coeff_, 0, sim_t);
}
void TrajOptimizationBridge::setGoal(Eigen::Vector3d goal) {
Eigen::Vector3d goal_dir = (goal - pos_) / (goal - pos_).norm();
Eigen::Vector3d temp_goal = goal;
temp_goal = pos_ + goal_length * goal_dir;
goal_ = temp_goal;
}
void TrajOptimizationBridge::loadParam(YAML::Node &cfg) {
horizon_num = cfg["horizon_num"].as<int>();
vertical_num = cfg["vertical_num"].as<int>();
vel_num = cfg["vel_num"].as<int>();
radio_num = cfg["radio_num"].as<int>();
horizon_fov = cfg["horizon_camera_fov"].as<double>() * (horizon_num - 1) / horizon_num;
vertical_fov = cfg["vertical_camera_fov"].as<double>() * (vertical_num - 1) / vertical_num;
vel_fov = cfg["vel_fov"].as<double>();
radio_range = cfg["radio_range"].as<double>();
vel_prefile = cfg["vel_prefile"].as<double>();
goal_length = cfg["goal_length"].as<double>();
}