IMUSim example

gistfile1.txt

import matplotlib.pyplot as plt
import bpy
import sys
import matplotlib.pyplot as plt
import numpy as np
import os
import pandas as pd
import math
from imusim.all import *

def getAngle(a,b,c):
    ab = [ b[0] - a[0], b[1] - a[1], b[2] - a[2]]
    bc = [ c[0] - b[0], c[1] - b[1], c[2] - b[2]]

    abVec = math.sqrt(ab[0] * ab[0] + ab[1] * ab[1] + ab[2] * ab[2])
    bcVec = math.sqrt(bc[0] * bc[0] + bc[1] * bc[1] + bc[2] * bc[2])

    abNorm = [ ab[0] / abVec, ab[1] / abVec, ab[2] / abVec]
    bcNorm = [ bc[0] / bcVec, bc[1] / bcVec, bc[2] / bcVec]

    res = abNorm[0] * bcNorm[0] + abNorm[1] * bcNorm[1] + abNorm[2] * bcNorm[2]

    return math.acos(res)*180.0/ 3.141592653589793;


upperBoneName = 'rThigh'
lowerBoneName = 'rShin'
BVHfile = '09/09_06.bvh'

#delete the basic scene
bpy.data.objects[0].select_set(True)
bpy.ops.object.delete()
bpy.data.objects[0].select_set(True)
bpy.ops.object.delete()

#import the BVH
bpy.ops.import_anim.bvh(filepath=BVHfile, axis_forward='Y', axis_up='Z')
armature = bpy.data.objects[0]



#subdivide the bones
bpy.ops.object.mode_set(mode='EDIT')

bone = armature.data.edit_bones.get(upperBoneName)
bone.select = True
bpy.ops.armature.subdivide(number_cuts=1)
for bone in armature.data.edit_bones:
    if bone.select:
        bone.select = False

bone = armature.data.edit_bones.get(lowerBoneName)
bone.select = True
bpy.ops.armature.subdivide(number_cuts=1)
for bone in armature.data.edit_bones:
    if bone.select:
        bone.select = False


#subdivided bones get named [name].001


#export the edited file
bpy.ops.object.mode_set(mode='OBJECT')
armature.select_set(True)
BVHfile = BVHfile.replace(".bvh","")
BVHfile += "_divided.bvh"
bpy.ops.export_anim.bvh(filepath=BVHfile)


#fig = plt.figure()
#ax = fig.add_subplot()
#for i in range(1,110):
#    frame = i
#    bpy.context.scene.frame_set(frame)
#
#    bone1 = armature.pose.bones.get(upperBoneName + ".001")
#    bone2 = armature.pose.bones.get(lowerBoneName)
#    current_position = (bone1.head.xyz, bone2.head.xyz, bone2.tail.xyz)
#
#    x = [current_position[0][0],current_position[1][0]]
#    y = [current_position[0][1],current_position[1][1]]
#    z = [current_position[0][2],current_position[1][2]]
#
#    x2 = [current_position[1][0],current_position[2][0]]
#    y2 = [current_position[1][1],current_position[2][1]]
#    z2 = [current_position[1][2],current_position[2][2]]
#
#    #angle of the knee for current frame
#    angle = getAngle(current_position[0],current_position[1],current_position[2])
#    print(angle)
#
#    ax.scatter(x, y,c='red', s=100)
#    ax.plot(x, y, color='black')
#    ax.scatter(x2, y2, c='blue', s=100)
#    ax.plot(x2, y2,  color='black')
#    ax.scatter(0,0,0,color="green")
#plt.show()


#setup IMUsim
print("setting up imusim...")
samplingPeriod = 0.00125
imu = Orient3IMU()
env = Environment()
samples = 100
rotationalVelocity = 20
print("calibrating...")
calibrator = ScaleAndOffsetCalibrator(env, samples, samplingPeriod, rotationalVelocity)
calibration = calibrator.calibrate(imu)
print("loading BVH...")
model = loadBVHFile(BVHfile, CM_TO_M_CONVERSION)

print("running model for upper joint")
splinedModel = SplinedBodyModel(model)
sim = Simulation(environment=env)
imu.simulation = sim
imu.trajectory = splinedModel.getJoint(upperBoneName+".001")
sim.time = splinedModel.startTime
BasicIMUBehaviour(imu, samplingPeriod, calibration, initialTime=sim.time)
sim.run(splinedModel.endTime)

upperBone = imu.accelerometer.calibratedMeasurements

print("running model for lower joint")
splinedModel = SplinedBodyModel(model)
sim = Simulation(environment=env)
imu.simulation = sim
imu.trajectory = splinedModel.getJoint(lowerBoneName+".001")
sim.time = splinedModel.startTime
BasicIMUBehaviour(imu, samplingPeriod, calibration, initialTime=sim.time)
sim.run(splinedModel.endTime)

lowerBone = imu.accelerometer.calibratedMeasurements



#plt.figure()
#plt.title("Accelerometer Readings Thigh")
#plt.xlabel("Time (s)")
#plt.ylabel("Acceleration (m/s^2)")
#plt.plot(rThigh.timestamps, rThigh.values[0], label="X")
#plt.plot(rThigh.timestamps, rThigh.values[1], label="Y")
#plt.plot(rThigh.timestamps, rThigh.values[2], label="Z")
#plt.legend()
#plt.show()

#plt.figure()
#plt.title("Accelerometer Readings Shin")
#plt.xlabel("Time (s)")
#plt.ylabel("Acceleration (m/s^2)")
#plt.plot(rShin.timestamps, rShin.values[0], label="X")
#plt.plot(rShin.timestamps, rShin.values[1], label="Y")
#plt.plot(rShin.timestamps, rShin.values[2], label="Z")
#plt.legend()
#plt.show()

#angle calcualtion

FinalData = [None] * len(upperBone.timestamps)
i=0
for t in upperBone.timestamps:
    current_frame = int(t)
    fraction = t % 1

    # Set the current frame
    bpy.context.scene.frame_set(current_frame)

    # Get the bone's positions at the current frame and the next frame
    bone1 = armature.pose.bones.get(upperBoneName+".001")
    bone2 = armature.pose.bones.get(lowerBoneName)#not adding the .001 here since we need the exact point on the knee rather than middle of the shin
    current_position = (bone1.head.xyz,bone2.head.xyz,bone2.tail.xyz)

    bpy.context.scene.frame_set(current_frame + 1)
    bone1 = armature.pose.bones.get(upperBoneName + ".001")
    bone2 = armature.pose.bones.get(lowerBoneName)
    next_position = (bone1.head.xyz,bone2.head.xyz,bone2.tail.xyz)

    # Interpolate the position between frames
    TopUpperBone = current_position[0].lerp(next_position[0], fraction)
    Joint = current_position[1].lerp(next_position[1], fraction)
    BottomLowerBone = current_position[2].lerp(next_position[2], fraction)


    angle = getAngle(TopUpperBone,Joint,BottomLowerBone)
    FinalData[i]=(t,
                  angle,
                  (upperBone.values[0][i],upperBone.values[1][i],upperBone.values[2][i]),
                  (lowerBone.values[0][i],lowerBone.values[1][i],lowerBone.values[2][i]),
                  )
    i+=1


print(FinalData)