Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
06eaa40e
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,发现更多精彩内容 >>
提交
06eaa40e
编写于
7月 17, 2019
作者:
D
deidaraho
提交者:
Xiangquan Xiao
7月 19, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: wrap open space osqp warm up config into proto
上级
eb61619a
变更
8
隐藏空白更改
内联
并排
Showing
8 changed file
with
81 addition
and
51 deletion
+81
-51
modules/planning/conf/planner_open_space_config.pb.txt
modules/planning/conf/planner_open_space_config.pb.txt
+7
-0
modules/planning/conf/planning_config.pb.txt
modules/planning/conf/planning_config.pb.txt
+7
-0
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.cc
...ctory_smoother/dual_variable_warm_start_osqp_interface.cc
+7
-6
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.h
...ectory_smoother/dual_variable_warm_start_osqp_interface.h
+1
-0
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.cc
...e/trajectory_smoother/dual_variable_warm_start_problem.cc
+10
-12
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.cc
...smoother/dual_variable_warm_start_slack_osqp_interface.cc
+31
-27
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.h
..._smoother/dual_variable_warm_start_slack_osqp_interface.h
+8
-6
modules/planning/proto/planner_open_space_config.proto
modules/planning/proto/planner_open_space_config.proto
+10
-0
未找到文件。
modules/planning/conf/planner_open_space_config.pb.txt
浏览文件 @
06eaa40e
...
...
@@ -42,6 +42,13 @@ dual_variable_warm_start_config {
min_safety_distance: 0.0
debug_osqp: false
beta: 1.0
osqp_config {
alpha: 1.0
eps_abs: 1.0e-3
eps_rel: 1.0e-3
max_iter: 10000
polish: true
}
}
distance_approach_config {
weight_steer: 0.3
...
...
modules/planning/conf/planning_config.pb.txt
浏览文件 @
06eaa40e
...
...
@@ -159,6 +159,13 @@ default_task_config: {
}
qp_format: OSQP
min_safety_distance: 0.01
osqp_config {
alpha: 1.0
eps_abs: 1.0e-3
eps_rel: 1.0e-3
max_iter: 10000
polish: true
}
}
distance_approach_config {
weight_steer: 0.3
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.cc
浏览文件 @
06eaa40e
...
...
@@ -72,6 +72,8 @@ DualVariableWarmStartOSQPInterface::DualVariableWarmStartOSQPInterface(
.
min_safety_distance
();
check_mode_
=
planner_open_space_config
.
dual_variable_warm_start_config
().
debug_osqp
();
osqp_config_
=
planner_open_space_config
.
dual_variable_warm_start_config
().
osqp_config
();
}
void
printMatrix
(
const
int
r
,
const
int
c
,
const
std
::
vector
<
c_float
>&
P_data
,
...
...
@@ -170,12 +172,11 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
// Define Solver settings as default
osqp_set_default_settings
(
settings
);
settings
->
alpha
=
1.0
;
// Change alpha parameter
settings
->
eps_abs
=
1.0e-03
;
settings
->
eps_rel
=
1.0e-03
;
settings
->
max_iter
=
10000
;
settings
->
polish
=
true
;
settings
->
verbose
=
FLAGS_enable_osqp_debug
;
settings
->
alpha
=
osqp_config_
.
alpha
();
// Change alpha parameter
settings
->
eps_abs
=
osqp_config_
.
eps_abs
();
settings
->
eps_rel
=
osqp_config_
.
eps_rel
();
settings
->
max_iter
=
osqp_config_
.
max_iter
();
settings
->
polish
=
osqp_config_
.
polish
();
// Populate data
OSQPData
*
data
=
reinterpret_cast
<
OSQPData
*>
(
c_malloc
(
sizeof
(
OSQPData
)));
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.h
浏览文件 @
06eaa40e
...
...
@@ -65,6 +65,7 @@ class DualVariableWarmStartOSQPInterface {
const
Eigen
::
MatrixXd
&
n_warm_up
);
private:
OSQPConfig
osqp_config_
;
int
num_of_variables_
;
int
num_of_constraints_
;
int
horizon_
;
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.cc
浏览文件 @
06eaa40e
...
...
@@ -45,15 +45,14 @@ bool DualVariableWarmStartProblem::Solve(
if
(
planner_open_space_config_
.
dual_variable_warm_start_config
()
.
qp_format
()
==
OSQP
)
{
DualVariableWarmStartOSQPInterface
*
ptop
=
new
DualVariableWarmStartOSQPInterface
(
DualVariableWarmStartOSQPInterface
ptop
=
DualVariableWarmStartOSQPInterface
(
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
xWS
,
planner_open_space_config_
);
bool
succ
=
ptop
->
optimize
();
if
(
succ
)
{
if
(
ptop
.
optimize
()
)
{
ADEBUG
<<
"dual warm up done."
;
ptop
->
get_optimization_results
(
l_warm_up
,
n_warm_up
);
ptop
.
get_optimization_results
(
l_warm_up
,
n_warm_up
);
auto
t_end
=
cyber
::
Time
::
Now
().
ToSecond
();
ADEBUG
<<
"Dual variable warm start solving time in second : "
...
...
@@ -62,20 +61,19 @@ bool DualVariableWarmStartProblem::Solve(
solver_flag
=
true
;
}
else
{
AWARN
<<
"dual warm up fail."
;
ptop
->
get_optimization_results
(
l_warm_up
,
n_warm_up
);
ptop
.
get_optimization_results
(
l_warm_up
,
n_warm_up
);
solver_flag
=
false
;
}
}
else
if
(
planner_open_space_config_
.
dual_variable_warm_start_config
()
.
qp_format
()
==
SLACKQP
)
{
DualVariableWarmStartSlackOSQPInterface
*
ptop
=
new
DualVariableWarmStartSlackOSQPInterface
(
DualVariableWarmStartSlackOSQPInterface
ptop
=
DualVariableWarmStartSlackOSQPInterface
(
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
xWS
,
planner_open_space_config_
);
bool
succ
=
ptop
->
optimize
();
if
(
succ
)
{
if
(
ptop
.
optimize
()
)
{
ADEBUG
<<
"dual warm up done."
;
ptop
->
get_optimization_results
(
l_warm_up
,
n_warm_up
);
ptop
.
get_optimization_results
(
l_warm_up
,
n_warm_up
);
auto
t_end
=
cyber
::
Time
::
Now
().
ToSecond
();
ADEBUG
<<
"Dual variable warm start solving time in second : "
...
...
@@ -84,7 +82,7 @@ bool DualVariableWarmStartProblem::Solve(
solver_flag
=
true
;
}
else
{
AWARN
<<
"dual warm up fail."
;
ptop
->
get_optimization_results
(
l_warm_up
,
n_warm_up
);
ptop
.
get_optimization_results
(
l_warm_up
,
n_warm_up
);
solver_flag
=
false
;
}
}
else
if
(
planner_open_space_config_
.
dual_variable_warm_start_config
()
...
...
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.cc
浏览文件 @
06eaa40e
...
...
@@ -17,9 +17,9 @@
/*
* @file
*/
#include <algorithm>
#include "modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.h"
#include <algorithm>
#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
...
...
@@ -78,6 +78,8 @@ DualVariableWarmStartSlackOSQPInterface::
check_mode_
=
planner_open_space_config
.
dual_variable_warm_start_config
().
debug_osqp
();
beta_
=
planner_open_space_config
.
dual_variable_warm_start_config
().
beta
();
osqp_config_
=
planner_open_space_config
.
dual_variable_warm_start_config
().
osqp_config
();
}
void
DualVariableWarmStartSlackOSQPInterface
::
printMatrix
(
...
...
@@ -125,22 +127,23 @@ void DualVariableWarmStartSlackOSQPInterface::assembleA(
}
bool
DualVariableWarmStartSlackOSQPInterface
::
optimize
()
{
int
kNumParam
=
num_of_variables_
;
int
kNumConst
=
num_of_constraints_
;
//
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
;
std
::
vector
<
c_int
>
P_indptr
;
assemble
_
P
(
&
P_data
,
&
P_indices
,
&
P_indptr
);
assembleP
(
&
P_data
,
&
P_indices
,
&
P_indptr
);
if
(
check_mode_
)
{
AINFO
<<
"print P_data in whole: "
;
printMatrix
(
kNumParam
,
kNumParam
,
P_data
,
P_indices
,
P_indptr
);
printMatrix
(
num_of_variables_
,
num_of_variables_
,
P_data
,
P_indices
,
P_indptr
);
}
// assemble q, linear term in objective, \sum{beta * slacks}
c_float
q
[
kNumParam
];
for
(
int
i
=
0
;
i
<
kNumParam
;
++
i
)
{
c_float
q
[
num_of_variables_
];
// NOLINT
for
(
int
i
=
0
;
i
<
num_of_variables_
;
++
i
)
{
q
[
i
]
=
0.0
;
if
(
i
>=
s_start_index_
)
{
q
[
i
]
=
beta_
;
...
...
@@ -151,18 +154,20 @@ bool DualVariableWarmStartSlackOSQPInterface::optimize() {
std
::
vector
<
c_float
>
A_data
;
std
::
vector
<
c_int
>
A_indices
;
std
::
vector
<
c_int
>
A_indptr
;
assemble
_c
onstraint
(
&
A_data
,
&
A_indices
,
&
A_indptr
);
assemble
C
onstraint
(
&
A_data
,
&
A_indices
,
&
A_indptr
);
if
(
check_mode_
)
{
AINFO
<<
"print A_data in whole: "
;
printMatrix
(
kNumConst
,
kNumParam
,
A_data
,
A_indices
,
A_indptr
);
assembleA
(
kNumConst
,
kNumParam
,
A_data
,
A_indices
,
A_indptr
);
printMatrix
(
num_of_constraints_
,
num_of_variables_
,
A_data
,
A_indices
,
A_indptr
);
assembleA
(
num_of_constraints_
,
num_of_variables_
,
A_data
,
A_indices
,
A_indptr
);
}
// assemble lb & ub, slack_variable <= 0
c_float
lb
[
kNumConst
];
c_float
ub
[
kNumConst
];
int
slack_indx
=
kNumConst
-
slack_horizon_
;
for
(
int
i
=
0
;
i
<
kNumConst
;
++
i
)
{
c_float
lb
[
num_of_constraints_
];
// NOLINT
c_float
ub
[
num_of_constraints_
];
// NOLINT
int
slack_indx
=
num_of_constraints_
-
slack_horizon_
;
for
(
int
i
=
0
;
i
<
num_of_constraints_
;
++
i
)
{
lb
[
i
]
=
0.0
;
if
(
i
>=
slack_indx
)
{
lb
[
i
]
=
-
2e19
;
...
...
@@ -183,17 +188,16 @@ bool DualVariableWarmStartSlackOSQPInterface::optimize() {
// Define Solver settings as default
osqp_set_default_settings
(
settings
);
settings
->
alpha
=
1.0
;
// Change alpha parameter
settings
->
eps_abs
=
1.0e-03
;
settings
->
eps_rel
=
1.0e-03
;
settings
->
max_iter
=
10000
;
settings
->
polish
=
true
;
settings
->
verbose
=
FLAGS_enable_osqp_debug
;
settings
->
alpha
=
osqp_config_
.
alpha
();
// Change alpha parameter
settings
->
eps_abs
=
osqp_config_
.
eps_abs
();
settings
->
eps_rel
=
osqp_config_
.
eps_rel
();
settings
->
max_iter
=
osqp_config_
.
max_iter
();
settings
->
polish
=
osqp_config_
.
polish
();
// Populate data
OSQPData
*
data
=
reinterpret_cast
<
OSQPData
*>
(
c_malloc
(
sizeof
(
OSQPData
)));
data
->
n
=
kNumParam
;
data
->
m
=
kNumConst
;
data
->
n
=
num_of_variables_
;
data
->
m
=
num_of_constraints_
;
data
->
P
=
csc_matrix
(
data
->
n
,
data
->
n
,
P_data
.
size
(),
P_data
.
data
(),
P_indices
.
data
(),
P_indptr
.
data
());
data
->
q
=
q
;
...
...
@@ -254,13 +258,13 @@ bool DualVariableWarmStartSlackOSQPInterface::optimize() {
return
succ
;
}
void
DualVariableWarmStartSlackOSQPInterface
::
check
_s
olution
(
void
DualVariableWarmStartSlackOSQPInterface
::
check
S
olution
(
const
Eigen
::
MatrixXd
&
l_warm_up
,
const
Eigen
::
MatrixXd
&
n_warm_up
)
{
// TODO(Runxin): extend
return
;
}
void
DualVariableWarmStartSlackOSQPInterface
::
assemble
_
P
(
void
DualVariableWarmStartSlackOSQPInterface
::
assembleP
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
)
{
// the objective function is norm(A' * lambda)
...
...
@@ -323,7 +327,7 @@ void DualVariableWarmStartSlackOSQPInterface::assemble_P(
CHECK_EQ
(
P_indptr
->
size
(),
num_of_variables_
+
1
);
}
void
DualVariableWarmStartSlackOSQPInterface
::
assemble
_c
onstraint
(
void
DualVariableWarmStartSlackOSQPInterface
::
assemble
C
onstraint
(
std
::
vector
<
c_float
>*
A_data
,
std
::
vector
<
c_int
>*
A_indices
,
std
::
vector
<
c_int
>*
A_indptr
)
{
/*
...
...
@@ -468,8 +472,8 @@ void DualVariableWarmStartSlackOSQPInterface::get_optimization_results(
}
}
LOG
(
INFO
)
<<
"max_s: "
<<
std
::
to_string
(
max_s
);
LOG
(
INFO
)
<<
"min_s: "
<<
std
::
to_string
(
min_s
);
ADEBUG
<<
"max_s: "
<<
std
::
to_string
(
max_s
);
ADEBUG
<<
"min_s: "
<<
std
::
to_string
(
min_s
);
}
}
// namespace planning
}
// namespace apollo
modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.h
浏览文件 @
06eaa40e
...
...
@@ -50,18 +50,18 @@ class DualVariableWarmStartSlackOSQPInterface {
bool
optimize
();
void
assemble
_
P
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
);
void
assembleP
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
);
void
assemble
_c
onstraint
(
std
::
vector
<
c_float
>*
A_data
,
std
::
vector
<
c_int
>*
A_indices
,
std
::
vector
<
c_int
>*
A_indptr
);
void
assemble
C
onstraint
(
std
::
vector
<
c_float
>*
A_data
,
std
::
vector
<
c_int
>*
A_indices
,
std
::
vector
<
c_int
>*
A_indptr
);
void
assembleA
(
const
int
r
,
const
int
c
,
const
std
::
vector
<
c_float
>&
P_data
,
const
std
::
vector
<
c_int
>&
P_indices
,
const
std
::
vector
<
c_int
>&
P_indptr
);
void
check
_s
olution
(
const
Eigen
::
MatrixXd
&
l_warm_up
,
void
check
S
olution
(
const
Eigen
::
MatrixXd
&
l_warm_up
,
const
Eigen
::
MatrixXd
&
n_warm_up
);
void
printMatrix
(
const
int
r
,
const
int
c
,
...
...
@@ -70,6 +70,8 @@ class DualVariableWarmStartSlackOSQPInterface {
const
std
::
vector
<
c_int
>&
P_indptr
);
private:
OSQPConfig
osqp_config_
;
int
num_of_variables_
;
int
num_of_constraints_
;
int
horizon_
;
...
...
modules/planning/proto/planner_open_space_config.proto
浏览文件 @
06eaa40e
...
...
@@ -75,6 +75,7 @@ message DualVariableWarmStartConfig {
optional
double
min_safety_distance
=
4
[
default
=
0.0
];
optional
bool
debug_osqp
=
5
[
default
=
false
];
optional
double
beta
=
6
[
default
=
1.0
];
optional
OSQPConfig
osqp_config
=
7
;
}
message
DistanceApproachConfig
{
...
...
@@ -129,6 +130,15 @@ message IpoptConfig {
// ipopt barrier parameter, default 0.1
}
// Dual variable configs for OSQP
message
OSQPConfig
{
optional
double
alpha
=
1
[
default
=
1.0
];
optional
double
eps_abs
=
2
[
default
=
1.0e-3
];
optional
double
eps_rel
=
3
[
default
=
1.0e-3
];
optional
int32
max_iter
=
4
[
default
=
10000
];
optional
bool
polish
=
5
[
default
=
true
];
}
message
IterativeAnchoringConfig
{
// Ipopt configs
optional
double
interpolated_delta_s
=
1
[
default
=
0.1
];
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录