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-model
Commits
a6f56162
Commit
a6f56162
authored
Jun 13, 2016
by
Florent Lamiraux
Browse files
Handle case when angle == pi in SO3JointConfig::interpolate
add two assertions to detect nan.
parent
a4969a31
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/joint-configuration.cc
View file @
a6f56162
...
...
@@ -198,14 +198,18 @@ namespace hpp {
// theta is between 0 and M_PI/2.
// sin(alpha*theta)/sin(theta) in 0 should be computed differently.
if
(
fabs
(
angle
)
>
1e-6
)
{
if
(
fabs
(
theta
)
>
1e-6
)
{
const
value_type
sintheta_inv
=
1
/
sin
(
theta
);
result
.
segment
(
index
,
4
)
=
(
sin
((
1
-
u
)
*
theta
)
*
sintheta_inv
)
*
q1
.
segment
(
index
,
4
)
+
invertor
*
(
sin
(
u
*
theta
)
*
sintheta_inv
)
*
q2
.
segment
(
index
,
4
);
assert
(
result
.
segment
(
index
,
4
).
norm
()
==
result
.
segment
(
index
,
4
).
norm
());
}
else
{
result
.
segment
(
index
,
4
)
=
(
1
-
u
)
*
q1
.
segment
(
index
,
4
)
+
invertor
*
u
*
q2
.
segment
(
index
,
4
);
assert
(
result
.
segment
(
index
,
4
).
norm
()
==
result
.
segment
(
index
,
4
).
norm
());
}
}
...
...
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