Commit 7afb96d4 authored by jcarpent's avatar jcarpent
Browse files

[Python] Handle the new convention for quaternion representation with the viewer

parent 02a6c986
......@@ -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
......
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
......@@ -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()
......
......@@ -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):
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment