Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
83bed011
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,发现更多精彩内容 >>
提交
83bed011
编写于
5月 08, 2019
作者:
Y
Yajia Zhang
提交者:
Qi Luo
5月 09, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: code refactor for PiecewiseJerkProblem
上级
94c5fb09
变更
17
隐藏空白更改
内联
并排
Showing
17 changed file
with
377 addition
and
418 deletion
+377
-418
modules/planning/common/BUILD
modules/planning/common/BUILD
+1
-1
modules/planning/common/speed_profile_generator.cc
modules/planning/common/speed_profile_generator.cc
+22
-16
modules/planning/math/piecewise_jerk/BUILD
modules/planning/math/piecewise_jerk/BUILD
+6
-19
modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.cc
...anning/math/piecewise_jerk/piecewise_jerk_path_problem.cc
+34
-25
modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.h
...lanning/math/piecewise_jerk/piecewise_jerk_path_problem.h
+16
-6
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
...es/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
+71
-89
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h
...les/planning/math/piecewise_jerk/piecewise_jerk_problem.h
+4
-42
modules/planning/math/piecewise_jerk/piecewise_jerk_problem_test.cc
...anning/math/piecewise_jerk/piecewise_jerk_problem_test.cc
+0
-158
modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc
...nning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc
+124
-0
modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.h
...anning/math/piecewise_jerk/piecewise_jerk_speed_problem.h
+29
-6
modules/planning/open_space/coarse_trajectory_generator/BUILD
...les/planning/open_space/coarse_trajectory_generator/BUILD
+1
-1
modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc
...g/open_space/coarse_trajectory_generator/hybrid_a_star.cc
+21
-14
modules/planning/tasks/deciders/BUILD
modules/planning/tasks/deciders/BUILD
+1
-1
modules/planning/tasks/optimizers/piecewise_jerk_path/BUILD
modules/planning/tasks/optimizers/piecewise_jerk_path/BUILD
+1
-1
modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc
...zers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc
+19
-12
modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD
modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD
+1
-1
modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc
...rs/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc
+26
-26
未找到文件。
modules/planning/common/BUILD
浏览文件 @
83bed011
...
@@ -252,7 +252,7 @@ cc_library(
...
@@ -252,7 +252,7 @@ cc_library(
"//modules/planning/math/curve1d:polynomial_curve1d"
,
"//modules/planning/math/curve1d:polynomial_curve1d"
,
"//modules/planning/math/curve1d:quartic_polynomial_curve1d"
,
"//modules/planning/math/curve1d:quartic_polynomial_curve1d"
,
"//modules/planning/math/curve1d:quintic_polynomial_curve1d"
,
"//modules/planning/math/curve1d:quintic_polynomial_curve1d"
,
"//modules/planning/math/piecewise_jerk:p
ath_time_qp
_problem"
,
"//modules/planning/math/piecewise_jerk:p
iecewise_jerk_speed
_problem"
,
"//modules/planning/proto:planning_config_proto"
,
"//modules/planning/proto:planning_config_proto"
,
],
],
)
)
...
...
modules/planning/common/speed_profile_generator.cc
浏览文件 @
83bed011
...
@@ -24,11 +24,11 @@
...
@@ -24,11 +24,11 @@
#include <limits>
#include <limits>
#include <memory>
#include <memory>
#include "../math/piecewise_jerk/piecewise_jerk_speed_problem.h"
#include "cyber/common/log.h"
#include "cyber/common/log.h"
#include "modules/planning/common/ego_info.h"
#include "modules/planning/common/ego_info.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/piecewise_jerk/path_time_qp_problem.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
...
@@ -149,39 +149,45 @@ SpeedData SpeedProfileGenerator::GenerateFallbackSpeed(
...
@@ -149,39 +149,45 @@ SpeedData SpeedProfileGenerator::GenerateFallbackSpeed(
// TODO(all): dt is too small;
// TODO(all): dt is too small;
double
delta_t
=
FLAGS_fallback_time_unit
;
double
delta_t
=
FLAGS_fallback_time_unit
;
double
total_time
=
FLAGS_fallback_total_time
;
double
total_time
=
FLAGS_fallback_total_time
;
int
num_of_knots
=
static_cast
<
int
>
(
total_time
/
delta_t
)
+
1
;
size_t
num_of_knots
=
static_cast
<
size_t
>
(
total_time
/
delta_t
)
+
1
;
// Start a PathTimeQpProblem
std
::
unique_ptr
<
PathTimeQpProblem
>
path_time_qp
(
new
PathTimeQpProblem
());
path_time_qp
->
InitProblem
(
num_of_knots
,
delta_t
,
w
,
init_s
,
end_s
);
path_time_qp
->
SetZeroOrderBounds
(
0.0
,
std
::
fmax
(
stop_distance
,
100.0
));
PiecewiseJerkSpeedProblem
piecewise_jerk_problem
(
path_time_qp
->
SetFirstOrderBounds
(
num_of_knots
,
delta_t
,
init_s
,
end_s
);
piecewise_jerk_problem
.
set_weight_x
(
w
[
0
]);
piecewise_jerk_problem
.
set_weight_dx
(
w
[
1
]);
piecewise_jerk_problem
.
set_weight_ddx
(
w
[
2
]);
piecewise_jerk_problem
.
set_weight_dddx
(
w
[
3
]);
piecewise_jerk_problem
.
set_weight_x_reference
(
w
[
4
]);
piecewise_jerk_problem
.
SetZeroOrderBounds
(
0.0
,
std
::
fmax
(
stop_distance
,
100.0
));
piecewise_jerk_problem
.
SetFirstOrderBounds
(
0.0
,
std
::
fmax
(
FLAGS_planning_upper_speed_limit
,
init_v
));
0.0
,
std
::
fmax
(
FLAGS_planning_upper_speed_limit
,
init_v
));
p
ath_time_qp
->
SetSecondOrderBounds
(
veh_param
.
max_deceleration
(),
p
iecewise_jerk_problem
.
SetSecondOrderBounds
(
veh_param
.
max_deceleration
(),
veh_param
.
max_acceleration
());
veh_param
.
max_acceleration
());
// TODO(Hongyi): Set back to vehicle_params when ready
// TODO(Hongyi): Set back to vehicle_params when ready
p
ath_time_qp
->
SetSecondOrderBounds
(
-
4.4
,
2.0
);
p
iecewise_jerk_problem
.
SetSecondOrderBounds
(
-
4.4
,
2.0
);
p
ath_time_qp
->
SetThirdOrderBound
(
FLAGS_longitudinal_jerk_bound
);
p
iecewise_jerk_problem
.
SetThirdOrderBound
(
FLAGS_longitudinal_jerk_bound
);
// Solve the problem
// Solve the problem
if
(
!
p
ath_time_qp
->
Optimize
())
{
if
(
!
p
iecewise_jerk_problem
.
Optimize
())
{
AERROR
<<
"Piecewise jerk fallback speed optimizer failed!"
;
AERROR
<<
"Piecewise jerk fallback speed optimizer failed!"
;
return
GenerateStopProfile
(
init_v
,
init_a
);
return
GenerateStopProfile
(
init_v
,
init_a
);
}
}
// Extract output
// Extract output
const
std
::
vector
<
double
>&
s
=
p
ath_time_qp
->
x
();
const
std
::
vector
<
double
>&
s
=
p
iecewise_jerk_problem
.
x
();
const
std
::
vector
<
double
>&
ds
=
p
ath_time_qp
->
x_derivative
();
const
std
::
vector
<
double
>&
ds
=
p
iecewise_jerk_problem
.
x_derivative
();
const
std
::
vector
<
double
>&
dds
=
p
ath_time_qp
->
x_second_order_derivative
();
const
std
::
vector
<
double
>&
dds
=
p
iecewise_jerk_problem
.
x_second_order_derivative
();
SpeedData
speed_data
;
SpeedData
speed_data
;
speed_data
.
AppendSpeedPoint
(
s
[
0
],
0.0
,
ds
[
0
],
dds
[
0
],
0.0
);
speed_data
.
AppendSpeedPoint
(
s
[
0
],
0.0
,
ds
[
0
],
dds
[
0
],
0.0
);
for
(
in
t
i
=
1
;
i
<
num_of_knots
;
++
i
)
{
for
(
size_
t
i
=
1
;
i
<
num_of_knots
;
++
i
)
{
// Avoid the very last points when already stopped
// Avoid the very last points when already stopped
if
(
s
[
i
]
-
s
[
i
-
1
]
<=
0.0
||
ds
[
i
]
<=
0.0
)
{
if
(
s
[
i
]
-
s
[
i
-
1
]
<=
0.0
||
ds
[
i
]
<=
0.0
)
{
break
;
break
;
}
}
speed_data
.
AppendSpeedPoint
(
s
[
i
],
delta_t
*
i
,
ds
[
i
],
dds
[
i
],
speed_data
.
AppendSpeedPoint
(
s
[
i
],
delta_t
*
static_cast
<
double
>
(
i
)
,
ds
[
i
],
dds
[
i
],
(
dds
[
i
]
-
dds
[
i
-
1
])
/
delta_t
);
(
dds
[
i
]
-
dds
[
i
-
1
])
/
delta_t
);
}
}
FillEnoughSpeedPoints
(
&
speed_data
);
FillEnoughSpeedPoints
(
&
speed_data
);
...
...
modules/planning/math/piecewise_jerk/BUILD
浏览文件 @
83bed011
...
@@ -17,26 +17,13 @@ cc_library(
...
@@ -17,26 +17,13 @@ cc_library(
],
],
)
)
cc_test
(
name
=
"piecewise_jerk_problem_test"
,
size
=
"small"
,
srcs
=
[
"piecewise_jerk_problem_test.cc"
,
],
deps
=
[
":piecewise_jerk_problem"
,
"//cyber/common:log"
,
"@gtest//:main"
,
],
)
cc_library
(
cc_library
(
name
=
"
fem_1d_qp
_problem"
,
name
=
"
piecewise_jerk_path
_problem"
,
srcs
=
[
srcs
=
[
"
fem_1d_qp
_problem.cc"
,
"
piecewise_jerk_path
_problem.cc"
,
],
],
hdrs
=
[
hdrs
=
[
"
fem_1d_qp
_problem.h"
,
"
piecewise_jerk_path
_problem.h"
,
],
],
deps
=
[
deps
=
[
":piecewise_jerk_problem"
,
":piecewise_jerk_problem"
,
...
@@ -44,12 +31,12 @@ cc_library(
...
@@ -44,12 +31,12 @@ cc_library(
)
)
cc_library
(
cc_library
(
name
=
"p
ath_time_qp
_problem"
,
name
=
"p
iecewise_jerk_speed
_problem"
,
srcs
=
[
srcs
=
[
"p
ath_time_qp
_problem.cc"
,
"p
iecewise_jerk_speed
_problem.cc"
,
],
],
hdrs
=
[
hdrs
=
[
"p
ath_time_qp
_problem.h"
,
"p
iecewise_jerk_speed
_problem.h"
,
],
],
deps
=
[
deps
=
[
":piecewise_jerk_problem"
,
":piecewise_jerk_problem"
,
...
...
modules/planning/math/piecewise_jerk/
fem_1d_qp
_problem.cc
→
modules/planning/math/piecewise_jerk/
piecewise_jerk_path
_problem.cc
浏览文件 @
83bed011
...
@@ -14,59 +14,68 @@
...
@@ -14,59 +14,68 @@
* limitations under the License.
* limitations under the License.
*****************************************************************************/
*****************************************************************************/
#include "modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h"
#include <algorithm>
#include <algorithm>
#include "cyber/common/log.h"
#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
void
Fem1dQpProblem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
PiecewiseJerkPathProblem
::
PiecewiseJerkPathProblem
(
const
size_t
num_of_knots
,
const
double
delta_s
,
const
std
::
array
<
double
,
3
>&
x_init
)
:
PiecewiseJerkProblem
(
num_of_knots
,
delta_s
,
x_init
)
{}
void
PiecewiseJerkPathProblem
::
SetZeroOrderReference
(
std
::
vector
<
double
>
x_ref
)
{
CHECK_EQ
(
x_ref
.
size
(),
num_of_knots_
);
x_ref_
=
std
::
move
(
x_ref
);
}
void
PiecewiseJerkPathProblem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
)
{
std
::
vector
<
c_int
>*
P_indptr
)
{
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
n
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
kNumParam
=
3
*
N
;
const
int
kNumParam
=
3
*
n
;
const
int
kNumValue
=
kNumParam
+
(
N
-
1
);
const
int
kNumValue
=
kNumParam
+
(
n
-
1
);
std
::
vector
<
std
::
vector
<
std
::
pair
<
c_int
,
c_float
>>>
columns
;
std
::
vector
<
std
::
vector
<
std
::
pair
<
c_int
,
c_float
>>>
columns
;
columns
.
resize
(
kNumParam
);
columns
.
resize
(
kNumParam
);
int
value_index
=
0
;
int
value_index
=
0
;
// x(i)^2 * (w_x + w_ref)
// x(i)^2 * (w_x + w_ref)
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
for
(
int
i
=
0
;
i
<
n
;
++
i
)
{
columns
[
i
].
emplace_back
(
i
,
(
weight_
.
x_w
+
weight_
.
x_ref_w
));
columns
[
i
].
emplace_back
(
i
,
(
weight_
x_
+
weight_x_reference_
));
++
value_index
;
++
value_index
;
}
}
// x(i)'^2 * w_dx
// x(i)'^2 * w_dx
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
for
(
int
i
=
0
;
i
<
n
;
++
i
)
{
columns
[
N
+
i
].
emplace_back
(
N
+
i
,
weight_
.
x_derivative_w
);
columns
[
n
+
i
].
emplace_back
(
n
+
i
,
weight_dx_
);
++
value_index
;
++
value_index
;
}
}
auto
delta_s_square
=
delta_s_
*
delta_s_
;
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
columns
[
2
*
N
].
emplace_back
(
columns
[
2
*
n
].
emplace_back
(
2
*
N
,
weight_
.
x_second_order_derivative_w
+
2
*
n
,
weight_ddx_
+
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
weight_
dddx_
/
delta_s_square
);
++
value_index
;
++
value_index
;
for
(
int
i
=
1
;
i
<
N
-
1
;
++
i
)
{
for
(
int
i
=
1
;
i
<
n
-
1
;
++
i
)
{
columns
[
2
*
N
+
i
].
emplace_back
(
columns
[
2
*
n
+
i
].
emplace_back
(
2
*
N
+
i
,
weight_
.
x_second_order_derivative_w
+
2
*
n
+
i
,
weight_ddx_
+
2.0
*
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
2.0
*
weight_
dddx_
/
delta_s_square
);
++
value_index
;
++
value_index
;
}
}
columns
[
3
*
N
-
1
].
emplace_back
(
columns
[
3
*
n
-
1
].
emplace_back
(
3
*
N
-
1
,
weight_
.
x_second_order_derivative_w
+
3
*
n
-
1
,
weight_ddx_
+
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
weight_
dddx_
/
delta_s_square
);
++
value_index
;
++
value_index
;
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
for
(
int
i
=
0
;
i
<
N
-
1
;
++
i
)
{
for
(
int
i
=
0
;
i
<
n
-
1
;
++
i
)
{
columns
[
2
*
N
+
i
].
emplace_back
(
columns
[
2
*
n
+
i
].
emplace_back
(
2
*
N
+
i
+
1
,
-
2.0
*
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
2
*
n
+
i
+
1
,
-
2.0
*
weight_dddx_
/
delta_s_square
);
++
value_index
;
++
value_index
;
}
}
...
@@ -84,13 +93,13 @@ void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
...
@@ -84,13 +93,13 @@ void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
P_indptr
->
push_back
(
ind_p
);
P_indptr
->
push_back
(
ind_p
);
}
}
void
Fem1dQp
Problem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
void
PiecewiseJerkPath
Problem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
CHECK_NOTNULL
(
q
);
CHECK_NOTNULL
(
q
);
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
kNumParam
=
3
*
N
;
const
int
kNumParam
=
3
*
N
;
q
->
resize
(
kNumParam
);
q
->
resize
(
kNumParam
);
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
q
->
at
(
i
)
+=
-
2.0
*
weight_
.
x_ref_w
*
x_ref_
[
i
];
q
->
at
(
i
)
+=
-
2.0
*
weight_
x_reference_
*
x_ref_
[
i
];
}
}
}
}
...
...
modules/planning/math/piecewise_jerk/
fem_1d_qp
_problem.h
→
modules/planning/math/piecewise_jerk/
piecewise_jerk_path
_problem.h
浏览文件 @
83bed011
...
@@ -47,19 +47,29 @@ namespace planning {
...
@@ -47,19 +47,29 @@ namespace planning {
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
*/
*/
class
Fem1dQp
Problem
:
public
PiecewiseJerkProblem
{
class
PiecewiseJerkPath
Problem
:
public
PiecewiseJerkProblem
{
public:
public:
Fem1dQpProblem
()
=
default
;
PiecewiseJerkPathProblem
(
const
size_t
num_of_knots
,
const
double
delta_s
,
const
std
::
array
<
double
,
3
>&
x_init
);
virtual
~
Fem1dQpProblem
()
=
default
;
virtual
~
PiecewiseJerkPathProblem
()
=
default
;
void
SetZeroOrderReference
(
std
::
vector
<
double
>
x_ref
);
void
set_weight_x_reference
(
const
double
weight_x_reference
)
{
weight_x_reference_
=
weight_x_reference
;
}
protected:
protected:
// naming convention follows osqp solver.
virtual
void
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
void
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
)
override
;
std
::
vector
<
c_int
>*
P_indptr
)
override
;
void
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
override
;
virtual
void
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
override
;
double
weight_x_reference_
=
0.0
;
std
::
vector
<
double
>
x_ref_
;
};
};
}
// namespace planning
}
// namespace planning
...
...
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
浏览文件 @
83bed011
...
@@ -29,15 +29,12 @@ namespace {
...
@@ -29,15 +29,12 @@ namespace {
constexpr
double
kMaxVariableRange
=
1.0e10
;
constexpr
double
kMaxVariableRange
=
1.0e10
;
}
// namespace
}
// namespace
void
PiecewiseJerkProblem
::
InitProblem
(
const
size_t
num_of_knots
,
PiecewiseJerkProblem
::
PiecewiseJerkProblem
(
const
size_t
num_of_knots
,
const
double
delta_s
,
const
double
delta_s
,
const
std
::
array
<
double
,
3
>&
x_init
)
{
const
std
::
array
<
double
,
3
>&
x_init
,
const
std
::
array
<
double
,
3
>&
x_end
)
{
CHECK_GE
(
num_of_knots
,
2
);
CHECK_GE
(
num_of_knots
,
2
);
num_of_knots_
=
num_of_knots
;
num_of_knots_
=
num_of_knots
;
x_init_
=
x_init
;
x_init_
=
x_init
;
x_end_
=
x_end
;
delta_s_
=
delta_s
;
delta_s_
=
delta_s
;
...
@@ -60,6 +57,8 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
...
@@ -60,6 +57,8 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
std
::
vector
<
c_float
>&
upper_bounds
,
// NOLINT
std
::
vector
<
c_float
>&
upper_bounds
,
// NOLINT
std
::
vector
<
c_float
>&
q
,
OSQPData
*
data
,
OSQPWorkspace
**
work
,
// NOLINT
std
::
vector
<
c_float
>&
q
,
OSQPData
*
data
,
OSQPWorkspace
**
work
,
// NOLINT
OSQPSettings
*
settings
)
{
OSQPSettings
*
settings
)
{
CHECK_EQ
(
lower_bounds
.
size
(),
upper_bounds
.
size
());
data
->
n
=
kernel_dim
;
data
->
n
=
kernel_dim
;
data
->
m
=
num_affine_constraint
;
data
->
m
=
num_affine_constraint
;
data
->
P
=
csc_matrix
(
data
->
n
,
data
->
n
,
P_data
.
size
(),
P_data
.
data
(),
data
->
P
=
csc_matrix
(
data
->
n
,
data
->
n
,
P_data
.
size
(),
P_data
.
data
(),
...
@@ -70,8 +69,6 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
...
@@ -70,8 +69,6 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
data
->
l
=
lower_bounds
.
data
();
data
->
l
=
lower_bounds
.
data
();
data
->
u
=
upper_bounds
.
data
();
data
->
u
=
upper_bounds
.
data
();
CHECK_EQ
(
lower_bounds
.
size
(),
upper_bounds
.
size
());
*
work
=
osqp_setup
(
data
,
settings
);
*
work
=
osqp_setup
(
data
,
settings
);
// Solve Problem
// Solve Problem
...
@@ -92,17 +89,6 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
...
@@ -92,17 +89,6 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
return
true
;
return
true
;
}
}
void
PiecewiseJerkProblem
::
SetZeroOrderReference
(
std
::
vector
<
double
>
x_ref
)
{
CHECK_EQ
(
x_ref
.
size
(),
num_of_knots_
);
x_ref_
=
std
::
move
(
x_ref
);
}
void
PiecewiseJerkProblem
::
SetFirstOrderPenalty
(
std
::
vector
<
double
>
penalty_dx
)
{
CHECK_EQ
(
penalty_dx
.
size
(),
num_of_knots_
);
penalty_dx_
=
std
::
move
(
penalty_dx
);
}
void
PiecewiseJerkProblem
::
SetZeroOrderBounds
(
void
PiecewiseJerkProblem
::
SetZeroOrderBounds
(
std
::
vector
<
std
::
pair
<
double
,
double
>>
x_bounds
)
{
std
::
vector
<
std
::
pair
<
double
,
double
>>
x_bounds
)
{
CHECK_EQ
(
x_bounds
.
size
(),
num_of_knots_
);
CHECK_EQ
(
x_bounds
.
size
(),
num_of_knots_
);
...
@@ -202,8 +188,6 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) {
...
@@ -202,8 +188,6 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) {
dx_
.
at
(
i
)
=
work
->
solution
->
x
[
i
+
num_of_knots_
];
dx_
.
at
(
i
)
=
work
->
solution
->
x
[
i
+
num_of_knots_
];
ddx_
.
at
(
i
)
=
work
->
solution
->
x
[
i
+
2
*
num_of_knots_
];
ddx_
.
at
(
i
)
=
work
->
solution
->
x
[
i
+
2
*
num_of_knots_
];
}
}
dx_
.
back
()
=
work
->
solution
->
x
[
2
*
num_of_knots_
-
1
];
ddx_
.
back
()
=
work
->
solution
->
x
[
3
*
num_of_knots_
-
1
];
// Cleanup
// Cleanup
osqp_cleanup
(
work
);
osqp_cleanup
(
work
);
...
@@ -215,63 +199,61 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) {
...
@@ -215,63 +199,61 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) {
return
true
;
return
true
;
}
}
void
PiecewiseJerkProblem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
//void PiecewiseJerkProblem::CalculateKernel(std::vector<c_float>* P_data,
std
::
vector
<
c_int
>*
P_indices
,
// std::vector<c_int>* P_indices,
std
::
vector
<
c_int
>*
P_indptr
)
{
// std::vector<c_int>* P_indptr) {
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
// const size_t n = num_of_knots_;
const
int
kNumParam
=
3
*
N
;
// const size_t num_of_variables = 3 * n;
const
int
kNumValue
=
kNumParam
+
(
N
-
1
);
// std::vector<std::vector<std::pair<c_int, c_float>>> columns;
std
::
vector
<
std
::
vector
<
std
::
pair
<
c_int
,
c_float
>>>
columns
;
// columns.resize(num_of_variables);
columns
.
resize
(
kNumParam
);
// size_t value_index = 0;
int
value_index
=
0
;
//
// // x(i)^2 * (w_x + w_ref)
// x(i)^2 * (w_x + w_ref)
// for (size_t i = 0; i < n; ++i) {
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
// columns[i].emplace_back(i, weight_x_ + weight_x_reference_);
columns
[
i
].
emplace_back
(
i
,
(
weight_x_
+
weight_x_reference_
));
// ++value_index;
++
value_index
;
// }
}
//
// // x(i)'^2 * w_dx
// x(i)'^2 * w_dx
// for (size_t i = 0; i < n; ++i) {
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
// columns[n + i].emplace_back(n + i, weight_dx_);
columns
[
N
+
i
].
emplace_back
(
N
+
i
,
weight_dx_
);
// ++value_index;
++
value_index
;
// }
}
//
// auto delta_s_square = delta_s_ * delta_s_;
auto
delta_s_sq_
=
delta_s_
*
delta_s_
;
// // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
// columns[2 * n].emplace_back(
columns
[
2
*
N
].
emplace_back
(
// 2 * n, weight_ddx_ + weight_dddx_ / delta_s_square);
2
*
N
,
weight_ddx_
+
weight_dddx_
/
delta_s_sq_
);
// ++value_index;
// for (size_t i = 1; i + 1 < n; ++i) {
++
value_index
;
// columns[2 * n + i].emplace_back(
for
(
int
i
=
1
;
i
<
N
-
1
;
++
i
)
{
// 2 * n + i, weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square);
columns
[
2
*
N
+
i
].
emplace_back
(
// ++value_index;
2
*
N
+
i
,
weight_ddx_
+
2.0
*
weight_dddx_
/
delta_s_sq_
);
// }
++
value_index
;
// columns[3 * n - 1].emplace_back(
}
// 3 * n - 1, weight_ddx_ + weight_dddx_ / delta_s_square);
columns
[
3
*
N
-
1
].
emplace_back
(
// ++value_index;
3
*
N
-
1
,
weight_ddx_
+
weight_dddx_
/
delta_s_sq_
);
//
++
value_index
;
// // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
// for (size_t i = 0; i + 1 < n; ++i) {
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
// columns[2 * n + i].emplace_back(
for
(
int
i
=
0
;
i
<
N
-
1
;
++
i
)
{
// 2 * n + i + 1, -2.0 * weight_dddx_ / delta_s_square);
columns
[
2
*
N
+
i
].
emplace_back
(
// ++value_index;
2
*
N
+
i
+
1
,
-
2.0
*
weight_dddx_
/
delta_s_sq_
);
// }
++
value_index
;
//
}
// CHECK_EQ(value_index, num_of_variables + n - 1);
//
CHECK_EQ
(
value_index
,
kNumValue
);
// size_t ind_p = 0;
// for (size_t i = 0; i < num_of_variables; ++i) {
int
ind_p
=
0
;
// P_indptr->push_back(ind_p);
for
(
int
i
=
0
;
i
<
kNumParam
;
++
i
)
{
// for (const auto& row_data_pair : columns[i]) {
P_indptr
->
push_back
(
ind_p
);
// P_indices->push_back(row_data_pair.first);
for
(
const
auto
&
row_data_pair
:
columns
[
i
])
{
// P_data->push_back(row_data_pair.second * 2.0);
P_data
->
push_back
(
row_data_pair
.
second
*
2.0
);
// ++ind_p;
P_indices
->
push_back
(
row_data_pair
.
first
);
// }
++
ind_p
;
// }
}
// P_indptr->push_back(ind_p);
}
//}
P_indptr
->
push_back
(
ind_p
);
}
void
PiecewiseJerkProblem
::
CalculateAffineConstraint
(
void
PiecewiseJerkProblem
::
CalculateAffineConstraint
(
std
::
vector
<
c_float
>*
A_data
,
std
::
vector
<
c_int
>*
A_indices
,
std
::
vector
<
c_float
>*
A_data
,
std
::
vector
<
c_int
>*
A_indices
,
...
@@ -371,18 +353,18 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
...
@@ -371,18 +353,18 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
A_indptr
->
push_back
(
ind_p
);
A_indptr
->
push_back
(
ind_p
);
}
}
void
PiecewiseJerkProblem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
//
void PiecewiseJerkProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL
(
q
);
//
CHECK_NOTNULL(q);
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
)
;
// const size_t N = num_of_knots_
;
const
int
kNumParam
=
3
*
N
;
// q->resize(3 * N, 0.0)
;
q
->
resize
(
kNumParam
);
//
for
(
in
t
i
=
0
;
i
<
N
;
++
i
)
{
// for (size_
t i = 0; i < N; ++i) {
q
->
at
(
i
)
+=
-
2.0
*
weight_x_reference_
*
x_ref_
[
i
];
//
q->at(i) += -2.0 * weight_x_reference_ * x_ref_[i];
}
//
}
q
->
at
(
N
-
1
)
+=
-
2.0
*
weight_x_
*
x_end_
[
0
];
////
q->at(N - 1) += -2.0 * weight_x_ * x_end_[0];
q
->
at
(
N
*
2
-
1
)
+=
-
2.0
*
weight_dx_
*
x_end_
[
1
];
////
q->at(N * 2 - 1) += -2.0 * weight_dx_ * x_end_[1];
q
->
at
(
N
*
3
-
1
)
+=
-
2.0
*
weight_ddx_
*
x_end_
[
2
];
////
q->at(N * 3 - 1) += -2.0 * weight_ddx_ * x_end_[2];
}
//
}
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h
浏览文件 @
83bed011
...
@@ -48,40 +48,11 @@ namespace planning {
...
@@ -48,40 +48,11 @@ namespace planning {
class
PiecewiseJerkProblem
{
class
PiecewiseJerkProblem
{
public:
public:
PiecewiseJerkProblem
()
=
default
;
PiecewiseJerkProblem
(
const
size_t
num_of_knots
,
const
double
delta_s
,
const
std
::
array
<
double
,
3
>&
x_init
);
virtual
~
PiecewiseJerkProblem
()
=
default
;
virtual
~
PiecewiseJerkProblem
()
=
default
;
/*
* @param
* x_init: the init status of x, x', x''
* delta_s: s(k) - s(k-1)
* x_bounds: x_bounds[i].first < x(i) < x_bounds[i].second
* w: weight array
* -- w[0]: x^2 term weight
* -- w[1]: (x')^2 term weight
* -- w[2]: (x'')^2 term weight
* -- w[3]: (x''')^2 term weight
* -- w[4]: default reference line weight, (x_bounds[k].first +
* x_bounds[k].second)/2
*/
virtual
void
InitProblem
(
const
size_t
num_of_knots
,
const
double
delta_s
,
const
std
::
array
<
double
,
3
>&
x_init
,
const
std
::
array
<
double
,
3
>&
x_end
);
virtual
void
SetInitCondition
(
const
std
::
array
<
double
,
3
>&
x_init
)
{
x_init_
=
x_init
;
}
virtual
void
SetEndCondition
(
const
std
::
array
<
double
,
3
>&
x_end
)
{
x_end_
=
x_end
;
}
void
SetZeroOrderReference
(
std
::
vector
<
double
>
x_ref
);
void
SetFirstOrderPenalty
(
std
::
vector
<
double
>
penalty_dx
);
void
SetZeroOrderBounds
(
std
::
vector
<
std
::
pair
<
double
,
double
>>
x_bounds
);
void
SetZeroOrderBounds
(
std
::
vector
<
std
::
pair
<
double
,
double
>>
x_bounds
);
void
SetZeroOrderBounds
(
const
double
x_lower_bound
,
void
SetZeroOrderBounds
(
const
double
x_lower_bound
,
...
@@ -118,10 +89,6 @@ class PiecewiseJerkProblem {
...
@@ -118,10 +89,6 @@ class PiecewiseJerkProblem {
weight_dddx_
=
weight_dddx
;
weight_dddx_
=
weight_dddx
;
}
}
void
set_weight_x_reference
(
const
double
weight_x_reference
)
{
weight_x_reference_
=
weight_x_reference
;
}
virtual
bool
Optimize
(
const
int
max_iter
=
4000
);
virtual
bool
Optimize
(
const
int
max_iter
=
4000
);
const
std
::
vector
<
double
>&
x
()
const
{
return
x_
;
}
const
std
::
vector
<
double
>&
x
()
const
{
return
x_
;
}
...
@@ -134,9 +101,9 @@ class PiecewiseJerkProblem {
...
@@ -134,9 +101,9 @@ class PiecewiseJerkProblem {
// naming convention follows osqp solver.
// naming convention follows osqp solver.
virtual
void
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
virtual
void
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
);
std
::
vector
<
c_int
>*
P_indptr
)
=
0
;
virtual
void
CalculateOffset
(
std
::
vector
<
c_float
>*
q
);
virtual
void
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
=
0
;
virtual
void
CalculateAffineConstraint
(
std
::
vector
<
c_float
>*
A_data
,
virtual
void
CalculateAffineConstraint
(
std
::
vector
<
c_float
>*
A_data
,
std
::
vector
<
c_int
>*
A_indices
,
std
::
vector
<
c_int
>*
A_indices
,
...
@@ -163,9 +130,6 @@ class PiecewiseJerkProblem {
...
@@ -163,9 +130,6 @@ class PiecewiseJerkProblem {
std
::
vector
<
double
>
ddx_
;
std
::
vector
<
double
>
ddx_
;
std
::
array
<
double
,
3
>
x_init_
;
std
::
array
<
double
,
3
>
x_init_
;
std
::
array
<
double
,
3
>
x_end_
;
std
::
vector
<
double
>
x_ref_
;
std
::
vector
<
double
>
penalty_dx_
;
std
::
vector
<
std
::
pair
<
double
,
double
>>
x_bounds_
;
std
::
vector
<
std
::
pair
<
double
,
double
>>
x_bounds_
;
std
::
vector
<
std
::
pair
<
double
,
double
>>
dx_bounds_
;
std
::
vector
<
std
::
pair
<
double
,
double
>>
dx_bounds_
;
...
@@ -180,8 +144,6 @@ class PiecewiseJerkProblem {
...
@@ -180,8 +144,6 @@ class PiecewiseJerkProblem {
double
weight_dddx_
=
0.0
;
double
weight_dddx_
=
0.0
;
double
weight_x_reference_
=
0.0
;
double
delta_s_
=
1.0
;
double
delta_s_
=
1.0
;
};
};
...
...
modules/planning/math/piecewise_jerk/piecewise_jerk_problem_test.cc
已删除
100644 → 0
浏览文件 @
94c5fb09
/******************************************************************************
* 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
**/
#include <math.h>
#include <chrono>
#include <memory>
#include "cyber/common/log.h"
#include "gtest/gtest.h"
#include "modules/planning/common/planning_gflags.h"
#define private public
#define protected public
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h"
namespace
apollo
{
namespace
planning
{
TEST
(
PiecewiseJerkProblemTest
,
basic_test
)
{
FLAGS_enable_osqp_debug
=
true
;
std
::
array
<
double
,
3
>
x_init
=
{
1.5
,
0.01
,
0.001
};
double
delta_s
=
0.5
;
size_t
n
=
400
;
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>
x_bounds
;
for
(
size_t
i
=
0
;
i
<
n
;
++
i
)
{
if
(
i
!=
2
)
{
x_bounds
.
emplace_back
(
std
::
make_tuple
(
static_cast
<
double
>
(
i
),
-
1.81
,
1.95
));
}
else
{
x_bounds
.
emplace_back
(
std
::
make_tuple
(
static_cast
<
double
>
(
i
),
0.81
,
1.95
));
}
}
std
::
array
<
double
,
5
>
w
=
{
1.0
,
2.0
,
3.0
,
4.0
,
1.45
};
double
max_x_third_order_derivative
=
1.25
;
std
::
unique_ptr
<
PiecewiseJerkProblem
>
fem_qp
(
new
PiecewiseJerkProblem
());
fem_qp
->
InitProblem
(
n
,
delta_s
,
w
,
x_init
);
fem_qp
->
SetVariableBounds
(
x_bounds
);
fem_qp
->
SetFirstOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
FLAGS_lateral_derivative_bound_default
);
fem_qp
->
SetSecondOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
FLAGS_lateral_derivative_bound_default
);
fem_qp
->
SetThirdOrderBound
(
max_x_third_order_derivative
);
auto
start_time
=
std
::
chrono
::
system_clock
::
now
();
EXPECT_TRUE
(
fem_qp
->
Optimize
());
auto
end_time
=
std
::
chrono
::
system_clock
::
now
();
std
::
chrono
::
duration
<
double
>
diff
=
end_time
-
start_time
;
AINFO
<<
"qp_optimizer used time: "
<<
diff
.
count
()
*
1000
<<
" ms."
;
const
std
::
vector
<
double
>
x
=
fem_qp
->
x
();
AINFO
<<
"x.size() = "
<<
x
.
size
();
for
(
size_t
i
=
0
;
i
<
x
.
size
();
++
i
)
{
EXPECT_LE
(
x
[
i
],
fem_qp
->
x_bounds_
[
i
].
second
);
EXPECT_GE
(
x
[
i
],
fem_qp
->
x_bounds_
[
i
].
first
);
}
}
TEST
(
PiecewiseJerkProblemTest
,
add_bounds_test
)
{
FLAGS_enable_osqp_debug
=
false
;
std
::
array
<
double
,
3
>
x_init
=
{
1.5
,
0.01
,
0.001
};
double
delta_s
=
0.5
;
size_t
n
=
400
;
std
::
array
<
double
,
5
>
w
=
{
1.0
,
2.0
,
3.0
,
4.0
,
1.45
};
std
::
unique_ptr
<
PiecewiseJerkProblem
>
fem_qp
(
new
PiecewiseJerkProblem
());
fem_qp
->
InitProblem
(
n
,
delta_s
,
w
,
x_init
);
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>
x_bounds
;
for
(
size_t
i
=
10
;
i
<
20
;
++
i
)
{
x_bounds
.
emplace_back
(
std
::
make_tuple
(
static_cast
<
double
>
(
i
),
-
1.81
,
1.95
));
}
fem_qp
->
SetVariableBounds
(
x_bounds
);
const
auto
&
x
=
fem_qp
->
x_bounds_
;
CHECK_EQ
(
n
,
x
.
size
());
for
(
size_t
i
=
20
;
i
<
40
;
i
+=
2
)
{
EXPECT_DOUBLE_EQ
(
std
::
get
<
0
>
(
x
[
i
]),
-
1.81
);
EXPECT_DOUBLE_EQ
(
std
::
get
<
1
>
(
x
[
i
]),
1.95
);
}
}
TEST
(
PiecewiseJerkProblemTest
,
derivative_constraint_test
)
{
FLAGS_enable_osqp_debug
=
true
;
std
::
array
<
double
,
3
>
x_init
=
{
4.5
,
0.00
,
0.0
};
double
delta_s
=
0.5
;
size_t
n
=
200
;
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>
x_bounds
;
for
(
size_t
i
=
0
;
i
<
n
;
++
i
)
{
x_bounds
.
emplace_back
(
std
::
make_tuple
(
static_cast
<
double
>
(
i
),
-
6.0
,
6.0
));
}
std
::
array
<
double
,
5
>
w
=
{
1.0
,
100.0
,
1000.0
,
1000.0
,
0.0
};
double
max_x_third_order_derivative
=
2.0
;
std
::
unique_ptr
<
PiecewiseJerkProblem
>
fem_qp
(
new
PiecewiseJerkProblem
());
fem_qp
->
InitProblem
(
n
,
delta_s
,
w
,
x_init
);
fem_qp
->
SetVariableBounds
(
x_bounds
);
fem_qp
->
SetFirstOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
FLAGS_lateral_derivative_bound_default
);
fem_qp
->
SetSecondOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
FLAGS_lateral_derivative_bound_default
);
fem_qp
->
SetThirdOrderBound
(
max_x_third_order_derivative
);
const
double
dx_max
=
std
::
sqrt
(
0.5
)
/
15.0
;
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>
dx_bounds
;
for
(
size_t
i
=
0
;
i
<
20
;
++
i
)
{
dx_bounds
.
emplace_back
(
std
::
make_tuple
(
static_cast
<
double
>
(
i
),
-
dx_max
,
dx_max
));
}
fem_qp
->
SetVariableDerivativeBounds
(
dx_bounds
);
auto
start_time
=
std
::
chrono
::
system_clock
::
now
();
EXPECT_TRUE
(
fem_qp
->
Optimize
());
auto
end_time
=
std
::
chrono
::
system_clock
::
now
();
std
::
chrono
::
duration
<
double
>
diff
=
end_time
-
start_time
;
AINFO
<<
"qp_optimizer used time: "
<<
diff
.
count
()
*
1000
<<
" ms."
;
const
std
::
vector
<
double
>&
x
=
fem_qp
->
x
();
AINFO
<<
"x.size() = "
<<
x
.
size
();
for
(
size_t
i
=
0
;
i
<
x
.
size
();
++
i
)
{
EXPECT_LT
(
x
[
i
],
fem_qp
->
x_bounds_
[
i
].
second
);
EXPECT_GT
(
x
[
i
],
fem_qp
->
x_bounds_
[
i
].
first
);
}
const
std
::
vector
<
double
>&
dx
=
fem_qp
->
x_derivative
();
AINFO
<<
"dx.size() = "
<<
dx
.
size
();
for
(
size_t
i
=
0
;
i
<
dx
.
size
();
++
i
)
{
EXPECT_LT
(
dx
[
i
]
-
1e-12
,
fem_qp
->
dx_bounds_
[
i
].
second
);
EXPECT_GT
(
dx
[
i
]
+
1e-12
,
fem_qp
->
dx_bounds_
[
i
].
first
);
}
}
}
// namespace planning
}
// namespace apollo
modules/planning/math/piecewise_jerk/p
ath_time_qp
_problem.cc
→
modules/planning/math/piecewise_jerk/p
iecewise_jerk_speed
_problem.cc
浏览文件 @
83bed011
...
@@ -14,63 +14,80 @@
...
@@ -14,63 +14,80 @@
* limitations under the License.
* limitations under the License.
*****************************************************************************/
*****************************************************************************/
#include "modules/planning/math/piecewise_jerk/path_time_qp_problem.h"
#include <algorithm>
#include <algorithm>
#include "cyber/common/log.h"
#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "piecewise_jerk_speed_problem.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
void
PathTimeQpProblem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
PiecewiseJerkSpeedProblem
::
PiecewiseJerkSpeedProblem
(
const
size_t
num_of_knots
,
const
double
delta_s
,
const
std
::
array
<
double
,
3
>&
x_init
,
const
std
::
array
<
double
,
3
>&
x_end
)
:
PiecewiseJerkProblem
(
num_of_knots
,
delta_s
,
x_init
)
{
end_state_target_
=
x_end
;
}
void
PiecewiseJerkSpeedProblem
::
SetZeroOrderReference
(
std
::
vector
<
double
>
x_ref
)
{
CHECK_EQ
(
x_ref
.
size
(),
num_of_knots_
);
x_reference_
=
std
::
move
(
x_ref
);
}
void
PiecewiseJerkSpeedProblem
::
SetFirstOrderPenalty
(
std
::
vector
<
double
>
penalty_dx
)
{
CHECK_EQ
(
penalty_dx
.
size
(),
num_of_knots_
);
penalty_dx_
=
std
::
move
(
penalty_dx
);
}
void
PiecewiseJerkSpeedProblem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
)
{
std
::
vector
<
c_int
>*
P_indptr
)
{
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
n
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
kNumParam
=
3
*
N
;
const
int
kNumParam
=
3
*
n
;
const
int
kNumValue
=
4
*
N
-
1
;
const
int
kNumValue
=
4
*
n
-
1
;
std
::
vector
<
std
::
vector
<
std
::
pair
<
c_int
,
c_float
>>>
columns
;
std
::
vector
<
std
::
vector
<
std
::
pair
<
c_int
,
c_float
>>>
columns
;
columns
.
resize
(
kNumParam
);
columns
.
resize
(
kNumParam
);
int
value_index
=
0
;
int
value_index
=
0
;
// x(i)^2 * w_ref
// x(i)^2 * w_ref
for
(
int
i
=
0
;
i
<
N
-
1
;
++
i
)
{
for
(
int
i
=
0
;
i
<
n
-
1
;
++
i
)
{
columns
[
i
].
emplace_back
(
i
,
weight_
.
x_ref_w
);
columns
[
i
].
emplace_back
(
i
,
weight_
x_reference_
);
++
value_index
;
++
value_index
;
}
}
// x(n-1)^2 * (w_ref + w_x)
// x(n-1)^2 * (w_ref + w_x)
columns
[
N
-
1
].
emplace_back
(
N
-
1
,
weight_
.
x_ref_w
+
weight_
.
x_w
);
columns
[
n
-
1
].
emplace_back
(
n
-
1
,
weight_x_reference_
+
weight_end_x_
);
++
value_index
;
++
value_index
;
// x(i)'^2 * w_dx
// x(i)'^2 * w_dx
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
for
(
int
i
=
0
;
i
<
n
-
1
;
++
i
)
{
columns
[
N
+
i
].
emplace_back
(
columns
[
n
+
i
].
emplace_back
(
N
+
i
,
weight_
.
x_derivative_w
*
(
1.0
+
penalty_dx_
[
i
]));
n
+
i
,
weight_dx_
*
(
1.0
+
penalty_dx_
[
i
]));
++
value_index
;
++
value_index
;
}
}
auto
delta_s_square
=
delta_s_
*
delta_s_
;
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
columns
[
2
*
N
].
emplace_back
(
columns
[
2
*
n
].
emplace_back
(
2
*
N
,
weight_
.
x_second_order_derivative_w
+
2
*
n
,
weight_ddx_
+
weight_dddx_
/
delta_s_square
);
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
++
value_index
;
++
value_index
;
for
(
int
i
=
1
;
i
<
N
-
1
;
++
i
)
{
columns
[
2
*
N
+
i
].
emplace_back
(
for
(
int
i
=
1
;
i
<
n
-
1
;
++
i
)
{
2
*
N
+
i
,
weight_
.
x_second_order_derivative_w
+
columns
[
2
*
n
+
i
].
emplace_back
(
2.0
*
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
2
*
n
+
i
,
weight_ddx_
+
2.0
*
weight_dddx_
/
delta_s_square
);
++
value_index
;
++
value_index
;
}
}
columns
[
3
*
N
-
1
].
emplace_back
(
3
*
N
-
1
,
weight_
.
x_second_order_derivative_w
+
columns
[
3
*
n
-
1
].
emplace_back
(
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
3
*
n
-
1
,
weight_ddx_
+
weight_dddx_
/
delta_s_square
);
++
value_index
;
++
value_index
;
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
for
(
int
i
=
0
;
i
<
N
-
1
;
++
i
)
{
for
(
int
i
=
0
;
i
<
n
-
1
;
++
i
)
{
columns
[
2
*
N
+
i
].
emplace_back
(
columns
[
2
*
n
+
i
].
emplace_back
(
2
*
N
+
i
+
1
,
-
2.0
*
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
2
*
n
+
i
+
1
,
-
2.0
*
weight_dddx_
/
delta_s_square
);
++
value_index
;
++
value_index
;
}
}
...
@@ -88,18 +105,19 @@ void PathTimeQpProblem::CalculateKernel(std::vector<c_float>* P_data,
...
@@ -88,18 +105,19 @@ void PathTimeQpProblem::CalculateKernel(std::vector<c_float>* P_data,
P_indptr
->
push_back
(
ind_p
);
P_indptr
->
push_back
(
ind_p
);
}
}
void
P
athTimeQp
Problem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
void
P
iecewiseJerkSpeed
Problem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
CHECK_NOTNULL
(
q
);
CHECK_NOTNULL
(
q
);
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
n
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
kNumParam
=
3
*
N
;
const
int
kNumParam
=
3
*
n
;
q
->
resize
(
kNumParam
);
q
->
resize
(
kNumParam
);
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
for
(
int
i
=
0
;
i
<
n
;
++
i
)
{
q
->
at
(
i
)
+=
-
2.0
*
weight_
.
x_ref_w
*
x_ref
_
[
i
];
q
->
at
(
i
)
+=
-
2.0
*
weight_
x_reference_
*
x_reference
_
[
i
];
q
->
at
(
N
+
i
)
+=
-
2.0
*
weight_
.
x_derivative_w
*
x_derivative_desire
;
q
->
at
(
n
+
i
)
+=
-
2.0
*
weight_dx_
*
dx_reference_
;
}
}
q
->
at
(
N
-
1
)
+=
-
2.0
*
weight_
.
x_w
*
x_end_
[
0
];
q
->
at
(
2
*
N
-
1
)
+=
-
2.0
*
weight_
.
x_derivative_w
*
x_end_
[
1
];
q
->
at
(
n
-
1
)
+=
-
2.0
*
weight_end_x_
*
end_state_target_
[
0
];
q
->
at
(
3
*
N
-
1
)
+=
-
2.0
*
weight_
.
x_second_order_derivative_w
*
x_end_
[
2
];
q
->
at
(
2
*
n
-
1
)
+=
-
2.0
*
weight_end_dx_
*
end_state_target_
[
1
];
q
->
at
(
3
*
n
-
1
)
+=
-
2.0
*
weight_end_ddx_
*
end_state_target_
[
2
];
}
}
}
// namespace planning
}
// namespace planning
...
...
modules/planning/math/piecewise_jerk/p
ath_time_qp
_problem.h
→
modules/planning/math/piecewise_jerk/p
iecewise_jerk_speed
_problem.h
浏览文件 @
83bed011
...
@@ -46,14 +46,23 @@ namespace planning {
...
@@ -46,14 +46,23 @@ namespace planning {
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
*/
*/
class
P
athTimeQp
Problem
:
public
PiecewiseJerkProblem
{
class
P
iecewiseJerkSpeed
Problem
:
public
PiecewiseJerkProblem
{
public:
public:
PathTimeQpProblem
()
=
default
;
PiecewiseJerkSpeedProblem
(
const
size_t
num_of_knots
,
const
double
delta_s
,
const
std
::
array
<
double
,
3
>&
x_init
,
const
std
::
array
<
double
,
3
>&
x_end
);
virtual
~
P
athTimeQp
Problem
()
=
default
;
virtual
~
P
iecewiseJerkSpeed
Problem
()
=
default
;
void
SetDesireDerivative
(
const
double
dx_desire
=
0.0
)
{
void
SetZeroOrderReference
(
std
::
vector
<
double
>
x_ref
);
x_derivative_desire
=
dx_desire
;
void
SetFirstOrderReference
(
const
double
dx_desire
=
0.0
)
{
dx_reference_
=
dx_desire
;
}
void
SetFirstOrderPenalty
(
std
::
vector
<
double
>
penalty_dx
);
void
set_weight_x_reference
(
const
double
weight_x_reference
)
{
weight_x_reference_
=
weight_x_reference
;
}
}
protected:
protected:
...
@@ -64,7 +73,21 @@ class PathTimeQpProblem : public PiecewiseJerkProblem {
...
@@ -64,7 +73,21 @@ class PathTimeQpProblem : public PiecewiseJerkProblem {
void
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
override
;
void
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
override
;
double
x_derivative_desire
=
0.0
;
double
dx_reference_
=
0.0
;
double
weight_x_reference_
=
0.0
;
std
::
vector
<
double
>
penalty_dx_
;
std
::
vector
<
double
>
x_reference_
;
std
::
array
<
double
,
3
>
end_state_target_
;
double
weight_end_x_
=
0.0
;
double
weight_end_dx_
=
0.0
;
double
weight_end_ddx_
=
0.0
;
};
};
}
// namespace planning
}
// namespace planning
...
...
modules/planning/open_space/coarse_trajectory_generator/BUILD
浏览文件 @
83bed011
...
@@ -76,7 +76,7 @@ cc_library(
...
@@ -76,7 +76,7 @@ cc_library(
"//modules/common/configs:vehicle_config_helper"
,
"//modules/common/configs:vehicle_config_helper"
,
"//modules/planning/common:obstacle"
,
"//modules/planning/common:obstacle"
,
"//modules/planning/common:planning_gflags"
,
"//modules/planning/common:planning_gflags"
,
"//modules/planning/math/piecewise_jerk:p
ath_time_qp
_problem"
,
"//modules/planning/math/piecewise_jerk:p
iecewise_jerk_speed
_problem"
,
"//modules/planning/proto:planner_open_space_config_proto"
,
"//modules/planning/proto:planner_open_space_config_proto"
,
],
],
)
)
...
...
modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc
浏览文件 @
83bed011
...
@@ -20,7 +20,7 @@
...
@@ -20,7 +20,7 @@
#include "modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.h"
#include "modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.h"
#include "modules/planning/math/piecewise_jerk/p
ath_time_qp
_problem.h"
#include "modules/planning/math/piecewise_jerk/p
iecewise_jerk_speed
_problem.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
...
@@ -406,28 +406,35 @@ bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
...
@@ -406,28 +406,35 @@ bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
ADEBUG
<<
"end_s: "
<<
result
->
accumulated_s
[
x_size
-
1
]
<<
" "
<<
0.0
<<
" "
ADEBUG
<<
"end_s: "
<<
result
->
accumulated_s
[
x_size
-
1
]
<<
" "
<<
0.0
<<
" "
<<
0.0
;
<<
0.0
;
std
::
unique_ptr
<
PathTimeQpProblem
>
path_time_qp
(
new
PathTimeQpProblem
());
path_time_qp
->
InitProblem
(
x_size
,
delta_t_
,
w
,
init_s
,
end_s
);
path_time_qp
->
SetZeroOrderBounds
(
const
size_t
num_of_knots
=
x_size
-
1
;
PiecewiseJerkSpeedProblem
path_time_qp
(
num_of_knots
,
delta_t_
,
init_s
,
end_s
);
path_time_qp
.
set_weight_x
(
w
[
0
]);
path_time_qp
.
set_weight_dx
(
w
[
1
]);
path_time_qp
.
set_weight_ddx
(
w
[
2
]);
path_time_qp
.
set_weight_dddx
(
w
[
3
]);
path_time_qp
.
set_weight_x_reference
(
w
[
4
]);
path_time_qp
.
SetZeroOrderBounds
(
*
(
std
::
min_element
(
std
::
begin
(
result
->
accumulated_s
),
*
(
std
::
min_element
(
std
::
begin
(
result
->
accumulated_s
),
std
::
end
(
result
->
accumulated_s
)))
-
std
::
end
(
result
->
accumulated_s
)))
-
10
,
10
,
*
(
std
::
max_element
(
std
::
begin
(
result
->
accumulated_s
),
*
(
std
::
max_element
(
std
::
begin
(
result
->
accumulated_s
),
std
::
end
(
result
->
accumulated_s
)))
+
std
::
end
(
result
->
accumulated_s
)))
+
10
);
10
);
path_time_qp
->
SetFirstOrderBounds
(
path_time_qp
.
SetFirstOrderBounds
(
*
(
std
::
min_element
(
std
::
begin
(
result
->
v
),
std
::
end
(
result
->
v
))
-
10
),
*
(
std
::
min_element
(
std
::
begin
(
result
->
v
),
std
::
end
(
result
->
v
))
-
10
),
*
(
std
::
max_element
(
std
::
begin
(
result
->
v
),
std
::
end
(
result
->
v
)))
+
10
);
*
(
std
::
max_element
(
std
::
begin
(
result
->
v
),
std
::
end
(
result
->
v
)))
+
10
);
// TODO(QiL): load this from configs
// TODO(QiL): load this from configs
path_time_qp
->
SetSecondOrderBounds
(
-
4.4
,
10.0
);
path_time_qp
.
SetSecondOrderBounds
(
-
4.4
,
10.0
);
path_time_qp
->
SetThirdOrderBound
(
FLAGS_longitudinal_jerk_bound
);
path_time_qp
.
SetThirdOrderBound
(
FLAGS_longitudinal_jerk_bound
);
path_time_qp
->
SetDesireDerivativ
e
(
0.0
);
path_time_qp
.
SetFirstOrderReferenc
e
(
0.0
);
path_time_qp
->
SetZeroOrderReference
(
result
->
accumulated_s
);
path_time_qp
.
SetZeroOrderReference
(
result
->
accumulated_s
);
// So
vl
e the problem
// So
lv
e the problem
if
(
!
path_time_qp
->
Optimize
())
{
if
(
!
path_time_qp
.
Optimize
())
{
std
::
string
msg
(
"Piecewise jerk speed optimizer failed!"
);
std
::
string
msg
(
"Piecewise jerk speed optimizer failed!"
);
AERROR
<<
msg
;
AERROR
<<
msg
;
return
false
;
return
false
;
...
@@ -436,9 +443,9 @@ bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
...
@@ -436,9 +443,9 @@ bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
// Extract output
// Extract output
result
->
v
.
clear
();
result
->
v
.
clear
();
result
->
accumulated_s
.
clear
();
result
->
accumulated_s
.
clear
();
result
->
accumulated_s
=
path_time_qp
->
x
();
result
->
accumulated_s
=
path_time_qp
.
x
();
result
->
v
=
path_time_qp
->
x_derivative
();
result
->
v
=
path_time_qp
.
x_derivative
();
result
->
a
=
path_time_qp
->
x_second_order_derivative
();
result
->
a
=
path_time_qp
.
x_second_order_derivative
();
result
->
a
.
pop_back
();
result
->
a
.
pop_back
();
// load steering from phi
// load steering from phi
...
...
modules/planning/tasks/deciders/BUILD
浏览文件 @
83bed011
...
@@ -23,7 +23,7 @@ cc_library(
...
@@ -23,7 +23,7 @@ cc_library(
"//modules/planning/common:st_graph_data"
,
"//modules/planning/common:st_graph_data"
,
"//modules/planning/common/util:common_lib"
,
"//modules/planning/common/util:common_lib"
,
"//modules/planning/common/util:util_lib"
,
"//modules/planning/common/util:util_lib"
,
"//modules/planning/math/piecewise_jerk:
fem_1d_qp
_problem"
,
"//modules/planning/math/piecewise_jerk:
piecewise_jerk_path
_problem"
,
"//modules/planning/scenarios/util:scenario_util_lib"
,
"//modules/planning/scenarios/util:scenario_util_lib"
,
"//modules/planning/tasks/deciders/speed_bounds_decider"
,
"//modules/planning/tasks/deciders/speed_bounds_decider"
,
"//modules/planning/tasks/deciders/speed_bounds_decider:st_boundary_mapper"
,
"//modules/planning/tasks/deciders/speed_bounds_decider:st_boundary_mapper"
,
...
...
modules/planning/tasks/optimizers/piecewise_jerk_path/BUILD
浏览文件 @
83bed011
...
@@ -32,7 +32,7 @@ cc_library(
...
@@ -32,7 +32,7 @@ cc_library(
"//modules/planning/math:polynomial_xd"
,
"//modules/planning/math:polynomial_xd"
,
"//modules/planning/math/curve1d:polynomial_curve1d"
,
"//modules/planning/math/curve1d:polynomial_curve1d"
,
"//modules/planning/math/curve1d:quintic_polynomial_curve1d"
,
"//modules/planning/math/curve1d:quintic_polynomial_curve1d"
,
"//modules/planning/math/piecewise_jerk:
fem_1d_qp
_problem"
,
"//modules/planning/math/piecewise_jerk:
piecewise_jerk_path
_problem"
,
"//modules/planning/proto:planning_proto"
,
"//modules/planning/proto:planning_proto"
,
"//modules/planning/reference_line"
,
"//modules/planning/reference_line"
,
"//modules/planning/tasks/optimizers:path_optimizer"
,
"//modules/planning/tasks/optimizers:path_optimizer"
,
...
...
modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc
浏览文件 @
83bed011
...
@@ -25,10 +25,10 @@
...
@@ -25,10 +25,10 @@
#include <utility>
#include <utility>
#include <vector>
#include <vector>
#include "../../../math/piecewise_jerk/piecewise_jerk_path_problem.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.h"
#include "modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.h"
#include "modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
...
@@ -128,14 +128,21 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
...
@@ -128,14 +128,21 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
const
std
::
vector
<
std
::
pair
<
double
,
double
>>&
lat_boundaries
,
const
std
::
vector
<
std
::
pair
<
double
,
double
>>&
lat_boundaries
,
const
std
::
array
<
double
,
5
>&
w
,
std
::
vector
<
double
>*
x
,
const
std
::
array
<
double
,
5
>&
w
,
std
::
vector
<
double
>*
x
,
std
::
vector
<
double
>*
dx
,
std
::
vector
<
double
>*
ddx
,
const
int
max_iter
)
{
std
::
vector
<
double
>*
dx
,
std
::
vector
<
double
>*
ddx
,
const
int
max_iter
)
{
std
::
unique_ptr
<
Fem1dQpProblem
>
fem_1d_qp
(
new
Fem1dQpProblem
());
PiecewiseJerkPathProblem
piecewise_jerk_problem
(
fem_1d_qp
->
InitProblem
(
lat_boundaries
.
size
(),
delta_s
,
w
,
init_state
.
second
,
lat_boundaries
.
size
(),
delta_s
,
init_state
.
second
);
end_state
);
fem_1d_qp
->
SetThirdOrderBound
(
FLAGS_lateral_jerk_bound
);
piecewise_jerk_problem
.
set_weight_x
(
w
[
0
]);
piecewise_jerk_problem
.
set_weight_dx
(
w
[
1
]);
piecewise_jerk_problem
.
set_weight_ddx
(
w
[
2
]);
piecewise_jerk_problem
.
set_weight_dddx
(
w
[
3
]);
// TODO(all): fillin the correct weight
piecewise_jerk_problem
.
set_weight_x_reference
(
w
[
4
]);
piecewise_jerk_problem
.
SetThirdOrderBound
(
FLAGS_lateral_jerk_bound
);
auto
start_time
=
std
::
chrono
::
system_clock
::
now
();
auto
start_time
=
std
::
chrono
::
system_clock
::
now
();
fem_1d_qp
->
SetZeroOrderBounds
(
lat_boundaries
);
piecewise_jerk_problem
.
SetZeroOrderBounds
(
lat_boundaries
);
double
first_order_bounds
=
AdjustLateralDerivativeBounds
(
double
first_order_bounds
=
AdjustLateralDerivativeBounds
(
init_state
.
first
[
1
],
init_state
.
second
[
1
],
init_state
.
second
[
2
],
init_state
.
first
[
1
],
init_state
.
second
[
1
],
init_state
.
second
[
2
],
...
@@ -145,12 +152,12 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
...
@@ -145,12 +152,12 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
<<
first_order_bounds
;
<<
first_order_bounds
;
// TODO(all): temp. disable AdjustLateralDerivativeBounds, enable later
// TODO(all): temp. disable AdjustLateralDerivativeBounds, enable later
// fem_1d_qp->SetFirstOrderBounds(-first_order_bounds, first_order_bounds);
// fem_1d_qp->SetFirstOrderBounds(-first_order_bounds, first_order_bounds);
fem_1d_qp
->
SetFirstOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
piecewise_jerk_problem
.
SetFirstOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
FLAGS_lateral_derivative_bound_default
);
FLAGS_lateral_derivative_bound_default
);
fem_1d_qp
->
SetSecondOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
piecewise_jerk_problem
.
SetSecondOrderBounds
(
-
FLAGS_lateral_derivative_bound_default
,
FLAGS_lateral_derivative_bound_default
);
FLAGS_lateral_derivative_bound_default
);
bool
success
=
fem_1d_qp
->
Optimize
(
max_iter
);
bool
success
=
piecewise_jerk_problem
.
Optimize
(
max_iter
);
auto
end_time
=
std
::
chrono
::
system_clock
::
now
();
auto
end_time
=
std
::
chrono
::
system_clock
::
now
();
std
::
chrono
::
duration
<
double
>
diff
=
end_time
-
start_time
;
std
::
chrono
::
duration
<
double
>
diff
=
end_time
-
start_time
;
...
@@ -161,9 +168,9 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
...
@@ -161,9 +168,9 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
return
false
;
return
false
;
}
}
*
x
=
fem_1d_qp
->
x
();
*
x
=
piecewise_jerk_problem
.
x
();
*
dx
=
fem_1d_qp
->
x_derivative
();
*
dx
=
piecewise_jerk_problem
.
x_derivative
();
*
ddx
=
fem_1d_qp
->
x_second_order_derivative
();
*
ddx
=
piecewise_jerk_problem
.
x_second_order_derivative
();
return
true
;
return
true
;
}
}
...
...
modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD
浏览文件 @
83bed011
...
@@ -15,7 +15,7 @@ cc_library(
...
@@ -15,7 +15,7 @@ cc_library(
"//modules/common/proto:error_code_proto"
,
"//modules/common/proto:error_code_proto"
,
"//modules/common/proto:pnc_point_proto"
,
"//modules/common/proto:pnc_point_proto"
,
"//modules/planning/common:st_graph_data"
,
"//modules/planning/common:st_graph_data"
,
"//modules/planning/math/piecewise_jerk:p
ath_time_qp
_problem"
,
"//modules/planning/math/piecewise_jerk:p
iecewise_jerk_speed
_problem"
,
"//modules/planning/tasks/optimizers:speed_optimizer"
,
"//modules/planning/tasks/optimizers:speed_optimizer"
,
],
],
)
)
...
...
modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc
浏览文件 @
83bed011
...
@@ -31,7 +31,7 @@
...
@@ -31,7 +31,7 @@
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/st_graph_data.h"
#include "modules/planning/common/st_graph_data.h"
#include "modules/planning/math/piecewise_jerk/p
ath_time_qp
_problem.h"
#include "modules/planning/math/piecewise_jerk/p
iecewise_jerk_speed
_problem.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
...
@@ -76,31 +76,31 @@ Status PiecewiseJerkSpeedOptimizer::Process(
...
@@ -76,31 +76,31 @@ Status PiecewiseJerkSpeedOptimizer::Process(
std
::
array
<
double
,
3
>
init_s
=
{
0.0
,
st_graph_data
.
init_point
().
v
(),
std
::
array
<
double
,
3
>
init_s
=
{
0.0
,
st_graph_data
.
init_point
().
v
(),
st_graph_data
.
init_point
().
a
()};
st_graph_data
.
init_point
().
a
()};
double
delta_t
=
0.1
;
double
delta_t
=
0.1
;
const
auto
&
piecewise_jerk_speed_config
=
config_
.
piecewise_jerk_speed_config
();
std
::
array
<
double
,
5
>
w
=
{
piecewise_jerk_speed_config
.
s_weight
(),
piecewise_jerk_speed_config
.
velocity_weight
(),
piecewise_jerk_speed_config
.
acc_weight
(),
piecewise_jerk_speed_config
.
jerk_weight
(),
piecewise_jerk_speed_config
.
ref_weight
()};
double
total_length
=
st_graph_data
.
path_length_by_conf
();
double
total_length
=
st_graph_data
.
path_length_by_conf
();
double
total_time
=
st_graph_data
.
total_time_by_conf
();
double
total_time
=
st_graph_data
.
total_time_by_conf
();
int
num_of_knots
=
static_cast
<
int
>
(
total_time
/
delta_t
)
+
1
;
int
num_of_knots
=
static_cast
<
int
>
(
total_time
/
delta_t
)
+
1
;
// Start a PathTimeQpProblem
std
::
unique_ptr
<
PathTimeQpProblem
>
path_time_qp
(
new
PathTimeQpProblem
());
path_time_qp
->
InitProblem
(
num_of_knots
,
delta_t
,
w
,
init_s
);
path_time_qp
->
SetZeroOrderBounds
(
0.0
,
total_length
);
PiecewiseJerkSpeedProblem
piecewise_jerk_problem
(
num_of_knots
,
delta_t
,
init_s
,
{
0.0
,
0.0
,
0.0
});
path_time_qp
->
SetFirstOrderBounds
(
0.0
,
const
auto
&
piecewise_jerk_speed_config
=
config_
.
piecewise_jerk_speed_config
();
piecewise_jerk_problem
.
set_weight_x
(
piecewise_jerk_speed_config
.
s_weight
());
piecewise_jerk_problem
.
set_weight_dx
(
piecewise_jerk_speed_config
.
velocity_weight
());
piecewise_jerk_problem
.
set_weight_ddx
(
piecewise_jerk_speed_config
.
acc_weight
());
piecewise_jerk_problem
.
set_weight_dddx
(
piecewise_jerk_speed_config
.
jerk_weight
());
piecewise_jerk_problem
.
set_weight_x_reference
(
piecewise_jerk_speed_config
.
ref_weight
());
piecewise_jerk_problem
.
SetZeroOrderBounds
(
0.0
,
total_length
);
piecewise_jerk_problem
.
SetFirstOrderBounds
(
0.0
,
std
::
fmax
(
FLAGS_planning_upper_speed_limit
,
std
::
fmax
(
FLAGS_planning_upper_speed_limit
,
st_graph_data
.
init_point
().
v
()));
st_graph_data
.
init_point
().
v
()));
p
ath_time_qp
->
SetSecondOrderBounds
(
veh_param
.
max_deceleration
(),
p
iecewise_jerk_problem
.
SetSecondOrderBounds
(
veh_param
.
max_deceleration
(),
veh_param
.
max_acceleration
());
veh_param
.
max_acceleration
());
p
ath_time_qp
->
SetThirdOrderBound
(
FLAGS_longitudinal_jerk_bound
);
p
iecewise_jerk_problem
.
SetThirdOrderBound
(
FLAGS_longitudinal_jerk_bound
);
// TODO(Hongyi): delete this when ready to use vehicle_params
// TODO(Hongyi): delete this when ready to use vehicle_params
p
ath_time_qp
->
SetSecondOrderBounds
(
-
4.4
,
2.0
);
p
iecewise_jerk_problem
.
SetSecondOrderBounds
(
-
4.4
,
2.0
);
p
ath_time_qp
->
SetDesireDerivativ
e
(
FLAGS_default_cruise_speed
);
p
iecewise_jerk_problem
.
SetFirstOrderReferenc
e
(
FLAGS_default_cruise_speed
);
// Update STBoundary
// Update STBoundary
std
::
vector
<
std
::
pair
<
double
,
double
>>
s_bounds
;
std
::
vector
<
std
::
pair
<
double
,
double
>>
s_bounds
;
...
@@ -138,7 +138,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(
...
@@ -138,7 +138,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(
}
}
s_bounds
.
emplace_back
(
s_lower_bound
,
s_upper_bound
);
s_bounds
.
emplace_back
(
s_lower_bound
,
s_upper_bound
);
}
}
p
ath_time_qp
->
SetZeroOrderBounds
(
std
::
move
(
s_bounds
));
p
iecewise_jerk_problem
.
SetZeroOrderBounds
(
std
::
move
(
s_bounds
));
// Update SpeedBoundary and ref_s
// Update SpeedBoundary and ref_s
std
::
vector
<
double
>
x_ref
;
std
::
vector
<
double
>
x_ref
;
...
@@ -163,12 +163,12 @@ Status PiecewiseJerkSpeedOptimizer::Process(
...
@@ -163,12 +163,12 @@ Status PiecewiseJerkSpeedOptimizer::Process(
v_upper_bound
=
speed_limit
.
GetSpeedLimitByS
(
path_s
);
v_upper_bound
=
speed_limit
.
GetSpeedLimitByS
(
path_s
);
s_dot_bounds
.
emplace_back
(
v_lower_bound
,
std
::
fmax
(
v_upper_bound
,
0.0
));
s_dot_bounds
.
emplace_back
(
v_lower_bound
,
std
::
fmax
(
v_upper_bound
,
0.0
));
}
}
p
ath_time_qp
->
SetZeroOrderReference
(
x_ref
);
p
iecewise_jerk_problem
.
SetZeroOrderReference
(
x_ref
);
p
ath_time_qp
->
SetFirstOrderPenalty
(
penalty_dx
);
p
iecewise_jerk_problem
.
SetFirstOrderPenalty
(
penalty_dx
);
p
ath_time_qp
->
SetFirstOrderBounds
(
s_dot_bounds
);
p
iecewise_jerk_problem
.
SetFirstOrderBounds
(
s_dot_bounds
);
// So
vl
e the problem
// So
lv
e the problem
if
(
!
p
ath_time_qp
->
Optimize
())
{
if
(
!
p
iecewise_jerk_problem
.
Optimize
())
{
std
::
string
msg
(
"Piecewise jerk speed optimizer failed!"
);
std
::
string
msg
(
"Piecewise jerk speed optimizer failed!"
);
AERROR
<<
msg
;
AERROR
<<
msg
;
speed_data
->
clear
();
speed_data
->
clear
();
...
@@ -176,9 +176,9 @@ Status PiecewiseJerkSpeedOptimizer::Process(
...
@@ -176,9 +176,9 @@ Status PiecewiseJerkSpeedOptimizer::Process(
}
}
// Extract output
// Extract output
const
std
::
vector
<
double
>&
s
=
p
ath_time_qp
->
x
();
const
std
::
vector
<
double
>&
s
=
p
iecewise_jerk_problem
.
x
();
const
std
::
vector
<
double
>&
ds
=
p
ath_time_qp
->
x_derivative
();
const
std
::
vector
<
double
>&
ds
=
p
iecewise_jerk_problem
.
x_derivative
();
const
std
::
vector
<
double
>&
dds
=
p
ath_time_qp
->
x_second_order_derivative
();
const
std
::
vector
<
double
>&
dds
=
p
iecewise_jerk_problem
.
x_second_order_derivative
();
for
(
int
i
=
0
;
i
<
num_of_knots
;
++
i
)
{
for
(
int
i
=
0
;
i
<
num_of_knots
;
++
i
)
{
ADEBUG
<<
"For t["
<<
i
*
delta_t
<<
"], s = "
<<
s
[
i
]
<<
", v = "
<<
ds
[
i
]
ADEBUG
<<
"For t["
<<
i
*
delta_t
<<
"], s = "
<<
s
[
i
]
<<
", v = "
<<
ds
[
i
]
<<
", a = "
<<
dds
[
i
];
<<
", a = "
<<
dds
[
i
];
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录