Commit cf574bd3 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

yapf

parent cae456d1
......@@ -28,11 +28,7 @@ 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')
......
......@@ -6,23 +6,17 @@
# 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,
PoseQuaternionToMatrixHomo, MatrixHomoToPoseRollPitchYaw,
MatrixHomoToPoseUTheta, MatrixToHomo, MatrixToQuaternion, MatrixToRPY,
MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo,
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)
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,
PoseQuaternionToMatrixHomo, MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo, MatrixToQuaternion,
MatrixToRPY, MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo, 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
from .matrix_constant import MatrixConstant
......
......@@ -29,8 +29,7 @@ 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)
......@@ -64,8 +63,7 @@ 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
......@@ -123,14 +121,7 @@ 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
......@@ -17,10 +17,8 @@ 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):
......@@ -34,12 +32,8 @@ 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):
......@@ -105,17 +99,12 @@ 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
......
......@@ -69,8 +69,7 @@ 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__))
......@@ -109,7 +108,6 @@ 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
......@@ -120,11 +118,9 @@ 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,8 +13,7 @@ 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
......@@ -41,13 +40,10 @@ 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]) + ' ')
......@@ -55,9 +51,7 @@ 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 = '")
......
......@@ -91,8 +91,7 @@ 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
......@@ -101,5 +100,4 @@ 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,7 +12,6 @@ class ViewerLoger:
robot.viewer = ViewerLoger(robot)
'''
def __init__(self, robot):
self.robot = robot
self.viewer = robot.viewer
......@@ -24,8 +23,7 @@ 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)
......@@ -23,12 +23,9 @@ 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)
......@@ -39,17 +36,13 @@ 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)))
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))
......@@ -57,8 +50,7 @@ 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))
......
......@@ -22,8 +22,7 @@ urdfDir = param_server_conf.model_path
class TestParameterServer(unittest.TestCase):
def test_set_parameter(self):
# Read talos model
path = join(
dirname(dirname(abspath(__file__))), 'models', 'others', 'python')
path = join(dirname(dirname(abspath(__file__))), 'models', 'others', 'python')
sys.path.append(path)
from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR
......@@ -35,17 +34,14 @@ class TestParameterServer(unittest.TestCase):
urdf_rrbot_model_string = fs.read()
fs.close()
param_server.setParameter("/robot_description",
urdf_rrbot_model_string)
param_server.setParameter("/robot_description", urdf_rrbot_model_string)
model2_string = param_server.getParameter("/robot_description")
self.assertEqual(urdf_rrbot_model_string, model2_string)
aValue = 0.122
param_server.setParameterDbl("/specificities/feet/right/size/height",
aValue)
a2Value = param_server.getParameterDbl(
"/specificities/feet/right/size/height")
param_server.setParameterDbl("/specificities/feet/right/size/height", aValue)
a2Value = param_server.getParameterDbl("/specificities/feet/right/size/height")
self.assertEqual(aValue, a2Value)
......
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