Apollo 11.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::planning::CosThetaIpoptInterface类 参考

#include <cos_theta_ipopt_interface.h>

类 apollo::planning::CosThetaIpoptInterface 继承关系图:
apollo::planning::CosThetaIpoptInterface 的协作图:

Public 成员函数

 CosThetaIpoptInterface (std::vector< std::pair< double, double > > points, std::vector< double > bounds)
 
virtual ~CosThetaIpoptInterface ()=default
 
void set_weight_cos_included_angle (const double weight_cos_included_angle)
 
void set_weight_anchor_points (const double weight_anchor_points)
 
void set_weight_length (const double weight_length)
 
void set_automatic_differentiation_flag (const bool use_ad)
 
void get_optimization_results (std::vector< double > *ptr_x, std::vector< double > *ptr_y) const
 
bool get_nlp_info (int &n, int &m, int &nnz_jac_g, int &nnz_h_lag, IndexStyleEnum &index_style) override
 Method to return some info about the nlp
 
bool get_bounds_info (int n, double *x_l, double *x_u, int m, double *g_l, double *g_u) override
 Method to return the bounds for my problem
 
bool get_starting_point (int n, bool init_x, double *x, bool init_z, double *z_L, double *z_U, int m, bool init_lambda, double *lambda) override
 Method to return the starting point for the algorithm
 
bool eval_f (int n, const double *x, bool new_x, double &obj_value) override
 Method to return the objective value
 
bool eval_grad_f (int n, const double *x, bool new_x, double *grad_f) override
 Method to return the gradient of the objective
 
bool eval_g (int n, const double *x, bool new_x, int m, double *g) override
 Method to return the constraint residuals
 
bool eval_jac_g (int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values) override
 Method to return: 1) The structure of the jacobian (if "values" is nullptr) 2) The values of the jacobian (if "values" is not nullptr)
 
bool eval_h (int n, const double *x, bool new_x, double obj_factor, int m, const double *lambda, bool new_lambda, int nele_hess, int *iRow, int *jCol, double *values) override
 Method to return: 1) The structure of the hessian of the lagrangian (if "values" is nullptr) 2) The values of the hessian of the lagrangian (if "values" is not nullptr)
 

Solution Methods

void finalize_solution (Ipopt::SolverReturn status, int n, const double *x, const double *z_L, const double *z_U, int m, const double *g, const double *lambda, double obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq) override
 This method is called when the algorithm is complete so the TNLP can store/write the solution
 
template<class T >
bool eval_obj (int n, const T *x, T *obj_value)
 Template to return the objective value
 
template<class T >
bool eval_constraints (int n, const T *x, int m, T *g)
 Template to compute contraints
 
virtual void generate_tapes (int n, int m, int *nnz_jac_g, int *nnz_h_lag)
 Method to generate the required tapes
 

详细描述

在文件 cos_theta_ipopt_interface.h 第 44 行定义.

构造及析构函数说明

◆ CosThetaIpoptInterface()

apollo::planning::CosThetaIpoptInterface::CosThetaIpoptInterface ( std::vector< std::pair< double, double > >  points,
std::vector< double >  bounds 
)

在文件 cos_theta_ipopt_interface.cc 第 29 行定义.

30 {
31 CHECK_GT(points.size(), 1U);
32 CHECK_GT(bounds.size(), 1U);
33 bounds_ = std::move(bounds);
34 ref_points_ = std::move(points);
35 num_of_points_ = ref_points_.size();
36}

◆ ~CosThetaIpoptInterface()

virtual apollo::planning::CosThetaIpoptInterface::~CosThetaIpoptInterface ( )
virtualdefault

成员函数说明

◆ eval_constraints()

template<class T >
bool apollo::planning::CosThetaIpoptInterface::eval_constraints ( int  n,
const T *  x,
int  m,
T *  g 
)

Template to compute contraints

在文件 cos_theta_ipopt_interface.cc 第 640 行定义.

640 {
641 // fill in the positional deviation constraints
642 for (size_t i = 0; i < num_of_points_; ++i) {
643 size_t index = i << 1;
644 g[index] = x[index];
645 g[index + 1] = x[index + 1];
646 }
647 return true;
648}

◆ eval_f()

bool apollo::planning::CosThetaIpoptInterface::eval_f ( int  n,
const double *  x,
bool  new_x,
double &  obj_value 
)
override

Method to return the objective value

在文件 cos_theta_ipopt_interface.cc 第 133 行定义.

