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
df761e9d
Commit
df761e9d
authored
Oct 28, 2015
by
Florent Lamiraux
Browse files
In JointConfiguration, make sure that distance and difference are zero
- whenever configurations are equal.
parent
0f53ce3b
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/joint-configuration.cc
View file @
df761e9d
...
...
@@ -162,6 +162,10 @@ namespace hpp {
const
size_type
&
index
,
bool
&
cosIsNegative
)
{
if
(
q1
.
segment
(
index
,
4
)
==
q2
.
segment
(
index
,
4
))
{
cosIsNegative
=
false
;
return
0
;
}
value_type
innerprod
=
q1
.
segment
(
index
,
4
).
dot
(
q2
.
segment
(
index
,
4
));
assert
(
fabs
(
innerprod
)
<
1.0001
);
if
(
innerprod
<
-
1
)
innerprod
=
-
1
;
...
...
@@ -242,6 +246,10 @@ namespace hpp {
const
size_type
&
indexVelocity
,
vectorOut_t
result
)
const
{
if
(
q1
.
segment
<
4
>
(
indexConfig
)
==
q2
.
segment
<
4
>
(
indexConfig
))
{
result
.
segment
<
3
>
(
indexVelocity
).
setZero
();
return
;
}
const
int
invertor
=
(
q1
.
segment
<
4
>
(
indexConfig
)
.
dot
(
q2
.
segment
<
4
>
(
indexConfig
))
<
0
...
...
@@ -397,14 +405,9 @@ namespace hpp {
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
{
// distance on the unit circle
value_type
innerprod
=
q1
.
segment
(
index
,
2
).
dot
(
q2
.
segment
(
index
,
2
));
assert
(
fabs
(
innerprod
)
<
1.0001
);
if
(
innerprod
<
-
1
)
innerprod
=
-
1
;
if
(
innerprod
>
1
)
innerprod
=
1
;
value_type
theta
=
acos
(
innerprod
);
return
theta
;
Eigen
::
Matrix
<
value_type
,
1
,
1
>
result
;
difference
(
q1
,
q2
,
index
,
0
,
result
);
return
fabs
(
result
[
0
]);
}
void
UnBounded
::
integrate
(
ConfigurationIn_t
q
,
vectorIn_t
v
,
...
...
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