Commit e801f6b4 authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

[python-linker] flake8 + yapf

parent 6e2f5285
......@@ -27,7 +27,11 @@ class FeaturePosition(Entity):
signalMap = dict()
def __init__(self, name, signalPosition=None, signalJacobian=None, referencePosition=None):
def __init__(self,
name,
signalPosition=None,
signalJacobian=None,
referencePosition=None):
self._feature = FeaturePoint6d(name)
self.obj = self._feature.obj
self._reference = FeaturePoint6d(name + '_ref')
......
......@@ -10,7 +10,8 @@ from dynamic_graph.signal_base import SignalBase
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
# Identity matrix of order 4
I4 = reduce(lambda m, i: m + (i * (0., ) + (1., ) + (3 - i) * (0., ), ), range(4), ())
I4 = reduce(lambda m, i: m + (i * (0., ) + (1., ) + (3 - i) * (0., ), ),
range(4), ())
class FeaturePositionRelative(Entity):
......
......@@ -6,15 +6,21 @@
# from operator import WeightDir
# from operator import Nullificator
from .operator import (
Add_of_double, Add_of_matrix, Add_of_vector, Component_of_vector, Compose_R_and_T, ConvolutionTemporal,
HomoToMatrix, HomoToRotation, HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo, Inverse_of_matrixrotation,
Inverse_of_matrixtwist, Inverse_of_unitquat, MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo, MatrixToQuaternion, MatrixToRPY,
MatrixToUTheta, MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector, Multiply_matrixTwist_vector,
Multiply_matrixHomo_vector, Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo, Multiply_of_matrixrotation,
Multiply_of_matrixtwist, Multiply_of_quaternion, Multiply_of_vector, PoseRollPitchYawToMatrixHomo,
PoseRollPitchYawToPoseUTheta, PoseUThetaToMatrixHomo, QuaternionToMatrix, RPYToMatrix, Selec_column_of_matrix,
Selec_of_matrix, Selec_of_vector, SkewSymToVector, Stack_of_vector, Substract_of_double, Substract_of_matrix,
Add_of_double, Add_of_matrix, Add_of_vector, Component_of_vector,
Compose_R_and_T, ConvolutionTemporal, HomoToMatrix, HomoToRotation,
HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo,
Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo,
MatrixToQuaternion, MatrixToRPY, MatrixToUTheta, MatrixTranspose,
Multiply_double_vector, Multiply_matrix_vector,
Multiply_matrixTwist_vector, Multiply_matrixHomo_vector,
Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo,
Multiply_of_matrixrotation, Multiply_of_matrixtwist,
Multiply_of_quaternion, Multiply_of_vector, PoseRollPitchYawToMatrixHomo,
PoseRollPitchYawToPoseUTheta, PoseUThetaToMatrixHomo, QuaternionToMatrix,
RPYToMatrix, Selec_column_of_matrix, Selec_of_matrix, Selec_of_vector,
SkewSymToVector, Stack_of_vector, Substract_of_double, Substract_of_matrix,
Substract_of_vector, UThetaToQuaternion)
from .derivator import Derivator_of_Matrix, Derivator_of_Vector
......
......@@ -29,7 +29,8 @@ def vectorToTuple(M):
# Convert from Roll, Pitch, Yaw to transformation Matrix
def rpy2tr(r, p, y):
mat = matrix(rotate('z', y)) * matrix(rotate('y', p)) * matrix(rotate('x', r))
mat = matrix(rotate('z', y)) * matrix(rotate('y', p)) * matrix(
rotate('x', r))
return matrixToTuple(mat)
......@@ -63,7 +64,8 @@ def generateOrthonormalM(v1):
e1 = e1.tolist()
e2 = e2.tolist()
e3 = e3.tolist()
M = ((e1[0][0], e2[0][0], e3[0][0]), (e1[0][1], e2[0][1], e3[0][1]), (e1[0][2], e2[0][2], e3[0][2]))
M = ((e1[0][0], e2[0][0], e3[0][0]), (e1[0][1], e2[0][1], e3[0][1]),
(e1[0][2], e2[0][2], e3[0][2]))
return M
......@@ -121,7 +123,14 @@ def rotate(axis, ang):
def quaternionToMatrix(q):
[qx, qy, qz, qw] = q
R = [[1 - 2 * qy**2 - 2 * qz**2, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw],
[2 * qx * qy + 2 * qz * qw, 1 - 2 * qx**2 - 2 * qz**2, 2 * qy * qz - 2 * qx * qw],
[2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx**2 - 2 * qy**2]]
R = [[
1 - 2 * qy**2 - 2 * qz**2, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw
], [
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx**2 - 2 * qz**2,
2 * qy * qz - 2 * qx * qw
], [
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx**2 - 2 * qy**2
]]
return R
......@@ -29,8 +29,14 @@ class MetaTask6d(object):
featureDes = 0
def opPointExist(self, opPoint):
sigsP = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == opPoint]
sigsJ = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == 'J' + opPoint]
sigsP = [
x for x in self.dyn.signals()
if x.getName().split(':')[-1] == opPoint
]
sigsJ = [
x for x in self.dyn.signals()
if x.getName().split(':')[-1] == 'J' + opPoint
]
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
......@@ -44,8 +50,12 @@ class MetaTask6d(object):
def createOpPointModif(self):
self.opPointModif = OpPointModifier('opmodif' + self.name)
plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN'))
plug(
self.dyn.signal(self.opPoint),
self.opPointModif.signal('positionIN'))
plug(
self.dyn.signal('J' + self.opPoint),
self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = False
def createFeatures(self):
......@@ -101,12 +111,16 @@ class MetaTask6d(object):
@opmodif.setter
def opmodif(self, m):
if isinstance(m, bool) and not m:
plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
plug(
self.dyn.signal(self.opPoint), self.feature.signal('position'))
plug(
self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
plug(self.opPointModif.signal('position'), self.feature.position)
plug(
self.opPointModif.signal('position'),
self.feature.position)
plug(self.opPointModif.signal('jacobian'), self.feature.Jq)
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
......@@ -17,8 +17,10 @@ class MetaTaskVisualPoint(object):
proj = 0
def opPointExist(self, opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint, self.dyn.signals())
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint,
self.dyn.signals())
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
......@@ -32,8 +34,12 @@ class MetaTaskVisualPoint(object):
def createOpPointModif(self):
self.opPointModif = OpPointModifier('opmodif' + self.name)
plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN'))
plug(
self.dyn.signal(self.opPoint),
self.opPointModif.signal('positionIN'))
plug(
self.dyn.signal('J' + self.opPoint),
self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = False
def createFeatures(self):
......@@ -99,12 +105,17 @@ class MetaTaskVisualPoint(object):
def opmodif(self, m):
if isinstance(m, bool) and not m:
plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
plug(
self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
plug(self.opPointModif.signal('position'), self.proj.signal('transfo'))
plug(self.opPointModif.signal('jacobian'), self.feature.signal('Jq'))
plug(
self.opPointModif.signal('position'),
self.proj.signal('transfo'))
plug(
self.opPointModif.signal('jacobian'),
self.feature.signal('Jq'))
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
......
......@@ -55,7 +55,8 @@ def generic6dReference(p):
M[0:3, 3] = p
elif isinstance(p, (matrix, ndarray)) and p.shape == (4, 4):
M = p
elif isinstance(p, (matrix, tuple)) and len(p) == 4 == len(p[0]) == len(p[1]) == len(p[2]) == len(p[3]):
elif isinstance(p, (matrix, tuple)) and len(p) == 4 == len(p[0]) == len(
p[1]) == len(p[2]) == len(p[3]):
M = matrix(p)
elif isinstance(p, (matrix, ndarray, tuple)) and len(p) == 6:
M = array(rpy2tr(*p[3:7]))
......@@ -70,7 +71,8 @@ def goto6d(task, position, gain=None, resetJacobian=True):
task.featureDes.position.value = matrixToTuple(M)
task.feature.selec.value = "111111"
setGain(task.gain, gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys(
) and resetJacobian:
task.task.resetJacobianDerivative()
......@@ -83,5 +85,6 @@ def gotoNd(task, position, selec=None, gain=None, resetJacobian=True):
task.feature.selec.value = toFlags(selec)
task.featureDes.position.value = matrixToTuple(M)
setGain(task.gain, gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys(
) and resetJacobian:
task.task.resetJacobianDerivative()
......@@ -19,13 +19,18 @@ class MetaTaskKine6dRel(MetaTask6d):
def createOpPointModifBase(self):
self.opPointModifBase = OpPointModifier('opmodifBase' + self.name)
plug(self.dyn.signal(self.opPointBase), self.opPointModifBase.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPointBase), self.opPointModifBase.signal('jacobianIN'))
plug(
self.dyn.signal(self.opPointBase),
self.opPointModifBase.signal('positionIN'))
plug(
self.dyn.signal('J' + self.opPointBase),
self.opPointModifBase.signal('jacobianIN'))
self.opPointModifBase.activ = False
def createFeatures(self):
self.feature = FeaturePoint6dRelative('featureRel' + self.name)
self.featureDes = FeaturePoint6dRelative('featureRel' + self.name + '_ref')
self.featureDes = FeaturePoint6dRelative(
'featureRel' + self.name + '_ref')
self.feature.selec.value = '111111'
self.feature.frame('current')
......@@ -33,8 +38,12 @@ class MetaTaskKine6dRel(MetaTask6d):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef'))
plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef'))
plug(
self.dyn.signal(self.opPointBase),
self.feature.signal('positionRef'))
plug(
self.dyn.signal('J' + self.opPointBase),
self.feature.signal('JqRef'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
......@@ -43,7 +52,13 @@ class MetaTaskKine6dRel(MetaTask6d):
self.feature.position.recompute(self.dyn.position.time)
self.feature.keep()
def __init__(self, name, dyn, opPoint, opPointBase, opPointRef='right-wrist', opPointRefBase='left-wrist'):
def __init__(self,
name,
dyn,
opPoint,
opPointBase,
opPointRef='right-wrist',
opPointRefBase='left-wrist'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
......@@ -65,13 +80,21 @@ class MetaTaskKine6dRel(MetaTask6d):
@opmodifBase.setter
def opmodifBase(self, m):
if isinstance(m, bool) and not m:
plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef'))
plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef'))
plug(
self.dyn.signal(self.opPointBase),
self.feature.signal('positionRef'))
plug(
self.dyn.signal('J' + self.opPointBase),
self.feature.signal('JqRef'))
self.opPointModifBase.activ = False
else:
if not self.opPointModifBase.activ:
plug(self.opPointModifBase.signal('position'), self.feature.positionRef)
plug(self.opPointModifBase.signal('jacobian'), self.feature.JqRef)
plug(
self.opPointModifBase.signal('position'),
self.feature.positionRef)
plug(
self.opPointModifBase.signal('jacobian'),
self.feature.JqRef)
self.opPointModifBase.setTransformation(m)
self.opPointModifBase.activ = True
......@@ -86,11 +109,17 @@ def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
task.featureDes.positionRef.value = matrixToTuple(MRef)
task.feature.selec.value = "111111"
setGain(task.gain, gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys(
) and resetJacobian:
task.task.resetJacobianDerivative()
def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=True):
def gotoNdRel(task,
position,
positionRef,
selec=None,
gain=None,
resetJacobian=True):
M = generic6dReference(position)
MRef = generic6dReference(positionRef)
if selec is not None:
......@@ -101,7 +130,8 @@ def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=
task.featureDes.position.value = matrixToTuple(M)
task.featureDes.positionRef.value = matrixToTuple(MRef)
setGain(task.gain, gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys(
) and resetJacobian:
task.task.resetJacobianDerivative()
......
......@@ -69,7 +69,8 @@ class Calendar:
self.registerEvent(iter, fun)
else: # assert iscallable(fun)
if 'functor' in fun.__dict__:
self.registerEvent(iter, (fun.functor, fun.functor.__doc__))
self.registerEvent(iter,
(fun.functor, fun.functor.__doc__))
else:
self.registerEvent(iter, (fun, fun.__doc__))
......@@ -108,6 +109,7 @@ class Calendar:
This next calling pattern is a little bit strange. Use it to decorate
a function definition: @attime(30) def run30(): ...
"""
class calendarDeco:
iterRef = iterarg
calendarRef = self
......@@ -118,9 +120,11 @@ class Calendar:
functer.__doc__ = "No doc fun"
if len(functer.__doc__) > 0:
selfdeco.__doc__ = functer.__doc__
selfdeco.__doc__ += " (will be run at time " + str(selfdeco.iterRef) + ")"
selfdeco.__doc__ += " (will be run at time " + str(
selfdeco.iterRef) + ")"
selfdeco.fun = functer
selfdeco.calendarRef.registerEvents(selfdeco.iterRef, functer, functer.__doc__)
selfdeco.calendarRef.registerEvents(selfdeco.iterRef, functer,
functer.__doc__)
def __call__(selfdeco, *args):
selfdeco.fun(*args)
......
......@@ -13,7 +13,8 @@ class History:
self.freq = freq
self.zmpSig = zmpSig
self.dynEnt = dynEnt
self.withZmp = (self.zmpSig is not None) and ("waist" in map(lambda x: x.name, self.dynEnt.signals()))
self.withZmp = (self.zmpSig is not None) and ("waist" in map(
lambda x: x.name, self.dynEnt.signals()))
def record(self):
i = self.dynEnt.position.time
......@@ -40,10 +41,13 @@ class History:
fileWaist = open(baseName + '.waist', 'w')
sampleT = 0.005
for nT, q in enumerate(self.q):
fileRPY.write(str(sampleT * nT) + ' ' + str(q[3]) + ' ' + str(q[4]) + ' ' + str(q[5]) + '\n')
fileRPY.write(
str(sampleT * nT) + ' ' + str(q[3]) + ' ' + str(q[4]) + ' ' +
str(q[5]) + '\n')
fileWaist.write(
str(sampleT * nT) + ' ' + str(q[0]) + ' ' + str(q[1]) + ' ' + str(q[2]) + ' ' + str(q[3]) + ' ' +
str(q[4]) + ' ' + str(q[5]) + '\n')
str(sampleT * nT) + ' ' + str(q[0]) + ' ' + str(q[1]) + ' ' +
str(q[2]) + ' ' + str(q[3]) + ' ' + str(q[4]) + ' ' +
str(q[5]) + '\n')
filePos.write(str(sampleT * nT) + ' ')
for j in range(6, 36):
filePos.write(str(q[j]) + ' ')
......@@ -51,7 +55,9 @@ class History:
if self.withZmp:
fileZMP = open(baseName + '.zmp', 'w')
for nT, z in enumerate(self.zmp):
fileZMP.write(str(sampleT * nT) + ' ' + str(z[0]) + ' ' + str(z[1]) + ' ' + str(z[2]) + '\n')
fileZMP.write(
str(sampleT * nT) + ' ' + str(z[0]) + ' ' + str(z[1]) +
' ' + str(z[2]) + '\n')
filePos0 = open(baseName + '_pos0.py', 'w')
filePos0.write("dyninv_posinit = '")
......
......@@ -90,7 +90,8 @@ class VisualPinger:
self.refresh()
def refresh(self):
self.viewer.updateElementConfig('pong', [0, 0.9, self.pos * 0.1, 0, 0, 0])
self.viewer.updateElementConfig('pong',
[0, 0.9, self.pos * 0.1, 0, 0, 0])
def __call__(self):
self.pos += 1
......@@ -99,4 +100,5 @@ class VisualPinger:
def updateComDisplay(robot, comsig, objname='com'):
if comsig.time > 0:
robot.viewer.updateElementConfig(objname, [comsig.value[0], comsig.value[1], 0, 0, 0, 0])
robot.viewer.updateElementConfig(
objname, [comsig.value[0], comsig.value[1], 0, 0, 0, 0])
......@@ -12,6 +12,7 @@ class ViewerLoger:
robot.viewer = ViewerLoger(robot)
'''
def __init__(self, robot):
self.robot = robot
self.viewer = robot.viewer
......@@ -23,7 +24,8 @@ class ViewerLoger:
t = self.robot.state.time
if name not in self.fileMap:
self.fileMap[name] = open('/tmp/view_' + name + '.dat', 'w')
self.fileMap[name].write("\t".join([str(f) for f in [
t,
] + list(state)]) + '\n')
self.fileMap[name].write(
"\t".join([str(f) for f in [
t,
] + list(state)]) + '\n')
self.viewer.updateElementConfig(name, state)
......@@ -11,8 +11,10 @@ class OpPointModifierTest(unittest.TestCase):
ent = ie.IntegratorEulerVectorDouble("ie")
with self.assertRaises(dgpy.dgpyError) as cm:
ent.initialize()
self.assertEqual(str(cm.exception),
'In SignalPtr: SIN ptr not set. (in signal <sotIntegratorAbstract(ie)::input(vector)::sin>)')
self.assertEqual(
str(cm.exception),
'In SignalPtr: SIN ptr not set. (in signal <sotIntegratorAbstract(ie)::input(vector)::sin>)'
)
if __name__ == '__main__':
......
......@@ -23,9 +23,12 @@ class MatrixUtilTest(unittest.TestCase):
def test_rpy_tr(self):
for rpy, tr in [
((0, 0, 0), mod.matrixToTuple(np.identity(4))),
((np.pi, 0, 0), ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
((0, np.pi, 0), ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
((0, 0, np.pi), ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))),
((np.pi, 0, 0), ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0),
(0, 0, 0, 1))),
((0, np.pi, 0), ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0),
(0, 0, 0, 1))),
((0, 0, np.pi), ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0),
(0, 0, 0, 1))),
]:
np.testing.assert_allclose(tr, mod.rpy2tr(*rpy), atol=1e-15)
# np.testing.assert_allclose(rpy, mod.tr2rpy(tr), atol=1e-15)
......@@ -39,14 +42,18 @@ class MatrixUtilTest(unittest.TestCase):
]:
np.testing.assert_allclose(rpy, mod.matrixToRPY(mat))
# np.testing.assert_allclose(mat, mod.RPYToMatrix(rpy))
np.testing.assert_allclose(rpy, mod.matrixToRPY(mod.RPYToMatrix(rpy)))
np.testing.assert_allclose(rpy,
mod.matrixToRPY(mod.RPYToMatrix(rpy)))
# np.testing.assert_allclose(mat, mod.RPYToMatrix(mod.matrixToRPY(mat)))
def test_rotate(self):
for axis, angle, mat in [
('x', np.pi, ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
('y', np.pi, ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
('z', np.pi, ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))),
('x', np.pi, ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0),
(0, 0, 0, 1))),
('y', np.pi, ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0),
(0, 0, 0, 1))),
('z', np.pi, ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0),
(0, 0, 0, 1))),
]:
self.assertEqual(mat, mod.rotate(axis, angle))
......@@ -54,7 +61,8 @@ class MatrixUtilTest(unittest.TestCase):
for quat, mat in [
((0, 0, 0, 1), np.identity(3)),
((0, 0, 1, 0), ((-1, 0, 0), (0, -1, 0), (0, 0, 1))),
((0, -0.5, 0, 0.5), ((0.5, 0, -0.5), (0, 1, 0), (0.5, 0, 0.5))),
((0, -0.5, 0, 0.5), ((0.5, 0, -0.5), (0, 1, 0), (0.5, 0,
0.5))),
]:
self.assertEqual(mat, mod.quaternionToMatrix(quat))
......
......@@ -7,17 +7,20 @@ import unittest
import numpy as np
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
gaze = tuple(((1.0, 0.0, 0.0, 0.025000000000000001), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.64800000000000002),
(0.0, 0.0, 0.0, 1.0)))
gaze = tuple(((1.0, 0.0, 0.0, 0.025000000000000001), (0.0, 1.0, 0.0, 0.0),
(0.0, 0.0, 1.0, 0.64800000000000002), (0.0, 0.0, 0.0, 1.0)))
Jgaze = tuple(
((1.0, 0.0, 0.0, 0.0, 0.64800000000000002, 0.0), (0.0, 1.0, 0.0, -0.64800000000000002, 0.0, 0.025000000000000001),
(0.0, 0.0, 1.0, 0.0, -0.025000000000000001,
0.0), (0.0, 0.0, 0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 0.0, 0.0, 1.0, 0.0), (0.0, 0.0, 0.0, 0.0, 0.0, 1.0)))
((1.0, 0.0, 0.0, 0.0, 0.64800000000000002,
0.0), (0.0, 1.0, 0.0, -0.64800000000000002, 0.0,
0.025000000000000001), (0.0, 0.0, 1.0, 0.0, -0.025000000000000001,
0.0), (0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
(0.0, 0.0, 0.0, 0.0, 1.0, 0.0), (0.0, 0.0, 0.0, 0.0, 0.0, 1.0)))
I4 = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., 0.), (0., 0., 0., 1.))
I6 = ((1., 0., 0., 0., 0., 0.), (0., 1., 0., 0., 0., 0.), (0., 0., 1., 0., 0., 0.), (0., 0., 0., 1., 0., 0.),
I6 = ((1., 0., 0., 0., 0., 0.), (0., 1., 0., 0., 0., 0.),
(0., 0., 1., 0., 0., 0.), (0., 0., 0., 1., 0., 0.),
(0., 0., 0., 0., 1., 0.), (0., 0., 0., 0., 0., 1.))
......@@ -39,7 +42,8 @@ class OpPointModifierTest(unittest.TestCase):
ty = 22.
tz = 33.
T = ((1., 0., 0., tx), (0., 1., 0., ty), (0., 0., 1., tz), (0., 0., 0., 1.))
T = ((1., 0., 0., tx), (0., 1., 0., ty), (0., 0., 1., tz), (0., 0., 0.,
1.))
op = OpPointModifier('op2')
op.setTransformation(T)
......@@ -59,8 +63,10 @@ class OpPointModifierTest(unittest.TestCase):
# Check w_M_s == w_M_s_ref
self.assertEqual(np.equal(w_M_s, w_M_s_ref).all(), True)
twist = np.matrix([[1., 0., 0., 0., tz, -ty], [0., 1., 0., -tz, 0., tx], [0., 0., 1., ty, -tx, 0.],
[0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 1.]])
twist = np.matrix([[1., 0., 0., 0., tz,
-ty], [0., 1., 0., -tz, 0., tx],
[0., 0., 1., ty, -tx, 0.], [0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 1.]])
J = np.asmatrix(op.jacobian.value)
J_ref = twist * Jgaze
......@@ -69,7 +75,8 @@ class OpPointModifierTest(unittest.TestCase):
self.assertEqual(np.equal(J, J_ref).all(), True)
def test_rotation(self):
T = ((0., 0., 1., 0.), (0., -1., 0., 0.), (1., 0., 0., 0.), (0., 0., 0., 1.))
T = ((0., 0., 1., 0.), (0., -1., 0., 0.), (1., 0., 0., 0.), (0., 0.,