Apollo 11.0
自动驾驶开放平台
distance_approach_problem.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17/*
18 * @file
19 */
20
21#include "modules/planning/planning_open_space/trajectory_smoother/distance_approach_problem.h"
22
23#include <string>
24#include <unordered_map>
25
26#include "modules/common/util/perf_util.h"
27
28namespace apollo {
29namespace planning {
30
31DistanceApproachProblem::DistanceApproachProblem(const PlannerOpenSpaceConfig& planner_open_space_config) {
32 planner_open_space_config_ = planner_open_space_config;
33 distance_approach_config_ = distance_approach_config_;
34}
35
36DistanceApproachProblem::DistanceApproachProblem(const DistanceApproachConfig& distance_approach_config) {
37 distance_approach_config_ = distance_approach_config;
38}
39
40bool DistanceApproachProblem::Solve(
41 const Eigen::MatrixXd& x0,
42 const Eigen::MatrixXd& xF,
43 const Eigen::MatrixXd& last_time_u,
44 const size_t horizon,
45 const double ts,
46 const Eigen::MatrixXd& ego,
47 const Eigen::MatrixXd& xWS,
48 const Eigen::MatrixXd& uWS,
49 const Eigen::MatrixXd& l_warm_up,
50 const Eigen::MatrixXd& n_warm_up,
51 const Eigen::MatrixXd& s_warm_up,
52 const std::vector<double>& XYbounds,
53 const size_t obstacles_num,
54 const Eigen::MatrixXi& obstacles_edges_num,
55 const Eigen::MatrixXd& obstacles_A,
56 const Eigen::MatrixXd& obstacles_b,
57 Eigen::MatrixXd* state_result,
58 Eigen::MatrixXd* control_result,
59 Eigen::MatrixXd* time_result,
60 Eigen::MatrixXd* dual_l_result,
61 Eigen::MatrixXd* dual_n_result) {
62 // TODO(QiL) : evaluate whether need to new it everytime
63 PERF_BLOCK_START();
64
65 DistanceApproachInterface* ptop = nullptr;
66
67 if (distance_approach_config_.distance_approach_mode() == DISTANCE_APPROACH_IPOPT) {
68 ptop = new DistanceApproachIPOPTInterface(
69 horizon,
70 ts,
71 ego,
72 xWS,
73 uWS,
74 l_warm_up,
75 n_warm_up,
76 x0,
77 xF,
78 last_time_u,
79 XYbounds,
80 obstacles_edges_num,
81 obstacles_num,
82 obstacles_A,
83 obstacles_b,
84 distance_approach_config_);
85 } else if (distance_approach_config_.distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_TS) {
86 ptop = new DistanceApproachIPOPTFixedTsInterface(
87 horizon,
88 ts,
89 ego,
90 xWS,
91 uWS,
92 l_warm_up,
93 n_warm_up,
94 x0,
95 xF,
96 last_time_u,
97 XYbounds,
98 obstacles_edges_num,
99 obstacles_num,
100 obstacles_A,
101 obstacles_b,
102 distance_approach_config_);
103 } else if (distance_approach_config_.distance_approach_mode() == DISTANCE_APPROACH_IPOPT_CUDA) {
104 ptop = new DistanceApproachIPOPTCUDAInterface(
105 horizon,
106 ts,
107 ego,
108 xWS,
109 uWS,
110 l_warm_up,
111 n_warm_up,
112 x0,
113 xF,
114 last_time_u,
115 XYbounds,
116 obstacles_edges_num,
117 obstacles_num,
118 obstacles_A,
119 obstacles_b,
120 distance_approach_config_);
121 } else if (distance_approach_config_.distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_DUAL) {
122 ptop = new DistanceApproachIPOPTFixedDualInterface(
123 horizon,
124 ts,
125 ego,
126 xWS,
127 uWS,
128 l_warm_up,
129 n_warm_up,
130 x0,
131 xF,
132 last_time_u,
133 XYbounds,
134 obstacles_edges_num,
135 obstacles_num,
136 obstacles_A,
137 obstacles_b,
138 distance_approach_config_);
139 } else if (distance_approach_config_.distance_approach_mode() == DISTANCE_APPROACH_IPOPT_RELAX_END) {
140 ptop = new DistanceApproachIPOPTRelaxEndInterface(
141 horizon,
142 ts,
143 ego,
144 xWS,
145 uWS,
146 l_warm_up,
147 n_warm_up,
148 x0,
149 xF,
150 last_time_u,
151 XYbounds,
152 obstacles_edges_num,
153 obstacles_num,
154 obstacles_A,
155 obstacles_b,
156 distance_approach_config_);
157 } else if (distance_approach_config_.distance_approach_mode() == DISTANCE_APPROACH_IPOPT_RELAX_END_SLACK) {
158 ptop = new DistanceApproachIPOPTRelaxEndSlackInterface(
159 horizon,
160 ts,
161 ego,
162 xWS,
163 uWS,
164 l_warm_up,
165 n_warm_up,
166 s_warm_up,
167 x0,
168 xF,
169 last_time_u,
170 XYbounds,
171 obstacles_edges_num,
172 obstacles_num,
173 obstacles_A,
174 obstacles_b,
175 distance_approach_config_);
176 }
177
178 Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;
179
180 // Create an instance of the IpoptApplication
181 Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
182
183 app->Options()->SetIntegerValue("print_level", distance_approach_config_.ipopt_config().ipopt_print_level());
184 app->Options()->SetIntegerValue("mumps_mem_percent", distance_approach_config_.ipopt_config().mumps_mem_percent());
185 app->Options()->SetNumericValue("mumps_pivtol", distance_approach_config_.ipopt_config().mumps_pivtol());
186 app->Options()->SetIntegerValue("max_iter", distance_approach_config_.ipopt_config().ipopt_max_iter());
187 app->Options()->SetNumericValue("tol", distance_approach_config_.ipopt_config().ipopt_tol());
188 app->Options()->SetNumericValue(
189 "acceptable_constr_viol_tol", distance_approach_config_.ipopt_config().ipopt_acceptable_constr_viol_tol());
190 app->Options()->SetNumericValue(
191 "min_hessian_perturbation", distance_approach_config_.ipopt_config().ipopt_min_hessian_perturbation());
192 app->Options()->SetNumericValue(
193 "jacobian_regularization_value",
194 distance_approach_config_.ipopt_config().ipopt_jacobian_regularization_value());
195 app->Options()->SetStringValue(
196 "print_timing_statistics", distance_approach_config_.ipopt_config().ipopt_print_timing_statistics());
197 app->Options()->SetStringValue("alpha_for_y", distance_approach_config_.ipopt_config().ipopt_alpha_for_y());
198 app->Options()->SetStringValue("recalc_y", distance_approach_config_.ipopt_config().ipopt_recalc_y());
199 app->Options()->SetNumericValue("mu_init", distance_approach_config_.ipopt_config().ipopt_mu_init());
200
201 Ipopt::ApplicationReturnStatus status = app->Initialize();
202 if (status != Ipopt::Solve_Succeeded) {
203 AERROR << "*** Distance Approach problem error during initialization!";
204 return false;
205 }
206
207 status = app->OptimizeTNLP(problem);
208
209 if (status == Ipopt::Solve_Succeeded || status == Ipopt::Solved_To_Acceptable_Level) {
210 // Retrieve some statistics about the solve
211 Ipopt::Index iter_count = app->Statistics()->IterationCount();
212 ADEBUG << "*** The problem solved in " << iter_count << " iterations!";
213
214 Ipopt::Number final_obj = app->Statistics()->FinalObjective();
215 ADEBUG << "*** The final value of the objective function is " << final_obj << '.';
216
217 PERF_BLOCK_END("DistanceApproachProblemSolving");
218 } else {
219 /*
220 return detailed failure information,
221 reference resource: Ipopt::ApplicationReturnStatus, https://
222 www.coin-or.org/Doxygen/CoinAll/_ip_return_codes__inc_8h-source.html
223 */
224 std::unordered_map<int, std::string> failure_status
225 = {{0, "Solve_Succeeded"},
226 {1, "Solved_To_Acceptable_Level"},
227 {2, "Infeasible_Problem_Detected"},
228 {3, "Search_Direction_Becomes_Too_Small"},
229 {4, "Diverging_Iterates"},
230 {5, "User_Requested_Stop"},
231 {6, "Feasible_Point_Found"},
232 {-1, "Maximum_Iterations_Exceeded"},
233 {-2, "Restoration_Failed"},
234 {-3, "Error_In_Step_Computation"},
235 {-10, "Not_Enough_Degrees_Of_Freedom"},
236 {-11, "Invalid_Problem_Definition"},
237 {-12, "Invalid_Option"},
238 {-13, "Invalid_Number_Detected"},
239 {-100, "Unrecoverable_Exception"},
240 {-101, "NonIpopt_Exception_Thrown"},
241 {-102, "Insufficient_Memory"},
242 {-199, "Internal_Error"}};
243
244 if (!failure_status.count(static_cast<size_t>(status))) {
245 AINFO << "Solver ends with unknown failure code: " << static_cast<int>(status);
246 } else {
247 AINFO << "Solver failure case: " << failure_status[static_cast<size_t>(status)];
248 }
249 }
250
251 ptop->get_optimization_results(state_result, control_result, time_result, dual_l_result, dual_n_result);
252
253 return status == Ipopt::Solve_Succeeded || status == Ipopt::Solved_To_Acceptable_Level;
254}
255
256} // namespace planning
257} // namespace apollo
apollo::planning::DistanceApproachInterface::get_optimization_results
virtual void get_optimization_results(Eigen::MatrixXd *state_result, Eigen::MatrixXd *control_result, Eigen::MatrixXd *time_result, Eigen::MatrixXd *dual_l_result, Eigen::MatrixXd *dual_n_result) const =0
apollo::planning::DistanceApproachProblem::DistanceApproachProblem
DistanceApproachProblem(const PlannerOpenSpaceConfig &planner_open_space_config)
Definition distance_approach_problem.cc:31
apollo::planning::DistanceApproachProblem::Solve
bool Solve(const Eigen::MatrixXd &x0, const Eigen::MatrixXd &xF, const Eigen::MatrixXd &last_time_u, const size_t horizon, const double ts, const Eigen::MatrixXd &ego, const Eigen::MatrixXd &xWS, const Eigen::MatrixXd &uWS, const Eigen::MatrixXd &l_warm_up, const Eigen::MatrixXd &n_warm_up, const Eigen::MatrixXd &s_warm_up, const std::vector< double > &XYbounds, const size_t obstacles_num, const Eigen::MatrixXi &obstacles_edges_num, const Eigen::MatrixXd &obstacles_A, const Eigen::MatrixXd &obstacles_b, Eigen::MatrixXd *state_result, Eigen::MatrixXd *control_result, Eigen::MatrixXd *time_result, Eigen::MatrixXd *dual_l_result, Eigen::MatrixXd *dual_n_result)
Definition distance_approach_problem.cc:40
planning
Planning module main class.
distance_approach_problem.h
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
apollo
class register implement
Definition arena_queue.h:37
perf_util.h
PERF_BLOCK_START
#define PERF_BLOCK_START()
Definition perf_util.h:126
PERF_BLOCK_END
#define PERF_BLOCK_END
Definition profiler.h:53