def quaternion_cleanup(object, prevent_flips=True, prevent_inverts=True):
"""fixes signs in quaternion fcurves swapping from one frame to another"""
for curves in get_all_quaternion_curves(object):
start = int(min((curves[i].keyframe_points[0].co.x for i in range(4))))
end = int(max((curves[i].keyframe_points[-1].co.x for i in range(4))))
for curve in curves:
for i in range(start, end):
curve.keyframe_points.insert(i, curve.evaluate(i)).interpolation = 'LINEAR'
zipped = list(zip(
curves[0].keyframe_points,
curves[1].keyframe_points,
curves[2].keyframe_points,
curves[3].keyframe_points))
for i in range(1, len(zipped)):
if prevent_flips:
rot_prev = Quaternion((zipped[i-1][j].co.y for j in range(4)))
rot_cur = Quaternion((zipped[i][j].co.y for j in range(4)))
diff = rot_prev.rotation_difference(rot_cur)
if abs(diff.angle - pi) < 0.5:
rot_cur.rotate(Quaternion(diff.axis, pi))
for j in range(4):
zipped[i][j].co.y = rot_cur[j]
if prevent_inverts:
change_amount = 0.0
for j in range(4):
change_amount += abs(zipped[i-1][j].co.y - zipped[i][j].co.y)
if change_amount > 1.0:
for j in range(4):
zipped[i][j].co.y *= -1.0
node.ui.add_button("calibrate_rotation", "Calibrate", "calibrate_rotation")
node.ui.add_text("orientation")
node.ui.add_text("temperature")
node.ui.update()
node.listen("calibrate_rotation", calibrate_rotation)
if __name__ == "__main__":
while node.running:
for _ in range(30):
x, y, z, w = bno.read_quaternion()
raw = Quaternion([w, x, y, z])
corrected = raw.rotation_difference(calibration)
node.broadcast(
"orientation",
[corrected.w, corrected.x, corrected.y, corrected.z])
time.sleep(1 / 30.0)
e = corrected.to_euler()
node.ui.get(
"orientation")["text"] = "Orientation: %f %f %f" % (e.x, e.y, e.z)
temperature = bno.read_temp()
node.ui.get("temperature")["text"] = "Temperature: %f" % temperature
node.broadcast("head_internal_temperature", temperature)
node.ui.update()
def _add_animation_to_scene(self, anim_name, anim_data):
# First, let's find out what animation data each object has
# We do this by looking at the indexes of the rotation, translation and
# scale data and see whether that lies within the AnimNodeData or the
# StillFrameData
node_data_map = odict()
rot_anim_len = len(anim_data['AnimFrameData'][0]['Rotation'])
trans_anim_len = len(anim_data['AnimFrameData'][0]['Translation'])
scale_anim_len = len(anim_data['AnimFrameData'][0]['Scale'])
for node_data in anim_data['NodeData']:
data = {'anim': dict(), 'still': dict()}
# For each node, check to see if the data is in the animation data
# or in the still frame data
rotIndex = int(node_data['RotIndex'])
if rotIndex >= rot_anim_len:
rotIndex -= rot_anim_len
data['still']['Rotation'] = rotIndex
else:
data['anim']['Rotation'] = rotIndex
transIndex = int(node_data['TransIndex'])
if transIndex >= trans_anim_len:
transIndex -= trans_anim_len
data['still']['Translation'] = transIndex
else:
data['anim']['Translation'] = transIndex
scaleIndex = int(node_data['ScaleIndex'])
if scaleIndex >= scale_anim_len:
scaleIndex -= scale_anim_len
data['still']['Scale'] = scaleIndex
else:
data['anim']['Scale'] = scaleIndex
node_data_map[node_data['Node']] = data
# Now that we have all the indexes sorted out, for each node, we create
# a new action and give it all the information it requires.
for name, data in node_data_map.items():
try:
obj = self.scn.objects[name]
except KeyError:
continue
obj.animation_data_create()
action_name = "{0}.{1}".format(anim_name, name)
obj.animation_data.action = bpy.data.actions.new(name=action_name)
# set the action to have a fake user
obj.animation_data.action.use_fake_user = True
fcurves = self._create_anim_channels(obj, action_name)
self._apply_animdata_to_fcurves(fcurves, data, anim_data, False)
# If we have a mesh with joint bindings, also animate the armature
if self.scn.nmsdk_anim_data.has_bound_mesh:
armature = bpy.data.objects['Armature']
armature.animation_data_create()
action_name = "{0}_Armature".format(anim_name)
armature.animation_data.action = bpy.data.actions.new(
name=action_name)
# set the action to have a fake user
armature.animation_data.action.use_fake_user = True
num_frames = anim_data['FrameCount']
for name, node_data in node_data_map.items():
# we only care about animating the joints
if name not in self.scn.nmsdk_anim_data.joints:
continue
print('-- adding {0} --'.format(name))
bone = armature.pose.bones[name]
still_data = node_data['still']
animated_data = node_data['anim']
location = None
rotation = None
scale = None
# Apply the transforms as required
for key, value in still_data.items():
data = anim_data['StillFrameData'][key][value]
if key == 'Translation':
location = Vector(data[:3])
elif key == 'Rotation':
# move the w value to the start to initialize the
# quaternion
rotation = Quaternion(
[data[3], data[0], data[1], data[2]])
elif key == 'Scale':
scale = Vector(data[:3])
# Apply the proper animated data
# bone_ref_mat = bone.matrix.copy()
for i, frame in enumerate(anim_data['AnimFrameData']):
# First apply the required transforms
for key, value in animated_data.items():
data = frame[key][value]
if key == 'Translation':
location = Vector(data[:3])
elif key == 'Rotation':
# move the w value to the start to initialize the
# quaternion
rotation = Quaternion(
[data[3], data[0], data[1], data[2]])
elif key == 'Scale':
scale = Vector(data[:3])
bind_data = self.scn.objects[name]['bind_data']
delta_loc = location - Vector(bind_data[0].to_list())
delta_rot = rotation.rotation_difference(
Quaternion(bind_data[1].to_list()))
ref_scale = Vector(bind_data[2].to_list())
delta_sca = Vector(
(scale[0] / ref_scale[0], scale[1] / ref_scale[1],
scale[2] / ref_scale[2]))
bone.location = delta_loc
bone.rotation_quaternion = delta_rot
bone.scale = delta_sca
# For each transform applied, add a keyframe
for key in ['Translation', 'Rotation', 'Scale']:
if key in still_data:
if i == 0 or i == num_frames - 1:
self._apply_pose_data(bone, DATA_PATH_MAP[key],
i, action_name)
elif key in animated_data:
self._apply_pose_data(bone, DATA_PATH_MAP[key], i,
action_name)
