Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 5f151a6c 编写于 作者: Y Yajia Zhang
浏览文件
操作

planning: 1. merged fem_1d_qp_problem. 2. fixed memory leak in unit test. 3.... 

planning: 1. merged fem_1d_qp_problem. 2. fixed memory leak in unit test. 3. simplifed fem_1d_qp_problem interface
上级 e947c7ee
隐藏空白更改
内联 并排
Showing with 258 addition and 376 deletion
modules/planning/math/finite_element_qp/BUILD
浏览文件 @ 5f151a6c
......@@ -17,28 +17,14 @@ cc_library(
],
)
cc_library(
name = "fem_1d_jerk_qp_problem",
srcs = [
"fem_1d_jerk_qp_problem.cc",
],
hdrs = [
"fem_1d_jerk_qp_problem.h",
],
deps = [
":fem_1d_qp_problem",
"//cyber/common:log",
],
)
cc_test(
name = "fem_1d_jerk_qp_problem_test",
name = "fem_1d_qp_problem_test",
size = "small",
srcs = [
"fem_1d_jerk_qp_problem_test.cc",
"fem_1d_qp_problem_test.cc",
],
deps = [
":fem_1d_jerk_qp_problem",
":fem_1d_qp_problem",
"//cyber/common:log",
"@gtest//:main",
],
......
modules/planning/math/finite_element_qp/fem_1d_jerk_qp_problem.cc 已删除 100644 → 0
浏览文件 @ e947c7ee
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/planning/math/finite_element_qp/fem_1d_jerk_qp_problem.h"
#include <chrono>
#include "cyber/common/log.h"
namespace apollo {
namespace planning {
bool Fem1dJerkQpProblem::Optimize() {
if (!bound_is_init_) {
AERROR << "Please SetVariableBounds() before running.";
return false;
}
// calculate kernal
std::vector<c_float> P_data;
std::vector<c_int> P_indices;
std::vector<c_int> P_indptr;
auto start_time = std::chrono::system_clock::now();
CalculateKernel(&P_data, &P_indices, &P_indptr);
auto end_time1 = std::chrono::system_clock::now();
std::chrono::duration<double> diff = end_time1 - start_time;
ADEBUG << "Set Kernel used time: " << diff.count() * 1000 << " ms.";
// calculate affine constraints
std::vector<c_float> A_data;
std::vector<c_int> A_indices;
std::vector<c_int> A_indptr;
std::vector<c_float> lower_bounds;
std::vector<c_float> upper_bounds;
CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
&upper_bounds);
auto end_time2 = std::chrono::system_clock::now();
diff = end_time2 - end_time1;
ADEBUG << "CalculateAffineConstraint used time: " << diff.count() * 1000
<< " ms.";
// calculate offset
std::vector<c_float> q;
CalculateOffset(&q);
auto end_time3 = std::chrono::system_clock::now();
diff = end_time3 - end_time2;
ADEBUG << "CalculateOffset used time: " << diff.count() * 1000 << " ms.";
OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
OSQPSettings* settings =
reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
OSQPWorkspace* work = nullptr;
OptimizeWithOsqp(3 * num_var_, lower_bounds.size(), P_data, P_indices,
P_indptr, A_data, A_indices, A_indptr, lower_bounds,
upper_bounds, q, data, &work, settings);
if (work == nullptr || work->solution == nullptr) {
AERROR << "Failed to find QP solution.";
return false;
}
// extract primal results
x_.resize(num_var_);
x_derivative_.resize(num_var_);
x_second_order_derivative_.resize(num_var_);
for (size_t i = 0; i < num_var_; ++i) {
x_.at(i) = work->solution->x[i];
x_derivative_.at(i) = work->solution->x[i + num_var_];
x_second_order_derivative_.at(i) = work->solution->x[i + 2 * num_var_];
}
x_derivative_.back() = 0.0;
x_second_order_derivative_.back() = 0.0;
// Cleanup
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
auto end_time4 = std::chrono::system_clock::now();
diff = end_time4 - end_time3;
ADEBUG << "Run OptimizeWithOsqp used time: " << diff.count() * 1000 << " ms.";
return true;
}
void Fem1dJerkQpProblem::CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
const int N = static_cast<int>(num_var_);
const int kNumParam = 3 * N;
P_data->resize(kNumParam);
P_indices->resize(kNumParam);
P_indptr->resize(kNumParam + 1);
for (int i = 0; i < kNumParam; ++i) {
if (i < N) {
P_data->at(i) = 2.0 * (weight_.x_w + weight_.x_mid_line_w);
} else if (i < 2 * N) {
P_data->at(i) = 2.0 * weight_.x_derivative_w;
} else {
P_data->at(i) = 2.0 * weight_.x_second_order_derivative_w;
}
P_indices->at(i) = i;
P_indptr->at(i) = i;
}
P_indptr->at(kNumParam) = kNumParam;
CHECK_EQ(P_data->size(), P_indices->size());
}
void Fem1dJerkQpProblem::CalculateAffineConstraint(
std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
std::vector<c_float>* upper_bounds) {
// 3N params bounds on x, x', x''
// 3(N-1) constraints on x, x', x''
// 3 constraints on x_init_
const int N = static_cast<int>(num_var_);
const int kNumParam = 3 * N;
const int kNumConstraint = kNumParam + 3 * (N - 1) + 3;
lower_bounds->resize(kNumConstraint);
upper_bounds->resize(kNumConstraint);
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(kNumParam);
int constraint_index = 0;
// set x, x', x'' bounds
for (int i = 0; i < kNumParam; ++i) {
columns[i].emplace_back(constraint_index, 1.0);
if (i < N) {
lower_bounds->at(constraint_index) = std::get<0>(x_bounds_[i]);
upper_bounds->at(constraint_index) = std::get<1>(x_bounds_[i]);
} else if (i < 2 * N) {
lower_bounds->at(constraint_index) = std::get<0>(dx_bounds_[i - N]);
upper_bounds->at(constraint_index) = std::get<1>(dx_bounds_[i - N]);
} else {
lower_bounds->at(constraint_index) = std::get<0>(ddx_bounds_[i - 2 * N]);
upper_bounds->at(constraint_index) = std::get<1>(ddx_bounds_[i - 2 * N]);
}
++constraint_index;
}
CHECK_EQ(constraint_index, kNumParam);
// x(i+1)'' - x(i)'' - x(i)''' * delta_s = 0
for (int i = 0; i + 1 < N; ++i) {
columns[2 * N + i].emplace_back(constraint_index, -1.0);
columns[2 * N + i + 1].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
-max_x_third_order_derivative_ * delta_s_;
upper_bounds->at(constraint_index) =
max_x_third_order_derivative_ * delta_s_;
++constraint_index;
}
// x(i+1)' - x(i)' - 0.5 * delta_s * (x(i+1)'' + x(i)'') = 0
for (int i = 0; i + 1 < N; ++i) {
columns[N + i].emplace_back(constraint_index, -1.0);
columns[N + i + 1].emplace_back(constraint_index, 1.0);
columns[2 * N + i].emplace_back(constraint_index, -0.5 * delta_s_);
columns[2 * N + i + 1].emplace_back(constraint_index, -0.5 * delta_s_);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}
// x(i+1) - x(i) - x(i)'*delta_s - 1/3*x(i)''*delta_s^2 - 1/6*x(i)''*delta_s^2
for (int i = 0; i + 1 < N; ++i) {
columns[i].emplace_back(constraint_index, -1.0);
columns[i + 1].emplace_back(constraint_index, 1.0);
columns[N + i].emplace_back(constraint_index, -delta_s_);
columns[2 * N + i].emplace_back(constraint_index, -delta_s_sq_ / 3.0);
columns[2 * N + i + 1].emplace_back(constraint_index, -delta_s_sq_ / 6.0);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}
// constrain on x_init
columns[0].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[0];
upper_bounds->at(constraint_index) = x_init_[0];
++constraint_index;
columns[N].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[1];
upper_bounds->at(constraint_index) = x_init_[1];
++constraint_index;
columns[2 * N].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[2];
upper_bounds->at(constraint_index) = x_init_[2];
++constraint_index;
CHECK_EQ(constraint_index, kNumConstraint);
int ind_p = 0;
for (int i = 0; i < kNumParam; ++i) {
A_indptr->push_back(ind_p);
for (const auto& row_data_pair : columns[i]) {
A_data->push_back(row_data_pair.second);
A_indices->push_back(row_data_pair.first);
++ind_p;
}
}
A_indptr->push_back(ind_p);
}
void Fem1dJerkQpProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL(q);
const int N = static_cast<int>(num_var_);
const int kNumParam = 3 * N;
q->resize(kNumParam);
for (int i = 0; i < kNumParam; ++i) {
if (i < N) {
q->at(i) = -2.0 * weight_.x_mid_line_w *
(std::get<0>(x_bounds_[i]) + std::get<1>(x_bounds_[i]));
} else {
q->at(i) = 0.0;
}
}
}
} // namespace planning
} // namespace apollo
modules/planning/math/finite_element_qp/fem_1d_jerk_qp_problem.h 已删除 100644 → 0
浏览文件 @ e947c7ee
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include <utility>
#include <vector>
#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
namespace apollo {
namespace planning {
/*
* @brief:
* construct a matrix by concatinate x, x', x''
*/
class Fem1dJerkQpProblem : public Fem1dQpProblem {
public:
Fem1dJerkQpProblem() = default;
virtual ~Fem1dJerkQpProblem() = default;
bool Optimize() override;
private:
// naming convention follows osqp solver.
void CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) override;
void CalculateAffineConstraint(std::vector<c_float>* A_data,
std::vector<c_int>* A_indices,
std::vector<c_int>* A_indptr,
std::vector<c_float>* lower_bounds,
std::vector<c_float>* upper_bounds) override;
void CalculateOffset(std::vector<c_float>* q) override;
};
} // namespace planning
} // namespace apollo
modules/planning/math/finite_element_qp/fem_1d_qp_problem.cc
浏览文件 @ 5f151a6c
......@@ -17,6 +17,7 @@
#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
#include <algorithm>
#include <chrono>
#include "cyber/common/log.h"
......@@ -29,14 +30,12 @@ namespace {
constexpr double kMaxVariableRange = 1e10;
} // namespace
bool Fem1dQpProblem::Init(const size_t num_var,
const std::array<double, 3>& x_init,
const double delta_s, const std::array<double, 5>& w,
const double max_x_third_order_derivative) {
Fem1dQpProblem::Fem1dQpProblem(const size_t num_var,
const std::array<double, 3>& x_init, const double delta_s,
const std::array<double, 5>& w, const double max_x_third_order_derivative) {
CHECK_GE(num_var, 4);
num_var_ = num_var;
if (num_var_ < 4) {
return false;
}
x_init_ = x_init;
......@@ -48,7 +47,6 @@ bool Fem1dQpProblem::Init(const size_t num_var,
max_x_third_order_derivative_ = max_x_third_order_derivative;
// pre-calcualte some consts
delta_s_ = delta_s;
delta_s_sq_ = delta_s * delta_s;
delta_s_tri_ = delta_s_sq_ * delta_s;
......@@ -64,9 +62,6 @@ bool Fem1dQpProblem::Init(const size_t num_var,
ddx_bounds_.resize(num_var_,
std::make_pair(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default));
is_init_ = true;
return true;
}
bool Fem1dQpProblem::OptimizeWithOsqp(
......@@ -128,10 +123,6 @@ void Fem1dQpProblem::ProcessBound(
// x_bounds: tuple(s, lower_bounds, upper_bounds)
void Fem1dQpProblem::SetVariableBounds(
const std::vector<std::tuple<double, double, double>>& x_bounds) {
if (!is_init_) {
AERROR << "Please Init() before setting bounds.";
return;
}
ProcessBound(x_bounds, &x_bounds_);
bound_is_init_ = true;
}
......@@ -139,20 +130,12 @@ void Fem1dQpProblem::SetVariableBounds(
// dx_bounds: tuple(s, lower_bounds, upper_bounds)
void Fem1dQpProblem::SetVariableDerivativeBounds(
const std::vector<std::tuple<double, double, double>>& dx_bounds) {
if (!is_init_) {
AERROR << "Please Init() before setting bounds.";
return;
}
ProcessBound(dx_bounds, &dx_bounds_);
}
// ddx_bounds: tuple(s, lower_bounds, upper_bounds)
void Fem1dQpProblem::SetVariableSecondOrderDerivativeBounds(
const std::vector<std::tuple<double, double, double>>& ddx_bounds) {
if (!is_init_) {
AERROR << "Please Init() before setting bounds.";
return;
}
ProcessBound(ddx_bounds, &ddx_bounds_);
}
......@@ -205,5 +188,216 @@ void Fem1dQpProblem::SetOutputResolution(const double resolution) {
x_third_order_derivative_ = new_dddx;
}
bool Fem1dQpProblem::Optimize() {
if (!bound_is_init_) {
AERROR << "Please SetVariableBounds() before running.";
return false;
}
// calculate kernal
std::vector<c_float> P_data;
std::vector<c_int> P_indices;
std::vector<c_int> P_indptr;
auto start_time = std::chrono::system_clock::now();
CalculateKernel(&P_data, &P_indices, &P_indptr);
auto end_time1 = std::chrono::system_clock::now();
std::chrono::duration<double> diff = end_time1 - start_time;
ADEBUG << "Set Kernel used time: " << diff.count() * 1000 << " ms.";
// calculate affine constraints
std::vector<c_float> A_data;
std::vector<c_int> A_indices;
std::vector<c_int> A_indptr;
std::vector<c_float> lower_bounds;
std::vector<c_float> upper_bounds;
CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
&upper_bounds);
auto end_time2 = std::chrono::system_clock::now();
diff = end_time2 - end_time1;
ADEBUG << "CalculateAffineConstraint used time: " << diff.count() * 1000
<< " ms.";
// calculate offset
std::vector<c_float> q;
CalculateOffset(&q);
auto end_time3 = std::chrono::system_clock::now();
diff = end_time3 - end_time2;
ADEBUG << "CalculateOffset used time: " << diff.count() * 1000 << " ms.";
OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
OSQPSettings* settings =
reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
OSQPWorkspace* work = nullptr;
OptimizeWithOsqp(3 * num_var_, lower_bounds.size(), P_data, P_indices,
P_indptr, A_data, A_indices, A_indptr, lower_bounds,
upper_bounds, q, data, &work, settings);
if (work == nullptr || work->solution == nullptr) {
AERROR << "Failed to find QP solution.";
return false;
}
// extract primal results
x_.resize(num_var_);
x_derivative_.resize(num_var_);
x_second_order_derivative_.resize(num_var_);
for (size_t i = 0; i < num_var_; ++i) {
x_.at(i) = work->solution->x[i];
x_derivative_.at(i) = work->solution->x[i + num_var_];
x_second_order_derivative_.at(i) = work->solution->x[i + 2 * num_var_];
}
x_derivative_.back() = 0.0;
x_second_order_derivative_.back() = 0.0;
// Cleanup
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
auto end_time4 = std::chrono::system_clock::now();
diff = end_time4 - end_time3;
ADEBUG << "Run OptimizeWithOsqp used time: " << diff.count() * 1000 << " ms.";
return true;
}
void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
const int N = static_cast<int>(num_var_);
const int kNumParam = 3 * N;
P_data->resize(kNumParam);
P_indices->resize(kNumParam);
P_indptr->resize(kNumParam + 1);
for (int i = 0; i < kNumParam; ++i) {
if (i < N) {
P_data->at(i) = 2.0 * (weight_.x_w + weight_.x_mid_line_w);
} else if (i < 2 * N) {
P_data->at(i) = 2.0 * weight_.x_derivative_w;
} else {
P_data->at(i) = 2.0 * weight_.x_second_order_derivative_w;
}
P_indices->at(i) = i;
P_indptr->at(i) = i;
}
P_indptr->at(kNumParam) = kNumParam;
CHECK_EQ(P_data->size(), P_indices->size());
}
void Fem1dQpProblem::CalculateAffineConstraint(
std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
std::vector<c_float>* upper_bounds) {
// 3N params bounds on x, x', x''
// 3(N-1) constraints on x, x', x''
// 3 constraints on x_init_
const int N = static_cast<int>(num_var_);
const int kNumParam = 3 * N;
const int kNumConstraint = kNumParam + 3 * (N - 1) + 3;
lower_bounds->resize(kNumConstraint);
upper_bounds->resize(kNumConstraint);
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(kNumParam);
int constraint_index = 0;
// set x, x', x'' bounds
for (int i = 0; i < kNumParam; ++i) {
columns[i].emplace_back(constraint_index, 1.0);
if (i < N) {
lower_bounds->at(constraint_index) = std::get<0>(x_bounds_[i]);
upper_bounds->at(constraint_index) = std::get<1>(x_bounds_[i]);
} else if (i < 2 * N) {
lower_bounds->at(constraint_index) = std::get<0>(dx_bounds_[i - N]);
upper_bounds->at(constraint_index) = std::get<1>(dx_bounds_[i - N]);
} else {
lower_bounds->at(constraint_index) = std::get<0>(ddx_bounds_[i - 2 * N]);
upper_bounds->at(constraint_index) = std::get<1>(ddx_bounds_[i - 2 * N]);
}
++constraint_index;
}
CHECK_EQ(constraint_index, kNumParam);
// x(i+1)'' - x(i)'' - x(i)''' * delta_s = 0
for (int i = 0; i + 1 < N; ++i) {
columns[2 * N + i].emplace_back(constraint_index, -1.0);
columns[2 * N + i + 1].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
-max_x_third_order_derivative_ * delta_s_;
upper_bounds->at(constraint_index) =
max_x_third_order_derivative_ * delta_s_;
++constraint_index;
}
// x(i+1)' - x(i)' - 0.5 * delta_s * (x(i+1)'' + x(i)'') = 0
for (int i = 0; i + 1 < N; ++i) {
columns[N + i].emplace_back(constraint_index, -1.0);
columns[N + i + 1].emplace_back(constraint_index, 1.0);
columns[2 * N + i].emplace_back(constraint_index, -0.5 * delta_s_);
columns[2 * N + i + 1].emplace_back(constraint_index, -0.5 * delta_s_);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}
// x(i+1) - x(i) - x(i)'*delta_s - 1/3*x(i)''*delta_s^2 - 1/6*x(i)''*delta_s^2
for (int i = 0; i + 1 < N; ++i) {
columns[i].emplace_back(constraint_index, -1.0);
columns[i + 1].emplace_back(constraint_index, 1.0);
columns[N + i].emplace_back(constraint_index, -delta_s_);
columns[2 * N + i].emplace_back(constraint_index, -delta_s_sq_ / 3.0);
columns[2 * N + i + 1].emplace_back(constraint_index, -delta_s_sq_ / 6.0);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}
// constrain on x_init
columns[0].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[0];
upper_bounds->at(constraint_index) = x_init_[0];
++constraint_index;
columns[N].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[1];
upper_bounds->at(constraint_index) = x_init_[1];
++constraint_index;
columns[2 * N].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[2];
upper_bounds->at(constraint_index) = x_init_[2];
++constraint_index;
CHECK_EQ(constraint_index, kNumConstraint);
int ind_p = 0;
for (int i = 0; i < kNumParam; ++i) {
A_indptr->push_back(ind_p);
for (const auto& row_data_pair : columns[i]) {
A_data->push_back(row_data_pair.second);
A_indices->push_back(row_data_pair.first);
++ind_p;
}
}
A_indptr->push_back(ind_p);
}
void Fem1dQpProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL(q);
const int N = static_cast<int>(num_var_);
const int kNumParam = 3 * N;
q->resize(kNumParam);
for (int i = 0; i < kNumParam; ++i) {
if (i < N) {
q->at(i) = -2.0 * weight_.x_mid_line_w *
(std::get<0>(x_bounds_[i]) + std::get<1>(x_bounds_[i]));
} else {
q->at(i) = 0.0;
}
}
}
} // namespace planning
} // namespace apollo
modules/planning/math/finite_element_qp/fem_1d_qp_problem.h
浏览文件 @ 5f151a6c
......@@ -49,10 +49,6 @@ namespace planning {
class Fem1dQpProblem {
public:
Fem1dQpProblem() = default;
virtual ~Fem1dQpProblem() = default;
/*
* @param
* x_init: the init status of x, x', x''
......@@ -66,9 +62,11 @@ class Fem1dQpProblem {
* -- w[4]: default reference line weight, (x_bounds[k].first +
* x_bounds[k].second)/2
*/
virtual bool Init(const size_t num_var, const std::array<double, 3>& x_init,
const double delta_s, const std::array<double, 5>& w,
const double max_x_third_order_derivative);
Fem1dQpProblem(const size_t num_var, const std::array<double, 3>& x_init,
const double delta_s, const std::array<double, 5>& w,
const double max_x_third_order_derivative);
virtual ~Fem1dQpProblem() = default;
virtual void AddReferenceLineKernel(const std::vector<double>& ref_line,
const double wweight) {}
......@@ -94,7 +92,7 @@ class Fem1dQpProblem {
virtual void PreSetKernel() {}
virtual bool Optimize() = 0;
virtual bool Optimize();
virtual std::vector<double> x() const { return x_; }
......@@ -116,14 +114,14 @@ class Fem1dQpProblem {
// naming convention follows osqp solver.
virtual void CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) = 0;
std::vector<c_int>* P_indptr);
virtual void CalculateOffset(std::vector<c_float>* q) = 0;
virtual void CalculateOffset(std::vector<c_float>* q);
virtual void CalculateAffineConstraint(
std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
std::vector<c_float>* upper_bounds) = 0;
std::vector<c_float>* upper_bounds);
bool OptimizeWithOsqp(
const size_t kernel_dim, const size_t num_affine_constraint,
......@@ -140,7 +138,6 @@ class Fem1dQpProblem {
std::vector<std::pair<double, double>>* dst);
protected:
bool is_init_ = false;
bool bound_is_init_ = false;
size_t num_var_ = 0;
......
modules/planning/math/finite_element_qp/fem_1d_jerk_qp_problem_test.cc → modules/planning/math/finite_element_qp/fem_1d_qp_problem_test.cc
浏览文件 @ 5f151a6c
......@@ -18,6 +18,7 @@
* @file
**/
#include <math.h>
#include <chrono>
#include "cyber/common/log.h"
......@@ -27,14 +28,13 @@
#define private public
#define protected public
#include "modules/planning/math/finite_element_qp/fem_1d_jerk_qp_problem.h"
#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
namespace apollo {
namespace planning {
TEST(Fem1dJerkQpProblemTest, basic_test) {
FLAGS_enable_osqp_debug = true;
Fem1dQpProblem* fem_qp = new Fem1dJerkQpProblem();
std::array<double, 3> x_init = {1.5, 0.01, 0.001};
double delta_s = 0.5;
size_t n = 400;
......@@ -50,8 +50,10 @@ TEST(Fem1dJerkQpProblemTest, basic_test) {
}
std::array<double, 5> w = {1.0, 2.0, 3.0, 4.0, 1.45};
double max_x_third_order_derivative = 1.25;
EXPECT_TRUE(
fem_qp->Init(n, x_init, delta_s, w, max_x_third_order_derivative));
Fem1dQpProblem* fem_qp = new Fem1dQpProblem(
n, x_init, delta_s, w, max_x_third_order_derivative);
EXPECT_FALSE(fem_qp->Optimize());
fem_qp->SetVariableBounds(x_bounds);
......@@ -68,18 +70,19 @@ TEST(Fem1dJerkQpProblemTest, basic_test) {
EXPECT_LE(x[i], fem_qp->x_bounds_[i].second);
EXPECT_GE(x[i], fem_qp->x_bounds_[i].first);
}
delete fem_qp;
}
TEST(Fem1dJerkQpProblemTest, add_bounds_test) {
FLAGS_enable_osqp_debug = false;
Fem1dQpProblem* fem_qp = new Fem1dJerkQpProblem();
std::array<double, 3> x_init = {1.5, 0.01, 0.001};
double delta_s = 0.5;
size_t n = 400;
std::array<double, 5> w = {1.0, 2.0, 3.0, 4.0, 1.45};
double max_x_third_order_derivative = 0.25;
EXPECT_TRUE(
fem_qp->Init(n, x_init, delta_s, w, max_x_third_order_derivative));
Fem1dQpProblem* fem_qp = new Fem1dQpProblem(
n, x_init, delta_s, w, max_x_third_order_derivative);
std::vector<std::tuple<double, double, double>> x_bounds;
for (size_t i = 10; i < 20; ++i) {
......@@ -95,11 +98,12 @@ TEST(Fem1dJerkQpProblemTest, add_bounds_test) {
EXPECT_DOUBLE_EQ(std::get<0>(x[i]), -1.81);
EXPECT_DOUBLE_EQ(std::get<1>(x[i]), 1.95);
}
delete fem_qp;
}
TEST(Fem1dJerkQpProblemTest, derivative_constraint_test) {
FLAGS_enable_osqp_debug = true;
Fem1dQpProblem* fem_qp = new Fem1dJerkQpProblem();
std::array<double, 3> x_init = {4.5, 0.00, 0.0};
double delta_s = 0.5;
size_t n = 200;
......@@ -109,8 +113,9 @@ TEST(Fem1dJerkQpProblemTest, derivative_constraint_test) {
}
std::array<double, 5> w = {1.0, 100.0, 1000.0, 1000.0, 0.0};
double max_x_third_order_derivative = 2.0;
EXPECT_TRUE(
fem_qp->Init(n, x_init, delta_s, w, max_x_third_order_derivative));
Fem1dQpProblem* fem_qp = new Fem1dQpProblem(
n, x_init, delta_s, w, max_x_third_order_derivative);
fem_qp->SetVariableBounds(x_bounds);
......@@ -141,6 +146,8 @@ TEST(Fem1dJerkQpProblemTest, derivative_constraint_test) {
EXPECT_LT(dx[i] - 1e-12, fem_qp->dx_bounds_[i].second);
EXPECT_GT(dx[i] + 1e-12, fem_qp->dx_bounds_[i].first);
}
delete fem_qp;
}
} // namespace planning
......
modules/planning/tasks/deciders/BUILD
浏览文件 @ 5f151a6c
......@@ -30,7 +30,7 @@ cc_library(
"//modules/planning/common:planning_context",
"//modules/planning/common:planning_gflags",
"//modules/planning/common:reference_line_info",
"//modules/planning/math/finite_element_qp:fem_1d_jerk_qp_problem",
"//modules/planning/math/finite_element_qp:fem_1d_qp_problem",
"//modules/planning/tasks:task",
],
)
......
modules/planning/tasks/deciders/side_pass_path_decider.cc
浏览文件 @ 5f151a6c
......@@ -56,9 +56,10 @@ void SidePassPathDecider::InitSolver() {
config.side_pass_path_decider_config().dddl_weight(),
config.side_pass_path_decider_config().guiding_line_weight(),
};
fem_qp_.reset(new Fem1dJerkQpProblem());
CHECK(fem_qp_->Init(n, l_init, delta_s_, w,
config.side_pass_path_decider_config().max_dddl()));
fem_qp_ = std::move(std::make_unique<Fem1dQpProblem>(
n, l_init, delta_s_, w,
config.side_pass_path_decider_config().max_dddl()));
}
Status SidePassPathDecider::Process(
......
modules/planning/tasks/deciders/side_pass_path_decider.h
浏览文件 @ 5f151a6c
......@@ -33,7 +33,7 @@
#include "modules/map/pnc_map/path.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/math/finite_element_qp/fem_1d_jerk_qp_problem.h"
#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
#include "modules/planning/tasks/deciders/decider.h"
namespace apollo {
......@@ -81,7 +81,7 @@ class SidePassPathDecider : public Decider {
private:
common::TrajectoryPoint adc_planning_start_point_;
common::FrenetFramePoint adc_frenet_frame_point_;
std::unique_ptr<Fem1dQpProblem> fem_qp_;
std::unique_ptr<Fem1dQpProblem> fem_qp_ = nullptr;
SidePassDirection decided_direction_ = SidePassDirection::LEFT;
double delta_s_ = 0.0;
double total_path_length_ = 0.0;
......
modules/planning/tasks/optimizers/qp_piecewise_jerk_path/BUILD
浏览文件 @ 5f151a6c
......@@ -30,7 +30,6 @@ cc_library(
"//modules/planning/math:polynomial_xd",
"//modules/planning/math/curve1d:polynomial_curve1d",
"//modules/planning/math/curve1d:quintic_polynomial_curve1d",
"//modules/planning/math/finite_element_qp:fem_1d_jerk_qp_problem",
"//modules/planning/math/finite_element_qp:fem_1d_qp_problem",
"//modules/planning/proto:planning_proto",
"//modules/planning/reference_line",
......
modules/planning/tasks/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc
浏览文件 @ 5f151a6c
......@@ -21,8 +21,10 @@
#include "modules/planning/tasks/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h"
#include <algorithm>
#include <memory>
#include "modules/common/time/time.h"
#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
namespace apollo {
namespace planning {
......@@ -238,14 +240,10 @@ Status QpPiecewiseJerkPathOptimizer::Process(
qp_config.guiding_line_weight(),
};
fem_1d_qp_.reset(new Fem1dJerkQpProblem());
constexpr double kMaxLThirdOrderDerivative = 2.0;
if (!fem_1d_qp_->Init(n, init_lateral_state, qp_delta_s, w,
kMaxLThirdOrderDerivative)) {
std::string msg = "lateral qp optimizer failed";
return Status(ErrorCode::PLANNING_ERROR, msg);
}
auto fem_1d_qp_ = std::make_unique<Fem1dQpProblem>(
n, init_lateral_state, qp_delta_s, w,
kMaxLThirdOrderDerivative);
auto start_time = std::chrono::system_clock::now();
......
modules/planning/tasks/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.h
浏览文件 @ 5f151a6c
......@@ -20,15 +20,12 @@
#pragma once
#include <memory>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/math/finite_element_qp/fem_1d_jerk_qp_problem.h"
#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
#include "modules/planning/proto/planning_config.pb.h"
#include "modules/planning/proto/qp_piecewise_jerk_path_config.pb.h"
#include "modules/planning/tasks/optimizers/path_optimizer.h"
......@@ -56,9 +53,6 @@ class QpPiecewiseJerkPathOptimizer : public PathOptimizer {
std::vector<std::tuple<double, double, double>>
GetLateralSecondOrderDerivativeBounds(
const common::TrajectoryPoint& init_point, const double qp_delta_s);
private:
std::unique_ptr<Fem1dQpProblem> fem_1d_qp_;
};
} // namespace planning
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或