diff --git a/src/dynamic_graph/sot/core/feature_position.py b/src/dynamic_graph/sot/core/feature_position.py
index c873f19efb3c2c3cfd42b199cddeda0163e2f0e1..c880613dff8af346d7e3ab3dc0185acd85ad1e92 100644
--- a/src/dynamic_graph/sot/core/feature_position.py
+++ b/src/dynamic_graph/sot/core/feature_position.py
@@ -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')
diff --git a/src/dynamic_graph/sot/core/feature_position_relative.py b/src/dynamic_graph/sot/core/feature_position_relative.py
index 028507b8ca9bb572af66c97e3aaf5d4edb122cc8..a220fb1066f4929c4bf0d9b04e1d1a5cbd9fc430 100644
--- a/src/dynamic_graph/sot/core/feature_position_relative.py
+++ b/src/dynamic_graph/sot/core/feature_position_relative.py
@@ -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):
diff --git a/src/dynamic_graph/sot/core/math_small_entities.py b/src/dynamic_graph/sot/core/math_small_entities.py
index a96aa47dcc36c8680ea3950e37e8f4179b3abd56..e371f9d6d15ab4fc7067970196212701f6fc7e04 100644
--- a/src/dynamic_graph/sot/core/math_small_entities.py
+++ b/src/dynamic_graph/sot/core/math_small_entities.py
@@ -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
diff --git a/src/dynamic_graph/sot/core/matrix_util.py b/src/dynamic_graph/sot/core/matrix_util.py
index 2f905562cd404e376a394e3bf3ec0e5346306868..588108c0e388192d927b08e803a6f0e8445f9064 100644
--- a/src/dynamic_graph/sot/core/matrix_util.py
+++ b/src/dynamic_graph/sot/core/matrix_util.py
@@ -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
diff --git a/src/dynamic_graph/sot/core/meta_task_6d.py b/src/dynamic_graph/sot/core/meta_task_6d.py
index 3e74db293c3796deb5186b5185fb33ac669b3d98..05dad08c8e03cdd0a28c4e66f3ad3a831224fc22 100644
--- a/src/dynamic_graph/sot/core/meta_task_6d.py
+++ b/src/dynamic_graph/sot/core/meta_task_6d.py
@@ -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
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 24befbbbee495ddfd9680300a05dae4c8f3749a7..196c755c19cc4be8663089890a2cb95dc66acc4d 100644
--- a/src/dynamic_graph/sot/core/meta_task_visual_point.py
+++ b/src/dynamic_graph/sot/core/meta_task_visual_point.py
@@ -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
diff --git a/src/dynamic_graph/sot/core/meta_tasks.py b/src/dynamic_graph/sot/core/meta_tasks.py
index d261a193ceb03bbda272b1100fb7b5bcccd8b515..95dd52a7fbbcb370e9fae60862b95622b3888726 100644
--- a/src/dynamic_graph/sot/core/meta_tasks.py
+++ b/src/dynamic_graph/sot/core/meta_tasks.py
@@ -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()
diff --git a/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py b/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
index db7decec194d4623c372ef1668c39574345b32d8..0367f8ed555936d5418b4f503a13b8236b8f2e05 100644
--- a/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
+++ b/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
@@ -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()
diff --git a/src/dynamic_graph/sot/core/utils/attime.py b/src/dynamic_graph/sot/core/utils/attime.py
index dc8047742d9ec26f01bfbcb9a1fc6d10e7896d5e..13a35789d62ec79507e1b66222034bac114ce15a 100644
--- a/src/dynamic_graph/sot/core/utils/attime.py
+++ b/src/dynamic_graph/sot/core/utils/attime.py
@@ -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)
diff --git a/src/dynamic_graph/sot/core/utils/history.py b/src/dynamic_graph/sot/core/utils/history.py
index c098b50e56b82eeb2dc740e76290a5775a3a5953..3a6698efea518f6687afc6b424e06d2d6c9a50fb 100644
--- a/src/dynamic_graph/sot/core/utils/history.py
+++ b/src/dynamic_graph/sot/core/utils/history.py
@@ -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 = '")
diff --git a/src/dynamic_graph/sot/core/utils/viewer_helper.py b/src/dynamic_graph/sot/core/utils/viewer_helper.py
index ca0fe12fcd8c108fd5a08b8582e7a3e07e3f2277..7dd41e8dc645b6a44b52f638f14b616f3d1221fe 100644
--- a/src/dynamic_graph/sot/core/utils/viewer_helper.py
+++ b/src/dynamic_graph/sot/core/utils/viewer_helper.py
@@ -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])
diff --git a/src/dynamic_graph/sot/core/utils/viewer_loger.py b/src/dynamic_graph/sot/core/utils/viewer_loger.py
index 39ef50f48ed575e1f1467357271add9dcd6de51c..cf5c1bd5d33a0847c14444bfd73a25e5dc8414c8 100644
--- a/src/dynamic_graph/sot/core/utils/viewer_loger.py
+++ b/src/dynamic_graph/sot/core/utils/viewer_loger.py
@@ -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)
diff --git a/tests/python/initialize-euler.py b/tests/python/initialize-euler.py
index 53a3e532f4e3c7917b5e65196d002cfeef06c76a..609a0093697afe4b34b20129ed10c4edd9cd6579 100755
--- a/tests/python/initialize-euler.py
+++ b/tests/python/initialize-euler.py
@@ -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__':
diff --git a/tests/python/matrix-util.py b/tests/python/matrix-util.py
index 25e1fbc4e735313470cdc9c1ba56e8446e25a84f..860f5a9958f0b6741a47395d3e2f963b9f1b311f 100644
--- a/tests/python/matrix-util.py
+++ b/tests/python/matrix-util.py
@@ -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))
diff --git a/tests/python/op-point-modifier.py b/tests/python/op-point-modifier.py
index 3a13f680a0b8471ba626d9428d9bfecab26c922c..b73953bfa7c824fa8da46c2ceaafb2a136cd5f01 100755
--- a/tests/python/op-point-modifier.py
+++ b/tests/python/op-point-modifier.py
@@ -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.,
+ 0., 1.))
op = OpPointModifier('op3')
op.setTransformation(T)
@@ -89,8 +96,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([[0., 0., 1., 0., 0., 0.], [0., -1., 0., 0., 0., 0.], [1., 0., 0., 0., 0., 0.],
- [0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., -1., 0.], [0., 0., 0., 1., 0., 0.]])
+ twist = np.matrix([[0., 0., 1., 0., 0., 0.], [0., -1., 0., 0., 0., 0.],
+ [1., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 1.],
+ [0., 0., 0., 0., -1., 0.], [0., 0., 0., 1., 0.,
+ 0.]])
J = np.asmatrix(op.jacobian.value)
J_ref = twist * Jgaze
@@ -103,7 +112,8 @@ class OpPointModifierTest(unittest.TestCase):
ty = 22.
tz = 33.
- T = ((0., 0., 1., tx), (0., -1., 0., ty), (1., 0., 0., tz), (0., 0., 0., 1.))
+ T = ((0., 0., 1., tx), (0., -1., 0., ty), (1., 0., 0., tz), (0., 0.,
+ 0., 1.))
op = OpPointModifier('op4')
op.setTransformation(T)
@@ -123,8 +133,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([[0., 0., 1., ty, -tx, 0.], [0., -1., 0., tz, 0., -tx], [1., 0., 0., 0., tz, -ty],
- [0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., -1., 0.], [0., 0., 0., 1., 0., 0.]])
+ twist = np.matrix(
+ [[0., 0., 1., ty, -tx, 0.], [0., -1., 0., tz, 0., -tx],
+ [1., 0., 0., 0., tz, -ty], [0., 0., 0., 0., 0., 1.],
+ [0., 0., 0., 0., -1., 0.], [0., 0., 0., 1., 0., 0.]])
J = np.asmatrix(op.jacobian.value)
J_ref = twist * Jgaze
diff --git a/tests/python/parameter_server_conf.py b/tests/python/parameter_server_conf.py
index b72ab6d3228fbc636706b25b95c00b713e7cdee6..ecfde29bbffa87350ec8a4ccf64e564cdbcd73ef 100644
--- a/tests/python/parameter_server_conf.py
+++ b/tests/python/parameter_server_conf.py
@@ -18,10 +18,9 @@ mapJointLimits = {
vfMax = [100.0, 100.0]
vfMin = [-100.0, -100.0]
-mapForceIdToForceLimits = {0: [vfMin, vfMax],
- 1: [vfMin, vfMax]}
+mapForceIdToForceLimits = {0: [vfMin, vfMax], 1: [vfMin, vfMax]}
-mapNameToForceId = {"rf": 0, "lf": 1 }
+mapNameToForceId = {"rf": 0, "lf": 1}
indexOfForceSensors = ()
diff --git a/tests/python/test-parameter-server.py b/tests/python/test-parameter-server.py
index fcb46b9d58b51b0688ce4efa7a820423d3f6b370..f9065002d649f18b8d81ee40c48402fccc326a5d 100644
--- a/tests/python/test-parameter-server.py
+++ b/tests/python/test-parameter-server.py
@@ -10,27 +10,25 @@ import parameter_server_conf as param_server_conf
from dynamic_graph.sot.core.parameter_server import ParameterServer
param_server = ParameterServer("param_server")
-param_server.init(0.001,"talos.urdf","talos")
+param_server.init(0.001, "talos.urdf", "talos")
# Control time interval
-dt=0.001
+dt = 0.001
robot_name = 'robot'
-
-
-urdfPath= param_server_conf.urdfFileName
-urdfDir= param_server_conf.model_path
+urdfPath = param_server_conf.urdfFileName
+urdfDir = param_server_conf.model_path
import sys
from os.path import dirname, join, abspath
-class TestParameterServer(unittest.TestCase):
+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
@@ -38,28 +36,29 @@ class TestParameterServer(unittest.TestCase):
urdf_file_name=EXAMPLE_ROBOT_DATA_MODEL_DIR+\
'/talos_data/robots/talos_reduced.urdf'
- fs=open(urdf_file_name,'r')
- urdf_rrbot_model_string=fs.read()
+ fs = open(urdf_file_name, 'r')
+ urdf_rrbot_model_string = fs.read()
fs.close()
print(EXAMPLE_ROBOT_DATA_MODEL_DIR)
#
model = pin.buildModelFromXML(urdf_rrbot_model_string)
- neutral_configuration_model=pin.neutral(model)
- jointIds=[]
+ neutral_configuration_model = pin.neutral(model)
+ jointIds = []
#print(model)
- reduced_model=pin.buildReducedModel(model, jointIds,
- neutral_configuration_model)
+ reduced_model = pin.buildReducedModel(model, jointIds,
+ neutral_configuration_model)
data = model.createData()
com = pin.centerOfMass(model, data, neutral_configuration_model)
pin.updateFramePlacements(model, data)
m = data.mass[0]
- param_server.setParameter("urdf_model",urdf_rrbot_model_string)
- model2_string=param_server.getParameter("urdf_model")
+ param_server.setParameter("urdf_model", urdf_rrbot_model_string)
+ model2_string = param_server.getParameter("urdf_model")
self.assertEqual(urdf_rrbot_model_string, model2_string)
+
if __name__ == '__main__':
unittest.main()