134 {
135 CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
136 if (use_automatic_differentiation_) {
137 eval_obj(n, x, &obj_value);
138 return true;
139 }
140
141 obj_value = 0.0;
142 for (size_t i = 0; i < num_of_points_; ++i) {
143 size_t index = i << 1;
144 obj_value +=
145 (x[index] - ref_points_[i].first) * (x[index] - ref_points_[i].first) +
146 (x[index + 1] - ref_points_[i].second) *
147 (x[index + 1] - ref_points_[i].second);
148 }
149 for (size_t i = 0; i < num_of_points_ - 2; i++) {
150 size_t findex = i << 1;
151 size_t mindex = findex + 2;
152 size_t lindex = mindex + 2;
153 obj_value -=
154 weight_cos_included_angle_ *
155 (((x[mindex] - x[findex]) * (x[lindex] - x[mindex])) +
156 ((x[mindex + 1] - x[findex + 1]) * (x[lindex + 1] - x[mindex + 1]))) /
157 std::sqrt((x[mindex] - x[findex]) * (x[mindex] - x[findex]) +
158 (x[mindex + 1] - x[findex + 1]) *
159 (x[mindex + 1] - x[findex + 1])) /
160 std::sqrt((x[lindex] - x[mindex]) * (x[lindex] - x[mindex]) +
161 (x[lindex + 1] - x[mindex + 1]) *
162 (x[lindex + 1] - x[mindex + 1]));
163 }
164 return true;
165}
apollo::planning::CosThetaIpoptInterface::eval_obj
bool eval_obj(int n, const T *x, T *obj_value)
Template to return the objective value
Definition cos_theta_ipopt_interface.cc:600

◆ eval_g()

bool apollo::planning::CosThetaIpoptInterface::eval_g ( int  n,
const double *  x,
bool  new_x,
int  m,
double *  g 
)
override

Method to return the constraint residuals

在文件 cos_theta_ipopt_interface.cc 第 232 行定义.

233 {
234 CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
235 CHECK_EQ(static_cast<size_t>(m), num_of_constraints_);
236
237 if (use_automatic_differentiation_) {
238 eval_constraints(n, x, m, g);
239 return true;
240 }
241 // fill in the positional deviation constraints
242 for (size_t i = 0; i < num_of_points_; ++i) {
243 size_t index = i << 1;
244 g[index] = x[index];
245 g[index + 1] = x[index + 1];
246 }
247 return true;
248}
apollo::planning::CosThetaIpoptInterface::eval_constraints
bool eval_constraints(int n, const T *x, int m, T *g)
Template to compute contraints
Definition cos_theta_ipopt_interface.cc:640

◆ eval_grad_f()

bool apollo::planning::CosThetaIpoptInterface::eval_grad_f ( int  n,
const double *  x,
bool  new_x,
double *  grad_f 
)
override

Method to return the gradient of the objective

在文件 cos_theta_ipopt_interface.cc 第 167 行定义.

168 {
169 CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
170
171 if (use_automatic_differentiation_) {
172 gradient(tag_f, n, x, grad_f);
173 return true;
174 }
175
176 std::fill(grad_f, grad_f + n, 0.0);
177 for (size_t i = 0; i < num_of_points_; ++i) {
178 size_t index = i << 1;
179 grad_f[index] = x[index] * 2 - ref_points_[i].first * 2;
180 grad_f[index + 1] = x[index + 1] * 2 - ref_points_[i].second * 2;
181 }
182
183 for (size_t i = 0; i < num_of_points_ - 2; ++i) {
184 size_t index = i << 1;
185 double q1 = (x[index] - x[index + 2]) * (x[index] - x[index + 2]) +
186 (x[index + 1] - x[index + 3]) * (x[index + 1] - x[index + 3]);
187 double q2 = (x[index + 2] - x[index + 4]) * (x[index + 2] - x[index + 4]) +
188 (x[index + 3] - x[index + 5]) * (x[index + 3] - x[index + 5]);
189 double q3 =
190 ((2 * x[index] - 2 * x[index + 2]) *
191 ((x[index] - x[index + 2]) * (x[index + 2] - x[index + 4]) +
192 (x[index + 1] - x[index + 3]) * (x[index + 3] - x[index + 5])));
193 double q4 =
194 ((2 * x[index + 1] - 2 * x[index + 3]) *
195 ((x[index] - x[index + 2]) * (x[index + 2] - x[index + 4]) +
196 (x[index + 1] - x[index + 3]) * (x[index + 3] - x[index + 5])));
197 double q5 =
198 ((2 * x[index + 2] - 2 * x[index + 4]) *
199 ((x[index] - x[index + 2]) * (x[index + 2] - x[index + 4]) +
200 (x[index + 1] - x[index + 3]) * (x[index + 3] - x[index + 5])));
201 double q6 =
202 ((2 * x[index + 3] - 2 * x[index + 5]) *
203 ((x[index] - x[index + 2]) * (x[index + 2] - x[index + 4]) +
204 (x[index + 1] - x[index + 3]) * (x[index + 3] - x[index + 5])));
205 double sqrt_q1 = std::sqrt(q1);
206 double sqrt_q2 = std::sqrt(q2);
207 grad_f[index] += -weight_cos_included_angle_ *
208 ((x[index + 2] - x[index + 4]) / (sqrt_q1 * sqrt_q2) -
209 q3 / (2 * q1 * sqrt_q1 * sqrt_q2));
210 grad_f[index + 1] += -weight_cos_included_angle_ *
211 ((x[index + 3] - x[index + 5]) / (sqrt_q1 * sqrt_q2) -
212 q4 / (2 * q1 * sqrt_q1 * sqrt_q2));
213 grad_f[index + 2] +=
214 -weight_cos_included_angle_ *
215 ((x[index] - 2 * x[index + 2] + x[index + 4]) / (sqrt_q1 * sqrt_q2) +
216 q3 / (2 * q1 * sqrt_q1 * sqrt_q2) - q5 / (2 * sqrt_q1 * q2 * sqrt_q2));
217 grad_f[index + 3] +=
218 -weight_cos_included_angle_ *
219 ((x[index + 1] - 2 * x[index + 3] + x[index + 5]) /
220 (sqrt_q1 * sqrt_q2) +
221 q4 / (2 * q1 * sqrt_q1 * sqrt_q2) - q6 / (2 * sqrt_q1 * q2 * sqrt_q2));
222 grad_f[index + 4] += -weight_cos_included_angle_ *
223 (q5 / (2 * sqrt_q1 * q2 * sqrt_q2) -
224 (x[index] - x[index + 2]) / (sqrt_q1 * sqrt_q2));
225 grad_f[index + 5] += -weight_cos_included_angle_ *
226 (q6 / (2 * sqrt_q1 * q2 * sqrt_q2) -
227 (x[index + 1] - x[index + 3]) / (sqrt_q1 * sqrt_q2));
228 }
229 return true;
230}
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36

