Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-core
Commits
358e65ca
Commit
358e65ca
authored
Nov 22, 2017
by
Joseph Mirabel
Committed by
Joseph Mirabel
Nov 28, 2017
Browse files
Add some checks in SimpleTimeParameterization
parent
b0d889f5
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/path-optimization/simple-time-parameterization.cc
View file @
358e65ca
...
...
@@ -42,6 +42,8 @@ namespace hpp {
T
=
(
s1
-
s0
)
/
B
;
a
[
0
]
=
s0
;
a
[
1
]
=
B
;
hppDout
(
info
,
"Time parametrization returned "
<<
a
.
transpose
()
<<
", "
<<
T
);
return
TimeParameterizationPtr_t
(
new
Polynomial
(
a
));
}
...
...
@@ -55,8 +57,31 @@ namespace hpp {
a
[
1
]
=
0
;
a
[
2
]
=
3
*
(
s1
-
s0
)
/
(
T
*
T
);
a
[
3
]
=
-
2
*
a
[
2
]
/
(
3
*
T
);
hppDout
(
info
,
"Time parametrization returned "
<<
a
.
transpose
()
<<
", "
<<
T
);
return
TimeParameterizationPtr_t
(
new
Polynomial
(
a
));
}
void
checkTimeParameterization
(
const
TimeParameterizationPtr_t
tp
,
const
bool
velocity
,
const
interval_t
sr
,
const
value_type
B
,
const
value_type
T
)
{
using
std
::
fabs
;
const
value_type
thr
=
Eigen
::
NumTraits
<
value_type
>::
dummy_precision
();
if
(
fabs
(
tp
->
value
(
0
)
-
sr
.
first
)
>=
thr
||
fabs
(
tp
->
value
(
T
)
-
sr
.
second
)
>=
thr
)
{
throw
std
::
logic_error
(
"Boundaries of TimeParameterization are not correct."
);
}
if
(
velocity
&&
(
tp
->
derivative
(
0
)
>
thr
||
tp
->
derivative
(
T
)
>
thr
||
fabs
(
tp
->
derivative
(
T
/
2
)
-
B
)
>
thr
||
tp
->
derivative
(
0
)
<
0
||
tp
->
derivative
(
T
)
<
0
||
fabs
(
tp
->
derivative
(
T
/
2
)
-
B
)
<
0
)
)
{
throw
std
::
logic_error
(
"Derivative of TimeParameterization are not correct."
);
}
}
}
SimpleTimeParameterizationPtr_t
SimpleTimeParameterization
::
create
(
const
Problem
&
problem
)
...
...
@@ -123,6 +148,8 @@ namespace hpp {
else
tp
=
computeTimeParameterizationFirstOrder
(
paramRange
.
first
,
paramRange
.
second
,
B
,
T
);
checkTimeParameterization
(
tp
,
velocity
,
paramRange
,
B
,
T
);
PathPtr_t
pp
=
p
->
copy
();
pp
->
timeParameterization
(
tp
,
interval_t
(
0
,
T
));
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment