Skip to content
GitLab
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
f54dc381
Commit
f54dc381
authored
Mar 17, 2017
by
Florent Lamiraux
Committed by
Florent Lamiraux florent@laas.fr
Mar 17, 2017
Browse files
Make Reeds and Shepp paths more robust.
parent
b4e153c1
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/reeds-shepp-path.cc
View file @
f54dc381
...
...
@@ -734,8 +734,9 @@ namespace hpp {
timeRange
().
second
==
timeRange
().
first
)
{
p
=
timeRange
().
first
;
}
if
(
p
>=
timeRange
().
second
)
{
p
=
timeRange
().
second
;
value_type
precision
(
sqrt
(
std
::
numeric_limits
<
value_type
>::
epsilon
()));
if
(
p
>=
timeRange
().
second
-
precision
)
{
p
=
timeRange
().
second
-
precision
;
}
// Does a linear interpolation on all the joints.
if
(
order
>
1
)
{
...
...
@@ -756,7 +757,9 @@ namespace hpp {
phi
=
atan2
(
initial_
(
rzId_
+
1
),
initial_
(
rzId_
));
value_type
dPhi
=
sqrt
(
-
1
);
if
(
t
==
0
)
t
=
std
::
numeric_limits
<
value_type
>::
epsilon
();
for
(
unsigned
int
i
=
0
;
i
<
5
&&
t
>
0
;
++
i
)
unsigned
int
i
;
bool
interrupt
(
false
);
for
(
i
=
0
;
i
<
5
&&
t
>
0
&&
!
interrupt
;
++
i
)
{
value_type
forward
;
if
(
lengths_
[
i
]
<
0
)
{
...
...
@@ -773,7 +776,7 @@ namespace hpp {
{
case
RS_LEFT
:
// Fill velocity only for last segment
if
(
t
<=
0
)
{
if
(
t
<=
1e-8
)
{
result
(
xyId_
+
0
)
=
forward
*
cos
(
phi
+
v
);
result
(
xyId_
+
1
)
=
forward
*
sin
(
phi
+
v
);
dPhi
=
forward
;
...
...
@@ -782,7 +785,7 @@ namespace hpp {
break
;
case
RS_RIGHT
:
// Fill velocity only for last segment
if
(
t
<=
0
)
{
if
(
t
<=
1e-8
)
{
result
(
xyId_
+
0
)
=
forward
*
cos
(
phi
-
v
);
result
(
xyId_
+
1
)
=
forward
*
sin
(
phi
-
v
);
dPhi
=
-
forward
;
...
...
@@ -791,13 +794,17 @@ namespace hpp {
break
;
case
RS_STRAIGHT
:
// Fill velocity only for last segment
if
(
t
<=
0
)
{
if
(
t
<=
1e-8
)
{
result
(
xyId_
+
0
)
=
forward
*
cos
(
phi
);
result
(
xyId_
+
1
)
=
forward
*
sin
(
phi
);
dPhi
=
0
;
}
break
;
case
RS_NOP
:
assert
(
false
&&
"Param out of bounds; RS_NOP"
);
break
;
default:
assert
(
false
&&
"Param out of bounds: default"
);
break
;
}
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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