yapf

Author: nim65s

src/dynamic_graph/sot/core/feature_position.py

@@ -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')

src/dynamic_graph/sot/core/math_small_entities.py

@@ -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

src/dynamic_graph/sot/core/matrix_util.py

@@ -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

src/dynamic_graph/sot/core/meta_task_visual_point.py

@@ -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 @@ 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):
@@ -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
 

src/dynamic_graph/sot/core/utils/attime.py

@@ -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__))
 
@@ -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
@@ -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)

src/dynamic_graph/sot/core/utils/history.py

@@ -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
@@ -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 = '")

src/dynamic_graph/sot/core/utils/viewer_helper.py

@@ -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
@@ -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])

src/dynamic_graph/sot/core/utils/viewer_loger.py

@@ -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 @@ 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)

tests/python/matrix-util.py

@@ -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)
 
@@ -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))
 

tests/python/test-parameter-server.py

@@ -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
@@ -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)