diff --git a/src/dynamic_graph/sot/core/feature_position.py b/src/dynamic_graph/sot/core/feature_position.py index 80f740c3e..36d6bbabb 100644 --- a/src/dynamic_graph/sot/core/feature_position.py +++ b/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') diff --git a/src/dynamic_graph/sot/core/math_small_entities.py b/src/dynamic_graph/sot/core/math_small_entities.py index 06e464e05..947966593 100644 --- a/src/dynamic_graph/sot/core/math_small_entities.py +++ b/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 diff --git a/src/dynamic_graph/sot/core/matrix_util.py b/src/dynamic_graph/sot/core/matrix_util.py index 588108c0e..2f905562c 100644 --- a/src/dynamic_graph/sot/core/matrix_util.py +++ b/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 diff --git a/src/dynamic_graph/sot/core/meta_task_visual_point.py b/src/dynamic_graph/sot/core/meta_task_visual_point.py index bf295548a..ec938df51 100644 --- a/src/dynamic_graph/sot/core/meta_task_visual_point.py +++ b/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 diff --git a/src/dynamic_graph/sot/core/utils/attime.py b/src/dynamic_graph/sot/core/utils/attime.py index 13a35789d..dc8047742 100644 --- a/src/dynamic_graph/sot/core/utils/attime.py +++ b/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) diff --git a/src/dynamic_graph/sot/core/utils/history.py b/src/dynamic_graph/sot/core/utils/history.py index 3a6698efe..c098b50e5 100644 --- a/src/dynamic_graph/sot/core/utils/history.py +++ b/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,13 +40,10 @@ 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]) + ' ') @@ -55,9 +51,7 @@ def dumpToOpenHRP(self, baseName="dyninv", sample=1): 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 = '") diff --git a/src/dynamic_graph/sot/core/utils/viewer_helper.py b/src/dynamic_graph/sot/core/utils/viewer_helper.py index e102ec868..2215cb059 100644 --- a/src/dynamic_graph/sot/core/utils/viewer_helper.py +++ b/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]) diff --git a/src/dynamic_graph/sot/core/utils/viewer_loger.py b/src/dynamic_graph/sot/core/utils/viewer_loger.py index cf5c1bd5d..39ef50f48 100644 --- a/src/dynamic_graph/sot/core/utils/viewer_loger.py +++ b/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) diff --git a/tests/python/matrix-util.py b/tests/python/matrix-util.py index e3c5312a0..40ebdf4cf 100644 --- a/tests/python/matrix-util.py +++ b/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,17 +36,13 @@ 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)) @@ -57,8 +50,7 @@ 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)) diff --git a/tests/python/test-parameter-server.py b/tests/python/test-parameter-server.py index 402be6908..71340dd1c 100644 --- a/tests/python/test-parameter-server.py +++ b/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)