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
Stack Of Tasks
pinocchio
Commits
12132aac
Commit
12132aac
authored
Nov 25, 2020
by
Gabriele Buondonno
Browse files
[unittest] Simplify test_rpyToMatrix
parent
3beb5df2
Changes
1
Hide whitespace changes
Inline
Side-by-side
unittest/rpy.cpp
View file @
12132aac
...
...
@@ -15,34 +15,26 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE
(
test_rpyToMatrix
)
{
#ifdef NDEBUG
const
int
n
=
1e5
;
#else
const
int
n
=
1e2
;
#endif
for
(
int
k
=
0
;
k
<
n
;
++
k
)
{
double
r
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
(
2
*
M_PI
)))
-
M_PI
;
double
p
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
M_PI
))
-
(
M_PI
/
2
);
double
y
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
(
2
*
M_PI
)))
-
M_PI
;
Eigen
::
Matrix3d
R
=
pinocchio
::
rpy
::
rpyToMatrix
(
r
,
p
,
y
);
Eigen
::
Matrix3d
Raa
=
(
Eigen
::
AngleAxisd
(
y
,
Eigen
::
Vector3d
::
UnitZ
())
*
Eigen
::
AngleAxisd
(
p
,
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
r
,
Eigen
::
Vector3d
::
UnitX
())
).
toRotationMatrix
();
BOOST_CHECK
(
R
.
isApprox
(
Raa
));
Eigen
::
Vector3d
v
;
v
<<
r
,
p
,
y
;
Eigen
::
Matrix3d
Rv
=
pinocchio
::
rpy
::
rpyToMatrix
(
v
);
BOOST_CHECK
(
Rv
.
isApprox
(
Raa
));
BOOST_CHECK
(
Rv
.
isApprox
(
R
));
}
double
r
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
(
2
*
M_PI
)))
-
M_PI
;
double
p
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
M_PI
))
-
(
M_PI
/
2
);
double
y
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
(
2
*
M_PI
)))
-
M_PI
;
Eigen
::
Matrix3d
R
=
pinocchio
::
rpy
::
rpyToMatrix
(
r
,
p
,
y
);
Eigen
::
Matrix3d
Raa
=
(
Eigen
::
AngleAxisd
(
y
,
Eigen
::
Vector3d
::
UnitZ
())
*
Eigen
::
AngleAxisd
(
p
,
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
r
,
Eigen
::
Vector3d
::
UnitX
())
).
toRotationMatrix
();
BOOST_CHECK
(
R
.
isApprox
(
Raa
));
Eigen
::
Vector3d
v
;
v
<<
r
,
p
,
y
;
Eigen
::
Matrix3d
Rv
=
pinocchio
::
rpy
::
rpyToMatrix
(
v
);
BOOST_CHECK
(
Rv
.
isApprox
(
Raa
));
BOOST_CHECK
(
Rv
.
isApprox
(
R
));
}
BOOST_AUTO_TEST_CASE
(
test_matrixToRpy
)
...
...
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