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
Gabriele Buondonno
pinocchio
Commits
7afb96d4
Commit
7afb96d4
authored
Jul 19, 2017
by
jcarpent
Browse files
[Python] Handle the new convention for quaternion representation with the viewer
parent
02a6c986
Changes
4
Hide whitespace changes
Inline
Side-by-side
bindings/python/CMakeLists.txt
View file @
7afb96d4
...
...
@@ -194,6 +194,7 @@ INSTALL(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR})
# --- INSTALL SCRIPTS
SET
(
PYTHON_FILES
__init__.py
deprecation.py
utils.py
robot_wrapper.py
romeo_wrapper.py
...
...
bindings/python/scripts/deprecation.py
0 → 100644
View file @
7afb96d4
import
functools
import
inspect
import
warnings
class
DeprecatedWarning
(
UserWarning
):
pass
def
deprecated
(
instructions
):
"""Flags a method as deprecated.
Args:
instructions: A human-friendly string of instructions, such
as: 'Please migrate to add_proxy() ASAP.'
"""
def
decorator
(
func
):
'''This is a decorator which can be used to mark functions
as deprecated. It will result in a warning being emitted
when the function is used.'''
@
functools
.
wraps
(
func
)
def
wrapper
(
*
args
,
**
kwargs
):
message
=
'Call to deprecated function {}. {}'
.
format
(
func
.
__name__
,
instructions
)
frame
=
inspect
.
currentframe
().
f_back
warnings
.
warn_explicit
(
message
,
category
=
DeprecatedWarning
,
filename
=
inspect
.
getfile
(
frame
.
f_code
),
lineno
=
frame
.
f_lineno
)
return
func
(
*
args
,
**
kwargs
)
return
wrapper
return
decorator
bindings/python/scripts/robot_wrapper.py
View file @
7afb96d4
...
...
@@ -209,9 +209,8 @@ class RobotWrapper(object):
for
visual
in
self
.
visual_model
.
geometryObjects
:
M
=
self
.
visual_data
.
oMg
[
self
.
visual_model
.
getGeometryId
(
visual
.
name
)]
pinocchioConf
=
utils
.
se3ToXYZQUAT
(
M
)
viewerConf
=
utils
.
XYZQUATToViewerConfiguration
(
pinocchioConf
)
self
.
viewer
.
gui
.
applyConfiguration
(
self
.
viewerNodeNames
(
visual
),
viewerConf
)
conf
=
utils
.
se3ToXYZQUAT
(
M
)
self
.
viewer
.
gui
.
applyConfiguration
(
self
.
viewerNodeNames
(
visual
),
conf
)
self
.
viewer
.
gui
.
refresh
()
...
...
bindings/python/scripts/utils.py
View file @
7afb96d4
...
...
@@ -22,6 +22,8 @@ import numpy.linalg as npl
import
libpinocchio_pywrap
as
se3
from
rpy
import
matrixToRpy
,
npToTTuple
,
npToTuple
,
rotate
,
rpyToMatrix
from
deprecation
import
deprecated
eye
=
lambda
n
:
np
.
matrix
(
np
.
eye
(
n
),
np
.
double
)
zero
=
lambda
n
:
np
.
matrix
(
np
.
zeros
([
n
,
1
]
if
isinstance
(
n
,
int
)
else
n
),
np
.
double
)
rand
=
lambda
n
:
np
.
matrix
(
np
.
random
.
rand
(
n
,
1
)
if
isinstance
(
n
,
int
)
else
np
.
random
.
rand
(
n
[
0
],
n
[
1
]),
np
.
double
)
...
...
@@ -55,24 +57,25 @@ def XYZQUATToSe3(xyzq):
return
se3
.
SE3
(
se3
.
Quaternion
(
xyzq
[
6
,
0
],
xyzq
[
3
,
0
],
xyzq
[
4
,
0
],
xyzq
[
5
,
0
]).
matrix
(),
xyzq
[:
3
])
@
deprecated
(
'Now useless.'
)
def
XYZQUATToViewerConfiguration
(
xyzq
):
'''
Convert the input 7D vector [X,Y,Z,x,y,z,w] to 7D vector [X,Y,Z,w,x,y,z]
Convert the input 7D vector [X,Y,Z,x,y,z,w] to 7D vector [X,Y,Z,x,y,z,w]
Gepetto Viewer Corba has changed its convention for quaternions - This function is not more required.
See https://github.com/humanoid-path-planner/gepetto-viewer-corba/pull/58 for more details.
'''
if
isinstance
(
xyzq
,
(
tuple
,
list
)):
xyzq
=
np
.
matrix
(
xyzq
,
np
.
float
).
T
return
[
float
(
xyzq
[
0
,
0
]),
float
(
xyzq
[
1
,
0
]),
float
(
xyzq
[
2
,
0
]),
float
(
xyzq
[
6
,
0
]),
float
(
xyzq
[
3
,
0
]),
float
(
xyzq
[
4
,
0
]),
float
(
xyzq
[
5
,
0
])]
if
isinstance
(
xyzq
,
(
np
.
matrix
)):
return
xyzq
.
A
.
squeeze
().
tolist
()
return
xyzq
@
deprecated
(
'Now useless.'
)
def
ViewerConfigurationToXYZQUAT
(
vconf
):
'''
Reverse function of XYZQUATToViewerConfiguration : convert [X,Y,Z,w,x,y,z] to [X,Y,Z,x,y,z,w]
Reverse function of XYZQUATToViewerConfiguration : convert [X,Y,Z,x,y,z,w] to [X,Y,Z,x,y,z,w]
Gepetto Viewer Corba has changed its convention for quaternions - This function is not more required.
See https://github.com/humanoid-path-planner/gepetto-viewer-corba/pull/58 for more details.
'''
if
isinstance
(
vconf
,
(
tuple
,
list
)):
vconf
=
np
.
matrix
(
vconf
,
np
.
float
).
T
return
[
float
(
vconf
[
0
,
0
]),
float
(
vconf
[
1
,
0
]),
float
(
vconf
[
2
,
0
]),
float
(
vconf
[
4
,
0
]),
float
(
vconf
[
5
,
0
]),
float
(
vconf
[
6
,
0
]),
float
(
vconf
[
3
,
0
])]
return
vconf
def
isapprox
(
a
,
b
,
epsilon
=
1e-6
):
...
...
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