bvh file parser (tested w Python2.6)

bvh_read.py

#Added functionality for interpreting the motion annotations into transformation matrices and euclidean coordinates.
#Restructured the code into an easy to instantiate class.
#To use: BVH(<open file handle>)

import re
import time
import zipfile
from StringIO import StringIO
from math import radians, sqrt, cos, sin
from numpy import matrix, dot, zeros, array, eye, ravel, asarray
from numpy.linalg import inv
from scipy.linalg import expm, norm

#   Matrix Math Utils
def translation_matrix(xyz):
    transform = eye(4)
    transform[:-1,-1] = xyz 
    return transform

def rotation_matrix(axis, theta):
    axis = asarray(axis)
    if(norm(axis != 1)):
        axis = axis/sqrt(dot(axis, axis))
    a = cos(theta/2.0)
    b, c, d = -axis*sin(theta/2.0)
    aa, bb, cc, dd = a*a, b*b, c*c, d*d
    bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
    return matrix([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac), 0],
                 [2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab), 0],
                 [2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc, 0],
                 [0,0,0,1]])

def euclidean_position(matrix):
    return ravel(dot(matrix, (0,0,0,1))[:,:-1])

class BVHBone:
    channels = []
    offsets = []
    def __init__(self, parent, name):
        self.parent = parent
        self.name = name

class BVH:
    id_t = matrix("0;0;0;1")
    current_token = 0
    skeleton = {}
    bone_context = []
    motion_channels = []
    raw_motions = []
    motions = []
    
    def __init__(self, bvh_file):
        if(bvh_file != None):
            self.tokens, remainder = self.scanner.scan(bvh_file.read())
            bvh_file.close()
            self.parse_hierarchy()
            self.parse_motion()
            self.process_motions()
        
#   BVH Tokensizing Scanner Definition
    def identifier(scanner, token):  return "IDENT", token
    def digit(scanner, token):       return "DIGIT", token
    def open_brace(scanner, token):  return "OPEN_BRACE", token
    def close_brace(scanner, token): return "CLOSE_BRACE", token
    reserved      = [ "HIERARCHY", "ROOT", "OFFSET", "CHANNELS", "MOTION" ]
    channel_names = [ "Xposition", "Yposition", "Zposition",  "Zrotation", "Xrotation",  "Yrotation" ]
    
    scanner = re.Scanner([
    (r"[a-zA-Z_]\w*", identifier),
    (r"-*\.?[0-9]+(\.[0-9]+)?", digit),
    (r"}", close_brace),
    (r"{", open_brace),
    (r":", None),
    (r"\s+", None),
    ])

#   BVH Hierarchy Utils
    def push_bone_context(self, name):
        self.bone_context.append(name)

    def get_bone_context(self):
        if(len(self.bone_context) == 0):
            return None
        else:
            return self.bone_context[len(self.bone_context)-1]

    def pop_bone_context(self):
        self.bone_context = self.bone_context[:-1]
        return self.get_bone_context()
    
    def next_token(self):
        self.current_token = self.current_token + 1
        return self.tokens[self.current_token-1]
    
    def next_float(self):
        return float(self.assert_token_type("DIGIT"))
    
    def next_int(self):
        return int(self.assert_token_type("DIGIT"))
    
    def assert_token_type(self, token_type):
        if(self.next_token()[0] != token_type):
            raise ValueError("Was expecting %s, got %s"%(token_type, self.tokens[self.current_token-1]))
        return self.tokens[self.current_token-1][1]
    
    def assert_token_value(self, token_value):
        if(self.next_token() != token_value):
            raise ValueError("Was expecting %s, got %s"%(token_value, self.tokens[self.current_token-1]))
            
    def read_offset(self):
        self.assert_token_value(("IDENT", "OFFSET"))
        return  [self.next_float(),self.next_float(),self.next_float()]
    
    def read_channels(self):
        self.assert_token_value(("IDENT", "CHANNELS"))
        channel_count = self.next_int()
        channels = []
        for i in range(0, channel_count):
            channels.append(self.next_token()[1])
        return channels
    
    def parse_joint(self):
        token = self.next_token()
        if(token[0] == "CLOSE_BRACE"):
            return False
        if(token[1] != "ROOT" and token[1] != "JOINT" and token[1] != "End"):
            raise ValueError("Parsing Error. Expected CLOSE_BRACE. got: (%s,%s)"%token)
        
        joint = BVHBone(self.get_bone_context(), self.assert_token_type("IDENT"))
        self.assert_token_type("OPEN_BRACE")
        joint.offsets = self.read_offset()
        if (token[1] == "End"): # end site
            joint.name = self.get_bone_context() + "_Nub"
        else:
            joint.channels = self.read_channels()
            for channel in joint.channels:
                self.motion_channels.append((joint.name,channel))
        self.skeleton[joint.name] = joint
        
        self.push_bone_context(joint.name)
        
        while (self.parse_joint()): 
            continue
            
        self.pop_bone_context()
        return True        
        
    def parse_hierarchy(self):
        self.current_token = 0
        self.assert_token_value(("IDENT", "HIERARCHY"))
        self.parse_joint()
        
    def parse_motion(self):
        self.assert_token_value(("IDENT","MOTION"))
        self.assert_token_value(("IDENT","Frames"))
        self.frame_count = self.next_int()
        self.assert_token_value(("IDENT","Frame"))
        self.assert_token_value(("IDENT","Time"))
        self.frame_rate = self.next_float()
        for i in range(self.frame_count):
            channel_values = {}
            for bone in self.skeleton:
                channel_values[bone] = {"channels": [], "keys": []}
            for channel in self.motion_channels:
                channel_values[channel[0]]["channels"].append(channel[1])
                channel_values[channel[0]]["keys"].append(self.next_float())
            self.raw_motions.append([i * self.frame_rate, channel_values])
    
#   Process Frame Data
    def bone_matrix_at_frame(self, bone_name, frame) :
        if(not self.motions[frame].has_key(bone_name)):     
            values = self.raw_motions[frame][1][bone_name]
            transform = translation_matrix(self.skeleton[bone_name].offsets)

            for action in values["channels"]:
                value = values["keys"][values["channels"].index(action)]
                if(action == "Xposition"):
                    transform[0,3] += value
                if(action == "Yposition"):
                    transform[1,3] += value
                if(action == "Zposition"):
                    transform[2,3] += value
                if(action == "Xrotation"):
                    transform = dot(transform, rotation_matrix((1,0,0), radians(value))) 
                if(action == "Yrotation"):
                    transform = dot(transform, rotation_matrix((0,1,0), radians(value)))
                if(action == "Zrotation"):
                    transform = dot(transform, rotation_matrix((0,0,1), radians(value)))

            if(self.skeleton[bone_name].parent != None):
                parent_matrix = self.bone_matrix_at_frame(self.skeleton[bone_name].parent, frame)
                transform = dot(parent_matrix, transform)

            self.motions[frame][bone_name] = transform
        return self.motions[frame][bone_name]

    def bone_at_frame(self, bone_name, frame) :
        return euclidean_position(self.bone_matrix_at_frame(bone_name, frame))
    
    def process_motions(self): 
        for frame in range(self.frame_count):
            self.motions.append({})
            for bone in self.skeleton:
                self.bone_matrix_at_frame(bone, frame)

#Test it out and display euclidean space coordinates for the bones at frame 1
#Special thanks to OSU Motion Capture Lab for hosting a bvh file!
'''
from StringIO import StringIO
from zipfile import ZipFile
from urllib import urlopen

url = urlopen("http://accad.osu.edu/research/mocap/data/run1_BVH.ZIP")
zipfile = ZipFile(StringIO(url.read()))
bvh_file = zipfile.open(zipfile.infolist()[0])
bvh = BVH(bvh_file)

for bone in bvh.skeleton:
    xyz = bvh.bone_at_frame(bone, 1)
    print "{name:<20}X:{coords[0]:<20}, Y:{coords[1]:<20}, Z:{coords[2]:<20}".format(name=bone, coords=xyz)
'''

DontPrintComments=True