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
d524ae07
Commit
d524ae07
authored
Jul 20, 2017
by
Justin Carpentier
Committed by
GitHub
Jul 20, 2017
Browse files
Merge pull request #395 from jcarpent/topic/devel
Handle the new convention for quaternion representation with the viewer
parents
02a6c986
7afb96d4
Changes
4
Hide whitespace changes
Inline
Side-by-side
bindings/python/CMakeLists.txt
View file @
d524ae07
...
...
@@ -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 @
d524ae07
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 @
d524ae07
...
...
@@ -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 @
d524ae07
...
...
@@ -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