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
3beb5df2
Commit
3beb5df2
authored
Nov 23, 2020
by
Gabriele Buondonno
Browse files
[bindings] Expose rpyToJacInv and rpyToJacDerivative
parent
e4520147
Changes
2
Hide whitespace changes
Inline
Side-by-side
bindings/python/math/expose-rpy.cpp
View file @
3beb5df2
...
...
@@ -25,6 +25,8 @@ namespace pinocchio
}
BOOST_PYTHON_FUNCTION_OVERLOADS
(
rpyToJac_overload
,
rpy
::
rpyToJac
,
1
,
2
)
BOOST_PYTHON_FUNCTION_OVERLOADS
(
rpyToJacInv_overload
,
rpy
::
rpyToJacInv
,
1
,
2
)
BOOST_PYTHON_FUNCTION_OVERLOADS
(
rpyToJacDerivative_overload
,
rpy
::
rpyToJacDerivative
,
2
,
3
)
Eigen
::
Matrix3d
rotate
(
const
std
::
string
&
axis
,
const
double
ang
)
{
...
...
@@ -95,6 +97,40 @@ namespace pinocchio
" Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
)
);
bp
::
def
(
"rpyToJacInv"
,
&
rpyToJacInv
<
Vector3d
>
,
rpyToJacInv_overload
(
bp
::
args
(
"rpy"
,
"reference_frame"
),
"Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion"
" Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
" and reference frame F (either LOCAL or WORLD),"
" the Jacobian is such that omega_F = J_F(phi)phidot,"
" where omega_F is the angular velocity expressed in frame F"
" and J_F is the Jacobian computed with reference frame F"
"
\n
Parameters:
\n
"
"
\t
rpy Roll-Pitch-Yaw vector"
"
\t
reference_frame Reference frame in which the angular velocity is expressed."
" Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
)
);
bp
::
def
(
"rpyToJacDerivative"
,
&
rpyToJacDerivative
<
Vector3d
,
Vector3d
>
,
rpyToJacDerivative_overload
(
bp
::
args
(
"rpy"
,
"rpydot"
,
"reference_frame"
),
"Compute the time derivative of the Jacobian of the Roll-Pitch-Yaw conversion"
" Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
" and reference frame F (either LOCAL or WORLD),"
" the Jacobian is such that omega_F = J_F(phi)phidot,"
" where omega_F is the angular velocity expressed in frame F"
" and J_F is the Jacobian computed with reference frame F"
"
\n
Parameters:
\n
"
"
\t
rpy Roll-Pitch-Yaw vector"
"
\t
reference_frame Reference frame in which the angular velocity is expressed."
" Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
)
);
}
}
...
...
unittest/python/rpy.py
View file @
3beb5df2
...
...
@@ -2,11 +2,12 @@ import unittest
from
math
import
pi
import
numpy
as
np
from
numpy.linalg
import
inv
from
random
import
random
import
pinocchio
as
pin
from
pinocchio.utils
import
npToTuple
from
pinocchio.rpy
import
matrixToRpy
,
rpyToMatrix
,
rotate
,
rpyToJac
from
pinocchio.rpy
import
matrixToRpy
,
rpyToMatrix
,
rotate
,
rpyToJac
,
rpyToJacInv
,
rpyToJacDerivative
from
test_case
import
PinocchioTestCase
as
TestCase
...
...
@@ -83,5 +84,63 @@ class TestRPY(TestCase):
omegaW
=
jW
.
dot
(
rpydot
)
self
.
assertApprox
(
Rdot
,
pin
.
skew
(
omegaW
).
dot
(
R
),
tol
)
def
test_rpyToJacInv
(
self
):
# Check correct identities between different versions
r
=
random
()
*
2
*
pi
-
pi
p
=
random
()
*
pi
-
pi
/
2
p
*=
0.999
# ensure we are not too close to a singularity
y
=
random
()
*
2
*
pi
-
pi
rpy
=
np
.
array
([
r
,
p
,
y
])
j0
=
rpyToJac
(
rpy
)
j0inv
=
rpyToJacInv
(
rpy
)
self
.
assertApprox
(
j0inv
,
inv
(
j0
))
jL
=
rpyToJac
(
rpy
,
pin
.
LOCAL
)
jLinv
=
rpyToJacInv
(
rpy
,
pin
.
LOCAL
)
self
.
assertApprox
(
jLinv
,
inv
(
jL
))
jW
=
rpyToJac
(
rpy
,
pin
.
WORLD
)
jWinv
=
rpyToJacInv
(
rpy
,
pin
.
WORLD
)
self
.
assertApprox
(
jWinv
,
inv
(
jW
))
jA
=
rpyToJac
(
rpy
,
pin
.
LOCAL_WORLD_ALIGNED
)
jAinv
=
rpyToJacInv
(
rpy
,
pin
.
LOCAL_WORLD_ALIGNED
)
self
.
assertApprox
(
jAinv
,
inv
(
jA
))
def
test_rpyToJacDerivative
(
self
):
# Check zero at zero velocity
r
=
random
()
*
2
*
pi
-
pi
p
=
random
()
*
pi
-
pi
/
2
y
=
random
()
*
2
*
pi
-
pi
rpy
=
np
.
array
([
r
,
p
,
y
])
rpydot
=
np
.
zeros
(
3
)
dj0
=
rpyToJacDerivative
(
rpy
,
rpydot
)
self
.
assertTrue
((
dj0
==
np
.
zeros
((
3
,
3
))).
all
())
djL
=
rpyToJacDerivative
(
rpy
,
rpydot
,
pin
.
LOCAL
)
self
.
assertTrue
((
djL
==
np
.
zeros
((
3
,
3
))).
all
())
djW
=
rpyToJacDerivative
(
rpy
,
rpydot
,
pin
.
WORLD
)
self
.
assertTrue
((
djW
==
np
.
zeros
((
3
,
3
))).
all
())
djA
=
rpyToJacDerivative
(
rpy
,
rpydot
,
pin
.
LOCAL_WORLD_ALIGNED
)
self
.
assertTrue
((
djA
==
np
.
zeros
((
3
,
3
))).
all
())
# Check correct identities between different versions
rpydot
=
np
.
random
.
rand
(
3
)
dj0
=
rpyToJacDerivative
(
rpy
,
rpydot
)
djL
=
rpyToJacDerivative
(
rpy
,
rpydot
,
pin
.
LOCAL
)
djW
=
rpyToJacDerivative
(
rpy
,
rpydot
,
pin
.
WORLD
)
djA
=
rpyToJacDerivative
(
rpy
,
rpydot
,
pin
.
LOCAL_WORLD_ALIGNED
)
self
.
assertTrue
((
dj0
==
djL
).
all
())
self
.
assertTrue
((
djW
==
djA
).
all
())
R
=
rpyToMatrix
(
rpy
)
jL
=
rpyToJac
(
rpy
,
pin
.
LOCAL
)
jW
=
rpyToJac
(
rpy
,
pin
.
WORLD
)
omegaL
=
jL
.
dot
(
rpydot
)
omegaW
=
jW
.
dot
(
rpydot
)
self
.
assertApprox
(
omegaW
,
R
.
dot
(
omegaL
))
self
.
assertApprox
(
djW
,
pin
.
skew
(
omegaW
).
dot
(
R
).
dot
(
jL
)
+
R
.
dot
(
djL
))
self
.
assertApprox
(
djW
,
R
.
dot
(
pin
.
skew
(
omegaL
)).
dot
(
jL
)
+
R
.
dot
(
djL
))
if
__name__
==
'__main__'
:
unittest
.
main
()
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