[Python] add Flags()

nim65s · nim65s · commit aebbf0b813cf · 2020-09-27T11:08:16.000+02:00
diff --git a/src/dynamic_graph/sot/core/feature_position.py b/src/dynamic_graph/sot/core/feature_position.py
@@ -3,6 +3,7 @@
 #
 
 from dynamic_graph import plug
+from dynamic_graph.sot.core import Flags
 from dynamic_graph.entity import Entity
 from dynamic_graph.sot.core.feature_point6d import FeaturePoint6d
 
@@ -42,7 +43,7 @@ def __init__(self,
         if signalJacobian:
             plug(signalJacobian, self._feature.signal('Jq'))
         self._feature.setReference(self._reference.name)
-        self._feature.signal('selec').value = '111111'
+        self._feature.signal('selec').value = Flags('111111')
         self._feature.frame('current')
 
         # Signals stored in members
diff --git a/src/dynamic_graph/sot/core/feature_position_relative.py b/src/dynamic_graph/sot/core/feature_position_relative.py
@@ -7,12 +7,11 @@
 from dynamic_graph import plug
 from dynamic_graph.entity import Entity
 from dynamic_graph.signal_base import SignalBase
-from dynamic_graph.sot.core.feature_point6d_relative \
-    import FeaturePoint6dRelative
+from dynamic_graph.sot.core import Flags
+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):
@@ -99,7 +98,7 @@ def __init__(self,
         if JqOther:
             plug(JqOther, self._feature.signal('Jq'))
         self._feature.setReference(self._reference.name)
-        self._feature.signal('selec').value = '111111'
+        self._feature.signal('selec').value = Flags('111111')
         self._feature.frame('current')
 
         # Signals stored in members
diff --git a/src/dynamic_graph/sot/core/meta_task_6d.py b/src/dynamic_graph/sot/core/meta_task_6d.py
@@ -1,4 +1,5 @@
 from dynamic_graph import plug
+from dynamic_graph.sot.core import Flags
 from dynamic_graph.sot.core.feature_point6d import FeaturePoint6d
 from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
 from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
@@ -18,7 +19,7 @@ def toFlags(arr):
     res = ''
     for i in lres:
         res += str(i)
-    return res
+    return Flags(res)
 
 
 class MetaTask6d(object):
@@ -30,14 +31,8 @@ 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):
@@ -51,18 +46,14 @@ 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):
         self.feature = FeaturePoint6d('feature' + self.name)
         self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
-        self.feature.selec.value = '111111'
+        self.feature.selec.value = Flags('111111')
         self.feature.frame('current')
 
     def createTask(self):
@@ -112,16 +103,12 @@ def opmodif(self):
     @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_tasks.py b/src/dynamic_graph/sot/core/meta_tasks.py
@@ -1,9 +1,11 @@
+from numpy import array, eye, matrix, ndarray
+
 from dynamic_graph import plug
+from dynamic_graph.sot.core import Flags
 from dynamic_graph.sot.core.feature_generic import FeatureGeneric
 from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
 from dynamic_graph.sot.core.matrix_util import matrixToTuple, rpy2tr
 from dynamic_graph.sot.core.meta_task_6d import toFlags
-from numpy import array, eye, matrix, ndarray
 
 
 class MetaTaskCom(object):
@@ -55,8 +57,7 @@ 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]))
@@ -69,22 +70,20 @@ def generic6dReference(p):
 def goto6d(task, position, gain=None, resetJacobian=True):
     M = generic6dReference(position)
     task.featureDes.position.value = array(M)
-    task.feature.selec.value = "111111"
+    task.feature.selec.value = Flags("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 gotoNd(task, position, selec=None, gain=None, resetJacobian=True):
     M = generic6dReference(position)
     if selec is not None:
         if isinstance(selec, str):
-            task.feature.selec.value = selec
+            task.feature.selec.value = Flags(selec)
         else:
             task.feature.selec.value = toFlags(selec)
     task.featureDes.position.value = array(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
@@ -1,6 +1,6 @@
 from dynamic_graph import plug
-from dynamic_graph.sot.core.feature_point6d_relative import \
-    FeaturePoint6dRelative
+from dynamic_graph.sot.core import Flags
+from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
 from dynamic_graph.sot.core.matrix_util import matrixToTuple
 from dynamic_graph.sot.core.meta_task_6d import MetaTask6d, toFlags
 from dynamic_graph.sot.core.meta_tasks import generic6dReference, setGain
@@ -19,31 +19,22 @@ def createOpPointBase(self, opPointBase, opPointRefBase='left-wrist'):
 
     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.feature.selec.value = '111111'
+        self.featureDes = FeaturePoint6dRelative('featureRel' + self.name + '_ref')
+        self.feature.selec.value = Flags('111111')
         self.feature.frame('current')
 
     def plugEverything(self):
         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)
@@ -52,13 +43,7 @@ def keep(self):
         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)
@@ -80,21 +65,13 @@ def opmodifBase(self):
     @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
 
@@ -107,19 +84,13 @@ def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
     MRef = generic6dReference(positionRef)
     task.featureDes.position.value = matrixToTuple(M)
     task.featureDes.positionRef.value = matrixToTuple(MRef)
-    task.feature.selec.value = "111111"
+    task.feature.selec.value = Flags("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:
@@ -130,8 +101,7 @@ def gotoNdRel(task,
     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()
 
 
