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
a448f174
Commit
a448f174
authored
Nov 25, 2020
by
Gabriele Buondonno
Browse files
[unittest/python] Better rpy tests
parent
12132aac
Pipeline
#12431
passed with stage
in 117 minutes and 24 seconds
Changes
1
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
unittest/python/rpy.py
View file @
a448f174
...
...
@@ -5,6 +5,7 @@ import numpy as np
from
numpy.linalg
import
inv
from
random
import
random
from
eigenpy
import
AngleAxis
import
pinocchio
as
pin
from
pinocchio.utils
import
npToTuple
from
pinocchio.rpy
import
matrixToRpy
,
rpyToMatrix
,
rotate
,
rpyToJac
,
rpyToJacInv
,
rpyToJacDerivative
...
...
@@ -42,6 +43,86 @@ class TestRPY(TestCase):
else
:
self
.
assertTrue
(
False
)
def
test_rpyToMatrix
(
self
):
r
=
random
()
*
2
*
pi
-
pi
p
=
random
()
*
pi
-
pi
/
2
y
=
random
()
*
2
*
pi
-
pi
R
=
rpyToMatrix
(
r
,
p
,
y
)
Raa
=
AngleAxis
(
y
,
np
.
array
([
0.
,
0.
,
1.
])).
matrix
()
\
.
dot
(
AngleAxis
(
p
,
np
.
array
([
0.
,
1.
,
0.
])).
matrix
())
\
.
dot
(
AngleAxis
(
r
,
np
.
array
([
1.
,
0.
,
0.
])).
matrix
())
self
.
assertApprox
(
R
,
Raa
)
v
=
np
.
array
([
r
,
p
,
y
])
Rv
=
rpyToMatrix
(
v
)
self
.
assertApprox
(
Rv
,
Raa
)
self
.
assertApprox
(
Rv
,
R
)
def
test_matrixToRpy
(
self
):
n
=
100
for
k
in
range
(
n
):
quat
=
pin
.
Quaternion
(
np
.
random
.
rand
(
4
,
1
)).
normalized
()
R
=
quat
.
toRotationMatrix
()
v
=
matrixToRpy
(
R
)
Rprime
=
rpyToMatrix
(
v
)
self
.
assertApprox
(
Rprime
,
R
)
self
.
assertTrue
(
-
pi
<=
v
[
0
]
and
v
[
0
]
<=
pi
)
self
.
assertTrue
(
-
pi
/
2
<=
v
[
1
]
and
v
[
1
]
<=
pi
/
2
)
self
.
assertTrue
(
-
pi
<=
v
[
2
]
and
v
[
2
]
<=
pi
)
n2
=
100
# Test singular case theta = pi/2
Rp
=
np
.
array
([[
0.0
,
0.0
,
1.0
],
[
0.0
,
1.0
,
0.0
],
[
-
1.0
,
0.0
,
0.0
]])
for
k
in
range
(
n2
):
r
=
random
()
*
2
*
pi
-
pi
y
=
random
()
*
2
*
pi
-
pi
R
=
AngleAxis
(
y
,
np
.
array
([
0.
,
0.
,
1.
])).
matrix
()
\
.
dot
(
Rp
)
\
.
dot
(
AngleAxis
(
r
,
np
.
array
([
1.
,
0.
,
0.
])).
matrix
())
v
=
matrixToRpy
(
R
)
Rprime
=
rpyToMatrix
(
v
)
self
.
assertApprox
(
Rprime
,
R
)
self
.
assertTrue
(
-
pi
<=
v
[
0
]
and
v
[
0
]
<=
pi
)
self
.
assertTrue
(
-
pi
/
2
<=
v
[
1
]
and
v
[
1
]
<=
pi
/
2
)
self
.
assertTrue
(
-
pi
<=
v
[
2
]
and
v
[
2
]
<=
pi
)
# Test singular case theta = -pi/2
Rp
=
np
.
array
([[
0.0
,
0.0
,
-
1.0
],
[
0.0
,
1.0
,
0.0
],
[
1.0
,
0.0
,
0.0
]])
for
k
in
range
(
n2
):
r
=
random
()
*
2
*
pi
-
pi
y
=
random
()
*
2
*
pi
-
pi
R
=
AngleAxis
(
y
,
np
.
array
([
0.
,
0.
,
1.
])).
matrix
()
\
.
dot
(
Rp
)
\
.
dot
(
AngleAxis
(
r
,
np
.
array
([
1.
,
0.
,
0.
])).
matrix
())
v
=
matrixToRpy
(
R
)
Rprime
=
rpyToMatrix
(
v
)
self
.
assertApprox
(
Rprime
,
R
)
self
.
assertTrue
(
-
pi
<=
v
[
0
]
and
v
[
0
]
<=
pi
)
self
.
assertTrue
(
-
pi
/
2
<=
v
[
1
]
and
v
[
1
]
<=
pi
/
2
)
self
.
assertTrue
(
-
pi
<=
v
[
2
]
and
v
[
2
]
<=
pi
)
def
test_rpyToJac
(
self
):
# Check identity at zero
rpy
=
np
.
zeros
(
3
)
...
...
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