Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
57cfb6ad
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,发现更多精彩内容 >>
提交
57cfb6ad
编写于
9月 27, 2018
作者:
L
luoqi06
提交者:
Qi Luo
9月 27, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning : open space planner for distance approach ipopt interface and test
上级
5dffebde
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
38 addition
and
19 deletion
+38
-19
modules/planning/planner/open_space/distance_approach_ipopt_interface.cc
...g/planner/open_space/distance_approach_ipopt_interface.cc
+14
-13
modules/planning/planner/open_space/distance_approach_ipopt_interface.h
...ng/planner/open_space/distance_approach_ipopt_interface.h
+7
-6
modules/planning/planner/open_space/distance_approach_ipopt_interface_test.cc
...nner/open_space/distance_approach_ipopt_interface_test.cc
+17
-0
未找到文件。
modules/planning/planner/open_space/distance_approach_ipopt_interface.cc
浏览文件 @
57cfb6ad
...
...
@@ -64,15 +64,15 @@ DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface(
*/
void
DistanceApproachIPOPTInterface
::
set_objective_weights
(
const
double
w
_u
,
const
double
w_time_1
,
const
double
w_time_2
,
const
double
w_reg
)
{
w
_u_
=
w
_u
;
const
double
w
eight_u
,
const
double
weight_time_1
,
const
double
w
eight_time_2
,
const
double
weight
_reg
)
{
w
eight_u_
=
weight
_u
;
w
_time_1_
=
w
_time_1
;
w
eight_time_1_
=
weight
_time_1
;
w
_time_2_
=
w
_time_2
;
w
eight_time_2_
=
weight
_time_2
;
w
_reg_
=
w
_reg
;
w
eight_reg_
=
weight
_reg
;
}
bool
DistanceApproachIPOPTInterface
::
get_nlp_info
(
int
&
n
,
int
&
m
,
...
...
@@ -445,7 +445,6 @@ bool DistanceApproachIPOPTInterface::eval_f(int n, const double* x, bool new_x,
// input, next horizon_ + 1 is sampling time, next horizon_ + 1 is
// lagrangian l, next horizon_ +1 is lagrangian n
// TODO(QiL) : Change the weight to configs
std
::
size_t
start_index
=
(
horizon_
+
1
)
*
4
;
std
::
size_t
time_start_index
=
start_index
+
horizon_
*
2
;
std
::
size_t
lagrangian_l_start_index
=
time_start_index
+
horizon_
+
1
;
...
...
@@ -454,17 +453,19 @@ bool DistanceApproachIPOPTInterface::eval_f(int n, const double* x, bool new_x,
// 1. objective to minimize u square
for
(
std
::
size_t
i
=
0
;
i
<
horizon_
;
++
i
)
{
obj_value
+=
0.1
*
x
[
start_index
+
i
]
*
x
[
start_index
+
i
]
+
x
[
start_index
+
i
]
*
x
[
start_index
+
i
+
1
]
+
0.5
*
x
[
time_start_index
+
i
]
+
x
[
time_start_index
+
i
]
*
x
[
time_start_index
+
i
];
obj_value
+=
weight_u_
*
x
[
start_index
+
i
]
*
x
[
start_index
+
i
]
+
x
[
start_index
+
i
]
*
x
[
start_index
+
i
+
1
]
+
weight_time_1_
*
x
[
time_start_index
+
i
]
+
weight_time_2_
*
x
[
time_start_index
+
i
]
*
x
[
time_start_index
+
i
];
}
// Add l , sum(obstacles_vertices_num) * (N+1)
constexpr
double
reg
=
1e-4
;
for
(
std
::
size_t
i
=
1
;
i
<
horizon_
+
1
;
++
i
)
{
for
(
std
::
size_t
j
=
1
;
j
<=
obstacles_vertices_sum_
;
++
j
)
{
obj_value
+=
reg
*
x
[
lagrangian_l_start_index
+
i
*
(
horizon_
+
1
)
+
j
]
*
obj_value
+=
weight_reg_
*
x
[
lagrangian_l_start_index
+
i
*
(
horizon_
+
1
)
+
j
]
*
x
[
lagrangian_l_start_index
+
i
*
(
horizon_
+
1
)
+
j
];
}
}
...
...
@@ -473,7 +474,7 @@ bool DistanceApproachIPOPTInterface::eval_f(int n, const double* x, bool new_x,
for
(
std
::
size_t
i
=
1
;
i
<
horizon_
+
1
;
++
i
)
{
// TODO(QiL) : Double check the dimensions here !!!!!!!!!
for
(
std
::
size_t
j
=
1
;
j
<=
4
*
obstacles_num_
;
++
j
)
{
obj_value
+=
reg
*
obj_value
+=
weight_reg_
*
x
[
lagrangian_n_start_index
+
i
*
4
*
obstacles_num_
+
j
]
*
x
[
lagrangian_n_start_index
+
i
*
4
*
obstacles_num_
+
j
];
}
...
...
modules/planning/planner/open_space/distance_approach_ipopt_interface.h
浏览文件 @
57cfb6ad
...
...
@@ -42,8 +42,9 @@ class DistanceApproachIPOPTInterface : public Ipopt::TNLP {
virtual
~
DistanceApproachIPOPTInterface
()
=
default
;
void
set_objective_weights
(
const
double
w_u
,
const
double
w_time_1
,
const
double
w_time_2
,
const
double
w_reg
);
void
set_objective_weights
(
const
double
weight_u
,
const
double
weight_time_1
,
const
double
weight_time_2
,
const
double
weight_reg
);
void
get_optimization_results
(
Eigen
::
MatrixXd
*
state_result
,
Eigen
::
MatrixXd
*
control_result
,
...
...
@@ -121,13 +122,13 @@ class DistanceApproachIPOPTInterface : public Ipopt::TNLP {
private:
// weight from u, timescale linear form, timescale qudartic form,
// regularization
double
w_u_
=
0.0
;
double
w
eight
_u_
=
0.0
;
double
w_time_1_
=
0.0
;
double
w
eight
_time_1_
=
0.0
;
double
w_time_2_
=
0.0
;
double
w
eight
_time_2_
=
0.0
;
double
w_reg_
=
0.0
;
double
w
eight
_reg_
=
0.0
;
};
}
// namespace planning
...
...
modules/planning/planner/open_space/distance_approach_ipopt_interface_test.cc
浏览文件 @
57cfb6ad
...
...
@@ -64,6 +64,13 @@ void DistanceApproachIPOPTInterfaceTest::ProblemSetup() {
ptop_
.
reset
(
new
DistanceApproachIPOPTInterface
(
num_of_variables_
,
num_of_constraints_
,
horizon_
,
ts_
,
ego_
,
x0_
,
xf_
,
XYbounds_
,
obstacles_vertices_num_
,
obstacles_num_
));
double
weight_u
=
0.1
;
double
weight_time_1
=
0.2
;
double
weight_time_2
=
0.3
;
double
weight_reg
=
0.4
;
ptop_
->
set_objective_weights
(
weight_u
,
weight_time_1
,
weight_time_2
,
weight_reg
);
}
TEST_F
(
DistanceApproachIPOPTInterfaceTest
,
initilization
)
{
...
...
@@ -109,5 +116,15 @@ TEST_F(DistanceApproachIPOPTInterfaceTest, get_starting_point) {
EXPECT_TRUE
(
res
);
}
TEST_F
(
DistanceApproachIPOPTInterfaceTest
,
eval_f
)
{
int
kNumOfVariables
=
520
;
double
obj_value
;
double
x
[
kNumOfVariables
];
std
::
fill_n
(
x
,
kNumOfVariables
,
1.2
);
bool
res
=
ptop_
->
eval_f
(
kNumOfVariables
,
x
,
true
,
obj_value
);
EXPECT_TRUE
(
res
);
EXPECT_DOUBLE_EQ
(
obj_value
,
241.10399999999802
);
}
}
// namespace planning
}
// namespace apollo
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录