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
47477707
Commit
47477707
authored
Mar 21, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 21, 2016
Browse files
Fix quaternion distance and add JointConfiguration::squaredDistance
parent
cf2cdece
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/hpp/model/configuration.hh
View file @
47477707
...
...
@@ -105,6 +105,26 @@ namespace hpp {
result
.
tail
(
dim
)
=
q2
.
tail
(
dim
)
-
q1
.
tail
(
dim
);
}
/// Distance between two configuration.
///
/// \param robot robot that describes the kinematic chain
/// \param q1 first configuration,
/// \param q2 second configuration,
inline
value_type
distance
(
const
DevicePtr_t
&
robot
,
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
)
{
value_type
result
=
0
;
const
JointVector_t
&
jv
(
robot
->
getJointVector
());
for
(
model
::
JointVector_t
::
const_iterator
itJoint
=
jv
.
begin
();
itJoint
!=
jv
.
end
();
itJoint
++
)
{
size_type
iC
=
(
*
itJoint
)
->
rankInConfiguration
();
result
+=
(
*
itJoint
)
->
configuration
()
->
squaredDistance
(
q1
,
q2
,
iC
);
}
const
size_type
&
dim
=
robot
->
extraConfigSpace
().
dimension
();
result
+=
(
q2
.
tail
(
dim
)
-
q1
.
tail
(
dim
)).
squaredNorm
();
return
sqrt
(
result
);
}
/// Normalize configuration
///
/// Configuration space is a represented by a sub-manifold of a vector
...
...
include/hpp/model/joint-configuration.hh
View file @
47477707
...
...
@@ -66,6 +66,14 @@ namespace hpp {
const
size_type
&
index
,
ConfigurationOut_t
result
)
=
0
;
/// Squared distance between two configurations of the joint
/// \param q1, q2 two configurations of the robot
/// \param index index of first component of q1 and q2 corresponding to
/// the joint.
virtual
value_type
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
=
0
;
/// Distance between two configurations of the joint
/// \param q1, q2 two configurations of the robot
/// \param index index of first component of q1 and q2 corresponding to
...
...
@@ -160,6 +168,9 @@ namespace hpp {
virtual
value_type
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
value_type
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
virtual
void
integrate
(
ConfigurationIn_t
q
,
vectorIn_t
v
,
...
...
@@ -198,6 +209,9 @@ namespace hpp {
virtual
value_type
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
value_type
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
virtual
void
integrate
(
ConfigurationIn_t
q
,
vectorIn_t
v
,
const
size_type
&
indexConfig
,
...
...
@@ -311,6 +325,9 @@ namespace hpp {
ConfigurationOut_t
result
);
value_type
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
value_type
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
void
integrate
(
ConfigurationIn_t
q
,
vectorIn_t
v
,
const
size_type
&
indexConfig
,
const
size_type
&
indexVelocity
,
...
...
@@ -338,6 +355,9 @@ namespace hpp {
ConfigurationOut_t
result
);
value_type
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
value_type
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
void
integrate
(
ConfigurationIn_t
q
,
vectorIn_t
v
,
const
size_type
&
indexConfig
,
const
size_type
&
indexVelocity
,
...
...
@@ -375,6 +395,9 @@ namespace hpp {
virtual
value_type
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
value_type
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
;
virtual
void
integrate
(
ConfigurationIn_t
q
,
vectorIn_t
v
,
...
...
src/joint-configuration.cc
View file @
47477707
...
...
@@ -117,6 +117,13 @@ namespace hpp {
{
}
value_type
AnchorJointConfig
::
squaredDistance
(
ConfigurationIn_t
,
ConfigurationIn_t
,
const
size_type
&
)
const
{
return
0
;
}
value_type
AnchorJointConfig
::
distance
(
ConfigurationIn_t
,
ConfigurationIn_t
,
const
size_type
&
)
const
...
...
@@ -152,6 +159,8 @@ namespace hpp {
}
/// Compute quaternion and angle from a SO(3) joint configuration
/// \note this angle corresponds to the angle between q1 and q2, in \f$ \mathbb{R}^4 \f$.
/// The angle of the rotation going from q1 to q2 is half this angle.
///
/// \param q1, q2, robot configurations
/// \param index index of joint configuration in robot configuration vector
...
...
@@ -199,6 +208,14 @@ namespace hpp {
}
}
value_type
SO3JointConfig
::
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
{
const
value_type
d
(
distance
(
q1
,
q2
,
index
));
return
d
*
d
;
}
value_type
SO3JointConfig
::
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
...
...
@@ -207,7 +224,7 @@ namespace hpp {
value_type
theta
=
angleBetweenQuaternions
(
q1
,
q2
,
index
,
cosIsNegative
);
assert
(
theta
>=
0
);
assert
(
M_PI
-
theta
>=
0
);
return
(
cosIsNegative
)
?
M_PI
-
theta
:
theta
;
return
2
*
(
cosIsNegative
?
M_PI
-
theta
:
theta
)
;
}
void
SO3JointConfig
::
integrate
(
ConfigurationIn_t
q
,
...
...
@@ -302,6 +319,13 @@ namespace hpp {
u
*
q2
.
segment
<
dimension
>
(
index
);
}
template
<
size_type
dimension
>
value_type
TranslationJointConfig
<
dimension
>::
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
{
return
(
q2
.
segment
<
dimension
>
(
index
)
-
q1
.
segment
<
dimension
>
(
index
)).
squaredNorm
();
}
template
<
size_type
dimension
>
value_type
TranslationJointConfig
<
dimension
>::
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
...
...
@@ -401,6 +425,14 @@ namespace hpp {
}
}
value_type
UnBounded
::
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
{
const
value_type
d
(
distance
(
q1
,
q2
,
index
));
return
d
*
d
;
}
value_type
UnBounded
::
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
...
...
@@ -475,6 +507,14 @@ namespace hpp {
result
[
index
]
=
(
1
-
u
)
*
q1
[
index
]
+
u
*
q2
[
index
];
}
value_type
Bounded
::
squaredDistance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
{
// linearly interpolate
return
(
q2
.
segment
<
1
>
(
index
)
-
q1
.
segment
<
1
>
(
index
)).
squaredNorm
();
}
value_type
Bounded
::
distance
(
ConfigurationIn_t
q1
,
ConfigurationIn_t
q2
,
const
size_type
&
index
)
const
{
...
...
tests/test-configuration.cc
View file @
47477707
...
...
@@ -71,6 +71,7 @@ DevicePtr_t createRobot ()
// SO3 joint
position
.
setIdentity
();
JointPtr_t
j1
=
factory
.
createJointSO3
(
position
);
j1
->
name
(
"so3"
);
joint
->
addChildJoint
(
j1
);
return
robot
;
...
...
@@ -109,14 +110,31 @@ BOOST_AUTO_TEST_CASE(difference_and_integrate)
Configuration_t
q1
;
q1
.
resize
(
robot
->
configSize
());
Configuration_t
q2
;
q2
.
resize
(
robot
->
configSize
());
vector_t
q1_minus_q0
;
q1_minus_q0
.
resize
(
robot
->
numberDof
());
const
value_type
eps_dist
=
robot
->
numberDof
()
*
sqrt
(
Eigen
::
NumTraits
<
value_type
>::
epsilon
());
for
(
size_type
i
=
0
;
i
<
10000
;
++
i
)
{
shootRandomConfig
(
robot
,
q0
);
shootRandomConfig
(
robot
,
q1
);
hpp
::
model
::
difference
(
robot
,
q1
,
q0
,
q1_minus_q0
);
hpp
::
model
::
integrate
(
robot
,
q0
,
q1_minus_q0
,
q2
);
std
::
cout
<<
"q1="
<<
q1
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"q2="
<<
q2
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"(q2 - q1).norm () = "
<<
(
q2
-
q1
).
norm
()
<<
std
::
endl
;
BOOST_CHECK
((
q2
-
q1
).
norm
()
<
1e-10
);
// Check that the distance is the norm of the difference
value_type
distance
=
hpp
::
model
::
distance
(
robot
,
q0
,
q1
);
BOOST_CHECK_MESSAGE
(
distance
-
q1_minus_q0
.
norm
()
<
Eigen
::
NumTraits
<
value_type
>::
dummy_precision
(),
"
\n
The distance is not the norm of the difference
\n
"
<<
"q0="
<<
q0
.
transpose
()
<<
"
\n
"
<<
"q1="
<<
q1
.
transpose
()
<<
"
\n
"
<<
"distance="
<<
distance
<<
"
\n
"
<<
"(q1 - q0).norm () = "
<<
q1_minus_q0
.
norm
()
);
// Check that distance (q0 + (q1 - q0), q1) is zero
distance
=
hpp
::
model
::
distance
(
robot
,
q1
,
q2
);
BOOST_CHECK_MESSAGE
(
distance
<
eps_dist
,
"
\n
(q0 + (q1 - q0)) is not equivalent to q1
\n
"
<<
"q1="
<<
q1
.
transpose
()
<<
"
\n
"
<<
"q2="
<<
q2
.
transpose
()
<<
"
\n
"
<<
"distance="
<<
distance
);
}
}
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