Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
5286634c
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,发现更多精彩内容 >>
提交
5286634c
编写于
5月 08, 2019
作者:
D
deidaraho
提交者:
Jinyun Zhou
5月 09, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: open space mode simplify, fix delta t
上级
5b2cee8a
变更
6
展开全部
隐藏空白更改
内联
并排
Showing
6 changed file
with
1334 addition
and
0 deletion
+1334
-0
modules/planning/open_space/trajectory_smoother/BUILD
modules/planning/open_space/trajectory_smoother/BUILD
+28
-0
modules/planning/open_space/trajectory_smoother/distance_approach_ipopt_fixed_ts_interface.cc
...ry_smoother/distance_approach_ipopt_fixed_ts_interface.cc
+1037
-0
modules/planning/open_space/trajectory_smoother/distance_approach_ipopt_fixed_ts_interface.h
...ory_smoother/distance_approach_ipopt_fixed_ts_interface.h
+261
-0
modules/planning/open_space/trajectory_smoother/distance_approach_problem.cc
...en_space/trajectory_smoother/distance_approach_problem.cc
+6
-0
modules/planning/open_space/trajectory_smoother/distance_approach_problem.h
...pen_space/trajectory_smoother/distance_approach_problem.h
+1
-0
modules/planning/proto/planner_open_space_config.proto
modules/planning/proto/planner_open_space_config.proto
+1
-0
未找到文件。
modules/planning/open_space/trajectory_smoother/BUILD
浏览文件 @
5286634c
...
...
@@ -99,6 +99,7 @@ cc_library(
deps
=
[
":distance_approach_ipopt_cuda_interface"
,
":distance_approach_ipopt_interface"
,
":distance_approach_ipopt_fixed_ts_interface"
,
"//cyber/common:log"
,
"//modules/common/util"
,
"//modules/planning/common:planning_gflags"
,
...
...
@@ -106,6 +107,33 @@ cc_library(
],
)
cc_library
(
name
=
"distance_approach_ipopt_fixed_ts_interface"
,
srcs
=
[
"distance_approach_ipopt_fixed_ts_interface.cc"
,
],
hdrs
=
[
"distance_approach_interface.h"
,
"distance_approach_ipopt_fixed_ts_interface.h"
,
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
"-fopenmp"
,
],
deps
=
[
"//cyber/common:log"
,
"//modules/common/configs:vehicle_config_helper"
,
"//modules/common/math"
,
"//modules/common/util"
,
"//modules/planning/common:planning_gflags"
,
"//modules/planning/proto:planner_open_space_config_proto"
,
"//modules/planning/proto:planning_proto"
,
"@adolc"
,
"@eigen"
,
"@ipopt"
,
],
)
cc_library
(
name
=
"distance_approach_ipopt_interface"
,
srcs
=
[
...
...
modules/planning/open_space/trajectory_smoother/distance_approach_ipopt_fixed_ts_interface.cc
0 → 100644
浏览文件 @
5286634c
此差异已折叠。
点击以展开。
modules/planning/open_space/trajectory_smoother/distance_approach_ipopt_fixed_ts_interface.h
0 → 100644
浏览文件 @
5286634c
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/*
* @file
*/
#pragma once
#include <omp.h>
#include <algorithm>
#include <limits>
#include <vector>
#include "Eigen/Dense"
#include "IpTNLP.hpp"
#include "IpTypes.hpp"
#include "adolc/adolc.h"
#include "adolc/adolc_openmp.h"
#include "adolc/adolc_sparse.h"
#include "adolc/adouble.h"
#include "cyber/common/log.h"
#include "cyber/common/macros.h"
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/open_space/trajectory_smoother/distance_approach_interface.h"
#include "modules/planning/proto/planner_open_space_config.pb.h"
#define tag_f 1
#define tag_g 2
#define tag_L 3
#define HPOFF 30
namespace
apollo
{
namespace
planning
{
class
DistanceApproachIPOPTFixedTsInterface
:
public
DistanceApproachInterface
{
public:
explicit
DistanceApproachIPOPTFixedTsInterface
(
const
size_t
horizon
,
const
double
ts
,
const
Eigen
::
MatrixXd
&
ego
,
const
Eigen
::
MatrixXd
&
xWS
,
const
Eigen
::
MatrixXd
&
uWS
,
const
Eigen
::
MatrixXd
&
l_warm_up
,
const
Eigen
::
MatrixXd
&
n_warm_up
,
const
Eigen
::
MatrixXd
&
x0
,
const
Eigen
::
MatrixXd
&
xf
,
const
Eigen
::
MatrixXd
&
last_time_u
,
const
std
::
vector
<
double
>&
XYbounds
,
const
Eigen
::
MatrixXi
&
obstacles_edges_num
,
const
size_t
obstacles_num
,
const
Eigen
::
MatrixXd
&
obstacles_A
,
const
Eigen
::
MatrixXd
&
obstacles_b
,
const
PlannerOpenSpaceConfig
&
planner_open_space_config
);
virtual
~
DistanceApproachIPOPTFixedTsInterface
()
=
default
;
/** Method to return some info about the nlp */
bool
get_nlp_info
(
int
&
n
,
int
&
m
,
int
&
nnz_jac_g
,
int
&
nnz_h_lag
,
// NOLINT
IndexStyleEnum
&
index_style
)
override
;
// NOLINT
/** Method to return the bounds for my problem */
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 starting point for the algorithm */
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 objective value */
bool
eval_f
(
int
n
,
const
double
*
x
,
bool
new_x
,
double
&
obj_value
)
override
;
/** Method to return the gradient of the objective */
bool
eval_grad_f
(
int
n
,
const
double
*
x
,
bool
new_x
,
double
*
grad_f
)
override
;
/** Method to return the constraint residuals */
bool
eval_g
(
int
n
,
const
double
*
x
,
bool
new_x
,
int
m
,
double
*
g
)
override
;
/** Check unfeasible constraints for futher study**/
bool
check_g
(
int
n
,
const
double
*
x
,
int
m
,
const
double
*
g
);
/** 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_jac_g
(
int
n
,
const
double
*
x
,
bool
new_x
,
int
m
,
int
nele_jac
,
int
*
iRow
,
int
*
jCol
,
double
*
values
)
override
;
// sequential implementation to jac_g
bool
eval_jac_g_ser
(
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 hessian of the lagrangian (if "values" is
* nullptr) 2) The values of the hessian of the lagrangian (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
;
/** @name Solution Methods */
/** This method is called when the algorithm is complete so the TNLP can
* store/write the solution */
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
;
void
get_optimization_results
(
Eigen
::
MatrixXd
*
state_result
,
Eigen
::
MatrixXd
*
control_result
,
Eigen
::
MatrixXd
*
time_result
,
Eigen
::
MatrixXd
*
dual_l_result
,
Eigen
::
MatrixXd
*
dual_n_result
)
const
override
;
//*************** start ADOL-C part ***********************************
/** Template to return the objective value */
template
<
class
T
>
void
eval_obj
(
int
n
,
const
T
*
x
,
T
*
obj_value
);
/** Template to compute constraints */
template
<
class
T
>
void
eval_constraints
(
int
n
,
const
T
*
x
,
int
m
,
T
*
g
);
/** Method to generate the required tapes by ADOL-C*/
void
generate_tapes
(
int
n
,
int
m
,
int
*
nnz_jac_g
,
int
*
nnz_h_lag
);
//*************** end ADOL-C part ***********************************
private:
int
num_of_variables_
=
0
;
int
num_of_constraints_
=
0
;
int
horizon_
=
0
;
int
lambda_horizon_
=
0
;
int
miu_horizon_
=
0
;
double
ts_
=
0.0
;
Eigen
::
MatrixXd
ego_
;
Eigen
::
MatrixXd
xWS_
;
Eigen
::
MatrixXd
uWS_
;
Eigen
::
MatrixXd
l_warm_up_
;
Eigen
::
MatrixXd
n_warm_up_
;
Eigen
::
MatrixXd
x0_
;
Eigen
::
MatrixXd
xf_
;
Eigen
::
MatrixXd
last_time_u_
;
std
::
vector
<
double
>
XYbounds_
;
// debug flag
bool
enable_constraint_check_
;
// penalty
double
weight_state_x_
=
0.0
;
double
weight_state_y_
=
0.0
;
double
weight_state_phi_
=
0.0
;
double
weight_state_v_
=
0.0
;
double
weight_input_steer_
=
0.0
;
double
weight_input_a_
=
0.0
;
double
weight_rate_steer_
=
0.0
;
double
weight_rate_a_
=
0.0
;
double
weight_stitching_steer_
=
0.0
;
double
weight_stitching_a_
=
0.0
;
double
weight_first_order_time_
=
0.0
;
double
weight_second_order_time_
=
0.0
;
double
w_ev_
=
0.0
;
double
l_ev_
=
0.0
;
std
::
vector
<
double
>
g_
;
double
offset_
=
0.0
;
Eigen
::
MatrixXi
obstacles_edges_num_
;
int
obstacles_num_
=
0
;
int
obstacles_edges_sum_
=
0
;
double
wheelbase_
=
0.0
;
Eigen
::
MatrixXd
state_result_
;
Eigen
::
MatrixXd
dual_l_result_
;
Eigen
::
MatrixXd
dual_n_result_
;
Eigen
::
MatrixXd
control_result_
;
Eigen
::
MatrixXd
time_result_
;
// obstacles_A
Eigen
::
MatrixXd
obstacles_A_
;
// obstacles_b
Eigen
::
MatrixXd
obstacles_b_
;
// whether to use fix time
bool
use_fix_time_
=
false
;
// state start index
int
state_start_index_
=
0
;
// control start index.
int
control_start_index_
=
0
;
// time start index
int
time_start_index_
=
0
;
// lagrangian l start index
int
l_start_index_
=
0
;
// lagrangian n start index
int
n_start_index_
=
0
;
double
min_safety_distance_
=
0.0
;
double
max_safety_distance_
=
0.0
;
double
max_steer_angle_
=
0.0
;
double
max_speed_forward_
=
0.0
;
double
max_speed_reverse_
=
0.0
;
double
max_acceleration_forward_
=
0.0
;
double
max_acceleration_reverse_
=
0.0
;
double
min_time_sample_scaling_
=
0.0
;
double
max_time_sample_scaling_
=
0.0
;
double
max_steer_rate_
=
0.0
;
double
max_lambda_
=
0.0
;
double
max_miu_
=
0.0
;
private:
DistanceApproachConfig
distance_approach_config_
;
PlannerOpenSpaceConfig
planner_open_space_config_
;
const
common
::
VehicleParam
vehicle_param_
=
common
::
VehicleConfigHelper
::
GetConfig
().
vehicle_param
();
private:
//*************** start ADOL-C part ***********************************
double
*
obj_lam
;
//** variables for sparsity exploitation
unsigned
int
*
rind_g
;
/* row indices */
unsigned
int
*
cind_g
;
/* column indices */
double
*
jacval
;
/* values */
unsigned
int
*
rind_L
;
/* row indices */
unsigned
int
*
cind_L
;
/* column indices */
double
*
hessval
;
/* values */
int
nnz_jac
;
int
nnz_L
;
int
options_g
[
4
];
int
options_L
[
4
];
//*************** end ADOL-C part ***********************************
};
}
// namespace planning
}
// namespace apollo
modules/planning/open_space/trajectory_smoother/distance_approach_problem.cc
浏览文件 @
5286634c
...
...
@@ -52,6 +52,12 @@ bool DistanceApproachProblem::Solve(
horizon
,
ts
,
ego
,
xWS
,
uWS
,
l_warm_up
,
n_warm_up
,
x0
,
xF
,
last_time_u
,
XYbounds
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
planner_open_space_config_
);
}
else
if
(
planner_open_space_config_
.
distance_approach_config
()
.
distance_approach_mode
()
==
DISTANCE_APPROACH_IPOPT_FIXED_TS
)
{
ptop
=
new
DistanceApproachIPOPTFixedTsInterface
(
horizon
,
ts
,
ego
,
xWS
,
uWS
,
l_warm_up
,
n_warm_up
,
x0
,
xF
,
last_time_u
,
XYbounds
,
obstacles_edges_num
,
obstacles_num
,
obstacles_A
,
obstacles_b
,
planner_open_space_config_
);
}
else
if
(
planner_open_space_config_
.
distance_approach_config
()
.
distance_approach_mode
()
==
DISTANCE_APPROACH_IPOPT_CUDA
)
{
ptop
=
new
DistanceApproachIPOPTCUDAInterface
(
...
...
modules/planning/open_space/trajectory_smoother/distance_approach_problem.h
浏览文件 @
5286634c
...
...
@@ -30,6 +30,7 @@
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/open_space/trajectory_smoother/distance_approach_ipopt_cuda_interface.h"
#include "modules/planning/open_space/trajectory_smoother/distance_approach_ipopt_interface.h"
#include "modules/planning/open_space/trajectory_smoother/distance_approach_ipopt_fixed_ts_interface.h"
#include "modules/planning/proto/planning.pb.h"
namespace
apollo
{
...
...
modules/planning/proto/planner_open_space_config.proto
浏览文件 @
5286634c
...
...
@@ -14,6 +14,7 @@ enum DualWarmUpMode {
enum
DistanceApproachMode
{
DISTANCE_APPROACH_IPOPT
=
0
;
DISTANCE_APPROACH_IPOPT_CUDA
=
1
;
DISTANCE_APPROACH_IPOPT_FIXED_TS
=
2
;
}
message
PlannerOpenSpaceConfig
{
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录