Commit d524ae07 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #395 from jcarpent/topic/devel

Handle the new convention for quaternion representation with the viewer
parents 02a6c986 7afb96d4
import functools
import inspect
import warnings
class DeprecatedWarning(UserWarning):
def deprecated(instructions):
"""Flags a method as deprecated.
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.'''
def wrapper(*args, **kwargs):
message = 'Call to deprecated function {}. {}'.format(
frame = inspect.currentframe().f_back
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(]
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)
......@@ -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 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 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