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

Minor modification on test

Browse Source
This commit is contained in:
Lu Junjie 2024-10-21 15:01:19 +08:00
parent 6a7aba86dc
commit 455db17f60
1 changed files with 34 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
flightlib/src/grad_traj_optimization/opt_utile.cpp
View File

@ -44,13 +44,11 @@ void getLatticeGuiding(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>
} }
} }
// TODO: 改为硬编码 Eigen::MatrixXd getAInverse(double Time) {
Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, const Eigen::Vector3d &Vel_init, const Eigen::Vector3d &Acc_init, static Eigen::MatrixXd A_inv = Eigen::MatrixXd::Zero(6, 6);
const Eigen::Vector3d &Pos_end, const Eigen::Vector3d &Vel_end, const Eigen::Vector3d &Acc_end, static double previousTime = -1.0;
double Time) {
Eigen::MatrixXd PolyCoeff(1, 3 * 6);
Eigen::VectorXd Px(6), Py(6), Pz(6);
if (Time != previousTime) {
const static auto Factorial = [](int x) { const static auto Factorial = [](int x) {
int fac = 1; int fac = 1;
for (int i = x; i > 0; i--) for (int i = x; i > 0; i--)
@ -58,13 +56,28 @@ Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, con
return fac; return fac;
}; };
/* Produce Mapping Matrix A to the entire trajectory. */
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(6, 6); Eigen::MatrixXd A = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
A(2 * i, i) = Factorial(i); A(2 * i, i) = Factorial(i);
for (int j = i; j < 6; j++) for (int j = i; j < 6; j++) {
A(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time, j - i); A(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time, j - i);
} }
}
A_inv = A.inverse();
previousTime = Time;
}
return A_inv;
}
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);
Eigen::MatrixXd A_inv = getAInverse(Time);
/* Produce the dereivatives in X, Y and Z axis directly. */ /* Produce the dereivatives in X, Y and Z axis directly. */
Eigen::VectorXd Dx = Eigen::VectorXd::Zero(6); Eigen::VectorXd Dx = Eigen::VectorXd::Zero(6);
@ -95,9 +108,9 @@ Eigen::MatrixXd solveCoeffFromBoundaryState(const Eigen::Vector3d &Pos_init, con
Dy(5) = Acc_end(1); Dy(5) = Acc_end(1);
Dz(5) = Acc_end(2); Dz(5) = Acc_end(2);
Px = A.inverse() * Dx; Px = A_inv * Dx;
Py = A.inverse() * Dy; Py = A_inv * Dy;
Pz = A.inverse() * Dz; Pz = A_inv * Dz;
PolyCoeff.block(0, 0, 1, 6) = Px.segment(0, 6).transpose(); 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, 6, 1, 6) = Py.segment(0, 6).transpose();
@ -111,8 +124,7 @@ void getPositionFromCoeff(Eigen::Vector3d &pos, Eigen::MatrixXd coeff, int index
double t = time; 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 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 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 = 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);
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(0) = x;
pos(1) = y; pos(1) = y;
@ -124,8 +136,7 @@ void getVelocityFromCoeff(Eigen::Vector3d &vel, Eigen::MatrixXd coeff, int index
double t = time; 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 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 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 = 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);
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(0) = vx;
vel(1) = vy; vel(1) = vy;