Beispiel #1
0
    def _readMotionData(self):
        '''
        Read the motion data from the BVH file.
        '''
        # update joint tree with new data.
        frame = 0
        lastRotation = {}
        for frame in range(self.frameCount):
            time = frame * self.framePeriod
            channelData = self._readFrame()
            for joint in self.model.joints:
                for chan in joint.channels:
                    chanData = channelData.pop(0)
                    if chan == "Xposition":
                        xPos = chanData
                    elif chan == "Yposition":
                        yPos = chanData
                    elif chan == "Zposition":
                        zPos = chanData
                    elif chan == "Xrotation":
                        xRot = chanData
                    elif chan == "Yrotation":
                        yRot = chanData
                    elif chan == "Zrotation":
                        zRot = chanData
                    else:
                        raise RuntimeError('Unknown channel: ' + chan)

                if not joint.hasParent:
                    try:
                        p = np.array([[xPos,yPos,zPos]]).T * \
                                self.conversionFactor
                        p = convertCGtoNED(p)
                        del xPos, yPos, zPos
                        joint.positionKeyFrames.add(time, p)
                    except UnboundLocalError:
                        raise SyntaxError("No position data for root joint")

                try:
                    if joint.hasChildren:
                        q = Quaternion.fromEuler((zRot, xRot, yRot),
                                                 order='zxy')
                        q = convertCGtoNED(q)
                        del xRot, yRot, zRot
                        # Rotation in BVH is relative to parent joint so
                        # combine the rotations
                        if joint.hasParent:
                            q = lastRotation[joint.parent] * q
                        joint.rotationKeyFrames.add(time, q)
                        lastRotation[joint] = q
                except UnboundLocalError:
                    raise SyntaxError("No rotation data for a joint that "
                                      "is not an end effector")
Beispiel #2
0
def testBVHInput():
    data = r"""HIERARCHY
ROOT root
{
    OFFSET 0.0 0.0 0.0
    CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
    JOINT j1
    {
        OFFSET 10.0 0.0 0.0
        CHANNELS 3 Zrotation Xrotation Yrotation
        End Site
        {
            OFFSET 0.0 10.0 0.0
        }
    }
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
    """
    testFile = tempfile.NamedTemporaryFile(mode='w+t', encoding='utf-8')
    with testFile:
        testFile.write(data)
        testFile.flush()
        model = loadBVHFile(testFile.name)

        assert len(list(model.points)) == 3

        assert model.name == 'root'
        assert len(model.channels) == 6
        assert len(model.children) == 1
        assert_almost_equal(model.position(0), vector(0, 0, 0))
        assertQuaternionAlmostEqual(model.rotation(0),
                                    convertCGtoNED(Quaternion(1, 0, 0, 0)))

        j1 = model.getJoint('j1')
        assert j1.parent is model
        assert len(j1.channels) == 3
        assert len(j1.children) == 1
        assert_almost_equal(j1.positionOffset, vector(10, 0, 0))
        assert_almost_equal(j1.position(0), convertCGtoNED(vector(10, 0, 0)))
        assertQuaternionAlmostEqual(
            j1.rotation(0),
            convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))

        j1end = model.getPoint('j1_end')
        assert j1end.parent is j1
        assert len(j1end.channels) == 0
        assert not j1end.isJoint
        assert_almost_equal(j1end.positionOffset, vector(0, 10, 0))
        assert_almost_equal(j1end.position(0), vector(0, 0, 0))
Beispiel #3
0
def testBVHInput():
    data = r"""HIERARCHY
ROOT root
{
    OFFSET 0.0 0.0 0.0
    CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
    JOINT j1
    {
        OFFSET 10.0 0.0 0.0
        CHANNELS 3 Zrotation Xrotation Yrotation
        End Site
        {
            OFFSET 0.0 10.0 0.0
        }
    }
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
    """
    testFile = tempfile.NamedTemporaryFile()
    with testFile:
        testFile.write(data)
        testFile.flush()
        model = loadBVHFile(testFile.name)

        assert len(list(model.points)) == 3

        assert model.name == 'root'
        assert len(model.channels) == 6
        assert len(model.children) == 1
        assert_almost_equal(model.position(0),vector(0,0,0))
        assertQuaternionAlmostEqual(model.rotation(0),
               convertCGtoNED(Quaternion(1,0,0,0)))

        j1 = model.getJoint('j1')
        assert j1.parent is model
        assert len(j1.channels) == 3
        assert len(j1.children) == 1
        assert_almost_equal(j1.positionOffset, vector(10,0,0))
        assert_almost_equal(j1.position(0), convertCGtoNED(vector(10,0,0)))
        assertQuaternionAlmostEqual(j1.rotation(0),
               convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))

        j1end = model.getPoint('j1_end')
        assert j1end.parent is j1
        assert len(j1end.channels) == 0
        assert not j1end.isJoint
        assert_almost_equal(j1end.positionOffset, vector(0,10,0))
        assert_almost_equal(j1end.position(0), vector(0,0,0))
Beispiel #4
0
    def _readJoint(self):
        # use jointStack[-1] to peek at the top of the jointStack
        currentJoint = self.jointStack[-1]
        name = self._token()
        # name will be 'Site' for End Site joints.
        if name == 'Site':
            name = currentJoint.parent.name + "_end"
        currentJoint.name = name

        self._checkToken("{")
        while True:
            token = self._token()
            if token == "OFFSET":
                x = self._floatToken()
                y = self._floatToken()
                z = self._floatToken()
                currentJoint.positionOffset = \
                        convertCGtoNED(np.array([[x,y,z]]).T*self.conversionFactor)
            elif token == "CHANNELS":
                n = self._intToken()
                channels = []
                for i in range(n):
                    token = self._token()
                    if token not in ["Xposition","Yposition","Zposition",\
                                     "Xrotation","Yrotation","Zrotation"]:
                        raise SyntaxError, "Syntax error in line %d: Invalid \
channel name '%s'" % (self.line, token)
                    else:
                        channels.append(token)
                self.totalChannels += n
                currentJoint.channels = channels
            elif token in ("JOINT", "End"):
                if token == "JOINT":
                    joint = SampledJoint(currentJoint)
                else:
                    joint = PointTrajectory(currentJoint)
                joint.channels = []
                self.jointStack.append(joint)
                self._readJoint()
            elif token == "}":
                self.jointStack.pop()
                break
            else:
                raise SyntaxError,\
                    "Syntax error in line %d: Unknown keyword '%s'" %(
                            self.line,token)
def testCG_to_NED_Quat():
    testing.assert_equal(
        transforms.convertCGtoNED(Quaternion()).components,
        Quaternion(0.5, -0.5, 0.5, -0.5).components)
def checkCG_to_NED(ned, cg):
    assert_almost_equal(transforms.convertCGtoNED(cg), ned)
Beispiel #7
0
def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod):
    """
    Load motion capture data from an ASF and AMC file pair.

    @param asfFileName: Name of the ASF file containing the description of
        the rigid body model
    @param amcFileName: Name of the AMC file containing the motion data
    @param scaleFactor: Scaling factor to convert lengths to m. For data
        from the CMU motion capture corpus this should be 2.54/100 to
        convert from inches to metres.

    @return: A {SampledBodyModel} representing the root of the rigid body
        model structure.
    """
    with open(asfFileName, 'r') as asfFile:
        data = asfParser.parseFile(asfFile)
        scale = (1.0/data.units.get('length',1)) * scaleFactor

        bones = dict((bone.name,bone) for bone in data.bones)
        asfModel = ASFRoot(data.root)

        for entry in data.hierarchy:
            parent = asfModel.getBone(entry.parent)
            for childName in entry.children:
                ASFBone(parent, bones[childName], scale)

        imusimModel = SampledBodyModel('root')
        for subtree in asfModel.children:
            for bone in subtree:
                if not bone.isDummy:
                    offset = vector(0,0,0)
                    ancestors = bone.ascendTree()
                    while True:
                        ancestor = ancestors.next().parent
                        offset += ancestor.childoffset
                        if not ancestor.isDummy:
                            break

                    SampledJoint(parent=imusimModel.getJoint(ancestor.name),
                            name=bone.name,
                            offset=offset)
                if not bone.hasChildren:
                    PointTrajectory(
                            parent=imusimModel.getJoint(bone.name),
                            name=bone.name+'_end',
                            offset=bone.childoffset
                            )

        with open(amcFileName) as amcFile:
            motion = amcParser.parseFile(amcFile)
            t = 0
            for frame in motion.frames:
                for bone in frame.bones:
                    bonedata = asfModel.getBone(bone.name)
                    if bone.name == 'root':
                        data = dict((chan.lower(), v) for chan,v in
                            zip(bonedata.channels,bone.channels))
                        position = convertCGtoNED(scale * vector(data['tx'],
                            data['ty'],data['tz']))
                        imusimModel.positionKeyFrames.add(t, position)

                    axes, angles = zip(*[(chan[-1], angle) for chan, angle in
                        zip(bonedata.channels, bone.channels) if
                                chan.lower().startswith('r')])
                    rotation = (bonedata.rotationOffset.conjugate *
                            Quaternion.fromEuler(angles[::-1], axes[::-1]))
                    joint = imusimModel.getJoint(bone.name)
                    if joint.hasParent:
                        parentRot = joint.parent.rotationKeyFrames.latestValue
                        parentRotOffset = bonedata.parent.rotationOffset
                        rotation = parentRot * parentRotOffset * rotation
                    else:
                        rotation = convertCGtoNED(rotation)
                    joint.rotationKeyFrames.add(t, rotation)
                t += framePeriod

        return imusimModel