◆ eval_h()

bool apollo::planning::CosThetaIpoptInterface::eval_h ( int  n,
const double *  x,
bool  new_x,
double  obj_factor,
int  m,
const double *  lambda,
bool  new_lambda,
int  nele_hess,
int *  iRow,
int *  jCol,
double *  values 
)
override

Method to return: 1) The structure of the hessian of the lagrangian (if "values" is nullptr) 2) The values of the hessian of the lagrangian (if "values" is not nullptr)

在文件 cos_theta_ipopt_interface.cc 第 293 行定义.

297 {
298 if (use_automatic_differentiation_) {
299 if (values == nullptr) {
300 // return the structure. This is a symmetric matrix, fill the lower left
301 // triangle only.
302 for (int idx = 0; idx < nnz_L_; idx++) {
303 iRow[idx] = rind_L_[idx];
304 jCol[idx] = cind_L_[idx];
305 }
306 } else {
307 // return the values. This is a symmetric matrix, fill the lower left
308 // triangle only
309 obj_lam_[0] = obj_factor;
310 for (int idx = 0; idx < m; idx++) {
311 obj_lam_[1 + idx] = lambda[idx];
312 }
313 // auto* rind_L = &rind_L_[0];
314 // auto* cind_L = &cind_L_[0];
315 // auto* hessval = &hessval_[0];
316 set_param_vec(tag_L, m + 1, &obj_lam_[0]);
317 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L_, &rind_L_,
318 &cind_L_, &hessval_, options_L_);
319
320 for (int idx = 0; idx < nnz_L_; idx++) {
321 values[idx] = hessval_[idx];
322 }
323 }
324 return true;
325 }
326
327 if (values == nullptr) {
328 int index = 0;
329 for (int i = 0; i < 6; ++i) {
330 for (int j = 0; j <= i; ++j) {
331 iRow[index] = i;
332 jCol[index] = j;
333 index++;
334 }
335 }
336
337 int shift = 0;
338 for (int i = 6; i < static_cast<int>(num_of_variables_); ++i) {
339 if (i % 2 == 0) {
340 for (int j = 2 + shift; j <= 6 + shift; ++j) {
341 iRow[index] = i;
342 jCol[index] = j;
343 index++;
344 }
345
346 } else {
347 for (int j = 2 + shift; j <= 7 + shift; ++j) {
348 iRow[index] = i;
349 jCol[index] = j;
350 index++;
351 }
352 shift += 2;
353 }
354 }
355 CHECK_EQ(index, nele_hess);
356 } else {
357 std::fill(values, values + nele_hess, 0.0);
358 // fill the included angle part of obj
359 for (size_t i = 0; i < num_of_points_ - 2; ++i) {
360 size_t topleft = i * 2;
361 const double q5 = (2 * x[topleft] - 2 * x[topleft + 2]);
362 const double q6 = (2 * x[topleft + 1] - 2 * x[topleft + 3]);
363 const double q7 = (x[topleft] - 2 * x[topleft + 2] + x[topleft + 4]);
364 const double q8 = (2 * x[topleft + 2] - 2 * x[topleft + 4]);
365 const double q9 = (2 * x[topleft + 3] - 2 * x[topleft + 5]);
366 const double q10 = (x[topleft + 1] - 2 * x[topleft + 3] + x[topleft + 5]);
367 const double q11 = (x[topleft + 3] - x[topleft + 5]);
368 const double q12 = (x[topleft] - x[topleft + 2]);
369 const double q13 = (x[topleft + 2] - x[topleft + 4]);
370 const double q14 = (x[topleft + 1] - x[topleft + 3]);
371 const double q1 = q12 * q12 + q14 * q14;
372 const double q2 = q13 * q13 + q11 * q11;
373 const double q3 = (3 * q5 * q5 * (q12 * q13 + q14 * q11));
374 const double q4 = (q12 * q13 + q14 * q11);
375 const double sqrt_q1 = std::sqrt(q1);
376 const double q1_sqrt_q1 = q1 * sqrt_q1;
377 const double sqrt_q2 = std::sqrt(q2);
378 const double q2_sqrt_q2 = q2 * sqrt_q2;
379 const double square_q1 = q1 * q1;
380 const double square_q2 = q2 * q2;
381 const double sqrt_q1q2 = sqrt_q1 * sqrt_q2;
382 const double q1_sqrt_q1q2 = q1 * sqrt_q1q2;
383 const double sqrt_q1_q2_sqrt_q2 = sqrt_q1 * q2_sqrt_q2;
384 const double q1_sqrt_q1_q2_sqrt_q2 = q1_sqrt_q1 * q2_sqrt_q2;
385 values[idx_map_[std::make_pair(topleft, topleft)]] +=
386 obj_factor * (-weight_cos_included_angle_) *
387 (q3 / (4 * square_q1 * sqrt_q1q2) - q4 / q1_sqrt_q1q2 -
388 (q5 * q13) / q1_sqrt_q1q2);
389 values[idx_map_[std::make_pair(topleft + 1, topleft)]] +=
390 obj_factor * (-weight_cos_included_angle_) *
391 ((3 * q5 * q6 * q4) / (4 * square_q1 * sqrt_q1q2) -
392 (q6 * q13) / (2 * q1_sqrt_q1q2) - (q5 * q11) / (2 * q1_sqrt_q1q2));
393 values[idx_map_[std::make_pair(topleft + 2, topleft)]] +=
394 obj_factor * (-weight_cos_included_angle_) *
395 (1 / sqrt_q1q2 + q4 / q1_sqrt_q1q2 - (q5 * q7) / (2 * q1_sqrt_q1q2) -
396 q3 / (4 * square_q1 * sqrt_q1q2) + (q5 * q13) / (2 * q1_sqrt_q1q2) -
397 (q8 * q13) / (2 * sqrt_q1_q2_sqrt_q2) +
398 (q5 * q8 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
399 values[idx_map_[std::make_pair(topleft + 3, topleft)]] +=
400 obj_factor * (-weight_cos_included_angle_) *
401 ((q6 * q13) / (2 * q1_sqrt_q1q2) - (q5 * q10) / (2 * q1_sqrt_q1q2) -
402 (q9 * q13) / (2 * sqrt_q1_q2_sqrt_q2) -
403 (3 * q5 * q6 * q4) / (4 * square_q1 * sqrt_q1q2) +
404 (q5 * q9 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
405 values[idx_map_[std::make_pair(topleft + 4, topleft)]] +=
406 obj_factor * (-weight_cos_included_angle_) *
407 ((q5 * q12) / (2 * q1_sqrt_q1q2) - 1 / sqrt_q1q2 +
408 (q8 * q13) / (2 * sqrt_q1_q2_sqrt_q2) -
409 (q5 * q8 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
410 values[idx_map_[std::make_pair(topleft + 5, topleft)]] +=
411 obj_factor * (-weight_cos_included_angle_) *
412 ((q5 * q14) / (2 * q1_sqrt_q1q2) +
413 (q9 * q13) / (2 * sqrt_q1_q2_sqrt_q2) -
414 (q5 * q9 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
415
416 values[idx_map_[std::make_pair(topleft + 1, topleft + 1)]] +=
417 obj_factor * (-weight_cos_included_angle_) *
418 ((3 * q6 * q6 * q4) / (4 * square_q1 * sqrt_q1q2) -
419 q4 / q1_sqrt_q1q2 - (q6 * q11) / q1_sqrt_q1q2);
420 values[idx_map_[std::make_pair(topleft + 2, topleft + 1)]] +=
421 obj_factor * (-weight_cos_included_angle_) *
422 ((q5 * q11) / (2 * q1_sqrt_q1q2) - (q6 * q7) / (2 * q1_sqrt_q1q2) -
423 (q8 * q11) / (2 * sqrt_q1_q2_sqrt_q2) -
424 (3 * q5 * q6 * q4) / (4 * square_q1 * sqrt_q1q2) +
425 (q8 * q6 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
426 values[idx_map_[std::make_pair(topleft + 3, topleft + 1)]] +=
427 obj_factor * (-weight_cos_included_angle_) *
428 (1 / sqrt_q1q2 + q4 / q1_sqrt_q1q2 - (q6 * q10) / (2 * q1_sqrt_q1q2) -
429 (3 * q6 * q6 * q4) / (4 * square_q1 * sqrt_q1q2) +
430 (q6 * q11) / (2 * q1_sqrt_q1q2) -
431 (q9 * q11) / (2 * sqrt_q1_q2_sqrt_q2) +
432 (q6 * q9 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
433 values[idx_map_[std::make_pair(topleft + 4, topleft + 1)]] +=
434 obj_factor * (-weight_cos_included_angle_) *
435 ((q6 * q12) / (2 * q1_sqrt_q1q2) +
436 (q8 * q11) / (2 * sqrt_q1_q2_sqrt_q2) -
437 (q8 * q6 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
438 values[idx_map_[std::make_pair(topleft + 5, topleft + 1)]] +=
439 obj_factor * (-weight_cos_included_angle_) *
440 ((q6 * q14) / (2 * q1_sqrt_q1q2) - 1 / sqrt_q1q2 +
441 (q9 * q11) / (2 * sqrt_q1_q2_sqrt_q2) -
442 (q6 * q9 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
443
444 values[idx_map_[std::make_pair(topleft + 2, topleft + 2)]] +=
445 obj_factor * (-weight_cos_included_angle_) *
446 ((q5 * q7) / q1_sqrt_q1q2 -
447 q4 / (sqrt_q1_q2_sqrt_q2)-q4 / q1_sqrt_q1q2 - 2 / sqrt_q1q2 -
448 (q8 * q7) / (sqrt_q1_q2_sqrt_q2) + q3 / (4 * square_q1 * sqrt_q1q2) +
449 (3 * q8 * q8 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2) -
450 (q5 * q8 * q4) / (2 * q1_sqrt_q1_q2_sqrt_q2));
451 values[idx_map_[std::make_pair(topleft + 3, topleft + 2)]] +=
452 obj_factor * (-weight_cos_included_angle_) *
453 ((q6 * q7) / (2 * q1_sqrt_q1q2) -
454 (q9 * q7) / (2 * sqrt_q1_q2_sqrt_q2) +
455 (q5 * q10) / (2 * q1_sqrt_q1q2) -
456 (q8 * q10) / (2 * sqrt_q1_q2_sqrt_q2) +
457 (3 * q5 * q6 * q4) / (4 * square_q1 * sqrt_q1q2) -
458 (q5 * q9 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2) -
459 (q8 * q6 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2) +
460 (3 * q8 * q9 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2));
461 values[idx_map_[std::make_pair(topleft + 4, topleft + 2)]] +=
462 obj_factor * (-weight_cos_included_angle_) *
463 (1 / sqrt_q1q2 + q4 / (sqrt_q1_q2_sqrt_q2) +
464 (q8 * q7) / (2 * sqrt_q1_q2_sqrt_q2) -
465 (3 * q8 * q8 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2) -
466 (q5 * q12) / (2 * q1_sqrt_q1q2) +
467 (q8 * q12) / (2 * sqrt_q1_q2_sqrt_q2) +
468 (q5 * q8 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
469 values[idx_map_[std::make_pair(topleft + 5, topleft + 2)]] +=
470 obj_factor * (-weight_cos_included_angle_) *
471 ((q9 * q7) / (2 * sqrt_q1_q2_sqrt_q2) -
472 (q5 * q14) / (2 * q1_sqrt_q1q2) +
473 (q8 * q14) / (2 * sqrt_q1_q2_sqrt_q2) +
474 (q5 * q9 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2) -
475 (3 * q8 * q9 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2));
476
477 values[idx_map_[std::make_pair(topleft + 3, topleft + 3)]] +=
478 obj_factor * (-weight_cos_included_angle_) *
479 ((q6 * q10) / q1_sqrt_q1q2 -
480 q4 / (sqrt_q1_q2_sqrt_q2)-q4 / q1_sqrt_q1q2 - 2 / sqrt_q1q2 -
481 (q9 * q10) / (sqrt_q1_q2_sqrt_q2) +
482 (3 * q6 * q6 * q4) / (4 * square_q1 * sqrt_q1q2) +
483 (3 * q9 * q9 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2) -
484 (q6 * q9 * q4) / (2 * q1_sqrt_q1_q2_sqrt_q2));
485 values[idx_map_[std::make_pair(topleft + 4, topleft + 3)]] +=
486 obj_factor * (-weight_cos_included_angle_) *
487 ((q8 * q10) / (2 * sqrt_q1_q2_sqrt_q2) -
488 (q6 * q12) / (2 * q1_sqrt_q1q2) +
489 (q9 * q12) / (2 * sqrt_q1_q2_sqrt_q2) +
490 (q8 * q6 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2) -
491 (3 * q8 * q9 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2));
492 values[idx_map_[std::make_pair(topleft + 5, topleft + 3)]] +=
493 obj_factor * (-weight_cos_included_angle_) *
494 (1 / sqrt_q1q2 + q4 / (sqrt_q1_q2_sqrt_q2) +
495 (q9 * q10) / (2 * sqrt_q1_q2_sqrt_q2) -
496 (3 * q9 * q9 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2) -
497 (q6 * q14) / (2 * q1_sqrt_q1q2) +
498 (q9 * q14) / (2 * sqrt_q1_q2_sqrt_q2) +
499 (q6 * q9 * q4) / (4 * q1_sqrt_q1_q2_sqrt_q2));
500
501 values[idx_map_[std::make_pair(topleft + 4, topleft + 4)]] +=
502 obj_factor * (-weight_cos_included_angle_) *
503 ((3 * q8 * q8 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2) -
504 q4 / (sqrt_q1_q2_sqrt_q2) - (q8 * q12) / (sqrt_q1_q2_sqrt_q2));
505 values[idx_map_[std::make_pair(topleft + 5, topleft + 4)]] +=
506 obj_factor * (-weight_cos_included_angle_) *
507 ((3 * q8 * q9 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2) -
508 (q9 * q12) / (2 * sqrt_q1_q2_sqrt_q2) -
509 (q8 * q14) / (2 * sqrt_q1_q2_sqrt_q2));
510
511 values[idx_map_[std::make_pair(topleft + 5, topleft + 5)]] +=
512 obj_factor * (-weight_cos_included_angle_) *
513 ((3 * q9 * q9 * q4) / (4 * sqrt_q1 * square_q2 * sqrt_q2) -
514 q4 / (sqrt_q1_q2_sqrt_q2) - (q9 * q14) / (sqrt_q1_q2_sqrt_q2));
515 }
516
517 // fill the deviation part of obj
518 for (size_t i = 0; i < num_of_variables_; ++i) {
519 values[idx_map_[std::make_pair(i, i)]] += obj_factor * 2;
520 }
521 }
522 return true;
523}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38

◆ eval_jac_g()

bool apollo::planning::CosThetaIpoptInterface::eval_jac_g ( int  n,
const double *  x,
bool  new_x,
int  m,
int  nele_jac,
int *  iRow,
int *  jCol,
double *  values 
)
override

Method to return: 1) The structure of the jacobian (if "values" is nullptr) 2) The values of the jacobian (if "values" is not nullptr)

在文件 cos_theta_ipopt_interface.cc 第 250 行定义.

252 {
253 CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
254 CHECK_EQ(static_cast<size_t>(m), num_of_constraints_);
255
256 if (use_automatic_differentiation_) {
257 if (values == nullptr) {
258 // return the structure of the jacobian
259 for (int idx = 0; idx < nnz_jac_; idx++) {
260 iRow[idx] = rind_g_[idx];
261 jCol[idx] = cind_g_[idx];
262 }
263 } else {
264 // return the values of the jacobian of the constraints
265 // auto* rind_g = &rind_g_[0];
266 // auto* cind_g = &cind_g_[0];
267 // auto* jacval = &jacval_[0];
268 sparse_jac(tag_g, m, n, 1, x, &nnz_jac_, &rind_g_, &cind_g_, &jacval_,
269 options_g_);
270 for (int idx = 0; idx < nnz_jac_; idx++) {
271 values[idx] = jacval_[idx];
272 }
273 }
274 return true;
275 }
276
277 if (values == nullptr) {
278 // positional deviation constraints
279 for (int i = 0; i < static_cast<int>(num_of_variables_); ++i) {
280 iRow[i] = i;
281 jCol[i] = i;
282 }
283 } else {
284 std::fill(values, values + nnz_jac_g_, 0.0);
285 // positional deviation constraints
286 for (size_t i = 0; i < num_of_variables_; ++i) {
287 values[i] = 1;
288 }
289 }
290 return true;
291}
tag_g
#define tag_g
Definition cos_theta_ipopt_interface.h:37

◆ eval_obj()

template<class T >
bool apollo::planning::CosThetaIpoptInterface::eval_obj ( int  n,
const T *  x,
T *  obj_value 
)

Template to return the objective value

在文件 cos_theta_ipopt_interface.cc 第 600 行定义.

600 {
601 *obj_value = 0.0;
602 for (size_t i = 0; i < num_of_points_; ++i) {
603 size_t index = i << 1;
604 *obj_value +=
605 weight_anchor_points_ *
606 ((x[index] - ref_points_[i].first) * (x[index] - ref_points_[i].first) +
607 (x[index + 1] - ref_points_[i].second) *
608 (x[index + 1] - ref_points_[i].second));
609 }
610 for (size_t i = 0; i < num_of_points_ - 2; ++i) {
611 size_t findex = i << 1;
612 size_t mindex = findex + 2;
613 size_t lindex = mindex + 2;
614 *obj_value -=
615 weight_cos_included_angle_ *
616 (((x[mindex] - x[findex]) * (x[lindex] - x[mindex])) +
617 ((x[mindex + 1] - x[findex + 1]) * (x[lindex + 1] - x[mindex + 1]))) /
618 (sqrt((x[mindex] - x[findex]) * (x[mindex] - x[findex]) +
619 (x[mindex + 1] - x[findex + 1]) *
620 (x[mindex + 1] - x[findex + 1])) *
621 sqrt((x[lindex] - x[mindex]) * (x[lindex] - x[mindex]) +
622 (x[lindex + 1] - x[mindex + 1]) *
623 (x[lindex + 1] - x[mindex + 1])));
624 }
625
626 // Total length
627 for (size_t i = 0; i < num_of_points_ - 1; ++i) {
628 size_t findex = i << 1;
629 size_t nindex = findex + 2;
630 *obj_value +=
631 weight_length_ *
632 ((x[findex] - x[nindex]) * (x[findex] - x[nindex]) +
633 (x[findex + 1] - x[nindex + 1]) * (x[findex + 1] - x[nindex + 1]));
634 }
635 return true;
636}

◆ finalize_solution()

void apollo::planning::CosThetaIpoptInterface::finalize_solution ( Ipopt::SolverReturn  status,
int  n,
const double *  x,
const double *  z_L,
const double *  z_U,
int  m,
const double *  g,
const double *  lambda,
double  obj_value,
const Ipopt::IpoptData *  ip_data,
Ipopt::IpoptCalculatedQuantities *  ip_cq 
)
override

This method is called when the algorithm is complete so the TNLP can store/write the solution

在文件 cos_theta_ipopt_interface.cc 第 525 行定义.

529 {
530 opt_x_.reserve(num_of_points_);
531 opt_y_.reserve(num_of_points_);
532 for (size_t i = 0; i < num_of_points_; ++i) {
533 size_t index = i << 1;
534 opt_x_.emplace_back(x[index]);
535 opt_y_.emplace_back(x[index + 1]);
536 }
537 if (use_automatic_differentiation_) {
538 free(rind_g_);
539 free(cind_g_);
540 free(rind_L_);
541 free(cind_L_);
542 free(jacval_);
543 free(hessval_);
544 }
545}

◆ generate_tapes()

void apollo::planning::CosThetaIpoptInterface::generate_tapes ( int  n,
int  m,
int *  nnz_jac_g,
int *  nnz_h_lag 
)
virtual

Method to generate the required tapes

在文件 cos_theta_ipopt_interface.cc 第 651 行定义.

652 {
653 std::vector<double> xp(n, 0.0);
654 std::vector<double> lamp(m, 0.0);
655 std::vector<double> zl(m, 0.0);
656 std::vector<double> zu(m, 0.0);
657 std::vector<adouble> xa(n, 0.0);
658 std::vector<adouble> g(m, 0.0);
659 std::vector<double> lam(m, 0.0);
660
661 double sig = 0.0;
662 adouble obj_value;
663
664 double dummy = 0.0;
665 obj_lam_.clear();
666 obj_lam_.resize(m + 1, 0.0);
667 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
668
669 // Trace on Objectives
670 trace_on(tag_f);
671 for (int idx = 0; idx < n; idx++) {
672 xa[idx] <<= xp[idx];
673 }
674 eval_obj(n, &xa[0], &obj_value);
675 obj_value >>= dummy;
676 trace_off();
677
678 // Trace on Jacobian
679 trace_on(tag_g);
680 for (int idx = 0; idx < n; idx++) {
681 xa[idx] <<= xp[idx];
682 }
683 eval_constraints(n, &xa[0], m, &g[0]);
684 for (int idx = 0; idx < m; idx++) {
685 g[idx] >>= dummy;
686 }
687 trace_off();
688
689 // Trace on Hessian
690 trace_on(tag_L);
691 for (int idx = 0; idx < n; idx++) {
692 xa[idx] <<= xp[idx];
693 }
694 for (int idx = 0; idx < m; idx++) {
695 lam[idx] = 1.0;
696 }
697 sig = 1.0;
698 eval_obj(n, &xa[0], &obj_value);
699 obj_value *= mkparam(sig);
700 eval_constraints(n, &xa[0], m, &g[0]);
701 for (int idx = 0; idx < m; idx++) {
702 obj_value += g[idx] * mkparam(lam[idx]);
703 }
704 obj_value >>= dummy;
705 trace_off();
706
707 // rind_g_.clear();
708 // cind_g_.clear();
709 // rind_L_.clear();
710 // cind_L_.clear();
711 rind_g_ = nullptr;
712 cind_g_ = nullptr;
713 rind_L_ = nullptr;
714 cind_L_ = nullptr;
715
716 options_g_[0] = 0; /* sparsity pattern by index domains (default) */
717 options_g_[1] = 0; /* safe mode (default) */
718 options_g_[2] = 0;
719 options_g_[3] = 0; /* column compression (default) */
720
721 // jacval_.clear();
722 // hessval_.clear();
723 jacval_ = nullptr;
724 hessval_ = nullptr;
725
726 // auto* rind_g = &rind_g_[0];
727 // auto* cind_g = &cind_g_[0];
728 // auto* jacval = &jacval_[0];
729 // auto* rind_L = &rind_L_[0];
730 // auto* cind_L = &cind_L_[0];
731 // auto* hessval = &hessval_[0];
732
733 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac_, &rind_g_, &cind_g_, &jacval_,
734 options_g_);
735 *nnz_jac_g = nnz_jac_;
736 options_L_[0] = 0;
737 options_L_[1] = 1;
738 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L_, &rind_L_, &cind_L_, &hessval_,
739 options_L_);
740 *nnz_h_lag = nnz_L_;
741}
apollo::planning::CosThetaIpoptInterface::get_starting_point
bool get_starting_point(int n, bool init_x, double *x, bool init_z, double *z_L, double *z_U, int m, bool init_lambda, double *lambda) override
Method to return the starting point for the algorithm
Definition cos_theta_ipopt_interface.cc:116

◆ get_bounds_info()

bool apollo::planning::CosThetaIpoptInterface::get_bounds_info ( int  n,
double *  x_l,
double *  x_u,
int  m,
double *  g_l,
double *  g_u 
)
override

Method to return the bounds for my problem

在文件 cos_theta_ipopt_interface.cc 第 74 行定义.

75 {
76 CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
77 CHECK_EQ(static_cast<size_t>(m), num_of_constraints_);
78 // variables
79 // a. for x, y
80 for (size_t i = 0; i < num_of_points_; ++i) {
81 size_t index = i << 1;
82 // x
83 x_l[index] = -1e20;
84 x_u[index] = 1e20;
85
86 // y
87 x_l[index + 1] = -1e20;
88 x_u[index + 1] = 1e20;
89 }
90
91 // constraints
92 // positional deviation constraints
93 for (size_t i = 0; i < num_of_points_; ++i) {
94 size_t index = i << 1;
95 double x_lower = 0.0;
96 double x_upper = 0.0;
97 double y_lower = 0.0;
98 double y_upper = 0.0;
99
100 x_lower = ref_points_[i].first - bounds_[i];
101 x_upper = ref_points_[i].first + bounds_[i];
102 y_lower = ref_points_[i].second - bounds_[i];
103 y_upper = ref_points_[i].second + bounds_[i];
104
105 // x
106 g_l[index] = x_lower;
107 g_u[index] = x_upper;
108
109 // y
110 g_l[index + 1] = y_lower;
111 g_u[index + 1] = y_upper;
112 }
113 return true;
114}

◆ get_nlp_info()

bool apollo::planning::CosThetaIpoptInterface::get_nlp_info ( int &  n,
int &  m,
int &  nnz_jac_g,
int &  nnz_h_lag,
IndexStyleEnum &  index_style 
)
override

Method to return some info about the nlp

在文件 cos_theta_ipopt_interface.cc 第 44 行定义.

46 {
47 // number of variables
48 n = static_cast<int>(num_of_points_ << 1);
49 num_of_variables_ = n;
50
51 // number of constraints
52 m = static_cast<int>(num_of_points_ << 1);
53 num_of_constraints_ = m;
54
55 if (use_automatic_differentiation_) {
56 generate_tapes(n, m, &nnz_jac_g, &nnz_h_lag);
57 } else {
58 // number of nonzero constraint jacobian.
59 nnz_jac_g = static_cast<int>(num_of_points_ << 1);
60 nnz_jac_g_ = nnz_jac_g;
61
62 // number of nonzero hessian and lagrangian.
63 nnz_h_lag = static_cast<int>(num_of_points_ * 11 - 12);
64 nnz_h_lag_ = nnz_h_lag;
65
66 // load hessian structure
67 hessian_strcuture();
68 }
69
70 index_style = IndexStyleEnum::C_STYLE;
71 return true;
72}
apollo::planning::CosThetaIpoptInterface::generate_tapes
virtual void generate_tapes(int n, int m, int *nnz_jac_g, int *nnz_h_lag)
Method to generate the required tapes
Definition cos_theta_ipopt_interface.cc:651

◆ get_optimization_results()

void apollo::planning::CosThetaIpoptInterface::get_optimization_results ( std::vector< double > *  ptr_x,
std::vector< double > *  ptr_y 
) const

在文件 cos_theta_ipopt_interface.cc 第 38 行定义.

39 {
40 *ptr_x = opt_x_;
41 *ptr_y = opt_y_;
42}

◆ get_starting_point()

bool apollo::planning::CosThetaIpoptInterface::get_starting_point ( int  n,
bool  init_x,
double *  x,
bool  init_z,
double *  z_L,
double *  z_U,
int  m,
bool  init_lambda,
double *  lambda 
)
override

Method to return the starting point for the algorithm

在文件 cos_theta_ipopt_interface.cc 第 116 行定义.

120 {
121 CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
122 std::random_device rd;
123 std::default_random_engine gen = std::default_random_engine(rd());
124 std::normal_distribution<> dis{0, 0.05};
125 for (size_t i = 0; i < num_of_points_; ++i) {
126 size_t index = i << 1;
127 x[index] = ref_points_[i].first + dis(gen);
128 x[index + 1] = ref_points_[i].second + dis(gen);
129 }
130 return true;
131}

◆ set_automatic_differentiation_flag()

void apollo::planning::CosThetaIpoptInterface::set_automatic_differentiation_flag ( const bool  use_ad)

在文件 cos_theta_ipopt_interface.cc 第 547 行定义.

548 {
549 use_automatic_differentiation_ = use_ad;
550}

◆ set_weight_anchor_points()

void apollo::planning::CosThetaIpoptInterface::set_weight_anchor_points ( const double  weight_anchor_points)

在文件 cos_theta_ipopt_interface.cc 第 557 行定义.

558 {
559 weight_anchor_points_ = weight_anchor_points;
560}

◆ set_weight_cos_included_angle()

void apollo::planning::CosThetaIpoptInterface::set_weight_cos_included_angle ( const double  weight_cos_included_angle)

在文件 cos_theta_ipopt_interface.cc 第 552 行定义.

553 {
554 weight_cos_included_angle_ = weight_cos_included_angle;
555}

◆ set_weight_length()

void apollo::planning::CosThetaIpoptInterface::set_weight_length ( const double  weight_length)

在文件 cos_theta_ipopt_interface.cc 第 562 行定义.

562 {
563 weight_length_ = weight_length;
564}
该类的文档由以下文件生成: