Commit aebbf0b8 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python] add Flags()

parent 4dfb5beb
......@@ -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 @@ class FeaturePosition(Entity):
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
......
......@@ -7,12 +7,11 @@ from functools import reduce
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 @@ class FeaturePositionRelative(Entity):
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
......
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 @@ 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):
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 @@ 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
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,10 +70,9 @@ 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()
......@@ -80,11 +80,10 @@ 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()
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 @@ 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.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 @@ 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)
......@@ -80,21 +65,13 @@ 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
......@@ -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()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment