From 64ff0110227a059fb82babc48cc01dd9860bade7 Mon Sep 17 00:00:00 2001 From: luoqi06 Date: Tue, 16 Oct 2018 16:39:30 -0700 Subject: [PATCH] Planning : use obstacles_edges_num in distance approach problem --- .../distance_approach_ipopt_interface.cc | 14 +++++++------- .../open_space/distance_approach_ipopt_interface.h | 4 ++-- .../distance_approach_ipopt_interface_test.cc | 6 +++--- .../open_space/distance_approach_problem.cc | 8 ++++---- .../open_space/distance_approach_problem.h | 6 +++--- .../open_space/distance_approach_problem_test.cc | 7 +++---- 6 files changed, 22 insertions(+), 23 deletions(-) diff --git a/modules/planning/open_space/distance_approach_ipopt_interface.cc b/modules/planning/open_space/distance_approach_ipopt_interface.cc index b272453f0bb..75288c47ac8 100644 --- a/modules/planning/open_space/distance_approach_ipopt_interface.cc +++ b/modules/planning/open_space/distance_approach_ipopt_interface.cc @@ -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 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; diff --git a/modules/planning/open_space/distance_approach_ipopt_interface.h b/modules/planning/open_space/distance_approach_ipopt_interface.h index b20632c1608..0e8f7a1a008 100644 --- a/modules/planning/open_space/distance_approach_ipopt_interface.h +++ b/modules/planning/open_space/distance_approach_ipopt_interface.h @@ -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 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_; diff --git a/modules/planning/open_space/distance_approach_ipopt_interface_test.cc b/modules/planning/open_space/distance_approach_ipopt_interface_test.cc index 1cefa5caecd..b119de88dfa 100644 --- a/modules/planning/open_space/distance_approach_ipopt_interface_test.cc +++ b/modules/planning/open_space/distance_approach_ipopt_interface_test.cc @@ -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_)); } diff --git a/modules/planning/open_space/distance_approach_problem.cc b/modules/planning/open_space/distance_approach_problem.cc index 3e6e8958d22..b3fd4b914da 100644 --- a/modules/planning/open_space/distance_approach_problem.cc +++ b/modules/planning/open_space/distance_approach_problem.cc @@ -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 problem = ptop; diff --git a/modules/planning/open_space/distance_approach_problem.h b/modules/planning/open_space/distance_approach_problem.h index d0a0afc3ebb..e59db448c96 100644 --- a/modules/planning/open_space/distance_approach_problem.h +++ b/modules/planning/open_space/distance_approach_problem.h @@ -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_; diff --git a/modules/planning/open_space/distance_approach_problem_test.cc b/modules/planning/open_space/distance_approach_problem_test.cc index 87fb9a14b0c..8378c98f050 100644 --- a/modules/planning/open_space/distance_approach_problem_test.cc +++ b/modules/planning/open_space/distance_approach_problem_test.cc @@ -39,12 +39,11 @@ 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); }; @@ -52,7 +51,7 @@ class DistanceApproachProblemTest : public ::testing::Test { 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); }