Beispiel #8
0
def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod):
    """
    Load motion capture data from an ASF and AMC file pair.

    @param asfFileName: Name of the ASF file containing the description of
        the rigid body model
    @param amcFileName: Name of the AMC file containing the motion data
    @param scaleFactor: Scaling factor to convert lengths to m. For data
        from the CMU motion capture corpus this should be 2.54/100 to
        convert from inches to metres.

    @return: A {SampledBodyModel} representing the root of the rigid body
        model structure.
    """
    with open(asfFileName, 'r') as asfFile:
        data = asfParser.parseFile(asfFile)
        scale = (1.0 / data.units.get('length', 1)) * scaleFactor

        bones = dict((bone.name, bone) for bone in data.bones)
        asfModel = ASFRoot(data.root)

        for entry in data.hierarchy:
            parent = asfModel.getBone(entry.parent)
            for childName in entry.children:
                ASFBone(parent, bones[childName], scale)

        imusimModel = SampledBodyModel('root')
        for subtree in asfModel.children:
            for bone in subtree:
                if not bone.isDummy:
                    offset = vector(0, 0, 0)
                    ancestors = bone.ascendTree()
                    while True:
                        ancestor = ancestors.next().parent
                        offset += ancestor.childoffset
                        if not ancestor.isDummy:
                            break

                    SampledJoint(parent=imusimModel.getJoint(ancestor.name),
                                 name=bone.name,
                                 offset=offset)
                if not bone.hasChildren:
                    PointTrajectory(parent=imusimModel.getJoint(bone.name),
                                    name=bone.name + '_end',
                                    offset=bone.childoffset)

        with open(amcFileName) as amcFile:
            motion = amcParser.parseFile(amcFile)
            t = 0
            for frame in motion.frames:
                for bone in frame.bones:
                    bonedata = asfModel.getBone(bone.name)
                    if bone.name == 'root':
                        data = dict((chan.lower(), v) for chan, v in zip(
                            bonedata.channels, bone.channels))
                        position = convertCGtoNED(
                            scale * vector(data['tx'], data['ty'], data['tz']))
                        imusimModel.positionKeyFrames.add(t, position)

                    axes, angles = zip(*[(chan[-1], angle)
                                         for chan, angle in zip(
                                             bonedata.channels, bone.channels)
                                         if chan.lower().startswith('r')])
                    rotation = (bonedata.rotationOffset.conjugate *
                                Quaternion.fromEuler(angles[::-1], axes[::-1]))
                    joint = imusimModel.getJoint(bone.name)
                    if joint.hasParent:
                        parentRot = joint.parent.rotationKeyFrames.latestValue
                        parentRotOffset = bonedata.parent.rotationOffset
                        rotation = parentRot * parentRotOffset * rotation
                    else:
                        rotation = convertCGtoNED(rotation)
                    joint.rotationKeyFrames.add(t, rotation)
                t += framePeriod

        return imusimModel
Beispiel #9
0
def testCG_to_NED_Quat():
    testing.assert_equal(
            transforms.convertCGtoNED(Quaternion()).components,
            Quaternion(0.5, -0.5, 0.5, -0.5).components)
Beispiel #10
0
def checkCG_to_NED(ned, cg):
    assert_almost_equal(transforms.convertCGtoNED(cg), ned)