Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
e7427b52
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
体验新版 GitCode,发现更多精彩内容 >>
提交
e7427b52
编写于
2月 15, 2019
作者:
D
deidaraho
提交者:
Qi Luo
2月 18, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: introduce dual warm up debug mode to compare three warm ups in open space
上级
dd3a611d
变更
7
隐藏空白更改
内联
并排
Showing
7 changed file
with
189 addition
and
7 deletion
+189
-7
modules/planning/conf/planner_open_space_config.pb.txt
modules/planning/conf/planner_open_space_config.pb.txt
+1
-0
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_ipopt_interface.cc
...tory_smoother/dual_variable_warm_start_ipopt_interface.cc
+2
-2
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.cc
...ctory_smoother/dual_variable_warm_start_osqp_interface.cc
+10
-3
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.h
...ectory_smoother/dual_variable_warm_start_osqp_interface.h
+2
-0
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.cc
...e/trajectory_smoother/dual_variable_warm_start_problem.cc
+171
-1
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.h
...ce/trajectory_smoother/dual_variable_warm_start_problem.h
+1
-1
modules/planning/proto/planner_open_space_config.proto
modules/planning/proto/planner_open_space_config.proto
+2
-0
未找到文件。
modules/planning/conf/planner_open_space_config.pb.txt
浏览文件 @
e7427b52
...
...
@@ -32,6 +32,7 @@ dual_variable_warm_start_config {
ipopt_recalc_y: "yes"
}
qp_format: OSQP
min_safety_distance: 0.0
}
distance_approach_config {
weight_steer: 0.3
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_ipopt_interface.cc
浏览文件 @
e7427b52
...
...
@@ -115,7 +115,7 @@ bool DualVariableWarmStartIPOPTInterface::get_starting_point(
// 1. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
for
(
int
i
=
0
;
i
<
horizon_
+
1
;
++
i
)
{
for
(
int
j
=
0
;
j
<
obstacles_edges_sum_
;
++
j
)
{
x
[
l_index
]
=
0.
5
;
x
[
l_index
]
=
0.
0
;
++
l_index
;
}
}
...
...
@@ -123,7 +123,7 @@ bool DualVariableWarmStartIPOPTInterface::get_starting_point(
// 2. lagrange constraint n, 4*obstacles_num * (horizon_+1)
for
(
int
i
=
0
;
i
<
horizon_
+
1
;
++
i
)
{
for
(
int
j
=
0
;
j
<
4
*
obstacles_num_
;
++
j
)
{
x
[
n_index
]
=
1
.0
;
x
[
n_index
]
=
0
.0
;
++
n_index
;
}
}
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.cc
浏览文件 @
e7427b52
...
...
@@ -66,11 +66,16 @@ DualVariableWarmStartOSQPInterface::DualVariableWarmStartOSQPInterface(
num_of_variables_
=
lambda_horizon_
+
miu_horizon_
;
// number of constraints
num_of_constraints_
=
3
*
obstacles_num_
*
(
horizon_
+
1
)
+
num_of_variables_
;
min_safety_distance_
=
planner_open_space_config
.
dual_variable_warm_start_config
().
min_safety_distance
();
}
bool
DualVariableWarmStartOSQPInterface
::
optimize
()
{
int
kNumParam
=
num_of_variables_
;
int
kNumConst
=
num_of_constraints_
;
bool
succ
=
true
;
// assemble P, quadratic term in objective
std
::
vector
<
c_float
>
P_data
;
std
::
vector
<
c_int
>
P_indices
;
...
...
@@ -96,7 +101,7 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
lb
[
i
]
=
0.0
;
if
(
i
>=
2
*
obstacles_num_
*
(
horizon_
+
1
)
&&
i
<
3
*
obstacles_num_
*
(
horizon_
+
1
))
{
lb
[
i
]
=
0.0
;
lb
[
i
]
=
min_safety_distance_
;
}
if
(
i
<
2
*
obstacles_num_
*
(
horizon_
+
1
))
{
ub
[
i
]
=
0.0
;
...
...
@@ -141,7 +146,7 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
AERROR
<<
"OSQP dual warm up unsuccess, "
<<
"return status: "
<<
work
->
info
->
status
;
return
false
;
succ
=
false
;
}
// extract primal results
...
...
@@ -164,6 +169,8 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
}
}
succ
=
succ
&
(
work
->
info
->
obj_val
<=
1.0
);
// Cleanup
osqp_cleanup
(
work
);
c_free
(
data
->
A
);
...
...
@@ -171,7 +178,7 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
c_free
(
data
);
c_free
(
settings
);
return
true
;
return
succ
;
}
void
DualVariableWarmStartOSQPInterface
::
assemble_P
(
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.h
浏览文件 @
e7427b52
...
...
@@ -79,6 +79,8 @@ class DualVariableWarmStartOSQPInterface {
int
obstacles_num_
;
int
obstacles_edges_sum_
;
double
min_safety_distance_
;
// lagrangian l start index
int
l_start_index_
=
0
;
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.cc
浏览文件 @
e7427b52
...
...
@@ -135,7 +135,8 @@ bool DualVariableWarmStartProblem::Solve(
return
status
==
Ipopt
::
Solve_Succeeded
||
status
==
Ipopt
::
Solved_To_Acceptable_Level
;
}
else
{
}
else
if
(
planner_open_space_config_
.
dual_variable_warm_start_config
().
qp_format
()
==
IPOPT
)
{
DualVariableWarmStartIPOPTInterface
*
ptop
=
new
DualVariableWarmStartIPOPTInterface
(
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
...
...
@@ -203,6 +204,175 @@ bool DualVariableWarmStartProblem::Solve(
return
status
==
Ipopt
::
Solve_Succeeded
||
status
==
Ipopt
::
Solved_To_Acceptable_Level
;
}
else
{
// debug mode
DualVariableWarmStartOSQPInterface
*
ptop_osqp
=
new
DualVariableWarmStartOSQPInterface
(
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
xWS
,
planner_open_space_config_
);
bool
succ
=
ptop_osqp
->
optimize
();
if
(
!
succ
)
{
AERROR
<<
"dual warm up fail."
;
ptop_osqp
->
get_optimization_results
(
l_warm_up
,
n_warm_up
);
}
AINFO
<<
"dual warm up done."
;
ptop_osqp
->
get_optimization_results
(
l_warm_up
,
n_warm_up
);
auto
t_end
=
cyber
::
Time
::
Now
().
ToSecond
();
AINFO
<<
"Dual vairable warm start solving time in second : "
<<
t_end
-
t_start
;
// ipoptqp result
Eigen
::
MatrixXd
l_warm_up_ipoptqp
(
l_warm_up
->
rows
(),
l_warm_up
->
cols
());
Eigen
::
MatrixXd
n_warm_up_ipoptqp
(
n_warm_up
->
rows
(),
n_warm_up
->
cols
());
DualVariableWarmStartIPOPTQPInterface
*
ptop_ipoptqp
=
new
DualVariableWarmStartIPOPTQPInterface
(
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
xWS
,
planner_open_space_config_
);
Ipopt
::
SmartPtr
<
Ipopt
::
TNLP
>
problem
=
ptop_ipoptqp
;
// Create an instance of the IpoptApplication
Ipopt
::
SmartPtr
<
Ipopt
::
IpoptApplication
>
app
=
IpoptApplicationFactory
();
app
->
Options
()
->
SetIntegerValue
(
"print_level"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_print_level
());
app
->
Options
()
->
SetIntegerValue
(
"mumps_mem_percent"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
mumps_mem_percent
());
app
->
Options
()
->
SetNumericValue
(
"mumps_pivtol"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
mumps_pivtol
());
app
->
Options
()
->
SetIntegerValue
(
"max_iter"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_max_iter
());
app
->
Options
()
->
SetNumericValue
(
"tol"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_tol
());
app
->
Options
()
->
SetNumericValue
(
"acceptable_constr_viol_tol"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_acceptable_constr_viol_tol
());
app
->
Options
()
->
SetNumericValue
(
"min_hessian_perturbation"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_min_hessian_perturbation
());
app
->
Options
()
->
SetNumericValue
(
"jacobian_regularization_value"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_jacobian_regularization_value
());
app
->
Options
()
->
SetStringValue
(
"print_timing_statistics"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_print_timing_statistics
());
app
->
Options
()
->
SetStringValue
(
"alpha_for_y"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_alpha_for_y
());
app
->
Options
()
->
SetStringValue
(
"recalc_y"
,
planner_open_space_config_
.
dual_variable_warm_start_config
().
ipopt_config
().
ipopt_recalc_y
());
// for qp problem speed up
app
->
Options
()
->
SetStringValue
(
"mehrotra_algorithm"
,
"yes"
);
Ipopt
::
ApplicationReturnStatus
status
=
app
->
Initialize
();
if
(
status
!=
Ipopt
::
Solve_Succeeded
)
{
AERROR
<<
"*** Dual variable wart start problem error during initialization!"
;
return
false
;
}
status
=
app
->
OptimizeTNLP
(
problem
);
if
(
status
==
Ipopt
::
Solve_Succeeded
||
status
==
Ipopt
::
Solved_To_Acceptable_Level
)
{
// Retrieve some statistics about the solve
Ipopt
::
Index
iter_count
=
app
->
Statistics
()
->
IterationCount
();
AINFO
<<
"*** IPOPTQP: The problem solved in "
<<
iter_count
<<
" iterations!"
;
Ipopt
::
Number
final_obj
=
app
->
Statistics
()
->
FinalObjective
();
AINFO
<<
"*** IPOPTQP: The final value of the objective function is "
<<
final_obj
<<
'.'
;
}
else
{
AINFO
<<
"Solve not succeeding, return status: "
<<
int
(
status
);
}
ptop_ipoptqp
->
get_optimization_results
(
&
l_warm_up_ipoptqp
,
&
n_warm_up_ipoptqp
);
// ipopt result
Eigen
::
MatrixXd
l_warm_up_ipopt
(
l_warm_up
->
rows
(),
l_warm_up
->
cols
());
Eigen
::
MatrixXd
n_warm_up_ipopt
(
n_warm_up
->
rows
(),
n_warm_up
->
cols
());
DualVariableWarmStartIPOPTInterface
*
ptop_ipopt
=
new
DualVariableWarmStartIPOPTInterface
(
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
xWS
,
planner_open_space_config_
);
problem
=
ptop_ipopt
;
// Create an instance of the IpoptApplication
status
=
app
->
Initialize
();
if
(
status
!=
Ipopt
::
Solve_Succeeded
)
{
AERROR
<<
"*** Dual variable wart start problem error during initialization!"
;
return
false
;
}
status
=
app
->
OptimizeTNLP
(
problem
);
if
(
status
==
Ipopt
::
Solve_Succeeded
||
status
==
Ipopt
::
Solved_To_Acceptable_Level
)
{
// Retrieve some statistics about the solve
Ipopt
::
Index
iter_count
=
app
->
Statistics
()
->
IterationCount
();
AINFO
<<
"*** IPOPT: The problem solved in "
<<
iter_count
<<
" iterations!"
;
Ipopt
::
Number
final_obj
=
app
->
Statistics
()
->
FinalObjective
();
AINFO
<<
"*** IPOPT: The final value of the objective function is "
<<
final_obj
<<
'.'
;
}
else
{
AINFO
<<
"Solve not succeeding, return status: "
<<
int
(
status
);
}
ptop_ipopt
->
get_optimization_results
(
&
l_warm_up_ipopt
,
&
n_warm_up_ipopt
);
// compare three results
double
l_max_diff1
=
0.0
;
double
l_max_diff2
=
0.0
;
double
l_max_diff3
=
0.0
;
for
(
int
c
=
0
;
c
<
l_warm_up
->
cols
();
++
c
)
{
for
(
int
r
=
0
;
r
<
l_warm_up
->
rows
();
++
r
)
{
l_max_diff1
=
std
::
max
(
l_max_diff1
,
std
::
abs
(
l_warm_up
->
coeff
(
r
,
c
)
-
l_warm_up_ipopt
(
r
,
c
)));
l_max_diff2
=
std
::
max
(
l_max_diff2
,
std
::
abs
(
l_warm_up
->
coeff
(
r
,
c
)
-
l_warm_up_ipoptqp
(
r
,
c
)));
l_max_diff3
=
std
::
max
(
l_max_diff3
,
std
::
abs
(
l_warm_up_ipoptqp
(
r
,
c
)
-
l_warm_up_ipopt
(
r
,
c
)));
}
}
AINFO
<<
"max l warm up diff between osqp & ipopt: "
<<
l_max_diff1
;
AINFO
<<
"max l warm up diff between osqp & ipoptqp: "
<<
l_max_diff2
;
AINFO
<<
"max l warm up diff between ipopt & ipoptqp: "
<<
l_max_diff3
;
double
n_max_diff1
=
0.0
;
double
n_max_diff2
=
0.0
;
double
n_max_diff3
=
0.0
;
for
(
int
c
=
0
;
c
<
n_warm_up
->
cols
();
++
c
)
{
for
(
int
r
=
0
;
r
<
n_warm_up
->
rows
();
++
r
)
{
n_max_diff1
=
std
::
max
(
n_max_diff1
,
std
::
abs
(
n_warm_up
->
coeff
(
r
,
c
)
-
n_warm_up_ipopt
(
r
,
c
)));
n_max_diff2
=
std
::
max
(
n_max_diff2
,
std
::
abs
(
n_warm_up
->
coeff
(
r
,
c
)
-
n_warm_up_ipoptqp
(
r
,
c
)));
n_max_diff3
=
std
::
max
(
n_max_diff3
,
std
::
abs
(
n_warm_up_ipoptqp
(
r
,
c
)
-
n_warm_up_ipopt
(
r
,
c
)));
}
}
AINFO
<<
"max n warm up diff between osqp & ipopt: "
<<
n_max_diff1
;
AINFO
<<
"max n warm up diff between osqp & ipoptqp: "
<<
n_max_diff2
;
AINFO
<<
"max n warm up diff between ipopt & ipoptqp: "
<<
n_max_diff3
;
return
true
;
}
}
}
// namespace planning
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.h
浏览文件 @
e7427b52
...
...
@@ -19,7 +19,7 @@
*/
#pragma once
#include <algorithm>
#include "Eigen/Dense"
#include "modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_ipopt_interface.h"
...
...
modules/planning/proto/planner_open_space_config.proto
浏览文件 @
e7427b52
...
...
@@ -6,6 +6,7 @@ enum DualWarmUpMode {
IPOPT
=
0
;
IPOPTQP
=
1
;
OSQP
=
2
;
DEBUG
=
3
;
}
message
PlannerOpenSpaceConfig
{
...
...
@@ -56,6 +57,7 @@ message DualVariableWarmStartConfig {
optional
double
weight_d
=
1
[
default
=
1.0
];
optional
IpoptConfig
ipopt_config
=
2
;
optional
DualWarmUpMode
qp_format
=
3
;
optional
double
min_safety_distance
=
4
[
default
=
0.0
];
}
message
DistanceApproachConfig
{
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录