Skip to content

Commit

Permalink
yapf
Browse files Browse the repository at this point in the history
  • Loading branch information
@nim65s
nim65s committed Feb 9, 2021
1 parent cae456d commit cf574bd
Show file tree
Hide file tree
Showing 10 changed files with 49 additions and 105 deletions.
6 changes: 1 addition & 5 deletions src/dynamic_graph/sot/core/feature_position.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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')
Expand Down
28 changes: 11 additions & 17 deletions src/dynamic_graph/sot/core/math_small_entities.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
19 changes: 5 additions & 14 deletions src/dynamic_graph/sot/core/matrix_util.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)


Expand Down Expand Up @@ -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


Expand Down Expand Up @@ -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
25 changes: 7 additions & 18 deletions src/dynamic_graph/sot/core/meta_task_visual_point.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -34,12 +32,8 @@ def createOpPoint(self, opPoint, opPointRef='right-wrist'):

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):
Expand Down Expand Up @@ -105,17 +99,12 @@ def opmodif(self):
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

Expand Down
10 changes: 3 additions & 7 deletions src/dynamic_graph/sot/core/utils/attime.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,8 +69,7 @@ def registerEvents(self, iter, *funs):
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__))

Expand Down Expand Up @@ -109,7 +108,6 @@ def generatorDecorator(self, iterarg):
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
Expand All @@ -120,11 +118,9 @@ def __init__(selfdeco, functer):
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)
Expand Down
16 changes: 5 additions & 11 deletions src/dynamic_graph/sot/core/utils/history.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,7 @@ def __init__(self, dynEnt, freq=100, zmpSig=None):
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
Expand All @@ -41,23 +40,18 @@ def dumpToOpenHRP(self, baseName="dyninv", sample=1):
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]) + ' ')
filePos.write(10 * ' 0' + '\n')
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 = '")
Expand Down
6 changes: 2 additions & 4 deletions src/dynamic_graph/sot/core/utils/viewer_helper.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,8 +91,7 @@ def __init__(self, viewer):
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
Expand All @@ -101,5 +100,4 @@ def __call__(self):

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])
8 changes: 3 additions & 5 deletions src/dynamic_graph/sot/core/utils/viewer_loger.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@ class ViewerLoger:
robot.viewer = ViewerLoger(robot)
'''

def __init__(self, robot):
self.robot = robot
self.viewer = robot.viewer
Expand All @@ -24,8 +23,7 @@ def updateElementConfig(self, name, state):
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)
24 changes: 8 additions & 16 deletions tests/python/matrix-util.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,12 +23,9 @@ def test_vector_to_tuple(self):
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)

Expand All @@ -39,26 +36,21 @@ def test_matrix_rpy(self):
]:
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))

def test_quat_mat(self):
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))

Expand Down
12 changes: 4 additions & 8 deletions tests/python/test-parameter-server.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,7 @@
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
Expand All @@ -35,17 +34,14 @@ def test_set_parameter(self):
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)


Expand Down

0 comments on commit cf574bd

Please sign in to comment.