示例#1
0
    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
示例#3
0
 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
示例#4
0
    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)
示例#5
0
    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())
示例#6
0
 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
示例#7
0
    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)
示例#8
0
    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())
示例#9
0
    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
示例#11
0
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
示例#12
0
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)