def test_rotation(self):
T = np.array(((0., 0., 1., 0.), (0., -1., 0., 0.), (1., 0., 0., 0.),
(0., 0., 0., 1.)))
op = OpPointModifier('op3')
op.setTransformation(T)
op.positionIN.value = gaze
op.jacobianIN.value = Jgaze
op.position.recompute(1)
op.jacobian.recompute(1)
self.assertTrue((op.getTransformation() == T).all())
# w_M_s = w_M_g * g_M_s
w_M_g = gaze
g_M_s = T
w_M_s_ref = w_M_g.dot(g_M_s)
w_M_s = op.position.value
# Check w_M_s == w_M_s_ref
self.assertTrue((w_M_s == w_M_s_ref).all())
twist = np.array([[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 = op.jacobian.value
J_ref = twist.dot(Jgaze)
# Check w_M_s == w_M_s_ref
self.assertTrue((J == J_ref).all())
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'))
self.opPointModifBase.activ = False
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'))
self.opPointModif.activ = False
def test_rotation(self):
T = ((0., 0., 1., 0.), (0., -1., 0., 0.), (1., 0., 0., 0.), (0., 0.,
0., 1.))
op = OpPointModifier('op3')
op.setTransformation(T)
op.positionIN.value = gaze
op.jacobianIN.value = Jgaze
op.position.recompute(1)
op.jacobian.recompute(1)
self.assertEqual(op.getTransformation(), T)
# w_M_s = w_M_g * g_M_s
w_M_g = np.asmatrix(gaze)
g_M_s = np.asmatrix(T)
w_M_s_ref = w_M_g * g_M_s
w_M_s = np.asmatrix(op.position.value)
# 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.]])
J = np.asmatrix(op.jacobian.value)
J_ref = twist * Jgaze
# Check w_M_s == w_M_s_ref
self.assertEqual(np.equal(J, J_ref).all(), True)
def test_translation(self):
tx = 11.
ty = 22.
tz = 33.
T = np.array(((1., 0., 0., tx), (0., 1., 0., ty), (0., 0., 1., tz),
(0., 0., 0., 1.)))
op = OpPointModifier('op2')
op.setTransformation(T)
op.positionIN.value = gaze
op.jacobianIN.value = Jgaze
op.position.recompute(1)
op.jacobian.recompute(1)
self.assertTrue((op.getTransformation() == T).all())
# w_M_s = w_M_g * g_M_s
w_M_g = gaze
g_M_s = T
w_M_s_ref = w_M_g.dot(g_M_s)
w_M_s = op.position.value
# Check w_M_s == w_M_s_ref
self.assertTrue((w_M_s == w_M_s_ref).all())
twist = np.array([[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 = op.jacobian.value
J_ref = twist.dot(Jgaze)
# Check w_M_s == w_M_s_ref
self.assertTrue((J == J_ref).all())
def createFrame(self, frameName, transformation, operationalPoint):
frame = OpPointModifier(frameName)
frame.setTransformation(transformation)
plug(self.dynamic.signal(operationalPoint), frame.positionIN)
plug(self.dynamic.signal("J{0}".format(operationalPoint)), frame.jacobianIN)
frame.position.recompute(frame.position.time + 1)
frame.jacobian.recompute(frame.jacobian.time + 1)
return frame
def test_simple(self):
op = OpPointModifier('op')
op.setTransformation(I4)
op.positionIN.value = I4
op.jacobianIN.value = I6
op.position.recompute(0)
op.jacobian.recompute(0)
self.assertEqual(op.getTransformation(), I4)
self.assertEqual(op.position.value, I4)
self.assertEqual(op.jacobian.value, I6)
def test_simple(self):
op = OpPointModifier('op')
op.setTransformation(I4)
op.positionIN.value = I4
op.jacobianIN.value = I6
op.position.recompute(0)
op.jacobian.recompute(0)
self.assertTrue((op.getTransformation() == I4).all())
self.assertTrue((op.position.value == I4).all())
self.assertTrue((op.jacobian.value == I6).all())
def test_translation(self):
tx = 11.
ty = 22.
tz = 33.
T = ((1., 0., 0., tx), (0., 1., 0., ty), (0., 0., 1., tz), (0., 0., 0.,
1.))
op = OpPointModifier('op2')
op.setTransformation(T)
op.positionIN.value = gaze
op.jacobianIN.value = Jgaze
op.position.recompute(1)
op.jacobian.recompute(1)
self.assertEqual(op.getTransformation(), T)
# w_M_s = w_M_g * g_M_s
w_M_g = np.asmatrix(gaze)
g_M_s = np.asmatrix(T)
w_M_s_ref = w_M_g * g_M_s
w_M_s = np.asmatrix(op.position.value)
# 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.]])
J = np.asmatrix(op.jacobian.value)
J_ref = twist * Jgaze
# Check w_M_s == w_M_s_ref
self.assertEqual(np.equal(J, J_ref).all(), True)
class MetaTaskKine6dRel(MetaTask6d):
opPointBase = ''
def createOpPointBase(self, opPointBase, opPointRefBase='left-wrist'):
self.opPointBase = opPointBase
if self.opPointExist(opPointBase):
return
self.dyn.createOpPoint(opPointBase, opPointRefBase)
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'))
self.opPointModifBase.activ = False
def createFeatures(self):
self.feature = FeaturePoint6dRelative('featureRel' + self.name)
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'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
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'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
self.createOpPointBase(opPointBase, opPointRefBase)
self.createOpPointModif()
self.createOpPointModifBase()
self.createFeatures()
self.createTask()
self.createGain()
self.plugEverything()
@property
def opmodifBase(self):
if not self.opPointModifBase.activ:
return False
else:
return self.opPointModifBase.getTransformation()
@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'))
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)
self.opPointModifBase.setTransformation(m)
self.opPointModifBase.activ = True
class MetaTask6d(object):
name = ''
opPoint = ''
dyn = 0
task = 0
feature = 0
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]
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
def createOpPoint(self, opPoint, opPointRef='right-wrist'):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
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'))
self.opPointModif.activ = False
def createFeatures(self):
self.feature = FeaturePoint6d('feature' + self.name)
self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
self.feature.selec.value = Flags('111111')
self.feature.frame('current')
def createTask(self):
self.task = Task('task' + self.name)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
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'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def keep(self):
self.feature.position.recompute(self.dyn.position.time)
self.feature.keep()
def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
self.createOpPointModif()
self.createFeatures()
self.createTask()
self.createGain()
self.plugEverything()
@property
def ref(self):
return self.featureDes.position.value
@ref.setter
def ref(self, m):
self.featureDes.position.value = m
@property
def opmodif(self):
if not self.opPointModif.activ:
return False
else:
return self.opPointModif.getTransformation()
@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'))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
plug(self.opPointModif.signal('position'), self.feature.position)
plug(self.opPointModif.signal('jacobian'), self.feature.Jq)
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
class MetaTaskVisualPoint(object):
name = ''
opPoint = ''
dyn = 0
task = 0
feature = 0
featureDes = 0
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())
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
def createOpPoint(self, opPoint, opPointRef='right-wrist'):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
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'))
self.opPointModif.activ = False
def createFeatures(self):
self.feature = FeatureVisualPoint('feature' + self.name)
self.featureDes = FeatureVisualPoint('feature' + self.name + '_ref')
self.feature.selec.value = '11'
def createTask(self):
self.task = Task('task' + self.name)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
def createProj(self):
self.proj = VisualPointProjecter('proj' + self.name)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
plug(self.proj.signal('point2D'), self.feature.signal('xy'))
plug(self.proj.signal('depth'), self.feature.signal('Z'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
self.createOpPointModif()
self.createFeatures()
self.createTask()
self.createGain()
self.createProj()
self.plugEverything()
@property
def ref(self):
return self.featureDes.xy.value
@ref.setter
def ref(self, m):
self.featureDes.xy.value = m
@property
def target(self):
return self.proj.point3D
@target.setter
def target(self, m):
self.proj.point3D.value = m
@property
def opmodif(self):
if not self.opPointModif.activ:
return False
else:
return self.opPointModif.getTransformation()
@opmodif.setter
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'))
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'))
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
def goto3D(self, point3D, gain=None, ref2D=None, selec=None):
self.target = point3D
if ref2D is not None:
self.ref = ref2D
if selec is not None:
self.feature.selec.value = selec
setGain(self.gain, gain)