Skip to content

Commit

Permalink
Planning : use obstacles_edges_num in distance approach problem
Browse files Browse the repository at this point in the history
Capri2014 authored and lianglia-apollo committed Oct 17, 2018

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
1 parent 113d433 commit 64ff011
Showing 6 changed files with 22 additions and 23 deletions.
14 changes: 7 additions & 7 deletions modules/planning/open_space/distance_approach_ipopt_interface.cc
Original file line number Diff line number Diff line change
@@ -29,7 +29,7 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(
const Eigen::MatrixXd& xWS, const Eigen::MatrixXd& uWS,
const Eigen::MatrixXd& timeWS, Eigen::MatrixXd x0, Eigen::MatrixXd xf,
Eigen::MatrixXd last_time_u, Eigen::MatrixXd XYbounds,
Eigen::MatrixXd obstacles_vertices_num, std::size_t obstacles_num,
Eigen::MatrixXd obstacles_edges_num, std::size_t obstacles_num,
const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
bool use_fix_time)
: num_of_variables_(num_of_variables),
@@ -44,7 +44,7 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(
xf_(xf),
last_time_u_(last_time_u),
XYbounds_(XYbounds),
obstacles_vertices_num_(obstacles_vertices_num),
obstacles_edges_num_(obstacles_edges_num),
obstacles_num_(obstacles_num),
obstacles_A_(obstacles_A),
obstacles_b_(obstacles_b),
@@ -54,7 +54,7 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(

g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
obstacles_vertices_sum_ = std::size_t(obstacles_vertices_num_.sum());
obstacles_vertices_sum_ = std::size_t(obstacles_edges_num_.sum());
state_result_ = Eigen::MatrixXd::Zero(horizon_ + 1, 4);
control_result_ = Eigen::MatrixXd::Zero(horizon_ + 1, 2);
time_result_ = Eigen::MatrixXd::Zero(horizon_ + 1, 1);
@@ -105,7 +105,7 @@ bool DistanceApproachIPOPTInterface::get_nlp_info(int& n, int& m,

for (std::size_t i = 0; i < horizon_ + 1; ++i) {
for (std::size_t j = 0; j < obstacles_num_; ++j) {
std::size_t current_vertice_num = obstacles_vertices_num_(j, 0);
std::size_t current_vertice_num = obstacles_edges_num_(j, 0);
tmp += current_vertice_num * 4 + 9 + 4;
}
}
@@ -396,7 +396,7 @@ bool DistanceApproachIPOPTInterface::eval_g(int n, const double* x, bool new_x,

for (std::size_t i = 0; i < horizon_ + 1; ++i) {
for (std::size_t j = 0; j < obstacles_num_; ++j) {
std::size_t current_vertice_num = obstacles_vertices_num_(i, 0);
std::size_t current_vertice_num = obstacles_edges_num_(i, 0);
Eigen::MatrixXd Aj =
obstacles_A_.block(counter, 0, current_vertice_num, 2);
std::vector<int> lj(&x[l_index], &x[l_index + current_vertice_num]);
@@ -697,7 +697,7 @@ bool DistanceApproachIPOPTInterface::eval_jac_g(int n, const double* x,

for (std::size_t i = 0; i < horizon_ + 1; ++i) {
for (std::size_t j = 0; j < obstacles_num_; ++j) {
std::size_t current_vertice_num = obstacles_vertices_num_(j, 0);
std::size_t current_vertice_num = obstacles_edges_num_(j, 0);

// 1. norm(A* lambda == 1)
for (std::size_t k = 0; k < current_vertice_num; ++k) {
@@ -1060,7 +1060,7 @@ bool DistanceApproachIPOPTInterface::eval_jac_g(int n, const double* x,

for (std::size_t i = 0; i < horizon_ + 1; ++i) {
for (std::size_t j = 0; j < obstacles_num_; ++j) {
std::size_t current_vertice_num = obstacles_vertices_num_(j, 0);
std::size_t current_vertice_num = obstacles_edges_num_(j, 0);
AINFO << "eval_jac_g, obstacle constraint values, current "
"vertice_num : "
<< current_vertice_num << " i : " << i << " j : " << j;
Original file line number Diff line number Diff line change
@@ -49,7 +49,7 @@ class DistanceApproachIPOPTInterface : public Ipopt::TNLP {
const Eigen::MatrixXd& xWS, const Eigen::MatrixXd& uWS,
const Eigen::MatrixXd& timeWS, Eigen::MatrixXd x0, Eigen::MatrixXd xf,
Eigen::MatrixXd last_time_u, Eigen::MatrixXd XYbounds,
Eigen::MatrixXd obstacles_vertices_num, std::size_t obstacles_num,
Eigen::MatrixXd obstacles_edges_num, std::size_t obstacles_num,
const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
bool use_fix_time);

@@ -140,7 +140,7 @@ class DistanceApproachIPOPTInterface : public Ipopt::TNLP {
double l_ev_;
std::vector<double> g_;
double offset_;
Eigen::MatrixXd obstacles_vertices_num_;
Eigen::MatrixXd obstacles_edges_num_;
std::size_t obstacles_num_;
std::size_t obstacles_vertices_sum_;
double wheelbase_;
Original file line number Diff line number Diff line change
@@ -58,7 +58,7 @@ class DistanceApproachIPOPTInterfaceTest : public ::testing::Test {
Eigen::MatrixXd xWS_ = Eigen::MatrixXd::Ones(4, 6);
Eigen::MatrixXd uWS_ = Eigen::MatrixXd::Ones(2, 5);
Eigen::MatrixXd timeWS_ = Eigen::MatrixXd::Ones(1, 5);
Eigen::MatrixXd obstacles_vertices_num_;
Eigen::MatrixXd obstacles_edges_num_;
Eigen::MatrixXd obstacles_A_ = Eigen::MatrixXd::Ones(10, 2);
Eigen::MatrixXd obstacles_b_ = Eigen::MatrixXd::Ones(10, 1);
int num_of_variables_;
@@ -72,7 +72,7 @@ class DistanceApproachIPOPTInterfaceTest : public ::testing::Test {
};

void DistanceApproachIPOPTInterfaceTest::ProblemSetup() {
obstacles_vertices_num_ = 4 * Eigen::MatrixXd::Ones(obstacles_num_, 1);
obstacles_edges_num_ = 4 * Eigen::MatrixXd::Ones(obstacles_num_, 1);

num_of_variables_ = 4 * (horizon_ + 1) + 2 * horizon_ + (horizon_ + 1) +
(horizon_ + 1) * 4 * obstacles_num_ +
@@ -84,7 +84,7 @@ void DistanceApproachIPOPTInterfaceTest::ProblemSetup() {

ptop_.reset(new DistanceApproachIPOPTInterface(
num_of_variables_, num_of_constraints_, horizon_, ts_, ego_, xWS_, uWS_,
timeWS_, x0_, xf_, last_time_u_, XYbounds_, obstacles_vertices_num_,
timeWS_, x0_, xf_, last_time_u_, XYbounds_, obstacles_edges_num_,
obstacles_num_, obstacles_A_, obstacles_b_, use_fix_time_));
}

8 changes: 4 additions & 4 deletions modules/planning/open_space/distance_approach_problem.cc
Original file line number Diff line number Diff line change
@@ -29,7 +29,7 @@ DistanceApproachProblem::DistanceApproachProblem(
Eigen::MatrixXd x0, Eigen::MatrixXd xF, Eigen::MatrixXd last_time_u,
std::size_t horizon, float ts, Eigen::MatrixXd ego, Eigen::MatrixXd xWS,
Eigen::MatrixXd uWS, Eigen::MatrixXd XYbounds, std::size_t obstacles_num,
Eigen::MatrixXd obstacles_vertices_num, Eigen::MatrixXd obstacles_A,
Eigen::MatrixXd obstacles_edges_num, Eigen::MatrixXd obstacles_A,
Eigen::MatrixXd obstacles_b)
: x0_(x0),
xF_(xF),
@@ -41,7 +41,7 @@ DistanceApproachProblem::DistanceApproachProblem(
uWS_(uWS),
XYbounds_(XYbounds),
obstacles_num_(obstacles_num),
obstacles_vertices_num_(obstacles_vertices_num),
obstacles_edges_num_(obstacles_edges_num),
obstacles_A_(obstacles_A),
obstacles_b_(obstacles_b) {}

@@ -61,7 +61,7 @@ bool DistanceApproachProblem::Solve(Eigen::MatrixXd* state_result,
int n3 = horizon_ + 1;

// n4 : dual multiplier associated with obstacleShape
int n4 = obstacles_vertices_num_.sum() * (horizon_ + 1);
int n4 = obstacles_edges_num_.sum() * (horizon_ + 1);

// n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
int n5 = obstacles_num_ * 4 * (horizon_ + 1);
@@ -92,7 +92,7 @@ bool DistanceApproachProblem::Solve(Eigen::MatrixXd* state_result,
bool use_fix_time_ = false;
DistanceApproachIPOPTInterface* ptop = new DistanceApproachIPOPTInterface(
num_of_variables, num_of_constraints, horizon_, ts_, ego_, xWS_, uWS_,
timeWS_, x0_, xF_, last_time_u_, XYbounds_, obstacles_vertices_num_,
timeWS_, x0_, xF_, last_time_u_, XYbounds_, obstacles_edges_num_,
obstacles_num_, obstacles_A_, obstacles_b_, use_fix_time_);

Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;
6 changes: 3 additions & 3 deletions modules/planning/open_space/distance_approach_problem.h
Original file line number Diff line number Diff line change
@@ -43,7 +43,7 @@ class DistanceApproachProblem {
Eigen::MatrixXd x0, Eigen::MatrixXd xF, Eigen::MatrixXd last_time_u,
std::size_t horizon, float ts, Eigen::MatrixXd ego, Eigen::MatrixXd xWS,
Eigen::MatrixXd uWS, Eigen::MatrixXd XYbounds, std::size_t obstacles_num,
Eigen::MatrixXd obstacles_vertices_num, Eigen::MatrixXd obstacles_A,
Eigen::MatrixXd obstacles_edges_num, Eigen::MatrixXd obstacles_A,
Eigen::MatrixXd obstacles_b);

virtual ~DistanceApproachProblem() = default;
@@ -85,8 +85,8 @@ class DistanceApproachProblem {
// number of obstacles
std::size_t obstacles_num_;

// obstacles_vertices_num
Eigen::MatrixXd obstacles_vertices_num_;
// obstacles_edges_num
Eigen::MatrixXd obstacles_edges_num_;

// obstacles_A
Eigen::MatrixXd obstacles_A_;
7 changes: 3 additions & 4 deletions modules/planning/open_space/distance_approach_problem_test.cc
Original file line number Diff line number Diff line change
@@ -39,20 +39,19 @@ class DistanceApproachProblemTest : public ::testing::Test {
Eigen::MatrixXd xf_ = Eigen::MatrixXd::Ones(4, 1);
Eigen::MatrixXd XYbounds_ = Eigen::MatrixXd::Ones(4, 1);
Eigen::MatrixXd last_time_u_ = Eigen::MatrixXd::Zero(2, 1);
Eigen::MatrixXd obstacles_vertices_num_ = Eigen::MatrixXd::Ones(12, 4);
Eigen::MatrixXd obstacles_edges_num_ = Eigen::MatrixXd::Ones(12, 4);
Eigen::MatrixXd xWS_ = Eigen::MatrixXd::Zero(4, horizon_ + 1);
Eigen::MatrixXd uWS_ = Eigen::MatrixXd::Zero(2, horizon_);
std::size_t obstacles_num = 10;
Eigen::MatrixXd obstacles_vertices_num =
Eigen::MatrixXd::Zero(1, horizon_ + 1);
Eigen::MatrixXd obstacles_edges_num = Eigen::MatrixXd::Zero(1, horizon_ + 1);
Eigen::MatrixXd obstacles_A = Eigen::MatrixXd::Ones(4, 1);
Eigen::MatrixXd obstacles_b = Eigen::MatrixXd::Ones(4, 1);
};

TEST_F(DistanceApproachProblemTest, initilization) {
distance_approach_.reset(new DistanceApproachProblem(
x0_, xf_, last_time_u_, horizon_, ts_, ego_, xWS_, uWS_, XYbounds_,
obstacles_num, obstacles_vertices_num, obstacles_A, obstacles_b));
obstacles_num, obstacles_edges_num, obstacles_A, obstacles_b));
EXPECT_NE(distance_approach_, nullptr);
}

0 comments on commit 64ff011

Please sign in to comment.