Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
1340894d
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,发现更多精彩内容 >>
提交
1340894d
编写于
11月 12, 2018
作者:
J
JasonZhou404
提交者:
Jiangtao Hu
12月 13, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: OpenSpace: use upper limit in ipopt
上级
20d4a351
变更
8
隐藏空白更改
内联
并排
Showing
8 changed file
with
40 addition
and
60 deletion
+40
-60
modules/planning/conf/planner_open_space_config.pb.txt
modules/planning/conf/planner_open_space_config.pb.txt
+0
-3
modules/planning/open_space/distance_approach_ipopt_interface.cc
.../planning/open_space/distance_approach_ipopt_interface.cc
+10
-13
modules/planning/open_space/distance_approach_problem.cc
modules/planning/open_space/distance_approach_problem.cc
+1
-1
modules/planning/open_space/dual_variable_warm_start_ipopt_interface.cc
...ng/open_space/dual_variable_warm_start_ipopt_interface.cc
+24
-10
modules/planning/open_space/dual_variable_warm_start_ipopt_interface.h
...ing/open_space/dual_variable_warm_start_ipopt_interface.h
+1
-2
modules/planning/open_space/dual_variable_warm_start_ipopt_interface_test.cc
...en_space/dual_variable_warm_start_ipopt_interface_test.cc
+1
-8
modules/planning/open_space/dual_variable_warm_start_problem.cc
...s/planning/open_space/dual_variable_warm_start_problem.cc
+3
-20
modules/planning/proto/planner_open_space_config.proto
modules/planning/proto/planner_open_space_config.proto
+0
-3
未找到文件。
modules/planning/conf/planner_open_space_config.pb.txt
浏览文件 @
1340894d
...
...
@@ -32,7 +32,6 @@ distance_approach_config : {
weight_time : 0.5
weight_time : 1
min_safety_distance : 0.0
max_safety_distance : 1.0e2
max_steer_angle : 0.6
max_steer_rate : 0.6
max_speed_forward : 2.0
...
...
@@ -41,7 +40,5 @@ distance_approach_config : {
max_acceleration_reverse : 1.0
min_time_sample_scaling : 0.1
max_time_sample_scaling : 2.0
max_miu : 1.5
max_lambda : 2.0
use_fix_time : false
}
modules/planning/open_space/distance_approach_ipopt_interface.cc
浏览文件 @
1340894d
...
...
@@ -83,7 +83,6 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(
weight_first_order_time_
=
distance_approach_config_
.
weight_time
(
0
);
weight_second_order_time_
=
distance_approach_config_
.
weight_time
(
1
);
min_safety_distance_
=
distance_approach_config_
.
min_safety_distance
();
max_safety_distance_
=
distance_approach_config_
.
max_safety_distance
();
max_steer_angle_
=
distance_approach_config_
.
max_steer_angle
();
max_speed_forward_
=
distance_approach_config_
.
max_speed_forward
();
max_speed_reverse_
=
distance_approach_config_
.
max_speed_reverse
();
...
...
@@ -96,8 +95,6 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(
max_time_sample_scaling_
=
distance_approach_config_
.
max_time_sample_scaling
();
max_steer_rate_
=
distance_approach_config_
.
max_steer_rate
();
max_lambda_
=
distance_approach_config_
.
max_lambda
();
max_miu_
=
distance_approach_config_
.
max_miu
();
use_fix_time_
=
distance_approach_config_
.
use_fix_time
();
wheelbase_
=
vehicle_param_
.
wheel_base
();
}
...
...
@@ -188,8 +185,8 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
x_u
[
variable_index
+
1
]
=
XYbounds_
[
3
];
// phi
x_l
[
variable_index
+
2
]
=
-
M_PI
;
x_u
[
variable_index
+
2
]
=
M_PI
;
x_l
[
variable_index
+
2
]
=
-
2e19
;
x_u
[
variable_index
+
2
]
=
2e19
;
// v
x_l
[
variable_index
+
3
]
=
-
max_speed_reverse_
;
...
...
@@ -241,7 +238,7 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
for
(
int
i
=
0
;
i
<
horizon_
+
1
;
++
i
)
{
for
(
int
j
=
0
;
j
<
obstacles_edges_sum_
;
++
j
)
{
x_l
[
variable_index
]
=
0.0
;
x_u
[
variable_index
]
=
max_lambda_
;
x_u
[
variable_index
]
=
2e19
;
// nlp_upper_bound_limit
++
variable_index
;
}
}
...
...
@@ -251,7 +248,7 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
for
(
int
i
=
0
;
i
<
horizon_
+
1
;
++
i
)
{
for
(
int
j
=
0
;
j
<
4
*
obstacles_num_
;
++
j
)
{
x_l
[
variable_index
]
=
0.0
;
x_u
[
variable_index
]
=
max_miu_
;
x_u
[
variable_index
]
=
2e19
;
// nlp_upper_bound_limit
++
variable_index
;
}
...
...
@@ -309,7 +306,7 @@ bool DistanceApproachIPOPTInterface::get_bounds_info(int n, double* x_l,
// c. -g'*mu + (A*t - b)*lambda > min_safety_distance_
g_l
[
constraint_index
+
3
]
=
min_safety_distance_
;
g_u
[
constraint_index
+
3
]
=
max_safety_distance_
;
g_u
[
constraint_index
+
3
]
=
2e19
;
// nlp_upper_bound_limit
constraint_index
+=
4
;
}
}
...
...
@@ -1204,16 +1201,16 @@ bool DistanceApproachIPOPTInterface::eval_obj(int n, const T* x, T* obj_value) {
T
x3_diff
=
x
[
state_index
+
2
]
-
xWS_
(
2
,
i
);
T
x4_abs
=
x
[
state_index
+
3
];
*
obj_value
+=
weight_state_x_
*
x1_diff
*
x1_diff
+
weight_state_y_
*
x2_diff
*
x2_diff
+
weight_state_phi_
*
x3_diff
*
x3_diff
+
weight_state_v_
*
x4_abs
*
x4_abs
;
weight_state_y_
*
x2_diff
*
x2_diff
+
weight_state_phi_
*
x3_diff
*
x3_diff
+
weight_state_v_
*
x4_abs
*
x4_abs
;
state_index
+=
4
;
}
// 2. objective to minimize u square
for
(
int
i
=
0
;
i
<
horizon_
;
++
i
)
{
*
obj_value
+=
weight_input_steer_
*
x
[
control_index
]
*
x
[
control_index
]
+
weight_input_a_
*
x
[
control_index
+
1
]
*
x
[
control_index
+
1
];
weight_input_a_
*
x
[
control_index
+
1
]
*
x
[
control_index
+
1
];
control_index
+=
2
;
}
...
...
@@ -1235,7 +1232,7 @@ bool DistanceApproachIPOPTInterface::eval_obj(int n, const T* x, T* obj_value) {
T
a_rate
=
(
x
[
control_index
+
3
]
-
x
[
control_index
+
1
])
/
x
[
time_index
]
/
ts_
;
*
obj_value
+=
weight_rate_steer_
*
steering_rate
*
steering_rate
+
weight_rate_a_
*
a_rate
*
a_rate
;
weight_rate_a_
*
a_rate
*
a_rate
;
control_index
+=
2
;
time_index
+=
1
;
}
...
...
modules/planning/open_space/distance_approach_problem.cc
浏览文件 @
1340894d
...
...
@@ -60,7 +60,7 @@ bool DistanceApproachProblem::Solve(
// app->Options()->SetStringValue("derivative_test", "first-order");
// app->Options()->SetNumericValue("derivative_test_tol", 1.0e-3);
// TODO(QiL) : Change IPOPT settings to flag or configs
app
->
Options
()
->
SetIntegerValue
(
"print_level"
,
0
);
app
->
Options
()
->
SetIntegerValue
(
"print_level"
,
5
);
app
->
Options
()
->
SetIntegerValue
(
"mumps_mem_percent"
,
6000
);
app
->
Options
()
->
SetNumericValue
(
"mumps_pivtol"
,
1e-6
);
app
->
Options
()
->
SetIntegerValue
(
"max_iter"
,
1000
);
...
...
modules/planning/open_space/dual_variable_warm_start_ipopt_interface.cc
浏览文件 @
1340894d
...
...
@@ -33,14 +33,12 @@ namespace apollo {
namespace
planning
{
DualVariableWarmStartIPOPTInterface
::
DualVariableWarmStartIPOPTInterface
(
int
num_of_variables
,
int
num_of_constraints
,
size_t
horizon
,
float
ts
,
const
Eigen
::
MatrixX
d
&
ego
,
const
Eigen
::
MatrixXi
&
obstacles_edg
es_num
,
const
size_t
obstacles_num
,
const
Eigen
::
MatrixXd
&
obstacles_A
,
const
Eigen
::
MatrixXd
&
obstacles_b
,
const
Eigen
::
MatrixXd
&
xWS
,
size_t
horizon
,
float
ts
,
const
Eigen
::
MatrixXd
&
ego
,
const
Eigen
::
MatrixX
i
&
obstacles_edges_num
,
const
size_t
obstacl
es_num
,
const
Eigen
::
MatrixXd
&
obstacles_A
,
const
Eigen
::
MatrixXd
&
obstacles_b
,
const
Eigen
::
MatrixXd
&
xWS
,
const
PlannerOpenSpaceConfig
&
planner_open_space_config
)
:
num_of_variables_
(
num_of_variables
),
num_of_constraints_
(
num_of_constraints
),
ts_
(
ts
),
:
ts_
(
ts
),
ego_
(
ego
),
obstacles_edges_num_
(
obstacles_edges_num
),
obstacles_A_
(
obstacles_A
),
...
...
@@ -69,6 +67,22 @@ DualVariableWarmStartIPOPTInterface::DualVariableWarmStartIPOPTInterface(
bool
DualVariableWarmStartIPOPTInterface
::
get_nlp_info
(
int
&
n
,
int
&
m
,
int
&
nnz_jac_g
,
int
&
nnz_h_lag
,
IndexStyleEnum
&
index_style
)
{
// n1 : lagrangian multiplier associated with obstacleShape
int
n1
=
obstacles_edges_sum_
*
(
horizon_
+
1
);
// n2 : lagrangian multipier associated with car shape, obstacles_num*4 *
// (N+1)
int
n2
=
obstacles_num_
*
4
*
(
horizon_
+
1
);
// n3 : dual variable, obstacles_num * (N+1)
int
n3
=
obstacles_num_
*
(
horizon_
+
1
);
// m1 : obstacle constraints
int
m1
=
4
*
obstacles_num_
*
(
horizon_
+
1
);
num_of_variables_
=
n1
+
n2
+
n3
;
num_of_constraints_
=
m1
;
// number of variables
n
=
num_of_variables_
;
...
...
@@ -152,7 +166,7 @@ bool DualVariableWarmStartIPOPTInterface::get_bounds_info(int n, double* x_l,
for
(
int
i
=
0
;
i
<
horizon_
+
1
;
++
i
)
{
for
(
int
j
=
0
;
j
<
obstacles_edges_sum_
;
++
j
)
{
x_l
[
variable_index
]
=
0.0
;
x_u
[
variable_index
]
=
10.0
;
x_u
[
variable_index
]
=
2e19
;
++
variable_index
;
}
}
...
...
@@ -162,7 +176,7 @@ bool DualVariableWarmStartIPOPTInterface::get_bounds_info(int n, double* x_l,
for
(
int
i
=
0
;
i
<
horizon_
+
1
;
++
i
)
{
for
(
int
j
=
0
;
j
<
4
*
obstacles_num_
;
++
j
)
{
x_l
[
variable_index
]
=
0.0
;
x_u
[
variable_index
]
=
10.0
;
x_u
[
variable_index
]
=
2e19
;
// nlp_upper_bound_limit
++
variable_index
;
}
}
...
...
@@ -173,7 +187,7 @@ bool DualVariableWarmStartIPOPTInterface::get_bounds_info(int n, double* x_l,
for
(
int
j
=
0
;
j
<
obstacles_num_
;
++
j
)
{
// TODO(QiL): Load this from configuration
x_l
[
variable_index
]
=
0.0
;
x_u
[
variable_index
]
=
10.0
;
x_u
[
variable_index
]
=
2e19
;
// nlp_upper_bound_limit
++
variable_index
;
}
}
...
...
modules/planning/open_space/dual_variable_warm_start_ipopt_interface.h
浏览文件 @
1340894d
...
...
@@ -44,8 +44,7 @@ namespace planning {
class
DualVariableWarmStartIPOPTInterface
:
public
Ipopt
::
TNLP
{
public:
explicit
DualVariableWarmStartIPOPTInterface
(
int
num_of_variables
,
int
num_of_constraints
,
std
::
size_t
horizon
,
float
ts
,
const
Eigen
::
MatrixXd
&
ego
,
std
::
size_t
horizon
,
float
ts
,
const
Eigen
::
MatrixXd
&
ego
,
const
Eigen
::
MatrixXi
&
obstacles_edges_num
,
const
std
::
size_t
obstacles_num
,
const
Eigen
::
MatrixXd
&
obstacles_A
,
const
Eigen
::
MatrixXd
&
obstacles_b
,
const
Eigen
::
MatrixXd
&
xWS
,
...
...
modules/planning/open_space/dual_variable_warm_start_ipopt_interface_test.cc
浏览文件 @
1340894d
...
...
@@ -67,15 +67,8 @@ class DualVariableWarmStartIPOPTInterfaceTest : public ::testing::Test {
void
DualVariableWarmStartIPOPTInterfaceTest
::
ProblemSetup
()
{
obstacles_edges_num_
=
4
*
Eigen
::
MatrixXi
::
Ones
(
obstacles_num_
,
1
);
num_of_variables_
=
obstacles_edges_num_
.
sum
()
*
(
horizon_
+
1
)
+
4
*
obstacles_num_
*
(
horizon_
+
1
)
+
obstacles_num_
*
(
horizon_
+
1
);
num_of_constraints_
=
4
*
obstacles_num_
*
(
horizon_
+
1
);
Eigen
::
MatrixXd
xWS
=
Eigen
::
MatrixXd
::
Ones
(
4
,
horizon_
+
1
);
ptop_
.
reset
(
new
DualVariableWarmStartIPOPTInterface
(
num_of_variables_
,
num_of_constraints_
,
horizon_
,
ts_
,
ego_
,
ptop_
.
reset
(
new
DualVariableWarmStartIPOPTInterface
(
horizon_
,
ts_
,
ego_
,
obstacles_edges_num_
,
obstacles_num_
,
obstacles_A_
,
obstacles_b_
,
xWS
,
planner_open_space_config_
));
}
...
...
modules/planning/open_space/dual_variable_warm_start_problem.cc
浏览文件 @
1340894d
...
...
@@ -46,28 +46,11 @@ bool DualVariableWarmStartProblem::Solve(
const
Eigen
::
MatrixXd
&
obstacles_A
,
const
Eigen
::
MatrixXd
&
obstacles_b
,
const
Eigen
::
MatrixXd
&
xWS
,
Eigen
::
MatrixXd
*
l_warm_up
,
Eigen
::
MatrixXd
*
n_warm_up
)
{
// n1 : lagrangian multiplier associated with obstacleShape
int
n1
=
obstacles_edges_num
.
sum
()
*
(
horizon
+
1
);
// n2 : lagrangian multipier associated with car shape, obstacles_num*4 *
// (N+1)
int
n2
=
obstacles_num
*
4
*
(
horizon
+
1
);
// n3 : dual variable, obstacles_num * (N+1)
int
n3
=
obstacles_num
*
(
horizon
+
1
);
// m1 : obstacle constraints
int
m1
=
4
*
obstacles_num
*
(
horizon
+
1
);
int
num_of_variables
=
n1
+
n2
+
n3
;
int
num_of_constraints
=
m1
;
auto
t_start
=
cyber
::
Time
::
Now
().
ToSecond
();
DualVariableWarmStartIPOPTInterface
*
ptop
=
new
DualVariableWarmStartIPOPTInterface
(
num_of_variables
,
num_of_constraints
,
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
xWS
,
planner_open_space_config_
);
horizon
,
ts
,
ego
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
xWS
,
planner_open_space_config_
);
Ipopt
::
SmartPtr
<
Ipopt
::
TNLP
>
problem
=
ptop
;
...
...
@@ -78,7 +61,7 @@ bool DualVariableWarmStartProblem::Solve(
// TODO(QiL) : Change IPOPT settings to flag or configs
// app->Options()->SetStringValue("derivative_test", "first-order");
// app->Options()->SetNumericValue("derivative_test_tol", 1.0e-3);
int
print_level
=
0
;
int
print_level
=
5
;
app
->
Options
()
->
SetIntegerValue
(
"print_level"
,
print_level
);
int
num_iterations
=
0
;
app
->
Options
()
->
SetIntegerValue
(
"max_iter"
,
num_iterations
);
...
...
modules/planning/proto/planner_open_space_config.proto
浏览文件 @
1340894d
...
...
@@ -49,8 +49,5 @@ message DistanceApproachConfig {
optional
double
max_acceleration_reverse
=
12
[
default
=
2.0
];
optional
double
min_time_sample_scaling
=
13
[
default
=
0.1
];
optional
double
max_time_sample_scaling
=
14
[
default
=
10.0
];
optional
double
max_miu
=
15
[
default
=
1.0e10
];
optional
double
max_lambda
=
16
[
default
=
0.2
];
optional
double
max_safety_distance
=
17
[
default
=
1.0e10
];
optional
bool
use_fix_time
=
18
[
default
=
false
];
}
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录