Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
88e0dd6d
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,发现更多精彩内容 >>
提交
88e0dd6d
编写于
4月 05, 2019
作者:
H
Hongyi
提交者:
Yajia Zhang
4月 05, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: refactor piecewise_jerk math functions
上级
c9a7fe98
变更
6
显示空白变更内容
内联
并排
Showing
6 changed file
with
223 addition
and
63 deletion
+223
-63
modules/planning/math/piecewise_jerk/BUILD
modules/planning/math/piecewise_jerk/BUILD
+18
-6
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.cc
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.cc
+103
-0
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h
+66
-0
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
...es/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
+17
-35
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h
...les/planning/math/piecewise_jerk/piecewise_jerk_problem.h
+9
-12
modules/planning/math/piecewise_jerk/piecewise_jerk_problem_test.cc
...anning/math/piecewise_jerk/piecewise_jerk_problem_test.cc
+10
-10
未找到文件。
modules/planning/math/
finite_element_qp
/BUILD
→
modules/planning/math/
piecewise_jerk
/BUILD
浏览文件 @
88e0dd6d
...
...
@@ -3,12 +3,12 @@ load("//tools:cpplint.bzl", "cpplint")
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"
fem_1d_qp
_problem"
,
name
=
"
piecewise_jerk
_problem"
,
srcs
=
[
"
fem_1d_qp
_problem.cc"
,
"
piecewise_jerk
_problem.cc"
,
],
hdrs
=
[
"
fem_1d_qp
_problem.h"
,
"
piecewise_jerk
_problem.h"
,
],
deps
=
[
"//cyber/common:log"
,
...
...
@@ -18,16 +18,28 @@ cc_library(
)
cc_test
(
name
=
"
fem_1d_qp
_problem_test"
,
name
=
"
piecewise_jerk
_problem_test"
,
size
=
"small"
,
srcs
=
[
"
fem_1d_qp_problem_test
.cc"
,
"
piecewise_jerk_problem
.cc"
,
],
deps
=
[
":
fem_1d_qp
_problem"
,
":
piecewise_jerk
_problem"
,
"//cyber/common:log"
,
"@gtest//:main"
,
],
)
cc_library
(
name
=
"fem_1d_qp_problem"
,
srcs
=
[
"fem_1d_qp_problem.cc"
,
],
hdrs
=
[
"fem_1d_qp_problem.h"
,
],
deps
=
[
":piecewise_jerk_problem"
,
],
)
cpplint
()
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.cc
0 → 100644
浏览文件 @
88e0dd6d
/******************************************************************************
* Copyright 2018 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.
*****************************************************************************/
#include "modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h"
#include <algorithm>
#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"
namespace
apollo
{
namespace
planning
{
void
Fem1dQpProblem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
)
{
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
kNumParam
=
3
*
N
;
const
int
kNumValue
=
kNumParam
+
(
N
-
1
);
std
::
vector
<
std
::
vector
<
std
::
pair
<
c_int
,
c_float
>>>
columns
;
columns
.
resize
(
kNumParam
);
int
value_index
=
0
;
// x(i)^2 * (w_x + w_mid_line)
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
columns
[
i
].
emplace_back
(
i
,
(
weight_
.
x_w
+
weight_
.
x_mid_line_w
));
++
value_index
;
}
// x(i)'^2 * w_dx
for
(
int
i
=
0
;
i
<
N
;
++
i
)
{
columns
[
N
+
i
].
emplace_back
(
N
+
i
,
weight_
.
x_derivative_w
);
++
value_index
;
}
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
columns
[
2
*
N
].
emplace_back
(
2
*
N
,
weight_
.
x_second_order_derivative_w
+
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
++
value_index
;
for
(
int
i
=
1
;
i
<
N
-
1
;
++
i
)
{
columns
[
2
*
N
+
i
].
emplace_back
(
2
*
N
+
i
,
weight_
.
x_second_order_derivative_w
+
2.0
*
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
++
value_index
;
}
columns
[
3
*
N
-
1
].
emplace_back
(
3
*
N
-
1
,
weight_
.
x_second_order_derivative_w
+
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
++
value_index
;
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
for
(
int
i
=
0
;
i
<
N
-
1
;
++
i
)
{
columns
[
2
*
N
+
i
].
emplace_back
(
2
*
N
+
i
+
1
,
-
2.0
*
weight_
.
x_third_order_derivative_w
/
delta_s_sq_
);
++
value_index
;
}
CHECK_EQ
(
value_index
,
kNumValue
);
int
ind_p
=
0
;
for
(
int
i
=
0
;
i
<
kNumParam
;
++
i
)
{
P_indptr
->
push_back
(
ind_p
);
for
(
const
auto
&
row_data_pair
:
columns
[
i
])
{
P_data
->
push_back
(
row_data_pair
.
second
*
2.0
);
P_indices
->
push_back
(
row_data_pair
.
first
);
++
ind_p
;
}
}
P_indptr
->
push_back
(
ind_p
);
}
void
Fem1dQpProblem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
CHECK_NOTNULL
(
q
);
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
kNumParam
=
3
*
N
;
q
->
resize
(
kNumParam
);
for
(
int
i
=
0
;
i
<
kNumParam
;
++
i
)
{
if
(
i
<
N
)
{
q
->
at
(
i
)
=
-
2.0
*
weight_
.
x_mid_line_w
*
(
std
::
get
<
0
>
(
x_bounds_
[
i
])
+
std
::
get
<
1
>
(
x_bounds_
[
i
]));
}
else
{
q
->
at
(
i
)
=
0.0
;
}
}
}
}
// namespace planning
}
// namespace apollo
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h
0 → 100644
浏览文件 @
88e0dd6d
/******************************************************************************
* Copyright 2018 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 <tuple>
#include <utility>
#include <vector>
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h"
namespace
apollo
{
namespace
planning
{
/*
* @brief:
* FEM stands for finite element method.
* This class solve an optimization problem:
* x
* |
* | P(s1, x1) P(s2, x2)
* | P(s0, x0) ... P(s(k-1), x(k-1))
* |P(start)
* |
* |________________________________________________________ s
*
* we suppose s(k+1) - s(k) == s(k) - s(k-1)
*
* Given the x, x', x'' at P(start), The goal is to find x0, x1, ... x(k-1)
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
*/
class
Fem1dQpProblem
:
public
PiecewiseJerkProblem
{
public:
Fem1dQpProblem
()
=
default
;
virtual
~
Fem1dQpProblem
()
=
default
;
protected:
// naming convention follows osqp solver.
void
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
)
override
;
void
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
override
;
};
}
// namespace planning
}
// namespace apollo
modules/planning/math/
finite_element_qp/fem_1d_qp
_problem.cc
→
modules/planning/math/
piecewise_jerk/piecewise_jerk
_problem.cc
浏览文件 @
88e0dd6d
...
...
@@ -14,10 +14,9 @@
* limitations under the License.
*****************************************************************************/
#include "modules/planning/math/
finite_element_qp/fem_1d_qp
_problem.h"
#include "modules/planning/math/
piecewise_jerk/piecewise_jerk
_problem.h"
#include <algorithm>
#include <chrono>
#include "cyber/common/log.h"
...
...
@@ -30,7 +29,7 @@ namespace {
constexpr
double
kMaxVariableRange
=
1e10
;
}
// namespace
Fem1dQpProblem
::
Fem1dQp
Problem
(
const
size_t
num_of_knots
,
void
PiecewiseJerkProblem
::
Init
Problem
(
const
size_t
num_of_knots
,
const
std
::
array
<
double
,
3
>&
x_init
,
const
double
delta_s
,
const
std
::
array
<
double
,
5
>&
w
,
...
...
@@ -61,7 +60,7 @@ Fem1dQpProblem::Fem1dQpProblem(const size_t num_of_knots,
std
::
make_pair
(
-
kMaxVariableRange
,
kMaxVariableRange
));
}
bool
Fem1dQp
Problem
::
OptimizeWithOsqp
(
bool
PiecewiseJerk
Problem
::
OptimizeWithOsqp
(
const
size_t
kernel_dim
,
const
size_t
num_affine_constraint
,
std
::
vector
<
c_float
>&
P_data
,
std
::
vector
<
c_int
>&
P_indices
,
// NOLINT
std
::
vector
<
c_int
>&
P_indptr
,
std
::
vector
<
c_float
>&
A_data
,
// NOLINT
...
...
@@ -104,25 +103,25 @@ bool Fem1dQpProblem::OptimizeWithOsqp(
return
true
;
}
void
Fem1dQp
Problem
::
SetZeroOrderBounds
(
void
PiecewiseJerk
Problem
::
SetZeroOrderBounds
(
std
::
vector
<
std
::
pair
<
double
,
double
>>
x_bounds
)
{
CHECK_EQ
(
x_bounds
.
size
(),
num_of_knots_
);
x_bounds_
=
std
::
move
(
x_bounds
);
}
void
Fem1dQp
Problem
::
SetFirstOrderBounds
(
void
PiecewiseJerk
Problem
::
SetFirstOrderBounds
(
std
::
vector
<
std
::
pair
<
double
,
double
>>
dx_bounds
)
{
CHECK_EQ
(
dx_bounds
.
size
(),
num_of_knots_
);
dx_bounds_
=
std
::
move
(
dx_bounds
);
}
void
Fem1dQp
Problem
::
SetSecondOrderBounds
(
void
PiecewiseJerk
Problem
::
SetSecondOrderBounds
(
std
::
vector
<
std
::
pair
<
double
,
double
>>
d2x_bounds
)
{
CHECK_EQ
(
d2x_bounds
.
size
(),
num_of_knots_
);
ddx_bounds_
=
std
::
move
(
d2x_bounds
);
}
void
Fem1dQp
Problem
::
SetZeroOrderBounds
(
const
double
x_bound
)
{
void
PiecewiseJerk
Problem
::
SetZeroOrderBounds
(
const
double
x_bound
)
{
CHECK_GT
(
x_bound
,
0.0
);
for
(
auto
&
x
:
x_bounds_
)
{
x
.
first
=
-
x_bound
;
...
...
@@ -130,7 +129,7 @@ void Fem1dQpProblem::SetZeroOrderBounds(const double x_bound) {
}
}
void
Fem1dQp
Problem
::
SetFirstOrderBounds
(
const
double
dx_bound
)
{
void
PiecewiseJerk
Problem
::
SetFirstOrderBounds
(
const
double
dx_bound
)
{
CHECK_GT
(
dx_bound
,
0.0
);
for
(
auto
&
x
:
dx_bounds_
)
{
x
.
first
=
-
dx_bound
;
...
...
@@ -138,7 +137,7 @@ void Fem1dQpProblem::SetFirstOrderBounds(const double dx_bound) {
}
}
void
Fem1dQp
Problem
::
SetSecondOrderBounds
(
const
double
ddx_bound
)
{
void
PiecewiseJerk
Problem
::
SetSecondOrderBounds
(
const
double
ddx_bound
)
{
CHECK_GT
(
ddx_bound
,
0.0
);
for
(
auto
&
x
:
ddx_bounds_
)
{
x
.
first
=
-
ddx_bound
;
...
...
@@ -146,7 +145,7 @@ void Fem1dQpProblem::SetSecondOrderBounds(const double ddx_bound) {
}
}
void
Fem1dQp
Problem
::
ProcessBound
(
void
PiecewiseJerk
Problem
::
ProcessBound
(
const
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>&
src
,
std
::
vector
<
std
::
pair
<
double
,
double
>>*
dst
)
{
DCHECK_NOTNULL
(
dst
);
...
...
@@ -166,33 +165,29 @@ void Fem1dQpProblem::ProcessBound(
}
// x_bounds: tuple(s, lower_bounds, upper_bounds)
void
Fem1dQp
Problem
::
SetVariableBounds
(
void
PiecewiseJerk
Problem
::
SetVariableBounds
(
const
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>&
x_bounds
)
{
ProcessBound
(
x_bounds
,
&
x_bounds_
);
}
// dx_bounds: tuple(s, lower_bounds, upper_bounds)
void
Fem1dQp
Problem
::
SetVariableDerivativeBounds
(
void
PiecewiseJerk
Problem
::
SetVariableDerivativeBounds
(
const
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>&
dx_bounds
)
{
ProcessBound
(
dx_bounds
,
&
dx_bounds_
);
}
// ddx_bounds: tuple(s, lower_bounds, upper_bounds)
void
Fem1dQp
Problem
::
SetVariableSecondOrderDerivativeBounds
(
void
PiecewiseJerk
Problem
::
SetVariableSecondOrderDerivativeBounds
(
const
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>&
ddx_bounds
)
{
ProcessBound
(
ddx_bounds
,
&
ddx_bounds_
);
}
bool
Fem1dQp
Problem
::
Optimize
(
const
int
max_iter
)
{
bool
PiecewiseJerk
Problem
::
Optimize
(
const
int
max_iter
)
{
// calculate kernel
std
::
vector
<
c_float
>
P_data
;
std
::
vector
<
c_int
>
P_indices
;
std
::
vector
<
c_int
>
P_indptr
;
auto
start_time
=
std
::
chrono
::
system_clock
::
now
();
CalculateKernel
(
&
P_data
,
&
P_indices
,
&
P_indptr
);
auto
end_time1
=
std
::
chrono
::
system_clock
::
now
();
std
::
chrono
::
duration
<
double
>
diff
=
end_time1
-
start_time
;
ADEBUG
<<
"Set Kernel used time: "
<<
diff
.
count
()
*
1000
<<
" ms."
;
// calculate affine constraints
std
::
vector
<
c_float
>
A_data
;
...
...
@@ -202,17 +197,10 @@ bool Fem1dQpProblem::Optimize(const int max_iter) {
std
::
vector
<
c_float
>
upper_bounds
;
CalculateAffineConstraint
(
&
A_data
,
&
A_indices
,
&
A_indptr
,
&
lower_bounds
,
&
upper_bounds
);
auto
end_time2
=
std
::
chrono
::
system_clock
::
now
();
diff
=
end_time2
-
end_time1
;
ADEBUG
<<
"CalculateAffineConstraint used time: "
<<
diff
.
count
()
*
1000
<<
" ms."
;
// calculate offset
std
::
vector
<
c_float
>
q
;
CalculateOffset
(
&
q
);
auto
end_time3
=
std
::
chrono
::
system_clock
::
now
();
diff
=
end_time3
-
end_time2
;
ADEBUG
<<
"CalculateOffset used time: "
<<
diff
.
count
()
*
1000
<<
" ms."
;
OSQPData
*
data
=
reinterpret_cast
<
OSQPData
*>
(
c_malloc
(
sizeof
(
OSQPData
)));
OSQPSettings
*
settings
=
...
...
@@ -260,14 +248,11 @@ bool Fem1dQpProblem::Optimize(const int max_iter) {
c_free
(
data
->
P
);
c_free
(
data
);
c_free
(
settings
);
auto
end_time4
=
std
::
chrono
::
system_clock
::
now
();
diff
=
end_time4
-
end_time3
;
ADEBUG
<<
"Run OptimizeWithOsqp used time: "
<<
diff
.
count
()
*
1000
<<
" ms."
;
return
true
;
}
void
Fem1dQp
Problem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
void
PiecewiseJerk
Problem
::
CalculateKernel
(
std
::
vector
<
c_float
>*
P_data
,
std
::
vector
<
c_int
>*
P_indices
,
std
::
vector
<
c_int
>*
P_indptr
)
{
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
...
...
@@ -324,12 +309,9 @@ void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
}
}
P_indptr
->
push_back
(
ind_p
);
ADEBUG
<<
"P_data.size()="
<<
P_data
->
size
();
ADEBUG
<<
"P_indices.size()="
<<
P_indices
->
size
();
ADEBUG
<<
"P_indptr.size()="
<<
P_indptr
->
size
();
}
void
Fem1dQp
Problem
::
CalculateAffineConstraint
(
void
PiecewiseJerk
Problem
::
CalculateAffineConstraint
(
std
::
vector
<
c_float
>*
A_data
,
std
::
vector
<
c_int
>*
A_indices
,
std
::
vector
<
c_int
>*
A_indptr
,
std
::
vector
<
c_float
>*
lower_bounds
,
std
::
vector
<
c_float
>*
upper_bounds
)
{
...
...
@@ -428,7 +410,7 @@ void Fem1dQpProblem::CalculateAffineConstraint(
A_indptr
->
push_back
(
ind_p
);
}
void
Fem1dQp
Problem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
void
PiecewiseJerk
Problem
::
CalculateOffset
(
std
::
vector
<
c_float
>*
q
)
{
CHECK_NOTNULL
(
q
);
const
int
N
=
static_cast
<
int
>
(
num_of_knots_
);
const
int
kNumParam
=
3
*
N
;
...
...
modules/planning/math/
finite_element_qp/fem_1d_qp
_problem.h
→
modules/planning/math/
piecewise_jerk/piecewise_jerk
_problem.h
浏览文件 @
88e0dd6d
...
...
@@ -31,7 +31,6 @@ namespace planning {
/*
* @brief:
* FEM stands for finite element method.
* This class solve an optimization problem:
* x
* |
...
...
@@ -47,8 +46,12 @@ namespace planning {
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
*/
class
Fem1dQp
Problem
{
class
PiecewiseJerk
Problem
{
public:
PiecewiseJerkProblem
()
=
default
;
virtual
~
PiecewiseJerkProblem
()
=
default
;
/*
* @param
* x_init: the init status of x, x', x''
...
...
@@ -62,15 +65,11 @@ class Fem1dQpProblem {
* -- w[4]: default reference line weight, (x_bounds[k].first +
* x_bounds[k].second)/2
*/
Fem1dQpProblem
(
const
size_t
num_var
,
const
std
::
array
<
double
,
3
>&
x_init
,
virtual
void
InitProblem
(
const
size_t
num_var
,
const
std
::
array
<
double
,
3
>&
x_init
,
const
double
delta_s
,
const
std
::
array
<
double
,
5
>&
w
,
const
double
max_x_third_order_derivative
);
virtual
~
Fem1dQpProblem
()
=
default
;
virtual
void
AddReferenceLineKernel
(
const
std
::
vector
<
double
>&
ref_line
,
const
double
wweight
)
{}
virtual
void
ResetInitConditions
(
const
std
::
array
<
double
,
3
>&
x_init
)
{
x_init_
=
x_init
;
}
...
...
@@ -102,8 +101,6 @@ class Fem1dQpProblem {
virtual
void
SetVariableSecondOrderDerivativeBounds
(
const
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>&
ddx_bounds
);
virtual
void
PreSetKernel
()
{}
virtual
bool
Optimize
(
const
int
max_iter
=
4000
);
const
std
::
vector
<
double
>&
x
()
const
{
return
x_
;
}
...
...
modules/planning/math/
finite_element_qp/fem_1d_qp
_problem_test.cc
→
modules/planning/math/
piecewise_jerk/piecewise_jerk
_problem_test.cc
浏览文件 @
88e0dd6d
...
...
@@ -30,12 +30,12 @@
#define private public
#define protected public
#include "modules/planning/math/
finite_element_qp/fem_1d_qp
_problem.h"
#include "modules/planning/math/
piecewise_jerk/piesewise_jerk
_problem.h"
namespace
apollo
{
namespace
planning
{
TEST
(
Fem1dQP
ProblemTest
,
basic_test
)
{
TEST
(
PiecewiseJerk
ProblemTest
,
basic_test
)
{
FLAGS_enable_osqp_debug
=
true
;
std
::
array
<
double
,
3
>
x_init
=
{
1.5
,
0.01
,
0.001
};
double
delta_s
=
0.5
;
...
...
@@ -53,8 +53,8 @@ TEST(Fem1dQPProblemTest, basic_test) {
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
<
Fem1dQp
Problem
>
fem_qp
(
new
Fem1dQp
Problem
(
n
,
x_init
,
delta_s
,
w
,
max_x_third_order_derivative
));
std
::
unique_ptr
<
PiecewiseJerk
Problem
>
fem_qp
(
new
PiecewiseJerk
Problem
(
n
,
x_init
,
delta_s
,
w
,
max_x_third_order_derivative
));
fem_qp
->
SetVariableBounds
(
x_bounds
);
fem_qp
->
SetFirstOrderBounds
(
FLAGS_lateral_derivative_bound_default
);
...
...
@@ -74,7 +74,7 @@ TEST(Fem1dQPProblemTest, basic_test) {
}
}
TEST
(
Fem1dQP
ProblemTest
,
add_bounds_test
)
{
TEST
(
PiecewiseJerk
ProblemTest
,
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
;
...
...
@@ -82,8 +82,8 @@ TEST(Fem1dQPProblemTest, add_bounds_test) {
std
::
array
<
double
,
5
>
w
=
{
1.0
,
2.0
,
3.0
,
4.0
,
1.45
};
double
max_x_third_order_derivative
=
0.25
;
std
::
unique_ptr
<
Fem1dQp
Problem
>
fem_qp
(
new
Fem1dQp
Problem
(
n
,
x_init
,
delta_s
,
w
,
max_x_third_order_derivative
));
std
::
unique_ptr
<
PiecewiseJerk
Problem
>
fem_qp
(
new
PiecewiseJerk
Problem
(
n
,
x_init
,
delta_s
,
w
,
max_x_third_order_derivative
));
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>
x_bounds
;
for
(
size_t
i
=
10
;
i
<
20
;
++
i
)
{
...
...
@@ -101,7 +101,7 @@ TEST(Fem1dQPProblemTest, add_bounds_test) {
}
}
TEST
(
Fem1dJerkQp
ProblemTest
,
derivative_constraint_test
)
{
TEST
(
PiecewiseJerk
ProblemTest
,
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
;
...
...
@@ -113,8 +113,8 @@ TEST(Fem1dJerkQpProblemTest, derivative_constraint_test) {
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
<
Fem1dQp
Problem
>
fem_qp
(
new
Fem1dQp
Problem
(
n
,
x_init
,
delta_s
,
w
,
max_x_third_order_derivative
));
std
::
unique_ptr
<
PiecewiseJerk
Problem
>
fem_qp
(
new
PiecewiseJerk
Problem
(
n
,
x_init
,
delta_s
,
w
,
max_x_third_order_derivative
));
fem_qp
->
SetVariableBounds
(
x_bounds
);
fem_qp
->
SetFirstOrderBounds
(
FLAGS_lateral_derivative_bound_default
);
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录