diff --git a/anim_utils.egg-info/PKG-INFO b/anim_utils.egg-info/PKG-INFO new file mode 100644 index 0000000..476c4e6 --- /dev/null +++ b/anim_utils.egg-info/PKG-INFO @@ -0,0 +1,65 @@ +Metadata-Version: 2.1 +Name: anim-utils +Version: 0.1 +Summary: Skeleton Animation Utilities. +Home-page: https://github.com/eherr/anim_utils +Author: DFKI GmbH +License: MIT +Keywords: skeleton animation data retargeting +Platform: UNKNOWN +Requires-Python: >=3.5, <4 +Description-Content-Type: text/markdown +License-File: LICENSE + +# Skeleton Animation Utilities + +Utility functions and data structures for skeleton animations loaded from BVH and ASF/AMC files. The library provides functions for inverse kinematics and retargeting. The main dependency, in addition to NumPy, SciPy and Matplotlib, is the transformations library by Christoph Gohlke https://www.lfd.uci.edu/~gohlke/. + +## Installation + +Clone the repository and install the package with editable flag or use the follwing command: +```bat +pip install git+https://github.com/eherr/anim_utils +``` +The optional FBX IO requires the [Python FBX SDK](https://www.autodesk.com/developer-network/platform-technologies/fbx-sdk-2020-3) + +## Example + +```python +from anim_utils.animation_data import BVHReader, MotionVector, SkeletonBuilder + +bvh = BVHReader("example.bvh") +mv = MotionVector() +mv.from_bvh_reader(bvh) +skeleton = SkeletonBuilder().load_from_bvh(bvh) +point_clouds = [] +for frame in mv.frames: + point_cloud = [] + for j in skeleton.animated_joints: + p = skeleton.nodes[j].get_global_position(frame) + point_cloud.append(p) + point_clouds.append(point_cloud) + +``` + +A retargeting script can be found in the example directory. + +## Developers + +Erik Herrmann1, Han Du1, Martin Manns2, Markus Mauer2 + +1DFKI GmbH +2Daimler AG + + +## License +Copyright (c) 2019 DFKI GmbH. +MIT License, see the LICENSE file. + +Contributions by Daimler AG in the following files are also licensed under terms of the MIT license: +anim_utils/animation_data/bvh.py +anim_utils/animation_data/utils.py + +Each file contains a copyright notice. + + diff --git a/anim_utils.egg-info/SOURCES.txt b/anim_utils.egg-info/SOURCES.txt new file mode 100644 index 0000000..ed7640d --- /dev/null +++ b/anim_utils.egg-info/SOURCES.txt @@ -0,0 +1,56 @@ +LICENSE +README.md +setup.py +anim_utils/__init__.py +anim_utils.egg-info/PKG-INFO +anim_utils.egg-info/SOURCES.txt +anim_utils.egg-info/dependency_links.txt +anim_utils.egg-info/requires.txt +anim_utils.egg-info/top_level.txt +anim_utils/animation_data/__init__.py +anim_utils/animation_data/acclaim.py +anim_utils/animation_data/bvh.py +anim_utils/animation_data/constants.py +anim_utils/animation_data/joint_constraints.py +anim_utils/animation_data/motion_blending.py +anim_utils/animation_data/motion_concatenation.py +anim_utils/animation_data/motion_distance.py +anim_utils/animation_data/motion_state.py +anim_utils/animation_data/motion_vector.py +anim_utils/animation_data/quaternion_frame.py +anim_utils/animation_data/skeleton.py +anim_utils/animation_data/skeleton_builder.py +anim_utils/animation_data/skeleton_models.py +anim_utils/animation_data/skeleton_node.py +anim_utils/animation_data/utils.py +anim_utils/animation_data/fbx/__init__.py +anim_utils/animation_data/fbx/fbx_export.py +anim_utils/animation_data/fbx/fbx_import.py +anim_utils/motion_editing/__init__.py +anim_utils/motion_editing/analytical_inverse_kinematics.py +anim_utils/motion_editing/coordinate_cyclic_descent.py +anim_utils/motion_editing/cubic_motion_spline.py +anim_utils/motion_editing/fabrik_chain.py +anim_utils/motion_editing/fabrik_node.py +anim_utils/motion_editing/footplant_constraint_generator.py +anim_utils/motion_editing/hybrit_ik.py +anim_utils/motion_editing/ik_constraints.py +anim_utils/motion_editing/ik_constraints_builder.py +anim_utils/motion_editing/motion_editing.py +anim_utils/motion_editing/motion_filtering.py +anim_utils/motion_editing/motion_grounding.py +anim_utils/motion_editing/numerical_ik_exp.py +anim_utils/motion_editing/numerical_ik_quat.py +anim_utils/motion_editing/skeleton_pose_model.py +anim_utils/motion_editing/utils.py +anim_utils/retargeting/__init__.py +anim_utils/retargeting/analytical.py +anim_utils/retargeting/constants.py +anim_utils/retargeting/constrained_retargeting.py +anim_utils/retargeting/fast_point_cloud_retargeting.py +anim_utils/retargeting/point_cloud_retargeting.py +anim_utils/retargeting/utils.py +anim_utils/utilities/__init__.py +anim_utils/utilities/custom_math.py +anim_utils/utilities/io_helper_functions.py +anim_utils/utilities/log.py \ No newline at end of file diff --git a/anim_utils.egg-info/dependency_links.txt b/anim_utils.egg-info/dependency_links.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/anim_utils.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/anim_utils.egg-info/requires.txt b/anim_utils.egg-info/requires.txt new file mode 100644 index 0000000..3f85c64 --- /dev/null +++ b/anim_utils.egg-info/requires.txt @@ -0,0 +1,4 @@ +matplotlib +numpy +scipy +transformations diff --git a/anim_utils.egg-info/top_level.txt b/anim_utils.egg-info/top_level.txt new file mode 100644 index 0000000..8dc8fff --- /dev/null +++ b/anim_utils.egg-info/top_level.txt @@ -0,0 +1 @@ +anim_utils diff --git a/build/lib/anim_utils/__init__.py b/build/lib/anim_utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/build/lib/anim_utils/animation_data/__init__.py b/build/lib/anim_utils/animation_data/__init__.py new file mode 100644 index 0000000..e05760a --- /dev/null +++ b/build/lib/anim_utils/animation_data/__init__.py @@ -0,0 +1,9 @@ +from .motion_vector import MotionVector +from .motion_concatenation import align_quaternion_frames, transform_euler_frames, transform_quaternion_frames +from .bvh import BVHReader, BVHWriter +from .acclaim import parse_asf_file, parse_amc_file +from .skeleton import Skeleton +from .skeleton_builder import SkeletonBuilder +from .skeleton_node import SKELETON_NODE_TYPE_ROOT, SKELETON_NODE_TYPE_JOINT, SKELETON_NODE_TYPE_END_SITE, SkeletonRootNode, SkeletonJointNode, SkeletonEndSiteNode +from .skeleton_models import SKELETON_MODELS +from .constants import * diff --git a/build/lib/anim_utils/animation_data/acclaim.py b/build/lib/anim_utils/animation_data/acclaim.py new file mode 100644 index 0000000..1b5c9cd --- /dev/null +++ b/build/lib/anim_utils/animation_data/acclaim.py @@ -0,0 +1,291 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" parser of asf and amc formats + https://research.cs.wisc.edu/graphics/Courses/cs-838-1999/Jeff/ASF-AMC.html + """ +import sys +import math +import numpy as np +from transformations import euler_matrix + +DEFAULT_FRAME_TIME = 1.0/120 + +def asf_to_bvh_channels(dof): + channels = [] + for d in dof: + type_str = "" + if d[0].lower() == "r": + type_str = "rotation" + elif d[0].lower() == "t": + type_str = "translation" + axis_str = d[-1].upper() + channels.append(axis_str + type_str) + return channels + +def rotate_around_x(alpha): + #Note vectors represent columns + cx = math.cos(alpha) + sx = math.sin(alpha) + m = np.array([[1.0, 0.0, 0.0, 0.0], + [0.0, cx , sx,0.0], + [0.0, -sx, cx,0.0], + [0.0,0.0,0.0,1.0]],np.float32) + return m.T + + +def rotate_around_y(beta): + #Note vectors represent columns + cy = math.cos(beta) + sy = math.sin(beta) + m = np.array([[cy,0.0,-sy ,0.0], + [0.0,1.0,0.0,0.0], + [sy, 0.0, cy,0.0], + [0.0,0.0,0.0,1.0]],np.float32) + return m.T + + +def rotate_around_z(gamma): + #Note vectors represent columns + cz = math.cos(gamma) + sz = math.sin(gamma) + m = np.array([[cz, sz,0.0,0.0], + [-sz, cz,0.0,0.0], + [0.0,0.0,1.0,0.0], + [0.0,0.0,0.0,1.0]],np.float32) + return m.T + + +AXES = "rxyz" +def create_euler_matrix(angles, order): + m = np.eye(4, dtype=np.float32) + for idx, d in enumerate(order): + a = np.radians(angles[idx]) + d = d[0].lower() + local_rot = np.eye(4) + if d =="x": + local_rot = euler_matrix(a,0,0, AXES) + elif d =="y": + local_rot = euler_matrix(0,a,0, AXES) + elif d =="z": + local_rot = euler_matrix(0,0,a, AXES) + m = np.dot(local_rot, m) + return m + +def create_euler_matrix2(angles, order): + m = np.eye(4, dtype=np.float32) + for idx, d in enumerate(order): + a = np.radians(angles[idx]) + d = d[0].lower() + local_rot = np.eye(4) + if d =="x": + local_rot = rotate_around_x(a) + elif d =="y": + local_rot = rotate_around_y(a) + elif d =="z": + local_rot = rotate_around_z(a) + m = np.dot(local_rot, m) + return m + +def create_coordinate_transform_matrices(data): + """ FROM https://research.cs.wisc.edu/graphics/Courses/cs-838-1999/Jeff/ASF-AMC.html + Precalculate some the transformation for each segment from the axis + """ + angles = data["root"]["orientation"] + order = data["root"]["axis"] + order = asf_to_bvh_channels(order) + c = create_euler_matrix(angles, order) + data["root"]["coordinate_transform"] = c + data["root"]["inv_coordinate_transform"] = np.linalg.inv(c) + + for key in data["bones"]: + if "axis" in data["bones"][key]: + angles, order = data["bones"][key]["axis"] + order = asf_to_bvh_channels(order) + c = create_euler_matrix(angles, order) + data["bones"][key]["coordinate_transform"] = c + data["bones"][key]["inv_coordinate_transform"] = np.linalg.inv(c) + return data + +def set_parents(data): + for parent in data["children"]: + for c in data["children"][parent]: + data["bones"][c]["parent"] = parent + return data + +def convert_bones_to_joints(data): + """ ASF stores bones that need to be converted into joints the internal skeleton class + based on the BVH format + additionally helper bones for the root need to be removed + """ + data = set_parents(data) + for b in data["bones"]: + if b == "root": + continue + # add offset to parent + parent = data["bones"][b]["parent"] + if parent in data["bones"] and "direction" in data["bones"][parent]: + offset = np.array(data["bones"][parent]["direction"]) + offset *= float(data["bones"][parent]["length"]) + data["bones"][b]["offset"] = offset + + # remove helper bones for hips without dofs + new_root_children = [] + old_root_children = [] + for c in data["children"]["root"]: + if "dof" in data["bones"][c] and len(data["bones"][c]["dof"])>0: + new_root_children.append(c) + else: # replace bones without dofs with their children + old_root_children.append(c) + for cc in data["children"][c]: + data["bones"][cc]["parent"] = "root" + new_root_children.append(cc) + data["children"]["root"] = new_root_children + for c in old_root_children: + del data["bones"][c] + return data + +def parse_asf_file(filepath): + with open(filepath, "rt") as in_file: + lines = in_file.readlines() + data = dict() + data["bones"] = dict() + idx = 0 + while idx < len(lines): + next_line = lines[idx].lstrip() + if next_line.startswith(":root"): + idx += 1 + data["root"], idx = read_root_data(lines, idx) + idx -=1 + if next_line.startswith(":name"): + data["name"] = next_line.split(" ")[1] + idx+=1 + elif next_line.startswith(":bonedata"): + idx+=1 + next_line = lines[idx].lstrip() + while not next_line.startswith(":hierarchy") and idx+1 < len(lines): + node, idx = read_bone_data(lines, idx) + if "name" in node: + name = node["name"] + data["bones"][name] = node + if idx < len(lines): + next_line = lines[idx].lstrip() + elif next_line.startswith(":hierarchy"): + data["children"], idx = read_hierarchy(lines, idx) + else: + idx+=1 + data = create_coordinate_transform_matrices(data) + data = convert_bones_to_joints(data) + return data + +def read_root_data(lines, idx): + data = dict() + #print("start root", idx) + next_line = lines[idx].strip() + while not next_line.startswith(":bonedata") and idx+1 < len(lines): + values = next_line.split(" ") + if len(values) > 0: + key = values[0] + if key == "position": + data["position"] = [values[1], values[2], values[3]] + elif key == "orientation": + data["orientation"] = [float(values[1]),float(values[2]), float(values[3])] + elif key == "axis": + data["axis"] = values[1] + elif key == "order": + data["order"] = [v for v in values if v != "order"] + #elif key == "limits": + # data[key] = [values[1], values[2], values[3]] + if idx+1 < len(lines): + idx+=1 + next_line = lines[idx].strip() # remove empty lines + + #print("end root", idx, next_line) + return data, idx + +def read_bone_data(lines, idx): + idx +=1 #skip begin + data = dict() + #print("start bone", idx) + next_line = lines[idx].strip() + while not next_line.startswith("end") and idx+1 < len(lines): + values = next_line.split(" ") + values = [v for v in values if v != ""] + if len(values) > 0: + key = values[0] + if key == "id": + data["id"] = values[1] + elif key == "name": + data["name"] = values[1] + elif key == "direction": + direction = np.array([float(v) for v in values if v != "direction"]) + direction /= np.linalg.norm(direction) + data["direction"] = direction.tolist() + elif key == "length": + data["length"] = float(values[1]) + elif key == "axis": + data["axis"] = [float(values[1]), float(values[2]), float(values[3])], values[4] + elif key == "dof": + data["dof"] = [v for v in values if v != "dof"] + #elif key == "limits": + # data[key] = [values[1], values[2], values[3]] + if idx+1 < len(lines): + idx+=1 + next_line = lines[idx].strip() # remove empty lines + idx +=1 #skip end + #print("end", idx, lines[idx]) + return data, idx + + +def read_hierarchy(lines, idx): + print("found hierarchy") + idx +=1 #skip begin + child_dict = dict() + next_line = lines[idx].strip() + while not next_line.startswith("end"): + values = next_line.split(" ") + if len(values) > 1: + child_dict[values[0]] = values[1:] + idx+=1 + next_line = lines[idx].strip() # remove empty lines + idx +=1 #skip end + return child_dict, idx + +def parse_amc_file(filepath): + with open(filepath, "rt") as in_file: + lines = in_file.readlines() + frames = [] + current_frame = None + idx = 0 + while idx < len(lines): + next_line = lines[idx].strip() + values = next_line.split(" ") + if len(values) == 1: + if values[0].isdigit(): + if current_frame is not None: + frames.append(current_frame) + current_frame = dict() + elif len(values) > 1 and current_frame is not None: + key = values[0] + current_frame[key] = [float(v) for v in values if v != key] + idx+=1 + return frames diff --git a/build/lib/anim_utils/animation_data/bvh.py b/build/lib/anim_utils/animation_data/bvh.py new file mode 100644 index 0000000..aebcc06 --- /dev/null +++ b/build/lib/anim_utils/animation_data/bvh.py @@ -0,0 +1,478 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH, Daimler AG. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" + +BVH +=== + +Biovision file format classes for reading and writing. +BVH Reader by Martin Manns +BVH Writer by Erik Herrmann + +""" + +import os +from collections import OrderedDict +import numpy as np +from transformations import quaternion_matrix, euler_from_matrix, euler_matrix +from .utils import rotation_order_to_string +from .quaternion_frame import quaternion_to_euler + +EULER_LEN = 3 +QUAT_LEN = 4 +TRANSLATION_LEN = 3 +TOE_NODES = ["Bip01_R_Toe0", "Bip01_L_Toe0"] +DEFAULT_FRAME_TIME = 0.013889 +DEFAULT_ROTATION_ORDER = ['Xrotation','Yrotation','Zrotation'] + + +class BVHData: + """Biovision data format class + """ + def __init__(self): + self.node_names = OrderedDict() + self.node_channels = [] + self.parent_dict = {} + self.root = None + self.frame_time = DEFAULT_FRAME_TIME + self.frames = None + self.animated_joints = None + self.filename = "" + + + def get_node_angles(self, node_name, frame): + """Returns the rotation for one node at one frame of an animation + Parameters + ---------- + * node_name: String + \tName of node + * frame: np.ndarray + \t animation keyframe frame + + """ + channels = self.node_names[node_name]["channels"] + euler_angles = [] + rotation_order = [] + for ch in channels: + if ch.lower().endswith("rotation"): + idx = self.node_channels.index((node_name, ch)) + rotation_order.append(ch) + euler_angles.append(frame[idx]) + return euler_angles, rotation_order + + def get_angles(self, node_channels): + """Returns numpy array of angles in all frames for specified channels + + Parameters + ---------- + * node_channels: 2-tuples of strings + \tEach tuple contains joint name and channel name + \te.g. ("hip", "Xposition") + + """ + indices = self.get_channel_indices(node_channels) + return self.frames[:, indices] + + def get_animated_joints(self): + """Returns an ordered list of joints which have animation channels""" + if self.animated_joints is not None: + for joint in self.animated_joints: + yield joint + else: + for name, node in self.node_names.items(): + if "channels" in node.keys() and len(node["channels"]) > 0: + yield name + + def set_animated_joints(self, animated_joints): + self.animated_joints = animated_joints + + def get_animated_frames(self): + channel_indices = [] + for joint in self.get_animated_joints(): + channel_indices += self.node_names[joint]["channel_indices"] + return self.frames[:, channel_indices] + + def convert_rotation_order(self, rotation_order): + self.convert_skeleton_rotation_order(rotation_order) + self.convert_motion_rotation_order(rotation_order_to_string(rotation_order)) + + def convert_skeleton_rotation_order(self, rotation_order): + # update channel indices + rotation_list = self.node_names[self.root]["channels"][3:] + #new_indices = sorted(range(len(rotation_list)), key=lambda k : rotation_list[k]) + for node_name, node in self.node_names.items(): + if 'End' not in node_name: + if len(node['channels']) == 6: + rotation_list = node['channels'][3:] + node['channels'][3:] = rotation_order + node['rotation_order'] = rotation_order_to_string(rotation_list) + else: + rotation_list = node['channels'] + node['channels'] = rotation_order + node['rotation_order'] = rotation_order_to_string(rotation_list) + + + def convert_motion_rotation_order(self, rotation_order_str): + new_frames = np.zeros(self.frames.shape) + for i in range(len(new_frames)): + for node_name, node in self.node_names.items(): + if 'End' not in node_name: + if len(node['channels']) == 6: + rot_mat = euler_matrix(*np.deg2rad(self.frames[i, node['channel_indices'][3:]]), + axes=node['rotation_order']) + new_frames[i, node['channel_indices'][:3]] = self.frames[i, node['channel_indices'][:3]] + new_frames[i, node['channel_indices'][3:]] = np.rad2deg(euler_from_matrix(rot_mat, rotation_order_str)) + else: + rot_mat = euler_matrix(*np.deg2rad(self.frames[i, node['channel_indices']]), + axes=node['rotation_order']) + new_frames[i, node['channel_indices']] = np.rad2deg(euler_from_matrix(rot_mat, rotation_order_str)) + self.frames = new_frames + + def scale(self, scale): + for node in self.node_names: + self.node_names[node]["offset"] = [scale * o for o in self.node_names[node]["offset"]] + if "channels" not in self.node_names[node]: + continue + ch = [(node, c) for c in self.node_names[node]["channels"] if "position" in c] + if len(ch) > 0: + ch_indices = self.get_channel_indices(ch) + scaled_params = [scale * o for o in self.frames[:, ch_indices]] + self.frames[:, ch_indices] = scaled_params + + def get_channel_indices(self, node_channels): + """Returns indices for specified channels + + Parameters + ---------- + * node_channels: 2-tuples of strings + \tEach tuple contains joint name and channel name + \te.g. ("hip", "Xposition") + + """ + return [self.node_channels.index(nc) for nc in node_channels] + + def get_node_channels(self, node_name): + channels = None + if node_name in self.node_names and "channels" in self.node_names[node_name]: + channels = self.node_names[node_name]["channels"] + return channels + +class BVHReader(BVHData): + """Biovision file format class + + Parameters + ---------- + * filename: string + \t path to BVH file that is loaded initially + """ + def __init__(self, filename=""): + super().__init__() + if filename != "": + self.filename = os.path.split(filename)[-1] + with open(filename, "r") as file: + lines = file.readlines() + self.process_lines(lines) + + @classmethod + def init_from_string(cls, skeleton_string): + bvh_reader = cls(infilename="") + lines = skeleton_string.split("\n") + bvh_reader.process_lines(lines) + return bvh_reader + + def process_lines(self, lines): + """Reads lines from a BVH file + + Parameters + ---------- + * lines: list of strings + + """ + lines = [l for l in lines if l.strip() != ""] + line_index = 0 + n_lines = len(lines) + while line_index < n_lines: + if lines[line_index].startswith("HIERARCHY"): + line_index = self._read_skeleton(lines, line_index, n_lines) + if lines[line_index].startswith("MOTION") and n_lines > line_index+3: + self._read_frametime(lines, line_index+2) + line_index = self._read_frames(lines, line_index+3, n_lines) + else: + line_index += 1 + + def _read_skeleton(self, lines, line_index, n_lines): + """Reads the skeleton part of a BVH file""" + line_index = line_index + parents = [] + node_name = None + parent_name = None + self.node_names = OrderedDict() + self.node_channels = [] + while line_index < n_lines and not lines[line_index].startswith("MOTION"): + split_line = lines[line_index].strip().split() + if not split_line: + continue + if "{" in split_line: + parents.append(node_name) + elif "}" in split_line: + node_name = parents.pop(-1) + level = len(parents) + if level > 0: + parent_name = parents[-1] + node_name = self._read_split_joint_line(split_line, level, node_name, parent_name) + if self.root is None: + self.root = node_name + line_index += 1 + return line_index + + + def _read_split_joint_line(self, split_line, level, node_name, parent): + if split_line[0] == "ROOT" or split_line[0] == "JOINT": + node_name = split_line[1] + node_data = {"children": [], "level": level, "channels": [], "channel_indices": []} + self.node_names[node_name] = node_data + if parent is not None: + self.node_names[parent]["children"].append(node_name) + + elif split_line[0] == "CHANNELS": + for channel in split_line[2:]: + self.node_channels.append((node_name, channel)) + self.node_names[node_name]["channels"].append(channel) + self.node_names[node_name]["channel_indices"].append(len(self.node_channels) - 1) + + elif split_line == ["End", "Site"]: + node_name = node_name + "_" + "".join(split_line) + self.node_names[node_name] = {"level": level} + # also the end sites need to be adde as children + self.node_names[parent]["children"].append(node_name) + + elif split_line[0] == "OFFSET" and node_name is not None: + self.node_names[node_name]["offset"] = list(map(float, split_line[1:])) + return node_name + + def _read_frametime(self, lines, line_index): + """Reads the frametime part of a BVH file""" + if lines[line_index].startswith("Frame Time:"): + self.frame_time = float(lines[line_index].split(":")[-1].strip()) + + def _read_frames(self, lines, line_index, n_lines=-1): + """Reads the frames part of a BVH file""" + line_index = line_index + if n_lines == -1: + n_lines = len(lines) + frames = [] + while line_index < n_lines: + line_split = lines[line_index].strip().split() + frames.append(np.array(list(map(float, line_split)))) + line_index += 1 + + self.frames = np.array(frames) + return line_index + + + +def write_euler_frames_to_bvh_file(filename, skeleton, euler_frames, frame_time): + """ Write the hierarchy string and the frame parameter string to a file + Parameters + ---------- + * filename: String + Name of the created bvh file. + * skeleton: Skeleton + Skeleton structure needed to copy the hierarchy + * euler_frames: np.ndarray + array of pose rotation parameters in euler angles + * frame_time: float + time in seconds for the display of each keyframe + """ + bvh_string = generate_bvh_string(skeleton, euler_frames, frame_time) + if filename[-4:] == '.bvh': + filename = filename + else: + filename = filename + '.bvh' + with open(filename, 'w') as outfile: + outfile.write(bvh_string) + + +def generate_bvh_string(skeleton, euler_frames, frame_time): + """ Write the hierarchy string and the frame parameter string to a file + Parameters + ---------- + * skeleton: Skeleton + Skeleton structure needed to copy the hierarchy + * euler_frames: np.ndarray + array of pose rotation parameters in euler angles + * frame_time: float + time in seconds for the display of each keyframe + """ + bvh_string = _generate_hierarchy_string(skeleton) + "\n" + bvh_string += _generate_bvh_frame_string(euler_frames, frame_time) + return bvh_string + +def _generate_hierarchy_string(skeleton): + """ Initiates the recursive generation of the skeleton structure string + by calling _generate_joint_string with the root joint + """ + hierarchy_string = "HIERARCHY\n" + hierarchy_string += _generate_joint_string(skeleton.root, skeleton, 0) + return hierarchy_string + +def _generate_joint_string(joint, skeleton, joint_level): + """ Recursive traversing of the joint hierarchy to create a + skeleton structure string in the BVH format + """ + joint_string = "" + temp_level = 0 + tab_string = "" + while temp_level < joint_level: + tab_string += "\t" + temp_level += 1 + + # determine joint type + if joint_level == 0: + joint_string += tab_string + "ROOT " + joint + "\n" + else: + if len(skeleton.nodes[joint].children) > 0: + joint_string += tab_string + "JOINT " + joint + "\n" + else: + joint_string += tab_string + "End Site" + "\n" + # open bracket add offset + joint_string += tab_string + "{" + "\n" + offset = skeleton.nodes[joint].offset + joint_string += tab_string + "\t" + "OFFSET " + "\t " + \ + str(offset[0]) + "\t " + str(offset[1]) + "\t " + str(offset[2]) + "\n" + + if len(skeleton.nodes[joint].children) > 0: + # channel information + channels = skeleton.nodes[joint].channels + joint_string += tab_string + "\t" + \ + "CHANNELS " + str(len(channels)) + " " + for tok in channels: + joint_string += tok + " " + joint_string += "\n" + + joint_level += 1 + # recursive call for all children + for child in skeleton.nodes[joint].children: + joint_string += _generate_joint_string(child.node_name, skeleton, joint_level) + + # close the bracket + joint_string += tab_string + "}" + "\n" + return joint_string + +def _generate_bvh_frame_string(euler_frames, frame_time): + """ Converts a list of euler frames into the BVH file representation. + Parameters + ---------- + * euler_frames: np.ndarray + array of pose rotation parameters in euler angles + * frame_time: float + time in seconds for the display of each keyframe + """ + frame_parameter_string = "MOTION\n" + frame_parameter_string += "Frames: " + str(len(euler_frames)) + "\n" + frame_parameter_string += "Frame Time: " + str(frame_time) + "\n" + for frame in euler_frames: + frame_parameter_string += ' '.join([str(f) for f in frame]) + frame_parameter_string += '\n' + return frame_parameter_string + + +def convert_quaternion_to_euler_frames(skeleton, quat_frames): + """ Converts the joint rotations from quaternion to euler rotations + Parameters + ---------- + * skeleton: Skeleton + Skeleton structure needed to copy the hierarchy + * quat_frames: np.ndarray + array of pose rotation parameters as quaternion + """ + joint_order = [] + rotation_info = dict() + def get_joint_meta_info(joint_name, skeleton): + if len(skeleton.nodes[joint_name].children) > 0: + joint_order.append(joint_name) + rot_order = [] + offset = 0 + for idx, ch in enumerate(skeleton.nodes[joint_name].channels): + if ch.lower().endswith("rotation"): + rot_order.append(ch) + else: + offset += 1 + rotation_info[joint_name] = rot_order, offset + for child in skeleton.nodes[joint_name].children: + get_joint_meta_info(child.node_name, skeleton) + + get_joint_meta_info(skeleton.root, skeleton) + n_frames = len(quat_frames) + n_params = sum([len(skeleton.nodes[j].channels) for j in joint_order]) + euler_frames = np.zeros((n_frames, n_params)) + for frame_idx, quat_frame in enumerate(quat_frames): + euler_frames[frame_idx,:TRANSLATION_LEN] = quat_frame[:TRANSLATION_LEN] + dst = 0 # the translation offset will be added + for joint_name in joint_order: + channels = skeleton.nodes[joint_name].channels + n_channels = len(channels) + rotation_order = rotation_info[joint_name][0] + rotation_offset = rotation_info[joint_name][1] + src = skeleton.nodes[joint_name].index * QUAT_LEN + TRANSLATION_LEN + q = quat_frame[src:src+QUAT_LEN] + e = quaternion_to_euler(q, rotation_order) + params_start = dst + rotation_offset + params_end = params_start + EULER_LEN + euler_frames[frame_idx, params_start:params_end] = e + dst += n_channels + return euler_frames + + +class BVHWriter(object): + """Saves an input motion defined either as an array of euler or quaternion + frame vectors to a BVH file. + Legacy interface that calls write_euler_frames_to_bvh_file + + Parameters + ---------- + * filename: String or None + Name of the created bvh file. Can be None. + * skeleton: Skeleton + Skeleton structure needed to copy the hierarchy + * frame_data: np.ndarray + array of motion vectors, either with euler or quaternion as rotation parameters + * frame_time: float + time in seconds for the display of each keyframe + * is_quaternion: Boolean + Defines wether the frame_data is quaternion data or euler data + """ + def __init__(self, filename, skeleton, frame_data, frame_time, is_quaternion=False): + self.skeleton = skeleton + self.frame_data = frame_data + self.frame_time = frame_time + if is_quaternion: + self.frame_data = convert_quaternion_to_euler_frames(self.skeleton, self.frame_data) + if filename is not None: + self.write(filename) + + def write(self, filename): + write_euler_frames_to_bvh_file(filename, self.skeleton, self.frame_data, self.frame_time) + + def generate_bvh_string(self): + return generate_bvh_string(self.skeleton, self.frame_data, self.frame_time) diff --git a/build/lib/anim_utils/animation_data/constants.py b/build/lib/anim_utils/animation_data/constants.py new file mode 100644 index 0000000..3321aae --- /dev/null +++ b/build/lib/anim_utils/animation_data/constants.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +DEFAULT_ROTATION_ORDER = ['Xrotation','Yrotation','Zrotation'] +DEFAULT_SMOOTHING_WINDOW_SIZE = 20 +LEN_QUAT = 4 +LEN_ROOT_POS = 3 +LEN_EULER = 3 +ROTATION_TYPE_EULER = 0 +ROTATION_TYPE_QUATERNION = 1 +LEN_ROOT = 3 \ No newline at end of file diff --git a/build/lib/anim_utils/animation_data/fbx/__init__.py b/build/lib/anim_utils/animation_data/fbx/__init__.py new file mode 100644 index 0000000..688b3ca --- /dev/null +++ b/build/lib/anim_utils/animation_data/fbx/__init__.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +from anim_utils.animation_data.motion_vector import MotionVector +from anim_utils.animation_data.skeleton_builder import SkeletonBuilder + +has_fbx = True +try: + import FbxCommon +except ImportError as e: + has_fbx = False + print("Info: Disable FBX IO due to missing library") + pass + + +if has_fbx: + from .fbx_import import load_fbx_file + from .fbx_export import export_motion_vector_to_fbx_file +else: + def load_fbx_file(file_path): + raise NotImplementedError + + def export_motion_vector_to_fbx_file(skeleton, motion_vector, out_file_name): + raise NotImplementedError + + + +def load_skeleton_and_animations_from_fbx(file_path): + skeleton_def, animations = load_fbx_file(file_path) + skeleton = SkeletonBuilder().load_from_fbx_data(skeleton_def) + anim_names = list(animations.keys()) + motion_vectors = dict() + if len(anim_names) > 0: + anim_name = anim_names[0] + mv = MotionVector() + mv.from_fbx(animations[anim_name], skeleton.animated_joints) + motion_vectors[anim_name] = mv + return skeleton, motion_vectors + diff --git a/build/lib/anim_utils/animation_data/fbx/fbx_export.py b/build/lib/anim_utils/animation_data/fbx/fbx_export.py new file mode 100644 index 0000000..2f23a08 --- /dev/null +++ b/build/lib/anim_utils/animation_data/fbx/fbx_export.py @@ -0,0 +1,219 @@ +""" +Code to export an animated skeleton to an FBX file based on a skeleton and a motion vector. +The code is based on Example01 of the FBX SDK samples. +""" + +from transformations import euler_from_quaternion +import numpy as np +import FbxCommon +from fbx import * + + +def create_scene(sdk_manager, scene, skeleton, frames, frame_time): + info = FbxDocumentInfo.Create(sdk_manager, "SceneInfo") + info.mTitle = "MotionExportScene" + scene.SetSceneInfo(info) + root_node = create_skeleton(sdk_manager, "", skeleton) + scene.GetRootNode().LclTranslation.Set(FbxDouble3(0, 0, 0)) + scene.GetRootNode().AddChild(root_node) + set_rest_pose(sdk_manager, scene, root_node, skeleton) + set_animation_curves(scene, root_node, skeleton, frames, frame_time) + return root_node + + +def create_skeleton(sdk_manager, name, skeleton): + root_node = create_root_node(sdk_manager, name, skeleton) + return root_node + +def create_root_node(sdk_manager, name, skeleton): + skeleton_root = create_skeleton_nodes_recursively(sdk_manager, name, skeleton, skeleton.root) + node_type = FbxSkeleton.eRoot + node_attribute = FbxSkeleton.Create(sdk_manager, name) + node_attribute.SetSkeletonType(node_type) + skeleton_root.SetNodeAttribute(node_attribute) + return skeleton_root + +def create_root_node2(sdk_manager, name, skeleton): + node_type = FbxSkeleton.eRoot + node_attribute = FbxSkeleton.Create(sdk_manager, name) + node_attribute.SetSkeletonType(node_type) + extra_root_node = FbxNode.Create(sdk_manager, "Root") + extra_root_node.SetNodeAttribute(node_attribute) + skeleton_root = create_skeleton_nodes_recursively(sdk_manager, name, skeleton, skeleton.root) + extra_root_node.AddChild(skeleton_root) + return extra_root_node + +def create_skeleton_nodes_recursively(sdk_manager, skeleton_name, skeleton, node_name): + node = skeleton.nodes[node_name] + name = skeleton_name + node_name + skeleton_node_attribute = FbxSkeleton.Create(sdk_manager, skeleton_name) + node_type = FbxSkeleton.eLimbNode + skeleton_node_attribute.SetSkeletonType(node_type) + skeleton_node = FbxNode.Create(sdk_manager, name) + skeleton_node.SetNodeAttribute(skeleton_node_attribute) + t = FbxDouble3(node.offset[0], node.offset[1], node.offset[2]) + skeleton_node.LclTranslation.Set(t) + for c_node in node.children: + c_name = c_node.node_name + c_node = create_skeleton_nodes_recursively(sdk_manager, skeleton_name, skeleton, c_name) + skeleton_node.AddChild(c_node) + return skeleton_node + + +def set_rest_pose_recursively(pose, fbx_node, skeleton): + name = fbx_node.GetName() + if name in skeleton.nodes.keys(): + node = skeleton.nodes[name] + t = node.offset + + l_t = FbxVector4(t[0], t[1], t[2]) + l_t = FbxVector4(0,0,0) + l_r = FbxVector4(0.0, 0.0, 0.0) + l_s = FbxVector4(1.0, 1.0, 1.0) + + transform = FbxMatrix() + transform.SetTRS(l_t, l_r, l_s) + pose.Add(fbx_node, transform, True)# maybe set this to false + n_children = fbx_node.GetChildCount() + for idx in range(n_children): + c_node = fbx_node.GetChild(idx) + set_rest_pose_recursively(pose, c_node, skeleton) + + +def set_rest_pose(sdk_manager, scene, root_node, skeleton): + pose = FbxPose.Create(sdk_manager, "RestPose") + set_rest_pose_recursively(pose, root_node, skeleton) + scene.AddPose(pose) + + +def create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, dimension, dim_idx): + t = FbxTime() + curve = fbx_node.LclTranslation.GetCurve(anim_layer, dimension, True) + curve.KeyModifyBegin() + for idx, frame in enumerate(euler_frames): + t.SetSecondDouble(idx * frame_time) + key_index = curve.KeyAdd(t)[0] + curve.KeySetValue(key_index, frame[dim_idx]) + curve.KeySetInterpolation(key_index, FbxAnimCurveDef.eInterpolationConstant) + curve.KeyModifyEnd() + + +def create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, dimension, dim_idx): + t = FbxTime() + curve = fbx_node.LclRotation.GetCurve(anim_layer, dimension, True) + curve.KeyModifyBegin() + for idx, frame in enumerate(euler_frames): + t.SetSecondDouble(idx * frame_time) + key_index = curve.KeyAdd(t)[0] + curve.KeySetValue(key_index, frame[dim_idx]) + curve.KeySetInterpolation(key_index, FbxAnimCurveDef.eInterpolationConstant) + curve.KeyModifyEnd() + + +def create_translation_curves(fbx_node, anim_layer, euler_frames, frame_time): + create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, "X", 0) + create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, "Y", 1) + create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, "Z", 2) + + +def create_rotation_curves(fbx_node, anim_layer, skeleton, euler_frames, frame_time): + node_name = fbx_node.GetName() + if node_name not in skeleton.animated_joints: + return + node_idx = skeleton.animated_joints.index(node_name) + offset = node_idx * 3 + 3 + create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, "X", offset) + create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, "Y", offset+1) + create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, "Z", offset+2) + + +def add_rotation_curves_recursively(fbx_node, anim_layer, skeleton, euler_frames, frame_time): + create_rotation_curves(fbx_node, anim_layer, skeleton, euler_frames, frame_time) + n_children = fbx_node.GetChildCount() + for idx in range(n_children): + c_node = fbx_node.GetChild(idx) + add_rotation_curves_recursively(c_node, anim_layer, skeleton, euler_frames, frame_time) + + +def convert_quaternion_to_euler_frame(skeleton, frame): + n_dims = len(skeleton.animated_joints) * 3 + 3 + euler_frame = np.zeros(n_dims) + euler_frame[:3] = frame[:3] + target_offset = 3 + src_offset = 3 + for idx, node in enumerate(skeleton.animated_joints): + q = frame[src_offset:src_offset + 4] + e = euler_from_quaternion(q) + euler_frame[target_offset:target_offset + 3] = np.degrees(e) + target_offset += 3 + src_offset += 4 + return euler_frame + +def set_animation_curves(scene, root_node, skeleton, frames, frame_time): + + # convert frames from quaternion to euler + euler_frames = [] + for frame in frames: + euler_frame = convert_quaternion_to_euler_frame(skeleton, frame) + euler_frames.append(euler_frame) + + anim_stack_name = "default" + anim_stack = FbxAnimStack.Create(scene, anim_stack_name) + anim_layer = FbxAnimLayer.Create(scene, "Base Layer") + anim_stack.AddMember(anim_layer) + + create_translation_curves(root_node, anim_layer, euler_frames, frame_time) + add_rotation_curves_recursively(root_node, anim_layer, skeleton, euler_frames, frame_time) + + +def export_motion_vector_to_fbx_file(skeleton, motion_vector, out_file_name, fbx_axis_system=FbxAxisSystem.MayaZUp): + """ Save a scene with a single skeleton and animation curves for the root and joints + """ + sdk_manager, scene = FbxCommon.InitializeSdkObjects() + root_node = create_scene(sdk_manager, scene, skeleton, motion_vector.frames, motion_vector.frame_time) + fbx_axis_system.MayaZUp.ConvertScene(scene) + #sdk_manager.GetIOSettings().SetBoolProp(EXP_FBX_EMBEDDED, True) + save_scene(sdk_manager, scene, out_file_name, pEmbedMedia=False) + print("finished") + + sdk_manager.Destroy() + + +def save_scene(pSdkManager, pScene, pFilename, pFileFormat = -1, pEmbedMedia = False): + """ copied from fbx common + changed settings to a Blender compatible version according to + http://stackoverflow.com/questions/29833986/fbx-sdk-exporting-into-older-fbx-file-format + """ + + lExporter = FbxExporter.Create(pSdkManager, "") + lExporter.SetFileExportVersion("FBX201200", FbxSceneRenamer.eFBX_TO_FBX) + + if pFileFormat < 0 or pFileFormat >= pSdkManager.GetIOPluginRegistry().GetWriterFormatCount(): + pFileFormat = pSdkManager.GetIOPluginRegistry().GetNativeWriterFormat() + if not pEmbedMedia: + lFormatCount = pSdkManager.GetIOPluginRegistry().GetWriterFormatCount() + for lFormatIndex in range(lFormatCount): + if pSdkManager.GetIOPluginRegistry().WriterIsFBX(lFormatIndex): + lDesc = pSdkManager.GetIOPluginRegistry().GetWriterFormatDescription(lFormatIndex) + if "binary" in lDesc: + pFileFormat = lFormatIndex + break + + if not pSdkManager.GetIOSettings(): + ios = FbxIOSettings.Create(pSdkManager, IOSROOT) + pSdkManager.SetIOSettings(ios) + + pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_MATERIAL, True) + pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_TEXTURE, True) + pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_EMBEDDED, pEmbedMedia) + pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_SHAPE, True) + pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_GOBO, True) + pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_ANIMATION, True) + pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_GLOBAL_SETTINGS, True) + + result = lExporter.Initialize(pFilename, pFileFormat, pSdkManager.GetIOSettings()) + if result == True: + result = lExporter.Export(pScene) + + lExporter.Destroy() + return result \ No newline at end of file diff --git a/build/lib/anim_utils/animation_data/fbx/fbx_import.py b/build/lib/anim_utils/animation_data/fbx/fbx_import.py new file mode 100644 index 0000000..5226024 --- /dev/null +++ b/build/lib/anim_utils/animation_data/fbx/fbx_import.py @@ -0,0 +1,198 @@ +import FbxCommon +from fbx import * +import numpy as np +import collections +from transformations import quaternion_matrix, euler_from_quaternion, quaternion_from_euler +from anim_utils.animation_data.skeleton_node import SKELETON_NODE_TYPE_ROOT,SKELETON_NODE_TYPE_JOINT, SKELETON_NODE_TYPE_END_SITE + +def load_fbx_file(file_path): + importer = FBXImporter(file_path) + importer.load() + importer.destroy() + return importer.skeleton, importer.animations + + +class FBXImporter(object): + def __init__(self, file_path): + self.skeleton = None + self.skeleton_root_node = None + self.skinning_data = dict() + self.animations = dict() + self.mesh_list = [] + + (self.sdk_manager, self.fbx_scene) = FbxCommon.InitializeSdkObjects() + FbxCommon.LoadScene(self.sdk_manager, self.fbx_scene, file_path) + FbxAxisSystem.OpenGL.ConvertScene(self.fbx_scene) + + def destroy(self): + self.sdk_manager.Destroy() + + def load(self): + self.skeleton = None + self.skinning_data = dict() + self.animations = dict() + self.mesh_list = [] + root_node = self.fbx_scene.GetRootNode() + self.parseFBXNodeHierarchy(root_node, 0) + if self.skeleton_root_node is not None: + self.animations = self.read_animations(self.skeleton_root_node) + + + def parseFBXNodeHierarchy(self, fbx_node, depth): + + n_attributes = fbx_node.GetNodeAttributeCount() + for idx in range(n_attributes): + attribute = fbx_node.GetNodeAttributeByIndex(idx) + if self.skeleton is None and attribute.GetAttributeType() == FbxNodeAttribute.eSkeleton: + self.skeleton_root_node = fbx_node + self.skeleton = self.create_skeleton(fbx_node) + + for idx in range(fbx_node.GetChildCount()): + self.parseFBXNodeHierarchy(fbx_node.GetChild(idx), depth + 1) + + + def create_skeleton(self, node): + current_time = FbxTime() + current_time.SetFrame(0, FbxTime.eFrames24) + scale = node.EvaluateGlobalTransform().GetS()[0] + def add_child_node_recursively(skeleton, fbx_node): + node_name = fbx_node.GetName() + node_idx = len(skeleton["animated_joints"]) + localTransform = fbx_node.EvaluateLocalTransform(current_time) + #lT = localTransform.GetT() + o = fbx_node.LclTranslation.Get() + offset = scale*np.array([o[0], o[1], o[2]]) + q = localTransform.GetQ() + rotation = np.array([q[3],q[0], q[1], q[2]]) + + node = {"name": node_name, + "children": [], + "channels": [], + "offset": offset, + "rotation": rotation, + "fixed": True, + "index": -1, + "quaternion_frame_index": -1, + "node_type": SKELETON_NODE_TYPE_JOINT} + n_children = fbx_node.GetChildCount() + if n_children > 0: + node["channels"] = ["Xrotation", "Yrotation", "Zrotation"] + node["index"] = node_idx + node["quaternion_frame_index"] = node_idx + node["fixed"] = False + skeleton["animated_joints"].append(node_name) + for idx in range(n_children): + c_node = add_child_node_recursively(skeleton, fbx_node.GetChild(idx)) + node["children"].append(c_node["name"]) + skeleton["nodes"][node_name] = node + return node + + o = node.LclTranslation.Get() + offset = scale*np.array([o[0], o[1], o[2]]) + e = node.LclRotation.Get() + rotation = quaternion_from_euler(*e, axes='sxyz') + root_name = node.GetName() + skeleton = dict() + skeleton["animated_joints"] = [root_name] + skeleton["node_names"] = dict() + skeleton["nodes"] = collections.OrderedDict() + skeleton["frame_time"] = 0.013889 + root_node = {"name": root_name, + "children": [], + "channels": ["Xposition", "Yposition", "Zposition", + "Xrotation", "Yrotation", "Zrotation"], + "offset": offset, + "rotation": rotation, + "fixed": False, + "index": 0, + "quaternion_frame_index": 0, + "inv_bind_pose": np.eye(4), + "node_type": SKELETON_NODE_TYPE_ROOT} + + skeleton["nodes"][root_name] = root_node + for idx in range(node.GetChildCount()): + c_node = add_child_node_recursively(skeleton, node.GetChild(idx)) + root_node["children"].append(c_node["name"]) + + skeleton["root"] = root_name + print('animated_joints', len(skeleton["animated_joints"])) + return skeleton + + + def read_animations(self, temp_node): + """src: http://gamedev.stackexchange.com/questions/59419/c-fbx-animation-importer-using-the-fbx-sdk + """ + anims = dict() + count = self.fbx_scene.GetSrcObjectCount(FbxCriteria.ObjectType(FbxAnimStack.ClassId)) + for idx in range(count): + anim = dict() + anim_stack = self.fbx_scene.GetSrcObject(FbxCriteria.ObjectType(FbxAnimStack.ClassId), idx) + self.fbx_scene.SetCurrentAnimationStack(anim_stack) + anim_name = anim_stack.GetName() + anim_layer = anim_stack.GetMember(FbxCriteria.ObjectType(FbxAnimLayer.ClassId), 0) + mLocalTimeSpan = anim_stack.GetLocalTimeSpan() + start = mLocalTimeSpan.GetStart() + end = mLocalTimeSpan.GetStop() + anim["n_frames"] = end.GetFrameCount(FbxTime.eFrames24) - start.GetFrameCount(FbxTime.eFrames24) + 1 + anim["duration"] = end.GetSecondCount() - start.GetSecondCount() + anim["frame_time"] = anim["duration"]/anim["n_frames"] # 0.013889 + anim["curves"] = collections.OrderedDict() + print("found animation", anim_name, anim["n_frames"], anim["duration"]) + is_root = True + nodes = [] + while temp_node is not None: + name = temp_node.GetName() + if has_curve(anim_layer, temp_node): + if is_root: + anim["curves"][name] = get_global_anim_curve(temp_node, start, end) + is_root = False + else: + anim["curves"][name] = get_local_anim_curve(temp_node, start, end) + for i in range(temp_node.GetChildCount()): + nodes.append(temp_node.GetChild(i)) + if len(nodes) > 0: + temp_node = nodes.pop(0) + else: + temp_node = None + anims[anim_name] = anim + return anims + +def get_local_anim_curve(node, start, end): + curve= [] + current_t = FbxTime() + for frame_idx in range(start.GetFrameCount(FbxTime.eFrames24), end.GetFrameCount(FbxTime.eFrames24)): + current_t.SetFrame(frame_idx, FbxTime.eFrames24) + local_transform = node.EvaluateLocalTransform(current_t) + q = local_transform.GetQ() + t = local_transform.GetT() + transform = {"rotation": [q[3], q[0], q[1], q[2]], + "translation": [t[0], t[1], t[2]]} + curve.append(transform) + return curve + +def get_global_anim_curve(node, start, end): + curve= [] + current_t = FbxTime() + for frame_idx in range(start.GetFrameCount(FbxTime.eFrames24), end.GetFrameCount(FbxTime.eFrames24)): + current_t.SetFrame(frame_idx, FbxTime.eFrames24) + local_transform = node.EvaluateGlobalTransform(current_t) + q = local_transform.GetQ() + t = local_transform.GetT() + transform = {"rotation": [q[3], q[0], q[1], q[2]], + "translation": [t[0], t[1], t[2]]} + curve.append(transform) + return curve + +def has_curve(anim_layer, node): + translation = node.LclTranslation.GetCurve(anim_layer, "X") + rotation = node.LclRotation.GetCurve(anim_layer, "X") + return rotation is not None or translation is not None + + +def FBXMatrixToNumpy(m): + q = m.GetQ() + t = m.GetT() + m = quaternion_matrix([q[0], q[1], q[2], q[3]]) + m[:3,3] = t[0], t[1],t[2] + return m + diff --git a/build/lib/anim_utils/animation_data/joint_constraints.py b/build/lib/anim_utils/animation_data/joint_constraints.py new file mode 100644 index 0000000..aafe6d3 --- /dev/null +++ b/build/lib/anim_utils/animation_data/joint_constraints.py @@ -0,0 +1,487 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +import math +from math import acos, cos, sin, atan +from transformations import quaternion_about_axis, quaternion_inverse, quaternion_multiply, euler_from_quaternion + + +def normalize(v): + return v/np.linalg.norm(v) + +def quaternion_to_axis_angle(q): + """http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/ + + """ + a = 2* math.acos(q[0]) + s = math.sqrt(1- q[0]*q[0]) + if s < 0.001: + x = q[1] + y = q[2] + z = q[3] + else: + x = q[1] / s + y = q[2] / s + z = q[3] / s + v = np.array([x,y,z]) + if np.sum(v)> 0: + return normalize(v),a + else: + return v, a + + + +def swing_twist_decomposition(q, twist_axis): + """ code by janis sprenger based on + Dobrowsolski 2015 Swing-twist decomposition in Clifford algebra. https://arxiv.org/abs/1506.05481 + """ + q = normalize(q) + #twist_axis = np.array((q * offset))[0] + projection = np.dot(twist_axis, np.array([q[1], q[2], q[3]])) * twist_axis + twist_q = np.array([q[0], projection[0], projection[1],projection[2]]) + if np.linalg.norm(twist_q) == 0: + twist_q = np.array([1,0,0,0]) + twist_q = normalize(twist_q) + swing_q = quaternion_multiply(q, quaternion_inverse(twist_q))#q * quaternion_inverse(twist) + return swing_q, twist_q + +OPENGL_UP = np.array([0,1,0]) + + + +def exponential(q): + """ https://math.stackexchange.com/questions/1030737/exponential-function-of-quaternion-derivation + chapter 2 of lee 2000 + """ + v = np.array([q[1], q[2], q[3]]) + theta = np.linalg.norm(v) + return (cos(theta) + v/ theta * sin(theta))# * np.exp(q[0]) + +def log(q): + """ https://math.stackexchange.com/questions/1030737/exponential-function-of-quaternion-derivation + chapter 2 of lee 2000 + """ + v = np.array([q[1], q[2], q[3]]) + w = q[0] + if w == 0: + return np.pi/2 * v + elif 0 < abs(w) < 1: + theta = np.linalg.norm(v) + return v / theta * atan(theta/w) + else: # w == 1 + return 0 + +def dist(q1, q2): + """ chapter 2 of lee 2000""" + return np.linalg.norm(log(quaternion_multiply(quaternion_inverse(q1), q2))) + +def rotate_vector_by_quaternion(v, q): + vq = [0, v[0], v[1], v[2]] + v_prime = quaternion_multiply(q, quaternion_multiply(vq , quaternion_inverse(q)))[1:] + v_prime /= np.linalg.norm(v_prime) + return v_prime + + +def apply_conic_constraint(q, ref_q, axis, k): + """ lee 2000 p. 48""" + q0 = ref_q + w = axis + #rel_q = quaternion_multiply(quaternion_inverse(q0), q) + #rel_q /np.linalg.norm(rel_q) + w_prime = rotate_vector_by_quaternion(w, q) + w_prime = normalize(w_prime) + phi = acos(np.dot(w, w_prime)) + if 0 < phi > k: # apply + delta_phi = k - phi + #print(delta_phi) + v = np.cross(w, w_prime) + delta_q = quaternion_about_axis(delta_phi, v) + q = quaternion_multiply(delta_q, q) + return q + + +def apply_axial_constraint(q, ref_q, axis, k1, k2): + """ lee 2000 p. 48""" + q0 = ref_q + w = axis + #rel_q = quaternion_multiply(quaternion_inverse(q0), q) + #rel_q = normalize(rel_q) + w_prime = rotate_vector_by_quaternion(w, q) + v = np.cross(w, w_prime) + phi = acos(np.dot(w, w_prime)) + + #remove rotation around v + # psi_q = exp(-phi*v) + inv_phi_q = quaternion_about_axis(-phi, v) + psi_q = quaternion_multiply(inv_phi_q, ref_q) + + # get angle around w from psi_q + w_prime2 = rotate_vector_by_quaternion(w, psi_q) + w_prime2 = normalize(w_prime2) + psi = acos(np.dot(w, w_prime2)) + if k1 < psi > k2: # apply + delta_psi1 = k1 - psi + delta_psi2 = k2 - psi + delta_psi = min(delta_psi1, delta_psi2) + delta_q = quaternion_about_axis(delta_psi, w) + q = quaternion_multiply(delta_q, q) + print("apply", psi) + return q + +def apply_axial_constraint2(q, ref_q, axis, min_angle, max_angle): + """ get delta orientation + do axis decomposition + restrict angle between min and max + """ + q = normalize(q) + inv_ref_q = quaternion_inverse(ref_q) + inv_ref_q = normalize(inv_ref_q) + delta_q = quaternion_multiply(inv_ref_q, q) + delta_q = normalize(delta_q) + swing_q, twist_q = swing_twist_decomposition(delta_q, axis) + swing_q = normalize(swing_q) + twist_q = normalize(twist_q) + v, a = quaternion_to_axis_angle(twist_q) + print("apply axial", q) + v2, a2 = quaternion_to_axis_angle(swing_q) + sign = 1 + if np.sum(v) < 0: + sign = -1 + a *= sign + print("twist swing", v, np.degrees(a), v2, np.degrees(a2)) + print("boundary", min_angle, max_angle) + a = max(a, min_angle) + a = min(a, max_angle) + new_twist_q = quaternion_about_axis(a, axis) + new_twist_q = normalize(new_twist_q) + delta_q = quaternion_multiply(swing_q, new_twist_q) + delta_q = normalize(delta_q) + q = quaternion_multiply(ref_q, delta_q) + q = normalize(q) + return q + + +def apply_head_constraint(q, ref_q, axis, t_min_angle, t_max_angle, s_min_angle, s_max_angle): + """ get delta orientation + do axis decomposition + restrict angle between min and max + """ + q = normalize(q) + inv_ref_q = quaternion_inverse(ref_q) + inv_ref_q = normalize(inv_ref_q) + delta_q = quaternion_multiply(inv_ref_q, q) + delta_q = normalize(delta_q) + swing_q, twist_q = swing_twist_decomposition(delta_q, axis) + swing_q = normalize(swing_q) + twist_q = normalize(twist_q) + v, a = quaternion_to_axis_angle(twist_q) + sign = 1 + if np.sum(v) < 0: + sign = -1 + a *= sign + v*=sign + #print("apply axial", q) + v2, a2 = quaternion_to_axis_angle(swing_q) + sign = 1 + if np.sum(v2) < 0: + sign = -1 + a2 *= sign + v2*= sign + #print("twist swing", v, np.degrees(a), v2, np.degrees(a2)) + #print("boundary", min_angle, max_angle) + a = max(a, t_min_angle) + a = min(a, t_max_angle) + new_twist_q = quaternion_about_axis(a, axis) + new_twist_q = normalize(new_twist_q) + a2 = max(a2, s_min_angle) + a2 = min(a2, s_max_angle) + new_swing_q = quaternion_about_axis(a2, v2) + new_swing_q = normalize(new_swing_q) + delta_q = quaternion_multiply(new_swing_q, new_twist_q) + delta_q = normalize(delta_q) + q = quaternion_multiply(ref_q, delta_q) + q = normalize(q) + return q + + +def apply_spherical_constraint(q, ref_q, axis, k): + """ lee 2000 p. 48""" + q0 = ref_q + w = axis + rel_q = quaternion_multiply(quaternion_inverse(q0), q) + rel_q /np.linalg.norm(rel_q) + w_prime = rotate_vector_by_quaternion(w, rel_q) + v = np.cross(w, w_prime) + v = normalize(v) + log_rel_q = log(rel_q) + phi = acos(np.dot(w, w_prime)) + if 2*np.linalg.norm(log_rel_q) > k: # apply + delta_phi = k - phi + delta_q = quaternion_about_axis(delta_phi, v) + delta_q = normalize(delta_q) + q = quaternion_multiply(delta_q, q) + return normalize(q) + +REFERENCE_QUATERNION = [1,0,0,0] + + +class HingeConstraint(object): + def __init__(self, axis, deg_angle_range): + self.axis = axis + self.angle_range = np.radians(deg_angle_range) + + def apply(self, q, parent_q): + #axis = rotate_vector_by_quaternion(self.axis, parent_q) + return apply_axial_constraint(q, parent_q, self.axis, self.angle_range[0], self.angle_range[1]) + + +class JointConstraint(object): + def __init__(self): + self.is_static = False + + @classmethod + def from_dict(cls, joint_name, data): + return JointConstraint() + + def get_axis(self): + return OPENGL_UP + + def apply(self, q): + return q + +class StaticConstraint(JointConstraint): + def __init__(self): + JointConstraint.__init__(self) + self.is_static = True + + @classmethod + def from_dict(cls, joint_name, data): + return StaticConstraint() + +class BallSocketConstraint(JointConstraint): + def __init__(self, axis, k): + JointConstraint.__init__(self) + self.axis = axis + self.k = k + + @classmethod + def from_dict(cls, joint_name, data): + axis = np.array(data["axis"]) + k = np.radians(data["k"]) + c = BallSocketConstraint(axis, k) + return c + + def apply(self, q): + ref_q = [1,0,0,0] + return apply_spherical_constraint(q, ref_q, self.axis, self.k) + + + def get_axis(self): + return self.axis + + +class ConeConstraint(JointConstraint): + def __init__(self, axis, k): + JointConstraint.__init__(self) + self.axis = axis + self.k = k + + @classmethod + def from_dict(cls, joint_name, data): + axis = np.array(data["axis"]) + k = np.radians(data["k"]) + c = ConeConstraint(axis, k) + return c + + def apply(self, q): + ref_q = [1, 0, 0, 0] + return apply_conic_constraint(q, ref_q, self.axis, self.k) + + def get_axis(self): + return self.axis + +class SpineConstraint(JointConstraint): + def __init__(self, ref_q, axis, twist_min, twist_max, swing_min, swing_max): + JointConstraint.__init__(self) + self.ref_q = ref_q + self.axis = axis + self.twist_min = twist_min + self.twist_max = twist_max + self.swing_min = swing_min + self.swing_max = swing_max + self.joint_name = "" + + @classmethod + def from_dict(cls, joint_name, data): + axis = np.array(data["axis"]) + tk1 = np.radians(data["tk1"]) + tk2 = np.radians(data["tk2"]) + sk1 = np.radians(data["sk1"]) + sk2 = np.radians(data["sk2"]) + ref_q = [1,0,0,0] + c = SpineConstraint(ref_q, axis, tk1, tk2, sk1, sk2) + c.joint_name = joint_name + return c + + def apply(self, q): + #print("apply spine constraint",self.joint_name, q) + #q = apply_spine_constraint(q, self.ref_q, self.axis, self.twist_min, self.twist_max, self.swing_min, self.swing_max) + #print() + return q + + def get_axis(self): + return self.axis + +class HeadConstraint(JointConstraint): + def __init__(self, ref_q, axis, twist_min, twist_max, swing_min, swing_max): + JointConstraint.__init__(self) + self.ref_q = ref_q + self.axis = axis + self.twist_min = twist_min + self.twist_max = twist_max + self.swing_min = swing_min + self.swing_max = swing_max + self.joint_name = "" + + @classmethod + def from_dict(cls, joint_name, data): + axis = np.array(data["axis"]) + tk1 = np.radians(data["tk1"]) + tk2 = np.radians(data["tk2"]) + sk1 = np.radians(data["sk1"]) + sk2 = np.radians(data["sk2"]) + ref_q = [1,0,0,0] + c = HeadConstraint(ref_q, axis, tk1, tk2, sk1, sk2) + c.joint_name = joint_name + return c + + def apply(self, q): + q = apply_head_constraint(q, self.ref_q, self.axis, self.twist_min, self.twist_max, self.swing_min, self.swing_max) + return q + + def get_axis(self): + return self.axis + +class HingeConstraint2(JointConstraint): + def __init__(self, swing_axis, twist_axis, deg_angle_range=None, verbose=False): + JointConstraint.__init__(self) + self.swing_axis = swing_axis + self.twist_axis = twist_axis + if deg_angle_range is not None: + self.angle_range = np.radians(deg_angle_range) + else: + self.angle_range = None + self.verbose = False + + @classmethod + def from_dict(cls, joint_name, data): + swing_axis = np.array(data["swing_axis"]) + twist_axis = np.array(data["twist_axis"]) + deg_angle_range = None + if "k1" in data and "k2" in data: + deg_angle_range = [data["k1"], data["k2"]] + c = HingeConstraint2(swing_axis, twist_axis, deg_angle_range) + return c + + def apply(self, q): + sq, tq = swing_twist_decomposition(q, self.swing_axis) + tv, ta = quaternion_to_axis_angle(tq) + if self.verbose: + print("before", np.degrees(ta), tv) + if self.angle_range is not None: + ta = max(ta, self.angle_range[0]) + ta = min(ta, self.angle_range[1]) + + if self.verbose: + print("after", np.degrees(ta), tv) + return quaternion_about_axis(ta, self.swing_axis) + + def apply_global(self, pm, q): + axis = np.dot(pm, self.axis) + axis = normalize(axis) + sq, tq = swing_twist_decomposition(q, axis) + tv, ta = quaternion_to_axis_angle(tq) + if self.verbose: + print("before", np.degrees(ta), tv) + if self.angle_range is not None: + ta = max(ta, self.angle_range[0]) + ta = min(ta, self.angle_range[1]) + + if self.verbose: + print("after", np.degrees(ta), tv) + return quaternion_about_axis(ta, self.swing_axis) + + def split(self,q): + axis = normalize(self.swing_axis) + sq, tq = swing_twist_decomposition(q, axis) + return sq, tq + + def split_correct(self,q): + axis = normalize(self.twist_axis) + sq, tq = swing_twist_decomposition(q, axis) + return sq, tq + + def get_axis(self): + return self.swing_axis + + +class ShoulderConstraint(JointConstraint): + """ combines conic and axial""" + def __init__(self, axis, k1,k2, k): + JointConstraint.__init__(self) + self.axis = axis + self.k1 = k1 + self.k2 = k2 + self.k = k + + @classmethod + def from_dict(cls, joint_name, data): + axis = np.array(data["axis"]) + k = np.radians(data["k"]) + k1 = np.radians(data["k1"]) + k2 = np.radians(data["k2"]) + #print("add shoulder socket constraint to", joint_name) + c = ShoulderConstraint(axis, k, k1, k2) + return c + + def apply(self, q): + ref_q = [1, 0, 0, 0] + q = apply_conic_constraint(q, ref_q, self.axis, self.k) + q = normalize(q) + #q = apply_axial_constraint(q, ref_q, self.axis, self.k1, self.k2) + return q + + def get_axis(self): + return self.axis + + + +CONSTRAINT_MAP= dict() +CONSTRAINT_MAP["static"] = StaticConstraint +CONSTRAINT_MAP["hinge"] = HingeConstraint2 +CONSTRAINT_MAP["ball"] = BallSocketConstraint +CONSTRAINT_MAP["cone"] = ConeConstraint +CONSTRAINT_MAP["shoulder"] = ShoulderConstraint +CONSTRAINT_MAP["head"] = HeadConstraint +CONSTRAINT_MAP["spine"] = SpineConstraint + diff --git a/build/lib/anim_utils/animation_data/motion_blending.py b/build/lib/anim_utils/animation_data/motion_blending.py new file mode 100644 index 0000000..ddcd8de --- /dev/null +++ b/build/lib/anim_utils/animation_data/motion_blending.py @@ -0,0 +1,615 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +from transformations import quaternion_slerp +import numpy as np +from copy import deepcopy +from .constants import LEN_QUAT, LEN_ROOT_POS + + +BLEND_DIRECTION_FORWARD = 0 +BLEND_DIRECTION_BACKWARD = 1 + + +def smooth_root_positions(positions, window): + h_window = int(window/2) + smoothed_positions = [] + n_pos = len(positions) + for idx, p in enumerate(positions): + start = max(idx-h_window, 0) + end = min(idx + h_window, n_pos) + #print start, end, positions[start:end] + avg_p = np.average(positions[start:end], axis=0) + smoothed_positions.append(avg_p) + return smoothed_positions + + +def blend_quaternion(a, b, w): + return quaternion_slerp(a, b, w, spin=0, shortestpath=True) + + +def smooth_joints_around_transition_using_slerp(quat_frames, joint_param_indices, discontinuity, window): + h_window = int(window/2) + start_frame = max(discontinuity-h_window, 0) + end_frame = min(discontinuity+h_window, len(quat_frames)-1) + start_window = discontinuity-start_frame + end_window = end_frame-discontinuity + if start_window > 0: + create_transition_for_joints_using_slerp(quat_frames, joint_param_indices, start_frame, discontinuity, start_window, BLEND_DIRECTION_FORWARD) + if end_window > 0: + create_transition_for_joints_using_slerp(quat_frames, joint_param_indices, discontinuity, end_frame, end_window, BLEND_DIRECTION_BACKWARD) + + + +def create_transition_for_joints_using_slerp(quat_frames, joint_param_indices, start_frame, end_frame, steps, direction=BLEND_DIRECTION_FORWARD): + new_quats = create_frames_using_slerp(quat_frames, start_frame, end_frame, steps, joint_param_indices) + for i in range(steps): + if direction == BLEND_DIRECTION_FORWARD: + t = float(i)/steps + else: + t = 1.0 - (i / steps) + old_quat = quat_frames[start_frame+i, joint_param_indices] + blended_quat = blend_quaternion(old_quat, new_quats[i], t) + quat_frames[start_frame + i, joint_param_indices] = blended_quat + return quat_frames + + +def smooth_quaternion_frames_using_slerp_(quat_frames, joint_parameter_indices, event_frame, window): + start_frame = event_frame-window/2 + end_frame = event_frame+window/2 + start_q = quat_frames[start_frame, joint_parameter_indices] + end_q = quat_frames[end_frame, joint_parameter_indices] + for i in range(window): + t = float(i)/window + #nlerp_q = self.nlerp(start_q, end_q, t) + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + #print "slerp",start_q, end_q, t, nlerp_q, slerp_q + quat_frames[start_frame+i, joint_parameter_indices] = slerp_q + + +def smooth_quaternion_frames_using_slerp_old(quat_frames, joint_param_indices, event_frame, window): + h_window = window/2 + start_frame = max(event_frame-h_window, 0) + end_frame = min(event_frame+h_window, quat_frames.shape[0]-1) + # create transition frames + from_start_to_event = create_frames_using_slerp(quat_frames, start_frame, event_frame, h_window, joint_param_indices) + from_event_to_end = create_frames_using_slerp(quat_frames, event_frame, end_frame, h_window, joint_param_indices) + + #blend transition frames with original frames + steps = event_frame-start_frame + for i in range(steps): + t = float(i)/steps + quat_frames[start_frame+i, joint_param_indices] = blend_quaternion(quat_frames[start_frame+i, joint_param_indices], from_start_to_event[i], t) + + steps = end_frame-event_frame + for i in range(steps): + t = 1.0-(i/steps) + quat_frames[event_frame+i, joint_param_indices] = blend_quaternion(quat_frames[start_frame+i, joint_param_indices], from_event_to_end[i], t) + + +def smooth_quaternion_frames_using_slerp_overwrite_frames(quat_frames, joint_param_indices, event_frame, window): + h_window = window/2 + start_frame = max(event_frame-h_window, 0) + end_frame = min(event_frame+h_window, quat_frames.shape[0]-1) + create_transition_using_slerp(quat_frames, start_frame, event_frame, joint_param_indices) + create_transition_using_slerp(quat_frames, event_frame, end_frame, joint_param_indices) + + +def blend_frames(quat_frames, start, end, new_frames, joint_parameter_indices): + steps = end-start + for i in range(steps): + t = i/steps + quat_frames[start+i, joint_parameter_indices] = blend_quaternion(quat_frames[start+i, joint_parameter_indices], new_frames[i], t) + + +def create_frames_using_slerp(quat_frames, start_frame, end_frame, steps, joint_parameter_indices): + start_q = quat_frames[start_frame, joint_parameter_indices] + end_q = quat_frames[end_frame, joint_parameter_indices] + frames = [] + for i in range(steps): + t = float(i)/steps + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + frames.append(slerp_q) + return frames + + +def create_transition_using_slerp(quat_frames, start_frame, end_frame, q_indices): + start_q = quat_frames[start_frame, q_indices] + end_q = quat_frames[end_frame, q_indices] + steps = end_frame-start_frame + for i in range(steps): + t = float(i)/steps + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + quat_frames[start_frame+i, q_indices] = slerp_q + +def create_transition_using_slerp_forward(quat_frames, start_frame, end_frame, q_indices): + end_q = quat_frames[end_frame, q_indices] + steps = end_frame - start_frame + for i in range(steps): + t = float(i) / steps + start_q = quat_frames[i, q_indices] + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + quat_frames[start_frame + i, q_indices] = slerp_q + + +def create_transition_using_slerp_backward(quat_frames, start_frame, end_frame, q_indices): + start_q = quat_frames[start_frame, q_indices] + steps = end_frame - start_frame + for i in range(steps): + t = float(i) / steps + end_q = quat_frames[i, q_indices] + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + quat_frames[start_frame + i, q_indices] = slerp_q + +def smooth_quaternion_frames(frames, discontinuity, window=20, include_root=True): + """ Smooth quaternion frames given discontinuity frame + + Parameters + ---------- + frames: list + \tA list of quaternion frames + discontinuity : int + The frame where the discontinuity is. (e.g. the transitionframe) + window : (optional) int, default is 20 + The smoothing window + include_root: (optional) bool, default is False + \tSets whether or not smoothing is applied on the x and z dimensions of the root translation + Returns + ------- + None. + """ + n_joints = int((len(frames[0]) - 3) / 4) + # smooth quaternion + n_frames = len(frames) + for i in range(n_joints): + for j in range(n_frames - 1): + start = 3 + i * 4 + end = 3 + (i + 1) * 4 + q1 = np.array(frames[j][start: end]) + q2 = np.array(frames[j + 1][start:end]) + if np.dot(q1, q2) < 0: + frames[j + 1][start:end] = -frames[j + 1][start:end] + + smoothing_factors = generate_smoothing_factors(discontinuity, window, n_frames) + d = int(discontinuity) + dofs = list(range(len(frames[0])))[3:] + if include_root: + dofs = [0,1,2] + dofs + else: + dofs = [1] + dofs + new_frames = np.array(frames) + for dof_idx in dofs: + curve = np.array(frames[:, dof_idx]) # extract dof curve + magnitude = curve[d] - curve[d - 1] + #print(dof_idx, magnitude, d) + new_frames[:, dof_idx] = curve + (magnitude * smoothing_factors) + return new_frames + + + +def slerp_quaternion_frame(frame_a, frame_b, weight): + frame_a = np.asarray(frame_a) + frame_b = np.asarray(frame_b) + assert len(frame_a) == len(frame_b) + n_joints = int((len(frame_a) - 3) / 4) + new_frame = np.zeros(len(frame_a)) + # linear interpolate root translation + new_frame[:3] = (1 - weight) * frame_a[:3] + weight * frame_b[:3] + for i in range(n_joints): + new_frame[3+ i*4 : 3 + (i+1) * 4] = quaternion_slerp(frame_a[3 + i*4 : 3 + (i+1) * 4], + frame_b[3 + i*4 : 3 + (i+1) * 4], + weight) + return new_frame + + +def smooth_quaternion_frames_with_slerp(frames, discontinuity, window=20): + n_frames = len(frames) + d = float(discontinuity) + ref_pose = slerp_quaternion_frame(frames[int(d)-1], frames[int(d)], 0.5) + w = float(window) + new_quaternion_frames = [] + for f in range(n_frames): + if f < d - w: + new_quaternion_frames.append(frames[f]) + elif d - w <= f < d: + tmp = (f - d + w) / w + weight = 2 * (0.5 * tmp ** 2) + new_quaternion_frames.append(slerp_quaternion_frame(frames[f], ref_pose, weight)) + elif d <= f <= d + w: + tmp = (f - d + w) / w + weight = 2 * (0.5 * tmp ** 2 - 2 * tmp + 2) + new_quaternion_frames.append(slerp_quaternion_frame(frames[f], ref_pose, weight)) + else: + new_quaternion_frames.append(frames[f]) + return np.asarray(new_quaternion_frames) + + +def smooth_quaternion_frames_with_slerp2(skeleton, frames, d, smoothing_window): + ''' + + :param new_frames (numpy.array): n_frames * n_dims + :param prev_frames (numpy.array): n_frames * n_dims + :param smoothing_window: + :return: + ''' + smooth_translation_in_quat_frames(frames, d, smoothing_window) + for joint_idx, joint_name in enumerate(skeleton.animated_joints): + start = joint_idx*4+3 + joint_indices = list(range(start, start+4)) + smooth_joints_around_transition_using_slerp(frames, joint_indices, d, smoothing_window) + return frames + + +def generate_smoothing_factors(discontinuity, window, n_frames): + """ Generate curve of smoothing factors + """ + d = float(discontinuity) + w = float(window) + smoothing_factors = [] + for f in range(n_frames): + value = 0.0 + if d - w <= f < d: + tmp = (f - d + w) / w + value = 0.5 * tmp ** 2 + elif d <= f <= d + w: + tmp = (f - d + w) / w + value = -0.5 * tmp ** 2 + 2 * tmp - 2 + smoothing_factors.append(value) + return np.array(smoothing_factors) + + +def smooth_quaternion_frames_joint_filter(skeleton, frames, discontinuity, joints, window=20): + """ Smooth quaternion frames given discontinuity frame + + Parameters + ---------- + skeleton: Skeleton + frames: list + \tA list of quaternion frames + discontinuity : int + The frame where the discontinuity is. (e.g. the transitionframe) + joints: list + \tA list of strings + window : (optional) int, default is 20 + The smoothing window + Returns + ------- + None. + """ + + n_frames = len(frames) + # generate curve of smoothing factors + smoothing_factors = generate_smoothing_factors(discontinuity, window, n_frames) + + # align quaternions and extract dofs + dof_filter_list = [] + if skeleton.root in joints: + dof_filter_list += [0,1,2] + for idx, j in enumerate(joints): + j_idx = skeleton.animated_joints.index(j) + q_start_idx = 3 + j_idx * 4 + q_end_idx = q_start_idx + 4 + dof_filter_list += [q_start_idx, q_start_idx + 1, q_start_idx + 2, q_start_idx + 3] + for f in range(n_frames - 1): + q1 = np.array(frames[f][q_start_idx: q_end_idx]) + q2 = np.array(frames[f + 1][q_start_idx:q_end_idx]) + if np.dot(q1, q2) < 0: + frames[f + 1][q_start_idx:q_end_idx] = -q2 + d = int(discontinuity) + new_frames = np.array(frames) + for dof_idx in dof_filter_list: + current_curve = np.array(frames[:, dof_idx]) # extract dof curve + magnitude = current_curve[d] - current_curve[d - 1] + new_curve = current_curve + (magnitude * smoothing_factors) + new_frames[:, dof_idx] = new_curve + return new_frames + +def smooth_quaternion_frames2(skeleton, frames, discontinuity, window=20): + """ Smooth quaternion frames given discontinuity frame + + Parameters + ---------- + skeleton: Skeleton + frames: list + \tA list of quaternion frames + discontinuity : int + The frame where the discontinuity is. (e.g. the transitionframe) + window : (optional) int, default is 20 + The smoothing window + Returns + ------- + None. + """ + + n_frames = len(frames) + smoothing_factors = generate_smoothing_factors(discontinuity, window, n_frames) + d = int(discontinuity) + new_frames = np.array(frames) + for idx in range(len(skeleton.animated_joints)): + o = idx *4 + 3 + dofs = [o, o+1, o+2, o+3] + q1 = frames[d, dofs] + q2 = frames[d+1, dofs] + if np.dot(q1, q2) < 0: + q2 = -q2 + magnitude = q1 - q2 + new_frames[:, dofs] += magnitude * smoothing_factors + return new_frames + +def smooth_translation_in_quat_frames(frames, discontinuity, window=20, only_height=False): + """ Smooth translation in quaternion frames given discontinuity frame + + Parameters + ---------- + frames: list + \tA list of quaternion frames + discontinuity : int + The frame where the discontinuity is. (e.g. the transitionframe) + window : (optional) int, default is 20 + The smoothing window + Returns + ------- + None. + """ + + n_frames = len(frames) + # generate curve of smoothing factors + d = int(discontinuity) + smoothing_factors = generate_smoothing_factors(d, window, n_frames) + if only_height: + dofs = [1] + else: + dofs = [0, 1, 2] + for dof_idx in dofs: + current_curve = np.array(frames[:, dof_idx]) # extract dof curve + magnitude = current_curve[d] - current_curve[d - 1] + new_curve = current_curve + (magnitude * smoothing_factors) + frames[:, dof_idx] = new_curve + return frames + + +def smooth_root_translation_around_transition(frames, d, window): + hwindow = int(window/2.0) + root_pos1 = frames[d-1, :3] + root_pos2 = frames[d, :3] + root_pos = (root_pos1 + root_pos2)/2 + start_idx = d-hwindow + end_idx = d + hwindow + start = frames[start_idx, :3] + end = root_pos + for i in range(hwindow): + t = float(i) / hwindow + frames[start_idx + i, :3] = start * (1 - t) + end * t + start = root_pos + end = frames[end_idx, :3] + for i in range(hwindow): + t = float(i) / hwindow + frames[d + i, :3] = start * (1 - t) + end * t + + +def linear_blending(ref_pose, quat_frames, skeleton, weights, joint_list=None): + ''' + Apply linear blending on quaternion motion data + :param ref_pose (numpy.array) + :param quat_frames: + :param skeleton (morphablegraphs.animation_data.Skeleton): + :param weights (numpy.array): weights used for slerp + :param joint_list (list): animated joint to be blended + :return: + ''' + if joint_list is None: + joint_list = skeleton.animated_joints + new_frames = deepcopy(quat_frames) + for i in range(len(quat_frames)): + for joint in joint_list: + joint_index = skeleton.nodes[joint].quaternion_frame_index + start_index = LEN_ROOT_POS + LEN_QUAT * joint_index + end_index = LEN_ROOT_POS + LEN_QUAT * (joint_index + 1) + ref_q = ref_pose[start_index: end_index] + motion_q = quat_frames[i, start_index: end_index] + new_frames[i, start_index: end_index] = quaternion_slerp(ref_q, motion_q, weights[i]) + return new_frames + + +def blend_quaternion_frames_linearly(new_frames, prev_frames, skeleton, smoothing_window=None): + ''' + Blend new frames linearly based on the last pose of previous frames + :param new_frames (Quaternion Frames): + :param prev_frames (Quaternion Frames): + :param skeleton (morphablegraphs.animation_data.Skeleton): + :param smoothing_window (int): smoothing window decides how many frames will be blended, if is None, then blend all + :return: + ''' + if smoothing_window is not None and smoothing_window != 0: + slerp_weights = np.linspace(0, 1, smoothing_window) + else: + slerp_weights = np.linspace(0, 1, len(new_frames)) + + return linear_blending(prev_frames[-1], new_frames, skeleton, slerp_weights) + + +def blend_between_frames(skeleton, frames, transition_start, transition_end, joint_list, window): + for c_joint in joint_list: + idx = skeleton.animated_joints.index(c_joint) * 4 + 3 + j_indices = [idx, idx + 1, idx + 2, idx + 3] + start_q = frames[transition_start][j_indices] + end_q = frames[transition_end][j_indices] + for i in range(window): + t = float(i) / window + t = (float(t) / window) + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + frames[transition_start + i][j_indices] = slerp_q + + +def generated_blend(start_q, end_q, window): + blend = np.zeros((window, 4)) + for i in range(window): + t = float(i) / window + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + blend[i] = slerp_q + return blend + + +def generate_blended_frames(skeleton, frames, start, end, joint_list, window): + blended_frames = deepcopy(frames[:]) + for c_joint in joint_list: + idx = skeleton.animated_joints.index(c_joint) * 4 + 3 + j_indices = [idx, idx + 1, idx + 2, idx + 3] + start_q = frames[start][j_indices] + end_q = frames[end][j_indices] + blended_qs = generated_blend(start_q, end_q, window) + for fi, q in enumerate(blended_qs): + blended_frames[start+fi][j_indices] = q + return blended_frames + + +def blend_quaternions_to_next_step(skeleton, frames, frame_idx, plant_joint, swing_joint, ik_chains, window): + start = frame_idx - window # end of blending range + end = frame_idx # modified frame + plant_ik_chain = ik_chains[plant_joint] + swing_ik_chain = ik_chains[swing_joint] + joint_list = [skeleton.root, "pelvis", plant_ik_chain["root"], plant_ik_chain["joint"], plant_joint, swing_ik_chain["root"], swing_ik_chain["joint"], swing_joint] + blend_between_frames(skeleton, frames, start, end, joint_list, window) + + +def interpolate_frames(skeleton, frames_a, frames_b, joint_list, start, end): + blended_frames = deepcopy(frames_a[:]) + window = end - start + for joint in joint_list: + idx = skeleton.animated_joints.index(joint) * 4 + 3 + j_indices = [idx, idx + 1, idx + 2, idx + 3] + for f in range(window): + t = (float(f) / window) + q_a = frames_a[start + f][j_indices] + q_b = frames_b[start + f][j_indices] + blended_frames[start + f][j_indices] = quaternion_slerp(q_a, q_b, t, spin=0, shortestpath=True) + return blended_frames + + +def blend_towards_next_step_linear_with_original(skeleton, frames, start, end, joint_list): + new_frames = generate_blended_frames(skeleton, frames, start, end, joint_list, end-start) + new_frames2 = interpolate_frames(skeleton, frames, new_frames, joint_list, start, end) + return new_frames2 + + +def generate_frame_using_iterative_slerp(skeleton, motions, frame_idx, weights): + """src: https://gamedev.stackexchange.com/questions/62354/method-for-interpolation-between-3-quaternions + """ + frame = None + w_sum = 0.0 + for n, w in weights.items(): + if frame is None: + frame = np.zeros(len(motions[n][0])) + frame[:] = motions[n][frame_idx][:] + w_sum += w + else: + new_w_sum = w_sum + w + if new_w_sum > 0: + w_a = w_sum / new_w_sum + w_b = w / new_w_sum + frame_b = motions[n][frame_idx] + frame[:3] = w_a * frame[:3] + w_b * frame_b[:3] + for idx, j in enumerate(skeleton.animated_joints): + q_start_idx = (idx * 4) + 3 + q_end_idx = q_start_idx + 4 + q_a = np.array(frame[q_start_idx:q_end_idx]) + q_b = frame_b[q_start_idx:q_end_idx] + new_q = quaternion_slerp(q_a, q_b, w_b) + new_q /= np.linalg.norm(new_q) + frame[q_start_idx:q_end_idx] = new_q + w_sum = new_w_sum + return frame + + +def generate_frame_using_iterative_slerp2(skeleton, frames, weights): + """src: https://gamedev.stackexchange.com/questions/62354/method-for-interpolation-between-3-quaternions + """ + frame = None + w_sum = 0.0 + for frame_idx, w in enumerate(weights): + if frame is None: + frame = frames[frame_idx][:] + w_sum += w + else: + new_w_sum = w_sum + w + if new_w_sum > 0: + w_a = w_sum / new_w_sum + w_b = w / new_w_sum + frame_b = frames[frame_idx][:] + frame[:3] = w_a * frame[:3] + w_b * frame_b[:3] + for idx, j in enumerate(skeleton.animated_joints): + q_start_idx = (idx * 4) + 3 + q_end_idx = q_start_idx + 4 + q_a = np.array(frame[q_start_idx:q_end_idx]) + q_b = frame_b[q_start_idx:q_end_idx] + new_q = quaternion_slerp(q_a, q_b, w_b) + new_q /= np.linalg.norm(new_q) + frame[q_start_idx:q_end_idx] = new_q + w_sum = new_w_sum + return frame + + + +def smooth_euler_frames(euler_frames, discontinuity, window=20): + """ Smooth a function around the given discontinuity frame + + Parameters + ---------- + motion : AnimationController + The motion to be smoothed + ATTENTION: This function changes the values of the motion + discontinuity : int + The frame where the discontinuity is. (e.g. the transitionframe) + window : (optional) int, default is 20 + The smoothing window + Returns + ------- + None. + """ + d = float(discontinuity) + s = float(window) + + smoothing_faktors = [] + for f in range(len(euler_frames)): + value = 0.0 + if d - s <= f < d: + tmp = ((f - d + s) / s) + value = 0.5 * tmp ** 2 + elif d <= f <= d + s: + tmp = ((f - d + s) / s) + value = -0.5 * tmp ** 2 + 2 * tmp - 2 + + smoothing_faktors.append(value) + + smoothing_faktors = np.array(smoothing_faktors) + new_euler_frames = [] + for i in range(len(euler_frames[0])): + current_value = euler_frames[:, i] + magnitude = current_value[int(d)] - current_value[int(d) - 1] + if magnitude > 180: + magnitude -= 360 + elif magnitude < -180: + magnitude += 360 + new_value = current_value + (magnitude * smoothing_faktors) + new_euler_frames.append(new_value) + new_euler_frames = np.array(new_euler_frames).T + return new_euler_frames diff --git a/build/lib/anim_utils/animation_data/motion_concatenation.py b/build/lib/anim_utils/animation_data/motion_concatenation.py new file mode 100644 index 0000000..13a3476 --- /dev/null +++ b/build/lib/anim_utils/animation_data/motion_concatenation.py @@ -0,0 +1,843 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from copy import deepcopy +from transformations import euler_from_matrix, quaternion_matrix, quaternion_about_axis, quaternion_multiply, quaternion_from_matrix, quaternion_from_euler, quaternion_slerp, euler_matrix, quaternion_inverse +from .utils import pose_orientation_general, transform_point, rotation_order_to_string, euler_substraction, point_to_euler_angle, euler_angles_to_rotation_matrix, get_rotation_angle, point_rotation_by_quaternion, LEN_QUAT, LEN_EULER, LEN_ROOT_POS, pose_orientation_euler, pose_up_vector_euler +from .constants import DEFAULT_SMOOTHING_WINDOW_SIZE, DEFAULT_ROTATION_ORDER +from .quaternion_frame import euler_to_quaternion, quaternion_to_euler +from .motion_blending import smooth_quaternion_frames_with_slerp, smooth_quaternion_frames, blend_quaternion_frames_linearly, smooth_quaternion_frames_with_slerp2, smooth_euler_frames +from ..motion_editing.motion_grounding import create_grounding_constraint_from_frame, generate_ankle_constraint_from_toe, interpolate_constraints +from ..motion_editing.analytical_inverse_kinematics import AnalyticalLimbIK +from ..motion_editing.utils import normalize, generate_root_constraint_for_two_feet, smooth_root_translation_at_start, smooth_root_translation_at_end, project_on_intersection_circle, global_position_to_root_translation +from .motion_blending import blend_between_frames, smooth_translation_in_quat_frames, generate_blended_frames, interpolate_frames, smooth_root_translation_around_transition, blend_quaternions_to_next_step, smooth_quaternion_frames_joint_filter, blend_towards_next_step_linear_with_original, smooth_root_positions + +ALIGNMENT_MODE_FAST = 0 +ALIGNMENT_MODE_PCL = 1 + +def concatenate_frames(prev_frames, new_frames): + frames = prev_frames.tolist() + for idx in range(1, len(new_frames)): # skip first frame + frames.append(new_frames[idx]) + frames = np.array(frames) + return frames + + +def convert_quat_frame_to_point_cloud(skeleton, frame, joints=None): + points = list() + if joints is None: + joints = [k for k, n in list(skeleton.nodes.items()) if len(n.children) > 0 and "Bip" not in n.node_name] + for j in joints: + p = skeleton.nodes[j].get_global_position(frame) + points.append(p) + return points + + +def _align_point_clouds_2D(a, b, weights): + ''' + Finds aligning 2d transformation of two equally sized point clouds. + from Motion Graphs paper by Kovar et al. + Parameters + --------- + *a: list + \t point cloud + *b: list + \t point cloud + *weights: list + \t weights of correspondences + Returns + ------- + *theta: float + \t angle around y axis in radians + *offset_x: float + \t + *offset_z: float + + ''' + if len(a) != len(b): + raise ValueError("two point cloud should have the same number points: "+str(len(a))+","+str(len(b))) + n_points = len(a) + numerator_left = 0 + denominator_left = 0 + weighted_sum_a_x = 0 + weighted_sum_b_x = 0 + weighted_sum_a_z = 0 + weighted_sum_b_z = 0 + sum_of_weights = 0.0 + # if not weights: + # weight = 1.0/n_points # todo set weight base on joint level + for index in range(n_points): + numerator_left += weights[index] * (a[index][0] * b[index][2] - + b[index][0] * a[index][2]) + denominator_left += weights[index] * (a[index][0] * b[index][0] + + a[index][2] * b[index][2]) + sum_of_weights += weights[index] + weighted_sum_a_x += weights[index] * a[index][0] + weighted_sum_b_x += weights[index] * b[index][0] + weighted_sum_a_z += weights[index] * a[index][2] + weighted_sum_b_z += weights[index] * b[index][2] + numerator_right = 1.0 / sum_of_weights * \ + (weighted_sum_a_x * weighted_sum_b_z - + weighted_sum_b_x * weighted_sum_a_z) + numerator = numerator_left - numerator_right + denominator_right = 1.0 / sum_of_weights * \ + (weighted_sum_a_x * weighted_sum_b_x + + weighted_sum_a_z * weighted_sum_b_z) + denominator = denominator_left - denominator_right + theta = np.arctan2(numerator, denominator) + offset_x = (weighted_sum_a_x - weighted_sum_b_x * + np.cos(theta) - weighted_sum_b_z * np.sin(theta)) / sum_of_weights + offset_z = (weighted_sum_a_z + weighted_sum_b_x * + np.sin(theta) - weighted_sum_b_z * np.cos(theta)) / sum_of_weights + + return theta, offset_x, offset_z + + +def align_euler_frames(euler_frames, + frame_idx, + ref_orientation_euler): + print("Deprecation Warning: Function marked as Deprecated!") + pass + new_euler_frames = deepcopy(euler_frames) + ref_quat = euler_to_quaternion(ref_orientation_euler) + root_rot_angles = euler_frames[frame_idx][3:6] + root_rot_quat = euler_to_quaternion(root_rot_angles) + quat_diff = quaternion_multiply(ref_quat, quaternion_inverse(root_rot_quat)) + for euler_frame in new_euler_frames: + root_trans = euler_frame[:3] + new_root_trans = point_rotation_by_quaternion(root_trans, quat_diff) + euler_frame[:3] = new_root_trans + root_rot_angles = euler_frame[3:6] + root_rot_quat = euler_to_quaternion(root_rot_angles) + + new_root_quat = quaternion_multiply(quat_diff, root_rot_quat) + new_root_euler = quaternion_to_euler(new_root_quat) + euler_frame[3:6] = new_root_euler + return new_euler_frames + + +def translate_euler_frames(euler_frames, frame_idx, ref_position, root_joint, skeleton): + """ + translate euler frames to ref_position, tralsation is 2D on the ground + :param euler_frames: + :param frame_idx: + :param ref_position: + :return: + """ + root_pos = skeleton.nodes[root_joint].get_global_position_from_euler(euler_frames[frame_idx]) + if len(ref_position) == 2: + offset = np.array([ref_position[0] - root_pos[0], 0, ref_position[1] - root_pos[2]]) + elif len(ref_position) == 3: + offset = ref_position - root_pos + offset[1] = 0 + else: + raise ValueError('the length of reference position is not correct! ') + rotation = [0, 0, 0] + return transform_euler_frames(euler_frames, rotation, offset) + + +def rotate_euler_frame(euler_frame, ref_orientation, body_plane_joints, skeleton): + forward = pose_orientation_general(euler_frame, body_plane_joints, skeleton) + rot_angle = get_rotation_angle(ref_orientation, forward) + translation = np.array([0, 0, 0]) + return transform_euler_frame(euler_frame, [0, rot_angle, 0], translation) + + +def rotate_euler_frames(euler_frames, frame_idx, ref_orientation, body_plane_joints, skeleton, rotation_order=None): + """ + Rotate a list of euler frames using the same rotation angle + :param euler_frames: a list of euler frames to be rotated + :param frame_idx: frame which uses to calculate rotation anlge + :param ref_orientation: reference orientation for alignment + :param rotation_order: define the rotation order for euler angles to calculate rotation matrix + :global_rotation: if true, the rotation is also applied to the global position. If false, the rotation will not be applied to global position + :return rotated_frames: a list of rotated euler frames + """ + forward = pose_orientation_general(euler_frames[frame_idx], + body_plane_joints, + skeleton) + rot_angle = get_rotation_angle(ref_orientation, forward) + translation = np.array([0, 0, 0]) + rotated_frames = transform_euler_frames(euler_frames, + [0, rot_angle, 0], + translation, + rotation_order) + return rotated_frames + + +def rotate_euler_frames_about_x_axis(euler_frames, frame_idx, ref_upvector): + sample_upvector = pose_up_vector_euler(euler_frames[frame_idx]) + rot_angle = get_rotation_angle(ref_upvector, sample_upvector) + translation = np.array([0, 0, 0]) + rotated_frames = transform_euler_frames(euler_frames, + [rot_angle, 0, 0], + translation) + return rotated_frames + + + +def smoothly_concatenate(euler_frames_a, euler_frames_b, window_size=20): + euler_frames = np.concatenate((euler_frames_a, euler_frames_b), axis=0) + euler_frames = smooth_euler_frames(euler_frames, len(euler_frames_a), window_size) + return euler_frames + +def fast_euler_frames_transformation(euler_frames_a, + euler_frames_b): + dir_vec_a = pose_orientation_euler(euler_frames_a[-1]) + dir_vec_b = pose_orientation_euler(euler_frames_b[0]) + angle = get_rotation_angle(dir_vec_a, dir_vec_b) + offset_x = euler_frames_a[-1][0] - euler_frames_b[0][0] + offset_z = euler_frames_a[-1][2] - euler_frames_b[0][2] + offset = [offset_x, 0.0, offset_z] + return angle, offset + + + +def fast_euler_frames_alignment(euler_frames_a, + euler_frames_b, + smooth=True, + smoothing_window=DEFAULT_SMOOTHING_WINDOW_SIZE): + angle, offset = fast_euler_frames_transformation(euler_frames_a, euler_frames_b) + transformed_frames = transform_euler_frames(euler_frames_b, [0, angle, 0],offset) + # concatenate the quaternion_frames_a and transformed_framess + if smooth: + euler_frames = smoothly_concatenate(euler_frames_a, transformed_frames, window_size=smoothing_window) + else: + euler_frames = np.concatenate((euler_frames_a, + transformed_frames)) + return euler_frames + +def transform_quaternion_frames_legacy(quat_frames, angles, offset, rotation_order=None): + """ Applies a transformation on the root joint of a list quaternion frames. + Parameters + ---------- + *quat_frames: np.ndarray + \tList of frames where the rotation is represented as euler angles in degrees. + *angles: list of floats + \tRotation angles in degrees + *offset: np.ndarray + \tTranslation + """ + if rotation_order is None: + rotation_order = DEFAULT_ROTATION_ORDER + offset = np.array(offset) + if round(angles[0], 3) == 0 and round(angles[2], 3) == 0: + rotation_q = quaternion_about_axis(np.deg2rad(angles[1]), [0, 1, 0]) + else: + rotation_q = euler_to_quaternion(angles, rotation_order) + rotation_matrix = euler_angles_to_rotation_matrix(angles, rotation_order)[:3, :3] + for frame in quat_frames: + ot = frame[:3] + oq = frame[3:7] + frame[:3] = np.dot(rotation_matrix, ot) + offset + frame[3:7] = quaternion_multiply(rotation_q, oq) + return quat_frames + +def pose_orientation_quat(quaternion_frame): + """Estimate pose orientation from root orientation + """ + ref_offset = np.array([0, 0, 1, 1]) + rotmat = quaternion_matrix(quaternion_frame[3:7]) + rotated_point = np.dot(rotmat, ref_offset) + dir_vec = np.array([rotated_point[0], rotated_point[2]]) + dir_vec /= np.linalg.norm(dir_vec) + return dir_vec + +def fast_quat_frames_transformation(quaternion_frames_a, + quaternion_frames_b): + dir_vec_a = pose_orientation_quat(quaternion_frames_a[-1]) + dir_vec_b = pose_orientation_quat(quaternion_frames_b[0]) + angle = get_rotation_angle(dir_vec_a, dir_vec_b) + offset_x = quaternion_frames_a[-1][0] - quaternion_frames_b[0][0] + offset_z = quaternion_frames_a[-1][2] - quaternion_frames_b[0][2] + offset = [offset_x, 0.0, offset_z] + return angle, offset + + + +def get_orientation_vector_from_matrix(m, v=[0, 0, 1]): + p = np.dot(m, v) + dir_vec = np.array([p[0], p[2]]) + dir_vec /= np.linalg.norm(dir_vec) + return dir_vec + + +def get_global_node_orientation_vector(skeleton, node_name, frame, v=[0, 0, 1]): + v = np.array(v) + m = skeleton.nodes[node_name].get_global_matrix(frame)[:3, :3] + p = np.dot(m, v) + dir_vec = np.array([p[0], p[2]]) + dir_vec /= np.linalg.norm(dir_vec) + return dir_vec + + +REF_VECTOR = np.array([0,0,1]) +def get_node_aligning_2d_transform(skeleton, node_name, prev_frames, new_frames, ref_vector=REF_VECTOR): + """from last of prev frames to first of new frames""" + + m_a = skeleton.nodes[node_name].get_global_matrix(prev_frames[-1]) + m_b = skeleton.nodes[node_name].get_global_matrix(new_frames[0]) + dir_vec_a = get_orientation_vector_from_matrix(m_a[:3, :3], ref_vector) + dir_vec_b = get_orientation_vector_from_matrix(m_b[:3, :3], ref_vector) + angle = get_rotation_angle(dir_vec_a, dir_vec_b) + q = quaternion_about_axis(np.deg2rad(angle), [0, 1, 0]) + m = quaternion_matrix(q) + + first_frame_pos = [new_frames[0][0], new_frames[0][1], new_frames[0][2],1.0] + rotated_first_frame_pos = np.dot(m, first_frame_pos)[:3] + delta = prev_frames[-1][:3] - rotated_first_frame_pos[:3] + m[0, 3] = delta[0] + #m[1, 3] = delta[1] + m[2, 3] = delta[2] + return m + +def get_transform_from_point_cloud_alignment(skeleton, prev_frames, new_frames): + weights = skeleton.get_joint_weights() + p_a = convert_quat_frame_to_point_cloud(skeleton, prev_frames[-1]) + p_b = convert_quat_frame_to_point_cloud(skeleton, new_frames[0]) + theta, offset_x, offset_z = _align_point_clouds_2D(p_a, p_b, weights) + euler = [0, np.radians(theta), 0] + m = np.eye(4) + m[:3,:3] = euler_matrix(*euler)[:3,:3] + m[0,3] = offset_x + m[2,3] = offset_z + print("run point cloud alignment", theta, offset_x, offset_z, m) + return m + +def transform_quaternion_frames(frames, m, + translation_param_range=(0, 3), + orientation_param_range=(3, 7)): + """ possibly broken because not 3,7 is the root orientation but 7,11 + """ + q = quaternion_from_matrix(m) + for frame in frames: + ot = frame[translation_param_range[0]:translation_param_range[1]].tolist() + [1] + oq = frame[orientation_param_range[0]:orientation_param_range[1]] + transformed_t = np.dot(m, ot)[:3] + frame[translation_param_range[0]:translation_param_range[1]] = transformed_t + frame[orientation_param_range[0]:orientation_param_range[1]] = quaternion_multiply(q, oq) + return frames + + +def concatenate_frames_smoothly(new_frames, prev_frames, smoothing_window=0): + d = len(prev_frames) + frames = concatenate_frames(prev_frames, new_frames) + if smoothing_window > 0: + frames = smooth_quaternion_frames(frames, d, smoothing_window) + return frames + + +def concatenate_frames_with_slerp(new_frames, prev_frames, smoothing_window=0): + ''' + + :param new_frames (numpy.array): n_frames * n_dims + :param prev_frames (numpy.array): n_frames * n_dims + :param smoothing_window: + :return: + ''' + d = len(prev_frames) + frames = concatenate_frames(prev_frames, new_frames) + if smoothing_window > 0: + frames = smooth_quaternion_frames_with_slerp(frames, d, smoothing_window) + return frames + + +def align_quaternion_frames_automatically(skeleton, node_name, new_frames, prev_frames, alignment_mode=ALIGNMENT_MODE_FAST): + if alignment_mode == ALIGNMENT_MODE_FAST: + m = get_node_aligning_2d_transform(skeleton, node_name, prev_frames, new_frames) + else: + m = get_transform_from_point_cloud_alignment(skeleton, prev_frames, new_frames) + new_frames = transform_quaternion_frames(new_frames, m) + return new_frames + + +def align_quaternion_frames_automatically2(skeleton, node_name, new_frames, prev_frames): + angle, offset = fast_quat_frames_transformation(prev_frames, new_frames) + new_frames = transform_quaternion_frames_legacy(new_frames, [0, angle, 0], offset) + return new_frames + + +def align_quaternion_frames(skeleton, node_name, new_frames, prev_frames=None, start_pose=None): + """align quaternion frames based on previous frames or + given transformation + + Parameters + ---------- + * new_frames: list + A list of quaternion frames + * prev_frames: list + A list of quaternion frames + * transformation: dict + Contains translation and orientation in Euler angles + Returns: + -------- + * transformed_frames: np.ndarray + Quaternion frames resulting from the back projection of s, + transformed to fit to prev_frames. + + """ + if prev_frames is not None: + return align_quaternion_frames_automatically(skeleton, node_name, new_frames, prev_frames) + elif start_pose is not None: + m = get_transform_from_start_pose(start_pose) + first_frame_pos = new_frames[0][:3].tolist() + [1] + t_pos = np.dot(m, first_frame_pos)[:3] + delta = start_pose["position"] + # FIXME this assumes the y translation is the up axis and can be ignored + delta[0] -= t_pos[0] + delta[2] -= t_pos[2] + m[:3, 3] = delta + transformed_frames = transform_quaternion_frames(new_frames, m) + return transformed_frames + else: + return new_frames + + +def get_transform_from_start_pose(start_pose): + e = np.deg2rad(start_pose["orientation"]) + p = start_pose["position"] + if None not in e: + q = quaternion_from_euler(*e) + m = quaternion_matrix(q) + else: + m = np.eye(4) + if None not in p: + m[:3,3] = p + return m + + +def align_and_concatenate_frames(skeleton, joint_name, new_frames, prev_frames=None, start_pose=None, smoothing_window=0, + blending_method='smoothing'): + new_frames = align_quaternion_frames(skeleton, joint_name, new_frames, prev_frames, start_pose) + + if prev_frames is not None: + d = len(prev_frames) + new_frames = concatenate_frames(prev_frames, new_frames) + if smoothing_window > 0: + if blending_method == 'smoothing': + new_frames = smooth_quaternion_frames(new_frames, d, smoothing_window) + elif blending_method == 'slerp': + new_frames = smooth_quaternion_frames_with_slerp(new_frames, d, smoothing_window) + elif blending_method == 'slerp2': + new_frames = smooth_quaternion_frames_with_slerp2(skeleton, new_frames, d, smoothing_window) + elif blending_method == 'linear': + new_frames = blend_quaternion_frames_linearly(new_frames, prev_frames, skeleton, smoothing_window) + else: + raise KeyError('Unknown method!') + return new_frames + + +blend = lambda x: 2 * x * x * x - 3 * x * x + 1 + + +def align_joint(skeleton, frames, frame_idx, foot_joint, ik_chain, ik_window=7): + transition_start = frame_idx + 1 + transition_end = frame_idx + ik_window + c = create_grounding_constraint_from_frame(skeleton, frames, frame_idx, foot_joint) + ik = AnalyticalLimbIK.init_from_dict(skeleton, c.joint_name, ik_chain) + frames[transition_start] = ik.apply(frames[transition_start], c.position, c.orientation) + + chain_joints = [ik_chain["root"], ik_chain["joint"], foot_joint] + for c_joint in chain_joints: + idx = skeleton.animated_joints.index(c_joint) * 4 + 3 + j_indices = [idx, idx + 1, idx + 2, idx + 3] + start_q = frames[transition_start][j_indices] + end_q = frames[transition_end][j_indices] + for i in range(ik_window): + t = float(i) / ik_window + frames[transition_start + 1 + i][j_indices] = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + return frames + + +def align_frames_and_fix_foot_to_prev(skeleton, aligning_joint, new_frames, prev_frames, start_pose, foot_joint, ik_chain, ik_window=7, smoothing_window=0): + new_frames = align_quaternion_frames(skeleton, aligning_joint, new_frames, prev_frames, start_pose) + if prev_frames is not None: + offset = prev_frames[-1][:3] - new_frames[0][:3] + + d = len(prev_frames) + frames = prev_frames.tolist() + for idx in range(1, len(new_frames)): # skip first frame + frames.append(new_frames[idx]) + frames = np.array(frames) + + transition_start = d + c = create_grounding_constraint_from_frame(skeleton, frames, d-1, foot_joint) + ik = AnalyticalLimbIK.init_from_dict(skeleton, c.joint_name, ik_chain) + before = skeleton.nodes[foot_joint].get_global_position(frames[transition_start]) + + frames[transition_start] = ik.apply(frames[transition_start], c.position, c.orientation) + + transition_end = d+ik_window + print("allign frames", c.position, foot_joint, d-1, transition_end, before, skeleton.nodes[foot_joint].get_global_position(frames[transition_start])) + print(skeleton.nodes[foot_joint].get_global_position(frames[d])) + + chain_joints = [ik_chain["root"], ik_chain["joint"], foot_joint] + for c_joint in chain_joints: + idx = skeleton.animated_joints.index(c_joint) * 4 + 3 + j_indices = [idx, idx + 1, idx + 2, idx + 3] + start_q = frames[transition_start][j_indices] + end_q = frames[transition_end][j_indices] + print(c_joint, start_q, end_q,j_indices) + for i in range(ik_window): + t = float(i) / ik_window + # nlerp_q = self.nlerp(start_q, end_q, t) + slerp_q = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + print(transition_start+i+1, frames[transition_start + 1 + i][j_indices], slerp_q) + frames[transition_start + 1 + i][j_indices] = slerp_q + if smoothing_window > 0 and False: + frames = smooth_quaternion_frames(frames, d, smoothing_window) + return frames + else: + return new_frames + +def get_limb_length(skeleton, joint_name): + limb_length = np.linalg.norm(skeleton.nodes[joint_name].offset) + limb_length += np.linalg.norm(skeleton.nodes[joint_name].parent.offset) + return limb_length + +def generate_root_constraint_for_one_foot(skeleton, frame, root, c): + root_pos = skeleton.nodes[root].get_global_position(frame) + target_length = np.linalg.norm(c.position - root_pos) + limb_length = get_limb_length(skeleton, c.joint_name) + if target_length >= limb_length: + new_root_pos = (c.position + normalize(root_pos - c.position) * limb_length) + print("one constraint on ", c.joint_name, "- before", root_pos, "after", new_root_pos) + return new_root_pos + #frame[:3] = new_root_pos + + else: + print("no change") + + +def translate_root(skeleton, frames, target_frame_idx, plant_heel, ground_height=0): + """ translate the next frames closer to the previous frames root translation""" + n_frames = len(frames) + foot_pos = skeleton.nodes[plant_heel].get_global_position(frames[target_frame_idx-1]) + print("foot pos before", foot_pos) + delta = ground_height - foot_pos[1] + n_frames = len(frames) + for f in range(target_frame_idx, n_frames): + frames[f][1] += delta + print("after", skeleton.nodes[plant_heel].get_global_position(frames[target_frame_idx])) + for f in range(target_frame_idx, n_frames): + frames[f,:3] += delta/2 + + +def apply_constraint(skeleton, frames, c, ik_chain, frame_idx, start, end, window): + print("apply swing foot constraint on frame", frame_idx, start, end) + ik = AnalyticalLimbIK.init_from_dict(skeleton, c.joint_name, ik_chain) + frames[frame_idx] = ik.apply(frames[frame_idx], c.position, c.orientation) + joint_list = [ik_chain["root"], ik_chain["joint"], c.joint_name] + blend_between_frames(skeleton, frames, start, end, joint_list, window) + + +def apply_constraint_on_window_prev(skeleton, frames, c, ik_chain, start, end, window): + ik = AnalyticalLimbIK.init_from_dict(skeleton, c.joint_name, ik_chain) + indices = list(range(start, end + 1)) + print("apply on frames", indices) + for f in indices: + frames[f] = ik.apply(frames[f], c.position, c.orientation) + joint_list = [ik_chain["root"], ik_chain["joint"], c.joint_name] + blend_between_frames(skeleton, frames, end, end+window, joint_list, window) + + +def apply_constraint_on_window_next(skeleton, frames, c, ik_chain, start, end, window): + ik = AnalyticalLimbIK.init_from_dict(skeleton, c.joint_name, ik_chain) + indices = list(range(start, end + 1)) + print("apply on frames", indices) + for f in indices: + frames[f] = ik.apply(frames[f], c.position, c.orientation) + joint_list = [ik_chain["root"], ik_chain["joint"], c.joint_name] + print("blend between frames",start-window, start) + blend_between_frames(skeleton, frames, start-window, start, joint_list, window) + + +def align_foot_to_next_step(skeleton, frames, foot_joint, ik_chain, target_frame_idx, window): + start = target_frame_idx - window # start of blending range + end = target_frame_idx - 1 # modified frame + + c = create_grounding_constraint_from_frame(skeleton, frames, target_frame_idx, foot_joint) + apply_constraint(skeleton, frames, c, ik_chain, target_frame_idx, start, end, window) + + +def align_foot_to_prev_step(skeleton, frames, foot_joint, ik_chain, target_frame_idx, window): + start = target_frame_idx # modified frame + end = target_frame_idx + window # end of blending range + c = create_grounding_constraint_from_frame(skeleton, frames, target_frame_idx-1, foot_joint) + apply_constraint(skeleton, frames, c, ik_chain, target_frame_idx, start, end, window) + + + + +def generate_feet_constraints(skeleton, frames, frame_idx, plant_side, swing_side, target_ground_height): + plant_foot_joint = skeleton.skeleton_model["joints"][plant_side + "_ankle"] + plant_toe_joint = skeleton.skeleton_model["joints"][plant_side + "_toe"] + plant_heel_joint = skeleton.skeleton_model["joints"][plant_side + "_heel"] + swing_foot_joint = skeleton.skeleton_model["joints"][swing_side + "_ankle"] + swing_toe_joint = skeleton.skeleton_model["joints"][swing_side + "_toe"] + swing_heel_joint = skeleton.skeleton_model["joints"][swing_side + "_heel"] + plant_constraint = generate_ankle_constraint_from_toe(skeleton, frames, frame_idx, plant_foot_joint, + plant_heel_joint, plant_toe_joint, target_ground_height) + swing_constraint = generate_ankle_constraint_from_toe(skeleton, frames, frame_idx, swing_foot_joint, + swing_heel_joint, swing_toe_joint, target_ground_height) + return plant_constraint, swing_constraint + + +def generate_feet_constraints2(skeleton, frames, frame_idx, plant_side, swing_side): + plant_foot_joint = skeleton.skeleton_model["joints"][plant_side + "_ankle"] + swing_foot_joint = skeleton.skeleton_model["joints"][swing_side + "_ankle"] + plant_constraint = create_grounding_constraint_from_frame(skeleton, frames, frame_idx - 1, plant_foot_joint) + swing_constraint = create_grounding_constraint_from_frame(skeleton, frames, frame_idx - 1, swing_foot_joint) + return plant_constraint, swing_constraint + + +def align_feet_to_prev_step(skeleton, frames, frame_idx, plant_constraint, swing_constraint, ik_chains, window): + start = frame_idx # modified frame + end = frame_idx + window # end of blending range + apply_constraint_on_window_prev(skeleton, frames, plant_constraint, ik_chains[plant_constraint.joint_name], start, end, window) + apply_constraint(skeleton, frames, swing_constraint, ik_chains[swing_constraint.joint_name], frame_idx, start, end, window) + + +def align_feet_to_next_step(skeleton, frames, frame_idx, plant_constraint, swing_constraint, ik_chains, plant_window, window): + start = frame_idx - window # end of blending range + end = frame_idx # modified frame + apply_constraint_on_window_next(skeleton, frames, plant_constraint, ik_chains[plant_constraint.joint_name], start, end, plant_window) + apply_constraint(skeleton, frames, swing_constraint, ik_chains[swing_constraint.joint_name], frame_idx, start, end, window) + + +def align_feet_to_next_step2(skeleton, frames, frame_idx, plant_constraint, swing_constraint, ik_chains, plant_window, window): + start = frame_idx - window # end of blending range + end = frame_idx # modified frame + ik_chain = ik_chains[plant_constraint.joint_name] + joint_list = [ik_chain["root"], ik_chain["joint"], plant_constraint.joint_name] + blend_between_frames(skeleton, frames, start, end, joint_list, window) + ik_chain = ik_chains[swing_constraint.joint_name] + joint_list = [ik_chain["root"], ik_chain["joint"], plant_constraint.joint_name] + blend_between_frames(skeleton, frames, start, end, joint_list, window) + + +def fix_feet_at_transition(skeleton, frames, d, plant_side, swing_side, ik_chains, ik_window=8, plant_window=20): + target_ground_height = 0 + smooth_root_translation_around_transition(frames, d, 2 * ik_window) + + plant_constraint, swing_constraint = generate_feet_constraints(skeleton, frames, d, plant_side, swing_side, target_ground_height) + root_pos = generate_root_constraint_for_two_feet(skeleton, frames[d-1], plant_constraint, swing_constraint) + if root_pos is not None: + frames[d - 1][:3] = root_pos + smooth_root_translation_at_end(frames, d - 1, ik_window) + smooth_root_translation_at_start(frames, d, ik_window) + + align_feet_to_next_step(skeleton, frames, d-1, plant_constraint, swing_constraint, ik_chains, plant_window, ik_window) + align_feet_to_prev_step(skeleton, frames, d, plant_constraint, swing_constraint, ik_chains, ik_window) + #swing_foot = skeleton.skeleton_model["joints"][swing_side + "_ankle"] + #align_foot_to_prev_step(skeleton, frames, swing_foot, ik_chains[swing_foot], d, ik_window) + + +def align_frames_using_forward_blending(skeleton, aligning_joint, new_frames, prev_frames, prev_start, start_pose, ik_chains, smoothing_window=0): + """ applies foot ik constraint to fit the prev motion primitive to the next motion primitive + """ + + new_frames = align_quaternion_frames(skeleton, aligning_joint, new_frames, prev_frames, start_pose) + if prev_frames is not None: + d = len(prev_frames) + frames = prev_frames.tolist() + for idx in range(1, len(new_frames)): # skip first frame + frames.append(new_frames[idx]) + frames = np.array(frames) + + blend_end = d + blend_start = int(prev_start + (blend_end-prev_start)/2) # start blending from the middle of the step + + left_joint = skeleton.skeleton_model["joints"]["left_ankle"] + right_joint = skeleton.skeleton_model["joints"]["right_ankle"] + pelvis = skeleton.skeleton_model["joints"]["pelvis"] + left_ik_chain = ik_chains[left_joint] + right_ik_chain = ik_chains[right_joint] + leg_joint_list = [skeleton.root, left_ik_chain["root"], left_ik_chain["joint"], left_joint, + right_ik_chain["root"], right_ik_chain["joint"], right_joint] + if pelvis != skeleton.root: + leg_joint_list.append(pelvis) + + frames = blend_towards_next_step_linear_with_original(skeleton, frames, blend_start, blend_end, leg_joint_list) + joint_list = [j for j in skeleton.animated_joints if j not in leg_joint_list] + + if smoothing_window > 0: + frames = smooth_quaternion_frames_joint_filter(skeleton, frames, d, joint_list, smoothing_window) + return frames + else: + return new_frames + +def blend_towards_next_step_linear(skeleton, frames, d, plant_side, swing_side, ik_chains, window=8): + target_ground_height = 0 + smooth_root_translation_around_transition(frames, d, 2 * window) + plant_constraint, swing_constraint = generate_feet_constraints(skeleton, frames, d, plant_side, swing_side, target_ground_height) + root_pos = generate_root_constraint_for_two_feet(skeleton, frames[d-1], plant_constraint, swing_constraint) + if root_pos is not None: + frames[d - 1][:3] = root_pos + smooth_root_translation_at_end(frames, d - 1, window) + smooth_root_translation_at_start(frames, d, window) + blend_quaternions_to_next_step(skeleton, frames, d, plant_constraint.joint_name, swing_constraint.joint_name, ik_chains, window) + + + +def blend_towards_next_step3(skeleton, frames, start, end, plant_side, swing_side, ik_chains, window=8): + plant_joint = skeleton.skeleton_model["joints"][plant_side + "_ankle"] + swing_joint = skeleton.skeleton_model["joints"][swing_side + "_ankle"] + plant_ik_chain = ik_chains[plant_joint] + swing_ik_chain = ik_chains[swing_joint] + joint_list = [skeleton.root, "pelvis", plant_ik_chain["root"], plant_ik_chain["joint"], plant_joint, + swing_ik_chain["root"], swing_ik_chain["joint"], swing_joint] + middle = int(start + (end-start)/2) + new_frames = generate_blended_frames(skeleton, frames, middle, end, joint_list, end-middle) + frames = interpolate_frames(skeleton, frames, new_frames, joint_list, middle, end) + return frames + + + +def transform_euler_frame(euler_frame, angles, offset, rotation_order=None, global_rotation=True): + """ + Calls transform_point for the root parameters and adds theta to the y rotation + channel of the frame. + + The offset of root is transformed by transform_point + The orientation of root is rotated by Rotation matrix + + Parameters + --------- + *euler_frame: np.ndarray + \t the parameters of a single frame + *angles: list of floats + \tRotation angles in degrees + *offset: np.ndarray + \tTranslation + """ + if rotation_order is None: + rotation_order = DEFAULT_ROTATION_ORDER + transformed_frame = deepcopy(euler_frame) + if global_rotation: + transformed_frame[:3] = transform_point(euler_frame[:3], angles, offset, rotation_order=rotation_order) + else: + transformed_frame[:3] = transform_point(euler_frame[:3], np.zeros(3), offset, rotation_order=rotation_order) + R = euler_matrix(np.deg2rad(angles[0]), np.deg2rad(angles[1]), np.deg2rad(angles[2]), axes='rxyz') + + src_e = np.deg2rad(euler_frame[3:6]) + rot_string = rotation_order_to_string(rotation_order) + OR = euler_matrix(*src_e, axes=rot_string) + rotmat = np.dot(R, OR) + eul_angles = np.rad2deg(euler_from_matrix(rotmat, rot_string)) + transformed_frame[3:6] = eul_angles + return transformed_frame + + +def transform_euler_frames(euler_frames, angles, offset, rotation_order=None): + """ Applies a transformation on the root joint of a list euler frames. + Parameters + ---------- + *euler_frames: np.ndarray + \tList of frames where the rotation is represented as euler angles in degrees. + *angles: list of floats + \tRotation angles in degrees + *offset: np.ndarray + \tTranslation + """ + transformed_euler_frames = [] + for frame in euler_frames: + transformed_euler_frames.append( + transform_euler_frame(frame, angles, offset, rotation_order)) + return np.array(transformed_euler_frames) + + +def shift_euler_frames_to_ground(euler_frames, ground_contact_joints, skeleton, align_index=0): + """ + shift all euler frames of motion to ground, which means the y-axis for + gound contact joint should be 0 + Step 1: apply forward kinematic to compute global position for ground + contact joint for each frame + Setp 2: find the offset from ground contact joint to ground, and shift + corresponding frame based on offset + """ + foot_contact_heights = [] + for joint in ground_contact_joints: + foot_contact_heights.append(skeleton.nodes[joint].get_global_position_from_euler(euler_frames[align_index])[1]) + avg_foot_height =np.array([0, -np.average(foot_contact_heights), 0]) + return transform_euler_frames(euler_frames, [0.0, 0.0, 0.0], avg_foot_height) + + +def shift_quat_frames_to_ground(quat_frames, ground_contact_joints, skeleton, align_index=0): + foot_contact_heights = [] + for joint in ground_contact_joints: + foot_contact_heights.append(skeleton.nodes[joint].get_global_position(quat_frames[align_index])[1]) + return transform_quaternion_frames(quat_frames, + [0.0, 0.0, 0.0], + np.array([0, -np.average(foot_contact_heights), 0])) + + + +def find_aligning_transformation(skeleton, euler_frames_a, euler_frames_b): + """ + performs alignment of the point clouds based on the poses at the end of + euler_frames_a and the start of euler_frames_b + Returns the rotation around y axis in radians, x offset and z offset + """ + point_cloud_a = convert_euler_frame_to_cartesian_frame(skeleton, euler_frames_a[-1]) + point_cloud_b = convert_euler_frame_to_cartesian_frame(skeleton, euler_frames_b[0]) + weights = skeleton.get_joint_weights() + theta, offset_x, offset_z = _align_point_clouds_2D(point_cloud_a, point_cloud_b, weights) + return theta, offset_x, offset_z + + +def align_frames(skeleton, euler_frames_a, euler_frames_b, smooth=True): + """ + calls find_aligning_transformation and concatenates the frames based on the + resulting transformation + Parameters + ---------- + *skeleton: Skeleton + \tUsed to extract hierarchy information. + *euler_frames_a: np.ndarray + \List of frames where the rotation is represented as euler angles in degrees. + *euler_frames_b: np.ndarray + \List of frames where the rotation is represented as euler angles in degrees. + *smooth: bool + \t Sets whether or not smoothing is supposed to be applied on the at the transition. + Returns + ------- + *aligned_frames : np.ndarray + \tAligned and optionally smoothed motion + """ + theta, offset_x, offset_z = find_aligning_transformation(skeleton, euler_frames_a, euler_frames_b) + + # apply 2d transformation + offset = np.array([offset_x, 0, offset_z]) + angles = [0, np.degrees(theta), 0] + euler_frames_b = transform_euler_frames(euler_frames_b, angles, + offset) + + # concatenate frames and optionally apply smoothing + if smooth: + euler_frames = smoothly_concatenate(euler_frames_a, euler_frames_b) + else: + euler_frames = np.concatenate((euler_frames_a, euler_frames_b), axis=0) + return euler_frames + + diff --git a/build/lib/anim_utils/animation_data/motion_distance.py b/build/lib/anim_utils/animation_data/motion_distance.py new file mode 100644 index 0000000..df4e13c --- /dev/null +++ b/build/lib/anim_utils/animation_data/motion_distance.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from math import sqrt +from transformations import euler_matrix +from .motion_concatenation import _align_point_clouds_2D + + +def _point_cloud_distance(a, b): + """ Calculates the distance between two point clouds with equal length and + corresponding indices + """ + assert len(a) == len(b) + distance = 0 + n_points = len(a) + for i in range(n_points): + d = [a[i][0] - b[i][0], a[i][1] - b[i][1], a[i][2] - b[i][2]] + distance += sqrt(d[0] ** 2 + d[1] ** 2 + d[2] ** 2) + return distance / n_points + + +def convert_quat_frame_to_point_cloud(skeleton, frame, joints=None): + points = [] + if joints is None: + joints = [k for k, n in list(skeleton.nodes.items()) if len(n.children) > 0] + for j in joints: + p = skeleton.nodes[j].get_global_position(frame) + points.append(p) + return points + + +def _transform_point_cloud(point_cloud, theta, offset_x, offset_z): + """ Transforms points in a point cloud by a rotation around the y axis and a translation + along x and z + """ + transformed_point_cloud = [] + for p in point_cloud: + if p is not None: + m = euler_matrix(0,np.radians(theta), 0) + m[0, 3] = offset_x + m[2, 3] = offset_z + p = p.tolist() + new_point = np.dot(m, p + [1])[:3] + transformed_point_cloud.append(new_point) + return transformed_point_cloud + + +def _transform_invariant_point_cloud_distance(pa,pb, weights=None): + if weights is None: + weights = [1.0] * len(pa) + theta, offset_x, offset_z = _align_point_clouds_2D(pa, pb, weights) + t_b = _transform_point_cloud(pb, theta, offset_x, offset_z) + distance = _point_cloud_distance(pa, t_b) + return distance + + +def frame_distance_measure(skeleton, a, b, weights=None): + """ Converts frames into point clouds and calculates + the distance between the aligned point clouds. + + Parameters + --------- + *skeleton: Skeleton + \t skeleton used for forward kinematics + *a: np.ndarray + \t the parameters of a single frame representing a skeleton pose. + *b: np.ndarray + \t the parameters of a single frame representing a skeleton pose. + *weights: list of floats + \t weights giving the importance of points during the alignment and distance + + Returns + ------- + Distance representing similarity of the poses. + """ + assert len(a) == len(b) + pa = convert_quat_frame_to_point_cloud(skeleton, a) + pb = convert_quat_frame_to_point_cloud(skeleton, b) + return _transform_invariant_point_cloud_distance(pa,pb, weights) diff --git a/build/lib/anim_utils/animation_data/motion_state.py b/build/lib/anim_utils/animation_data/motion_state.py new file mode 100644 index 0000000..b9d7eb5 --- /dev/null +++ b/build/lib/anim_utils/animation_data/motion_state.py @@ -0,0 +1,352 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import collections +import numpy as np +from transformations import quaternion_inverse, quaternion_conjugate, quaternion_multiply, quaternion_matrix + +def get_random_color(): + random_color = np.random.rand(3, ) + if np.sum(random_color) < 0.5: + random_color += np.array([0, 0, 1]) + return random_color.tolist() + + +class MotionStateInterface(object): + def update(self, dt): + pass + + def get_pose(self, frame_idx=None): + pass + + def reset(self): + pass + + def get_frames(self): + pass + + def get_n_frames(self): + pass + + def get_frame_time(self): + pass + + def set_frame_idx(self, frame_idx): + pass + + def set_color_annotation_from_labels(self, labels, colors): + pass + + def replace_current_frame(self, frame): + pass + + def replace_frames(self, new_frames): + pass + + def set_color_annotation(self, semantic_annotation, color_map): + pass + + def set_time_function(self, time_function): + pass + + def set_color_annotation_legacy(self, annotation, color_map): + pass + + def set_random_color_annotation(self): + pass + + def get_current_frame_idx(self): + pass + + def get_current_annotation(self): + return None + + def get_n_annotations(self): + return 0 + + def get_semantic_annotation(self): + return list() + + def set_time(self, t): + return + + +class MotionState(MotionStateInterface): + """ Wrapper of a MotionVector to add functions for keeping track of the current time and frame index as well as semantic information. + """ + def __init__(self, mv): + self.mv = mv + self.time = 0 + self.frame_idx = 0 + self.play = False + self._frame_annotation = None + self.label_color_map = dict() + self._semantic_annotation = collections.OrderedDict() + self.events = dict() + self.n_annotations = 0 + self.hold_frames = list() # sorted list + self.paused = False + self.interpolate = False + self.tick = None + self._time_function = None + + def update(self, dt): + if self.play and not self.paused: + self.time += dt + self.frame_idx = int(self.time / self.mv.frame_time) + if len(self.hold_frames) > 0: + #print("hold frame", self.frame_idx,self.hold_frame, self.mv.n_frames) + if self.frame_idx >= self.hold_frames[0]: + self.paused = True + self.frame_idx = self.hold_frames[0] + self.hold_frames = self.hold_frames[1:] # remove entry + if self.tick is not None: + self.tick.update(dt) + if self.frame_idx >= self.mv.n_frames: + self.reset() + return True + return False + + + def get_pose(self, frame_idx=None): + if self.interpolate: + return self.get_pose_interpolate(frame_idx) + else: + if frame_idx is None: + return self.mv.frames[self.frame_idx] + else: + return self.mv.frames[frame_idx] + + def get_pose_interpolate(self, frame_idx=None): + #FIXME interpolation causes jump in translation at start and end + if frame_idx is None: + start_frame_idx = int(self.time / self.mv.frame_time) + end_frame_idx = start_frame_idx+1 + t = (self.time % self.mv.frame_time)/ self.mv.frame_time + if end_frame_idx >= self.mv.n_frames: + end_frame_idx = start_frame_idx + t = 0 + #print("get pose", self.time, start_frame_idx, t, end_frame_idx) + return self.mv.interpolate(start_frame_idx, end_frame_idx, t) + #return self.mv.frames[self.frame_idx] + else: + return self.mv.frames[frame_idx] + + def reset(self): + self.time = 0 + self.frame_idx = 0 + self.paused = False + if self.tick is not None: + self.tick.reset() + + def get_frames(self): + return self.mv.frames + + def get_n_frames(self): + return self.mv.n_frames + + def get_frame_time(self): + return self.mv.frame_time + + def get_current_annotation(self): + return self._frame_annotation[self.frame_idx] + + + def get_current_annotation_label(self): + if self._frame_annotation is not None and 0 <= self.frame_idx < len(self._frame_annotation): + return self._frame_annotation[self.frame_idx]["label"] + else: + return "" + + def set_frame_idx(self, frame_idx): + self.frame_idx = frame_idx + self.time = self.get_frame_time() * self.frame_idx + + def set_color_annotation_from_labels(self, labels, colors=None): + + if colors is None: + def getRGBfromI(RGBint): + """ https://stackoverflow.com/questions/33124347/convert-integers-to-rgb-values-and-back-with-python + """ + blue = RGBint & 255 + green = (RGBint >> 8) & 255 + red = (RGBint >> 16) & 255 + return red, green, blue + n_classes = int(max(labels)+1) + step_size = 255255255 / n_classes + self.label_color_map = dict() + for n in range(n_classes): + l = int(n*step_size) + self.label_color_map[str(n)] = np.array(getRGBfromI(l), dtype=float)/255 + else: + n_classes = len(colors) + for n in range(n_classes): + self.label_color_map[str(n)] = colors[n] + + self._semantic_annotation = collections.OrderedDict() + for n in range(n_classes): + self._semantic_annotation[str(n)] = [] + self.n_annotations = len(labels) + self._frame_annotation = [] + for idx, label in enumerate(labels): + label = str(int(label)) + self._frame_annotation.append({"label": label, "color": self.label_color_map[label]}) + self._semantic_annotation[label].append(idx) + + def replace_current_frame(self, frame): + if 0 <= self.frame_idx < self.mv.n_frames: + self.mv.frames[self.frame_idx] = frame + + def replace_frames(self, new_frames): + self.time = 0 + self.frame_idx = 0 + self.mv.frames = new_frames + self.mv.n_frames = len(new_frames) + + def set_color_annotation(self, semantic_annotation, color_map): + self._semantic_annotation = semantic_annotation + self.label_color_map = color_map + self.n_annotations = self.mv.n_frames + self._frame_annotation = [] + for idx in range(self.n_annotations): + self._frame_annotation.append({"label": "none", "color": [1,1,1]}) + for label in semantic_annotation.keys(): + if label in color_map.keys(): + entry = {"label": label, "color": color_map[label]} + if len(semantic_annotation[label]) > 0 and type(semantic_annotation[label][0]) == list: + for indices in semantic_annotation[label]: + for i in indices: + if i < self.n_annotations: + self._frame_annotation[i] = entry + else: + for idx in semantic_annotation[label]: + if idx < self.n_annotations: + self._frame_annotation[idx] = entry + else: + print("warning", idx, self.n_annotations) + + + def set_time_function(self, time_function): + self._time_function = time_function + + def set_color_annotation_legacy(self, annotation, color_map): + self._semantic_annotation = collections.OrderedDict() + self.label_color_map = color_map + self.n_annotations = len(annotation) + self._frame_annotation = [] + for idx, label in enumerate(annotation): + + self._frame_annotation.append({"label": label, + "color": color_map[label]}) + + if label not in list(self._semantic_annotation.keys()): + self._semantic_annotation[label] = [] + self._semantic_annotation[label].append(idx) + + def set_random_color_annotation(self): + self._frame_annotation = [] + for idx in range(self.mv.n_frames): + self._frame_annotation.append({"color": get_random_color()}) + self.n_annotations = self.mv.n_frames + + def get_current_frame_idx(self): + return self.frame_idx + + def get_n_annotations(self): + return self.n_annotations + + def get_semantic_annotation(self): + return list(self._semantic_annotation.items()) + + def get_label_color_map(self): + return self.label_color_map + + def apply_delta_frame(self, skeleton, delta_frame): + self.mv.apply_delta_frame(skeleton, delta_frame) + + def set_time(self, t): + self.frame_idx = int(t / self.get_frame_time()) + self.frame_idx = min(self.frame_idx, self.mv.n_frames-1) + #self.time = self.frame_idx*self.get_frame_time() + self.time = t + + def set_position(self, position): + delta = position - self.mv.frames[self.frame_idx, :3] + self.mv.frames[:, :3] += delta + + def set_orientation(self, orientation): + print("rotate frames") + current_q = self.mv.frames[self.frame_idx, 3:7] + delta_q = quaternion_multiply(orientation, quaternion_inverse(current_q)) + delta_q /= np.linalg.norm(delta_q) + delta_m = quaternion_matrix(delta_q)[:3, :3] + for idx in range(self.mv.n_frames): + old_t = self.mv.frames[idx, :3] + old_q = self.mv.frames[idx, 3:7] + self.mv.frames[idx, :3] = np.dot(delta_m, old_t)[:3] + self.mv.frames[idx, 3:7] = quaternion_multiply(delta_q, old_q) + #self.mv.frames[i, 3:7] = quaternion_multiply(delta_q, old_q) + + def set_ticker(self, tick): + self.tick = tick + +class MotionSplineState(MotionStateInterface): + def __init__(self, spline, frame_time): + self.spline = spline + self.time = 0.0 + self.frame_idx = 0.0 + self.frame_time = frame_time + self.events = dict() + + def update(self, dt): + self.time += dt + self.frame_idx = self.time / self.frame_time + if self.frame_idx >= self.spline.get_domain()[-1]: + self.reset() + return True + else: + return False + + def get_pose(self, frame_idx=None): + if frame_idx is None: + return self.spline.evaluate(self.frame_idx) + else: + return self.spline.evaluate(frame_idx) + + def reset(self): + self.time = 0 + + def get_frames(self): + return self.spline.get_motion_vector() + + def get_last_frame(self): + return self.spline.evaluate(self.spline.get_domain()[-1]) + + def get_n_frames(self): + return self.spline.get_domain()[-1] + + def get_frame_time(self): + return self.frame_time + + def set_time(self, t): + self.time = t + diff --git a/build/lib/anim_utils/animation_data/motion_vector.py b/build/lib/anim_utils/animation_data/motion_vector.py new file mode 100644 index 0000000..94ae946 --- /dev/null +++ b/build/lib/anim_utils/animation_data/motion_vector.py @@ -0,0 +1,383 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +from datetime import datetime +import os +import numpy as np +import imp +from transformations import quaternion_inverse, quaternion_multiply, quaternion_slerp, quaternion_from_euler, euler_matrix, quaternion_from_matrix +from .quaternion_frame import convert_euler_frames_to_quaternion_frames +from .motion_concatenation import align_and_concatenate_frames, smooth_root_positions +from .constants import ROTATION_TYPE_QUATERNION, ROTATION_TYPE_EULER +from .bvh import BVHWriter +from .acclaim import create_euler_matrix, DEFAULT_FRAME_TIME + + +def get_quaternion_delta(a, b): + return quaternion_multiply(quaternion_inverse(b), a) + + +def add_frames(skeleton, a, b): + """ returns c = a + b""" + c = np.zeros(len(a)) + c[:3] = a[:3] + b[:3] + for idx, j in enumerate(skeleton.animated_joints): + o = idx * 4 + 3 + q_a = a[o:o + 4] + q_b = b[o:o + 4] + q_prod = quaternion_multiply(q_a, q_b) + c[o:o + 4] = q_prod / np.linalg.norm(q_prod) + return c + + +def substract_frames(skeleton, a, b): + """ returns c = a - b""" + c = np.zeros(len(a)) + c[:3] = a[:3] - b[:3] + for idx, j in enumerate(skeleton.animated_joints): + o = idx*4 + 3 + q_a = a[o:o+4] + q_b = b[o:o+4] + q_delta = get_quaternion_delta(q_a, q_b) + c[o:o+4] = q_delta / np.linalg.norm(q_delta) + return c + +def amc_euler_to_quaternion(angles, dofs, c, c_inv, align_quat=True): + """ https://research.cs.wisc.edu/graphics/Courses/cs-838-1999/Jeff/ASF-AMC.html + """ + m = create_euler_matrix(angles, dofs) + #apply joint coordinate system defined by asf skeleton: C_inv x M x C + m = np.dot(c, np.dot(m, c_inv)) + q = quaternion_from_matrix(m) + if align_quat: + dot = np.sum(q) + if dot < 0: + q = -q + return q.tolist() + +class MotionVector(object): + """ Contains a list of skeleton animation frames. Each frame represents a list of parameters for the degrees of freedom of a skeleton. + """ + def __init__(self, skeleton=None, **kwargs): + self.n_frames = 0 + self._prev_n_frames = 0 + self.frames = None + self.start_pose = None + self.rotation_type = kwargs.get("rotation_type",ROTATION_TYPE_QUATERNION) + self.apply_spatial_smoothing = kwargs.get("apply_spatial_smoothing",False) + self.apply_foot_alignment = kwargs.get("apply_foot_alignment",False) + self.smoothing_window = kwargs.get("smoothing_window",0) + self.spatial_smoothing_method = kwargs.get("spatial_smoothing_method","smoothing") + self.frame_time = 1.0/30.0 + self.skeleton = skeleton + + + def from_bvh_reader(self, bvh_reader, filter_joints=True, animated_joints=None): + if self.rotation_type == ROTATION_TYPE_QUATERNION: + quat_frame = convert_euler_frames_to_quaternion_frames(bvh_reader, bvh_reader.frames, filter_joints, animated_joints) + self.frames = np.array(quat_frame) + elif self.rotation_type == ROTATION_TYPE_EULER: + self.frames = bvh_reader.frames + self.n_frames = len(self.frames) + self._prev_n_frames = 0 + self.frame_time = bvh_reader.frame_time + + def get_relative_frames(self): + relative_frames = [] + for idx in range(1, len(self.frames)): + delta_frame = np.zeros(len(self.frames[0])) + delta_frame[7:] = self.frames[idx][7:] + delta_frame[:3] = self.frames[idx][:3] - self.frames[idx-1][:3] + currentq = self.frames[idx][3:7] / np.linalg.norm(self.frames[idx][3:7]) + prevq = self.frames[idx-1][3:7] / np.linalg.norm(self.frames[idx-1][3:7]) + delta_q = quaternion_multiply(quaternion_inverse(prevq), currentq) + delta_frame[3:7] = delta_q + #print(idx, self.frames[idx][:3], self.frames[idx - 1][:3], delta_frame[:3], delta_frame[3:7]) + + relative_frames.append(delta_frame) + return relative_frames + + def append_frames_generic(self, new_frames): + """Align quaternion frames to previous frames + + Parameters + ---------- + * new_frames: list + A list of frames with the same rotation format type as the motion vector + """ + if self.apply_spatial_smoothing: + smoothing_window = self.smoothing_window + else: + smoothing_window = 0 + self.frames = align_and_concatenate_frames(self.skeleton, self.skeleton.aligning_root_node, new_frames, self.frames, self.start_pose, + smoothing_window=smoothing_window, blending_method=self.spatial_smoothing_method) + + self.n_frames = len(self.frames) + self._prev_n_frames = self.n_frames + + + def append_frames_using_forward_blending(self, new_frames): + if self.apply_spatial_smoothing: + smoothing_window = self.smoothing_window + else: + smoothing_window = 0 + from . import motion_concatenation + imp.reload(motion_concatenation) + ik_chains = self.skeleton.skeleton_model["ik_chains"] + self.frames = motion_concatenation.align_frames_using_forward_blending(self.skeleton, self.skeleton.aligning_root_node, new_frames, + self.frames, self._prev_n_frames, self.start_pose, + ik_chains, smoothing_window) + self._prev_n_frames = self.n_frames + self.n_frames = len(self.frames) + + def append_frames(self, new_frames, plant_foot=None): + if self.apply_foot_alignment and self.skeleton.skeleton_model is not None: + self.append_frames_using_forward_blending(new_frames) + else: + self.append_frames_generic(new_frames) + + def export(self, skeleton, output_filename, add_time_stamp=False): + bvh_writer = BVHWriter(None, skeleton, self.frames, self.frame_time, True) + if add_time_stamp: + output_filename = output_filename + "_" + \ + str(datetime.now().strftime("%d%m%y_%H%M%S")) + ".bvh" + elif output_filename != "": + if not output_filename.endswith("bvh"): + output_filename = output_filename + ".bvh" + else: + output_filename = "output.bvh" + bvh_writer.write(output_filename) + + def reduce_frames(self, n_frames): + if n_frames == 0: + self.frames = None + self.n_frames = 0 + self._prev_n_frames = self.n_frames + else: + self.frames = self.frames[:n_frames] + self.n_frames = len(self.frames) + self._prev_n_frames = 0 + + def has_frames(self): + return self.frames is not None + + def clear(self, end_frame=0): + if end_frame == 0: + self.frames = None + self.n_frames = 0 + self._prev_n_frames = 0 + else: + self.frames = self.frames[:end_frame] + self.n_frames = len(self.frames) + self._prev_n_frames = 0 + + def translate_root(self, offset): + for idx in range(self.n_frames): + self.frames[idx][:3] += offset + + def scale_root(self, scale_factor): + for idx in range(self.n_frames): + self.frames[idx][:3] *= scale_factor + + def from_fbx(self, animation, animated_joints): + self.frame_time = animation["frame_time"] + root_joint = animated_joints[0] + self.n_frames = len(animation["curves"][root_joint]) + self.frames = [] + for idx in range(self.n_frames): + frame = self._create_frame_from_fbx(animation, animated_joints, idx) + self.frames.append(frame) + + def _create_frame_from_fbx(self, animation, animated_joints, idx): + n_dims = len(animated_joints) * 4 + 3 + frame = np.zeros(n_dims) + offset = 3 + root_name = animated_joints[0] + frame[:3] = animation["curves"][root_name][idx]["translation"] + for node_name in animated_joints: + if node_name in animation["curves"].keys(): + rotation = animation["curves"][node_name][idx]["rotation"] + frame[offset:offset+4] = rotation + offset += 4 + + return frame + + def to_unity_format(self, scale=1.0): + """ Converts the frames into a custom json format for use in a Unity client""" + animated_joints = [j for j, n in list(self.skeleton.nodes.items()) if + "EndSite" not in j and len(n.children) > 0] # self.animated_joints + unity_frames = [] + + for node in list(self.skeleton.nodes.values()): + node.quaternion_index = node.index + + for frame in self.frames: + unity_frame = self._convert_frame_to_unity_format(frame, animated_joints, scale) + unity_frames.append(unity_frame) + + result_object = dict() + result_object["frames"] = unity_frames + result_object["frameTime"] = self.frame_time + result_object["jointSequence"] = animated_joints + return result_object + + def to_db_format(self, scale=1.0, animated_joints=None): + """ Converts the frames into a custom json format for use in a Unity client""" + if animated_joints is None: + animated_joints = [j for j, n in self.skeleton.nodes.items() if + n.children is not None and len(n.children) > 0] # self.animated_joints + poses = [] + for frame in self.frames: + pose = self._convert_frame_to_db_format(frame, animated_joints, scale) + poses.append(pose) + + result_object = dict() + result_object["poses"] = poses + result_object["frame_time"] = self.frame_time + result_object["joint_sequence"] = animated_joints + return result_object + + def _convert_frame_to_db_format(self, frame, animated_joints, scale=1.0): + """ Converts the frame into a custom json format and converts the transformations + to the left-handed coordinate system of Unity. + src: http://answers.unity3d.com/questions/503407/need-to-convert-to-right-handed-coordinates.html + """ + + t = frame[:3] * scale + pose = [-t[0], t[1], t[2]] + for node_name in self.skeleton.nodes.keys(): + if node_name in animated_joints: + node = self.skeleton.nodes[node_name] + if node_name in self.skeleton.animated_joints: # use rotation from frame + # TODO fix: the animated_joints is ordered differently than the nodes list for the latest model + index = self.skeleton.animated_joints.index(node_name) + offset = index * 4 + 3 + r = frame[offset:offset + 4] + pose += [-r[0], -r[1], r[2], r[3]] + else: # use fixed joint rotation + r = node.rotation + pose += [-float(r[0]), -float(r[1]), float(r[2]), float(r[3])] + return pose + + + def _convert_frame_to_unity_format(self, frame, animated_joints, scale=1.0): + """ Converts the frame into a custom json format and converts the transformations + to the left-handed coordinate system of Unity. + src: http://answers.unity3d.com/questions/503407/need-to-convert-to-right-handed-coordinates.html + """ + unity_frame = {"rotations": [], "rootTranslation": None} + for node_name in self.skeleton.nodes.keys(): + if node_name in animated_joints: + node = self.skeleton.nodes[node_name] + if node_name == self.skeleton.root: + t = frame[:3] * scale + unity_frame["rootTranslation"] = {"x": -t[0], "y": t[1], "z": t[2]} + + if node_name in self.skeleton.animated_joints: # use rotation from frame + # TODO fix: the animated_joints is ordered differently than the nodes list for the latest model + index = self.skeleton.animated_joints.index(node_name) + offset = index * 4 + 3 + r = frame[offset:offset + 4] + unity_frame["rotations"].append({"x": -r[1], "y": r[2], "z": r[3], "w": -r[0]}) + else: # use fixed joint rotation + r = node.rotation + unity_frame["rotations"].append( + {"x": -float(r[1]), "y": float(r[2]), "z": float(r[3]), "w": -float(r[0])}) + return unity_frame + + def from_custom_db_format(self, data): + self.frames = [] + for f in data["poses"]: + t = f[:3] + o = 3 + new_f = [-t[0], t[1], t[2]] + for i in data["joint_sequence"]: + new_f.append(-f[o]) + new_f.append(-f[o+1]) + new_f.append(f[o+2]) + new_f.append(f[o+3]) + o+=4 + + self.frames.append(new_f) + self.frames = np.array(self.frames) + self.n_frames = len(self.frames) + self.frame_time = data["frame_time"] + + def from_custom_unity_format(self, data): + self.frames = [] + for f in data["frames"]: + t = f["rootTranslation"] + new_f = [-t["x"], t["y"], t["z"]] + for q in f["rotations"]: + new_f.append(-q["w"]) + new_f.append(-q["x"]) + new_f.append(q["y"]) + new_f.append(q["z"]) + + self.frames.append(new_f) + self.frames = np.array(self.frames) + self.n_frames = len(self.frames) + self.frame_time = data["frameTime"] + + def apply_low_pass_filter_on_root(self, window): + self.frames[:, :3] = smooth_root_positions(self.frames[:, :3], window) + + def apply_delta_frame(self, skeleton, delta_frame): + for f in range(self.n_frames): + self.frames[f] = add_frames(skeleton, self.frames[f], delta_frame) + + def interpolate(self, start_idx, end_idx, t): + new_frame = np.zeros(self.frames[0].shape) + new_frame[:3] = (1-t) * self.frames[start_idx][:3] + t * self.frames[end_idx][:3] + for i in range(3, new_frame.shape[0], 4): + start_q = self.frames[start_idx][i:i+4] + end_q = self.frames[end_idx][i:i+4] + new_frame[i:i+4] = quaternion_slerp(start_q, end_q, t, spin=0, shortestpath=True) + return new_frame + + def from_amc_data(self, skeleton, amc_frames, frame_time=DEFAULT_FRAME_TIME): + identity = np.eye(4, dtype=np.float32) + self.frames = [] + for f in amc_frames: + new_f = [] + for key in skeleton.animated_joints: + b_data = skeleton.nodes[key].format_meta_data + if "coordinate_transform" in b_data and "inv_coordinate_transform" in b_data: + c = b_data["coordinate_transform"] + c_inv = b_data["inv_coordinate_transform"] + else: + c = identity + c_inv = identity + if key == "root": + new_f += f["root"][:3] + dof = b_data["asf_channels"] + values = amc_euler_to_quaternion(f[key][3:], dof, c, c_inv) + new_f += values + elif key in f: + dof = b_data["asf_channels"] + new_f += amc_euler_to_quaternion(f[key], dof, c, c_inv) + else: + new_f += [1,0,0,0] + self.frames.append(new_f) + self.frames = np.array(self.frames) + self.n_frames = len(self.frames) + self.frame_time = frame_time diff --git a/build/lib/anim_utils/animation_data/quaternion_frame.py b/build/lib/anim_utils/animation_data/quaternion_frame.py new file mode 100644 index 0000000..0f13f0e --- /dev/null +++ b/build/lib/anim_utils/animation_data/quaternion_frame.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" +Created on Fri Nov 24 14:10:21 2014 + +@author: Erik Herrmann, Han Du + +""" +import collections +import numpy as np +from transformations import euler_matrix, quaternion_from_matrix, quaternion_matrix, euler_from_matrix +from .utils import rotation_order_to_string +from .constants import DEFAULT_ROTATION_ORDER, LEN_EULER, LEN_QUAT, LEN_ROOT_POS + + + + +def check_quat(test_quat, ref_quat): + """check locomotion_synthesis_test quat needs to be filpped or not + """ + test_quat = np.asarray(test_quat) + ref_quat = np.asarray(ref_quat) + dot = np.dot(test_quat, ref_quat) + if dot < 0: + test_quat = - test_quat + return test_quat.tolist() + +def euler_to_quaternion(euler_angles, rotation_order=DEFAULT_ROTATION_ORDER,filter_values=True): + """Convert euler angles to quaternion vector [qw, qx, qy, qz] + Parameters + ---------- + * euler_angles: list of floats + \tA list of ordered euler angles in degress + * rotation_order: Iteratable + \t a list that specifies the rotation axis corresponding to the values in euler_angles + * filter_values: Bool + \t enforce a unique rotation representation + + """ + assert len(euler_angles) == 3, ('The length of euler angles should be 3!') + euler_angles = np.deg2rad(euler_angles) + rotmat = euler_matrix(*euler_angles, rotation_order_to_string(rotation_order)) + # convert rotation matrix R into quaternion vector (qw, qx, qy, qz) + quat = quaternion_from_matrix(rotmat) + # filter the quaternion see + # http://physicsforgames.blogspot.de/2010/02/quaternions.html + if filter_values: + dot = np.sum(quat) + if dot < 0: + quat = -quat + return [quat[0], quat[1], quat[2], quat[3]] + + +def quaternion_to_euler(quat, rotation_order=DEFAULT_ROTATION_ORDER): + """ + Parameters + ---------- + * q: list of floats + \tQuaternion vector with form: [qw, qx, qy, qz] + + Return + ------ + * euler_angles: list + \tEuler angles in degree with specified order + """ + quat = np.asarray(quat) + quat = quat / np.linalg.norm(quat) + rotmat_quat = quaternion_matrix(quat) + rotation_order_str = rotation_order_to_string(rotation_order) + euler_angles = np.rad2deg(euler_from_matrix(rotmat_quat, rotation_order_str)) + return euler_angles + + +def convert_euler_to_quaternion_frame(bvh_data, e_frame, filter_values=True, animated_joints=None): + """Convert a BVH frame into an ordered dict of quaternions for each skeleton node + Parameters + ---------- + * bvh_data: BVHData + \t Contains skeleton information + * frame_vector: np.ndarray + \t animation keyframe frame represented by Euler angles + * filter_values: Bool + \t enforce a unique rotation representation + + Returns: + ---------- + * quat_frame: OrderedDict that contains a quaternion for each joint + """ + if animated_joints is None: + animated_joints = list(bvh_data.node_names.keys()) + n_dims = len(animated_joints)*4 + quat_frame = np.zeros((n_dims)) + offset = 0 + for node_name in animated_joints: + if bvh_data.get_node_channels(node_name) is not None: + angles, order = bvh_data.get_node_angles(node_name, e_frame) + q = euler_to_quaternion(angles, order, filter_values) + quat_frame[offset:offset+4] = q + offset +=4 + return quat_frame + + +def convert_euler_frames_to_quaternion_frames(bvh_data, euler_frames, filter_values=True, animated_joints=None): + """ + Parameters + ---------- + * bvh_data: BVHData + \t Contains skeleton information + * euler_frames: np.ndarray + \t list of euler frames + Returns: + ---------- + * quater_frames: a list of quaternion frames + """ + if animated_joints is None: + animated_joints = list(bvh_data.node_names.keys()) + quat_frames = [] + prev_frame = None + for e_frame in euler_frames: + q_frame = convert_euler_to_quaternion_frame(bvh_data, e_frame, filter_values, animated_joints=animated_joints) + o = 0 + if prev_frame is not None and filter_values: + for joint_name in animated_joints: + q = check_quat(q_frame[o:o+4], prev_frame[o:o+4]) + q_frame[o:o+4] = q + o+=4 + prev_frame = q_frame + q_frame = np.concatenate([e_frame[:3],q_frame]) + quat_frames.append(q_frame) + return quat_frames + + + +def convert_quaternion_frames_to_euler_frames(quat_frames): + """Returns an nparray of Euler frames + + Parameters + ---------- + * quat_frames: List of quaternion frames + \tQuaternion frames that shall be converted to Euler frames + + Returns + ------- + * euler_frames: numpy array + \tEuler frames + """ + n_frames = len(quat_frames) + assert n_frames > 0 + n_joints = int((len(quat_frames[0])-LEN_ROOT_POS) /LEN_QUAT) + n_params = n_joints*LEN_EULER + LEN_ROOT_POS + euler_frames = np.zeros((n_frames, n_params)) + for frame_idx, quat_frame in enumerate(quat_frames): + euler_frames[frame_idx,:LEN_ROOT_POS] = quat_frame[:LEN_ROOT_POS] + src = LEN_ROOT_POS + dst = LEN_ROOT_POS + for i in range(n_joints): + q = quat_frame[src:src+LEN_QUAT] + euler_frames[frame_idx, dst:dst+LEN_EULER] = quaternion_to_euler(q) + dst += LEN_EULER + src += LEN_QUAT + return euler_frames diff --git a/build/lib/anim_utils/animation_data/skeleton.py b/build/lib/anim_utils/animation_data/skeleton.py new file mode 100644 index 0000000..ad31388 --- /dev/null +++ b/build/lib/anim_utils/animation_data/skeleton.py @@ -0,0 +1,600 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" +Created on Tue Jul 14 14:18:37 2015 + +@author: Erik Herrmann, Martin Manns +""" + +import collections +from copy import copy +import json +import numpy as np +from transformations import quaternion_matrix, quaternion_from_matrix +from .constants import ROTATION_TYPE_QUATERNION, ROTATION_TYPE_EULER, LEN_EULER, LEN_ROOT_POS, LEN_QUAT +from ..motion_editing.coordinate_cyclic_descent import run_ccd, normalize, set_global_orientation, run_ccd_look_at, orient_node_to_target_look_at, LOOK_AT_DIR, SPINE_LOOK_AT_DIR, orient_node_to_target_look_at_projected +from .joint_constraints import CONSTRAINT_MAP + + +class Skeleton(object): + """ Data structure that stores the skeleton hierarchy information + extracted from a BVH file with additional meta information. + """ + def __init__(self): + self.animated_joints = [] + self.free_joints_map = dict() + self.skeleton_model = None + self.frame_time = None + self.root = None + self.aligning_root_node = None # Node that defines body orientation. Can be different from the root node. + self.aligning_root_dir = None + self.reference_frame = None + self.reference_frame_length = None + self.nodes = collections.OrderedDict() + self.tool_nodes = [] + self.max_level = -1 + self.parent_dict = dict() + self._chain_names = [] + self.identity_frame = None + self.annotation = None + + def _get_node_desc(self, name): + node_desc = dict() + node = self.nodes[name] + node_desc["name"] = node.node_name + if node.parent is not None: + node_desc["parent"] = node.parent.node_name + else: + node_desc["parent"] = None + node_desc["quaternion_frame_index"] = node.quaternion_frame_index + node_desc["index"] = node.index + if type(node.offset) == np.ndarray: + offset = node.offset.tolist() + else: + offset = node.offset + node_desc["offset"] = offset + node_desc["channels"] = node.channels + node_desc["rotation"] = node.rotation.tolist() + node_desc["fixed"] = node.fixed + node_desc["level"] = node.level + node_desc["node_type"] = node.node_type + node_desc["children"] = [] + for c in node.children: + c_desc = self._get_node_desc(c.node_name) + node_desc["children"].append(c_desc) + return node_desc + + def to_json(self): + data = dict() + data["animated_joints"] = self.animated_joints + data["free_joints_map"] = self.free_joints_map + data["skeleton_model"] = self.skeleton_model + data["frame_time"] = self.frame_time + data["root"] = self._get_node_desc(self.root) + data["reference_frame"] = self.reference_frame.tolist() + data["tool_nodes"] = self.tool_nodes + data["aligning_root_node"] = self.aligning_root_node + return data + + def save_to_json(self, file_name): + with open(file_name, 'w') as outfile: + tmp = json.dumps(self.to_json(), indent=4) + outfile.write(tmp) + outfile.close() + + def is_motion_vector_complete(self, frames, is_quaternion): + if is_quaternion: + rotation_type = ROTATION_TYPE_QUATERNION + else: + rotation_type = ROTATION_TYPE_EULER + return len(frames[0]) == self.get_number_of_frame_parameters(rotation_type) + + def get_number_of_frame_parameters(self, rotation_type): + n_parameters = 0 + for node_name in self.nodes.keys(): + local_parameters = self.nodes[node_name].get_number_of_frame_parameters(rotation_type) + n_parameters += local_parameters + return n_parameters + + def add_fixed_joint_parameters_to_motion(self, reduced_quat_frames): + new_quat_frames = np.zeros((len(reduced_quat_frames), self.reference_frame_length)) + for idx, reduced_frame in enumerate(reduced_quat_frames): + new_quat_frames[idx] = self.add_fixed_joint_parameters_to_frame(reduced_frame) + return new_quat_frames + + def add_fixed_joint_parameters_to_frame(self, reduced_frame): + """ + Takes parameters from the reduced frame for each joint of the complete skeleton found in the reduced skeleton + otherwise it takes parameters from the reference frame + Parameters: + reduced_frame (np.array): pose parameters ignoring fixed joints. + Returns: + new_frame (np.array): pose parameters including fixed joints. + """ + new_frame = np.zeros(self.reference_frame_length) + joint_index = 0 + for joint_name in self.nodes.keys(): + if len(self.nodes[joint_name].children) > 0 and "EndSite" not in joint_name: + if joint_name == self.root: + new_frame[:7] = reduced_frame[:7] + else: + dest_start = joint_index * 4 + 3 + if self.nodes[joint_name].fixed: + new_frame[dest_start: dest_start+4] = self.nodes[joint_name].rotation + else: + src_start = self.nodes[joint_name].quaternion_frame_index * 4 + 3 + new_frame[dest_start: dest_start+4] = reduced_frame[src_start: src_start + 4] + + joint_index += 1 + return new_frame + + + def add_fixed_joint_parameters_to_other_frame(self, reduced_frame, other_animated_joints): + """ + Takes parameters from the reduced frame for each joint of the complete skeleton found in the reduced skeleton + otherwise it takes parameters from the reference frame + Parameters + reduced_frame (np.array): frame ignoring parameters of joints in other_animated_joints. + other_animated_joints (List): names of animated joints. + Returns + new_frame (np.array): frame including fixed joints. + """ + new_frame = np.zeros(self.reference_frame_length) + src_joint_index = 0 + dest_joint_index = 0 + for joint_name in list(self.nodes.keys()): + if len(self.nodes[joint_name].children) > 0 and "EndSite" not in joint_name: + if joint_name == self.root: + new_frame[:7] = reduced_frame[:7] + src_joint_index += 1 + else: + dest_start = dest_joint_index * 4 + 3 + if joint_name in other_animated_joints: + src_start = other_animated_joints.index(joint_name) * 4 + 3 + new_frame[dest_start: dest_start + 4] = reduced_frame[src_start: src_start + 4] + src_joint_index+=1 + else: + new_frame[dest_start: dest_start + 4] = self.nodes[joint_name].rotation + + dest_joint_index += 1 + return new_frame + + def _get_max_level(self): + levels = [node.level for node in list(self.nodes.values())] + if len(levels) > 0: + return max(levels) + else: + return 0 + + def _get_parent_dict(self): + """Returns a dict of node names to their parent node's name""" + + parent_dict = {} + for node_name in list(self.nodes.keys()): + for child_node in self.nodes[node_name].children: + parent_dict[child_node.node_name] = node_name + + return parent_dict + + def gen_all_parents(self, node_name): + """Generator of all parents' node names of node with node_name""" + + while node_name in self.parent_dict: + parent_name = self.parent_dict[node_name] + yield parent_name + node_name = parent_name + + def _set_joint_weights(self): + """ Gives joints weights according to their distance in the joint hierarchy + to the root joint. The further away the smaller the weight. + """ + self.joint_weights = [1.0/(self.nodes[n].level + 1.0) for n in list(self.nodes.keys())] + self.joint_weight_map = collections.OrderedDict() + weight_index = 0 + for node_name in list(self.nodes.keys()): + self.joint_weight_map[node_name] = self.joint_weights[weight_index] + weight_index += 1 + self.joint_weight_map["RightHand"] = 2.0 + self.joint_weight_map["LeftHand"] = 2.0 + self.joint_weights = list(self.joint_weight_map.values()) + + def get_joint_weights(self): + return list(self.joint_weight_map.values()) + + def _generate_chain_names(self): + chain_names = dict() + for node_name in list(self.nodes.keys()): + chain_names[node_name] = list(self.gen_all_parents(node_name)) + # Names are generated bottom to up --> reverse + chain_names[node_name].reverse() + chain_names[node_name] += [node_name] # Node is not in its parent list + return chain_names + + def get_cartesian_coordinates_from_quaternion(self, target_node_name, quaternion_frame, return_global_matrix=False): + """Returns cartesian coordinates for one node at one frame. Modified to + handle frames with omitted values for joints starting with "Bip" + + Parameters + ---------- + + * node_name: String + \tName of node + * skeleton: Skeleton + \tBVH data structure read from a file + + """ + if self.nodes[target_node_name].level == 0: + root_frame_position = quaternion_frame[:3] + root_node_offset = self.nodes[target_node_name].offset + return [t + o for t, o in + zip(root_frame_position, root_node_offset)] + else: + offsets = [list(self.nodes[node_name].offset) + for node_name in self._chain_names[target_node_name]] + root_position = quaternion_frame[:3].flatten() + offsets[0] = [r + o for r, o in zip(root_position, offsets[0])] + j_matrices = [] + count = 0 + for node_name in self._chain_names[target_node_name]: + if len(self.nodes[target_node_name].children) > 0: # check if it is a joint or an end site + index = self.animated_joints.index(node_name) * 4 + 3 + j_matrix = quaternion_matrix(quaternion_frame[index: index + 4]) + j_matrix[:, 3] = offsets[count] + [1] + else: + j_matrix = np.identity(4) + j_matrix[:, 3] = offsets[count] + [1] + j_matrices.append(j_matrix) + count += 1 + + global_matrix = np.identity(4) + for j_matrix in j_matrices: + global_matrix = np.dot(global_matrix, j_matrix) + if return_global_matrix: + return global_matrix + else: + point = np.array([0, 0, 0, 1]) + point = np.dot(global_matrix, point) + return point[:3].tolist() + + def convert_quaternion_frame_to_cartesian_frame(self, quat_frame, node_names=None): + """ + Converts quaternion frames to cartesian frames by calling get_cartesian_coordinates_from_quaternion for each joint + """ + if node_names is None: + node_names = self.nodes.keys() + cartesian_frame = [] + for node_name in node_names: + if node_name in self.nodes: + position = self.nodes[node_name].get_global_position(quat_frame) + cartesian_frame.append(position) + return cartesian_frame + + def clear_cached_global_matrices(self): + for joint in self.nodes.values(): + joint.clear_cached_global_matrix() + + def get_root_reference_orientation(self): + # reference orientation from BVH: 179.477078182 3.34148613293 -87.6482840381 x y z euler angles + return self.reference_frame[3:7] + + def get_joint_indices(self, joint_names): + indices = [] + for name in joint_names: + if name in list(self.nodes.keys()): + node = self.nodes[name] + indices.append(node.index) + return indices + + def get_n_joints(self): + return len([node for node in list(self.nodes.values()) if len(node.channels) > 0]) + + def get_joint_names(self): + return [k for k, n in list(self.nodes.items()) if len(n.children) > 0] + + def scale(self, scale_factor): + for node in list(self.nodes.values()): + node.offset = np.array(node.offset) * scale_factor + + def get_channels(self, euler=False): + channels = collections.OrderedDict() + for node in list(self.nodes.values()): + if node.node_name in self.animated_joints: + node_channels = copy(node.channels) + if not euler: + if np.all([ch in node_channels for ch in ["Xrotation", "Yrotation", "Zrotation"]]): + node_channels += ["Wrotation"] # TODO fix order + channels[node.node_name] = node_channels + return channels + + + def to_unity_format(self, joint_name_map=None, animated_joints=None, scale=1): + """ Converts the skeleton into a custom json format and applies a coordinate transform to the left-handed coordinate system of Unity. + src: http://answers.unity3d.com/questions/503407/need-to-convert-to-right-handed-coordinates.html + """ + if animated_joints is None: + animated_joints = [j for j, n in self.nodes.items() if "EndSite" not in j and len(n.children) > 0] # self.animated_joints + + #print("animated joints", len(animated_joints)) + joint_descs = [] + self.nodes[self.root].to_unity_format(joint_descs, scale, joint_name_map=joint_name_map) + + data = dict() + data["root"] = self.aligning_root_node + data["jointDescs"] = joint_descs + data["jointSequence"] = animated_joints + default_pose = dict() + default_pose["rotations"] = [] + default_pose["translations"] = [] + for node_name in animated_joints: + node = self.nodes[node_name] + q = node.rotation + if len(q) == 4: + r = {"x": -float(q[1]), "y": float(q[2]), "z": float(q[3]), "w": -float(q[0])} + else: + r = {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + default_pose["rotations"].append(r) + o = node.offset + if node_name == self.root: + o = self.reference_frame[:3] + t = {"x": -float(scale * o[0]), "y": float(scale * o[1]), "z": float(scale * o[2])} + default_pose["translations"].append(t) + + print("new reference", default_pose["rotations"][0]) + data["referencePose"] = default_pose + return data + + def get_reduced_reference_frame(self): + frame = list(self.reference_frame[:3]) + for joint_name in self.animated_joints: + frame += list(self.nodes[joint_name].rotation) + return frame + + def set_reference_frame(self, frame): + self.reference_frame = frame + o = 3 + for joint_name in self.animated_joints: + self.nodes[joint_name].rotation = frame[o:o+4] + o += 4 + + def apply_joint_constraints(self, frame): + for n in self.animated_joints: + if self.nodes[n].joint_constraint is not None: + idx = self.nodes[n].quaternion_frame_index * 4 + 3 + q = frame[idx:idx + 4] + frame[idx:idx + 4] = self.nodes[n].joint_constraint.apply(q) + return frame + + def reach_target_position(self, frame, constraint, eps=0.01, max_iter=50, verbose=False, chain_end_joint=None): + frame, error = run_ccd(self, frame, constraint.joint_name, constraint, eps, max_iter, chain_end_joint, verbose) + print("reached with error", error) + return frame + + def reach_target_positions(self, frame, constraints, chain_end_joints=None, eps=0.0001, n_max_iter=500, ccd_iters=20, verbose=False): + error = np.inf + prev_error = error + n_iters = 0 + is_stuck = False + if chain_end_joints is None: + chain_end_joints = dict() + for c in constraints: + chain_end_joints[c.joint_name] = self.root + while n_iters < n_max_iter and error > eps and not is_stuck: + error = self._reach_target_positions_iteration(frame, constraints, chain_end_joints, eps, n_max_iter, ccd_iters, verbose) + if abs(prev_error - error) < eps: + is_stuck = True + #print("iter", is_stuck, error,eps, prev_error, len(constraints)) + prev_error = error + n_iters += 1 + print("reached with error", error, n_iters) + + # run a last correction step for each constraint separatley + for c in constraints: + root = self.nodes[c.joint_name].parent + d_iter = 1 + while root is not None and root.parent is not None and d_iter < 2: + root = root.parent + if root is not None: + chain_end_joints[c.joint_name] = root.node_name + error = self._reach_target_positions_iteration(frame, [c], chain_end_joints, eps, n_max_iter, ccd_iters, verbose) + n_iters += 1 + print("error in last correction", c.joint_name, error) + return frame + + def _reach_target_positions_iteration(self, frame, constraints, chain_end_joints=None, eps=0.0001, n_max_iter=500, ccd_iters=20, verbose=False): + error = 0 + for c in constraints: + joint_error = 0 + if c.look_at: + pos = c.look_at_pos + if pos is None: + pos = c.position + joint_name = self.skeleton_model["joints"]["head"] + if joint_name is not None: + local_dir = LOOK_AT_DIR + if "look_at_dir" in self.skeleton_model: + local_dir = self.skeleton_model["look_at_dir"] + parent_node = self.nodes[joint_name].parent + if parent_node is not None: + frame = orient_node_to_target_look_at(self, frame, parent_node.node_name, joint_name, pos, local_dir=local_dir) + if c.position is not None and c.relative_parent_joint_name is None: + frame, _joint_error = run_ccd(self, frame, c.joint_name, c, eps, ccd_iters, chain_end_joints[c.joint_name], verbose) + joint_error += _joint_error + elif c.relative_parent_joint_name is not None: # run ccd on relative constraint + #turn relative constraint into a normal constraint + _c = c.instantiate_relative_constraint(self, frame) + #print("create relative constraint", _c.joint_name, c.relative_offset) + frame, _joint_error = run_ccd(self, frame, _c.joint_name, _c, eps, ccd_iters, chain_end_joints[c.joint_name], verbose) + joint_error += _joint_error + elif c.orientation is not None: + frame = set_global_orientation(self, frame, c.joint_name, c.orientation) + else: + print("ignore constraint on", c.joint_name, c.position) + error += joint_error + return error + + def set_joint_orientation(self, frame, joint_name, orientation): + m = quaternion_matrix(orientation) + parent = self.nodes[joint_name].parent + if parent is not None: + parent_m = parent.get_global_matrix(frame, use_cache=False) + local_m = np.dot(np.linalg.inv(parent_m), m) + q = quaternion_from_matrix(local_m) + offset = self.nodes[joint_name].quaternion_frame_index*4+3 + frame[offset:offset+4] = normalize(q) + return frame + + def look_at(self, frame, joint_name, position, eps=0.0001, n_max_iter=1, local_dir=LOOK_AT_DIR, chain_end_joint=None): + #self.clear_cached_global_matrices() + frame, error = run_ccd_look_at(self, frame, joint_name, position, eps, n_max_iter, local_dir, chain_end_joint) + #frame = orient_node_to_target_look_at(self,frame,joint_name, joint_name, position) + return frame + + def look_at_projected(self, frame, joint_name, position, local_dir=LOOK_AT_DIR): + """ apply a 2d rotation around global y """ + twist_axis = None + max_angle = None + p_name = self.nodes[joint_name].parent.node_name + if self.nodes[p_name ].joint_constraint is not None: + twist_axis = self.nodes[p_name ].joint_constraint.axis + max_angle = np.rad2deg(self.nodes[p_name ].joint_constraint.twist_max) + frame = orient_node_to_target_look_at_projected(self, frame, self.nodes[joint_name].parent.node_name, joint_name, position, local_dir, twist_axis, max_angle) + return frame + + def get_reduced_euler_frames(self, euler_frames, has_root=True): + ''' + create reduced size of euler frames based on animated joints + :param euler_frames: n_frames * n_dims + :return: + ''' + nonEndSite_joints = self.get_joints_without_EndSite() + euler_frames = np.asarray(euler_frames) + if has_root: + assert euler_frames.shape[1] == LEN_ROOT_POS + LEN_EULER * len(nonEndSite_joints) + new_euler_frames = np.zeros([euler_frames.shape[0], LEN_ROOT_POS + LEN_EULER * len(self.animated_joints)]) + else: + assert euler_frames.shape[1] == LEN_EULER * len(nonEndSite_joints) + new_euler_frames = np.zeros([euler_frames.shape[0], LEN_EULER * len(self.animated_joints)]) + for i in range(euler_frames.shape[0]): + new_euler_frames[i] = self.get_reduced_euler_frame(euler_frames[i], has_root=has_root) + return new_euler_frames + + def get_reduced_euler_frame(self, euler_frame, has_root=True): + ''' + create a reduced size of euler frame based on animated joints + :param euler_frames: n_frames * n_dims + :return: + ''' + if has_root: + new_euler_frame = np.zeros(LEN_ROOT_POS + LEN_EULER * len(self.animated_joints)) + new_euler_frame[:LEN_ROOT_POS] = euler_frame[:LEN_ROOT_POS] + i = 0 + for joint in self.animated_joints: + if joint == self.root: + new_euler_frame[LEN_ROOT_POS+i*LEN_EULER: LEN_ROOT_POS+(i+1)*LEN_EULER] = euler_frame[self.nodes[joint].channel_indices[LEN_ROOT_POS:]] + else: + new_euler_frame[LEN_ROOT_POS+i*LEN_EULER: LEN_ROOT_POS+(i+1)*LEN_EULER] = euler_frame[self.nodes[joint].channel_indices] + i += 1 + else: + new_euler_frame = np.zeros(LEN_EULER * len(self.animated_joints)) + i = 0 + for joint in self.animated_joints: + if joint == self.root: + new_euler_frame[LEN_ROOT_POS + i * LEN_EULER: LEN_ROOT_POS + (i + 1) * LEN_EULER] = euler_frame[ + self.nodes[joint].channel_indices[LEN_ROOT_POS:]] + else: + new_euler_frame[LEN_ROOT_POS + i * LEN_EULER: LEN_ROOT_POS + (i + 1) * LEN_EULER] = euler_frame[ + self.nodes[joint].channel_indices] + i += 1 + return new_euler_frame + + def get_joints_without_EndSite(self): + joints = [] + for joint in self.nodes.keys(): + if 'EndSite' not in joint: + joints.append(joint) + return joints + + def get_body_hip2foot_height(self): + """ helper function for autoscale by gemlongman""" + if self.skeleton_model is None: + print("skeleton.skeleton_model is None") + return None + joint_pelvis = self.skeleton_model["joints"]["pelvis"] # upperleg maybe better + joint_right_toe = self.skeleton_model["joints"]["right_ankle"]# ["right_toe"] + ident_f = self.identity_frame + joint_pelvis_p = self.nodes[joint_pelvis].get_global_position(ident_f) + joint_right_toe_p = self.nodes[joint_right_toe].get_global_position(ident_f) + delta = joint_pelvis_p - joint_right_toe_p + #it should be same dircetion as body up,there is a simple way to get length + if delta[1] < 0.01: + return max(abs(delta.max()), abs(delta.min())) + return delta[1] + + def add_constraints(self, joint_constraints): + joint_map = self.skeleton_model["joints"] + for j in joint_constraints: + if j in joint_map: + skel_j = joint_map[j] + else: + continue + if skel_j not in self.nodes: + continue + c_desc = joint_constraints[j] + if "stiffness" in c_desc: + self.nodes[skel_j].stiffness = c_desc["stiffness"] + c_type = c_desc["type"] + c = None + if c_type in CONSTRAINT_MAP: + c = CONSTRAINT_MAP[c_type].from_dict(skel_j, c_desc) + print("add"+c_desc["type"] +"constraint to", skel_j) + self.nodes[skel_j].joint_constraint = c + + + def get_bounding_box(self, frame=None): + """ Calculate axis aligned bounding box. + Parameters: + frame (np.array): pose parameters for the skeleton. + Returns: + tuple(min_p, max_p): minimum and maximum points. + """ + if frame is None and self.reference_frame is not None: + frame = self.reference_frame + else: + raise Exception("No frame given and reference_frame is None") + min_p = np.ones((3))*np.inf + max_p = np.ones((3))*-np.inf + for k, n in self.nodes.items(): + p = n.get_global_position(frame) + if p[0] >= max_p[0]: + max_p[0] = p[0] + elif p[0] < min_p[0]: + min_p[0] = p[0] + if p[1] >= max_p[1]: + max_p[1] = p[1] + elif p[1] < min_p[1]: + min_p[1] = p[1] + if p[2] >= max_p[2]: + max_p[2] = p[2] + elif p[2] < min_p[2]: + min_p[2] = p[2] + return min_p, max_p diff --git a/build/lib/anim_utils/animation_data/skeleton_builder.py b/build/lib/anim_utils/animation_data/skeleton_builder.py new file mode 100644 index 0000000..a1fc7e8 --- /dev/null +++ b/build/lib/anim_utils/animation_data/skeleton_builder.py @@ -0,0 +1,529 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import collections +from copy import deepcopy +import json +import numpy as np + +from .skeleton import Skeleton +from .skeleton_node import SkeletonRootNode, SkeletonJointNode, SkeletonEndSiteNode, SKELETON_NODE_TYPE_JOINT, SKELETON_NODE_TYPE_END_SITE +from .quaternion_frame import convert_euler_to_quaternion_frame +from .joint_constraints import HingeConstraint2 +from .acclaim import asf_to_bvh_channels + + +DEFAULT_ROOT_DIR = [0, 0, 1] + + +def create_identity_frame(skeleton): + skeleton.identity_frame = np.zeros(skeleton.reference_frame_length) + offset = 3 + for j in skeleton.animated_joints: + skeleton.identity_frame[offset:offset + 4] = [1, 0, 0, 0] + offset += 4 + + +def create_euler_frame_indices(skeleton): + nodes_without_endsite = [node for node in list(skeleton.nodes.values()) if node.node_type != SKELETON_NODE_TYPE_END_SITE] + for node in nodes_without_endsite: + node.euler_frame_index = nodes_without_endsite.index(node) + + +def read_reference_frame_from_bvh_reader(bvh_reader, frame_index=0): + quat_frame = convert_euler_to_quaternion_frame(bvh_reader, bvh_reader.frames[frame_index], False, animated_joints=None) + return np.array(bvh_reader.frames[0][:3].tolist() + quat_frame.tolist()) + + +def add_tool_nodes(skeleton, node_names, new_tool_bones): + for b in new_tool_bones: + add_new_end_site(skeleton, node_names, b["parent_node_name"], b["new_node_offset"]) + skeleton.tool_nodes.append(b["new_node_name"]) + + +def add_new_end_site(skeleton, node_names, parent_node_name, offset): + if parent_node_name in list(node_names.keys()): + level = node_names[parent_node_name]["level"] + 1 + node_desc = dict() + node_desc["level"] = level + node_desc["offset"] = offset + +def reference_frame_from_unity(data): + n_j = len(data["rotations"]) + q_frame = np.zeros(n_j*4+3) + q_frame[:3] = arr_from_unity_t(data["translations"][0]) + o = 3 + for q in data["rotations"]: + q_frame[o:o+4] = arr_from_unity_q(q) + o+=4 + return q_frame + + +def generate_reference_frame(skeleton, animated_joints): + identity_frame = [0,0,0] + frame = [0, 0, 0] + joint_idx = 0 + node_list = [(skeleton.nodes[n].index, n) for n in skeleton.nodes.keys() if skeleton.nodes[n].index >= 0] + node_list.sort() + for idx, node in node_list: + frame += list(skeleton.nodes[node].rotation) + if node in animated_joints: + identity_frame += [1.0,0.0,0.0,0.0] + skeleton.nodes[node].quaternion_frame_index = joint_idx + joint_idx += 1 + else: + skeleton.nodes[node].quaternion_frame_index = -1 + skeleton.reference_frame = np.array(frame) + skeleton.reference_frame_length = len(frame) + skeleton.identity_frame = np.array(identity_frame) + + +def arr_from_unity_q(_q): + q = np.zeros(4) + q[0] = - _q["w"] + q[1] = - _q["x"] + q[2] = _q["y"] + q[3] = _q["z"] + return q + + +def arr_from_unity_t(_t): + t = np.zeros(3) + t[0] = - _t["x"] + t[1] = _t["y"] + t[2] = _t["z"] + return t + +class SkeletonBuilder(object): + + def load_from_bvh(self, bvh_reader, animated_joints=None, reference_frame=None, skeleton_model=None, tool_bones=None): + skeleton = Skeleton() + if animated_joints is None: + animated_joints = list(bvh_reader.get_animated_joints()) + skeleton.animated_joints = animated_joints + skeleton.frame_time = bvh_reader.frame_time + skeleton.root = bvh_reader.root + skeleton.aligning_root_dir = DEFAULT_ROOT_DIR + if reference_frame is None: + skeleton.reference_frame = read_reference_frame_from_bvh_reader(bvh_reader) + else: + skeleton.reference_frame = reference_frame + skeleton.reference_frame_length = len(skeleton.reference_frame) + skeleton.tool_nodes = [] + if tool_bones is not None: + add_tool_nodes(skeleton, bvh_reader.node_names, tool_bones) + skeleton.nodes = collections.OrderedDict() + joint_list = list(bvh_reader.get_animated_joints()) + self.construct_hierarchy_from_bvh(skeleton, joint_list, bvh_reader.node_names, skeleton.root, 0) + + create_euler_frame_indices(skeleton) + SkeletonBuilder.set_meta_info(skeleton) + + if skeleton_model is not None: + skeleton.skeleton_model = skeleton_model + #skeleton.add_heels(skeleton_model) + return skeleton + + def construct_hierarchy_from_bvh(self, skeleton, joint_list, node_info, node_name, level): + + channels = [] + if "channels" in node_info[node_name]: + channels = node_info[node_name]["channels"] + is_fixed = True + quaternion_frame_index = -1 + if node_name in skeleton.animated_joints: + is_fixed = False + quaternion_frame_index = skeleton.animated_joints.index(node_name) + joint_index = -1 + if node_name in joint_list: + joint_index = joint_list.index(node_name) + if node_name == skeleton.root: + node = SkeletonRootNode(node_name, channels, None, level) + elif "children" in list(node_info[node_name].keys()) and len(node_info[node_name]["children"]) > 0: + node = SkeletonJointNode(node_name, channels, None, level) + offset = joint_index * 4 + 3 + node.rotation = skeleton.reference_frame[offset: offset + 4] + else: + node = SkeletonEndSiteNode(node_name, channels, None, level) + node.fixed = is_fixed + node.quaternion_frame_index = quaternion_frame_index + node.index = joint_index + node.offset = node_info[node_name]["offset"] + skeleton.nodes[node_name] = node + if "children" in node_info[node_name]: + for c in node_info[node_name]["children"]: + c_node = self.construct_hierarchy_from_bvh(skeleton, joint_list, node_info, c, level+1) + c_node.parent = node + node.children.append(c_node) + return node + + def load_from_json_file(self, filename): + with open(filename) as infile: + data = json.load(infile) + skeleton = self.load_from_custom_unity_format(data) + return skeleton + + def load_from_custom_unity_format(self, data, frame_time=1.0/30, add_extra_end_site=False): + skeleton = Skeleton() + animated_joints = data["jointSequence"] + if len(animated_joints) == 0: + print("Error no joints defined") + return skeleton + + print("load from json", len(animated_joints)) + skeleton.animated_joints = animated_joints + + skeleton.skeleton_model = collections.OrderedDict() + skeleton.skeleton_model["joints"] = dict() + if "head_joint" in data: + skeleton.skeleton_model["joints"]["head"] = data["head_joint"] + if "neck_joint" in data: + skeleton.skeleton_model["joints"]["neck"] = data["neck_joint"] + + skeleton.frame_time = frame_time + skeleton.nodes = collections.OrderedDict() + skeleton.root = animated_joints[0] + root = self._create_node_from_unity_desc(skeleton, skeleton.root, data, None, 0, add_extra_end_site) + + SkeletonBuilder.set_meta_info(skeleton) + create_euler_frame_indices(skeleton) + if "root" in data: + skeleton.aligning_root_node = data["root"] + else: + skeleton.aligning_root_node = skeleton.root + skeleton.aligning_root_dir = DEFAULT_ROOT_DIR + + skeleton.reference_frame = reference_frame_from_unity(data["referencePose"]) + print("reference", skeleton.reference_frame[3:7],data["referencePose"]["rotations"][0]) + skeleton.reference_frame_length = len(skeleton.reference_frame) + return skeleton + + def load_from_json_data(self, data, animated_joints=None, use_all_joints=False): + def extract_animated_joints(node, animated_joints): + animated_joints.append(node["name"]) + for c in node["children"]: + if c["index"] >= 0 and len(c["children"]) > 0: + extract_animated_joints(c, animated_joints) + + skeleton = Skeleton() + if animated_joints is not None: + skeleton.animated_joints = animated_joints + elif "animated_joints" in data: + skeleton.animated_joints = data["animated_joints"] + else: + animated_joints = list() + extract_animated_joints(data["root"], animated_joints) + skeleton.animated_joints = animated_joints + + skeleton.free_joints_map = data.get("free_joints_map", dict()) + if "skeleton_model" in data: + skeleton.skeleton_model = data["skeleton_model"] + else: + skeleton.skeleton_model = collections.OrderedDict() + skeleton.skeleton_model["joints"] = dict() + if "head_joint" in data: + skeleton.skeleton_model["joints"]["head"] = data["head_joint"] + if "neck_joint" in data: + skeleton.skeleton_model["joints"]["neck"] = data["neck_joint"] + + skeleton.frame_time = data["frame_time"] + skeleton.nodes = collections.OrderedDict() + root = self._create_node_from_desc(skeleton, data["root"], None, 0) + skeleton.root = root.node_name + if "tool_nodes" in data: + skeleton.tool_nodes = data["tool_nodes"] + create_euler_frame_indices(skeleton) + skeleton.aligning_root_node = data.get("aligning_root_node",skeleton.root) + skeleton.aligning_root_dir = data.get("aligning_root_dir",DEFAULT_ROOT_DIR) + if "reference_frame" in data: + skeleton.reference_frame = data["reference_frame"] + skeleton.reference_frame_length = len(skeleton.reference_frame) + if skeleton.reference_frame is None or use_all_joints: + generate_reference_frame(skeleton, skeleton.animated_joints) + SkeletonBuilder.set_meta_info(skeleton) + return skeleton + + def load_from_fbx_data(self, data): + skeleton = Skeleton() + skeleton.nodes = collections.OrderedDict() + skeleton.animated_joints = data["animated_joints"] + skeleton.root = data["root"] + self._create_node_from_fbx_node_desc(skeleton, data, skeleton.root, None, 0) + skeleton.frame_time = data["frame_time"] + SkeletonBuilder.set_meta_info(skeleton) + return skeleton + + def _create_node_from_fbx_node_desc(self, skeleton, data, node_name, parent, level=0): + node_data = data["nodes"][node_name] + channels = node_data["channels"] + if parent is None: + node = SkeletonRootNode(node_name, channels, parent, level) + elif node_data["node_type"] == SKELETON_NODE_TYPE_JOINT: + node = SkeletonJointNode(node_name, channels, parent, level) + else: + node = SkeletonEndSiteNode(node_name, channels, parent, level) + node.fixed = node_data["fixed"] + node.index = node_data["index"] + node.offset = np.array(node_data["offset"]) + node.rotation = np.array(node_data["rotation"]) + if node_name in skeleton.animated_joints: + node.quaternion_frame_index = skeleton.animated_joints.index(node_name) + node.fixed = False + else: + node.quaternion_frame_index = -1 + node.fixed = True + node.children = [] + skeleton.nodes[node_name] = node + for c_name in node_data["children"]: + c_node = self._create_node_from_fbx_node_desc(skeleton, data, c_name, node, level+1) + node.children.append(c_node) + + return node + + def _create_node_from_desc(self, skeleton, data, parent, level): + node_name = data["name"] + channels = data["channels"] + if parent is None: + node = SkeletonRootNode(node_name, channels, parent, level) + elif data["node_type"] == SKELETON_NODE_TYPE_JOINT: + node = SkeletonJointNode(node_name, channels, parent, level) + else: + node = SkeletonEndSiteNode(node_name, channels, parent, level) + # node.fixed = data["fixed"] + node.index = data["index"] + node.offset = np.array(data["offset"]) + node.rotation = np.array(data["rotation"]) + if node_name in skeleton.animated_joints: + node.quaternion_frame_index = skeleton.animated_joints.index(node_name) + node.fixed = False + else: + node.quaternion_frame_index = -1 + node.fixed = True + + skeleton.nodes[node_name] = node + skeleton.nodes[node_name].children = [] + for c_desc in data["children"]: + skeleton.nodes[node_name].children.append(self._create_node_from_desc(skeleton, c_desc, node, level+1)) + return node + + def get_joint_desc(self, data, name): + for idx in range(len(data["jointDescs"])): + if data["jointDescs"][idx]["name"] == name: + return data["jointDescs"][idx] + return None + + def _create_node_from_unity_desc(self, skeleton, node_name, data, parent, level, add_extra_end_site=False): + node_data = self.get_joint_desc(data, node_name) + if node_data is None: + return + + if parent is None: + channels = ["Xposition","Yposition","Zposition", "Xrotation","Yrotation","Zrotation"] + else:# len(node_data["children"]) > 0: + channels = ["Xrotation","Yrotation","Zrotation"] + + if parent is None: + node = SkeletonRootNode(node_name, channels, parent, level) + else:# len(node_data["children"]) > 0: + node = SkeletonJointNode(node_name, channels, parent, level) + node.offset = np.array(node_data["offset"]) + node.offset[0] *= -1 + node.rotation = np.array(node_data["rotation"]) + node.rotation[0] *= -1 + node.rotation[1] *= -1 + + if node_name in skeleton.animated_joints: + node.quaternion_frame_index = skeleton.animated_joints.index(node_name) + node.fixed = False + else: + node.quaternion_frame_index = -1 + node.fixed = True + + skeleton.nodes[node_name] = node + skeleton.nodes[node_name].children = [] + if len(node_data["children"]) > 0: + node.index = node.quaternion_frame_index + for c_name in node_data["children"]: + c_node = self._create_node_from_unity_desc(skeleton, c_name, data, node, level+1) + if c_node is not None: + skeleton.nodes[node_name].children.append(c_node) + if add_extra_end_site: + print("add extra end site") + node.index = -1 + channels = [] + c_name = node_name+"_EndSite" + c_node = SkeletonEndSiteNode(c_name, channels, node, level+1) + skeleton.nodes[node_name].children.append(c_node) + skeleton.nodes[c_name] = c_node + return node + + + @classmethod + def construct_arm_with_constraints(cls, n_joints, length): + skeleton = Skeleton() + skeleton.frame_time = 1 / 30 + animated_joints = [] + animated_joints.append("root") + skeleton.root = "root" + channels = ["rotationX", "rotationY", "rotationZ", "rotationW"] + node = SkeletonRootNode("root", channels, None, 0) + node.fixed = False + node.index = 0 + node.offset = [0, 0, 0] + node.rotation = [1, 0, 0, 0] + node.quaternion_frame_index = 0 + skeleton.nodes["root"] = node + parent = node + swing_axis = np.array([0,0,1]) + twist_axis = np.array([0, 1, 0]) + + angle_range = [0,90] + for n in range(1, n_joints + 1): # start after the root joint and add one endsite + if n + 1 < n_joints + 1: + node_name = "joint" + str(n) + node = SkeletonJointNode(node_name, channels, parent, n) + animated_joints.append(node_name) + node.fixed = False + node.quaternion_frame_index = n + node.index = n + node.offset = np.array([0, length, 0], dtype=np.float) + print("create", node_name, node.offset) + if n in [1]: + node.joint_constraint = HingeConstraint2(swing_axis, twist_axis) + else: + node_name = "joint" + str(n - 1) + "_EndSite" + node = SkeletonEndSiteNode(node_name, channels, parent, n) + node.fixed = True + node.quaternion_frame_index = -1 + node.index = -1 + node.offset = np.array([0, 0, 0], dtype=np.float) + print("create", node_name, node.offset) + parent.children.append(node) + + node.rotation = [1, 0, 0, 0] + skeleton.nodes[node_name] = node + parent = node + + skeleton.animated_joints = animated_joints + SkeletonBuilder.set_meta_info(skeleton) + return skeleton + + + @classmethod + def generate_reference_frame(cls, skeleton): + n_animated_joints = len(skeleton.animated_joints) + reference_frame = np.zeros(n_animated_joints * 4 + 3) + o = 3 + for n in skeleton.animated_joints: + reference_frame[o:o + 4] = skeleton.nodes[n].rotation + o += 4 + return reference_frame + + + + def load_from_asf_data(self, data, frame_time=1.0/30): + skeleton = Skeleton() + animated_joints = ["root"]+list(data["bones"].keys()) + + print("load from asf", len(animated_joints)) + skeleton.animated_joints =animated_joints + skeleton.skeleton_model = collections.OrderedDict() + skeleton.skeleton_model["joints"] = dict() + + skeleton.frame_time = frame_time + skeleton.nodes = collections.OrderedDict() + skeleton.root = "root" + if skeleton.root is None: + skeleton.root = animated_joints[0] + root = self._create_node_from_asf_data(skeleton, skeleton.root, data, None, 0) + + SkeletonBuilder.set_meta_info(skeleton) + return skeleton + + def _create_node_from_asf_data(self, skeleton, node_name, data, parent, level): + if parent is None: + channels = asf_to_bvh_channels(data["root"]["order"]) + node = SkeletonRootNode(node_name, channels, parent, level) + node.format_meta_data = data["root"] + node.quaternion_frame_index = 0 + node.fixed = False + node.format_meta_data["asf_channels"] = deepcopy(node.rotation_order) + elif "dof" in data["bones"][node_name]: + channels = asf_to_bvh_channels(data["bones"][node_name]["dof"]) + asf_channels = deepcopy(channels) + if len(channels) < 3: + channels = ['Xrotation','Yrotation','Zrotation'] + node = SkeletonJointNode(node_name, channels, parent, level) + node.format_meta_data = data["bones"][node_name] + node.format_meta_data["asf_channels"] = asf_channels + if "offset" in data["bones"][node_name]: + node.offset = data["bones"][node_name]["offset"] + #if parent.node_name is not "root": + # node.offset = np.array(data["bones"][parent.node_name]["direction"]) + # node.offset *= data["bones"][parent.node_name]["length"] + if node_name in skeleton.animated_joints: + node.quaternion_frame_index = skeleton.animated_joints.index(node_name) + node.fixed = False + else: + channels = [] + node = SkeletonJointNode(node_name, channels, parent, level) + node.format_meta_data = data["bones"][node_name] + node.quaternion_frame_index = -1 + node.fixed = True + node.format_meta_data["asf_channels"] = channels + + n_children = 0 + if node_name in data["children"]: + n_children = len(data["children"][node_name]) + + skeleton.nodes[node_name] = node + skeleton.nodes[node_name].children = [] + if n_children > 0: + node.index = node.quaternion_frame_index + for c_name in data["children"][node_name]: + c_node = self._create_node_from_asf_data(skeleton, c_name, data, node, level+1) + if c_node is not None: + skeleton.nodes[node_name].children.append(c_node) + else: + node.index = node.quaternion_frame_index + channels = [] + end_site_name = node_name +"EndSite" + end_site_node = SkeletonJointNode(end_site_name, channels, node, level+1) + end_site_node.quaternion_frame_index = -1 + end_site_node.fixed = True + skeleton.nodes[end_site_name] = end_site_node + skeleton.nodes[end_site_name].children = [] + skeleton.nodes[node_name].children.append(end_site_node) + return node + + + @classmethod + def set_meta_info(cls, skeleton): + skeleton.max_level = skeleton._get_max_level() + skeleton._set_joint_weights() + skeleton.parent_dict = skeleton._get_parent_dict() + skeleton._chain_names = skeleton._generate_chain_names() + skeleton.aligning_root_node = skeleton.root + if skeleton.reference_frame is None: + skeleton.reference_frame = SkeletonBuilder.generate_reference_frame(skeleton) + skeleton.reference_frame_length = len(skeleton.reference_frame) + create_identity_frame(skeleton) diff --git a/build/lib/anim_utils/animation_data/skeleton_models.py b/build/lib/anim_utils/animation_data/skeleton_models.py new file mode 100644 index 0000000..80b957a --- /dev/null +++ b/build/lib/anim_utils/animation_data/skeleton_models.py @@ -0,0 +1,1314 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" This module contains mappings from a standard set of human joints to different skeletons + +""" +import numpy as np +import collections + + + +ROCKETBOX_TOOL_BONES = [{ + "new_node_name": 'LeftToolEndSite', + "parent_node_name": 'LeftHand', + "new_node_offset": [6.1522069, -0.09354633, 3.33790343] +}, { + "new_node_name": 'RightToolEndSite', + "parent_node_name": 'RightHand', + "new_node_offset": [6.1522069, 0.09354633, 3.33790343] +}, { + "new_node_name": 'RightScrewDriverEndSite', + "parent_node_name": 'RightHand', + "new_node_offset": [22.1522069, -9.19354633, 3.33790343] +}, { + "new_node_name": 'LeftScrewDriverEndSite', + "parent_node_name": 'LeftHand', + "new_node_offset": [22.1522069, 9.19354633, 3.33790343] +} +] +ROCKETBOX_FREE_JOINTS_MAP = {"LeftHand": ["Spine", "LeftArm", "LeftForeArm"], + "RightHand": ["Spine", "RightArm", "RightForeArm"], + "LeftToolEndSite": ["Spine", "LeftArm", "LeftForeArm"], + "RightToolEndSite": ["Spine", "RightArm", "RightForeArm"], # , "RightHand" + "Head": [], + "RightScrewDriverEndSite": ["Spine", "RightArm", "RightForeArm"], + "LeftScrewDriverEndSite": ["Spine", "LeftArm", "LeftForeArm"] + } +ROCKETBOX_REDUCED_FREE_JOINTS_MAP = {"LeftHand": ["LeftArm", "LeftForeArm"], + "RightHand": ["RightArm", "RightForeArm"], + "LeftToolEndSite": ["LeftArm", "LeftForeArm"], + "RightToolEndSite": ["RightArm", "RightForeArm"], + "Head": [], + "RightScrewDriverEndSite": ["RightArm", "RightForeArm"], + "LeftScrewDriverEndSite": ["LeftArm", "LeftForeArm"] + } +DEG2RAD = np.pi / 180 +hand_bounds = [{"dim": 0, "min": 30 * DEG2RAD, "max": 180 * DEG2RAD}, + {"dim": 1, "min": -15 * DEG2RAD, "max": 120 * DEG2RAD}, + {"dim": 1, "min": -40 * DEG2RAD, "max": 40 * DEG2RAD}] + + +ROCKETBOX_BOUNDS = {"LeftArm": [], # {"dim": 1, "min": 0, "max": 90} + "RightArm": [] # {"dim": 1, "min": 0, "max": 90},{"dim": 0, "min": 0, "max": 90} + , "RightHand": hand_bounds, # [[-90, 90],[0, 0],[-90,90]] + "LeftHand": hand_bounds # [[-90, 90],[0, 0],[-90,90]] + } + +ROCKETBOX_ANIMATED_JOINT_LIST = ["Hips", "Spine", "Spine_1", "Neck", "Head", "LeftShoulder", "LeftArm", "LeftForeArm", + "LeftHand", "RightShoulder", "RightArm", "RightForeArm", "RightHand", "LeftUpLeg", + "LeftLeg", "LeftFoot", "RightUpLeg", "RightLeg", "RightFoot"] + + +IK_CHAINS_DEFAULT_SKELETON = dict() +IK_CHAINS_DEFAULT_SKELETON["right_ankle"] = {"root": "right_hip", "joint": "right_knee", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_DEFAULT_SKELETON["left_ankle"] = {"root": "left_hip", "joint": "left_knee", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} + + +IK_CHAINS_RAW_SKELETON = dict() +IK_CHAINS_RAW_SKELETON["RightFoot"] = {"root": "RightUpLeg", "joint": "RightLeg", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_RAW_SKELETON["LeftFoot"] = {"root": "LeftUpLeg", "joint": "LeftLeg", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} + +#IK_CHAINS_RAW_SKELETON["RightToeBase"] = {"root": "RightUpLeg", "joint": "RightLeg", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} +#IK_CHAINS_RAW_SKELETON["LeftToeBase"] = {"root": "LeftUpLeg", "joint": "LeftLeg", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} + +IK_CHAINS_RAW_SKELETON["RightHand"] = {"root": "RightArm", "joint": "RightForeArm", "joint_axis": [0, 1, 0], "end_effector_dir": [1,0,0]} +IK_CHAINS_RAW_SKELETON["LeftHand"] = {"root": "LeftArm", "joint": "LeftForeArm", "joint_axis": [0, 1, 0], "end_effector_dir": [1,0,0]} + + +IK_CHAINS_ROCKETBOX_SKELETON = dict() +IK_CHAINS_ROCKETBOX_SKELETON["RightFoot"] = {"root": "RightUpLeg", "joint": "RightLeg", "joint_axis": [0, 1, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_ROCKETBOX_SKELETON["LeftFoot"] = {"root": "LeftUpLeg", "joint": "LeftLeg", "joint_axis": [0, 1, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_ROCKETBOX_SKELETON["RightHand"] = {"root": "RightArm", "joint": "RightForeArm", "joint_axis": [0, 1, 0], "end_effector_dir": [1,0,0]} +IK_CHAINS_ROCKETBOX_SKELETON["LeftHand"] = {"root": "LeftArm", "joint": "LeftForeArm", "joint_axis": [0, 1, 0], "end_effector_dir": [1,0,0]} + + +IK_CHAINS_GAME_ENGINE_SKELETON = dict() +IK_CHAINS_GAME_ENGINE_SKELETON["foot_l"] = {"root": "thigh_l", "joint": "calf_l", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_GAME_ENGINE_SKELETON["foot_r"] = {"root": "thigh_r", "joint": "calf_r", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_GAME_ENGINE_SKELETON["hand_r"] = {"root": "upperarm_r", "joint": "lowerarm_r", "joint_axis": [1, 0, 0], "end_effector_dir": [1,0,0]} +IK_CHAINS_GAME_ENGINE_SKELETON["hand_l"] = {"root": "upperarm_l", "joint": "lowerarm_l", "joint_axis": [1, 0, 0], "end_effector_dir": [1,0,0]} + + + +IK_CHAINS_CUSTOM_SKELETON = dict() +IK_CHAINS_CUSTOM_SKELETON["L_foot_jnt"] = {"root": "L_upLeg_jnt", "joint": "L_lowLeg_jnt", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_CUSTOM_SKELETON["R_foot_jnt"] = {"root": "R_upLeg_jnt", "joint": "R_lowLeg_jnt", "joint_axis": [1, 0, 0], "end_effector_dir": [0,0,1]} +IK_CHAINS_CUSTOM_SKELETON["R_hand_jnt"] = {"root": "R_upArm_jnt", "joint": "R_lowArm_jnt", "joint_axis": [1, 0, 0], "end_effector_dir": [1,0,0]} +IK_CHAINS_CUSTOM_SKELETON["L_hand_jnt"] = {"root": "L_upArm_jnt", "joint": "L_lowArm_jnt", "joint_axis": [1, 0, 0], "end_effector_dir": [1,0,0]} + + +RIGHT_SHOULDER = "RightShoulder" +RIGHT_ELBOW = "RightElbow" +RIGHT_WRIST = "RightHand" +ELBOW_AXIS = [0,1,0] + +LOCOMOTION_ACTIONS = ["walk", "carryRight", "carryLeft", "carryBoth"] +DEFAULT_WINDOW_SIZE = 20 +LEFT_FOOT = "LeftFoot" +RIGHT_FOOT = "RightFoot" +RIGHT_TOE = "RightToeBase" +LEFT_TOE = "LeftToeBase" +LEFT_HEEL = "LeftHeel" +RIGHT_HEEL = "RightHeel" + +OFFSET = 0 +RAW_SKELETON_FOOT_JOINTS = [RIGHT_TOE, LEFT_TOE, RIGHT_HEEL,LEFT_HEEL] +HEEL_OFFSET = [0, -6.480602, 0] +RAW_SKELETON_JOINTS = collections.OrderedDict() +RAW_SKELETON_JOINTS["root"] = "Hips" +RAW_SKELETON_JOINTS["pelvis"] = "Hips" +RAW_SKELETON_JOINTS["spine"] = "Spine" +RAW_SKELETON_JOINTS["spine_1"] = "Spine1" +RAW_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +RAW_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +RAW_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +RAW_SKELETON_JOINTS["right_shoulder"] = "RightArm" +RAW_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +RAW_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +RAW_SKELETON_JOINTS["left_wrist"] = "LeftHand" +RAW_SKELETON_JOINTS["right_wrist"] = "RightHand" +RAW_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +RAW_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +RAW_SKELETON_JOINTS["left_knee"] = "LeftLeg" +RAW_SKELETON_JOINTS["right_knee"] = "RightLeg" +RAW_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +RAW_SKELETON_JOINTS["right_ankle"] = "RightFoot" +RAW_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +RAW_SKELETON_JOINTS["right_toe"] = "RightToeBase" +RAW_SKELETON_JOINTS["left_heel"] = "LeftHeel" +RAW_SKELETON_JOINTS["right_heel"] = "RightHeel" +RAW_SKELETON_JOINTS["neck"] = "Neck" +RAW_SKELETON_JOINTS["head"] = "Head" +RAW_SKELETON_MODEL = collections.OrderedDict() +RAW_SKELETON_MODEL["joints"] = RAW_SKELETON_JOINTS +RAW_SKELETON_MODEL["foot_joints"] = RAW_SKELETON_FOOT_JOINTS +RAW_SKELETON_MODEL["heel_offset"] = [0, -6.480602, 0] +RAW_SKELETON_MODEL["ik_chains"] = IK_CHAINS_RAW_SKELETON + + + +GAME_ENGINE_JOINTS = collections.OrderedDict() +GAME_ENGINE_JOINTS["root"] = "Root" +GAME_ENGINE_JOINTS["pelvis"] = "pelvis" +GAME_ENGINE_JOINTS["spine"] = "spine_01" +GAME_ENGINE_JOINTS["spine_1"] = "spine_02" +GAME_ENGINE_JOINTS["spine_2"] = "spine_03" +GAME_ENGINE_JOINTS["left_clavicle"] = "clavicle_l" +GAME_ENGINE_JOINTS["right_clavicle"] = "clavicle_r" +GAME_ENGINE_JOINTS["left_shoulder"] = "upperarm_l" +GAME_ENGINE_JOINTS["right_shoulder"] = "upperarm_r" +GAME_ENGINE_JOINTS["left_elbow"] = "lowerarm_l" +GAME_ENGINE_JOINTS["right_elbow"] = "lowerarm_r" +GAME_ENGINE_JOINTS["left_wrist"] = "hand_l" +GAME_ENGINE_JOINTS["right_wrist"] = "hand_r" +GAME_ENGINE_JOINTS["left_finger"] = "middle_03_l" +GAME_ENGINE_JOINTS["right_finger"] = "middle_03_r" +GAME_ENGINE_JOINTS["left_hip"] = "thigh_l" +GAME_ENGINE_JOINTS["right_hip"] = "thigh_r" +GAME_ENGINE_JOINTS["left_knee"] = "calf_l" +GAME_ENGINE_JOINTS["right_knee"] = "calf_r" +GAME_ENGINE_JOINTS["left_ankle"] = "foot_l" +GAME_ENGINE_JOINTS["right_ankle"] = "foot_r" +GAME_ENGINE_JOINTS["left_toe"] = "ball_l" +GAME_ENGINE_JOINTS["right_toe"] = "ball_r" +GAME_ENGINE_JOINTS["left_heel"] = "heel_l" +GAME_ENGINE_JOINTS["right_heel"] = "heel_r" +GAME_ENGINE_JOINTS["neck"] = "neck_01" +GAME_ENGINE_JOINTS["head"] = "head" + +GAME_ENGINE_SKELETON_MODEL = collections.OrderedDict() +GAME_ENGINE_SKELETON_MODEL["joints"] = GAME_ENGINE_JOINTS +GAME_ENGINE_SKELETON_MODEL["foot_joints"] = ["foot_l", "foot_r", "ball_r", "ball_l", "heel_r", "heel_l"] +GAME_ENGINE_SKELETON_MODEL["heel_offset"] = (np.array([0, 2.45, 3.480602]) * 2.5).tolist() +GAME_ENGINE_SKELETON_MODEL["ik_chains"] = IK_CHAINS_GAME_ENGINE_SKELETON + +GAME_ENGINE_SKELETON_COS_MAP = collections.defaultdict(dict) +GAME_ENGINE_SKELETON_COS_MAP["Game_engine"]["y"] = [0, 1, 0] +GAME_ENGINE_SKELETON_COS_MAP["Game_engine"]["x"] = [1, 0, 0] +GAME_ENGINE_SKELETON_COS_MAP["head"]["y"] = [0, 1, 0] +GAME_ENGINE_SKELETON_COS_MAP["head"]["x"] = [1, 0, 0] +GAME_ENGINE_SKELETON_MODEL["cos_map"] = GAME_ENGINE_SKELETON_COS_MAP + + + +GAME_ENGINE_WRONG_ROOT_JOINTS = collections.OrderedDict() +GAME_ENGINE_WRONG_ROOT_JOINTS["root"] = "Game_engine" +GAME_ENGINE_WRONG_ROOT_JOINTS["pelvis"] = "pelvis" +GAME_ENGINE_WRONG_ROOT_JOINTS["spine"] = "spine_01" +GAME_ENGINE_WRONG_ROOT_JOINTS["spine_1"] = "spine_02" +GAME_ENGINE_WRONG_ROOT_JOINTS["spine_2"] = "spine_03" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_clavicle"] = "clavicle_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_clavicle"] = "clavicle_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_shoulder"] = "upperarm_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_shoulder"] = "upperarm_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_elbow"] = "lowerarm_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_elbow"] = "lowerarm_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_wrist"] = "hand_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_wrist"] = "hand_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_finger"] = "middle_03_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_finger"] = "middle_03_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_hip"] = "thigh_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_hip"] = "thigh_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_knee"] = "calf_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_knee"] = "calf_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_ankle"] = "foot_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_ankle"] = "foot_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_toe"] = "ball_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_toe"] = "ball_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["left_heel"] = "heel_l" +GAME_ENGINE_WRONG_ROOT_JOINTS["right_heel"] = "heel_r" +GAME_ENGINE_WRONG_ROOT_JOINTS["neck"] = "neck_01" +GAME_ENGINE_WRONG_ROOT_JOINTS["head"] = "head" + +GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL = collections.OrderedDict() +GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL["name"] = "game_engine_wrong_root" +GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL["joints"] = GAME_ENGINE_WRONG_ROOT_JOINTS +GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL["foot_joints"] = ["foot_l", "foot_r", "ball_r", "ball_l", "heel_r", "heel_l"] +GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL["heel_offset"] = (np.array([0, 2.45, 3.480602]) * 2.5).tolist() +GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL["ik_chains"] = IK_CHAINS_GAME_ENGINE_SKELETON + +GAME_ENGINE_WRONG_ROOT_SKELETON_COS_MAP = collections.defaultdict(dict) +GAME_ENGINE_WRONG_ROOT_SKELETON_COS_MAP["Game_engine"]["y"] = [0, 1, 0] +GAME_ENGINE_WRONG_ROOT_SKELETON_COS_MAP["Game_engine"]["x"] = [0, 0, -1] +GAME_ENGINE_WRONG_ROOT_SKELETON_COS_MAP["head"]["y"] = [0, 1, 0] +GAME_ENGINE_WRONG_ROOT_SKELETON_COS_MAP["head"]["x"] = [1, 0, 0] +GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL["cos_map"] = GAME_ENGINE_WRONG_ROOT_SKELETON_COS_MAP + + +ROCKETBOX_JOINTS = collections.OrderedDict() +ROCKETBOX_JOINTS["root"] = "Hips" +ROCKETBOX_JOINTS["pelvis"] = "Hips" +ROCKETBOX_JOINTS["spine"] = "Spine" +ROCKETBOX_JOINTS["spine_1"] = "Spine_1" +ROCKETBOX_JOINTS["left_clavicle"] = "LeftShoulder" +ROCKETBOX_JOINTS["right_clavicle"] = "RightShoulder" +ROCKETBOX_JOINTS["left_shoulder"] = "LeftArm" +ROCKETBOX_JOINTS["right_shoulder"] = "RightArm" +ROCKETBOX_JOINTS["left_elbow"] = "LeftForeArm" +ROCKETBOX_JOINTS["right_elbow"] = "RightForeArm" +ROCKETBOX_JOINTS["left_wrist"] = "LeftHand" +ROCKETBOX_JOINTS["right_wrist"] = "RightHand" +ROCKETBOX_JOINTS["left_finger"] = "Bip01_L_Finger2" +ROCKETBOX_JOINTS["right_finger"] = "Bip01_R_Finger2" +ROCKETBOX_JOINTS["left_hip"] = "LeftUpLeg" +ROCKETBOX_JOINTS["right_hip"] = "RightUpLeg" +ROCKETBOX_JOINTS["left_knee"] = "LeftLeg" +ROCKETBOX_JOINTS["right_knee"] = "RightLeg" +ROCKETBOX_JOINTS["left_ankle"] = "LeftFoot" +ROCKETBOX_JOINTS["right_ankle"] = "RightFoot" +ROCKETBOX_JOINTS["left_toe"] = "Bip01_L_Toe0" +ROCKETBOX_JOINTS["right_toe"] = "Bip01_R_Toe0" +ROCKETBOX_JOINTS["left_heel"] = "LeftHeel" +ROCKETBOX_JOINTS["right_heel"] = "RightHeel" +ROCKETBOX_JOINTS["neck"] = "Neck" +ROCKETBOX_JOINTS["head"] = "Head" + +ROCKETBOX_SKELETON_MODEL = collections.OrderedDict() +ROCKETBOX_SKELETON_MODEL["name"] = "rocketbox" +ROCKETBOX_SKELETON_MODEL["joints"] = ROCKETBOX_JOINTS +ROCKETBOX_SKELETON_MODEL["heel_offset"] = [0, -6.480602, 0] +ROCKETBOX_SKELETON_MODEL["foot_joints"] = RAW_SKELETON_FOOT_JOINTS +ROCKETBOX_SKELETON_MODEL["ik_chains"] = IK_CHAINS_RAW_SKELETON +ROCKETBOX_SKELETON_COS_MAP = collections.defaultdict(dict) +ROCKETBOX_SKELETON_COS_MAP["LeftHand"]["y"] = [0.99969438, -0.01520068, -0.01949593] +ROCKETBOX_SKELETON_COS_MAP["LeftHand"]["x"] = [-1.52006822e-02, -9.99884452e-01, 1.48198341e-04] +ROCKETBOX_SKELETON_COS_MAP["RightHand"]["y"] = [0.99969439, 0.01520077, -0.01949518] +ROCKETBOX_SKELETON_COS_MAP["RightHand"]["x"] = [ 1.52007742e-02, -9.99884451e-01, -1.48193551e-04] + +ROCKETBOX_SKELETON_COS_MAP["LeftHand"]["y"] = [1, 0, 0] +ROCKETBOX_SKELETON_COS_MAP["RightHand"]["y"] = [1, 0, 0] +ROCKETBOX_SKELETON_COS_MAP["LeftHand"]["x"] = [0, 0, 1] +ROCKETBOX_SKELETON_COS_MAP["RightHand"]["x"] = [0, 0, -1] + + +ROCKETBOX_SKELETON_MODEL["cos_map"] = ROCKETBOX_SKELETON_COS_MAP + + +CMU_SKELETON_JOINTS = collections.OrderedDict() +CMU_SKELETON_JOINTS["root"] = "hip" +CMU_SKELETON_JOINTS["pelvis"] = "hip" +CMU_SKELETON_JOINTS["spine"] = "abdomen" +CMU_SKELETON_JOINTS["spine_1"] = "chest" +CMU_SKELETON_JOINTS["left_clavicle"] = "lCollar" +CMU_SKELETON_JOINTS["right_clavicle"] = "rCollar" +CMU_SKELETON_JOINTS["left_shoulder"] = "lShldr" +CMU_SKELETON_JOINTS["right_shoulder"] = "rShldr" +CMU_SKELETON_JOINTS["left_elbow"] = "lForeArm" +CMU_SKELETON_JOINTS["right_elbow"] = "rForeArm" +CMU_SKELETON_JOINTS["left_wrist"] = "lHand" +CMU_SKELETON_JOINTS["right_wrist"] = "rHand" +CMU_SKELETON_JOINTS["left_hip"] = "lThigh" +CMU_SKELETON_JOINTS["right_hip"] = "rThigh" +CMU_SKELETON_JOINTS["left_knee"] = "lShin" +CMU_SKELETON_JOINTS["right_knee"] = "rShin" +CMU_SKELETON_JOINTS["left_ankle"] = "lFoot" +CMU_SKELETON_JOINTS["right_ankle"] = "rFoot" +CMU_SKELETON_JOINTS["left_toe"] = "lFoot_EndSite" +CMU_SKELETON_JOINTS["right_toe"] = "rFoot_EndSite" +CMU_SKELETON_JOINTS["left_heel"] = None +CMU_SKELETON_JOINTS["right_heel"] = None +CMU_SKELETON_JOINTS["neck"] = "neck" +CMU_SKELETON_JOINTS["head"] = "head" +CMU_SKELETON_MODEL = collections.OrderedDict() +CMU_SKELETON_MODEL["cmu"] = "cmu" +CMU_SKELETON_MODEL["joints"] = CMU_SKELETON_JOINTS +CMU_SKELETON_MODEL["foot_joints"] = [] +CMU_SKELETON_MODEL["foot_correction"] = {"x":-22, "y":3} + +MOVIEMATION_SKELETON_JOINTS = collections.OrderedDict() +MOVIEMATION_SKELETON_JOINTS["root"] = "Hips" +MOVIEMATION_SKELETON_JOINTS["pelvis"] = "Hips" +MOVIEMATION_SKELETON_JOINTS["spine"] = "Ab" +MOVIEMATION_SKELETON_JOINTS["spine_1"] = "Chest" +MOVIEMATION_SKELETON_JOINTS["left_clavicle"] = "LeftCollar" +MOVIEMATION_SKELETON_JOINTS["right_clavicle"] = "RightCollar" +MOVIEMATION_SKELETON_JOINTS["left_shoulder"] = "LeftShoulder" +MOVIEMATION_SKELETON_JOINTS["right_shoulder"] = "RightShoulder" +MOVIEMATION_SKELETON_JOINTS["left_elbow"] = "LeftElbow" +MOVIEMATION_SKELETON_JOINTS["right_elbow"] = "RightElbow" +MOVIEMATION_SKELETON_JOINTS["left_wrist"] = "LeftWrist" +MOVIEMATION_SKELETON_JOINTS["right_wrist"] = "RightWrist" +MOVIEMATION_SKELETON_JOINTS["left_hip"] = "LeftHip" +MOVIEMATION_SKELETON_JOINTS["right_hip"] = "RightHip" +MOVIEMATION_SKELETON_JOINTS["left_knee"] = "LeftKnee" +MOVIEMATION_SKELETON_JOINTS["right_knee"] = "RightKnee" +MOVIEMATION_SKELETON_JOINTS["left_ankle"] = "LeftAnkle" +MOVIEMATION_SKELETON_JOINTS["right_ankle"] = "RightAnkle" +MOVIEMATION_SKELETON_JOINTS["left_toe"] = "LeftAnkle_EndSite" +MOVIEMATION_SKELETON_JOINTS["right_toe"] = "RightAnkle_EndSite" +MOVIEMATION_SKELETON_JOINTS["left_heel"] = None +MOVIEMATION_SKELETON_JOINTS["right_heel"] = None +MOVIEMATION_SKELETON_JOINTS["neck"] = "Neck" +MOVIEMATION_SKELETON_JOINTS["head"] = "Head" +MOVIEMATION_SKELETON_MODEL = collections.OrderedDict() +MOVIEMATION_SKELETON_MODEL["name"] = "moviemation" +MOVIEMATION_SKELETON_MODEL["joints"] = MOVIEMATION_SKELETON_JOINTS +MOVIEMATION_SKELETON_MODEL["foot_joints"] = [] + + +MCS_SKELETON_JOINTS = collections.OrderedDict() +MCS_SKELETON_JOINTS["root"] = "Hips" +MCS_SKELETON_JOINTS["pelvis"] = "Hips" +MCS_SKELETON_JOINTS["spine"] = None +MCS_SKELETON_JOINTS["spine_1"] = "Chest" +MCS_SKELETON_JOINTS["left_clavicle"] = "LeftCollar" +MCS_SKELETON_JOINTS["right_clavicle"] = "RightCollar" +MCS_SKELETON_JOINTS["left_shoulder"] = "LeftShoulder" +MCS_SKELETON_JOINTS["right_shoulder"] = "RightShoulder" +MCS_SKELETON_JOINTS["left_elbow"] = "LeftElbow" +MCS_SKELETON_JOINTS["right_elbow"] = "RightElbow" +MCS_SKELETON_JOINTS["left_wrist"] = "LeftWrist" +MCS_SKELETON_JOINTS["right_wrist"] = "RightWrist" +MCS_SKELETON_JOINTS["left_hip"] = "LeftHip" +MCS_SKELETON_JOINTS["right_hip"] = "RightHip" +MCS_SKELETON_JOINTS["left_knee"] = "LeftKnee" +MCS_SKELETON_JOINTS["right_knee"] = "RightKnee" +MCS_SKELETON_JOINTS["left_ankle"] = "LeftAnkle" +MCS_SKELETON_JOINTS["right_ankle"] = "RightAnkle" +MCS_SKELETON_JOINTS["left_toe"] = "LeftAnkle_EndSite" +MCS_SKELETON_JOINTS["right_toe"] = "RightAnkle_EndSite" +MCS_SKELETON_JOINTS["left_heel"] = None +MCS_SKELETON_JOINTS["right_heel"] = None +MCS_SKELETON_JOINTS["neck"] = "Neck" +MCS_SKELETON_JOINTS["head"] = "Head" +MCS_SKELETON_MODEL = collections.OrderedDict() +MCS_SKELETON_MODEL["name"] = "mcs" +MCS_SKELETON_MODEL["joints"] = MCS_SKELETON_JOINTS +MCS_SKELETON_MODEL["foot_joints"] = [] + + + +MH_CMU_SKELETON_JOINTS = collections.OrderedDict() +MH_CMU_SKELETON_JOINTS["root"] = None#"CMU compliant skeleton" +MH_CMU_SKELETON_JOINTS["pelvis"] = "Hips" +MH_CMU_SKELETON_JOINTS["spine"] = "LowerBack" +MH_CMU_SKELETON_JOINTS["spine_1"] = "Spine" +MH_CMU_SKELETON_JOINTS["spine_2"] = "Spine1" +MH_CMU_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +MH_CMU_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +MH_CMU_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +MH_CMU_SKELETON_JOINTS["right_shoulder"] = "RightArm" +MH_CMU_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +MH_CMU_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +MH_CMU_SKELETON_JOINTS["left_wrist"] = "LeftHand" +MH_CMU_SKELETON_JOINTS["right_wrist"] = "RightHand" +MH_CMU_SKELETON_JOINTS["left_finger"] = "LeftHandFinger1" +MH_CMU_SKELETON_JOINTS["right_finger"] = "RightHandFinger1" +MH_CMU_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +MH_CMU_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +MH_CMU_SKELETON_JOINTS["left_knee"] = "LeftLeg" +MH_CMU_SKELETON_JOINTS["right_knee"] = "RightLeg" +MH_CMU_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +MH_CMU_SKELETON_JOINTS["right_ankle"] = "RightFoot" +MH_CMU_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +MH_CMU_SKELETON_JOINTS["right_toe"] = "RightToeBase" +MH_CMU_SKELETON_JOINTS["left_heel"] = None +MH_CMU_SKELETON_JOINTS["right_heel"] = None +MH_CMU_SKELETON_JOINTS["neck"] = "Neck" +MH_CMU_SKELETON_JOINTS["head"] = "Head" + +MH_CMU_SKELETON_MODEL = collections.OrderedDict() +MH_CMU_SKELETON_MODEL["name"] = "mh_cmu" +MH_CMU_SKELETON_MODEL["joints"] = MH_CMU_SKELETON_JOINTS +MH_CMU_SKELETON_MODEL["foot_joints"] = [] +MH_CMU_SKELETON_MODEL["foot_correction"] = {"x":-22, "y":3} +MH_CMU_SKELETON_MODEL["flip_x_axis"] = False + +MH_CMU_2_SKELETON_JOINTS = collections.OrderedDict() +MH_CMU_2_SKELETON_JOINTS["root"] = None#"CMU compliant skeleton" +MH_CMU_2_SKELETON_JOINTS["pelvis"] = "Hips" +MH_CMU_2_SKELETON_JOINTS["spine"] = "LowerBack" +MH_CMU_2_SKELETON_JOINTS["spine_1"] = "Spine" +MH_CMU_2_SKELETON_JOINTS["spine_2"] = "Spine1" +MH_CMU_2_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +MH_CMU_2_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +MH_CMU_2_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +MH_CMU_2_SKELETON_JOINTS["right_shoulder"] = "RightArm" +MH_CMU_2_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +MH_CMU_2_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +MH_CMU_2_SKELETON_JOINTS["left_wrist"] = "LeftHand" +MH_CMU_2_SKELETON_JOINTS["right_wrist"] = "RightHand" +MH_CMU_2_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +MH_CMU_2_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +MH_CMU_2_SKELETON_JOINTS["left_knee"] = "LeftLeg" +MH_CMU_2_SKELETON_JOINTS["right_knee"] = "RightLeg" +MH_CMU_2_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +MH_CMU_2_SKELETON_JOINTS["right_ankle"] = "RightFoot" +MH_CMU_2_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +MH_CMU_2_SKELETON_JOINTS["right_toe"] = "RightToeBase" +MH_CMU_2_SKELETON_JOINTS["left_heel"] = None +MH_CMU_2_SKELETON_JOINTS["right_heel"] = None +MH_CMU_2_SKELETON_JOINTS["neck"] = "Neck" +MH_CMU_2_SKELETON_JOINTS["head"] = "Head" + +MH_CMU_2_SKELETON_MODEL = collections.OrderedDict() +MH_CMU_2_SKELETON_MODEL["name"] = "mh_cmu2" +MH_CMU_2_SKELETON_MODEL["joints"] = MH_CMU_2_SKELETON_JOINTS +MH_CMU_2_SKELETON_MODEL["foot_joints"] = [] +MH_CMU_2_SKELETON_MODEL["foot_correction"] = {"x":-22, "y":3} +MH_CMU_2_SKELETON_MODEL["flip_x_axis"] = False +#MH_CMU_2_SKELETON_MODEL["foot_joints"] = [] +#MH_CMU_2_SKELETON_MODEL["x_cos_fixes"] = ["Neck", "Pelvis", "Spine", "Spine1", "LowerBack", "LeftArm"] +#MH_CMU_2_SKELETON_COS_MAP = collections.defaultdict(dict) +#MH_CMU_2_SKELETON_COS_MAP["Hips"]["y"] = [0, 1,0] +#MH_CMU_2_SKELETON_COS_MAP["Hips"]["x"] = [1, 0, 0] +#MH_CMU_2_SKELETON_MODEL["cos_map"] = MH_CMU_2_SKELETON_COS_MAP + +ICLONE_SKELETON_JOINTS = collections.OrderedDict() +ICLONE_SKELETON_JOINTS["root"] = "CC_Base_BoneRoot" +ICLONE_SKELETON_JOINTS["pelvis"] = "CC_Base_Hip"#CC_Base_Pelvis +ICLONE_SKELETON_JOINTS["spine"] = "CC_Base_Waist" +ICLONE_SKELETON_JOINTS["spine_1"] = "CC_Base_Spine01" +ICLONE_SKELETON_JOINTS["spine_2"] = "CC_Base_Spine02" +ICLONE_SKELETON_JOINTS["left_clavicle"] = "CC_Base_L_Clavicle" +ICLONE_SKELETON_JOINTS["right_clavicle"] = "CC_Base_R_Clavicle" +ICLONE_SKELETON_JOINTS["left_shoulder"] = "CC_Base_L_Upperarm" +ICLONE_SKELETON_JOINTS["right_shoulder"] = "CC_Base_R_Upperarm" +ICLONE_SKELETON_JOINTS["left_elbow"] = "CC_Base_L_Forearm" +ICLONE_SKELETON_JOINTS["right_elbow"] = "CC_Base_R_Forearm" +ICLONE_SKELETON_JOINTS["left_wrist"] = "CC_Base_L_Hand" +ICLONE_SKELETON_JOINTS["right_wrist"] = "CC_Base_R_Hand" +ICLONE_SKELETON_JOINTS["left_hip"] = "CC_Base_L_Thigh" +ICLONE_SKELETON_JOINTS["right_hip"] = "CC_Base_R_Thigh" +ICLONE_SKELETON_JOINTS["left_knee"] = "CC_Base_L_Calf" +ICLONE_SKELETON_JOINTS["right_knee"] = "CC_Base_R_Calf" +ICLONE_SKELETON_JOINTS["left_ankle"] = "CC_Base_L_Foot" +ICLONE_SKELETON_JOINTS["right_ankle"] = "CC_Base_R_Foot" +ICLONE_SKELETON_JOINTS["left_toe"] = "CC_Base_L_ToeBase" +ICLONE_SKELETON_JOINTS["right_toe"] = "CC_Base_R_ToeBase" +ICLONE_SKELETON_JOINTS["left_heel"] = None +ICLONE_SKELETON_JOINTS["right_heel"] = None +ICLONE_SKELETON_JOINTS["neck"] = "CC_Base_NeckTwist01" +ICLONE_SKELETON_JOINTS["head"] = "CC_Base_Head" + +ICLONE_SKELETON_MODEL = collections.OrderedDict() +ICLONE_SKELETON_MODEL["name"] = "iclone" +ICLONE_SKELETON_MODEL["joints"] = ICLONE_SKELETON_JOINTS +ICLONE_SKELETON_MODEL["foot_joints"] = [] +ICLONE_SKELETON_MODEL["x_cos_fixes"] = ["CC_Base_L_Thigh", "CC_Base_R_Thigh", "CC_Base_L_Calf", "CC_Base_R_Calf"] +ICLONE_SKELETON_COS_MAP = collections.defaultdict(dict) +ICLONE_SKELETON_COS_MAP["CC_Base_L_Foot"]["y"] = [0, 0.4, 0.6] +ICLONE_SKELETON_COS_MAP["CC_Base_L_Foot"]["x"] = [-1, 0, 0] +ICLONE_SKELETON_COS_MAP["CC_Base_R_Foot"]["y"] = [0, 0.4, 0.6] +ICLONE_SKELETON_COS_MAP["CC_Base_R_Foot"]["x"] = [-1, 0, 0] +ICLONE_SKELETON_MODEL["cos_map"] = ICLONE_SKELETON_COS_MAP +ICLONE_SKELETON_MODEL["foot_correction"] = {"x":-40, "y":3} +ICLONE_SKELETON_MODEL["flip_x_axis"] = True + + +CUSTOM_SKELETON_JOINTS = collections.OrderedDict() + +CUSTOM_SKELETON_JOINTS["root"] = None +CUSTOM_SKELETON_JOINTS = dict() +CUSTOM_SKELETON_JOINTS["pelvis"] = "FK_back1_jnt" +CUSTOM_SKELETON_JOINTS["spine"] = "FK_back2_jnt" +CUSTOM_SKELETON_JOINTS["spine_1"] = "FK_back3_jnt" +CUSTOM_SKELETON_JOINTS["left_clavicle"] = "L_shoulder_jnt" +CUSTOM_SKELETON_JOINTS["right_clavicle"] = "R_shoulder_jnt" +CUSTOM_SKELETON_JOINTS["left_shoulder"] = "L_upArm_jnt" +CUSTOM_SKELETON_JOINTS["right_shoulder"] = "R_upArm_jnt" +CUSTOM_SKELETON_JOINTS["left_elbow"] = "L_lowArm_jnt" +CUSTOM_SKELETON_JOINTS["right_elbow"] = "R_lowArm_jnt" +CUSTOM_SKELETON_JOINTS["left_wrist"] = "L_hand_jnt" +CUSTOM_SKELETON_JOINTS["right_wrist"] = "R_hand_jnt" +CUSTOM_SKELETON_JOINTS["left_hip"] = "L_upLeg_jnt" +CUSTOM_SKELETON_JOINTS["right_hip"] = "R_upLeg_jnt" +CUSTOM_SKELETON_JOINTS["left_knee"] = "L_lowLeg_jnt" +CUSTOM_SKELETON_JOINTS["right_knee"] = "R_lowLeg_jnt" +CUSTOM_SKELETON_JOINTS["left_ankle"] = "L_foot_jnt" +CUSTOM_SKELETON_JOINTS["right_ankle"] = "R_foot_jnt" +CUSTOM_SKELETON_JOINTS["left_toe"] = "L_toe_jnt" +CUSTOM_SKELETON_JOINTS["right_toe"] = "R_toe_jnt" +CUSTOM_SKELETON_JOINTS["neck"] = "FK_back4_jnt" +CUSTOM_SKELETON_JOINTS["head"] = "head_jnt" + +CUSTOM_SKELETON_JOINTS["left_hold_point"] = "L_hand_jnt_hold_point" +CUSTOM_SKELETON_JOINTS["right_hold_point"] = "R_hand_jnt_hold_point" + +CUSTOM_SKELETON_JOINTS["right_thumb_base"] = "R_thumb_base_jnt" +CUSTOM_SKELETON_JOINTS["right_thumb_mid"] = "R_thumb_mid_jnt" +CUSTOM_SKELETON_JOINTS["right_thumb_tip"] = "R_thumb_tip_jnt" +CUSTOM_SKELETON_JOINTS["right_thumb_end"] = "R_thumb_end_jnt" + +CUSTOM_SKELETON_JOINTS["right_index_finger_root"] = "R_index_root_jnt" +CUSTOM_SKELETON_JOINTS["right_index_finger_base"] = "R_index_base_jnt" +CUSTOM_SKELETON_JOINTS["right_index_finger_mid"] = "R_index_mid_jnt" +CUSTOM_SKELETON_JOINTS["right_index_finger_tip"] = "R_index_tip_jnt" +CUSTOM_SKELETON_JOINTS["right_index_finger_end"] = "R_index_end_jnt" + +CUSTOM_SKELETON_JOINTS["right_middle_finger_root"] = "R_middle_root_jnt" +CUSTOM_SKELETON_JOINTS["right_middle_finger_base"] = "R_middle_base_jnt" +CUSTOM_SKELETON_JOINTS["right_middle_finger_mid"] = "R_middle_mid_jnt" +CUSTOM_SKELETON_JOINTS["right_middle_finger_tip"] = "R_middle_tip_jnt" +CUSTOM_SKELETON_JOINTS["right_middle_finger_end"] = "R_middle_end_jnt" + +CUSTOM_SKELETON_JOINTS["right_ring_finger_root"] = "R_ring_base_jnt" +CUSTOM_SKELETON_JOINTS["right_ring_finger_base"] = "R_ring_root_jnt" +CUSTOM_SKELETON_JOINTS["right_ring_finger_mid"] = "R_ring_mid_jnt" +CUSTOM_SKELETON_JOINTS["right_ring_finger_tip"] = "R_ring_tip_jnt" +CUSTOM_SKELETON_JOINTS["right_ring_finger_end"] = "R_ring_end_jnt" + +CUSTOM_SKELETON_JOINTS["right_pinky_finger_root"] = "R_pinky_root_jnt" +CUSTOM_SKELETON_JOINTS["right_pinky_finger_base"] = "R_pinky_base_jnt" +CUSTOM_SKELETON_JOINTS["right_pinky_finger_mid"] = "R_pinky_mid_jnt" +CUSTOM_SKELETON_JOINTS["right_pinky_finger_tip"] = "R_pinky_tip_jnt" +CUSTOM_SKELETON_JOINTS["right_pinky_finger_end"] = "R_pinky_end_jnt" + +CUSTOM_SKELETON_JOINTS["left_thumb_base"] = "L_thumb_base_jnt" +CUSTOM_SKELETON_JOINTS["left_thumb_mid"] = "L_thumb_mid_jnt" +CUSTOM_SKELETON_JOINTS["left_thumb_tip"] = "L_thumb_tip_jnt" +CUSTOM_SKELETON_JOINTS["left_thumb_end"] = "L_thumb_end_jnt" + +CUSTOM_SKELETON_JOINTS["left_index_finger_root"] = "L_index_root_jnt" +CUSTOM_SKELETON_JOINTS["left_index_finger_base"] = "L_index_base_jnt" +CUSTOM_SKELETON_JOINTS["left_index_finger_mid"] = "L_index_mid_jnt" +CUSTOM_SKELETON_JOINTS["left_index_finger_tip"] = "L_index_tip_jnt" +CUSTOM_SKELETON_JOINTS["left_index_finger_end"] = "L_index_end_jnt" + +CUSTOM_SKELETON_JOINTS["left_middle_finger_root"] = "L_middle_root_jnt" +CUSTOM_SKELETON_JOINTS["left_middle_finger_base"] = "L_middle_base_jnt" +CUSTOM_SKELETON_JOINTS["left_middle_finger_mid"] = "L_middle_mid_jnt" +CUSTOM_SKELETON_JOINTS["left_middle_finger_tip"] = "L_middle_tip_jnt" +CUSTOM_SKELETON_JOINTS["left_middle_finger_end"] = "L_middle_end_jnt" + +CUSTOM_SKELETON_JOINTS["left_ring_finger_root"] = "L_ring_base_jnt" +CUSTOM_SKELETON_JOINTS["left_ring_finger_base"] = "L_ring_root_jnt" +CUSTOM_SKELETON_JOINTS["left_ring_finger_mid"] = "L_ring_mid_jnt" +CUSTOM_SKELETON_JOINTS["left_ring_finger_tip"] = "L_ring_tip_jnt" +CUSTOM_SKELETON_JOINTS["left_ring_finger_end"] = "L_ring_end_jnt" + +CUSTOM_SKELETON_JOINTS["left_pinky_finger_root"] = "L_pinky_root_jnt" +CUSTOM_SKELETON_JOINTS["left_pinky_finger_base"] = "L_pinky_base_jnt" +CUSTOM_SKELETON_JOINTS["left_pinky_finger_mid"] = "L_pinky_mid_jnt" +CUSTOM_SKELETON_JOINTS["left_pinky_finger_tip"] = "L_pinky_tip_jnt" +CUSTOM_SKELETON_JOINTS["left_pinky_finger_end"] = "L_pinky_end_jnt" + + + + +CUSTOM_SKELETON_MODEL = collections.OrderedDict() +CUSTOM_SKELETON_MODEL["name"] = "custom" +CUSTOM_SKELETON_MODEL["joints"] = CUSTOM_SKELETON_JOINTS +CUSTOM_SKELETON_MODEL["foot_joints"] = [] +CUSTOM_SKELETON_COS_MAP = collections.defaultdict(dict) +CUSTOM_SKELETON_COS_MAP["R_foot_jnt"]["y"] = [1, 0, 0] +#CUSTOM_SKELETON_COS_MAP["R_foot_jnt"]["y"] = [0.4, 0.6, 0] +CUSTOM_SKELETON_COS_MAP["R_foot_jnt"]["x"] = [0, 0, 1] +CUSTOM_SKELETON_COS_MAP["L_foot_jnt"]["y"] = [1, 0, 0] +#CUSTOM_SKELETON_COS_MAP["L_foot_jnt"]["y"] = [0.4, 0.6, 0] +CUSTOM_SKELETON_COS_MAP["L_foot_jnt"]["x"] = [0, 0, 1] + +CUSTOM_SKELETON_COS_MAP["L_upLeg_jnt"]["y"] = [1, 0, 0] +CUSTOM_SKELETON_COS_MAP["L_upLeg_jnt"]["x"] = [0, 0, 1] +CUSTOM_SKELETON_COS_MAP["L_lowLeg_jnt"]["y"] = [1, 0, 0] +CUSTOM_SKELETON_COS_MAP["L_lowLeg_jnt"]["x"] = [0, 0, 1] + + +CUSTOM_SKELETON_COS_MAP["R_upLeg_jnt"]["y"] = [1, 0, 0] +CUSTOM_SKELETON_COS_MAP["R_upLeg_jnt"]["x"] = [0, 0, 1] +CUSTOM_SKELETON_COS_MAP["R_lowLeg_jnt"]["y"] = [1, 0, 0] +CUSTOM_SKELETON_COS_MAP["R_lowLeg_jnt"]["x"] = [0, 0, 1] + +CUSTOM_SKELETON_COS_MAP["FK_back2_jnt"]["y"] = [0, 1, 0] +CUSTOM_SKELETON_COS_MAP["FK_back2_jnt"]["x"] = [1, 0, 0] + +CUSTOM_SKELETON_COS_MAP["R_shoulder_jnt"]["y"] = [0, 0, -1] +CUSTOM_SKELETON_COS_MAP["R_shoulder_jnt"]["x"] = [0, -1, 0] +CUSTOM_SKELETON_COS_MAP["R_upArm_jnt"]["y"] = [0, 0, -1] +CUSTOM_SKELETON_COS_MAP["R_upArm_jnt"]["x"] = [0, -1, 0] +CUSTOM_SKELETON_COS_MAP["R_lowArm_jnt"]["y"] = [0, 0, -1] +CUSTOM_SKELETON_COS_MAP["R_lowArm_jnt"]["x"] = [0, -1, 0] + +CUSTOM_SKELETON_COS_MAP["L_shoulder_jnt"]["y"] = [0, 0, 1] +CUSTOM_SKELETON_COS_MAP["L_shoulder_jnt"]["x"] = [0, -1, 0] +CUSTOM_SKELETON_COS_MAP["L_upArm_jnt"]["y"] = [0, 0, 1] +CUSTOM_SKELETON_COS_MAP["L_upArm_jnt"]["x"] = [0, -1, 0] +CUSTOM_SKELETON_COS_MAP["L_lowArm_jnt"]["y"] = [0, 0, 1] +CUSTOM_SKELETON_COS_MAP["L_lowArm_jnt"]["x"] = [0, -1, 0] + +CUSTOM_SKELETON_COS_MAP["R_hand_jnt"]["y"] = [0, 0, -1] +CUSTOM_SKELETON_COS_MAP["R_hand_jnt"]["x"] = [0, -1, 0] + +CUSTOM_SKELETON_COS_MAP["L_hand_jnt"]["y"] = [0, 0, 1] +CUSTOM_SKELETON_COS_MAP["L_hand_jnt"]["x"] = [0, -1, 0] +if False: + CUSTOM_SKELETON_COS_MAP["FK_back3_jnt"]["y"] = [0, 0.7, -0.3] + CUSTOM_SKELETON_COS_MAP["FK_back3_jnt"]["x"] = [1, 0, 0] + CUSTOM_SKELETON_COS_MAP["head_jnt"]["y"] = [0.6, 0.4, 0] + CUSTOM_SKELETON_COS_MAP["head_jnt"]["x"] = [0, 0, -1] + +for k, j in CUSTOM_SKELETON_JOINTS.items(): + if j is None: + continue + if "finger" in k: + if "L_" in j: + CUSTOM_SKELETON_COS_MAP[j]["x"] = [0, 1, 0] + CUSTOM_SKELETON_COS_MAP[j]["y"] = [0, 0, 1] + elif "R_" in j: + CUSTOM_SKELETON_COS_MAP[j]["x"] = [0, -1, 0] + CUSTOM_SKELETON_COS_MAP[j]["y"] = [0, 0, -1] + elif "thumb" in k: + if "L_" in j: + CUSTOM_SKELETON_COS_MAP[j]["x"] = [1, 0, 0] + CUSTOM_SKELETON_COS_MAP[j]["y"] = [0, 0, 1] + elif "R_" in j: + CUSTOM_SKELETON_COS_MAP[j]["x"] = [-1, 0, 0] + CUSTOM_SKELETON_COS_MAP[j]["y"] = [0, 0, -1] + +CUSTOM_SKELETON_MODEL["cos_map"] = CUSTOM_SKELETON_COS_MAP +CUSTOM_SKELETON_MODEL["foot_joints"] = [] +CUSTOM_SKELETON_MODEL["heel_offset"] = [0, -6.480602, 0] +CUSTOM_SKELETON_MODEL["ik_chains"] = IK_CHAINS_CUSTOM_SKELETON +CUSTOM_SKELETON_MODEL["aligning_root_node"] = "FK_back1_jnt" +CUSTOM_SKELETON_MODEL["free_joints_map"] = {"R_hand_jnt_hold_point":["FK_back2_jnt", "R_upArm_jnt", "R_lowArm_jnt"], "L_hand_jnt_hold_point": ["FK_back2_jnt", "L_upArm_jnt", "L_lowArm_jnt"]} +CUSTOM_SKELETON_MODEL["relative_head_dir"] = [0.0, -1.0, 0.0] +CUSTOM_SKELETON_MODEL["flip_x_axis"] = True + + + +CAPTURY_SKELETON_JOINTS = collections.OrderedDict() +CAPTURY_SKELETON_JOINTS["root"] = "Hips" +CAPTURY_SKELETON_JOINTS["pelvis"] = "Hips" +CAPTURY_SKELETON_JOINTS["spine"] = "Spine" +CAPTURY_SKELETON_JOINTS["spine_1"] = "Spine1" +CAPTURY_SKELETON_JOINTS["spine_2"] = "Spine3" +CAPTURY_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +CAPTURY_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +CAPTURY_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +CAPTURY_SKELETON_JOINTS["right_shoulder"] = "RightArm" +CAPTURY_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +CAPTURY_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +CAPTURY_SKELETON_JOINTS["left_wrist"] = "LeftHand" +CAPTURY_SKELETON_JOINTS["right_wrist"] = "RightHand" +CAPTURY_SKELETON_JOINTS["left_finger"] = "LeftHand_EndSite" +CAPTURY_SKELETON_JOINTS["right_finger"] = "RightHand_EndSite" +CAPTURY_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +CAPTURY_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +CAPTURY_SKELETON_JOINTS["left_knee"] = "LeftLeg" +CAPTURY_SKELETON_JOINTS["right_knee"] = "RightLeg" +CAPTURY_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +CAPTURY_SKELETON_JOINTS["right_ankle"] = "RightFoot" +CAPTURY_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +CAPTURY_SKELETON_JOINTS["right_toe"] = "RightToeBase" +CAPTURY_SKELETON_JOINTS["left_heel"] = None +CAPTURY_SKELETON_JOINTS["right_heel"] = None +CAPTURY_SKELETON_JOINTS["neck"] = "Neck" +CAPTURY_SKELETON_JOINTS["head"] = "Head" +CAPTURY_SKELETON_COS_MAP = collections.defaultdict(dict) +CAPTURY_SKELETON_COS_MAP["LeftHand"]["y"] = [-1, 0, 0] +CAPTURY_SKELETON_COS_MAP["LeftHand"]["x"] = [0, -1, 0] +CAPTURY_SKELETON_COS_MAP["LeftArm"]["y"] = [-1, 0, 0] +CAPTURY_SKELETON_COS_MAP["LeftArm"]["x"] = [0, -1, 0] +CAPTURY_SKELETON_COS_MAP["LeftShoulder"]["y"] = [-1, 0, 0] +CAPTURY_SKELETON_COS_MAP["LeftShoulder"]["x"] = [0, -1, 0] + +CAPTURY_SKELETON_COS_MAP["LeftForeArm"]["y"] = [-1, 0, 0] +CAPTURY_SKELETON_COS_MAP["LeftForeArm"]["x"] = [0, -1, 0] +CAPTURY_SKELETON_COS_MAP["RightHand"]["y"] = [1, 0, 0] +CAPTURY_SKELETON_COS_MAP["RightHand"]["x"] = [0, 1, 0] +CAPTURY_SKELETON_COS_MAP["RightArm"]["y"] = [1, 0, 0] +CAPTURY_SKELETON_COS_MAP["RightArm"]["x"] = [0, 1, 0] +CAPTURY_SKELETON_COS_MAP["RightShoulder"]["y"] = [1, 0, 0] +CAPTURY_SKELETON_COS_MAP["RightShoulder"]["x"] = [0, 1, 0] + +CAPTURY_SKELETON_COS_MAP["RightForeArm"]["y"] = [1, 0, 0] +CAPTURY_SKELETON_COS_MAP["RightForeArm"]["x"] = [0, 1, 0] + + +CAPTURY_SKELETON_MODEL = collections.OrderedDict() +CAPTURY_SKELETON_MODEL["name"] = "captury" +CAPTURY_SKELETON_MODEL["joints"] = CAPTURY_SKELETON_JOINTS +CAPTURY_SKELETON_MODEL["foot_joints"] = [] +CAPTURY_SKELETON_MODEL["cos_map"] = CAPTURY_SKELETON_COS_MAP + + +HOLDEN_SKELETON_JOINTS = collections.OrderedDict() +HOLDEN_SKELETON_JOINTS["root"] = None +HOLDEN_SKELETON_JOINTS["pelvis"] = "Hips" +HOLDEN_SKELETON_JOINTS["spine"] = "Spine" +HOLDEN_SKELETON_JOINTS["spine_1"] = "Spine1" +HOLDEN_SKELETON_JOINTS["spine_2"] = None +HOLDEN_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +HOLDEN_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +HOLDEN_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +HOLDEN_SKELETON_JOINTS["right_shoulder"] = "RightArm" +HOLDEN_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +HOLDEN_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +HOLDEN_SKELETON_JOINTS["left_wrist"] = "LeftHand" +HOLDEN_SKELETON_JOINTS["right_wrist"] = "RightHand" +HOLDEN_SKELETON_JOINTS["left_finger"] = None +HOLDEN_SKELETON_JOINTS["right_finger"] = None +HOLDEN_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +HOLDEN_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +HOLDEN_SKELETON_JOINTS["left_knee"] = "LeftLeg" +HOLDEN_SKELETON_JOINTS["right_knee"] = "RightLeg" +HOLDEN_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +HOLDEN_SKELETON_JOINTS["right_ankle"] = "RightFoot" +HOLDEN_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +HOLDEN_SKELETON_JOINTS["right_toe"] = "RightToeBase" +HOLDEN_SKELETON_JOINTS["left_heel"] = None +HOLDEN_SKELETON_JOINTS["right_heel"] = None +HOLDEN_SKELETON_JOINTS["neck"] = "Neck1" +HOLDEN_SKELETON_JOINTS["head"] = "Head" +HOLDEN_SKELETON_COS_MAP = collections.defaultdict(dict) + +HOLDEN_SKELETON_MODEL = collections.OrderedDict() +HOLDEN_SKELETON_MODEL["name"] = "holden" +HOLDEN_SKELETON_MODEL["joints"] = HOLDEN_SKELETON_JOINTS +HOLDEN_SKELETON_MODEL["foot_joints"] = [] +HOLDEN_SKELETON_MODEL["cos_map"] = HOLDEN_SKELETON_COS_MAP + +MIXAMO_SKELETON_JOINTS = collections.OrderedDict() +MIXAMO_SKELETON_JOINTS["root"] = None +MIXAMO_SKELETON_JOINTS["pelvis"] = "mixamorig:Hips" +MIXAMO_SKELETON_JOINTS["spine"] = "mixamorig:Spine" +MIXAMO_SKELETON_JOINTS["spine_1"] = "mixamorig:Spine1" +MIXAMO_SKELETON_JOINTS["spine_2"] = "mixamorig:Spine2" +MIXAMO_SKELETON_JOINTS["left_clavicle"] = "mixamorig:LeftShoulder" +MIXAMO_SKELETON_JOINTS["right_clavicle"] = "mixamorig:RightShoulder" +MIXAMO_SKELETON_JOINTS["left_shoulder"] = "mixamorig:LeftArm" +MIXAMO_SKELETON_JOINTS["right_shoulder"] = "mixamorig:RightArm" +MIXAMO_SKELETON_JOINTS["left_elbow"] = "mixamorig:LeftForeArm" +MIXAMO_SKELETON_JOINTS["right_elbow"] = "mixamorig:RightForeArm" +MIXAMO_SKELETON_JOINTS["left_wrist"] = "mixamorig:LeftHand" +MIXAMO_SKELETON_JOINTS["right_wrist"] = "mixamorig:RightHand" +MIXAMO_SKELETON_JOINTS["left_finger"] = None +MIXAMO_SKELETON_JOINTS["right_finger"] = None +MIXAMO_SKELETON_JOINTS["left_hip"] = "mixamorig:LeftUpLeg" +MIXAMO_SKELETON_JOINTS["right_hip"] = "mixamorig:RightUpLeg" +MIXAMO_SKELETON_JOINTS["left_knee"] = "mixamorig:LeftLeg" +MIXAMO_SKELETON_JOINTS["right_knee"] = "mixamorig:RightLeg" +MIXAMO_SKELETON_JOINTS["left_ankle"] = "mixamorig:LeftFoot" +MIXAMO_SKELETON_JOINTS["right_ankle"] = "mixamorig:RightFoot" +MIXAMO_SKELETON_JOINTS["left_toe"] = "mixamorig:LeftToeBase" +MIXAMO_SKELETON_JOINTS["right_toe"] = "mixamorig:RightToeBase" +MIXAMO_SKELETON_JOINTS["left_heel"] = None +MIXAMO_SKELETON_JOINTS["right_heel"] = None +MIXAMO_SKELETON_JOINTS["neck"] = "mixamorig:Neck" +MIXAMO_SKELETON_JOINTS["head"] = "mixamorig:Head" +MIXAMO_SKELETON_COS_MAP = collections.defaultdict(dict) +MIXAMO_SKELETON_COS_MAP["mixamorig:LeftUpLeg"]["x"] = [-1, 0, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:LeftUpLeg"]["y"] = [0, 1, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:RightUpLeg"]["x"] = [-1, 0, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:RightUpLeg"]["y"] = [0, 1, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:LeftLeg"]["x"] = [-1, 0, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:LeftLeg"]["y"] = [0, 1, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:RightLeg"]["x"] = [-1, 0, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:RightLeg"]["y"] = [0, 1, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:LeftFoot"]["x"] = [-1, 0, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:LeftFoot"]["y"] = [0, 1, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:RightFoot"]["x"] = [-1, 0, 0] +MIXAMO_SKELETON_COS_MAP["mixamorig:RightFoot"]["y"] = [0, 1, 0] + +MIXAMO_SKELETON_MODEL = collections.OrderedDict() +MIXAMO_SKELETON_MODEL["name"] = "mixamo" +MIXAMO_SKELETON_MODEL["joints"] = MIXAMO_SKELETON_JOINTS +MIXAMO_SKELETON_MODEL["foot_joints"] = [] +MIXAMO_SKELETON_MODEL["cos_map"] = MIXAMO_SKELETON_COS_MAP +MIXAMO_SKELETON_MODEL["foot_correction"] = {"x":22, "y":3} +MIXAMO_SKELETON_MODEL["flip_x_axis"] = True +MIXAMO_SKELETON_MODEL["fixed_joint_corrections"] = dict() +MIXAMO_SKELETON_MODEL["fixed_joint_corrections"]["RightClavicle"] = -90 +MIXAMO_SKELETON_MODEL["fixed_joint_corrections"]["LeftClavicle"] = 90 + +MAX_SKELETON_JOINTS = collections.OrderedDict() +MAX_SKELETON_JOINTS["root"] = None +MAX_SKELETON_JOINTS["pelvis"] = "Hips" +MAX_SKELETON_JOINTS["spine"] = None +MAX_SKELETON_JOINTS["spine_1"] = None +MAX_SKELETON_JOINTS["spine_2"] = "Chest" +MAX_SKELETON_JOINTS["left_clavicle"] = "LeftCollar" +MAX_SKELETON_JOINTS["right_clavicle"] = "RightCollar" +MAX_SKELETON_JOINTS["left_shoulder"] = "LeftShoulder" +MAX_SKELETON_JOINTS["right_shoulder"] = "RightShoulder" +MAX_SKELETON_JOINTS["left_elbow"] = "LeftElbow" +MAX_SKELETON_JOINTS["right_elbow"] = "RightElbow" +MAX_SKELETON_JOINTS["left_wrist"] = "LeftWrist" +MAX_SKELETON_JOINTS["right_wrist"] = "RightWrist" +MAX_SKELETON_JOINTS["left_finger"] = "LeftWrist_EndSite" +MAX_SKELETON_JOINTS["right_finger"] = "RightWrist_EndSite" +MAX_SKELETON_JOINTS["left_hip"] = "LeftHip" +MAX_SKELETON_JOINTS["right_hip"] = "RightHip" +MAX_SKELETON_JOINTS["left_knee"] = "LeftKnee" +MAX_SKELETON_JOINTS["right_knee"] = "RightKnee" +MAX_SKELETON_JOINTS["left_ankle"] = "LeftAnkle" +MAX_SKELETON_JOINTS["right_ankle"] = "RightAnkle" +MAX_SKELETON_JOINTS["left_toe"] = None +MAX_SKELETON_JOINTS["right_toe"] = None +MAX_SKELETON_JOINTS["left_heel"] = None +MAX_SKELETON_JOINTS["right_heel"] = None +MAX_SKELETON_JOINTS["neck"] = "Neck" +MAX_SKELETON_JOINTS["head"] = "Head" +MAX_SKELETON_COS_MAP = collections.defaultdict(dict) + +MAX_SKELETON_COS_MAP["LeftShoulder"]["y"] = [0, -1, 0] +MAX_SKELETON_COS_MAP["RightShoulder"]["y"] = [0, -1, 0] +MAX_SKELETON_COS_MAP["LeftShoulder"]["x"] = [-1, 0, 0] +MAX_SKELETON_COS_MAP["RightShoulder"]["x"] = [-1, 0, 0] + +MAX_SKELETON_COS_MAP["LeftElbow"]["y"] = [0, -1, 0] +MAX_SKELETON_COS_MAP["RightElbow"]["y"] = [0, -1, 0] +MAX_SKELETON_COS_MAP["LeftElbow"]["x"] = [-1, 0, 0] +MAX_SKELETON_COS_MAP["RightElbow"]["x"] = [-1, 0, 0] + +MAX_SKELETON_COS_MAP["LeftWrist"]["y"] = [0, -1, 0] +MAX_SKELETON_COS_MAP["RightWrist"]["y"] = [0, -1, 0] +MAX_SKELETON_COS_MAP["LeftWrist"]["x"] = [-1, 0, 0] +MAX_SKELETON_COS_MAP["RightWrist"]["x"] = [-1, 0, 0] + + +MAX_SKELETON_MODEL = collections.OrderedDict() +MAX_SKELETON_MODEL["name"] = "max" +MAX_SKELETON_MODEL["joints"] = MAX_SKELETON_JOINTS +MAX_SKELETON_MODEL["foot_joints"] = [] +MAX_SKELETON_MODEL["cos_map"] = MAX_SKELETON_COS_MAP + +AACHEN_SKELETON_JOINTS = collections.OrderedDict() +AACHEN_SKELETON_JOINTS["root"] = None#"reference" +AACHEN_SKELETON_JOINTS["pelvis"] = "Hips" +AACHEN_SKELETON_JOINTS["spine"] = "LowerBack" +AACHEN_SKELETON_JOINTS["spine_1"] = "Spine" +AACHEN_SKELETON_JOINTS["spine_2"] = "Spine1" +AACHEN_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +AACHEN_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +AACHEN_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +AACHEN_SKELETON_JOINTS["right_shoulder"] = "RightArm" +AACHEN_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +AACHEN_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +AACHEN_SKELETON_JOINTS["left_wrist"] = "LeftHand" +AACHEN_SKELETON_JOINTS["right_wrist"] = "RightHand" +AACHEN_SKELETON_JOINTS["left_finger"] = "LeftFingerBase" +AACHEN_SKELETON_JOINTS["right_finger"] = "RightFingerBase" +AACHEN_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +AACHEN_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +AACHEN_SKELETON_JOINTS["left_knee"] = "LeftLeg" +AACHEN_SKELETON_JOINTS["right_knee"] = "RightLeg" +AACHEN_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +AACHEN_SKELETON_JOINTS["right_ankle"] = "RightFoot" +AACHEN_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +AACHEN_SKELETON_JOINTS["right_toe"] = "RightToeBase" +AACHEN_SKELETON_JOINTS["left_heel"] = None +AACHEN_SKELETON_JOINTS["right_heel"] = None +AACHEN_SKELETON_JOINTS["neck"] = "Neck" +AACHEN_SKELETON_JOINTS["head"] = "Head" +AACHEN_SKELETON_MODEL = collections.OrderedDict() +AACHEN_SKELETON_MODEL["name"] = "aachen" +AACHEN_SKELETON_MODEL["joints"] = AACHEN_SKELETON_JOINTS +AACHEN_SKELETON_MODEL["foot_joints"] = [] + +AACHEN_SKELETON_COS_MAP = collections.defaultdict(dict) +AACHEN_SKELETON_COS_MAP["Hips"]["y"] = [0, 1, 0] +AACHEN_SKELETON_COS_MAP["Hips"]["x"] = [1, 0, 0] +AACHEN_SKELETON_MODEL["cos_map"] = AACHEN_SKELETON_COS_MAP + + +MH_CMU_RED_SKELETON_JOINTS = collections.OrderedDict() +MH_CMU_RED_SKELETON_JOINTS["root"] = "root" +MH_CMU_RED_SKELETON_JOINTS["pelvis"] = "pelvis" +MH_CMU_RED_SKELETON_JOINTS["thorax"] = "Spine" +MH_CMU_RED_SKELETON_JOINTS["left_clavicle"] = "lclavicle" +MH_CMU_RED_SKELETON_JOINTS["right_clavicle"] = "rclavicle" +MH_CMU_RED_SKELETON_JOINTS["left_shoulder"] = "lhumerus" +MH_CMU_RED_SKELETON_JOINTS["right_shoulder"] = "rhumerus" +MH_CMU_RED_SKELETON_JOINTS["left_elbow"] = "lradius" +MH_CMU_RED_SKELETON_JOINTS["right_elbow"] = "rradius" +MH_CMU_RED_SKELETON_JOINTS["left_wrist"] = "lhand" +MH_CMU_RED_SKELETON_JOINTS["right_wrist"] = "rhand" +MH_CMU_RED_SKELETON_JOINTS["left_hip"] = "lfemur" +MH_CMU_RED_SKELETON_JOINTS["right_hip"] = "rfemur" +MH_CMU_RED_SKELETON_JOINTS["left_knee"] = "ltibia" +MH_CMU_RED_SKELETON_JOINTS["right_knee"] = "rtibia" +MH_CMU_RED_SKELETON_JOINTS["left_ankle"] = "lfoot" +MH_CMU_RED_SKELETON_JOINTS["right_ankle"] = "rfoot" +MH_CMU_RED_SKELETON_JOINTS["left_toe"] = "ltoes" +MH_CMU_RED_SKELETON_JOINTS["right_toe"] = "rtoes" +MH_CMU_RED_SKELETON_JOINTS["left_heel"] = None +MH_CMU_RED_SKELETON_JOINTS["right_heel"] = None +MH_CMU_RED_SKELETON_JOINTS["neck"] = "head" +MH_CMU_RED_SKELETON_JOINTS["head"] = "head_EndSite" +MH_CMU_RED_SKELETON_MODEL = collections.OrderedDict() +MH_CMU_RED_SKELETON_MODEL["name"] = "mh_cmu_red" +MH_CMU_RED_SKELETON_MODEL["joints"] = MH_CMU_RED_SKELETON_JOINTS +MH_CMU_RED_SKELETON_MODEL["foot_joints"] = [] + +RAW2_SKELETON_FOOT_JOINTS = [RIGHT_TOE, LEFT_TOE, RIGHT_HEEL,LEFT_HEEL] + +RAW2_SKELETON_JOINTS = collections.OrderedDict() +RAW2_SKELETON_JOINTS["root"] = "Hips" +RAW2_SKELETON_JOINTS["pelvis"] = "ToSpine" +RAW2_SKELETON_JOINTS["spine"] = "Spine" +RAW2_SKELETON_JOINTS["spine_1"] = "Spine1" +RAW2_SKELETON_JOINTS["spine_2"] = "Neck" +RAW2_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +RAW2_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +RAW2_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +RAW2_SKELETON_JOINTS["right_shoulder"] = "RightArm" +RAW2_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +RAW2_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +RAW2_SKELETON_JOINTS["left_wrist"] = "LeftHand" +RAW2_SKELETON_JOINTS["right_wrist"] = "RightHand" +RAW2_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +RAW2_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +RAW2_SKELETON_JOINTS["left_knee"] = "LeftLeg" +RAW2_SKELETON_JOINTS["right_knee"] = "RightLeg" +RAW2_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +RAW2_SKELETON_JOINTS["right_ankle"] = "RightFoot" +RAW2_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +RAW2_SKELETON_JOINTS["right_toe"] = "RightToeBase" +RAW2_SKELETON_JOINTS["left_heel"] = "LeftHeel" +RAW2_SKELETON_JOINTS["right_heel"] = "RightHeel" +RAW2_SKELETON_JOINTS["neck"] = "Neck" +RAW2_SKELETON_JOINTS["head"] = "Head" + +RAW2_SKELETON_COS_MAP = collections.defaultdict(dict) +RAW2_SKELETON_COS_MAP["Neck"]["x"] = [1, 0, 0] +RAW2_SKELETON_COS_MAP["Neck"]["y"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["Head"]["x"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["Head"]["y"] = [0, 1, 0] + +""" +RAW2_SKELETON_COS_MAP["LeftShoulder"]["x"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["LeftShoulder"]["y"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["LeftForeArm"]["x"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["LeftForeArm"]["y"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["LeftArm"]["x"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["LeftArm"]["y"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["LeftHand"]["x"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["LeftHand"]["y"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["RightShoulder"]["x"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["RightShoulder"]["y"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["RightArm"]["x"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["RightArm"]["y"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["RightForeArm"]["x"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["RightForeArm"]["y"] = [0, 1, 0] +RAW2_SKELETON_COS_MAP["RightHand"]["x"] = [-1, 0, 0] +RAW2_SKELETON_COS_MAP["RightHand"]["y"] = [0, 1, 0] +""" +RAW2_SKELETON_MODEL = collections.OrderedDict() +RAW2_SKELETON_MODEL["name"] = "raw2" +RAW2_SKELETON_MODEL["joints"] = RAW2_SKELETON_JOINTS +RAW2_SKELETON_MODEL["foot_joints"] = RAW2_SKELETON_FOOT_JOINTS +RAW2_SKELETON_MODEL["heel_offset"] = [0, -6.480602, 0] +RAW2_SKELETON_MODEL["ik_chains"] = IK_CHAINS_RAW_SKELETON +RAW2_SKELETON_MODEL["cos_map"] = RAW2_SKELETON_COS_MAP + + + + +NOITOM_SKELETON_JOINTS = collections.OrderedDict() +NOITOM_SKELETON_JOINTS["root"] = None +NOITOM_SKELETON_JOINTS["pelvis"] = "Hips" +NOITOM_SKELETON_JOINTS["spine"] = "Spine" +NOITOM_SKELETON_JOINTS["spine_1"] = "Spine1" +NOITOM_SKELETON_JOINTS["spine_2"] = "Spine2" +NOITOM_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +NOITOM_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +NOITOM_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +NOITOM_SKELETON_JOINTS["right_shoulder"] = "RightArm" +NOITOM_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +NOITOM_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +NOITOM_SKELETON_JOINTS["left_wrist"] = "LeftHand" +NOITOM_SKELETON_JOINTS["right_wrist"] = "RightHand" +#NOITOM_SKELETON_JOINTS["left_finger"] = "RightInHandMiddle" +#NOITOM_SKELETON_JOINTS["right_finger"] = "LeftInHandMiddle" +NOITOM_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +NOITOM_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +NOITOM_SKELETON_JOINTS["left_knee"] = "LeftLeg" +NOITOM_SKELETON_JOINTS["right_knee"] = "RightLeg" +NOITOM_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +NOITOM_SKELETON_JOINTS["right_ankle"] = "RightFoot" +NOITOM_SKELETON_JOINTS["left_toe"] = "LeftFoot_EndSite" +NOITOM_SKELETON_JOINTS["right_toe"] = "RightFoot_EndSite" +NOITOM_SKELETON_JOINTS["left_heel"] = None +NOITOM_SKELETON_JOINTS["right_heel"] = None +NOITOM_SKELETON_JOINTS["neck"] = "Neck" +NOITOM_SKELETON_JOINTS["head"] = "Head" + +NOITOM_SKELETON_JOINTS["right_thumb_base"] = "RightHandThumb1" +NOITOM_SKELETON_JOINTS["right_thumb_mid"] = "RightHandThumb2" +NOITOM_SKELETON_JOINTS["right_thumb_tip"] = "RightHandThumb3" +NOITOM_SKELETON_JOINTS["right_thumb_end"] = "RightHandThumb3_EndSite" + +NOITOM_SKELETON_JOINTS["right_index_finger_root"] = "RightInHandIndex" +NOITOM_SKELETON_JOINTS["right_index_finger_base"] = "RightHandIndex1" +NOITOM_SKELETON_JOINTS["right_index_finger_mid"] = "RightHandIndex2" +NOITOM_SKELETON_JOINTS["right_index_finger_tip"] = "RightHandIndex3" +NOITOM_SKELETON_JOINTS["right_index_finger_end"] = "RightHandIndex3_EndSite" + +NOITOM_SKELETON_JOINTS["right_middle_finger_root"] = "RightInHandMiddle" +NOITOM_SKELETON_JOINTS["right_middle_finger_base"] = "RightHandMiddle1" +NOITOM_SKELETON_JOINTS["right_middle_finger_mid"] = "RightHandMiddle2" +NOITOM_SKELETON_JOINTS["right_middle_finger_tip"] = "RightHandMiddle3" +NOITOM_SKELETON_JOINTS["right_middle_finger_end"] = "RightHandMiddle3_EndSite" + +NOITOM_SKELETON_JOINTS["right_ring_finger_root"] = "RightInHandRing" +NOITOM_SKELETON_JOINTS["right_ring_finger_base"] = "RightHandRing1" +NOITOM_SKELETON_JOINTS["right_ring_finger_mid"] = "RightHandRing2" +NOITOM_SKELETON_JOINTS["right_ring_finger_tip"] = "RightHandRing3" +NOITOM_SKELETON_JOINTS["right_ring_finger_end"] = "RightHandRing3_EndSite" + +NOITOM_SKELETON_JOINTS["right_pinky_finger_root"] = "RightInHandPinky" +NOITOM_SKELETON_JOINTS["right_pinky_finger_base"] = "RightHandPinky1" +NOITOM_SKELETON_JOINTS["right_pinky_finger_mid"] = "RightHandPinky2" +NOITOM_SKELETON_JOINTS["right_pinky_finger_tip"] = "RightHandPinky3" +NOITOM_SKELETON_JOINTS["right_pinky_finger_end"] = "RightHandPinky3_EndSite" + + +NOITOM_SKELETON_JOINTS["left_thumb_base"] = "LeftHandThumb1" +NOITOM_SKELETON_JOINTS["left_thumb_mid"] = "LeftHandThumb2" +NOITOM_SKELETON_JOINTS["left_thumb_tip"] = "LeftHandThumb3" +NOITOM_SKELETON_JOINTS["left_thumb_end"] = "LeftHandThumb3_EndSite" + +NOITOM_SKELETON_JOINTS["left_index_finger_root"] = "LeftInHandIndex" +NOITOM_SKELETON_JOINTS["left_index_finger_base"] = "LeftHandIndex1" +NOITOM_SKELETON_JOINTS["left_index_finger_mid"] = "LeftHandIndex2" +NOITOM_SKELETON_JOINTS["left_index_finger_tip"] = "LeftHandIndex3" +NOITOM_SKELETON_JOINTS["left_index_finger_end"] = "LeftHandIndex3_EndSite" + +NOITOM_SKELETON_JOINTS["left_middle_finger_root"] = "LeftInHandMiddle" +NOITOM_SKELETON_JOINTS["left_middle_finger_base"] = "LeftHandMiddle1" +NOITOM_SKELETON_JOINTS["left_middle_finger_mid"] = "LeftHandMiddle2" +NOITOM_SKELETON_JOINTS["left_middle_finger_tip"] = "LeftHandMiddle3" +NOITOM_SKELETON_JOINTS["left_middle_finger_end"] = "LeftHandMiddle3_EndSite" + +NOITOM_SKELETON_JOINTS["left_ring_finger_root"] = "LeftInHandRing" +NOITOM_SKELETON_JOINTS["left_ring_finger_base"] = "LeftHandRing1" +NOITOM_SKELETON_JOINTS["left_ring_finger_mid"] = "LeftHandRing2" +NOITOM_SKELETON_JOINTS["left_ring_finger_tip"] = "LeftHandRing3" +NOITOM_SKELETON_JOINTS["left_ring_finger_end"] = "LeftHandRing3_EndSite" + +NOITOM_SKELETON_JOINTS["left_pinky_finger_root"] = "LeftInHandPinky" +NOITOM_SKELETON_JOINTS["left_pinky_finger_base"] = "LeftHandPinky1" +NOITOM_SKELETON_JOINTS["left_pinky_finger_mid"] = "LeftHandPinky2" +NOITOM_SKELETON_JOINTS["left_pinky_finger_tip"] = "LeftHandPinky3" +NOITOM_SKELETON_JOINTS["left_pinky_finger_end"] = "LeftHandPinky3_EndSite" + + +NOITOM_SKELETON_MODEL = collections.OrderedDict() +NOITOM_SKELETON_MODEL["name"] = "noitom" +NOITOM_SKELETON_MODEL["joints"] = NOITOM_SKELETON_JOINTS +NOITOM_SKELETON_COS_MAP = collections.defaultdict(dict) +NOITOM_SKELETON_COS_MAP["LeftHand"]["x"] = [0, -1, 0] +NOITOM_SKELETON_COS_MAP["LeftHand"]["y"] = [1, 0, 0] +NOITOM_SKELETON_COS_MAP["RightHand"]["x"] = [0, 1, 0] +NOITOM_SKELETON_COS_MAP["RightHand"]["y"] = [-1, 0, 0] +for k, j in NOITOM_SKELETON_JOINTS.items(): + if j is None: + continue + if "finger" in k or "thumb" in k: + if "Left" in j: + NOITOM_SKELETON_COS_MAP[j]["x"] = [0, 1, 0] + NOITOM_SKELETON_COS_MAP[j]["y"] = [1, 0, 0] + elif "Right" in j: + NOITOM_SKELETON_COS_MAP[j]["x"] = [0, 1, 0] + NOITOM_SKELETON_COS_MAP[j]["y"] = [-1, 0, 0] +NOITOM_SKELETON_MODEL["cos_map"] = NOITOM_SKELETON_COS_MAP + + + + + + + +MO_SKELETON_JOINTS = collections.OrderedDict() +MO_SKELETON_JOINTS["root"] = None +MO_SKELETON_JOINTS["pelvis"] = "Hips" +MO_SKELETON_JOINTS["spine"] = "Spine" +MO_SKELETON_JOINTS["spine_1"] = "Spine1" +MO_SKELETON_JOINTS["spine_2"] = "Spine2" +MO_SKELETON_JOINTS["left_clavicle"] = "LeftShoulder" +MO_SKELETON_JOINTS["right_clavicle"] = "RightShoulder" +MO_SKELETON_JOINTS["left_shoulder"] = "LeftArm" +MO_SKELETON_JOINTS["right_shoulder"] = "RightArm" +MO_SKELETON_JOINTS["left_elbow"] = "LeftForeArm" +MO_SKELETON_JOINTS["right_elbow"] = "RightForeArm" +MO_SKELETON_JOINTS["left_wrist"] = "LeftHand" +MO_SKELETON_JOINTS["right_wrist"] = "RightHand" +#MO_SKELETON_JOINTS["left_finger"] = "RightInHandMiddle" +#MO_SKELETON_JOINTS["right_finger"] = "LeftInHandMiddle" +MO_SKELETON_JOINTS["left_hip"] = "LeftUpLeg" +MO_SKELETON_JOINTS["right_hip"] = "RightUpLeg" +MO_SKELETON_JOINTS["left_knee"] = "LeftLeg" +MO_SKELETON_JOINTS["right_knee"] = "RightLeg" +MO_SKELETON_JOINTS["left_ankle"] = "LeftFoot" +MO_SKELETON_JOINTS["right_ankle"] = "RightFoot" +MO_SKELETON_JOINTS["left_toe"] = "LeftToeBase" +MO_SKELETON_JOINTS["right_toe"] = "RightToeBase" +MO_SKELETON_JOINTS["left_heel"] = None +MO_SKELETON_JOINTS["right_heel"] = None +MO_SKELETON_JOINTS["neck"] = "Neck" +MO_SKELETON_JOINTS["head"] = "Head" + +MO_SKELETON_MODEL = collections.OrderedDict() +MO_SKELETON_MODEL["name"] = "mo" +MO_SKELETON_MODEL["joints"] = MO_SKELETON_JOINTS +MO_SKELETON_COS_MAP = collections.defaultdict(dict) +MO_SKELETON_COS_MAP["LeftHand"]["x"] = [0, -1, 0] +MO_SKELETON_COS_MAP["LeftHand"]["y"] = [1, 0, 0] +MO_SKELETON_COS_MAP["RightHand"]["x"] = [0, 1, 0] +MO_SKELETON_COS_MAP["RightHand"]["y"] = [-1, 0, 0] +MO_SKELETON_MODEL["cos_map"] = NOITOM_SKELETON_COS_MAP + + +SKELETON_MODELS = collections.OrderedDict() +SKELETON_MODELS["rocketbox"] = ROCKETBOX_SKELETON_MODEL +SKELETON_MODELS["game_engine"] = GAME_ENGINE_SKELETON_MODEL +SKELETON_MODELS["raw"] = RAW_SKELETON_MODEL +SKELETON_MODELS["cmu"] = CMU_SKELETON_MODEL +SKELETON_MODELS["mcs"] = MCS_SKELETON_MODEL +SKELETON_MODELS["mh_cmu"] = MH_CMU_SKELETON_MODEL +SKELETON_MODELS["mh_cmu2"] = MH_CMU_2_SKELETON_MODEL +SKELETON_MODELS["iclone"] = ICLONE_SKELETON_MODEL +SKELETON_MODELS["moviemation"] = MOVIEMATION_SKELETON_MODEL +SKELETON_MODELS["custom"] = CUSTOM_SKELETON_MODEL +SKELETON_MODELS["captury"] = CAPTURY_SKELETON_MODEL +SKELETON_MODELS["holden"] = HOLDEN_SKELETON_MODEL +SKELETON_MODELS["mixamo"] = MIXAMO_SKELETON_MODEL +SKELETON_MODELS["max"] = MAX_SKELETON_MODEL +SKELETON_MODELS["aachen"] = AACHEN_SKELETON_MODEL +SKELETON_MODELS["mh_cmu_red"] = MH_CMU_RED_SKELETON_MODEL +SKELETON_MODELS["game_engine_wrong_root"] = GAME_ENGINE_WRONG_ROOT_SKELETON_MODEL +SKELETON_MODELS["raw2"] = RAW2_SKELETON_MODEL +SKELETON_MODELS["noitom"] = NOITOM_SKELETON_MODEL +SKELETON_MODELS["mo"] = MO_SKELETON_MODEL + +STANDARD_JOINTS = ["root","pelvis", "spine_1", "spine_2", "neck", "left_clavicle", "head", "left_shoulder", "left_elbow", "left_wrist", "right_clavicle", "right_shoulder", + "left_hip", "left_knee", "left_ankle", "left_toe", "right_elbow", "right_hip", "right_knee", "right_ankle", "right_toe"] + +FINGER_JOINTS = ["left_thumb_base","left_thumb_mid", "left_thumb_tip","left_thumb_end", + "left_index_finger_root","left_index_finger_base","left_index_finger_mid", "left_index_finger_tip","left_index_finger_end", + "left_middle_finger_root","left_middle_finger_base","left_middle_finger_mid","left_middle_finger_tip","left_middle_finger_end", + "left_ring_finger_root","left_ring_finger_base","left_ring_finger_mid","left_ring_finger_tip", "left_ring_finger_end", + "left_pinky_finger_root","left_pinky_finger_base","left_pinky_finger_mid","left_pinky_finger_tip", "left_pinky_finger_end", + + "right_thumb_base","right_thumb_mid","right_thumb_tip","right_thumb_end", + "right_index_finger_root","right_index_finger_base","right_index_finger_mid","right_index_finger_tip","right_index_finger_end", + "right_middle_finger_root","right_middle_finger_base","right_middle_finger_mid","right_middle_finger_tip","right_middle_finger_end", + "right_ring_finger_root","right_ring_finger_base","right_ring_finger_mid","right_ring_finger_tip","right_ring_finger_end", + "right_pinky_finger_root","right_pinky_finger_base","right_pinky_finger_mid","right_pinky_finger_tip","right_pinky_finger_end" + ] +UPPER_BODY_JOINTS = ["spine", "spine_1", "spine_2","neck", "head" + "left_clavicle", "right_clavicle", + "left_shoulder", "right_shoulder", + "left_elbow", "right_elbow", + "left_wrist", "right_wrist", + "left_hold_point","right_hold_point" +] + + + +JOINT_CHILD_MAP = dict() +JOINT_CHILD_MAP["root"] = "pelvis" +JOINT_CHILD_MAP["pelvis"] = "spine" +JOINT_CHILD_MAP["spine"] = "spine_1" +JOINT_CHILD_MAP["spine_1"] = "spine_2" +JOINT_CHILD_MAP["spine_2"] = "neck" +JOINT_CHILD_MAP["neck"] = "head" +JOINT_CHILD_MAP["left_clavicle"] = "left_shoulder" +JOINT_CHILD_MAP["left_shoulder"] = "left_elbow" +JOINT_CHILD_MAP["left_elbow"] = "left_wrist" +JOINT_CHILD_MAP["left_wrist"] = "left_finger" # TODO rename to hand center +JOINT_CHILD_MAP["right_clavicle"] = "right_shoulder" +JOINT_CHILD_MAP["right_shoulder"] = "right_elbow" +JOINT_CHILD_MAP["right_elbow"] = "right_wrist" +JOINT_CHILD_MAP["right_wrist"] = "right_finger" # TODO rename to hand center +JOINT_CHILD_MAP["left_hip"] = "left_knee" +JOINT_CHILD_MAP["left_knee"] = "left_ankle" +JOINT_CHILD_MAP["right_elbow"] = "right_wrist" +JOINT_CHILD_MAP["right_hip"] = "right_knee" +JOINT_CHILD_MAP["right_knee"] = "right_ankle" +JOINT_CHILD_MAP["left_ankle"] = "left_toe" +JOINT_CHILD_MAP["right_ankle"] = "right_toe" + +JOINT_PARENT_MAP = {v: k for k, v in JOINT_CHILD_MAP.items()} + +FABRIK_CHAINS = dict() +FABRIK_CHAINS["right_wrist"] = {"joint_order": ["right_shoulder", "right_elbow", "right_wrist"]} +FABRIK_CHAINS["left_wrist"] = {"joint_order": ["left_shoulder", "left_elbow", "left_wrist"]} + +JOINT_CONSTRAINTS = dict() +JOINT_CONSTRAINTS["right_elbow"] = {"type":"hinge", "swing_axis": [0,-1,0], "twist_axis": [0,0,1], "k1":-90, "k2":90} +JOINT_CONSTRAINTS["left_elbow"] = {"type": "hinge", "swing_axis": [0,-1,0], "twist_axis": [0,0,1], "k1":-90, "k2":90} +#JOINT_CONSTRAINTS["right_shoulder"] = {"type":"cone", "axis": [0,0,1], "k": 0.4} +#JOINT_CONSTRAINTS["left_shoulder"] = {"type": "cone", "axis": [0,0,1], "k": 0.4} +#JOINT_CONSTRAINTS["spine"] = {"type":"cone", "axis": [0,1,0], "k": 45)} +JOINT_CONSTRAINTS["spine_1"] = {"type":"spine", "axis": [0,1,0], "tk1": -90, "tk2": 90, "sk1": -0, "sk2": 0} +##OINT_CONSTRAINTS["spine"] = {"type":"shoulder", "axis": [0,1,0], "k": 20), "k1": -90), "k2": 90) } +JOINT_CONSTRAINTS["left_shoulder"] = {"type":"shoulder", "axis": [0,0,1], "k": 0.4, "k1": -180, "k2": 180 , "stiffness":0.9} +JOINT_CONSTRAINTS["right_shoulder"] = {"type":"shoulder", "axis": [0,0,1], "k": 0.4, "k1": -180, "k2": 180, "stiffness":0.9} + +JOINT_CONSTRAINTS["left_wrist"] = {"type":"cone", "axis": [0,0,1], "k": 90} +JOINT_CONSTRAINTS["right_wrist"] = {"type":"cone", "axis": [0,0,1], "k": 90} +#JOINT_CONSTRAINTS["head"] = {"type":"cone", "axis": [0,1,0], "k": 0)} +#JOINT_CONSTRAINTS["neck"] = {"type":"head", "axis": [0,1,0], "k1": -65), "k2": 65)} +JOINT_CONSTRAINTS["head"] = {"type":"head", "axis": [0,1,0], "tk1": -85, "tk2": 85, "sk1": -45, "sk2": 45} +JOINT_CONSTRAINTS["left_hold_point"] = {"type":"static"} +JOINT_CONSTRAINTS["right_hold_point"] = {"type":"static"} + +STANDARD_MIRROR_MAP_LEFT = {"left_hip": "right_hip", + "left_knee": "right_knee", + "left_ankle":"right_ankle", + "left_clavicle": "right_clavicle", + "left_shoulder": "right_shoulder", + "left_elbow": "right_elbow", + "left_elbow": "right_elbow", + "left_wrist": "right_wrist", + "left_hold_point": "right_hold_point" + } + +STANDARD_MIRROR_MAP_LEFT["left_thumb_base"] = "right_thumb_base" +STANDARD_MIRROR_MAP_LEFT["left_thumb_mid"] = "right_thumb_mid" +STANDARD_MIRROR_MAP_LEFT["left_thumb_tip"] = "right_thumb_tip" +STANDARD_MIRROR_MAP_LEFT["left_thumb_end"] = "right_thumb_end" + +STANDARD_MIRROR_MAP_LEFT["left_index_finger_root"] = "right_index_finger_root" +STANDARD_MIRROR_MAP_LEFT["left_index_finger_base"] = "right_index_finger_base" +STANDARD_MIRROR_MAP_LEFT["left_index_finger_mid"] = "right_index_finger_mid" +STANDARD_MIRROR_MAP_LEFT["left_index_finger_tip"] = "right_index_finger_tip" +STANDARD_MIRROR_MAP_LEFT["left_index_finger_end"] = "right_index_finger_end" + +STANDARD_MIRROR_MAP_LEFT["left_middle_finger_root"] = "right_middle_finger_root" +STANDARD_MIRROR_MAP_LEFT["left_middle_finger_base"] = "right_middle_finger_base" +STANDARD_MIRROR_MAP_LEFT["left_middle_finger_mid"] = "right_middle_finger_mid" +STANDARD_MIRROR_MAP_LEFT["left_middle_finger_tip"] = "right_middle_finger_tip" +STANDARD_MIRROR_MAP_LEFT["left_middle_finger_end"] = "right_middle_finger_end" + +STANDARD_MIRROR_MAP_LEFT["left_ring_finger_root"] = "right_ring_finger_root" +STANDARD_MIRROR_MAP_LEFT["left_ring_finger_base"] = "right_ring_finger_base" +STANDARD_MIRROR_MAP_LEFT["left_ring_finger_mid"] = "right_ring_finger_mid" +STANDARD_MIRROR_MAP_LEFT["left_ring_finger_tip"] = "right_ring_finger_tip" +STANDARD_MIRROR_MAP_LEFT["left_ring_finger_end"] = "right_ring_finger_end" + +STANDARD_MIRROR_MAP_LEFT["left_pinky_finger_root"] = "right_pinky_finger_root" +STANDARD_MIRROR_MAP_LEFT["left_pinky_finger_base"] = "right_pinky_finger_base" +STANDARD_MIRROR_MAP_LEFT["left_pinky_finger_mid"] = "right_pinky_finger_mid" +STANDARD_MIRROR_MAP_LEFT["left_pinky_finger_tip"] = "right_pinky_finger_tip" +STANDARD_MIRROR_MAP_LEFT["left_pinky_finger_end"] = "right_pinky_finger_end" + +STANDARD_MIRROR_MAP_RIGHT = dict() +for key, value in STANDARD_MIRROR_MAP_LEFT.items(): + STANDARD_MIRROR_MAP_RIGHT[value] = key + +STANDARD_MIRROR_MAP = dict() +STANDARD_MIRROR_MAP.update(STANDARD_MIRROR_MAP_LEFT) +STANDARD_MIRROR_MAP.update(STANDARD_MIRROR_MAP_RIGHT) diff --git a/build/lib/anim_utils/animation_data/skeleton_node.py b/build/lib/anim_utils/animation_data/skeleton_node.py new file mode 100644 index 0000000..008e862 --- /dev/null +++ b/build/lib/anim_utils/animation_data/skeleton_node.py @@ -0,0 +1,303 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from transformations import quaternion_matrix, euler_matrix, euler_from_matrix, quaternion_from_matrix, quaternion_about_axis +from .constants import ROTATION_TYPE_EULER, ROTATION_TYPE_QUATERNION +from .utils import rotation_order_to_string + +SKELETON_NODE_TYPE_ROOT = 0 +SKELETON_NODE_TYPE_JOINT = 1 +SKELETON_NODE_TYPE_END_SITE = 2 +ORIGIN = point = np.array([0, 0, 0, 1]) + + +class SkeletonNodeBase(object): + def __init__(self, node_name, channels, parent=None, level=0, channel_indices=None): + self.parent = parent + self.node_name = node_name + self.channels = channels + self.level = level + self.children = [] + self.index = -1 + self.quaternion_frame_index = -1 + self.euler_frame_index = -1 + self.node_type = None + self.offset = [0.0, 0.0, 0.0] + self.rotation = np.array([1.0, 0.0, 0.0, 0.0]) + self.euler_angles = np.array([0.0, 0.0, 0.0]) + self.translation = np.array([0.0, 0.0, 0.0]) + self.fixed = True + self.cached_global_matrix = None + self.joint_constraint = None + self.format_meta_data = dict() # store format specific data + self.stiffness = 0 + self.rotation_order = [c for c in self.channels if "rotation" in c] + self._rotation_order_str = rotation_order_to_string(self.rotation_order) + self.channel_indices = channel_indices + self.translation_order = [c for c in self.channels if "position" in c] + if self.channel_indices is not None: + self.rotation_channel_indices = [self.channel_indices[self.channels.index(r)] for r in self.rotation_order] + self.translation_channel_indices = [self.channel_indices[self.channels.index(t)] for t in self.translation_order] + + def clear_cached_global_matrix(self): + self.cached_global_matrix = None + + def get_global_position(self, quaternion_frame, use_cache=False): + global_matrix = self.get_global_matrix(quaternion_frame, use_cache) + point = np.dot(global_matrix, ORIGIN) + return point[:3] + + def get_global_position_from_euler(self, euler_frame, use_cache=False): + global_matrix = self.get_global_matrix_from_euler_frame(euler_frame, use_cache) + point = np.dot(global_matrix, ORIGIN) + return point[:3] + + def get_global_orientation_quaternion(self, quaternion_frame, use_cache=False): + global_matrix = self.get_global_matrix(quaternion_frame, use_cache) + return quaternion_from_matrix(global_matrix) + + def get_global_orientation_euler(self, quaternion_frame, use_cache=False): + global_matrix = self.get_global_matrix(quaternion_frame, use_cache) + return euler_from_matrix(global_matrix) + + def get_global_matrix(self, quaternion_frame, use_cache=False): + if self.cached_global_matrix is not None and use_cache: + return self.cached_global_matrix + else: + if self.parent is not None: + parent_matrix = self.parent.get_global_matrix(quaternion_frame, use_cache) + self.cached_global_matrix = np.dot(parent_matrix, self.get_local_matrix(quaternion_frame)) + return self.cached_global_matrix + else: + self.cached_global_matrix = self.get_local_matrix(quaternion_frame) + return self.cached_global_matrix + + def get_global_matrix_from_euler_frame(self, euler_frame, use_cache=False): + if self.parent is not None: + parent_matrix = self.parent.get_global_matrix_from_euler_frame(euler_frame) + self.cached_global_matrix = np.dot(parent_matrix, self.get_local_matrix_from_euler(euler_frame)) + return self.cached_global_matrix + else: + self.cached_global_matrix = self.get_local_matrix_from_euler(euler_frame) + return self.cached_global_matrix + + def get_global_matrix_from_anim_frame(self, frame, use_cache=False): + if self.cached_global_matrix is not None and use_cache: + return self.cached_global_matrix + else: + if self.parent is not None: + parent_matrix = self.parent.get_global_matrix2(frame, use_cache) + self.cached_global_matrix = np.dot(parent_matrix, frame[self.node_name]) + return self.cached_global_matrix + else: + self.cached_global_matrix = frame[self.node_name] + return self.cached_global_matrix + + def get_local_matrix(self, quaternion_frame): + pass + + def get_local_matrix_from_euler(self, euler_frame): + pass + + def get_frame_parameters(self, frame, rotation_type): + pass + + def get_number_of_frame_parameters(self, rotation_type): + pass + + def to_unity_format(self, joints, scale=1.0, joint_name_map=None): + joint_desc = dict() + joint_desc["name"] = self.node_name + if type(self.offset) == list: + offset = list(self.offset) + else: + offset = self.offset.tolist() + offset[0] *= -scale + offset[1] *= scale + offset[2] *= scale + joint_desc["offset"] = offset + + if type(self.rotation) == list: + rotation = list(self.rotation) + else: + rotation = self.rotation.tolist() + rotation[0] *= -scale + rotation[1] *= -scale + rotation[2] *= scale + rotation[3] *= scale + joint_desc["rotation"] = rotation + if joint_name_map is not None: + if self.node_name in joint_name_map: + joint_desc["targetName"] = joint_name_map[self.node_name] + else: + joint_desc["targetName"] = "none" + else: + joint_desc["targetName"] = self.node_name + joint_desc["children"] = [] + joints.append(joint_desc) + for c in self.children: + joint_desc["children"].append(c.node_name) + c.to_unity_format(joints, scale, joint_name_map=joint_name_map) + + def get_fk_chain_list(self): + pass + + def get_parent_name(self, animated_joint_list): + ''' + :return: string, the name of parent node. If parent node is None, return None + ''' + if self.parent is None: + return None + else: + if animated_joint_list is None: + return self.parent.node_name + else: + if self.parent.node_name in animated_joint_list: + return self.parent.node_name + else: + return self.parent.get_parent_name(animated_joint_list) + + +class SkeletonRootNode(SkeletonNodeBase): + def __init__(self, node_name, channels, parent=None, level=0, channel_indices=None): + super(SkeletonRootNode, self).__init__(node_name, channels, parent, level, channel_indices) + self.node_type = SKELETON_NODE_TYPE_ROOT + + def get_local_matrix(self, quaternion_frame): + local_matrix = quaternion_matrix(quaternion_frame[3:7]) + local_matrix[:3, 3] = [t + o for t, o in zip(quaternion_frame[:3], self.offset)] + return local_matrix + + def get_local_matrix_from_euler(self, euler_frame): + if not self.rotation_order is None and not self.rotation_channel_indices is None: + local_matrix = euler_matrix(*euler_frame[self.rotation_channel_indices], self._rotation_order_str) + else: + raise ValueError("no rotation order!") + if len(self.translation_channel_indices)> 0: + local_matrix[:3, 3] = [t + o for t, o in zip(euler_frame[self.translation_channel_indices], self.offset)] + else: + local_matrix[:3, 3] = self.offset + return local_matrix + + def get_frame_parameters(self, frame, rotation_type): + if not self.fixed: + return frame[:7].tolist() + else: + return [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] + + def get_euler_frame_parameters(self, euler_frame): + if not self.fixed: + return euler_frame[:6].tolist() + else: + return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + def get_number_of_frame_parameters(self, rotation_type): + if rotation_type == ROTATION_TYPE_QUATERNION: + return 7 + else: + return 6 + + def get_fk_chain_list(self): + return [self.node_name] + +class SkeletonJointNode(SkeletonNodeBase): + def __init__(self, node_name, channels, parent=None, level=0, channel_indices=None): + super(SkeletonJointNode, self).__init__(node_name, channels, parent, level, channel_indices) + self.node_type = SKELETON_NODE_TYPE_JOINT + + def get_local_matrix(self, quaternion_frame): + if not self.fixed: + frame_index = self.quaternion_frame_index * 4 + 3 + m = quaternion_matrix(quaternion_frame[frame_index: frame_index + 4]) + else: + m = quaternion_matrix(self.rotation) + m[:3, 3] = self.offset + return m + + def get_local_matrix_from_euler(self, euler_frame): + if not self.fixed: + if not self.rotation_order is None: + local_matrix = euler_matrix(*euler_frame[self.rotation_channel_indices], self._rotation_order_str) + else: + raise ValueError("no rotation order!") + else: + local_matrix = euler_matrix(*np.radians(self.euler_angles), axes='rxyz') + # local_matrix[:3, 3] = self.offset + if self.translation_channel_indices != []: + local_matrix[:3, 3] = [t + o for t, o in zip(euler_frame[self.translation_channel_indices], self.offset)] + else: + local_matrix[:3, 3] = self.offset + return local_matrix + + def get_frame_parameters(self, frame, rotation_type): + if rotation_type == ROTATION_TYPE_QUATERNION: + if not self.fixed: + frame_index = self.quaternion_frame_index * 4 + 3 + return frame[frame_index:frame_index+4].tolist() + else: + return self.rotation + else: + if not self.fixed: + frame_index = self.quaternion_frame_index * 3 + 3 + return frame[frame_index:frame_index+3].tolist() + else: + return [0.0, 0.0, 0.0] + + def get_number_of_frame_parameters(self, rotation_type): + if rotation_type == ROTATION_TYPE_QUATERNION: + return 4 + else: + return 3 + + def get_fk_chain_list(self): + fk_chain = [self.node_name] + if self.parent is not None: + fk_chain += self.parent.get_fk_chain_list() + return fk_chain + +class SkeletonEndSiteNode(SkeletonNodeBase): + def __init__(self, node_name, channels, parent=None, level=0): + super(SkeletonEndSiteNode, self).__init__(node_name, channels, parent, level) + self.node_type = SKELETON_NODE_TYPE_END_SITE + + def get_local_matrix(self, quaternion_frame): + local_matrix = np.identity(4) + local_matrix[:3, 3] = self.offset + return local_matrix + + def get_local_matrix_from_euler(self, euler_frame): + local_matrix = np.identity(4) + local_matrix[:3, 3] = self.offset + return local_matrix + + def get_frame_parameters(self, frame, rotation_type): + return None + + def get_number_of_frame_parameters(self, rotation_type): + return 0 + + def get_fk_chain_list(self): + fk_chain = [] + if self.parent is not None: + fk_chain += self.parent.get_fk_chain_list() + return fk_chain diff --git a/build/lib/anim_utils/animation_data/utils.py b/build/lib/anim_utils/animation_data/utils.py new file mode 100644 index 0000000..b8794bb --- /dev/null +++ b/build/lib/anim_utils/animation_data/utils.py @@ -0,0 +1,947 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH, Daimler AG. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" +Created on Thu Feb 12 20:11:35 2015 + +@author: Erik Herrmann, Han Du +""" +import os +import glob +from math import sqrt, radians, sin, cos, isnan +from copy import deepcopy +import numpy as np +from scipy.stats import linregress +from collections import OrderedDict +from .constants import * +from transformations import quaternion_matrix, euler_from_matrix, \ + quaternion_from_matrix, euler_matrix, \ + quaternion_multiply, \ + quaternion_about_axis, \ + rotation_matrix, \ + quaternion_conjugate, \ + quaternion_inverse, \ + rotation_from_matrix, \ + quaternion_slerp, \ + quaternion_conjugate, quaternion_inverse, rotation_from_matrix + + +def rotation_order_to_string(rotation_order): + r_order_string = "r" + for c in rotation_order: + if c == "Xrotation": + r_order_string += "x" + elif c == "Yrotation": + r_order_string += "y" + elif c == "Zrotation": + r_order_string += "z" + return r_order_string + + +def get_arc_length_from_points(points): + """ + Note: accuracy depends on the granulariy of points + """ + points = np.asarray(points) + arc_length = 0.0 + last_p = None + for p in points: + if last_p is not None: + delta = p - last_p + arc_length += np.linalg.norm(delta) + last_p = p + return arc_length + + + +def quaternion_data_smoothing(quat_frames): + smoothed_quat_frames = {} + filenames = quat_frames.keys() + smoothed_quat_frames_data = np.asarray(deepcopy(quat_frames.values())) + print('quaternion frame data shape: ', smoothed_quat_frames_data.shape) + assert len(smoothed_quat_frames_data.shape) == 3, ('The shape of quaternion frames is not correct!') + n_samples, n_frames, n_dims = smoothed_quat_frames_data.shape + n_joints = (n_dims - 3)/4 + for i in range(n_joints): + ref_quat = smoothed_quat_frames_data[0, 0, i*LEN_QUAT + LEN_ROOT : (i+1)*LEN_QUAT + LEN_ROOT] + for j in range(1, n_samples): + test_quat = smoothed_quat_frames_data[j, 0, i*LEN_QUAT + LEN_ROOT : (i+1)*LEN_QUAT + LEN_ROOT] + if not areQuatClose(ref_quat, test_quat): + smoothed_quat_frames_data[j, :, i*LEN_QUAT + LEN_ROOT : (i+1)*LEN_QUAT + LEN_ROOT] *= -1 + for i in range(len(filenames)): + smoothed_quat_frames[filenames[i]] = smoothed_quat_frames_data[i] + return smoothed_quat_frames + + +def areQuatClose(quat1, quat2): + dot = np.dot(quat1, quat2) + if dot < 0.0: + return False + else: + return True + + +def get_step_length(frames): + """ + + :param frames: a list of euler or quaternion frames + :return step_len: travelled distance of root joint + """ + root_points = extract_root_positions(frames) + step_len = get_arc_length_from_points(root_points) + return step_len + + +def quaternion_from_vector_to_vector(a, b): + """src: http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another + http://wiki.ogre3d.org/Quaternion+and+Rotation+Primer""" + + v = np.cross(a, b) + w = np.sqrt((np.linalg.norm(a) ** 2) * (np.linalg.norm(b) ** 2)) + np.dot(a, b) + q = np.array([w, v[0], v[1], v[2]]) + if np.dot(q,q) != 0: + return q/ np.linalg.norm(q) + else: + idx = np.nonzero(a)[0] + q = np.array([0, 0, 0, 0]) + q[1 + ((idx + 1) % 2)] = 1 # [0, 0, 1, 0] for a rotation of 180 around y axis + return q + + +def convert_euler_frame_to_reduced_euler_frame(bvhreader, euler_frame): + if type(euler_frame) != list: + euler_frame = list(euler_frame) + reduced_euler_frame = euler_frame[:LEN_ROOT] + for node_name in bvhreader.node_names: + if not node_name.startswith('Bip') and 'EndSite' not in node_name: + x_idx = bvhreader.node_channels.index((node_name, 'Xrotation')) + reduced_euler_frame += euler_frame[x_idx: x_idx + LEN_EULER] + return reduced_euler_frame + + +def convert_euler_frames_to_reduced_euler_frames(bvhreader, euler_frames): + reduced_euler_frames = [] + for euler_frame in euler_frames: + reduced_euler_frames.append(convert_euler_frame_to_reduced_euler_frame(bvhreader, euler_frame)) + return reduced_euler_frames + + +def euler_substraction(theta1, theta2): + ''' compute the angular distance from theta2 to theta1, positive value is anti-clockwise, negative is clockwise + @param theta1, theta2: angles in degree + ''' + theta1 %= 360 + theta2 %= 360 + if theta1 > 180: + theta1 -= 360 + elif theta1 < -180: + theta1 += 360 + + if theta2 > 180: + theta2 -= 360 + elif theta2 < - 180: + theta2 += 360 + + theta = theta1 - theta2 + if theta > 180: + theta -= 360 + elif theta < -180: + theta += 360 + if theta > 180 or theta < - 180: + raise ValueError(' exception value') + return theta + + +def get_cartesian_coordinates_from_quaternion(skeleton, node_name, quaternion_frame, return_gloabl_matrix=False): + """Returns cartesian coordinates for one node at one frame. Modified to + handle frames with omitted values for joints starting with "Bip" + + Parameters + ---------- + * node_name: String + \tName of node + * skeleton: Skeleton + \tBVH data structure read from a file + + """ + if skeleton.node_names[node_name]["level"] == 0: + root_frame_position = quaternion_frame[:3] + root_node_offset = skeleton.node_names[node_name]["offset"] + + return [t + o for t, o in + zip(root_frame_position, root_node_offset)] + + else: + # Names are generated bottom to up --> reverse + chain_names = list(skeleton.gen_all_parents(node_name)) + chain_names.reverse() + chain_names += [node_name] # Node is not in its parent list + + offsets = [skeleton.node_names[nodename]["offset"] + for nodename in chain_names] + root_position = quaternion_frame[:3].flatten() + offsets[0] = [r + o for r, o in zip(root_position, offsets[0])] + + j_matrices = [] + count = 0 + for node_name in chain_names: + index = skeleton.node_name_frame_map[node_name] * 4 + 3 + j_matrix = quaternion_matrix(quaternion_frame[index: index + 4]) + j_matrix[:, 3] = offsets[count] + [1] + j_matrices.append(j_matrix) + count += 1 + + global_matrix = np.identity(4) + for j_matrix in j_matrices: + global_matrix = np.dot(global_matrix, j_matrix) + if return_gloabl_matrix: + return global_matrix + else: + point = np.array([0, 0, 0, 1]) + point = np.dot(global_matrix, point) + return point[:3].tolist() + + +def convert_euler_frame_to_cartesian_frame(skeleton, euler_frame, animated_joints=None): + """ + converts euler frames to cartesian frames by calling get_cartesian_coordinates for each joint + """ + if animated_joints is None: + n_joints = len(skeleton.animated_joints) + cartesian_frame = np.zeros([n_joints, LEN_ROOT_POS]) + for i in range(n_joints): + cartesian_frame[i] = skeleton.nodes[skeleton.animated_joints[i]].get_global_position_from_euler(euler_frame) + else: + n_joints = len(animated_joints) + cartesian_frame = np.zeros([n_joints, LEN_ROOT_POS]) + for i in range(n_joints): + cartesian_frame[i] = skeleton.nodes[animated_joints[i]].get_global_position_from_euler(euler_frame) + return cartesian_frame + + +def convert_quaternion_frame_to_cartesian_frame(skeleton, quat_frame): + """ + Converts quaternion frames to cartesian frames by calling get_cartesian_coordinates_from_quaternion for each joint + """ + cartesian_frame = [] + for node_name in skeleton.node_names: + # ignore Bip joints and end sites + if not node_name.startswith( + "Bip") and "children" in list(skeleton.node_names[node_name].keys()): + cartesian_frame.append( + get_cartesian_coordinates_from_quaternion( + skeleton, + node_name, + quat_frame)) # get_cartesian_coordinates2 + + return cartesian_frame + + +def convert_quaternion_frames_to_cartesian_frames(skeleton, quat_frames): + cartesian_frames = [] + for i in range(len(quat_frames)): + cartesian_frames.append(convert_quaternion_frame_to_cartesian_frame(skeleton, quat_frames[i])) + return cartesian_frames + + +def transform_invariant_point_cloud_distance_2d(a, b, weights=None): + ''' + + :param a: + :param b: + :param weights: + :return: + ''' + theta, offset_x, offset_z = align_point_clouds_2D(a, b, weights) + transformed_b = transform_point_cloud(b, theta, offset_x, offset_z) + dist = calculate_point_cloud_distance(a, transformed_b) + return dist + + +def align_point_clouds_2D(a, b, weights=None): + ''' + Finds aligning 2d transformation of two equally sized point clouds. + from Motion Graphs paper by Kovar et al. + Parameters + --------- + *a: list + \t point cloud + *b: list + \t point cloud + *weights: list + \t weights of correspondences + Returns + ------- + *theta: float + \t angle around y axis in radians + *offset_x: float + \t + *offset_z: float + + ''' + if len(a) != len(b): + raise ValueError("two point cloud should have the same number points: "+str(len(a))+","+str(len(b))) + n_points = len(a) + if weights is None: + weights = np.ones(n_points) + numerator_left = 0.0 + denominator_left = 0.0 + # if not weights: + # weight = 1.0/n_points # todo set weight base on joint level + weighted_sum_a_x = 0.0 + weighted_sum_b_x = 0.0 + weighted_sum_a_z = 0.0 + weighted_sum_b_z = 0.0 + sum_of_weights = 0.0 + for index in range(n_points): + numerator_left += weights[index] * (a[index][0] * b[index][2] - + b[index][0] * a[index][2]) + denominator_left += weights[index] * (a[index][0] * b[index][0] + + a[index][2] * b[index][2]) + sum_of_weights += weights[index] + weighted_sum_a_x += weights[index] * a[index][0] + weighted_sum_b_x += weights[index] * b[index][0] + weighted_sum_a_z += weights[index] * a[index][2] + weighted_sum_b_z += weights[index] * b[index][2] + numerator_right = 1.0 / sum_of_weights * \ + (weighted_sum_a_x * weighted_sum_b_z - + weighted_sum_b_x * weighted_sum_a_z) + numerator = numerator_left - numerator_right + denominator_right = 1.0 / sum_of_weights * \ + (weighted_sum_a_x * weighted_sum_b_x + + weighted_sum_a_z * weighted_sum_b_z) + denominator = denominator_left - denominator_right + theta = np.arctan2(numerator, denominator) + offset_x = (weighted_sum_a_x - weighted_sum_b_x * np.cos(theta) - weighted_sum_b_z * np.sin(theta)) / sum_of_weights + offset_z = (weighted_sum_a_z + weighted_sum_b_x * np.sin(theta) - weighted_sum_b_z * np.cos(theta)) / sum_of_weights + return theta, offset_x, offset_z + + +def convert_euler_frames_to_cartesian_frames(skeleton, euler_frames, animated_joints=None): + """ + converts to euler frames to cartesian frames + """ + cartesian_frames = [] + for euler_frame in euler_frames: + cartesian_frames.append( + convert_euler_frame_to_cartesian_frame(skeleton, euler_frame, animated_joints)) + return np.array(cartesian_frames) + + + +def transform_point_by_quaternion(point, quaternion, offset, origin=None): + """ + http://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion + :param point: + :param quaternion: + :return: + """ + if origin is not None: + origin = np.asarray(origin) + # print "point",point,origin + point = point - origin + else: + origin = [0,0,0] + homogenous_point = np.append([0], point) + tmp_q = quaternion_multiply(quaternion, homogenous_point) + temp_q = quaternion_multiply(tmp_q, quaternion_conjugate(quaternion)) + new_point = [temp_q[i+1] + offset[i] + origin[i] for i in range(3)] + return new_point + + +def transform_point_by_quaternion_faster(point, quaternion, offset, origin=None): + """ + http://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/ + :param point: + :param quaternion: + :return: + """ + if origin is not None: + origin = np.asarray(origin) + # print "point",point,origin + point = point - origin + else: + origin = [0,0,0] + t = 2 * np.cross(quaternion[1:], point) + new_point = np.array(point) + quaternion[0] * t + np.cross(quaternion[1:], t) + new_point = [new_point[i] + offset[i] + origin[i] for i in range(3)] + return new_point + + +def transform_point_by_quaternion_faster2(point, quaternion, offset, origin=None): + """ + http://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion + :param point: + :param quaternion: + :return: + """ + if origin is not None: + origin = np.asarray(origin) + # print "point",point,origin + point = point - origin + else: + origin = [0,0,0] + u = quaternion[1:] + s = quaternion[0] + new_point = 2.0 * np.dot(u, point) * u + (s*s - np.dot(u, u)) * point + 2.0 * s * np.cross(u, point) + new_point = [new_point[i] + offset[i] + origin[i] for i in range(3)] + return new_point + + +def euler_angles_to_rotation_matrix(euler_angles, rotation_order=DEFAULT_ROTATION_ORDER): + # generate rotation matrix based on rotation order + assert len(euler_angles) == 3, ('The length of rotation angles should be 3') + if round(euler_angles[0], 3) == 0 and round(euler_angles[2], 3) == 0: + # rotation about y axis + R = rotation_matrix(np.deg2rad(euler_angles[1]), [0, 1, 0]) + else: + euler_angles = np.deg2rad(euler_angles) + R = euler_matrix(euler_angles[0], euler_angles[1], euler_angles[2], axes=rotation_order_to_string(rotation_order)) + return R + + +def create_transformation_matrix(translation, euler_angles, rotation_order=DEFAULT_ROTATION_ORDER): + #m = np.eye(4,4) + m = euler_angles_to_rotation_matrix(euler_angles, rotation_order) + m[:3,3] = translation + return m + + +def transform_point(point, euler_angles, offset, origin=None, rotation_order=DEFAULT_ROTATION_ORDER): + """ + rotate point around y axis and translate it by an offset + Parameters + --------- + *point: numpy.array + \t coordinates + *angles: list of floats + \tRotation angles in degrees + *offset: list of floats + \tTranslation + """ + assert len(point) == 3, ('the point should be a list of length 3') + # translate point to original point + point = np.asarray(point) + if origin is not None: + origin = np.asarray(origin) + point = point - origin + point = np.insert(point, len(point), 1) + R = euler_angles_to_rotation_matrix(euler_angles, rotation_order=rotation_order) + rotated_point = np.dot(R, point) + if origin is not None: + rotated_point[:3] += origin + #print rotated_point, + transformed_point = rotated_point[:3] + offset + return transformed_point + + +def smooth_quaternion_frames(quaternion_frames, discontinuity, window=20): + """ Smooth quaternion frames given discontinuity frame + + Parameters + ---------- + quaternion_frames: list + \tA list of quaternion frames + discontinuity : int + The frame where the discontinuity is. (e.g. the transitionframe) + window : (optional) int, default is 20 + The smoothing window + Returns + ------- + None. + """ + n_joints = (len(quaternion_frames[0]) - 3) / 4 + # smooth quaternion + n_frames = len(quaternion_frames) + for i in range(n_joints): + for j in range(n_frames - 1): + q1 = np.array(quaternion_frames[j][3 + i * 4: 3 + (i + 1) * 4]) + q2 = np.array(quaternion_frames[j + 1][3 + i * 4:3 + (i + 1) * 4]) + if np.dot(q1, q2) < 0: + quaternion_frames[ + j + 1][3 + i * 4:3 + (i + 1) * 4] = -quaternion_frames[j + 1][3 + i * 4:3 + (i + 1) * 4] + # generate curve of smoothing factors + d = float(discontinuity) + w = float(window) + smoothing_factors = [] + for f in range(n_frames): + value = 0.0 + if d - w <= f < d: + tmp = (f - d + w) / w + value = 0.5 * tmp ** 2 + elif d <= f <= d + w: + tmp = (f - d + w) / w + value = -0.5 * tmp ** 2 + 2 * tmp - 2 + smoothing_factors.append(value) + smoothing_factors = np.array(smoothing_factors) + new_quaternion_frames = [] + for i in range(len(quaternion_frames[0])): + current_value = quaternion_frames[:, i] + magnitude = current_value[int(d)] - current_value[int(d) - 1] + new_value = current_value + (magnitude * smoothing_factors) + new_quaternion_frames.append(new_value) + new_quaternion_frames = np.array(new_quaternion_frames).T + return new_quaternion_frames + + +def smooth_quaternion_frames_partially(quaternion_frames, joint_parameter_indices, discontinuity, window=20): + """ Smooth quaternion frames given discontinuity frame + + Parameters + ---------- + quaternion_frames: list + \tA list of quaternion frames + parameters_indices: list + \tThe list of joint parameter indices that should be smoothed + discontinuity : int + The frame where the discontinuity is. (e.g. the transitionframe) + window : (optional) int, default is 20 + The smoothing window + Returns + ------- + None. + """ + n_joints = (len(quaternion_frames[0]) - 3) / 4 + # smooth quaternion + n_frames = len(quaternion_frames) + for i in range(n_joints): + for j in range(n_frames - 1): + q1 = np.array(quaternion_frames[j][3 + i * 4: 3 + (i + 1) * 4]) + q2 = np.array(quaternion_frames[j + 1][3 + i * 4:3 + (i + 1) * 4]) + if np.dot(q1, q2) < 0: + quaternion_frames[ + j + 1][3 + i * 4:3 + (i + 1) * 4] = -quaternion_frames[j + 1][3 + i * 4:3 + (i + 1) * 4] + # generate curve of smoothing factors + transition_frame = float(discontinuity) + window_size = float(window) + smoothing_factors = [] + for frame_number in range(n_frames): + value = 0.0 + if transition_frame - window_size <= frame_number < transition_frame: + tmp = (frame_number - transition_frame + window_size) / window_size + value = 0.5 * tmp ** 2 + elif transition_frame <= frame_number <= transition_frame + window_size: + tmp = (frame_number - transition_frame + window_size) / window_size + value = -0.5 * tmp ** 2 + 2 * tmp - 2 + smoothing_factors.append(value) + #smoothing_factors = np.array(smoothing_factors) + transition_frame = int(transition_frame) + #new_quaternion_frames = [] + magnitude_vector = np.array(quaternion_frames[transition_frame]) - quaternion_frames[transition_frame-1] + #print "magnitude ", magnitude_vector + for i in joint_parameter_indices: + joint_parameter_index = i*4+3 + for frame_index in range(n_frames): + quaternion_frames[frame_index][joint_parameter_index] = quaternion_frames[frame_index][joint_parameter_index]+magnitude_vector[joint_parameter_index] * smoothing_factors[frame_index] + + + + +def get_rotation_angle(point1, point2): + """ + estimate the rotation angle from point2 to point1 + point1, point2 are normalized points + rotate point2 to be the same as point1 + + Parameters + ---------- + *point1, point2: list or numpy array + \tUnit 2d points + + Return + ------ + *rotation_angle: float (in degree) + \tRotation angle from point2 to point1 + """ + theta1 = point_to_euler_angle(point1) + theta2 = point_to_euler_angle(point2) + rotation_angle = euler_substraction(theta2, theta1) + return rotation_angle + + +def point_to_euler_angle(vec): + ''' + @brief: covert a 2D point vec = (cos, sin) to euler angle (in degree) + The output range is [-180, 180] + ''' + vec = np.array(vec) + theta = np.rad2deg(np.arctan2(vec[1], vec[0])) + return theta + + +def calculate_point_cloud_distance(a, b): + """ + calculates the distance between two point clouds with equal length and + corresponding distances + """ + assert len(a) == len(b) + distance = 0 + n_points = len(a) + for i in range(n_points): + d = [a[i][0] - b[i][0], a[i][1] - b[i][1], a[i][2] - b[i][2]] + distance += sqrt(d[0] ** 2 + d[1] ** 2 + d[2] ** 2) + return distance / n_points + + +def rotate_and_translate_point(p, theta, offset_x, offset_z): + """rotate and translate a 3d point, first rotation them translation + theta is in radians + """ + rotation_angles = [0, theta, 0] + rotation_angles = np.rad2deg(rotation_angles) + offset = [offset_x, 0, offset_z] + transformed_point = transform_point(p, rotation_angles, offset) + return transformed_point + + +def transform_point_cloud(point_cloud, theta, offset_x, offset_z): + """ + transforms points in a point cloud by a rotation around y and a translation + along x and z + """ + transformed_point_cloud = [] + for p in point_cloud: + if p is not None: + transformed_point_cloud.append( + rotate_and_translate_point(p, theta, offset_x, offset_z)) + return transformed_point_cloud + + +def calculate_pose_distance(skeleton, euler_frames_a, euler_frames_b): + ''' Converts euler frames to point clouds and finds the aligning transformation + and calculates the distance after the aligning transformation + ''' + + # theta, offset_x, offset_z = find_aligning_transformation(bvh_reader, euler_frames_a, euler_frames_b, node_name_map) + point_cloud_a = convert_euler_frame_to_cartesian_frame( + skeleton, euler_frames_a[-1]) + point_cloud_b = convert_euler_frame_to_cartesian_frame( + skeleton, euler_frames_b[0]) + + weights = skeleton.joint_weights + theta, offset_x, offset_z = align_point_clouds_2D( + point_cloud_a, point_cloud_b, weights) + t_point_cloud_b = transform_point_cloud( + point_cloud_b, theta, offset_x, offset_z) + error = calculate_point_cloud_distance(point_cloud_a, t_point_cloud_b) + return error + + +def calculate_frame_distance(skeleton, + euler_frame_a, + euler_frame_b, + weights=None, + return_transform=False): + point_cloud_a = convert_euler_frame_to_cartesian_frame(skeleton, + euler_frame_a) + point_cloud_b = convert_euler_frame_to_cartesian_frame(skeleton, + euler_frame_b) + theta, offset_x, offset_z = align_point_clouds_2D(point_cloud_a, point_cloud_b, weights=weights) + t_point_cloud_b = transform_point_cloud(point_cloud_b, theta, offset_x, offset_z) + error = calculate_point_cloud_distance(point_cloud_a, t_point_cloud_b) + if return_transform: + return error, theta, offset_x, offset_z + else: + return error + + +def quat_distance(quat_a, quat_b): + # normalize quaternion vector first + quat_a = np.asarray(quat_a) + quat_b = np.asarray(quat_b) + quat_a /= np.linalg.norm(quat_a) + quat_b /= np.linalg.norm(quat_b) + rotmat_a = quaternion_matrix(quat_a) + rotmat_b = quaternion_matrix(quat_b) + diff_mat = rotmat_a - rotmat_b + tmp = np.ravel(diff_mat) + diff = np.linalg.norm(tmp) + return diff + + +def calculate_weighted_frame_distance_quat(quat_frame_a, + quat_frame_b, + weights): + assert len(quat_frame_a) == len(quat_frame_b) and \ + len(quat_frame_a) == (len(weights) - 2) * LEN_QUAT + LEN_ROOT + diff = 0 + for i in range(len(weights) - 2): + quat1 = quat_frame_a[(i+1)*LEN_QUAT+LEN_ROOT_POS: (i+2)*LEN_QUAT+LEN_ROOT_POS] + quat2 = quat_frame_b[(i+1)*LEN_QUAT+LEN_ROOT_POS: (i+2)*LEN_QUAT+LEN_ROOT_POS] + tmp = quat_distance(quat1, quat2)*weights[i] + diff += tmp + return diff + + + +def extract_root_positions(frames): + """ + + :param frames: a list of euler or quaternion frames + :return roots_2D: a list of root position in 2D space + """ + roots_2D = [] + for i in range(len(frames)): + position_2D = [frames[i][0], frames[i][2]] + roots_2D.append(position_2D) + + return roots_2D + + +def pose_orientation_quat(quaternion_frame, param_range=(3,7), ref_offset=[0, 0, 1, 1]): + """Estimate pose orientation from root orientation + """ + ref_offset = np.array(ref_offset) + q = quaternion_frame[param_range[0]:param_range[1]] + rotmat = quaternion_matrix(q) + rotated_point = np.dot(rotmat, ref_offset) + dir_vec = np.array([rotated_point[0], rotated_point[2]]) + dir_vec /= np.linalg.norm(dir_vec) + return dir_vec + +def pose_orientation_euler(euler_frame, ref_offset = np.array([0, 0, 1, 1])): + ''' + only works for rocketbox skeleton + :param euler_frame: + :return: + ''' + rot_angles = euler_frame[3:6] + rot_angles_rad = np.deg2rad(rot_angles) + rotmat = euler_matrix(*rot_angles_rad, 'rxyz') + rotated_point = np.dot(rotmat, ref_offset) + dir_vec = np.array([rotated_point[0],0, rotated_point[2]]) + dir_vec /= np.linalg.norm(dir_vec) + return dir_vec + + +def pose_orientation_general(euler_frame, joints, skeleton, rotation_order=['Xrotation', 'Yrotation', 'Zrotation']): + ''' + estimate pose heading direction, the position of joints define a body plane, the projection of the normal of + the body plane is the heading direction + :param euler_frame: + :param joints: + :return: + ''' + points = [] + for joint in joints: + points.append(skeleton.nodes[joint].get_global_position_from_euler(euler_frame, rotation_order)) + points = np.asarray(points) + body_plane = BodyPlane(points) + normal_vec = body_plane.normal_vector + dir_vec = np.array([normal_vec[0], normal_vec[2]]) + return dir_vec/np.linalg.norm(dir_vec) + + +def pose_orientation_from_point_cloud(point_cloud, body_plane_joint_indices): + assert len(point_cloud.shape) == 2 + points = [] + for index in body_plane_joint_indices: + points.append(point_cloud[index]) + points = np.asarray(points) + body_plane = BodyPlane(points) + normal_vec = body_plane.normal_vector + dir_vec = np.array([normal_vec[0], normal_vec[2]]) + return dir_vec/np.linalg.norm(dir_vec) + + +def get_trajectory_dir_from_2d_points(points): + """Estimate the trajectory heading + + Parameters + *\Points: numpy array + Step 1: fit the points with a 2d straight line + Step 2: estimate the direction vector from first and last point + """ + points = np.asarray(points) + dir_vector = points[-1] - points[0] + slope, intercept, r_value, p_value, std_err = linregress(*points.T) + if isnan(slope): + orientation_vec1 = np.array([0, 1]) + orientation_vec2 = np.array([0, -1]) + else: + orientation_vec1 = np.array([slope, 1]) + orientation_vec2 = np.array([-slope, -1]) + if np.dot( + orientation_vec1, + dir_vector) > np.dot( + orientation_vec2, + dir_vector): + orientation_vec = orientation_vec1 + else: + orientation_vec = orientation_vec2 + orientation_vec = orientation_vec / np.linalg.norm(orientation_vec) + return orientation_vec + + +def get_dir_from_2d_points(points): + points = np.asarray(points) + dir = points[-1] - points[2] + dir = dir/np.linalg.norm(dir) + return dir + +def pose_up_vector_euler(euler_frame): + """ + Estimate up vector of given euler frame. Assume the pose is aligned in negative Z axis, so the projection of + 3D up vector into XOY plane (Y is vertical axis) is taken into consideration. + :param euler_frame: + :return: + """ + ref_offset = np.array([1, 0, 0, 1]) + rot_angles = euler_frame[3:6] + rot_angles_rad = np.deg2rad(rot_angles) + rotmat = euler_matrix(rot_angles_rad[0], + rot_angles_rad[1], + rot_angles_rad[2], + 'rxyz') + rotated_point = np.dot(rotmat, ref_offset) + up_vec = np.array([rotated_point[0], rotated_point[1]]) + up_vec /= np.linalg.norm(up_vec) + return up_vec + + + +def quaternion_rotate_vector(q, vector): + """ src: http://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion + Args: + q: + vector: + + Returns: + + """ + tmp_result = quaternion_multiply(q, vector) + return quaternion_multiply(tmp_result, quaternion_conjugate(q))[1:] + + +def point_rotation_by_quaternion(point, q): + """ + Rotate a 3D point by quaternion + :param point: [x, y, z] + :param q: [qw, qx, qy, qz] + :return rotated_point: [x, y, ] + """ + r = [0, point[0], point[1], point[2]] + q_conj = [q[0], -1*q[1], -1*q[2], -1*q[3]] + return quaternion_multiply(quaternion_multiply(q, r), q_conj)[1:] + + +def get_lookAt_direction(bvh_analyzer, frame_index): + ref_vec = np.array([0, 0, 1, 0]) + global_trans = bvh_analyzer.get_global_transform("Head", frame_index) + dir_vec = np.dot(global_trans, ref_vec) + return np.asarray([dir_vec[0], dir_vec[2]]) + + +def find_rotation_angle_between_two_vector(v1, v2): + from scipy.optimize import minimize + ''' + positive angle means counterclockwise, negative angle means clockwise + :return: + ''' + def objective_function(theta, data): + src_dir, target_dir = data + rotmat = np.zeros((2, 2)) + rotmat[0, 0] = np.cos(theta) + rotmat[0, 1] = -np.sin(theta) + rotmat[1, 0] = np.sin(theta) + rotmat[1, 1] = np.cos(theta) + rotated_src_dir = np.dot(rotmat, src_dir) + return np.linalg.norm(rotated_src_dir - target_dir) + + initial_guess = 0 + params = [v1, v2] + res = minimize(objective_function, initial_guess, args=params, method='L-BFGS-B') + return np.rad2deg(res.x[0]) + + +def get_rotation_angles_for_vectors(dir_vecs, ref_dir, up_axis): + ''' + + :param dir_vecs: nd array N*3 + :param ref_dir: 3d array + :param up_axis: 3d array + :return: + ''' + axis_indice = np.where(up_axis == 0) + angles = [] + for i in range(len(dir_vecs)): + angles.append(get_rotation_angle(dir_vecs[i][axis_indice], ref_dir[axis_indice])) + return np.deg2rad(np.asarray(angles)) + + +def smooth_quat_frames(quat_frames): + smoothed_quat_frames = OrderedDict() + filenames = list(quat_frames.keys()) + smoothed_quat_frames_data = np.asarray(deepcopy(list(quat_frames.values()))) + print(('quaternion frame data shape: ', smoothed_quat_frames_data.shape)) + assert len(smoothed_quat_frames_data.shape) == 3, ('The shape of quaternion frames is not correct!') + n_samples, n_frames, n_dims = smoothed_quat_frames_data.shape + n_joints = int((n_dims - 3) / 4) + for i in range(n_joints): + ref_quat = smoothed_quat_frames_data[0, 0, + i * LEN_QUAT + LEN_ROOT: (i + 1) * LEN_QUAT + LEN_ROOT] + for j in range(1, n_samples): + test_quat = smoothed_quat_frames_data[j, 0, + i * LEN_QUAT + LEN_ROOT: (i + 1) * LEN_QUAT + LEN_ROOT] + if not areQuatClose(ref_quat, test_quat): + smoothed_quat_frames_data[j, :, + i * LEN_QUAT + LEN_ROOT: (i + 1) * LEN_QUAT + LEN_ROOT] *= -1 + for i in range(len(filenames)): + smoothed_quat_frames[filenames[i]] = smoothed_quat_frames_data[i] + return smoothed_quat_frames + + +def combine_motion_clips(clips, motion_len, window_step): + ''' + + :param clips: + :param motion_len: + :param window_step: + :return: + ''' + clips = np.asarray(clips) + n_clips, window_size, n_dims = clips.shape + + ## case 1: motion length is smaller than window_step + if motion_len <= window_step: + assert n_clips == 1 + left_index = (window_size - motion_len) // 2 + (window_size - motion_len) % 2 + right_index = window_size - (window_size - motion_len) // 2 + return clips[0][left_index: right_index] + + ## case 2: motion length is larger than window_step and smaller than window + if motion_len > window_step and motion_len <= window_size: + assert n_clips == 2 + left_index = (window_size - motion_len) // 2 + (window_size - motion_len) % 2 + right_index = window_size - (window_size - motion_len) // 2 + return clips[0][left_index: right_index] + + residue_frames = motion_len % window_step + print('residue_frames: ', residue_frames) + ## case 3: residue frames is smaller than window step + if residue_frames <= window_step: + residue_frames += window_step + combined_frames = np.concatenate(clips[0:n_clips - 2, :window_step], axis=0) + left_index = (window_size - residue_frames) // 2 + (window_size - residue_frames) % 2 + right_index = window_size - (window_size - residue_frames) // 2 + combined_frames = np.concatenate((combined_frames, clips[-2, left_index:right_index]), axis=0) + return combined_frames diff --git a/build/lib/anim_utils/motion_editing/__init__.py b/build/lib/anim_utils/motion_editing/__init__.py new file mode 100644 index 0000000..88e715b --- /dev/null +++ b/build/lib/anim_utils/motion_editing/__init__.py @@ -0,0 +1,4 @@ +from .motion_editing import MotionEditing +from .motion_grounding import MotionGrounding, add_heels_to_skeleton +from .footplant_constraint_generator import FootplantConstraintGenerator +from .utils import get_average_joint_position, get_average_joint_direction diff --git a/build/lib/anim_utils/motion_editing/analytical_inverse_kinematics.py b/build/lib/anim_utils/motion_editing/analytical_inverse_kinematics.py new file mode 100644 index 0000000..e450eb7 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/analytical_inverse_kinematics.py @@ -0,0 +1,286 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" Analytical IK for arms and legs based on Section 5.3 of [1] and Section 4.4 of [2] + +[1] Lee, Jehee, and Sung Yong Shin. "A hierarchical approach to interactive motion editing for human-like figures." +Proceedings of the 26th annual conference on Computer graphics and interactive techniques. 1999. +[2] Lucas Kovar, John Schreiner, and Michael Gleicher. "Footskate cleanup for motion capture editing." Proceedings of the 2002 ACM SIGGRAPH/Eurographics symposium on Computer animation. ACM, 2002. + +""" +import math +import numpy as np +import scipy.integrate as integrate +from transformations import quaternion_multiply, quaternion_about_axis, quaternion_matrix, quaternion_from_matrix, quaternion_inverse + + +def normalize(v): + return v/np.linalg.norm(v) + + +def quaternion_from_axis_angle(axis, angle): + q = [1,0,0,0] + q[1] = axis[0] * math.sin(angle / 2) + q[2] = axis[1] * math.sin(angle / 2) + q[3] = axis[2] * math.sin(angle / 2) + q[0] = math.cos(angle / 2) + return normalize(q) + + +def find_rotation_between_vectors(a, b): + """http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another""" + if np.array_equiv(a, b): + return [1, 0, 0, 0] + + axis = normalize(np.cross(a, b)) + dot = np.dot(a, b) + if dot >= 1.0: + return [1, 0, 0, 0] + angle = math.acos(dot) + q = quaternion_from_axis_angle(axis, angle) + return q + +def calculate_angle(upper_limb, lower_limb, ru, rl, target_length): + upper_limb_sq = upper_limb * upper_limb + lower_limb_sq = lower_limb * lower_limb + ru_sq = ru * ru + rl_sq = rl * rl + lusq_rusq = upper_limb_sq - ru_sq + lusq_rusq = max(0, lusq_rusq) + llsq_rlsq = lower_limb_sq - rl_sq + llsq_rlsq = max(0, llsq_rlsq) + temp = upper_limb_sq + rl_sq + temp += 2 * math.sqrt(lusq_rusq) * math.sqrt(llsq_rlsq) + temp += - (target_length*target_length) + temp /= 2 * ru * rl + print("temp",temp) + temp = max(-1, temp) + return math.acos(temp) + + +def calculate_angle2(upper_limb,lower_limb,target_length): + """ get angle between upper and lower limb based on desired length + https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html""" + a = upper_limb + b = lower_limb + c = target_length + temp = (a*a + b*b - c*c) / (2 * a * b) + temp = min(1, temp) + temp = max(-1, temp) + angle = math.acos(temp) + return angle + + +def damp_angle(orig_angle, target_angle, p=0.1*np.pi, a=0.01): + """ src: Kovar et al. [2] Section 4.4. eq. 10 and 11""" + def func(x): + if x < p: + return 1.0 + elif p <= x <= np.pi: + return a * ((x-p)/(np.pi-p)) + else: + return 0.0 + res = integrate.quad(func, orig_angle, target_angle) + return orig_angle + res[0] + + +def to_local_coordinate_system(skeleton, frame, joint_name, q): + """ given a global rotation concatenate it with an existing local rotation and bring it to the local coordinate system""" + # delta*parent*old_local = parent*new_local + # inv_parent*delta*parent*old_local = new_local + if skeleton.nodes[joint_name].parent is not None: + global_m = quaternion_matrix(q) + parent_joint = skeleton.nodes[joint_name].parent.node_name + parent_m = skeleton.nodes[parent_joint].get_global_matrix(frame, use_cache=False) + old_global = np.dot(parent_m, skeleton.nodes[joint_name].get_local_matrix(frame)) + new_global = np.dot(global_m, old_global) + new_local = np.dot(np.linalg.inv(parent_m), new_global) + return quaternion_from_matrix(new_local) + else: + return q + + + + +def calculate_limb_joint_rotation(skeleton, root, joint, end_effector, local_joint_axis, frame, target_position): + """ find angle so the distance from root to end effector is equal to the distance from the root to the target""" + root_pos = skeleton.nodes[root].get_global_position(frame) + joint_pos = skeleton.nodes[joint].get_global_position(frame) + end_effector_pos = skeleton.nodes[end_effector].get_global_position(frame) + + upper_limb = np.linalg.norm(root_pos - joint_pos) + lower_limb = np.linalg.norm(joint_pos - end_effector_pos) + target_length = np.linalg.norm(root_pos - target_position) + #current_length = np.linalg.norm(root_pos - end_effector_pos) + #current_angle = calculate_angle2(upper_limb, lower_limb, current_length) + target_angle = calculate_angle2(upper_limb, lower_limb, target_length) + + joint_delta_angle = np.pi - target_angle + joint_delta_q = quaternion_about_axis(joint_delta_angle, local_joint_axis) + joint_delta_q = normalize(joint_delta_q) + return joint_delta_q + + +def calculate_limb_root_rotation(skeleton, root, end_effector, frame, target_position): + """ find angle between the vectors end_effector - root and target- root """ + + # align vectors + root_pos = skeleton.nodes[root].get_global_position(frame) + end_effector_pos = skeleton.nodes[end_effector].get_global_position(frame) + src_delta = end_effector_pos - root_pos + src_dir = src_delta / np.linalg.norm(src_delta) + + target_delta = target_position - root_pos + target_dir = target_delta / np.linalg.norm(target_delta) + + root_delta_q = find_rotation_between_vectors(src_dir, target_dir) + root_delta_q = normalize(root_delta_q) + + return to_local_coordinate_system(skeleton, frame, root, root_delta_q) + + +class AnalyticalLimbIK(object): + def __init__(self, skeleton, limb_root, limb_joint, end_effector, joint_axis, local_end_effector_dir, damp_angle=None, damp_factor=0.01): + self.skeleton = skeleton + self.limb_root = limb_root + self.limb_joint = limb_joint + self.end_effector = end_effector + self.local_joint_axis = joint_axis + self.local_end_effector_dir = local_end_effector_dir + joint_idx = self.skeleton.animated_joints.index(self.limb_joint) * 4 + 3 + self.joint_indices = [joint_idx, joint_idx + 1, joint_idx + 2, joint_idx + 3] + root_idx = self.skeleton.animated_joints.index(self.limb_root) * 4 + 3 + self.root_indices = [root_idx, root_idx + 1, root_idx + 2, root_idx + 3] + end_effector_idx = self.skeleton.animated_joints.index(self.end_effector) * 4 + 3 + self.end_effector_indices = [end_effector_idx, end_effector_idx + 1, end_effector_idx + 2, end_effector_idx + 3] + self.damp_angle = damp_angle + self.damp_factor = damp_factor + + @classmethod + def init_from_dict(cls, skeleton, joint_name, data, damp_angle=None, damp_factor=None): + limb_root = data["root"] + limb_joint = data["joint"] + joint_axis = data["joint_axis"] + end_effector_dir = data["end_effector_dir"] + return AnalyticalLimbIK(skeleton, limb_root, limb_joint, joint_name, joint_axis, end_effector_dir, damp_angle, damp_factor) + + def calculate_limb_joint_rotation(self, frame, target_position): + """ find angle so the distance from root to end effector is equal to the distance from the root to the target""" + root_pos = self.skeleton.nodes[self.limb_root].get_global_position(frame) + joint_pos = self.skeleton.nodes[self.limb_joint].get_global_position(frame) + end_effector_pos = self.skeleton.nodes[self.end_effector].get_global_position(frame) + + upper_limb = np.linalg.norm(root_pos - joint_pos) + lower_limb = np.linalg.norm(joint_pos - end_effector_pos) + target_length = np.linalg.norm(root_pos - target_position) + current_length = np.linalg.norm(root_pos - end_effector_pos) + current_angle = calculate_angle2(upper_limb, lower_limb, current_length) + target_angle = calculate_angle2(upper_limb, lower_limb, target_length) + if self.damp_angle is not None: + target_angle = damp_angle(current_angle, target_angle, self.damp_angle, self.damp_factor) + #if abs(target_angle - np.pi) < self.min_angle: + # target_angle -= self.min_angle + joint_delta_angle = np.pi - target_angle + joint_delta_q = quaternion_about_axis(joint_delta_angle, self.local_joint_axis) + joint_delta_q = normalize(joint_delta_q) + frame[self.joint_indices] = joint_delta_q + return joint_delta_q + + def calculate_limb_root_rotation(self, frame, target_position): + """ find angle between the vectors end_effector - root and target- root """ + + # align vectors + root_pos = self.skeleton.nodes[self.limb_root].get_global_position(frame) + end_effector_pos = self.skeleton.nodes[self.end_effector].get_global_position(frame) + src_delta = end_effector_pos - root_pos + src_dir = src_delta / np.linalg.norm(src_delta) + + target_delta = target_position - root_pos + target_dir = target_delta / np.linalg.norm(target_delta) + + root_delta_q = find_rotation_between_vectors(src_dir, target_dir) + root_delta_q = normalize(root_delta_q) + + frame[self.root_indices] = self._to_local_coordinate_system(frame, self.limb_root, root_delta_q) + + def _to_local_coordinate_system(self, frame, joint_name, q): + """ given a global rotation concatenate it with an existing local rotation and bring it to the local coordinate system""" + # delta*parent*old_local = parent*new_local + # inv_parent*delta*parent*old_local = new_local + global_m = quaternion_matrix(q) + parent_joint = self.skeleton.nodes[joint_name].parent.node_name + parent_m = self.skeleton.nodes[parent_joint].get_global_matrix(frame, use_cache=False) + old_global = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(frame)) + new_global = np.dot(global_m, old_global) + new_local = np.dot(np.linalg.inv(parent_m), new_global) + return quaternion_from_matrix(new_local) + + def calculate_end_effector_rotation(self, frame, target_dir): + src_dir = self.get_joint_dir(frame, self.end_effector) + global_delta_q = find_rotation_between_vectors(src_dir, target_dir) + new_local_q = self._to_local_coordinate_system(frame, self.end_effector, global_delta_q) + frame[self.end_effector_indices] = new_local_q + + def set_end_effector_rotation(self, frame, target_orientation): + #print "set orientation", target_orientation + q = self.get_global_joint_orientation(self.end_effector, frame) + delta_orientation = quaternion_multiply(target_orientation, quaternion_inverse(q)) + new_local_q = self._to_local_coordinate_system(frame, self.end_effector, delta_orientation) + frame[self.end_effector_indices] = new_local_q + + def get_global_joint_orientation(self, joint_name, frame): + m = self.skeleton.nodes[joint_name].get_global_matrix(frame) + m[:3, 3] = [0, 0, 0] + return normalize(quaternion_from_matrix(m)) + + def get_joint_dir(self, frame, joint_name): + pos1 = self.skeleton.nodes[joint_name].get_global_position(frame) + pos2 = self.skeleton.nodes[joint_name].children[0].get_global_position(frame) + return normalize(pos2 - pos1) + + + def apply(self, frame, position, orientation): + if position is not None: + # 1 calculate joint angle based on the distance to target position + self.calculate_limb_joint_rotation(frame, position) + + # 2 calculate limb root rotation to align the end effector with the target position + self.calculate_limb_root_rotation(frame, position) + + # 3 orient end effector + if orientation is not None: + self.set_end_effector_rotation2(frame, orientation) + return frame + + + def set_end_effector_rotation2(self, frame, target_orientation): + #print("set", target_orientation) + new_local_q = self.to_local_cos2(self.end_effector, frame, target_orientation) + frame[self.end_effector_indices] = new_local_q + + def to_local_cos2(self, joint_name, frame, q): + # bring into parent coordinate system + parent_joint = self.skeleton.nodes[joint_name].parent.node_name + pm = self.skeleton.nodes[parent_joint].get_global_matrix(frame)#[:3, :3] + inv_p = quaternion_inverse(quaternion_from_matrix(pm)) + normalize(inv_p) + return quaternion_multiply(inv_p, q) diff --git a/build/lib/anim_utils/motion_editing/coordinate_cyclic_descent.py b/build/lib/anim_utils/motion_editing/coordinate_cyclic_descent.py new file mode 100644 index 0000000..da675ed --- /dev/null +++ b/build/lib/anim_utils/motion_editing/coordinate_cyclic_descent.py @@ -0,0 +1,353 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from transformations import quaternion_from_matrix, quaternion_matrix, quaternion_slerp, quaternion_about_axis, quaternion_multiply, quaternion_inverse +from ..animation_data.joint_constraints import quaternion_to_axis_angle +from math import acos +import math +from ..animation_data.utils import get_rotation_angle + + +def normalize(v): + return v / np.linalg.norm(v) + + +def to_local_coordinate_system(skeleton, frame, joint_name, q, use_cache=True): + """ given a global rotation bring it to the local coordinate system""" + if skeleton.nodes[joint_name].parent is not None: + global_m = quaternion_matrix(q) + parent_joint = skeleton.nodes[joint_name].parent.node_name + + # delete global matrix of parent_joint but use cache to save time + skeleton.nodes[parent_joint].cached_global_matrix = None + parent_m = skeleton.nodes[parent_joint].get_global_matrix(frame, use_cache=use_cache) + + new_local = np.dot(np.linalg.inv(parent_m), global_m) + return quaternion_from_matrix(new_local) + else: + return q + + +def quaternion_from_vector_to_vector(a, b): + """src: http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another + http://wiki.ogre3d.org/Quaternion+and+Rotation+Primer""" + + v = np.cross(a, b) + w = np.sqrt((np.linalg.norm(a) ** 2) * (np.linalg.norm(b) ** 2)) + np.dot(a, b) + q = np.array([w, v[0], v[1], v[2]]) + if np.dot(q,q) != 0: + return q/ np.linalg.norm(q) + else: + idx = np.nonzero(a)[0] + q = np.array([0, 0, 0, 0]) + q[1 + ((idx + 1) % 2)] = 1 # [0, 0, 1, 0] for a rotation of 180 around y axis + return q + + +def orient_end_effector_to_target(skeleton, root, end_effector, frame, constraint): + """ find angle between the vectors end_effector - root and target- root """ + + # align vectors + root_pos = skeleton.nodes[root].get_global_position(frame) + if constraint.offset is not None: + m = skeleton.nodes[end_effector].get_global_matrix(frame) + end_effector_pos = np.dot(m, constraint.offset)[:3] + else: + end_effector_pos = skeleton.nodes[end_effector].get_global_position(frame) + + src_delta = end_effector_pos - root_pos + src_dir = src_delta / np.linalg.norm(src_delta) + + target_delta = constraint.position - root_pos + target_dir = target_delta / np.linalg.norm(target_delta) + + root_delta_q = quaternion_from_vector_to_vector(src_dir, target_dir) + root_delta_q = normalize(root_delta_q) + + if skeleton.nodes[root].stiffness > 0: + t = 1 - skeleton.nodes[root].stiffness + root_delta_q = quaternion_slerp([1,0,0,0], root_delta_q, t) + root_delta_q = normalize(root_delta_q) + + global_m = quaternion_matrix(root_delta_q) + parent_joint = skeleton.nodes[root].parent.node_name + parent_m = skeleton.nodes[parent_joint].get_global_matrix(frame, use_cache=True) + old_global = np.dot(parent_m, skeleton.nodes[root].get_local_matrix(frame)) + new_global = np.dot(global_m, old_global) + q = quaternion_from_matrix(new_global) + return normalize(q) + + +def orient_node_to_target(skeleton,frame,node_name, end_effector, constraint): + #o = skeleton.animated_joints.index(node_name) * 4 + 3 + o = skeleton.nodes[node_name].quaternion_frame_index * 4 + 3 + q = orient_end_effector_to_target(skeleton, node_name, end_effector, frame, constraint) + q = to_local_coordinate_system(skeleton, frame, node_name, q) + frame[o:o + 4] = q + return frame + + +def apply_joint_constraint(skeleton,frame,node_name): + #o = skeleton.animated_joints.index(node_name) * 4 + 3 + o = skeleton.nodes[node_name].quaternion_frame_index * 4 + 3 + q = np.array(frame[o:o + 4]) + q = skeleton.nodes[node_name].joint_constraint.apply(q) + frame[o:o + 4] = q + return frame + +def set_global_orientation(skeleton, frame, node_name, orientation): + """ set orientation of parent of node assuming node is an end point""" + parent_node = skeleton.nodes[node_name].parent + if parent_node is not None and parent_node.parent is not None: + parent_name = parent_node.node_name + m = quaternion_matrix(orientation) + #o = skeleton.animated_joints.index(node_name) * 4 + 3 + o = skeleton.nodes[parent_name].quaternion_frame_index * 4 + 3 + parent_m = skeleton.nodes[parent_name].parent.get_global_matrix(frame, use_cache=True) + local_m = np.dot(np.linalg.inv(parent_m), m) + q = quaternion_from_matrix(local_m) + frame[o:o + 4] = normalize(q) + return frame + +AXES = ["x", "y"] +def set_tool_orientation(skeleton, frame, node_name, constraint): + """ set orientation of parent of node based on coordinate system assuming node is an end point""" + parent_node = skeleton.nodes[node_name].parent + parent_name = parent_node.node_name + o = parent_node.quaternion_frame_index * 4 + 3 + for a in AXES: + # delete global matrix of parent_joint but use cache to save time + parent_node.cached_global_matrix = None + global_m = parent_node.get_global_matrix(frame, use_cache=True) + #add the offset of the end effector + global_m[:3, 3] += skeleton.nodes[node_name].offset + if a in constraint.src_tool_cos and a in constraint.dest_tool_cos: + tool_axis_offset = constraint.src_tool_cos[a] + global_tool_target_axis = constraint.dest_tool_cos[a] + src_axis = np.dot(global_m, tool_axis_offset)[:3] + delta_q = quaternion_from_vector_to_vector(src_axis, global_tool_target_axis) + delta_q = normalize(delta_q) + delta_m = quaternion_matrix(delta_q) + global_m = np.dot(delta_m, global_m) + q = quaternion_from_matrix(global_m) + q = to_local_coordinate_system(skeleton, frame, parent_name, q) + frame[o:o + 4] = normalize(q) + return frame + +def run_ccd(skeleton, frame, end_effector_name, constraint, eps=0.01, n_max_iter=50, chain_end_joint=None, verbose=False): + pos = skeleton.nodes[end_effector_name].get_global_position(frame) + error = np.linalg.norm(constraint.position-pos) + prev_error = error + n_iters = 0 + while error > eps and n_iters < n_max_iter: + node = skeleton.nodes[end_effector_name].parent + depth = 0 + + while node is not None and node.node_name != chain_end_joint and node.parent is not None: + static = False + if node.joint_constraint is not None and node.joint_constraint.is_static: + static = True + + if not static: + frame = orient_node_to_target(skeleton,frame, node.node_name, end_effector_name, constraint) + if constraint.orientation is not None: + frame = set_global_orientation(skeleton, frame, end_effector_name, constraint.orientation) + elif constraint.src_tool_cos is not None and constraint.dest_tool_cos is not None: + frame = set_tool_orientation(skeleton, frame, end_effector_name, constraint) + + if node.joint_constraint is not None: + frame = apply_joint_constraint(skeleton, frame, node.node_name) + node = node.parent + depth += 1 + + if constraint.offset is not None: + m = skeleton.nodes[end_effector_name].get_global_matrix(frame) + end_effector_pos = np.dot(m, constraint.offset)[:3] + else: + end_effector_pos = skeleton.nodes[end_effector_name].get_global_position(frame) + + error = np.linalg.norm(constraint.position - end_effector_pos) + n_iters += 1 + + if verbose: + print("error at", n_iters, ":", error, "c:",constraint.position,"pos:", pos, chain_end_joint) + return frame, error + + +LOOK_AT_DIR = [0, -1,0] +SPINE_LOOK_AT_DIR = [0,0,1] + +def look_at_target(skeleton, root, end_effector, frame, position, local_dir=LOOK_AT_DIR): + """ find angle between the look direction and direction between end effector and target""" + #direction of endeffector + m = skeleton.nodes[end_effector].get_global_matrix(frame) + #offset = skeleton.nodes[end_effector].offset + end_effector_dir = np.dot(m[:3,:3], local_dir) + end_effector_dir = end_effector_dir / np.linalg.norm(end_effector_dir) + + # direction from endeffector to target + end_effector_pos = m[:3, 3] + target_delta = position - end_effector_pos + target_dir = target_delta / np.linalg.norm(target_delta) + + # find rotation to align vectors + root_delta_q = quaternion_from_vector_to_vector(end_effector_dir, target_dir) + root_delta_q = normalize(root_delta_q) + + #apply global delta to get new global matrix of joint + global_m = quaternion_matrix(root_delta_q) + parent_joint = skeleton.nodes[root].parent.node_name + parent_m = skeleton.nodes[parent_joint].get_global_matrix(frame, use_cache=True) + old_global = np.dot(parent_m, skeleton.nodes[root].get_local_matrix(frame)) + new_global = np.dot(global_m, old_global) + q = quaternion_from_matrix(new_global) + return normalize(q) + + + +def orient_node_to_target_look_at(skeleton,frame,node_name, end_effector, position, local_dir=LOOK_AT_DIR): + o = skeleton.nodes[node_name].quaternion_frame_index * 4 + 3 + q = look_at_target(skeleton, node_name, end_effector, frame, position, local_dir) + q = to_local_coordinate_system(skeleton, frame, node_name, q) + frame[o:o + 4] = q + return frame + + + +def run_ccd_look_at(skeleton, frame, end_effector_name, position, eps=0.01, n_max_iter=1, local_dir=LOOK_AT_DIR, chain_end_joint=None, verbose=False): + error = np.inf + n_iter = 0 + while error > eps and n_iter < n_max_iter: + node = skeleton.nodes[end_effector_name].parent + depth = 0 + while node is not None and node.node_name != chain_end_joint and node.parent is not None:#and node.node_name != skeleton.root: + frame = orient_node_to_target_look_at(skeleton,frame, node.node_name, end_effector_name, position, local_dir) + if node.joint_constraint is not None: + frame = apply_joint_constraint(skeleton, frame, node.node_name) + node = node.parent + depth += 1 + + m = skeleton.nodes[end_effector_name].get_global_matrix(frame) + + end_effector_dir = np.dot(m[:3, :3], local_dir) + end_effector_dir = end_effector_dir / np.linalg.norm(end_effector_dir) + + # direction from endeffector to target + end_effector_pos = m[:3,3] + target_delta = position - end_effector_pos + target_dir = target_delta / np.linalg.norm(target_delta) + root_delta_q = quaternion_from_vector_to_vector(end_effector_dir, target_dir) + root_delta_q = normalize(root_delta_q) + v, a = quaternion_to_axis_angle(root_delta_q) + error = abs(a) + #error = np.linalg.norm(target_dir-end_effector_dir) + #print(error) + n_iter+=1 + if verbose: + print("error at", n_iter, ":", error, "c:",position,"pos:") + + return frame, error + + + + +def look_at_target_projected(skeleton, root, end_effector, frame, position, local_dir=LOOK_AT_DIR): + """ find angle between the look direction and direction from end effector to target projected on the xz plane""" + #direction of endeffector + m = skeleton.nodes[root].get_global_matrix(frame) + end_effector_dir = np.dot(m[:3,:3], local_dir) + end_effector_dir[1] = 0 + end_effector_dir = end_effector_dir / np.linalg.norm(end_effector_dir) + + # direction from endeffector to target + end_effector_pos = m[:3, 3] + target_delta = position - end_effector_pos + target_delta[1] = 0 + target_dir = target_delta / np.linalg.norm(target_delta) + + # find rotation to align vectors + root_delta_q = quaternion_from_vector_to_vector(end_effector_dir, target_dir) + root_delta_q = normalize(root_delta_q) + #apply global delta to get new global matrix of joint + global_m = quaternion_matrix(root_delta_q) + old_global = skeleton.nodes[root].get_global_matrix(frame) + new_global = np.dot(global_m, old_global) + q = quaternion_from_matrix(new_global) + return normalize(q) + +def quaternion_to_axis_angle(q): + """http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/ + + """ + a = 2* math.acos(q[0]) + s = math.sqrt(1- q[0]*q[0]) + if s < 0.001: + x = q[1] + y = q[2] + z = q[3] + else: + x = q[1] / s + y = q[2] / s + z = q[3] / s + v = np.array([x,y,z]) + if np.sum(v)> 0: + return normalize(v),a + else: + return v, a + +def swing_twist_decomposition(q, twist_axis): + """ code by janis sprenger based on + Dobrowsolski 2015 Swing-twist decomposition in Clifford algebra. https://arxiv.org/abs/1506.05481 + """ + q = normalize(q) + #twist_axis = np.array((q * offset))[0] + projection = np.dot(twist_axis, np.array([q[1], q[2], q[3]])) * twist_axis + twist_q = np.array([q[0], projection[0], projection[1],projection[2]]) + if np.linalg.norm(twist_q) == 0: + twist_q = np.array([1,0,0,0]) + twist_q = normalize(twist_q) + swing_q = quaternion_multiply(q, quaternion_inverse(twist_q))#q * quaternion_inverse(twist) + return swing_q, twist_q + + +def orient_node_to_target_look_at_projected(skeleton, frame,node_name, end_effector, position, local_dir=LOOK_AT_DIR, twist_axis=None, max_angle=None): + o = skeleton.nodes[node_name].quaternion_frame_index * 4 + 3 + q = look_at_target_projected(skeleton, node_name, end_effector, frame, position, local_dir) + q = to_local_coordinate_system(skeleton, frame, node_name, q) + + if max_angle is not None and twist_axis is not None: + t_min_angle = np.radians(-max_angle) + t_max_angle = np.radians(max_angle) + swing_q, twist_q = swing_twist_decomposition(q, twist_axis) + v, a = quaternion_to_axis_angle(twist_q) + sign = 1 + if np.sum(v) < 0: + sign = -1 + a *= sign + a = max(a, t_min_angle) + a = min(a, t_max_angle) + new_twist_q = quaternion_about_axis(a, twist_axis) + q = quaternion_multiply(swing_q, new_twist_q) + q = normalize(q) + frame[o:o + 4] = q + return frame diff --git a/build/lib/anim_utils/motion_editing/cubic_motion_spline.py b/build/lib/anim_utils/motion_editing/cubic_motion_spline.py new file mode 100644 index 0000000..c0a5944 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/cubic_motion_spline.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from scipy.interpolate import interp1d +from transformations import quaternion_multiply, quaternion_inverse + + +def get_quaternion_delta(a, b): + return quaternion_multiply(quaternion_inverse(b), a) + + +def add_frames(skeleton, a, b): + """ returns c = a + b""" + #print("add frames", len(a), len(b)) + c = np.zeros(len(a)) + c[:3] = a[:3] + b[:3] + for idx, j in enumerate(skeleton.animated_joints): + o = idx * 4 + 3 + q_a = a[o:o + 4] + q_b = b[o:o + 4] + #print(q_a,q_b) + q_prod = quaternion_multiply(q_a, q_b) + c[o:o + 4] = q_prod / np.linalg.norm(q_prod) + return c + + +def substract_frames(skeleton, a, b): + """ returns c = a - b""" + c = np.zeros(len(a)) + c[:3] = a[:3] - b[:3] + for idx, j in enumerate(skeleton.animated_joints): + o = idx*4 + 3 + q_a = a[o:o+4] + q_b = b[o:o+4] + q_delta = get_quaternion_delta(q_a, q_b) + c[o:o+4] = q_delta / np.linalg.norm(q_delta) + return c + + +def generate_knots(n_frames, n_knots): + """https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.LSQUnivariateSpline.html""" + knots = np.linspace(0, n_frames - 1, n_knots) + return knots + + +class CubicMotionSpline(object): + def __init__(self, skeleton, splines, n_dims): + self.skeleton = skeleton + self.n_dims = n_dims + self.splines = splines + + @classmethod + def fit_frames(cls, skeleton, times, frames, kind='cubic'): + if type(frames) is np.ndarray: + n_frames, n_dims = frames.shape + else: + n_dims = 1 + splines = [] + for d in range(n_dims): + s = interp1d(times, frames[:, d], kind=kind) + splines.append(s) + return CubicMotionSpline(skeleton, splines, n_dims) + + def evaluate(self, t): + return np.asarray([s(t) for s in self.splines]).T + + def get_displacement_map(self, frames): + d = [] + for idx, f in enumerate(frames): + s_f = self.evaluate(idx) + d.append(substract_frames(self.skeleton, f, s_f)) + return np.array(d) + + def to_frames(self, times): + frames = [] + for t in times: + frame = self.evaluate(t) + frames.append(frame) + return frames + + def add_spline_discrete(self, other, times): + new_frames = [] + for t in times: + f = add_frames(self.skeleton, self.evaluate(t), other.evaluate(t)) + new_frames.append(f) + new_frames = np.array(new_frames) + splines = [] + for d in range(self.n_dims): + s = interp1d(times, new_frames[:, d], kind='cubic') + splines.append(s) + self.splines = splines + + def plot(self, x): + import matplotlib.pyplot as plt + for s in self.splines: + plt.plot(x, s(x)) + plt.show() diff --git a/build/lib/anim_utils/motion_editing/fabrik_chain.py b/build/lib/anim_utils/motion_editing/fabrik_chain.py new file mode 100644 index 0000000..be358f2 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/fabrik_chain.py @@ -0,0 +1,551 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" + https://www.sciencedirect.com/science/article/pii/S1524070311000178?via%3Dihub + + based on the pseudocode by Renzo Poddighe + https://project.dke.maastrichtuniversity.nl/robotlab/wp-content/uploads/Bachelor-thesis-Renzo-Poddighe.pdf +""" +import math +import numpy as np +from transformations import quaternion_inverse, quaternion_multiply, quaternion_from_matrix, euler_from_quaternion +from .analytical_inverse_kinematics import calculate_limb_joint_rotation, calculate_limb_root_rotation, to_local_coordinate_system + +def sign(x): + return 1 if x >= 0 else -1 + +def quaternion_to_av(q): + """ according to lee 2000 + the purely imaginary quaternion is identical to the angular velocity + the sign of the real part gives the direction + Since the unit quaternion space is folded by the antipodal equivalence, + the angular velocity is twice as fast + """ + return 2 * np.array(q[1:]) * sign(q[0]) + +def normalize(v): + return v/ np.linalg.norm(v) + +def get_quaternion_delta(a, b): + return quaternion_multiply(quaternion_inverse(b), a) + + +def quaternion_from_axis_angle(axis, angle): + q = [1,0,0,0] + q[1] = axis[0] * math.sin(angle / 2) + q[2] = axis[1] * math.sin(angle / 2) + q[3] = axis[2] * math.sin(angle / 2) + q[0] = math.cos(angle / 2) + return normalize(q) + + + +def get_offset_quat(a, b): + a_len = np.linalg.norm(a) + b_len = np.linalg.norm(b) + if a_len > 0 and b_len > 0: + q = quaternion_from_vector_to_vector(a/a_len,b/b_len) + q /= np.linalg.norm(q) + return q + else: + return [1,0,0,0] + +def quaternion_from_vector_to_vector(a, b): + """src: http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another + http://wiki.ogre3d.org/Quaternion+and+Rotation+Primer""" + + v = np.cross(a, b) + w = np.sqrt((np.linalg.norm(a) ** 2) * (np.linalg.norm(b) ** 2)) + np.dot(a, b) + q = np.array([w, v[0], v[1], v[2]]) + if np.dot(q,q) != 0: + return q/ np.linalg.norm(q) + else: + idx = np.nonzero(a)[0] + q = np.array([0, 0, 0, 0]) + q[1 + ((idx + 1) % 2)] = 1 # [0, 0, 1, 0] for a rotation of 180 around y axis + return q + +def quaternion_from_vector_to_vector2(a, b): + """http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another""" + if np.array_equiv(a, b): + return [1, 0, 0, 0] + + axis = normalize(np.cross(a, b)) + dot = np.dot(a, b) + if dot >= 1.0: + return [1, 0, 0, 0] + angle = math.acos(dot) + q = quaternion_from_axis_angle(axis, angle) + return q + +def to_local_cos(skeleton, node_name, frame, q): + # bring into parent coordinate system + pm = skeleton.nodes[node_name].get_global_matrix(frame)[:3,:3] + inv_p = quaternion_inverse(quaternion_from_matrix(pm)) + inv_p /= np.linalg.norm(inv_p) + return quaternion_multiply(inv_p, q) + + +def orient_joint_to_target(skeleton, node, frame, src_pos, target_pos): + if skeleton.nodes[node].parent is None: + parent_pos = [0, 0, 0] + else: + parent_pos = skeleton.nodes[node].parent.get_global_position(frame) + src_dir = normalize(src_pos - parent_pos) + target_dir = normalize(target_pos - parent_pos) + delta_q = quaternion_from_vector_to_vector(src_dir, target_dir) + return normalize(delta_q) + + +class FABRIKBone(object): + def __init__(self, name, child): + self.name = name + self.child = child + self.position = np.array([0, 0, 0], np.float32) # position of joint + self.length = 0 + self.is_root = False + self.is_leaf = False + + +ROOT_OFFSET = np.array([0,0,0], np.float32) + +class FABRIKChain(object): + def __init__(self, skeleton, bones, node_order, tolerance=0.01, delta_tolerance=0.0001, max_iter=500, frame_offset=3, root_offset=ROOT_OFFSET, activate_constraints=False): + self.skeleton = skeleton + self.bones = bones + self.node_order = node_order + self.reversed_node_order = list(reversed(node_order)) + self.tolerance = tolerance + self.max_iter = max_iter + self.target = None + self.root_pos = None + self.chain_length = 0 + self.root_offset = root_offset + self.activate_constraints = activate_constraints + self.frame_offset = frame_offset + self.delta_tolerance = delta_tolerance + + def set_positions_from_frame(self, frame, parent_length): + self.skeleton.clear_cached_global_matrices() + for idx, node in enumerate(self.node_order): + p = self.skeleton.nodes[node].get_global_position(frame, use_cache=True) + #print("pos ", node, p) + self.bones[node].position = p + if idx ==0: + self.root_pos = p + self.chain_length = 0 + for node in self.node_order: + next_node = self.bones[node].child + if next_node is None: + break + d = np.linalg.norm(self.bones[next_node].position - self.bones[node].position) + self.bones[node].length = d + #print("length ",node, d) + self.chain_length += d + self.parent_length = parent_length + + def target_is_reachable(self): + dist = np.linalg.norm(self.target - self.root_pos) + #print("unreachable", dist, self.chain_length) + return dist < self.chain_length+ self.parent_length + + def run(self, frame, target): + self.target = target + self.set_positions_from_frame(frame, 0) + self.solve() + return self.get_joint_parameters() + + def run_partial(self, frame, target): + self.target = target + self.set_positions_from_frame(frame, 0) + self.solve() + return self.set_partial_joint_parameters(frame) + + def run_partial_with_constraints(self, frame, target): + self.target = target + self.set_positions_from_frame(frame, 0) + self.solve_partial(frame) + return self.set_partial_joint_parameters(frame) + + def run_with_constraints(self, frame, target): + self.target = target + self.set_positions_from_frame(frame, 0) + self.solve_with_constraints() + return self.get_joint_parameters() + + def solve(self): + if not self.target_is_reachable(): + print("unreachable") + # if unreachable orient joints to target + self.orient_to_target() + else: + print("reachable") + # if reachable perform forward and backward reaching until tolerance is reached + iter = 0 + distance = self.get_error() + while distance > self.tolerance and iter< self.max_iter: + self.backward() + self.forward() + iter+=1 + distance = self.get_error() + print("iter",iter, distance) + + def solve_with_constraints(self): + if not self.target_is_reachable(): + print("unreachable") + # if unreachable orient joints to target + self.orient_to_target() + else: + print("reachable") + # if reachable perform forward and backward reaching until tolerance is reached + iter = 0 + distance = self.get_error() + while distance > self.tolerance and iter < self.max_iter: + self.backward() + self.forward() + self.apply_constraints() + iter+=1 + distance = self.get_error() + print("iter",iter, distance) + + def solve_partial(self, frame): + if not self.target_is_reachable(): + print("unreachable") + # if unreachable orient joints to target + self.orient_to_target() + else: + print("reachable") + # if reachable perform forward and backward reaching until tolerance is reached + iter = 0 + distance = self.get_error() + while distance > self.tolerance and iter < self.max_iter: + self.backward() + self.forward() + + self.set_partial_joint_parameters(frame) + self.set_positions_from_frame(frame, 0) + + iter += 1 + distance = self.get_error() + print("iter", iter, distance) + + def get_error(self): + end_effector = self.node_order[-1] + return np.linalg.norm(self.bones[end_effector].position - self.target) + + def orient_to_target(self): + for idx, node in enumerate(self.node_order[:-1]): + next_node = self.bones[node].child + if next_node is None: + print("Error: none at ",node) + break + r = np.linalg.norm(self.target - self.bones[node].position) + if r > 0: + l = self.bones[node].length / r + self.bones[next_node].position = (1 - l) * self.bones[node].position + l * self.target + + def backward(self): + end_effector = self.node_order[-1] + self.bones[end_effector].position = np.array(self.target) + n_points = len(self.node_order) + for idx in range(n_points - 2, -1, -1): + node = self.node_order[idx] + next_node = self.bones[node].child + r = np.linalg.norm(self.bones[next_node].position - self.bones[node].position) + if r > 0: + l = self.bones[node].length / r + self.bones[node].position = (1 - l) * self.bones[next_node].position + l * self.bones[node].position + + def forward(self): + root_node = self.node_order[0] + self.bones[root_node].position = self.root_pos + for idx, node in enumerate(self.node_order[:-1]): #for p_idx in range(0, self.n_points - 1, 1): + #next_node = self.node_order[idx + 1] + next_node = self.bones[node].child + r = np.linalg.norm(self.bones[next_node].position - self.bones[node].position) + if r > 0: + l = self.bones[node].length / r + self.bones[next_node].position = l * self.bones[next_node].position + (1 - l) * self.bones[node].position + + def apply_constraints(self): + frame = self.get_joint_parameters() + self.set_positions_from_frame(frame, 0) + return + + def get_joint_parameters(self): + n_joints = len(self.node_order) - 1 + frame = np.zeros(n_joints*4+3) + o = 3 + prev_point = self.root_offset + for idx, node in enumerate(self.node_order[:-1]): + #for node in self.skeleton.animated_joints: + next_node = self.bones[node].child + q = self.get_global_rotation(node, next_node) + frame[o:o + 4] = to_local_cos(self.skeleton, node, frame, q) + if self.skeleton.nodes[node].joint_constraint is not None and self.activate_constraints: + self.apply_constraint_with_swing(node, frame, o) + prev_point = self.bones[next_node].position + o += 4 + return frame + + def set_partial_joint_parameters(self, frame): + o = self.frame_offset + for idx, node in enumerate(self.node_order[:-1]): + next_node = self.bones[node].child + q = self.get_global_rotation_non_cos(node, next_node, frame) + frame[o:o + 4] = to_local_coordinate_system(self.skeleton,frame, node, q) + if self.skeleton.nodes[node].joint_constraint is not None and self.activate_constraints: + self.apply_constraint_with_swing(node, frame, o) + o += 4 + return frame + + def apply_constraint_with_swing(self, node, frame, o, eps=0.01): + old_q = np.array(frame[o:o + 4]) + + #remove twist rotation + swing_q, twist_q = self.skeleton.nodes[node].joint_constraint.split(old_q) + frame[o:o + 4] = twist_q + + # apply swing_q to parent + parent_q = np.array(frame[o - 4:o]) + new_parent_q = quaternion_multiply(parent_q, swing_q) + new_parent_q = normalize(new_parent_q) + frame[o - 4:o] = new_parent_q + new_node_pos = self.skeleton.nodes[node].children[0].get_global_position(frame) + diff = np.linalg.norm(new_node_pos - self.target) + + # calculate rotation fix if necessary + if diff > eps: + delta_q = orient_joint_to_target(self.skeleton, node, frame, new_node_pos, self.target) + aligned_parent_q = quaternion_multiply(delta_q, new_parent_q) + aligned_parent_q = normalize(aligned_parent_q) + frame[o - 4:o] = aligned_parent_q + + def apply_constraint_with_swing_global(self, node, frame, o, eps=0.01): + old_q = np.array(frame[o:o + 4]) + next_node = self.bones[node].child + parent_m = self.skeleton.nodes[node].get_global_matrix(frame)[:3, :3] + node_m = self.skeleton.nodes[next_node].get_global_matrix(frame)[:3, :3] + node_q = quaternion_from_matrix(node_m) + node_q = normalize(node_q) + # remove twist rotation + swing_q, twist_q = self.skeleton.nodes[node].joint_constraint.split_global(parent_m, node_q) + frame[o:o + 4] = twist_q + + # apply swing_q to parent + parent_q = np.array(frame[o - 4:o]) + new_parent_q = quaternion_multiply(parent_q, swing_q) + new_parent_q = normalize(new_parent_q) + frame[o - 4:o] = new_parent_q + new_node_pos = self.skeleton.nodes[node].children[0].get_global_position(frame) + diff = np.linalg.norm(new_node_pos - self.target) + return + # calculate rotation fix if necessary + if diff > eps: + delta_q = orient_joint_to_target(self.skeleton, node, frame, new_node_pos, self.target) + aligned_parent_q = quaternion_multiply(delta_q, new_parent_q) + aligned_parent_q = normalize(aligned_parent_q) + frame[o - 4:o] = aligned_parent_q + + + def apply_constraint_with_swing_and_lee(self, node, frame, o, eps=0.01): + old_q = frame[o:o + 4] + old_node_pos = self.skeleton.nodes[node].children[0].get_global_position(frame) + delta_q, new_q = self.skeleton.nodes[node].joint_constraint.split(old_q) + frame[o:o + 4] = new_q + new_node_pos = self.skeleton.nodes[node].children[0].get_global_position(frame) + diff = np.linalg.norm(new_node_pos - old_node_pos) + if diff > eps: + parent_q = frame[o - 4:o] + new_parent_q = quaternion_multiply(parent_q, delta_q) + new_parent_q = normalize(new_parent_q) + frame[o - 4:o] = new_parent_q + new_node_pos = self.skeleton.nodes[node].children[0].get_global_position(frame) + diff = np.linalg.norm(new_node_pos -old_node_pos) + if diff > eps: + parent_q = frame[o - 4:o] + root = None + if self.skeleton.nodes[node].parent is not None: + root = self.skeleton.nodes[node].parent.node_name + end_effector = self.skeleton.nodes[node].children[0].node_name + print("apply lee tolani",root, node, end_effector, diff) + local_axis = self.skeleton.nodes[node].joint_constraint.axis + #frame[o:o + 4] = calculate_limb_joint_rotation(self.skeleton, root, node, end_effector, local_axis, frame, self.target) + delta_q = calculate_limb_root_rotation(self.skeleton, root, end_effector, frame, self.target) + new_parent_q = quaternion_multiply(parent_q, delta_q) + new_parent_q = normalize(new_parent_q) + frame[o - 4:o] = new_parent_q + + def apply_constraint_with_swing2(self, node, parent_node, frame, o): + next_node = self.bones[node].child + target_global_m = self.skeleton.nodes[next_node].get_global_matrix(frame)[:3,:3] + old_q = frame[o:o + 4] + delta_q, new_q = self.skeleton.nodes[node].joint_constraint.split(old_q) + frame[o:o + 4] = new_q + new_global_m = self.skeleton.nodes[next_node].get_global_matrix(frame)[:3,:3] + delta_global_m = np.dot(np.linalg.inv(new_global_m), target_global_m) + actual_delta_q = normalize(quaternion_from_matrix(delta_global_m)) + parent_q = frame[o - 4:o] + new_parent_q = quaternion_multiply(actual_delta_q, parent_q) + new_parent_q = normalize(new_parent_q) + frame[o - 4:o] = new_parent_q + + + + def apply_constraint_with_vector(self, node, parent_node, frame, o): + next_node = self.bones[node].child + old_pos = self.bones[next_node].position + old_q = frame[o:o + 4] + twist_q, swing_q = self.skeleton.nodes[node].joint_constraint.split(old_q) + frame[o:o + 4] = swing_q + + # get global delta quaternion to apply on parent + parent_pos = self.skeleton.nodes[parent_node].get_global_position(frame) + next_node_pos = self.skeleton.nodes[next_node].get_global_position(frame) + position_delta = np.linalg.norm(old_pos-next_node_pos) + print("position delta", position_delta) + if position_delta < 0.001: + return + desired_offset = normalize(old_pos - parent_pos) + offset = normalize(next_node_pos - parent_pos) + delta_q = quaternion_from_vector_to_vector(offset, desired_offset) + print("deltaq", parent_node, node, next_node, next_node_pos, old_pos, twist_q, swing_q) + # apply global delta on parent + if True: + global_m = self.skeleton.nodes[parent_node].get_global_matrix(frame) + global_parent_q = normalize(quaternion_from_matrix(global_m)) + new_parent_q = quaternion_multiply(global_parent_q, delta_q) + new_parent_q = normalize(new_parent_q) + frame[o - 4:o] = to_local_cos(self.skeleton, parent_node, frame, new_parent_q) + else: + local_parent_q = frame[o - 4:o] + local_delta = to_local_cos(self.skeleton, parent_node, frame, delta_q) + + new_parent_q = quaternion_multiply(local_parent_q, local_delta) + new_parent_q = normalize(new_parent_q) + frame[o - 4:o] = new_parent_q + print(new_parent_q, local_parent_q, local_delta, delta_q) + + + + def get_global_rotation(self, node, next_node): + """ FIXME works only when identity frame coordinate system is the same as the offset """ + + #print("set", node, next_node) + target = self.bones[next_node].position - self.bones[node].position + next_offset = np.array(self.skeleton.nodes[next_node].offset) + target_len = np.linalg.norm(target) + if target_len > 0: + target /= target_len + next_offset /= np.linalg.norm(next_offset) + + # 1. sum over offsets of static nodes + local_offset = self.get_child_offset(node, next_node) + actual_offset = next_offset + local_offset + actual_offset /= np.linalg.norm(actual_offset) # actual_offset = [0.5, 0.5,0] + # 2. get global rotation + q = quaternion_from_vector_to_vector(actual_offset, target) + return q + + else: + #print("skip", target_len, self.bones[next_node].position) + return [1, 0, 0, 0] + + def get_global_rotation_non_cos(self, node, next_node, frame): + target_position = self.bones[next_node].position + root_pos = self.skeleton.nodes[node].get_global_position(frame) + #src_dir = np.dot(m, offset) + end_effector_pos = self.skeleton.nodes[next_node].get_global_position(frame) + src_delta = end_effector_pos - root_pos + src_dir = src_delta / np.linalg.norm(src_delta) + + target_delta = target_position - root_pos + target_dir = target_delta / np.linalg.norm(target_delta) + + #q = quaternion_from_vector_to_vector(offset, target_dir) + q = quaternion_from_vector_to_vector2(src_dir, target_dir) + q = normalize(q) + return q + + + def get_child_offset(self, node, child_node): + """ + """ + actual_offset = np.array([0, 0, 0], np.float) + while node is not None and self.skeleton.nodes[node].children[0].node_name != child_node: + local_offset = np.array(self.skeleton.nodes[node].children[0].offset) + local_offset /= np.linalg.norm(local_offset) + actual_offset = actual_offset + local_offset + node = self.skeleton.nodes[node].children[0].node_name + if len(self.skeleton.nodes[node].children) < 1: + node = None + + return actual_offset + + def get_joint_parameters_global(self): + n_joints = len(self.node_order)-1 + frame = np.zeros(n_joints*4+3) + o = self.frame_offset + for idx, node in enumerate(self.node_order[:-1]): + offset = np.array(self.skeleton.nodes[node].children[0].offset) + offset /= np.linalg.norm(offset) + next_node = self.bones[node].child + if idx == 0: + if self.skeleton.root == node: + dir_vector = self.bones[next_node].position + dir_vector_len = np.linalg.norm(dir_vector) + if dir_vector_len > 0 and np.linalg.norm(offset) > 0: + dir_vector /= dir_vector_len + q = quaternion_from_vector_to_vector(offset, dir_vector) + frame[o:o + 4] = q + else: + print("work around", offset,dir_vector_len, node) + frame[o:o + 4] = [1, 0, 0, 0] + else: + print("work root around") + frame[o:o + 4] = [1, 0, 0, 0] + + else: + q = self.get_global_rotation(node, next_node) + frame[o:o+4] =q + o += 4 + print(frame) + return frame + + def get_global_positions(self): + position_dict = dict() + for node in self.node_order: + position_dict[node] = self.bones[node].position + return position_dict + + + def get_end_effector_position(self): + root_node = self.node_order[-1] + return self.bones[root_node].position + + def get_next_nodes(self, next_nodes): + for idx, n in enumerate(self.node_order): + if idx+1 < len(self.node_order): + next_nodes[n] = self.node_order[idx+1] + else: + next_nodes[n] = None diff --git a/build/lib/anim_utils/motion_editing/fabrik_node.py b/build/lib/anim_utils/motion_editing/fabrik_node.py new file mode 100644 index 0000000..b8aea92 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/fabrik_node.py @@ -0,0 +1,316 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from .fabrik_chain import FABRIKChain, to_local_cos, quaternion_from_vector_to_vector, FABRIKBone, ROOT_OFFSET + + +def get_child_offset(skeleton, node, child_node): + actual_offset = np.array([0, 0, 0], np.float) + while node is not None and skeleton.nodes[node].children[0].node_name != child_node: + local_offset = np.array(skeleton.nodes[node].children[0].offset) + local_offset_len = np.linalg.norm(local_offset) + if local_offset_len > 0: + local_offset /= local_offset_len + actual_offset = actual_offset + local_offset + node = skeleton.nodes[node].children[0].node_name + if len(skeleton.nodes[node].children) < 1: + node = None + else: + node = None + return actual_offset + + +def get_global_joint_parameters_from_positions(skeleton, positions, next_nodes): + n_parameters = len(skeleton.animated_joints)*4+3 + frame = np.zeros(n_parameters) + frame[:3] = positions[skeleton.root] + o = 3 + print(skeleton.animated_joints) + print(next_nodes.keys()) + print(positions.keys()) + for node in skeleton.animated_joints: + next_node = next_nodes[node] + if next_node in positions: + next_offset = np.array(skeleton.nodes[next_node].offset) + next_offset /= np.linalg.norm(next_offset) + + # 1. sum over offsets of static nodes + local_offset = get_child_offset(skeleton, node, next_node) + #print(node, local_offset) + actual_offset = next_offset + local_offset + actual_offset /= np.linalg.norm(actual_offset) # actual_offset = [0.5, 0.5,0] + + + dir_to_child = positions[next_node] - positions[node] + dir_to_child /= np.linalg.norm(dir_to_child) + + # 2. get global rotation + global_q = quaternion_from_vector_to_vector(actual_offset, dir_to_child) + frame[o:o + 4] = global_q + else: + #print("ignore", node, next_node) + frame[o:o + 4] = [1,0,0,0] + o += 4 + return frame + + +def global_to_local_frame(skeleton, global_frame): + n_parameters = len(skeleton.animated_joints)*4+3 + frame = np.zeros(n_parameters) + o = 3 + for node in skeleton.animated_joints: + frame[o:o + 4] = to_local_cos(skeleton, node, frame, global_frame[o:o + 4]) + + o += 4 + + return frame + + +class FABRIKNode(object): + def __init__(self, skeleton, root, parent_chain=None, tolerance=0.01, max_iter=100, root_offset=ROOT_OFFSET): + self.root_pos = np.array([0,0,0], dtype=np.float) + self.skeleton = skeleton + self.n_parameters = len(self.skeleton.animated_joints)*4+3 + self.tolerance = tolerance + self.max_iter = max_iter + self.parent_chain = parent_chain + self.root = root + self.root_offset = root_offset + self.child_nodes = list() + # self.target = None + # self.position = None + self.is_leaf = True + self.construct_from_skeleton(skeleton, root, tolerance, max_iter) + + def construct_from_skeleton(self, skeleton, root, tolerance, max_iter): + """ there need to be two end effectors""" + children = [c.node_name for c in skeleton.nodes[root].children] + for c in children: + self.is_leaf = False + current_node = c + node_order = [self.root] + while current_node is not None: + n_children = len(skeleton.nodes[current_node].children) + if n_children == 1: # append to chain + child_node = skeleton.nodes[current_node].children[0].node_name + # only add list to joints + if not skeleton.nodes[current_node].fixed: + node_order.append(current_node)# skip fixed nodes + current_node = child_node + else: # stop chain # split up by adding child nodes + if n_children > 0: + node_order.append(current_node) + bones = dict() + for idx, node in enumerate(node_order): + child_node = None + if idx+1 < len(node_order): + child_node = node_order[idx + 1] + bones[node] = FABRIKBone(node, child_node) + if idx == 0 and self.parent_chain is None : + bones[node].is_root = True + else: + bones[node].is_root = False + parent_chain = FABRIKChain(skeleton, bones, node_order) + print("construct node at",self.root , current_node, node_order) + node = FABRIKNode(skeleton, current_node, parent_chain, tolerance, max_iter) + self.child_nodes.append(node) + current_node = None + + def backward_stage(self): + for c in self.child_nodes: + c.backward_stage() # calculate backward update of children of child to start at the end effectors + + if not self.is_leaf: + positions = [c.get_chain_root_position() for c in self.child_nodes] + t = np.mean(positions, axis=0) + #print("centroid",t) + #print("no leaf") + else: + t = self.parent_chain.target + #print("leaf") + + if self.parent_chain is not None: + self.parent_chain.target = t + if self.target_is_reachable(): + self.parent_chain.backward() + else: + print("unreachable") + # if unreachable orient joints to target + self.parent_chain.orient_to_target() + + def target_is_reachable(self): + return self.parent_chain.target_is_reachable() + + def forward_stage(self): + # calculate forward update of parent chain + if self.parent_chain is not None: + #if self.parent_chain.target_is_reachable(): + self.parent_chain.forward() + #else: + # self.parent_chain.orient_to_target() + + # calculate forward update of children of child + for c in self.child_nodes: + if self.parent_chain is not None: + c.parent_chain.root_pos = self.parent_chain.get_end_effector_position() + else: + c.parent_chain.root_pos = self.root_pos + c.forward_stage() + + def get_chain_root_position(self): + root_node = self.parent_chain.node_order[0] + return self.parent_chain.bones[root_node].position + + + def set_positions_from_frame(self, frame, parent_length): + if self.parent_chain is not None: + self.parent_chain.set_positions_from_frame(frame, parent_length) + parent_length += self.parent_chain.chain_length + for c in self.child_nodes: + c.set_positions_from_frame(frame, parent_length) + + def get_error(self): + error = 0 + for c in self.child_nodes: + error += c.get_error() + if self.is_leaf: + if self.parent_chain.target_is_reachable(): + error += self.parent_chain.get_error() + else: + error += 0 + return error + + def solve(self): + iter = 0 + distance = self.get_error() + while distance > self.tolerance and iter < self.max_iter: + self.backward_stage() + self.forward_stage() + distance = self.get_error() + iter += 1 + n_out_of_reach = self.targets_out_of_reach() + if n_out_of_reach > 0: + self.orient_to_targets() + print("solved", distance, n_out_of_reach) + self.print_end_effectors() + + def print_end_effectors(self): + n_targets = 0 + for c in self.child_nodes: + if c.is_leaf: + print(c.root, c.parent_chain.target, c.parent_chain.get_end_effector_position()) + else: + c.print_end_effectors() + return n_targets + + def targets_out_of_reach(self): + n_targets = 0 + for c in self.child_nodes: + if c.is_leaf and not c.parent_chain.target_is_reachable(): + n_targets+=1 + else: + n_targets += c.targets_out_of_reach() + return n_targets + + def orient_to_targets(self): + for c in self.child_nodes: + if c.is_leaf and not c.parent_chain.target_is_reachable(): + c.parent_chain.orient_to_target() + else: + c.orient_to_targets() + + def get_joint_parameters(self, frame): + #print("get joint parameters") + for c in self.child_nodes: + print("from", c.parent_chain.node_order[0], "to", c.parent_chain.node_order[-1]) + global_frame = c.parent_chain.get_joint_parameters_global() + src = 3 + if self.parent_chain is None: + animated_joints = c.parent_chain.node_order + else: + animated_joints = c.parent_chain.node_order[1:] + for j in animated_joints: #ignore root rotation + dest = self.skeleton.animated_joints.index(j)*4 +3 + frame[dest:dest+4] = to_local_cos(self.skeleton, j, frame, global_frame[src:src+4]) + src += 4 + c.get_joint_parameters(frame) + return frame + + def set_targets(self, targets): + #print("set targets",targets) + if self.is_leaf: + if self.root in targets: + self.target = targets[self.root] + self.parent_chain.target = self.target + #print("set target",self.root) + else: + self.target = self.parent_chain.get_end_effector_position() # keep the initial position as target for unconstrained leafs + self.parent_chain.target = self.target + print("set end effector position") + else: + for c in self.child_nodes: + c.set_targets(targets) + + def run(self, orig_frame, targets): + self.set_positions_from_frame(orig_frame, 0) + self.set_targets(targets) + self.solve() + if False: + frame = np.zeros(self.n_parameters) + self.get_joint_parameters(frame) + else: + joint_pos = dict() + self.get_global_positions(joint_pos) + next_nodes = dict() + self.get_next_nodes(next_nodes) + frame = get_global_joint_parameters_from_positions(self.skeleton, joint_pos, next_nodes) + frame = global_to_local_frame(self.skeleton, frame) + #print("done") + #self.print_frame_parameters(frame) + return frame + + def get_next_nodes(self, next_nodes): + if self.parent_chain is not None: + self.parent_chain.get_next_nodes(next_nodes) + for c in self.child_nodes: + c.get_next_nodes(next_nodes) + + def print_frame_parameters(self, frame): + idx = 3 + for n in self.skeleton.nodes: + if len(self.skeleton.nodes[n].children) > 0: + print(n, frame[idx: idx+4]) + idx +=4 + return + + def draw(self, m,v,p, l): + if self.parent_chain is not None: + self.parent_chain.draw(m,v,p,l) + for c in self.child_nodes: + c.draw(m, v, p, l) + + def get_global_positions(self, positions): + if self.parent_chain is not None: + positions.update(self.parent_chain.get_global_positions()) + for c in self.child_nodes: + c.get_global_positions(positions) \ No newline at end of file diff --git a/build/lib/anim_utils/motion_editing/footplant_constraint_generator.py b/build/lib/anim_utils/motion_editing/footplant_constraint_generator.py new file mode 100644 index 0000000..7cd4ad6 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/footplant_constraint_generator.py @@ -0,0 +1,692 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import collections +import numpy as np +from scipy.interpolate import UnivariateSpline +from transformations import quaternion_from_matrix, quaternion_multiply, quaternion_matrix, quaternion_slerp, quaternion_inverse +from ..animation_data.skeleton_models import * +from .motion_grounding import MotionGroundingConstraint, FOOT_STATE_SWINGING +from .utils import get_average_joint_position, get_average_joint_direction, normalize, get_average_direction_from_target +from ..animation_data.utils import quaternion_from_vector_to_vector + +def get_velocity_and_acceleration(scalars): + """ https://stackoverflow.com/questions/40226357/second-derivative-in-python-scipy-numpy-pandas + """ + x = np.linspace(0, len(scalars), len(scalars)) + ys = np.array(scalars, dtype=np.float32) + y_spl = UnivariateSpline(x, ys, s=0, k=4) + velocity = y_spl.derivative(n=1) + acceleration = y_spl.derivative(n=2) + return velocity(x), acceleration(x) + +def get_joint_vertical_velocity_and_acceleration(skeleton, frames, joints): + joint_heights = dict() + for joint in joints: + ps = [] + for frame in frames: + p = skeleton.nodes[joint].get_global_position(frame) + ps.append(p[1]) + if len(ps)> 1: + v, a = get_velocity_and_acceleration(ps) + joint_heights[joint] = ps[:-1], v, a + elif len(ps) == 1: + joint_heights[joint] = [ps[0]], [0], [0] + else: + joint_heights[joint] = [], [], [] + + return joint_heights + + +def get_joint_height(skeleton, frames, joints): + joint_heights = dict() + for joint in joints: + ps = [] + for frame in frames: + p = skeleton.nodes[joint_name].get_global_position(frame) + ps.append(p) + joint_heights[joint] = positions + return joint_heights + + +def guess_ground_height(skeleton, frames, start_frame, n_frames, foot_joints): + minimum_height = np.inf + joint_heights = get_joint_height(skeleton, frames[start_frame:start_frame+n_frames], foot_joints) + for joint in joint_heights: + p = joint_heights[joint] + pT = np.array(p).T + new_min_height = min(pT[1]) + if new_min_height < minimum_height: + minimum_height = new_min_height + return minimum_height + + +def get_quat_delta(qa, qb): + """ get quaternion from quat a to quat b """ + return quaternion_multiply(qb, quaternion_inverse(qa)) + + +def get_leg_state(plant_range, foot_definitions, side, frame): + state = "swinging" + for foot in foot_definitions[side]: + if side in plant_range: + if foot in plant_range[side]: + if plant_range[side][foot]["start"] is not None and plant_range[side][foot]["start"] <= frame <= plant_range[side][foot]["end"]: + state = "planted" + return state + + +def create_leg_state_model(plant_range, start_frame, end_frame, foot_definitions): + model = collections.OrderedDict() + for f in range(start_frame, end_frame): + model[f] = dict() + for side in foot_definitions: + model[f][side] = get_leg_state(plant_range, foot_definitions, side, f) + return model + + +def regenerate_ankle_constraint_with_new_orientation(position, joint_offset, new_orientation): + """ move ankle so joint is on the ground using the new orientation""" + m = quaternion_matrix(new_orientation)[:3, :3] + target_offset = np.dot(m, joint_offset) + ca = position - target_offset + return ca + +def regenerate_ankle_constraint_with_new_orientation2(position, joint_offset, delta): + """ move ankle so joint is on the ground using the new orientation""" + m = quaternion_matrix(delta)[:3, :3] + target_offset = np.dot(m, joint_offset) + ca = position + target_offset + return ca + +def get_global_orientation(skeleton, joint_name, frame): + m = skeleton.nodes[joint_name].get_global_matrix(frame) + m[:3, 3] = [0, 0, 0] + return normalize(quaternion_from_matrix(m)) + + +def convert_plant_range_to_ground_contacts(start_frame, end_frame, plant_range): + print("convert plant range", start_frame, end_frame) + ground_contacts = collections.OrderedDict() + for f in range(start_frame, end_frame+1): + ground_contacts[f] = [] + for side in list(plant_range.keys()): + for joint in list(plant_range[side].keys()): + if plant_range[side][joint]["start"] is not None: + start = plant_range[side][joint]["start"] + end = plant_range[side][joint]["end"] + print(joint, start,end) + for f in range(start, end): + ground_contacts[f].append(joint) + return ground_contacts + + +def find_first_frame(skeleton, frames, joint_name, start_frame, end_frame, tolerance=2.0): + p = skeleton.nodes[joint_name].get_global_position(frames[end_frame-1]) + h = p[1] + search_window = list(range(start_frame, end_frame)) + for f in reversed(search_window): + tp = skeleton.nodes[joint_name].get_global_position(frames[f]) + if abs(tp[1]-h) > tolerance: + return f + return start_frame + + +def find_last_frame(skeleton, frames, joint_name, start_frame, end_frame, tolerance=2.0): + p = skeleton.nodes[joint_name].get_global_position(frames[start_frame]) + h = p[1] + search_window = list(range(start_frame, end_frame)) + for f in search_window: + tp = skeleton.nodes[joint_name].get_global_position(frames[f]) + if abs(tp[1] - h) > tolerance: + return f + return end_frame + + +def merge_constraints(a,b): + for key, item in list(b.items()): + if key in a: + a[key] += b[key] + else: + a[key] = b[key] + return a + +def align_quaternion(q, ref_q): + if np.dot(ref_q, q) < 0: + return -q + else: + return q + + +def blend(x): + return 2 * x * x * x - 3 * x * x + 1 + + +def get_plant_frame_range(step, search_window): + start_frame = step.start_frame + end_frame = step.end_frame + 1 + half_window = search_window / 2 + end_offset = -5 + plant_range = dict() + L = "left" + R = "right" + plant_range[L] = dict() + plant_range[R] = dict() + + plant_range[L]["start"] = None + plant_range[L]["end"] = None + plant_range[R]["start"] = None + plant_range[R]["end"] = None + + if step.node_key[1] == "beginLeftStance": + plant_range[R]["start"] = start_frame + plant_range[R]["end"] = end_frame - half_window + end_offset + plant_range[L]["start"] = start_frame + plant_range[L]["end"] = start_frame + 20 + + elif step.node_key[1] == "beginRightStance": + plant_range[L]["start"] = start_frame + plant_range[L]["end"] = end_frame - half_window + end_offset + plant_range[R]["start"] = start_frame + plant_range[R]["end"] = start_frame + 20 + + elif step.node_key[1] == "endLeftStance": + plant_range[R]["start"] = start_frame + half_window + plant_range[R]["end"] = end_frame + plant_range[L]["start"] = end_frame - 20 + plant_range[L]["end"] = end_frame + + elif step.node_key[1] == "endRightStance": + plant_range[L]["start"] = start_frame + half_window + plant_range[L]["end"] = end_frame + plant_range[R]["start"] = end_frame - 20 + plant_range[R]["end"] = end_frame + + elif step.node_key[1] == "leftStance": + plant_range[R]["start"] = start_frame + half_window + plant_range[R]["end"] = end_frame - half_window + end_offset + + elif step.node_key[1] == "rightStance": + plant_range[L]["start"] = start_frame + half_window + plant_range[L]["end"] = end_frame - half_window + end_offset + return plant_range + + +def get_plant_frame_range_using_search(skeleton, motion_vector, step, foot_definitions, search_window, lift_tolerance): + """ Use the assumption that depending on the motion primitive different + feet are grounded at the beginning, middle and end of the step to find the plant range for each joint. + Starting from those frames search forward and backward until the foot is lifted. + """ + start_frame = step.start_frame + end_frame = step.end_frame + 1 + frames = motion_vector.frames + w = search_window + search_start = max(end_frame - w, start_frame) # start of search range for the grounding range + search_end = min(start_frame + w, end_frame) # end of search range for the grounding range + plant_range = dict() + L = "left" + R = "right" + joint_types = ["heel", "toe"] # first check heel then toe + plant_range[L] = dict() + plant_range[R] = dict() + for side in list(plant_range.keys()): + plant_range[side] = dict() + for joint_type in joint_types: + joint = foot_definitions[side][joint_type] + plant_range[side][joint] = dict() + plant_range[side][joint]["start"] = None + plant_range[side][joint]["end"] = None + + if step.node_key[1] == "beginLeftStance": + plant_range[side][joint]["start"] = start_frame + plant_range[side][joint]["end"] = find_last_frame(skeleton, frames, joint, start_frame, search_end, lift_tolerance) + + elif step.node_key[1] == "beginRightStance": + plant_range[side][joint]["start"] = start_frame + plant_range[side][joint]["end"] = find_last_frame(skeleton, frames, joint, start_frame, search_end, lift_tolerance) + + elif step.node_key[1] == "endLeftStance": + heel = foot_definitions[side]["heel"] + if plant_range[side][heel]["start"] is not None: + local_search_start = plant_range[side][heel]["start"] + else: + local_search_start = start_frame + plant_range[side][joint]["start"] = find_first_frame(skeleton, frames, joint, local_search_start, end_frame, lift_tolerance+3.0) + plant_range[side][joint]["end"] = end_frame + + elif step.node_key[1] == "endRightStance": + heel = foot_definitions[side]["heel"] + if plant_range[side][heel]["start"] is not None: + local_search_start = plant_range[side][heel]["start"] + else: + local_search_start = start_frame + plant_range[side][joint]["start"] = find_first_frame(skeleton, frames, joint, local_search_start, end_frame, lift_tolerance+3.0) + plant_range[side][joint]["end"] = end_frame + + elif step.node_key[1] == "leftStance" and side == R: + middle_frame = int((end_frame-start_frame)/2) + start_frame + plant_range[side][joint]["start"] = find_first_frame(skeleton, frames, joint, search_start, middle_frame, lift_tolerance) + plant_range[side][joint]["end"] = find_last_frame(skeleton, frames, joint, middle_frame + 1, search_end, lift_tolerance) + + elif step.node_key[1] == "rightStance" and side == L: + middle_frame = int((end_frame - start_frame) / 2) + start_frame + plant_range[side][joint]["start"] = find_first_frame(skeleton, frames, joint, search_start, middle_frame, lift_tolerance) + plant_range[side][joint]["end"] = find_last_frame(skeleton, frames, joint, middle_frame + 1, search_end, lift_tolerance) + return plant_range + + + +class SceneInterface(object): + def __init__(self, height): + self.h = height + + def get_height(self,x, z): + return self.h + +def create_ankle_constraint(skeleton, frames, ankle_joint_name, heel_joint_name, toe_joint, frame_idx, end_frame, ground_height): + """ create constraint on ankle position and orientation """ + c = get_average_joint_position(skeleton, frames, heel_joint_name, frame_idx, end_frame) + c[1] = ground_height + a = get_average_joint_position(skeleton, frames, ankle_joint_name, frame_idx, end_frame) + h = get_average_joint_position(skeleton, frames, heel_joint_name, frame_idx, end_frame) + target_heel_offset = a - h + ca = c + target_heel_offset + avg_direction = None + if len(skeleton.nodes[ankle_joint_name].children) > 0: + avg_direction = get_average_direction_from_target(skeleton, frames, ca, toe_joint, + frame_idx, end_frame) + toe_length = np.linalg.norm(skeleton.nodes[toe_joint].offset) + ct = ca + avg_direction * toe_length + ct[1] = ground_height + avg_direction = normalize(ct - ca) + return MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, avg_direction) + + +class FootplantConstraintGenerator(object): + def __init__(self, skeleton, settings, scene_interface=None): + self.skeleton = skeleton + self.left_foot = skeleton.skeleton_model["joints"]["left_ankle"] + self.right_foot = skeleton.skeleton_model["joints"]["right_ankle"] + self.right_heel = skeleton.skeleton_model["joints"]["right_heel"] + self.left_heel = skeleton.skeleton_model["joints"]["left_heel"] + self.right_toe = skeleton.skeleton_model["joints"]["right_toe"] + self.left_toe = skeleton.skeleton_model["joints"]["left_toe"] + self.contact_joints = [self.right_heel, self.left_heel, self.right_toe, self.left_toe] + + self.foot_definitions = {"right": {"heel": self.right_heel, "toe": self.right_toe, "ankle": self.right_foot}, + "left": {"heel": self.left_heel, "toe": self.left_toe, "ankle": self.left_foot}} + + if scene_interface is None: + self.scene_interface = SceneInterface(0) + else: + self.scene_interface = scene_interface + + self.contact_tolerance = settings["contact_tolerance"] + self.foot_lift_tolerance = settings["foot_lift_tolerance"] + self.constraint_generation_range = settings["constraint_range"] + self.smoothing_constraints_window = settings["smoothing_constraints_window"] + self.foot_lift_search_window = settings["foot_lift_search_window"] + + self.position_constraint_buffer = dict() + self.orientation_constraint_buffer = dict() + self.velocity_tolerance = 0 + if "velocity_tolerance" in settings: + self.velocity_tolerance = settings["velocity_tolerance"] + + def detect_ground_contacts(self, frames, joints, ground_height=0): + """https://stackoverflow.com/questions/3843017/efficiently-detect-sign-changes-in-python + """ + joint_y_vel_acc = get_joint_vertical_velocity_and_acceleration(self.skeleton, frames, joints) + ground_contacts = [] + for f in frames: + ground_contacts.append([]) + for joint in joints: + ps, yv, ya = joint_y_vel_acc[joint] + for idx, p in enumerate(ps): + velocity = np.sqrt(yv[idx]*yv[idx]) + if p - ground_height < self.contact_tolerance or velocity < self.velocity_tolerance :#and zero_crossings[frame_idx] + ground_contacts[idx].append(joint) + # copy contacts of frame not covered by velocity + if len(ground_contacts) > 1: + ground_contacts[-1] = ground_contacts[-2] + return self.filter_outliers(ground_contacts, joints) + + def filter_outliers(self, ground_contacts, joints): + n_frames = len(ground_contacts) + filtered_ground_contacts = [[] for idx in range(n_frames)] + filtered_ground_contacts[0] = ground_contacts[0] + filtered_ground_contacts[-1] = ground_contacts[-1] + frames_indices = range(1,n_frames-1) + for frame_idx in frames_indices: + filtered_ground_contacts[frame_idx] = [] + prev_frame = ground_contacts[frame_idx - 1] + current_frame = ground_contacts[frame_idx] + next_frame = ground_contacts[frame_idx + 1] + for joint in joints: + if joint in current_frame: + if joint not in prev_frame and joint not in next_frame: + continue + else: + filtered_ground_contacts[frame_idx].append(joint) + return filtered_ground_contacts + + def generate_from_graph_walk(self, motion_vector): + """ the interpolation range must start at end_frame-1 because this is the last modified frame """ + self.position_constraint_buffer = dict() + self.orientation_constraint_buffer = dict() + + constraints = dict() + for frame_idx in range(motion_vector.n_frames): + self.position_constraint_buffer[frame_idx] = dict() + self.orientation_constraint_buffer[frame_idx] = dict() + constraints[frame_idx] = [] + + ground_contacts = collections.OrderedDict() + blend_ranges = dict() + blend_ranges[self.right_foot] = [] + blend_ranges[self.left_foot] = [] + graph_walk = motion_vector.graph_walk + for idx, step in enumerate(graph_walk.steps): + step_length = step.end_frame - step.start_frame + if step_length <= 0: + print("small frame range ", idx, step.node_key, step.start_frame, step.end_frame) + + continue + if step.node_key[0] in LOCOMOTION_ACTIONS: + step_plant_range = get_plant_frame_range_using_search(self.skeleton, motion_vector, step, self.foot_definitions, + self.foot_lift_search_window, self.foot_lift_tolerance) + leg_state_model = create_leg_state_model(step_plant_range, step.start_frame, step.end_frame, self.foot_definitions) + step_ground_contacts = convert_plant_range_to_ground_contacts(step.start_frame, step.end_frame, step_plant_range) + ground_contacts.update(step_ground_contacts) + for frame_idx, joint_names in list(step_ground_contacts.items()): + constraints[frame_idx] = self.generate_grounding_constraints(motion_vector.frames, frame_idx, + joint_names) + + self.set_smoothing_constraints(motion_vector.frames, constraints) + + n_frames = len(motion_vector.frames) + blend_ranges = self.generate_blend_ranges(constraints, n_frames) + + return constraints, blend_ranges, ground_contacts + + def generate(self, motion_vector, ground_contacts=None): + self.position_constraint_buffer = dict() + self.orientation_constraint_buffer = dict() + + constraints = dict() + if ground_contacts is None: + ground_contacts = self.detect_ground_contacts(motion_vector.frames, self.contact_joints) + # generate constraints + for frame_idx, joint_names in enumerate(ground_contacts): + constraints[frame_idx] = self.generate_grounding_constraints(motion_vector.frames, frame_idx, joint_names) + + self.set_smoothing_constraints(motion_vector.frames, constraints) + + n_frames = len(motion_vector.frames) + blend_ranges = self.generate_blend_ranges(constraints, n_frames) + return constraints, blend_ranges + + def generate_blend_ranges(self, constraints, n_frames): + blend_ranges = dict() + blend_ranges[self.right_foot] = [] + blend_ranges[self.left_foot] = [] + + state = dict() + state[self.right_foot] = -1 + state[self.left_foot] = -1 + + joint_names = [self.right_foot, self.left_foot] + + for frame_idx in range(n_frames): + if frame_idx in constraints: + for j in joint_names: + constrained_joints = [c.joint_name for c in constraints[frame_idx]] + if j in constrained_joints: + if state[j] == -1: + # start new constrained range + blend_ranges[j].append([frame_idx, n_frames]) + state[j] = len(blend_ranges[j])-1 + else: + # update constrained range + idx = state[j] + blend_ranges[j][idx][1] = frame_idx + else: + state[j] = -1 # stop constrained range no constraint defined + else: + for c in joint_names: + state[c.joint_name] = -1 # stop constrained range no constraint defined + return blend_ranges + + def generate_grounding_constraints(self, frames, frame_idx, joint_names): + self.position_constraint_buffer[frame_idx] = dict() + self.orientation_constraint_buffer[frame_idx] = dict() + + new_constraints = [] + end_frame = frame_idx + self.constraint_generation_range + for side in self.foot_definitions: + foot_joints = self.foot_definitions[side] + c = None + if foot_joints["heel"] in joint_names and foot_joints["toe"] in joint_names: + c = self.generate_grounding_constraint_from_heel_and_toe(frames, foot_joints["ankle"], foot_joints["heel"], foot_joints["toe"], frame_idx, end_frame) + elif foot_joints["heel"] in joint_names: + c = self.generate_grounding_constraint_from_heel(frames, foot_joints["ankle"], foot_joints["heel"], frame_idx, end_frame) + elif foot_joints["toe"] in joint_names: + c = self.generate_grounding_constraint_from_toe(frames, foot_joints["ankle"], foot_joints["toe"], frame_idx, end_frame) + if c is not None: + c.heel_offset = self.skeleton.nodes[foot_joints["heel"]].offset + new_constraints.append(c) + return new_constraints + + def generate_grounding_constraint_from_heel_and_toe(self, frames, ankle_joint_name, heel_joint_name, toe_joint_name, frame_idx, end_frame): + """ create constraint on ankle position and orientation """ + # get target global ankle orientation based on the direction between grounded heel and toe + ct = self.get_previous_joint_position_from_buffer(frames, frame_idx, end_frame, toe_joint_name) + ct[1] = self.scene_interface.get_height(ct[0], ct[2]) + ch = self.get_previous_joint_position_from_buffer(frames, frame_idx, end_frame, heel_joint_name) + ch[1] = self.scene_interface.get_height(ct[0], ct[2]) + target_direction = normalize(ct - ch) + t = self.skeleton.nodes[toe_joint_name].get_global_position(frames[frame_idx]) + h = self.skeleton.nodes[heel_joint_name].get_global_position(frames[frame_idx]) + original_direction = normalize(t-h) + + global_delta_q = quaternion_from_vector_to_vector(original_direction, target_direction) + global_delta_q = normalize(global_delta_q) + + m = self.skeleton.nodes[ankle_joint_name].get_global_matrix(frames[frame_idx]) + m[:3, 3] = [0, 0, 0] + oq = quaternion_from_matrix(m) + oq = normalize(oq) + orientation = normalize(quaternion_multiply(global_delta_q, oq)) + + self.orientation_constraint_buffer[frame_idx][ankle_joint_name] = orientation # save orientation to buffer + + # set target ankle position based on the grounded heel and the global target orientation of the ankle + m = quaternion_matrix(orientation)[:3,:3] + target_heel_offset = np.dot(m, self.skeleton.nodes[heel_joint_name].offset) + ca = ch - target_heel_offset + #print "set ankle constraint both", ch, ca, target_heel_offset + constraint = MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, None, orientation) + constraint.toe_position = ct + constraint.heel_position = ch + return constraint + + def generate_grounding_constraint_from_heel(self, frames, ankle_joint_name, heel_joint_name, frame_idx, end_frame): + """ create constraint on the ankle position without an orientation constraint""" + #print "add ankle constraint from heel" + ch = self.get_previous_joint_position_from_buffer(frames, frame_idx, end_frame, heel_joint_name) + a = self.skeleton.nodes[ankle_joint_name].get_global_position(frames[frame_idx]) + h = self.skeleton.nodes[heel_joint_name].get_global_position(frames[frame_idx]) + ch[1] = self.scene_interface.get_height(ch[0], ch[2]) # set heel constraint on the ground + target_heel_offset = a - h # difference between unmodified heel and ankle + ca = ch + target_heel_offset # move ankle so heel is on the ground + #print "set ankle constraint single", ch, ca, target_heel_offset + constraint = MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, None, None) + constraint.heel_position = ch + return constraint + + def generate_grounding_constraint_from_toe(self, frames, ankle_joint_name, toe_joint_name, frame_idx, end_frame): + """ create a constraint on the ankle position based on the toe constraint position""" + #print "add toe constraint" + ct = self.get_previous_joint_position_from_buffer(frames, frame_idx, end_frame, toe_joint_name) + ct[1] = self.scene_interface.get_height(ct[0], ct[2]) # set toe constraint on the ground + a = self.skeleton.nodes[ankle_joint_name].get_global_position(frames[frame_idx]) + t = self.skeleton.nodes[toe_joint_name].get_global_position(frames[frame_idx]) + + target_toe_offset = a - t # difference between unmodified toe and ankle at the frame + ca = ct + target_toe_offset # move ankle so toe is on the ground + constraint = MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, None, None) + + constraint.toe_position = ct + constraint.global_toe_offset = target_toe_offset + return constraint + + def set_smoothing_constraints(self, frames, constraints): + """ Set orientation constraints to singly constrained frames to smooth the foot orientation (Section 4.2) + """ + for frame_idx in constraints: + for constraint in constraints[frame_idx]: + if constraint.joint_name not in self.orientation_constraint_buffer[frame_idx]: # singly constrained + self.set_smoothing_constraint(frames, constraint, frame_idx, self.smoothing_constraints_window) + + def set_smoothing_constraint(self, frames, constraint, frame_idx, window): + backward_hit = None + forward_hit = None + + start_window = max(frame_idx - window, 0) + end_window = min(frame_idx + window, len(frames)) + + # look backward + search_window = list(range(start_window, frame_idx)) + for f in reversed(search_window): + if constraint.joint_name in self.orientation_constraint_buffer[f]: + backward_hit = f + break + # look forward + for f in range(frame_idx, end_window): + if constraint.joint_name in self.orientation_constraint_buffer[f]: + forward_hit = f + break + + # update q + oq = get_global_orientation(self.skeleton, constraint.joint_name, frames[frame_idx]) + if backward_hit is not None and constraint.toe_position is not None: + bq = self.orientation_constraint_buffer[backward_hit][constraint.joint_name] + j = frame_idx - backward_hit + t = float(j + 1) / (window + 1) + global_q = normalize(quaternion_slerp(bq, oq, t, spin=0, shortestpath=True)) + constraint.orientation = normalize(global_q) + delta = get_quat_delta(oq, global_q) + delta = normalize(delta) + # rotate stored vector by global delta + if constraint.joint_name == self.left_foot: + + constraint.position = regenerate_ankle_constraint_with_new_orientation2(constraint.toe_position, + constraint.global_toe_offset, + delta) + else: + constraint.position = regenerate_ankle_constraint_with_new_orientation2(constraint.toe_position, + constraint.global_toe_offset, + delta) + + elif forward_hit is not None and constraint.heel_position is not None: + k = forward_hit - frame_idx + t = float(k + 1) / (window + 1) + fq = self.orientation_constraint_buffer[forward_hit][constraint.joint_name] + global_q = normalize(quaternion_slerp(oq, fq, t, spin=0, shortestpath=True)) + constraint.orientation = normalize(global_q) + constraint.position = regenerate_ankle_constraint_with_new_orientation(constraint.heel_position, + constraint.heel_offset, + constraint.orientation) + + def get_previous_joint_position_from_buffer(self, frames, frame_idx, end_frame, joint_name): + """ Gets the joint position of the previous frame from the buffer if it exists. + otherwise the position is calculated for the current frame and updated in the buffer + """ + prev_frame_idx = frame_idx - 1 + prev_p = self.get_joint_position_from_buffer(prev_frame_idx, joint_name) + if prev_p is not None: + self.position_constraint_buffer[frame_idx][joint_name] = prev_p + return prev_p + else: + self.update_joint_position_in_buffer(frames, frame_idx, end_frame, joint_name) + p = self.position_constraint_buffer[frame_idx][joint_name] + p[1] = self.scene_interface.get_height(p[0], p[2]) + #print "joint constraint",joint_name, p + return p + + def get_joint_position_from_buffer(self, frame_idx, joint_name): + if frame_idx not in self.position_constraint_buffer: + return None + if joint_name not in self.position_constraint_buffer[frame_idx]: + return None + return self.position_constraint_buffer[frame_idx][joint_name] + + def update_joint_position_in_buffer(self, frames, frame_idx, end_frame, joint_name): + end_frame = min(end_frame, frames.shape[0]) + if frame_idx not in self.position_constraint_buffer: + self.position_constraint_buffer[frame_idx] = dict() + if joint_name not in self.position_constraint_buffer[frame_idx]: + p = get_average_joint_position(self.skeleton, frames, joint_name, frame_idx, end_frame) + #p[1] = self.scene_interface.get_height(p[0], p[2]) + #print "add", joint_name, p, frame_idx, end_frame + self.position_constraint_buffer[frame_idx][joint_name] = p + + def generate_ankle_constraints_legacy(self, frames, frame_idx, joint_names, prev_constraints, prev_joint_names): + end_frame = frame_idx + 10 + new_constraints = dict() + temp_constraints = {"left": None, "right": None} + if self.right_heel and self.right_toe in joint_names: + if self.right_heel and self.right_toe in prev_joint_names: + temp_constraints["right"] = prev_constraints["right"] + else: + temp_constraints["right"] = self.create_ankle_constraints_from_heel_and_toe(frames, self.right_foot, self.right_heel, frame_idx, end_frame) + if self.left_heel and self.left_toe in joint_names: + if joint_names == prev_joint_names: + temp_constraints["left"] = prev_constraints["left"] + else: + temp_constraints["left"] = self.create_ankle_constraints_from_heel_and_toe(frames, self.left_foot,self.left_heel, frame_idx, end_frame) + for side in temp_constraints: + new_constraints[side] = temp_constraints[side][0] + return new_constraints + + def create_ankle_constraints_from_heel_and_toe(self, frames, ankle_joint_name, heel_joint_name, start_frame, end_frame): + """ create constraint on ankle position and orientation """ + constraints = dict() + ct = get_average_joint_position(self.skeleton, frames, heel_joint_name, start_frame, end_frame) + ct[1] = self.scene_interface.get_height(ct[0], ct[2]) + pa = get_average_joint_position(self.skeleton, frames, ankle_joint_name, start_frame, end_frame) + ph = get_average_joint_position(self.skeleton, frames, heel_joint_name, start_frame, end_frame) + delta = ct - ph + ca = pa + delta + avg_direction = None + if len(self.skeleton.nodes[ankle_joint_name].children) > 0: + child_joint_name = self.skeleton.nodes[ankle_joint_name].children[0].node_name + avg_direction = get_average_joint_direction(self.skeleton, frames, ankle_joint_name, child_joint_name, + start_frame, end_frame) + #print "constraint ankle at",pa, ct,ca, start_frame, end_frame, avg_direction + for frame_idx in range(start_frame, end_frame): + c = MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, avg_direction) + constraints[frame_idx] = [] + constraints[frame_idx].append(c) + return constraints + + diff --git a/build/lib/anim_utils/motion_editing/hybrit_ik.py b/build/lib/anim_utils/motion_editing/hybrit_ik.py new file mode 100644 index 0000000..94884b4 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/hybrit_ik.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" Hybrit IK using numerical IK based on an exponential displacement map and analytical IK +based on Section 5.1 of [1] + +Use current configuration as q_0 +Find v such that (q_i = q_0_i exp(v_i) for 0 self.success_threshold and iter_counter < self.max_retries: + r = minimize(self._objective, guess, args=(self.skeleton, reference, constraints, self.joint_weights, self.analytical_ik), + method=self.ik_settings["optimization_method"], + tol=self.ik_settings["tolerance"], + options=self._optimization_options) + guess = r["x"] + error = self._objective(guess, self.skeleton, reference, constraints, self.joint_weights, self.analytical_ik) + iter_counter += 1 + + exp_frame = guess + return exp_frame + + def modify_frame(self, reference, constraints): + exp_frame = self.run(reference, constraints) + d = convert_exp_frame_to_quat_frame(self.skeleton, exp_frame) + q_frame = add_quat_frames(self.skeleton, reference, d) + for c in constraints: + if c.joint_name in self.analytical_ik: + q_frame = self.analytical_ik[c.joint_name].apply(q_frame, c.position, c.orientation) + return q_frame diff --git a/build/lib/anim_utils/motion_editing/ik_constraints.py b/build/lib/anim_utils/motion_editing/ik_constraints.py new file mode 100644 index 0000000..4894ec1 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/ik_constraints.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np + +SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION = "keyframe_position" +SPATIAL_CONSTRAINT_TYPE_TWO_HAND_POSITION = "keyframe_two_hands" +SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION = "keyframe_relative_position" +SPATIAL_CONSTRAINT_TYPE_KEYFRAME_LOOK_AT = "keyframe_look_at" +SUPPORTED_CONSTRAINT_TYPES = [SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION, + SPATIAL_CONSTRAINT_TYPE_TWO_HAND_POSITION, + SPATIAL_CONSTRAINT_TYPE_KEYFRAME_LOOK_AT, + SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION] +class IKConstraint(object): + @staticmethod + def evaluate(params, data): + pass + + +class JointIKConstraint(IKConstraint): + def __init__(self, joint_name, position, orientation, keyframe, free_joints, step_idx=-1, frame_range=None, look_at=False, optimize=True, offset=None, look_at_pos=None): + self.joint_name = joint_name + self.position = position + self.orientation = orientation + self.keyframe = keyframe + self.frame_range = frame_range + self.free_joints = free_joints + self.step_idx = step_idx + self.look_at = look_at + self.optimize = optimize + self.offset = offset + self.look_at_pos = look_at_pos + # set in case it is a relative constraint + self.relative_parent_joint_name = None # joint the offsets points from to the target + self.relative_offset = None + + # tool orientation constraint + self.src_tool_cos = None # tool coordinate system + self.dest_tool_cos = None # target direction + + # set a fk chain root to reduce the degrees of freedom + self.fk_chain_root = None + + + @staticmethod + def evaluate(parameters, data): + pose, free_joints, target_joint, target_position, target_orientation, offset = data + pose.set_channel_values(parameters, free_joints) + #parent_joint = pose.get_parent_joint(target_joint) + #pose.apply_bounds_on_joint(parent_joint) + if target_orientation is not None: + parent_joint = pose.get_parent_joint(target_joint) + if parent_joint is not None: + pose.set_hand_orientation(parent_joint, target_orientation) + #pose.apply_bounds_on_joint(parent_joint) + if offset is not None: + m = pose.evaluate_matrix(target_joint) + p = np.dot(m, offset)[:3] + d = target_position - p + else: + d = pose.evaluate_position(target_joint) - target_position + return np.dot(d, d) + + def data(self, ik, free_joints=None): + if free_joints is None: + free_joints = self.free_joints + if ik.optimize_orientation: + orientation = self.orientation + else: + orientation = None + return ik.pose, free_joints, self.joint_name, self.position, orientation, self.offset + + def get_joint_names(self): + return [self.joint_name] + +class RelativeJointIKConstraint(IKConstraint): + def __init__(self, ref_joint_name, target_joint_name, rel_position, keyframe, free_joints, step_idx=-1, frame_range=None): + self.ref_joint_name = ref_joint_name + self.target_joint_name = target_joint_name + self.rel_position = rel_position + self.keyframe = keyframe + self.frame_range = frame_range + self.free_joints = free_joints + self.step_idx = step_idx + + @staticmethod + def evaluate(parameters, data): + pose, free_joints, ref_joint, target_joint, target_delta = data + pose.set_channel_values(parameters, free_joints) + ref_matrix = pose.skeleton.nodes[ref_joint].get_global_matrix(pose.get_vector()) + target = np.dot(ref_matrix, target_delta)[:3] + d = pose.evaluate_position(target_joint) - target + return np.dot(d, d) + + def data(self, ik, free_joints=None): + if free_joints is None: + free_joints = self.free_joints + return ik.pose, free_joints, self.ref_joint_name, self.target_joint_name, self.rel_position + + def get_joint_names(self): + return [self.target_joint_name] + + +class TwoJointIKConstraint(IKConstraint): + def __init__(self, joint_names, target_positions, target_center, target_delta, target_direction, keyframe, free_joints): + self.joint_names = joint_names + self.target_positions = target_positions + self.target_center = target_center + self.target_delta = target_delta + self.target_direction = target_direction + self.keyframe = keyframe + self.free_joints = free_joints + + @staticmethod + def evaluate(parameters, data): + pose, free_joints, joint_names, target_positions, target_center, target_delta, target_direction = data + pose.set_channel_values(parameters, free_joints) + left = pose.evaluate_position(joint_names[0]) + right = pose.evaluate_position(joint_names[1]) + delta_vector = right - left + residual_vector = [0.0, 0.0, 0.0] + #get distance to center + residual_vector[0] = np.linalg.norm(target_center - (left + 0.5 * delta_vector)) + #get difference to distance between hands + delta = np.linalg.norm(delta_vector) + residual_vector[1] = abs(target_delta - delta) + #print "difference", residual_vector[1] + #get difference of global orientation + direction = delta_vector/delta + + residual_vector[2] = abs(target_direction[0] - direction[0]) + \ + abs(target_direction[1] - direction[1]) + \ + abs(target_direction[2] - direction[2]) + residual_vector[2] *= 10.0 + + #print (target_center, (left + 0.5 * delta_vector), left, right) + #error = np.linalg.norm(left-target_positions[0]) + np.linalg.norm(right-target_positions[1]) + return sum(residual_vector)#error#residual_vector[0]#sum(residual_vector) + + def data(self, ik, free_joints=None): + if free_joints is None: + free_joints = self.free_joints + return ik.pose, free_joints, self.joint_names, self.target_positions, self.target_center, self.target_delta, self.target_direction + + def get_joint_names(self): + return self.joint_names + + diff --git a/build/lib/anim_utils/motion_editing/ik_constraints_builder.py b/build/lib/anim_utils/motion_editing/ik_constraints_builder.py new file mode 100644 index 0000000..7d3cde8 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/ik_constraints_builder.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import collections +from copy import deepcopy +import numpy as np +from transformations import quaternion_from_matrix +from anim_utils.motion_editing.motion_editing import KeyframeConstraint +from .ik_constraints import SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION, SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION, SUPPORTED_CONSTRAINT_TYPES + +class IKConstraintsBuilder(object): + def __init__(self, skeleton, force_orientation=False): + self.skeleton = skeleton + self.force_orientation = force_orientation + + def convert_to_ik_constraints(self, constraints, frame_offset=0, time_function=None, constrain_orientation=True): + ik_constraints = collections.OrderedDict() + for c in constraints: + if c.constraint_type not in SUPPORTED_CONSTRAINT_TYPES or "generated" in c.semantic_annotation.keys(): + print("skip unsupported constraint") + continue + start_frame_idx = self.get_global_frame_idx(c.canonical_keyframe, frame_offset, time_function) + if c.canonical_end_keyframe is not None: + print("apply ik constraint on region") + end_frame_idx = self.get_global_frame_idx(c.canonical_end_keyframe, frame_offset, time_function) + else: + print("no end keyframe defined") + end_frame_idx = start_frame_idx+1 + + for frame_idx in range(start_frame_idx, end_frame_idx): + ik_constraint = self.convert_mg_constraint_to_ik_constraint(frame_idx, c, constrain_orientation) + + ik_constraint.src_tool_cos = c.src_tool_cos + ik_constraint.dest_tool_cos = c.dest_tool_cos + + ik_constraint.orientation = None + if c.orientation is not None: + if c.constrain_orientation_in_region: + if start_frame_idx < frame_idx and frame_idx < end_frame_idx -1: + ik_constraint.inside_region_orientation = True + elif frame_idx == start_frame_idx: + ik_constraint.inside_region_orientation = True + ik_constraint.keep_orientation = True + elif frame_idx == start_frame_idx: + ik_constraint.orientation = c.orientation + + + ik_constraint.position = None + if c.position is not None: + if c.constrain_position_in_region: + ik_constraint.inside_region_position = False + if start_frame_idx < frame_idx and frame_idx < end_frame_idx -1: + ik_constraint.inside_region_position = True + elif frame_idx == end_frame_idx-1: + ik_constraint.end_of_region = True + elif frame_idx == start_frame_idx: + ik_constraint.position = c.position + + if ik_constraint is not None: + if frame_idx not in ik_constraints: + ik_constraints[frame_idx] = dict() + if c.joint_name not in ik_constraints[frame_idx]: + ik_constraints[frame_idx][c.joint_name] = [] + ik_constraints[frame_idx][c.joint_name] = ik_constraint + + return ik_constraints + + def get_global_frame_idx(self, mp_frame_idx, frame_offset, time_function): + if time_function is not None: + frame_idx = frame_offset + int(time_function[mp_frame_idx]) + 1 + else: + frame_idx = frame_offset + int(mp_frame_idx) + return frame_idx + + def convert_mg_constraint_to_ik_constraint(self, frame_idx, mg_constraint, constrain_orientation=False): + if mg_constraint.constraint_type == SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION: + ik_constraint = self._create_keyframe_ik_constraint(mg_constraint, frame_idx, constrain_orientation, look_at=mg_constraint.look_at) + elif mg_constraint.constraint_type == SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION: + ik_constraint = KeyframeConstraint(frame_idx, mg_constraint.joint_name, mg_constraint.position, None, mg_constraint.look_at, mg_constraint.offset) + else: + ik_constraint = None + return ik_constraint + + def _create_keyframe_ik_constraint(self, constraint, keyframe, constrain_orientation, look_at): + if constrain_orientation: + orientation = constraint.orientation + else: + orientation = None + return KeyframeConstraint(keyframe, constraint.joint_name, constraint.position, orientation, look_at) + + def generate_relative_constraint(self, keyframe, frame, joint_name, relative_joint_name): + joint_pos = self.skeleton.nodes[joint_name].get_global_position(frame) + rel_joint_pos = self.skeleton.nodes[relative_joint_name].get_global_position(frame) + #create a keyframe constraint but indicate that it is a relative constraint + ik_constraint = KeyframeConstraint(keyframe, joint_name, None, None, None) + ik_constraint.relative_parent_joint_name = relative_joint_name + ik_constraint.relative_offset = joint_pos - rel_joint_pos + return ik_constraint + + def generate_mirror_constraint(self, keyframe, ref_frame, frame, mirror_joint_name): + """ generate a constraint on the mirror joint with the similar offset to the root as in the reference frame""" + ref_mirror_joint_pos = self.skeleton.nodes[mirror_joint_name].get_global_position(ref_frame) + ref_root_joint_pos = self.skeleton.nodes[self.skeleton.root].get_global_position(ref_frame) + ref_offset_from_root = ref_mirror_joint_pos-ref_root_joint_pos + + target_root_joint_pos = self.skeleton.nodes[self.skeleton.root].get_global_position(frame) + mirror_joint_pos = ref_offset_from_root + target_root_joint_pos + print("generate mirror constraint on",mirror_joint_name, mirror_joint_pos, ref_mirror_joint_pos) + #create a keyframe constraint and set the original position as constraint + ik_constraint = KeyframeConstraint(keyframe, mirror_joint_name, mirror_joint_pos, None, None) + return ik_constraint + + + def generate_orientation_constraint_from_frame(self, frame, joint_name): + if joint_name in self.skeleton.nodes: + m = self.skeleton.nodes[joint_name].get_global_matrix(frame)#[:3,:3] + return quaternion_from_matrix(m) + + def convert_to_ik_constraints_with_relative(self, frames, constraints, frame_offset=0, time_function=None, constrain_orientation=True): + """ Create an orderered dictionary containing for each constrained frame, a KeyframeConstraint for the constrained joint""" + ik_constraints = collections.OrderedDict() + for c in constraints: + if c.constraint_type not in SUPPORTED_CONSTRAINT_TYPES or "generated" in c.semantic_annotation.keys(): + print("skip unsupported constraint") + continue + start_frame_idx = self.get_global_frame_idx(c.canonical_keyframe, frame_offset, time_function) + if c.canonical_end_keyframe is not None: + print("apply ik constraint on region") + end_frame_idx = self.get_global_frame_idx(c.canonical_end_keyframe, frame_offset, time_function) + else: + print("no end keyframe defined") + end_frame_idx = start_frame_idx + 1 + if c.orientation is None and self.force_orientation: + c.orientation = self.generate_orientation_constraint_from_frame(frames[start_frame_idx], c.joint_name) + + base_ik_constraint = self.convert_mg_constraint_to_ik_constraint(start_frame_idx, c, constrain_orientation) + + relative_ik_constraint = None + if base_ik_constraint is None: + continue + base_ik_constraint.src_tool_cos = c.src_tool_cos + base_ik_constraint.dest_tool_cos = c.dest_tool_cos + for frame_idx in range(start_frame_idx, end_frame_idx): + ik_constraint = deepcopy(base_ik_constraint) + if frame_idx not in ik_constraints: + ik_constraints[frame_idx] = dict() + + ik_constraint.orientation = None + if c.orientation is not None and constrain_orientation: + if c.constrain_orientation_in_region: + ik_constraint.orientation = c.orientation + if start_frame_idx < frame_idx < end_frame_idx -1: + ik_constraint.inside_region_orientation = True + elif frame_idx == start_frame_idx: + ik_constraint.inside_region_orientation = True + ik_constraint.keep_orientation = True + elif frame_idx == start_frame_idx: + ik_constraint.orientation = c.orientation + + ik_constraint.position = None + if c.position is not None: + if c.constrain_position_in_region and c.relative_joint_name is None:# ignore region if it is relative + ik_constraint.inside_region_position = False + ik_constraint.position = c.position + if start_frame_idx < frame_idx < end_frame_idx -1: + ik_constraint.inside_region_position = True + elif frame_idx == end_frame_idx-1: + ik_constraint.end_of_region = True + elif frame_idx == start_frame_idx: + ik_constraint.position = c.position + # overwrite with relative constraint if larger than start frame + if c.relative_joint_name is not None and frame_idx == start_frame_idx: + relative_ik_constraint = self.generate_relative_constraint(frame_idx, frames[frame_idx], + c.joint_name, + c.relative_joint_name) + relative_ik_constraint.orientation = ik_constraint.orientation + ik_constraints[frame_idx][c.joint_name] = ik_constraint + + elif relative_ik_constraint is not None and frame_idx > start_frame_idx: + relative_ik_constraint.frame_idx = frame_idx + ik_constraints[frame_idx][c.joint_name] = relative_ik_constraint + else: + ik_constraints[frame_idx][c.joint_name] = ik_constraint + + # add also a mirror constraint + if c.mirror_joint_name is not None: + _ik_constraint = self.generate_mirror_constraint(frame_idx, frames[0], frames[frame_idx], c.mirror_joint_name) + ik_constraints[frame_idx][c.mirror_joint_name] = _ik_constraint + + # add also a parent constraint + if c.constrained_parent is not None and c.vector_to_parent is not None: + print("generate parent constraint", c.vector_to_parent) + # "get length of vector and calculate constraint position" + a = self.skeleton.nodes[c.joint_name].get_global_position(frames[0]) + b = self.skeleton.nodes[c.constrained_parent].get_global_position(frames[0]) + bone_length = np.linalg.norm(b-a) + c.vector_to_parent /= np.linalg.norm(c.vector_to_parent) + parent_pos = c.position + c.vector_to_parent * bone_length + + #create a keyframe constraint but indicate that it is a relative constraint + _ik_constraint = KeyframeConstraint(frame_idx, c.constrained_parent, parent_pos, None, None) + + + #remove degrees of freedom of the child constraint + ik_constraints[frame_idx][c.joint_name].fk_chain_root = c.constrained_parent + + #change order so that the parent constraint is applied first + # TODO order constraints based on joint order in fk chain + reversed_ordered_dict = collections.OrderedDict() + reversed_ordered_dict[c.constrained_parent] = _ik_constraint + for k, v in ik_constraints[frame_idx].items(): + reversed_ordered_dict[k] = v + ik_constraints[frame_idx] = reversed_ordered_dict + return ik_constraints diff --git a/build/lib/anim_utils/motion_editing/motion_editing.py b/build/lib/anim_utils/motion_editing/motion_editing.py new file mode 100644 index 0000000..82260fa --- /dev/null +++ b/build/lib/anim_utils/motion_editing/motion_editing.py @@ -0,0 +1,900 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +from copy import copy +import numpy as np +import collections +from transformations import quaternion_matrix, euler_from_matrix, quaternion_multiply, quaternion_matrix, quaternion_from_matrix +from .numerical_ik_quat import NumericalInverseKinematicsQuat +from .numerical_ik_exp import NumericalInverseKinematicsExp +from .skeleton_pose_model import SkeletonPoseModel +from .cubic_motion_spline import CubicMotionSpline, get_quaternion_delta +from ..animation_data.motion_blending import smooth_joints_around_transition_using_slerp, create_transition_using_slerp, \ + smooth_quaternion_frames, create_transition_for_joints_using_slerp, \ + BLEND_DIRECTION_BACKWARD, BLEND_DIRECTION_FORWARD, smooth_translation_in_quat_frames +from ..utilities.log import write_message_to_log, LOG_MODE_DEBUG +from .utils import convert_exp_frame_to_quat_frame +from .fabrik_chain import FABRIKChain, FABRIKBone +from ..animation_data.skeleton import LOOK_AT_DIR, SPINE_LOOK_AT_DIR + +SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION = "keyframe_position" +SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION = "keyframe_relative_position" +SUPPORTED_CONSTRAINT_TYPES = [SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION, SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION] + + + +def create_fabrik_chain(skeleton, frame, node_order, activate_constraints=False): + bones = dict() + root = node_order[0] + root_offset = skeleton.nodes[root].get_global_position(frame) + frame_offset = skeleton.animated_joints.index(root)*4 + 3 + for idx, j in enumerate(node_order[:-1]): + bones[j] = FABRIKBone(j, node_order[idx + 1]) + if idx == 0: + bones[j].is_root = True + else: + bones[j].is_root = False + + bones[node_order[-1]] = FABRIKBone(node_order[-1], None) + max_iter = 50 + chain = FABRIKChain(skeleton, bones, node_order, max_iter=max_iter, frame_offset=frame_offset, root_offset=root_offset, + activate_constraints=activate_constraints) + return chain + +def add_frames(skeleton, a, b): + """ returns c = a + b""" + c = np.zeros(len(a)) + c[:3] = a[:3] + b[:3] + for idx, j in enumerate(skeleton.animated_joints): + o = idx * 4 + 3 + q_a = a[o:o + 4] + q_b = b[o:o + 4] + q_prod = quaternion_multiply(q_a, q_b) + c[o:o + 4] = q_prod / np.linalg.norm(q_prod) + return c + +def add_reduced_frames(skeleton, a, delta, joints): + """ returns c = a + delta where delta are the parameters of the joints list""" + c = np.array(a) + o = 0 + for idx, j in enumerate(skeleton.animated_joints): + if j not in joints: + continue + if j == skeleton.root: + dest = 0 + c[:3] = a[dest:dest+3] + delta[o:o+3] + q_dest = dest+3 + q_o = o+3 + q_a = a[q_dest:q_dest + 4] + q_delta = delta[q_o:q_o + 4] + + q_prod = quaternion_multiply(q_a, q_delta) + c[q_dest:q_dest + 4] = q_prod / np.linalg.norm(q_prod) + o += 7 + else: + dest = idx* 4 + 3 + + q_a = a[dest:dest + 4] + q_delta = delta[o:o + 4] + + q_prod = quaternion_multiply(q_a, q_delta) + c[dest:dest + 4] = q_prod / np.linalg.norm(q_prod) + o += 4 + return c + +def substract_frames(skeleton, a, b): + """ returns c = a - b""" + c = np.zeros(len(a)) + c[:3] = a[:3] - b[:3] + for idx, j in enumerate(skeleton.animated_joints): + o = idx*4 + 3 + q_a = a[o:o+4] + q_b = b[o:o+4] + if np.dot(q_a, q_b) < 0: + q_a *= -1 + q_delta = get_quaternion_delta(q_a, q_b) + q_delta = q_delta / np.linalg.norm(q_delta) + #dot = np.sum(q_delta) + #if dot < 0: + # q_delta = -q_delta + c[o:o+4] = q_delta + return c + + +class KeyframeConstraint(object): + def __init__(self, frame_idx, joint_name, position, orientation=None, look_at=False, offset=None, look_at_pos=None): + self.frame_idx = frame_idx + self.joint_name = joint_name + self.position = position + self.orientation = orientation + self.look_at = look_at + self.look_at_pos = look_at_pos + self.offset = offset # tool offset + self.inside_region_position = False + self.end_of_region = False + self.inside_region_orientation = False + self.keep_orientation = False + + # set in case it is a has a relative constraint + self.relative_parent_joint_name = None # joint the offsets points from to the target + self.relative_offset = None + + # tool orientation constraint + self.src_tool_cos = None # tool coordinate system + self.dest_tool_cos = None # target direction + + # set a fk chain root to reduce the degrees of freedom + self.fk_chain_root = None + + def instantiate_relative_constraint(self, skeleton, frame): + """ turn relative constraint into a normal constraint""" + ppos = skeleton.nodes[self.relative_parent_joint_name].get_global_position(frame) + pos = ppos + self.relative_offset + return KeyframeConstraint(self.frame_idx, self.joint_name, pos, orientation=self.orientation) + + def evaluate(self, skeleton, frame): + if self.orientation is not None: + parent_joint = skeleton.nodes[self.joint_name].parent + if parent_joint is not None: + m = quaternion_matrix(self.orientation) + parent_m = parent_joint.get_global_matrix(frame, use_cache=False) + local_m = np.dot(np.linalg.inv(parent_m), m) + q = quaternion_from_matrix(local_m) + idx = skeleton.animated_joints.index(parent_joint.node_name) + # idx = skeleton.nodes[c.joint_name].quaternion_frame_index * 4 + frame[idx:idx + 4] = q + if self.offset is not None: + m = skeleton.nodes[self.joint_name].get_global_matrix(frame) + p = np.dot(m, self.offset)[:3] + d = self.position - p + else: + d = self.position - skeleton.nodes[self.joint_name].get_global_position(frame) + return np.dot(d, d) + + +class MotionEditing(object): + def __init__(self, skeleton, ik_settings): + self.skeleton = skeleton + self._ik_settings = ik_settings + self.window = int(self._ik_settings["interpolation_window"]) + self.transition_window = int(self._ik_settings["transition_window"]) + self.verbose = False + self.use_euler = self._ik_settings["use_euler_representation"] + self.solving_method = self._ik_settings["solving_method"] + self.success_threshold = self._ik_settings["success_threshold"] + self.max_retries = int(self._ik_settings["max_retries"]) + self.activate_look_at = self._ik_settings["activate_look_at"] + self.optimize_orientation = self._ik_settings["optimize_orientation"] + self.elementary_action_max_iterations = int(self._ik_settings["elementary_action_max_iterations"]) + self.elementary_action_epsilon = self._ik_settings["elementary_action_optimization_eps"] + self.adapt_hands_during_both_hand_carry = self._ik_settings["adapt_hands_during_carry_both"] + self.pose = SkeletonPoseModel(self.skeleton, self.use_euler) + self._ik = NumericalInverseKinematicsQuat(self.pose, self._ik_settings) + self._ik_exp = NumericalInverseKinematicsExp(self.skeleton, self._ik_settings) + self._fabrik_chains = dict() + + def add_fabrik_chain(self, joint_name, node_order, activate_constraints=False): + self._fabrik_chains[joint_name] = create_fabrik_chain(self.skeleton, self.skeleton.reference_frame, node_order, activate_constraints) + + def add_constraints_to_skeleton(self, joint_constraints): + """ legacy interface""" + self.skeleton.add_constraints(joint_constraints) + + def modify_motion_vector(self, motion_vector): + for idx, action_ik_constraints in enumerate(motion_vector.ik_constraints): + write_message_to_log("Apply IK to elementary action " + str(idx), LOG_MODE_DEBUG) + self._optimize_action_ik_constraints(motion_vector, action_ik_constraints) + + def _optimize_action_ik_constraints(self, motion_vector, action_ik_constraints): + i = 0 + last_error = None + keep_running = True + trajectory_weights = 1.0 + # modify individual keyframes based on constraints + while keep_running: + error = 0.0 + if "trajectories" in list(action_ik_constraints.keys()): + constraints = action_ik_constraints["trajectories"] + c_error = self._modify_motion_vector_using_trajectory_constraint_list(motion_vector, constraints) + error += c_error * trajectory_weights + if "keyframes" in list(action_ik_constraints.keys()): + constraints = action_ik_constraints["keyframes"] + error += self._modify_motion_vector_using_keyframe_constraint_list(motion_vector, constraints) + if last_error is not None: + delta = abs(last_error - error) + else: + delta = np.inf + last_error = error + i += 1 + keep_running = i < self.elementary_action_max_iterations and delta > self.elementary_action_epsilon + write_message_to_log("IK iteration " + str(i) + " " + str(error) + " " + str(delta) + " " + str( + self.elementary_action_epsilon), LOG_MODE_DEBUG) + + def _modify_motion_vector_using_keyframe_constraint_list(self, motion_vector, constraints): + error = 0.0 + for keyframe, keyframe_constraints in list(constraints.items()): + keyframe = int(keyframe) + if "single" in list(keyframe_constraints.keys()): + for c in keyframe_constraints["single"]: + if c.optimize: + if c.frame_range is not None: + error += self._modify_motion_vector_using_keyframe_constraint_range(motion_vector, c, + c.frame_range) + else: + error += self._modify_frame_using_keyframe_constraint(motion_vector, c, keyframe) + start = keyframe + end = keyframe + 1 + if self.activate_look_at and c.look_at: + self._look_at_in_range(motion_vector.frames, c.position, start, end) + print("set hand orientation", c.orientation) + if c.orientation is not None and self.optimize_orientation: + self._set_hand_orientation(motion_vector.frames, c.orientation, c.joint_name, keyframe, start, end) + return error + + def _modify_frame_using_keyframe_constraint(self, motion_vector, constraint, keyframe): + self.set_pose_from_frame(motion_vector.frames[keyframe]) + error = self._ik.modify_pose_general(constraint) + motion_vector.frames[keyframe] = self.pose.get_vector() + if self.window > 0: + self.interpolate_around_keyframe(motion_vector.frames, constraint.get_joint_names(), keyframe, self.window) + return error + + def _modify_motion_vector_using_keyframe_constraint_range(self, motion_vector, constraint, frame_range): + error = 0.0 + for frame in range(frame_range[0], frame_range[1] + 1): + self.set_pose_from_frame(motion_vector.frames[frame]) + error += self._ik.modify_pose_general(constraint) + motion_vector.frames[frame] = self.pose.get_vector() + + self._create_transition_for_frame_range(motion_vector.frames, frame_range[0], frame_range[1], + self.pose.free_joints_map[constraint.joint_name]) + return error + + def interpolate_around_keyframe(self, frames, joint_names, keyframe, window): + write_message_to_log("Smooth and interpolate" + str(joint_names), LOG_MODE_DEBUG) + for target_joint_name in joint_names: + joint_parameter_indices = self._extract_free_parameter_indices(self.pose.free_joints_map[target_joint_name]) + for joint_name in self.pose.free_joints_map[target_joint_name]: + smooth_joints_around_transition_using_slerp(frames, joint_parameter_indices[joint_name], keyframe, window) + + def _look_at_in_range(self, frames, position, start, end): + start = max(0, start) + end = min(frames.shape[0], end) + for idx in range(start, end): + self.set_pose_from_frame(frames[idx]) + self.pose.lookat(position) + frames[idx] = self.pose.get_vector() + self._create_transition_for_frame_range(frames, start, end - 1, [self.pose.head_joint]) + + def _create_transition_for_frame_range(self, frames, start, end, target_joints): + for target_joint in target_joints: + joint_parameter_indices = list(range(*self.pose.extract_parameters_indices(target_joint))) + transition_start = max(start - self.transition_window, 0) + transition_end = min(end + self.transition_window, frames.shape[0]) - 1 + create_transition_using_slerp(frames, transition_start, start, joint_parameter_indices) + create_transition_using_slerp(frames, end, transition_end, joint_parameter_indices) + + def _set_hand_orientation(self, frames, orientation, joint_name, keyframe, start, end): + parent_joint_name = self.pose.get_parent_joint(joint_name) + self.set_pose_from_frame(frames[keyframe]) + self.pose.set_hand_orientation(parent_joint_name, orientation) + start = max(0, start) + end = min(frames.shape[0], end) + self._create_transition_for_frame_range(frames, start, end - 1, [parent_joint_name]) + + def set_pose_from_frame(self, reference_frame): + self.pose.set_pose_parameters(reference_frame) + self.pose.clear_cache() + + def _extract_free_parameter_indices(self, free_joints): + """get parameter indices of joints from reference frame + """ + indices = {} + for joint_name in free_joints: + indices[joint_name] = list(range(*self.pose.extract_parameters_indices(joint_name))) + return indices + + def _modify_motion_vector_using_trajectory_constraint_list(self, motion_vector, constraints): + error = 0.0 + for c in constraints: + if c["fixed_range"]: + error += self._modify_motion_vector_using_trajectory_constraint(motion_vector, c) + else: + error += self._modify_motion_vector_using_trajectory_constraint_search_start(motion_vector, c) + return error + + def _modify_motion_vector_using_trajectory_constraint(self, motion_vector, traj_constraint): + error_sum = 0.0 + d = traj_constraint["delta"] + trajectory = traj_constraint["trajectory"] + start_idx = traj_constraint["start_frame"] + end_idx = traj_constraint["end_frame"] - 1 + end_idx = min(len(motion_vector.frames) - 1, end_idx) + n_frames = end_idx - start_idx + 1 + target_direction = None + if traj_constraint["constrain_orientation"]: + target_direction = trajectory.get_direction() + if np.linalg.norm(target_direction) == 0: + target_direction = None + + full_length = n_frames * d + for idx in range(n_frames): + t = (idx * d) / full_length + target_position = trajectory.query_point_by_parameter(t) + keyframe = start_idx + idx + self.set_pose_from_frame(motion_vector.frames[keyframe]) + error = np.inf + iter_counter = 0 + while error > self.success_threshold and iter_counter < self.max_retries: + error = self._ik.modify_pose(traj_constraint["joint_name"], target_position, target_direction) + iter_counter += 1 + error_sum += error + motion_vector.frames[keyframe] = self.pose.get_vector() + parent_joint = self.pose.get_parent_joint(traj_constraint["joint_name"]) + + if traj_constraint["joint_name"] in list(self.pose.free_joints_map.keys()): + free_joints = self.pose.free_joints_map[traj_constraint["joint_name"]] + free_joints = list(set(free_joints + [parent_joint])) + else: + free_joints = [parent_joint] + self._create_transition_for_frame_range(motion_vector.frames, start_idx, end_idx, free_joints) + return error_sum + + def _modify_motion_vector_using_trajectory_constraint_search_start(self, motion_vector, traj_constraint): + error_sum = 0.0 + trajectory = traj_constraint["trajectory"] + start_target = trajectory.query_point_by_parameter(0.0) + start_idx = self._find_corresponding_frame(motion_vector, + traj_constraint["start_frame"], + traj_constraint["end_frame"], + traj_constraint["joint_name"], + start_target) + n_frames = traj_constraint["end_frame"]-start_idx + 1 + arc_length = 0.0 + self.set_pose_from_frame(motion_vector.frames[start_idx]) + prev_position = self.pose.evaluate_position(traj_constraint["joint_name"]) + for idx in range(n_frames): + keyframe = start_idx+idx + self.set_pose_from_frame(motion_vector.frames[keyframe]) + current_position = self.pose.evaluate_position(traj_constraint["joint_name"]) + arc_length += np.linalg.norm(prev_position-current_position) + prev_position = current_position + if arc_length >= trajectory.full_arc_length: + break + target = trajectory.query_point_by_absolute_arc_length(arc_length) + + error = np.inf + iter_counter = 0 + while error > self.success_threshold and iter_counter < self.max_retries: + error = self._ik.modify_pose(traj_constraint["joint_name"], target) + iter_counter += 1 + error_sum += error + motion_vector.frames[keyframe] = self.pose.get_vector() + + self._create_transition_for_frame_range(motion_vector.frames, start_idx, keyframe-1, self.pose.free_joints_map[traj_constraint["joint_name"]]) + return error_sum + + def _find_corresponding_frame(self, motion_vector, start_idx, end_idx, target_joint, target_position): + closest_start_frame = copy(start_idx) + min_error = np.inf + n_frames = end_idx - start_idx + for idx in range(n_frames): + keyframe = start_idx + idx + self.set_pose_from_frame(motion_vector.frames[keyframe]) + position = self.pose.evaluate_position(target_joint) + error = np.linalg.norm(position - target_position) + if error <= min_error: + min_error = error + closest_start_frame = keyframe + return closest_start_frame + + def generate_zero_frame(self): + n_dims = len(self.skeleton.animated_joints) * 4 + 3 + zero_frame = np.zeros(n_dims) + for j in range(len(self.skeleton.animated_joints)): + o = j * 4 + 3 + zero_frame[o:o + 4] = [1, 0, 0, 0] + return zero_frame + + def generate_delta_frames(self, frames, constraints, influence_range=40): + n_frames = frames.shape[0] + zero_frame = self.generate_zero_frame() + constrained_frames = list(constraints.keys()) + delta_frames = collections.OrderedDict() + delta_frames[0] = zero_frame + delta_frames[1] = zero_frame + for f in range(0, n_frames, influence_range): + delta_frames[f] = zero_frame + delta_frames[f+1] = zero_frame + delta_frames[n_frames - 2] = zero_frame + delta_frames[n_frames - 1] = zero_frame + for frame_idx, frame_constraints in constraints.items(): + # delete zero frames in range around constraint + start = max(frame_idx - influence_range, min(frame_idx, 2)) + end = min(frame_idx + influence_range, max(frame_idx, n_frames - 2)) + for i in range(start, end): + if i in delta_frames and i not in constrained_frames: + del delta_frames[i] + + frame_constraints = list(frame_constraints.values()) + exp_frame = self._ik_exp.run(frames[frame_idx], frame_constraints) + + n_dims = len(self.skeleton.animated_joints) * 4 + 3 + delta_frames[frame_idx] = np.zeros(n_dims) + delta_frames[frame_idx][3:] = convert_exp_frame_to_quat_frame(self.skeleton, exp_frame) + delta_frames = collections.OrderedDict(sorted(delta_frames.items(), key=lambda x: x[0])) + return list(delta_frames.keys()), np.array(list(delta_frames.values())) + + def modify_motion_vector2(self, motion_vector, plot=False): + motion_vector.frames = self.edit_motion_using_displacement_map(motion_vector.frames, motion_vector.ik_constraints, plot=plot) + self.apply_orientation_constraints(motion_vector.frames, motion_vector.ik_constraints) + + def edit_motion_using_displacement_map(self, frames, constraints, influence_range=40, plot=False): + """ References + Witkin and Popovic: Motion Warping, 1995. + Bruderlin and Williams: Motion Signal Processing, 1995. + Lee and Shin: A Hierarchical Approach to Interactive Motion Editing for Human-like Figures, 1999. + """ + d_times, delta_frames = self.generate_delta_frames(frames, constraints, influence_range) + return self.add_delta_curve(frames, d_times, delta_frames, plot=plot) + + def get_reduced_frame(self, frame, joint_list): + n_dims = len(joint_list)*4 + if self.skeleton.root in joint_list: + n_dims += 3 + rf = np.zeros(n_dims) + o = 0 + for idx, j in enumerate(self.skeleton.animated_joints): + if j not in joint_list: + continue + if j == self.skeleton.root: + src = 0 + rf[o:o+7] = frame[src:src+7] + o+=7 + else: + src = idx * 4 + 3 + rf[o:o+4] = frame[src:src+4] + o+=4 + return rf + + def add_delta_curve(self, frames, d_times, delta_frames, plot=False): + n_frames = len(frames) + times = list(range(n_frames)) + d_curve = CubicMotionSpline.fit_frames(self.skeleton, d_times, delta_frames) + new_frames = [] + for t in times: + d_frame = d_curve.evaluate(t) + f = add_frames(self.skeleton, frames[t], d_frame) + new_frames.append(f) + if plot: + t = np.linspace(0, n_frames - 1, num=100, endpoint=True) + d_curve.plot(t) + return np.array(new_frames) + + def add_reduced_delta_curve(self, frames, d_times, delta_frames, joint_list=None, plot=False): + n_frames = len(frames) + times = list(range(n_frames)) + if joint_list is not None: + reduced_delta_frames = [] + for f in delta_frames: + rf = self.get_reduced_frame(f, joint_list) + reduced_delta_frames.append(rf) + reduced_delta_frames = np.array(reduced_delta_frames) + else: + reduced_delta_frames = delta_frames + d_curve = CubicMotionSpline.fit_frames(self.skeleton, d_times, reduced_delta_frames) + new_frames = [] + if joint_list is None: + joint_list = self.skeleton.animated_joints + for t in times: + d_frame = d_curve.evaluate(t) + f = add_reduced_frames(self.skeleton, frames[t], d_frame, joint_list) + new_frames.append(f) + + if plot: + t = np.linspace(0, n_frames - 1, num=100, endpoint=True) + d_curve.plot(t) + return np.array(new_frames) + + def generate_delta_frames_using_ccd(self, frames, constraints, n_max_iter=25, root_joint=None, influence_range=40): + n_frames = frames.shape[0] + zero_frame = self.generate_zero_frame() + constrained_frames = list(constraints.keys()) + delta_frames = collections.OrderedDict() + delta_frames[0] = zero_frame + delta_frames[1] = zero_frame + joint_list = set() + chain_end_joints = dict() + for frame_idx, frame_constraints in constraints.items(): + for joint_name in frame_constraints: + if joint_name not in chain_end_joints: + chain_end_joints[joint_name] = root_joint + for j in self.get_fk_chain(joint_name, root_joint): + joint_list.add(j) + for f in range(0, n_frames, influence_range): + delta_frames[f] = zero_frame + delta_frames[f+1] = zero_frame + delta_frames[n_frames - 2] = zero_frame + delta_frames[n_frames - 1] = zero_frame + + + for frame_idx, frame_constraints in constraints.items(): + # delete zero frames in range around constraint + start = max(frame_idx - influence_range, min(frame_idx, 2)) + end = min(frame_idx + influence_range, max(frame_idx, n_frames - 2)) + for i in range(start, end): + if i in delta_frames and i not in constrained_frames: + del delta_frames[i] + + frame_constraints = list(frame_constraints.values()) + frame_copy = np.array(frames[frame_idx]) + new_frame = self.skeleton.reach_target_positions(frame_copy, frame_constraints, chain_end_joints, n_max_iter=n_max_iter, verbose=False) + delta_frames[frame_idx] = substract_frames(self.skeleton, new_frame, frames[frame_idx]) + delta_frames = collections.OrderedDict(sorted(delta_frames.items(), key=lambda x: x[0])) + return list(delta_frames.keys()), np.array(list(delta_frames.values())), list(joint_list) + + def edit_motion_using_displacement_map_and_ccd(self, frames, constraints, n_max_iter=100, root_joint=None, transition_window=None, plot=False): + """ Apply IK and create a transition using a displacement map + References: + Witkin and Popovic: Motion Warping, 1995. + Bruderlin and Williams: Motion Signal Processing, 1995. + Lee and Shin: A Hierarchical Approach to Interactive Motion Editing for Human-like Figures, 1999. + Args: + frames(np.array): input frames to be modified + constraints(list): list of constraints + n_max_iter(int): optional maximum ik iterations + root_joint(str): optional root joint of ik chain + transition_window(int): optional blending window size + plot(bool): optional plot delta curve + Returns: + frames(np.array): modifed frames + """ + if transition_window is None: + transition_window = self.transition_window + d_times, delta_frames, joint_list = self.generate_delta_frames_using_ccd(frames, constraints, n_max_iter, root_joint, transition_window) + if self.skeleton.skeleton_model is not None and "joints" in self.skeleton.skeleton_model and "neck" in self.skeleton.skeleton_model["joints"]: + joint_name = self.skeleton.skeleton_model["joints"]["neck"] + if joint_name is not None: + joint_list.append(joint_name) + return self.add_reduced_delta_curve(frames, d_times, delta_frames, joint_list, plot=plot) + + def apply_orientation_constraints(self, frames, constraints): + for frame_idx, frame_constraints in constraints.items(): + for joint_name, c in frame_constraints.items(): + if c.orientation is not None and self.optimize_orientation: + start = c.frame_idx + end = c.frame_idx + 1 + if self.activate_look_at and c.look_at: + self._look_at_in_range(frames, c.position, start, end) + print("set hand orientation", c.orientation) + self._set_hand_orientation(frames, c.orientation, c.joint_name, c.frame_idx, start, end) + + def edit_motion_using_fabrik(self, frames, constraints): + new_frames = np.array(frames) + for frame_idx, frame_constraints in constraints.items(): + joint_names = [] + fk_nodes = set() + for joint_name, c in frame_constraints.items(): + print("use fabrik on", joint_name, "at", frame_idx) + if joint_name in self._fabrik_chains: + joint_names += self._fabrik_chains[joint_name].node_order[:1] + new_frame = self._fabrik_chains[joint_name].run_partial_with_constraints(frames[frame_idx], c.position) + new_frames[frame_idx] = new_frame + joint_fk_nodes = self.skeleton.nodes[joint_name].get_fk_chain_list() + fk_nodes.update(joint_fk_nodes) + + if self.window > 0: + self.interpolate_around_frame(fk_nodes, new_frames, frame_idx, self.window) + return new_frames + + def edit_motion_to_look_at_target(self, frames, look_at_target, spine_target, start_idx, end_idx, orient_spine=False, look_at_dir=LOOK_AT_DIR, spine_look_at_dir=SPINE_LOOK_AT_DIR): + if look_at_target is None: + return frames + spine_joint_name = self.skeleton.skeleton_model["joints"]["spine_1"] + head_joint_name = self.skeleton.skeleton_model["joints"]["head"] + self.skeleton.clear_cached_global_matrices() + fk_nodes = None + for frame_idx in range(start_idx, end_idx): + if orient_spine and spine_target is not None: + frames[frame_idx] = self.skeleton.look_at_projected(frames[frame_idx], spine_joint_name, spine_target, local_dir=spine_look_at_dir) + frames[frame_idx] = self.skeleton.look_at(frames[frame_idx], head_joint_name, look_at_target, n_max_iter=2, local_dir=look_at_dir, chain_end_joint=spine_joint_name) + n_joints = len(self.skeleton.animated_joints) + fk_nodes = self.skeleton.nodes[head_joint_name].get_fk_chain_list() + if fk_nodes is not None: + self.interpolate_around_frame(fk_nodes, frames, start_idx, self.window) + if end_idx < len(frames): + self.interpolate_around_frame(fk_nodes, frames, end_idx, self.window) + return frames + + def get_static_joints(self, frame_constraints): + static_joints = set() + for joint_name, c in frame_constraints.items(): + if c.inside_region_position: + static_joints.add(joint_name) + return static_joints + + def find_free_root_joints(self, constraints, joint_chains): + """ check for each joint in the constraints if it is free""" + root_joints = dict() + for c in constraints: + root_joints[c.joint_name] = None + for free_joint in joint_chains[c.joint_name]: + is_free = True + for joint_name in joint_chains: + if joint_name == c.joint_name: + continue + if free_joint in joint_chains[joint_name]: + is_free = False + if not is_free: + root_joints[c.joint_name] = free_joint + print("set root joint for ", c.joint_name, "to", free_joint) + break + return root_joints + + def get_fk_chain(self, joint_name, root_joint): + joint_chain = [] + joint_fk_nodes = self.skeleton.nodes[joint_name].get_fk_chain_list() + abort = False + if root_joint is not None: # remove root joint + for j in joint_fk_nodes: + joint_chain.append(j) + if abort: + break + if j == root_joint: + abort = True + else: + joint_chain = joint_fk_nodes + print("copy fk chain", joint_name, joint_chain) + return joint_chain + + def get_active_constraints(self, new_frames, frame_idx, frame_constraints, joint_chain_buffer, prev_static_joints, root_joint): + + keep_static_joints = True + static_joints = self.get_static_joints(frame_constraints) + active_constraints = [] + region_overlaps = [] + fk_nodes = set() + for joint_name, c in frame_constraints.items(): + copied_joints = False + if joint_name not in joint_chain_buffer: + j_root_joint = root_joint + if c.fk_chain_root is not None: + j_root_joint = c.fk_chain_root + joint_chain_buffer[joint_name] = self.get_fk_chain(joint_name, None) # copy entire chain + if c.inside_region_position and prev_static_joints == static_joints: + #print("copy parameters for", joint_name, len(joint_chain_buffer[joint_name])) + #copy guess from previous frame if it is part of a region + #print("copy parameters", frame_idx) + self.copy_joint_parameters(joint_chain_buffer[joint_name], new_frames, frame_idx - 1, frame_idx) + copied_joints = True + if not copied_joints or not keep_static_joints: + #if c.orientation is not None: + # print("use ccd on", joint_name, "at", frame_idx, " with orientation") + #else: + # print("use ccd on", joint_name, "at", frame_idx) + active_constraints.append(c) + fk_nodes.update(joint_chain_buffer[joint_name]) + if c.inside_region_position and prev_static_joints != static_joints: + region_overlaps.append(frame_idx) + return active_constraints, fk_nodes, region_overlaps, joint_chain_buffer, static_joints + + def edit_motion_using_ccd(self, frames, constraints, n_max_iter=100, root_joint=None, activate_smoothing=True): + """ edit frame parameters using ccd and applying blending""" + + new_frames = np.array(frames) + joint_chain_buffer = dict() + delta_frames = dict() + n_frames = len(frames) + prev_static_joints = set() + + region_overlaps = [] + for frame_idx, frame_constraints in constraints.items(): + active_constraints, fk_nodes, _region_overlaps, joint_chain_buffer, static_joints = self.get_active_constraints(new_frames, frame_idx, frame_constraints, joint_chain_buffer, prev_static_joints, root_joint) + region_overlaps += _region_overlaps + if len(active_constraints) > 0: + #print("find free joints at", frame_idx) + if len(static_joints) > 0: + chain_end_joints = self.find_free_root_joints(active_constraints, joint_chain_buffer) + elif root_joint is not None: + chain_end_joints = dict() + for c in active_constraints: + chain_end_joints[c.joint_name] = root_joint + else: + chain_end_joints = None + # init frame with changes from prev frame if it was edited + prev_frame_idx = frame_idx-1 + if prev_frame_idx in delta_frames: + new_frames[frame_idx] = add_frames(self.skeleton, frames[frame_idx], delta_frames[prev_frame_idx]) + #print("apply delta",delta_frames[prev_frame_idx]) + + new_frame = self.skeleton.reach_target_positions(new_frames[frame_idx], active_constraints, chain_end_joints, n_max_iter=n_max_iter, verbose=False) + delta_frames[frame_idx] = substract_frames(self.skeleton,new_frame, frames[frame_idx]) + + # interpolate outside of region constraints + is_at_border = self.is_at_constrain_region_border(frame_idx, constraints) + if is_at_border and self.window > 0 and len(active_constraints) > 0 and len(fk_nodes) > 0 and activate_smoothing: + #print("outside of region", list(prev_static_joints), list(static_joints)) + fk_nodes = list(fk_nodes) + #fk_nodes = self.skeleton.animated_joints + self.interpolate_around_frame(fk_nodes, new_frames, frame_idx, self.window) + #new_frames = smooth_quaternion_frames(new_frames, frame_idx, self.window, False) + prev_static_joints = static_joints + if activate_smoothing: + for frame_idx in region_overlaps: + new_frames = smooth_quaternion_frames(new_frames, frame_idx, self.window, False) + return new_frames + + def is_at_constrain_region_border(self, frame_idx, constraints): + """check if the frame index is at the border of a constrained region """ + prev_frame_unconstrained = True + if frame_idx + 1 in constraints: + prev_frame_unconstrained = len(constraints[frame_idx+1]) == 0 + next_frame_unconstrained = True + if frame_idx-1 in constraints: + next_frame_unconstrained = len(constraints[frame_idx-1]) == 0 + return next_frame_unconstrained or prev_frame_unconstrained + + def apply_carry_constraints(self, frames, constraints): + print("generate carry constraints") + n_frames = frames.shape[0] + active_orientations = dict() + for frame_idx in range(0, n_frames): + # update active orientations + if frame_idx in constraints: + for joint_name, c in constraints[frame_idx].items(): + if c.keep_orientation and c.orientation is not None: + active_orientations[c.joint_name] = c.orientation + elif c.joint_name in active_orientations: + active_orientations[c.joint_name] = None + #else: + # print("no constraint on frame", frame_idx, c.keep_orientation) + # apply active orientations + for joint_name in active_orientations: + if active_orientations[joint_name] is not None: + #print("set orientation for", joint_name, "at", frame_idx) + frames[frame_idx] = self.skeleton.set_joint_orientation(frames[frame_idx], joint_name, active_orientations[joint_name] ) + return frames + + def set_global_joint_orientations(self, frames, constraints, frame_offset=0, time_function=None): + for c in constraints: + if c.constraint_type not in SUPPORTED_CONSTRAINT_TYPES or "generated" in c.semantic_annotation.keys(): + #print("skip unsupported constraint") + continue + joint_name = c.joint_name + start_frame_idx = self.get_global_frame_idx(c.canonical_keyframe, frame_offset, time_function) + if c.constrain_orientation_in_region and c.canonical_end_keyframe is not None: + end_frame_idx = self.get_global_frame_idx(c.canonical_end_keyframe, frame_offset, time_function) + for frame_idx in range(start_frame_idx, end_frame_idx): + frames[frame_idx] = self.skeleton.set_joint_orientation(frames[frame_idx], joint_name, c.orientation) + return frames + + def get_global_frame_idx(self, mp_frame_idx, frame_offset, time_function): + if time_function is not None: + frame_idx = frame_offset + int(time_function[mp_frame_idx]) + 1 + else: + frame_idx = frame_offset + int(mp_frame_idx) + return frame_idx + + def set_joint_orientation(self, joint_name, frames, start_idx, end_idx, target_orientation): + for frame_idx in range(start_idx, end_idx): + frames[frame_idx] = self.skeleton.set_joint_orientation(frames[frame_idx], joint_name, target_orientation) + + def copy_joint_parameters(self, nodes, frames, src_idx, dst_idx): + for node in nodes: + if self.skeleton.nodes[node].quaternion_frame_index == 0: + frames[dst_idx][:7] = frames[src_idx][:7] + else: + o = self.skeleton.nodes[node].quaternion_frame_index * 4 + 3 + frames[dst_idx][o:o+4] = frames[src_idx][o:o+4] + + def interpolate_around_frame(self, fk_nodes, frames, keyframe, window): + print("interpolate around frame", keyframe) + for node in fk_nodes: + o = self.skeleton.nodes[node].quaternion_frame_index * 4 + 3 + indices = list(range(o,o+4)) + smooth_joints_around_transition_using_slerp(frames, indices, keyframe, window) + #window = 1000 + #h_window = int(window / 2) + #start_idx = max(keyframe - h_window, 0) + #end_idx = min(keyframe + h_window, len(frames)) + #self.apply_joint_constraints(frames, start_idx, end_idx) + + def apply_joint_constraints(self, frames, start_idx, end_idx): + return frames + #print("apply joint constraints in range", start_idx, end_idx) + for frame_idx in range(start_idx, end_idx): + frames[frame_idx] = self.skeleton.apply_joint_constraints(frames[frame_idx]) + return frames + + def resample_motion(self, frames, resample_factor): + n_frames = len(frames) + times = list(range(0, n_frames)) + spline = CubicMotionSpline.fit_frames(self.skeleton, times, frames) + n_dest_frames = n_frames*resample_factor + step_size = (n_frames-1)/n_dest_frames + streched_times = np.arange(0,n_frames-1,step_size) + #print(streched_times) + new_frames = [] + for t in streched_times: + f = spline.evaluate(t) + new_frames.append(f) + return np.array(new_frames) + + def copy_joint_values_from_src(self, left_frames, right_frames, joint_list, joint_index_list, src_start, src_end, dest_start, dest_end): + n_copied_frames = src_end - src_start + n_dest_frames = dest_end - dest_start + modified_frames = np.array(right_frames) + if n_copied_frames > 1: + src_frames = self.stretch_motion(left_frames[src_start:src_end], n_dest_frames) + else: + src_frames = [] + for i in range(n_dest_frames): + src_frames.append(left_frames[src_start]) + src_frames = np.array(src_frames) + #print("copy ", n_copied_frames, n_dest_frames) + for frame_idx in range(n_dest_frames): + modified_frames[dest_start+frame_idx][joint_index_list] = src_frames[frame_idx][joint_index_list] + return modified_frames + + def apply_blending(self, frames, joint_list, joint_index_list, dest_start, dest_end, n_blend_range): + n_frames = len(frames) + blend_start = max(dest_start- n_blend_range, 0) + start_window = dest_start -blend_start + blend_end = min(dest_end +n_blend_range, n_frames-1) + end_window = blend_end- dest_end + #remove root indices + print("blend ", dest_start, dest_end, n_blend_range, start_window, end_window) + quat_joint_index_list = list(joint_index_list) + if self.skeleton.root in joint_list: + # apply root smnoothing and remove from index list + if start_window > 0: + frames = smooth_translation_in_quat_frames(frames, dest_start, start_window) + if end_window > 0: + frames = smooth_translation_in_quat_frames(frames, dest_end, end_window) + for i in range(3): + quat_joint_index_list.remove(i) + + if len(quat_joint_index_list) > 0: + o = 0 + for j in joint_list: + q_indices = quat_joint_index_list[o:o+4] + if start_window > 0: + frames = create_transition_for_joints_using_slerp(frames, q_indices, blend_start, dest_start, start_window, BLEND_DIRECTION_FORWARD) + if end_window > 0: + print(j, q_indices) + frames = create_transition_for_joints_using_slerp(frames, q_indices, dest_end, blend_end, end_window, BLEND_DIRECTION_BACKWARD) + o += 4 + return frames + + def stretch_motion(self, frames, n_dest_frames): + n_frames = len(frames) + times = list(range(0, n_frames)) + spline = CubicMotionSpline.fit_frames(self.skeleton, times, frames) + step_size = (n_frames-1)/n_dest_frames + streched_times = np.arange(0,n_frames-1,step_size) + new_frames = [] + for t in streched_times: + f = spline.evaluate(t) + new_frames.append(f) + print("new frames", len(new_frames)) + return new_frames + \ No newline at end of file diff --git a/build/lib/anim_utils/motion_editing/motion_filtering.py b/build/lib/anim_utils/motion_editing/motion_filtering.py new file mode 100644 index 0000000..4bd61b0 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/motion_filtering.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from ..motion_vector import MotionVector + + +def smooth(x, window_len=11,window='hanning'): + """smooth the data using a window with requested size. + http://scipy-cookbook.readthedocs.io/items/SignalSmooth.html + + This method is based on the convolution of a scaled window with the signal. + The signal is prepared by introducing reflected copies of the signal + (with the window size) in both ends so that transient parts are minimized + in the begining and end part of the output signal. + + input: + x: the input signal + window_len: the dimension of the smoothing window; should be an odd integer + window: the type of window from 'flat', 'hanning', 'hamming', 'bartlett', 'blackman' + flat window will produce a moving average smoothing. + + output: + the smoothed signal + + example: + + t=linspace(-2,2,0.1) + x=sin(t)+randn(len(t))*0.1 + y=smooth(x) + + see also: + + numpy.hanning, numpy.hamming, numpy.bartlett, numpy.blackman, numpy.convolve + scipy.signal.lfilter + + TODO: the window parameter could be the window itself if an array instead of a string + NOTE: length(output) != length(input), to correct this: return y[(window_len/2-1):-(window_len/2)] instead of just y. + """ + + if x.ndim != 1: + raise (ValueError, "smooth only accepts 1 dimension arrays.") + + if x.size < window_len: + raise (ValueError, "Input vector needs to be bigger than window size.") + + if window_len<3: + return x + + if not window in ['flat', 'hanning', 'hamming', 'bartlett', 'blackman']: + raise(ValueError, "Window is on of 'flat', 'hanning', 'hamming', 'bartlett', 'blackman'") + + + s=np.r_[x[window_len-1:0:-1],x,x[-2:-window_len-1:-1]] + #print(len(s)) + if window == 'flat': #moving average + w=np.ones(window_len,'d') + else: + w=eval('np.'+window+'(window_len)') + + y=np.convolve(w/w.sum(),s,mode='valid') + return y + + +def low_pass(src, alpha = 0.5): + """ http://blog.thomnichols.org/2011/08/smoothing-sensor-data-with-a-low-pass-filter + https://en.wikipedia.org/wiki/Low-pass_filter + """ + + n_frames = len(src.frames) + n_dims = len(src.frames[0]) + new_frames = [src.frames[0]] + for i in range(1, n_frames): + new_f = [] + for j in range(n_dims): + v = new_frames[i-1][j] + alpha * (src.frames[i][j] - new_frames[i-1][j]) + print("filter", v, src.frames[i][j]) + new_f.append(v) + new_frames.append(new_f) + + target_motion = MotionVector() + target_motion.skeleton = src.skeleton + target_motion.frames = np.array(new_frames) + target_motion.n_frames = len(new_frames) + return target_motion + + +def moving_average(src, window=4): + """ https://www.wavemetrics.com/products/igorpro/dataanalysis/signalprocessing/smoothing.htm#MovingAverage + """ + n_frames = len(src.frames) + n_dims = len(src.frames[0]) + new_frames = np.zeros(src.frames.shape) + new_frames[0,:] = src.frames[0,:] + hw = int(window/2) + for i in range(1, n_frames): + for j in range(n_dims): + start = max(0, i-hw) + end = min(n_frames-1, i+hw) + w = end-start + #print(i, j, start, end, w, n_frames, n_dims) + #print("filter", v, src.frames[i, j]) + new_frames[i, j] = np.sum(src.frames[start:end, j])/w + target_motion = MotionVector() + target_motion.skeleton = src.skeleton + target_motion.frames = np.array(new_frames) + print("new shape", target_motion.frames.shape) + target_motion.n_frames = len(new_frames) + return target_motion \ No newline at end of file diff --git a/build/lib/anim_utils/motion_editing/motion_grounding.py b/build/lib/anim_utils/motion_editing/motion_grounding.py new file mode 100644 index 0000000..3f27488 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/motion_grounding.py @@ -0,0 +1,483 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import collections +import numpy as np +from copy import copy +from transformations import quaternion_from_matrix, quaternion_matrix, quaternion_multiply, quaternion_slerp +from .analytical_inverse_kinematics import AnalyticalLimbIK +from .numerical_ik_exp import NumericalInverseKinematicsExp +from .utils import normalize, project_on_intersection_circle, smooth_root_positions +from ..animation_data.skeleton_node import SkeletonEndSiteNode +from ..animation_data.utils import quaternion_from_vector_to_vector +from ..animation_data.motion_blending import create_transition_for_joints_using_slerp, BLEND_DIRECTION_FORWARD, BLEND_DIRECTION_BACKWARD, smooth_translation_in_quat_frames +from ..animation_data.skeleton_models import IK_CHAINS_DEFAULT_SKELETON + +FOOT_STATE_GROUNDED = 0 +FOOT_STATE_SWINGING = 1 + +def add_heels_to_skeleton(skeleton, left_foot, right_foot, left_heel, right_heel, offset=[0, -5, 0]): + + left_heel_node = SkeletonEndSiteNode(left_heel, [], skeleton.nodes[left_foot]) + left_heel_node.offset = np.array(offset) + skeleton.nodes[left_heel] = left_heel_node + skeleton.nodes[left_foot].children.append(left_heel_node) + + right_heel_node = SkeletonEndSiteNode(right_heel, [], skeleton.nodes[right_foot]) + right_heel_node.offset = np.array(offset) + skeleton.nodes[right_heel] = right_heel_node + skeleton.nodes[right_foot].children.append(right_heel_node) + return skeleton + + + +def get_heel_offset(skeleton, foot_name, toe_name, frame): + """ calculate heel offset from foot assuming the "y" axis of the foot coordinate system is aligned to the ground + """ + m = skeleton.nodes[foot_name].get_global_matrix(frame) + foot_position = m[:3,3] + print("foot_position", foot_position) + toe_offset = skeleton.nodes[toe_name].offset + #if len(skeleton.nodes[toe_name].children) > 0: + # toe_offset += skeleton.nodes[toe_name].children[0].offset + up_vector = np.array(skeleton.skeleton_model["cos_map"][foot_name]["y"]) + up_vector /= np.linalg.norm(up_vector) + #project toe offset on up vector that should be aligned with the ground + scale = np.dot(up_vector, toe_offset) + + # get global position of toe aligned to ground + local_offset = scale*up_vector + local_offset = [local_offset[0],local_offset[1],local_offset[2], 1] + projected_toe_pos = np.dot(m, local_offset)[:3] + print("projected_toe_pos", projected_toe_pos) + # use offset from projected toe position to position to get the global heel position + toe_pos = skeleton.nodes[toe_name].get_global_position(frame) + heel_position = foot_position + (toe_pos - projected_toe_pos) + + # bring into local coordinate system + heel_position = [heel_position[0],heel_position[1],heel_position[2], 1] + heel_offset = np.dot(np.linalg.inv(m), heel_position)[:3] + print("heel_offset", heel_offset) + print("toe_offset", toe_offset) + return heel_offset + + + +def get_heel_offset2(skeleton, foot_name, toe_name, frame): + """ calculate heel offset from foot assuming the "y" axis of the foot coordinate system is aligned to the ground + """ + m = skeleton.nodes[foot_name].get_global_matrix(frame) + foot_position = m[:3,3] + print("foot_position", foot_position) + toe_offset = skeleton.nodes[toe_name].offset + #if len(skeleton.nodes[toe_name].children) > 0: + # toe_offset += skeleton.nodes[toe_name].children[0].offset + foot_cos_map =skeleton.skeleton_model["cos_map"][foot_name] + up_vector = np.array(foot_cos_map["y"], dtype=np.float32) + up_vector /= np.linalg.norm(up_vector) + x_vector = np.array(foot_cos_map["x"], dtype=np.float32) + x_vector /= np.linalg.norm(x_vector) + z_vector = np.cross(up_vector, x_vector) + z_vector /= np.linalg.norm(z_vector) + #project toe offset on up vector that should be aligned with the ground + scale = np.dot(z_vector, toe_offset) + heel_offset = scale*z_vector + # bring into local coordinate system + print("heel_offse2", heel_offset) + print("toe_offset", toe_offset) + return heel_offset + +def add_temporary_heels_to_skeleton(skeleton, left_foot, right_foot, left_toe, right_toe, left_heel, right_heel): + + left_heel_node = SkeletonEndSiteNode(left_heel, [], skeleton.nodes[left_foot]) + left_heel_node.offset = get_heel_offset2(skeleton, left_foot, left_toe, skeleton.reference_frame) + skeleton.nodes[left_heel] = left_heel_node + skeleton.nodes[left_foot].children.append(left_heel_node) + skeleton.skeleton_model["joints"]["left_heel"] = left_heel + + right_heel_node = SkeletonEndSiteNode(right_heel, [], skeleton.nodes[right_foot]) + right_heel_node.offset = get_heel_offset2(skeleton, right_foot, right_toe, skeleton.reference_frame) + skeleton.nodes[right_heel] = right_heel_node + skeleton.nodes[right_foot].children.append(right_heel_node) + skeleton.skeleton_model["joints"]["right_heel"] = right_heel + return skeleton + +def create_grounding_constraint_from_frame(skeleton, frames, frame_idx, joint_name): + position = skeleton.nodes[joint_name].get_global_position(frames[frame_idx]) + m = skeleton.nodes[joint_name].get_global_matrix(frames[frame_idx]) + m[:3, 3] = [0, 0, 0] + orientation = normalize(quaternion_from_matrix(m)) + return MotionGroundingConstraint(frame_idx, joint_name, position, None, orientation) + + +def generate_ankle_constraint_from_toe(skeleton, frames, frame_idx, ankle_joint_name, heel_joint, toe_joint_name, target_ground_height, toe_pos=None): + """ create a constraint on the ankle position based on the toe constraint position""" + #print "add toe constraint" + if toe_pos is None: + ct = skeleton.nodes[toe_joint_name].get_global_position(frames[frame_idx]) + ct[1] = target_ground_height # set toe constraint on the ground + else: + ct = toe_pos + + a = skeleton.nodes[ankle_joint_name].get_global_position(frames[frame_idx]) + t = skeleton.nodes[toe_joint_name].get_global_position(frames[frame_idx]) + + target_toe_offset = a - t # difference between unmodified toe and ankle at the frame + ca = ct + target_toe_offset # move ankle so toe is on the ground + + m = skeleton.nodes[heel_joint].get_global_matrix(frames[frame_idx]) + m[:3, 3] = [0, 0, 0] + oq = quaternion_from_matrix(m) + oq = normalize(oq) + + return MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, None, oq) + + +def create_ankle_constraint_from_toe_and_heel(skeleton, frames, frame_idx, ankle_joint, heel_joint, toe_joint,heel_offset, target_ground_height, heel_pos=None, toe_pos=None, is_swinging=False): + if toe_pos is None: + ct = skeleton.nodes[toe_joint].get_global_position(frames[frame_idx]) + ct[1] = target_ground_height + else: + ct = toe_pos + if heel_pos is None: + ch = skeleton.nodes[heel_joint].get_global_position(frames[frame_idx]) + ch[1] = target_ground_height + else: + ch = heel_pos + target_direction = normalize(ct - ch) + t = skeleton.nodes[toe_joint].get_global_position(frames[frame_idx]) + h = skeleton.nodes[heel_joint].get_global_position(frames[frame_idx]) + original_direction = normalize(t - h) + + global_delta_q = quaternion_from_vector_to_vector(original_direction, target_direction) + global_delta_q = normalize(global_delta_q) + + m = skeleton.nodes[heel_joint].get_global_matrix(frames[frame_idx]) + m[:3, 3] = [0, 0, 0] + oq = quaternion_from_matrix(m) + oq = normalize(oq) + orientation = normalize(quaternion_multiply(global_delta_q, oq)) + + # set target ankle position based on the grounded heel and the global target orientation of the ankle + m = quaternion_matrix(orientation)[:3, :3] + target_heel_offset = np.dot(m, heel_offset) + ca = ch - target_heel_offset + print("set ankle constraint both", ch, ca, target_heel_offset, target_ground_height) + foot_state = FOOT_STATE_GROUNDED + if is_swinging: + foot_state = FOOT_STATE_SWINGING + return MotionGroundingConstraint(frame_idx, ankle_joint, ca, None, orientation, foot_state) + + +def interpolate_constraints(c1, c2): + p = (c1.position + c2.position)/2 + o = quaternion_slerp(c1.orientation, c2.orientation, 0.5) + o = normalize(o) + return MotionGroundingConstraint(c1.frame_idx, c1.joint_name, p, None, o) + + +class MotionGroundingConstraint(object): + def __init__(self, frame_idx, joint_name, position, direction=None, orientation=None, foot_state=FOOT_STATE_GROUNDED): + self.frame_idx = frame_idx + self.joint_name = joint_name + self.position = position + self.direction = direction + self.orientation = orientation + self.toe_position = None + self.heel_position = None + self.global_toe_offset = None + self.foot_state = foot_state + + def evaluate(self, skeleton, q_frame): + d = self.position - skeleton.nodes[self.joint_name].get_global_position(q_frame) + return np.dot(d, d) + + +class IKConstraintSet(object): + def __init__(self, frame_range, joint_names, positions): + self.frame_range = frame_range + self.joint_names = joint_names + self.constraints = [] + for idx in range(frame_range[0], frame_range[1]): + for idx, joint_name in enumerate(joint_names): + c = MotionGroundingConstraint(idx, joint_name, positions[idx], None) + self.constraints.append(c) + + def add_constraint(self, c): + self.constraints.append(c) + + def evaluate(self, skeleton, q_frame): + error = 0 + for c in self.constraints: + d = c.position - skeleton.nodes[c.joint_name].get_global_position(q_frame) + error += np.dot(d, d) + return error + + +def add_fixed_dofs_to_frame(skeleton, frame): + o = 3 + full_frame = frame[:3].tolist() + for key, node in list(skeleton.nodes.items()): + if len(node.children) == 0: + continue + if not node.fixed: + full_frame += frame[o:o+4].tolist() + o += 4 + else: + full_frame += node.rotation.tolist() + return full_frame + +def extract_ik_chains(skeleton, damp_angle, damp_factor): + joints_map = skeleton.skeleton_model["joints"] + cos_map = skeleton.skeleton_model["cos_map"] + new_ik_chains = dict() + for j in IK_CHAINS_DEFAULT_SKELETON: + mapped_j = joints_map[j] + root_joint = IK_CHAINS_DEFAULT_SKELETON[j]["root"] + free_joint = IK_CHAINS_DEFAULT_SKELETON[j]["joint"] + if root_joint in joints_map and free_joint in joints_map: + mapped_free_joint = joints_map[free_joint] + if mapped_free_joint in cos_map: + data = copy(IK_CHAINS_DEFAULT_SKELETON[j]) + data["root"] = joints_map[root_joint] + data["joint"] = mapped_free_joint + data["joint_axis"] = cos_map[mapped_free_joint]["x"] + data["end_effector_dir"] = cos_map[mapped_free_joint]["y"] + new_ik_chains[mapped_j] = AnalyticalLimbIK.init_from_dict(skeleton, mapped_j, data, damp_angle=damp_angle, damp_factor=damp_factor) + return new_ik_chains + +class MotionGrounding(object): + def __init__(self, skeleton, ik_settings, skeleton_model, use_analytical_ik=True, damp_angle=None, damp_factor=None): + self.skeleton = skeleton + self._ik = NumericalInverseKinematicsExp(skeleton, ik_settings) + self._constraints = collections.OrderedDict() + self.transition_window = 10 + self.root_smoothing_window = 20 + self.translation_blend_window = 40 + self._blend_ranges = collections.OrderedDict() + self.use_analytical_ik = use_analytical_ik + self.skeleton_model = skeleton_model + self.damp_angle = damp_angle + self.damp_factor = damp_factor + if "joints" in skeleton_model and "left_toe" in skeleton_model["joints"] and "right_toe" in skeleton_model["joints"]: + joints_map = skeleton_model["joints"] + self.ik_chains = extract_ik_chains(skeleton, self.damp_angle, self.damp_factor) + + add_temporary_heels_to_skeleton(skeleton, joints_map["left_ankle"], joints_map["right_ankle"], joints_map["left_toe"], joints_map["right_toe"], "left_heel", "right_heel") + self.initialized = True + else: + self.ik_chains = dict() + self.initialized = False + + + def set_constraints(self, constraints): + self._constraints = constraints + + def add_constraint(self, joint_name, frame_range, position, direction=None): + for frame_idx in range(*frame_range): + c = MotionGroundingConstraint(frame_idx, joint_name, position, direction) + if frame_idx not in list(self._constraints.keys()): + self._constraints[frame_idx] = [] + self._constraints[frame_idx].append(c) + + def add_blend_range(self, joint_names, frame_range): + if frame_range not in list(self._constraints.keys()): + self._blend_ranges[frame_range] = [] + for j in joint_names: + self._blend_ranges[frame_range].append(j) + + def clear_constraints(self): + self._constraints = collections.OrderedDict() + + def clear_blend_ranges(self): + self._blend_ranges = collections.OrderedDict() + + def clear(self): + self.clear_constraints() + self.clear_blend_ranges() + + def run(self, motion_vector, scene_interface=None): + new_frames = motion_vector.frames[:] + if scene_interface is not None: + self.shift_root_to_ground(new_frames, scene_interface) + self.shift_root_to_reach_constraints(new_frames) + if len(new_frames) > 1: + self.blend_at_transitions(new_frames) + if self.use_analytical_ik: + self.apply_analytical_ik(new_frames) + else: + self.apply_ik_constraints(new_frames) + if len(new_frames) > 1: + self.blend_at_transitions(new_frames) + return new_frames + + def apply_on_frame(self, frame, scene_interface): + x = frame[0] + z = frame[2] + target_ground_height = scene_interface.get_height(x, z) + shift = target_ground_height - frame[1] + frame[1] += shift + #self.apply_analytical_ik_on_frame(frame, constraints) + return frame + + def _blend_around_frame_range(self, frames, start, end, joint_names): + for joint_name in joint_names: + transition_start = max(start - self.transition_window, 0) + transition_end = min(end + self.transition_window, frames.shape[0]-1) - 1 + forward_steps = start - transition_start + backward_steps = transition_end - end + if joint_name == self.skeleton.root: + if start > 0: + frames = smooth_translation_in_quat_frames(frames, start, self.translation_blend_window) + temp_frame = min(end + 1, frames.shape[0]-1) + frames = smooth_translation_in_quat_frames(frames, temp_frame, self.translation_blend_window) + + idx = self._ik.skeleton.animated_joints.index(joint_name)*4+3 + joint_parameter_indices = [idx, idx+1, idx+2, idx+3] + if start > 0: + create_transition_for_joints_using_slerp(frames, joint_parameter_indices, transition_start, start, forward_steps, BLEND_DIRECTION_FORWARD) + create_transition_for_joints_using_slerp(frames, joint_parameter_indices, end, transition_end, backward_steps, BLEND_DIRECTION_BACKWARD) + + def apply_ik_constraints(self, frames): + for frame_idx, constraints in self._constraints.items(): + if 0 <= frame_idx < len(frames): + frames[frame_idx] = self._ik.modify_frame(frames[frame_idx], constraints) + + def shift_root_to_reach_constraints(self, frames): + root_positions = self.generate_root_positions_from_foot_constraints(frames) + root_positions = smooth_root_positions(root_positions, self.root_smoothing_window) + self.apply_root_constraints(frames, root_positions) + + def generate_root_positions_from_foot_constraints(self, frames): + root_constraints = [] + for frame_idx, constraints in self._constraints.items(): + if 0 <= frame_idx < len(frames): + grounding_constraints = [c for c in constraints if c.foot_state==FOOT_STATE_GROUNDED] + n_constraints = len(grounding_constraints) + p = None + if n_constraints == 1: + p = self.generate_root_constraint_for_one_foot(frames[frame_idx], grounding_constraints[0]) + elif n_constraints > 1: + p = self.generate_root_constraint_for_two_feet(frames[frame_idx], grounding_constraints[0], grounding_constraints[1]) + if p is None: + p = frames[frame_idx, :3] + root_constraints.append(p) + return np.array(root_constraints) + + def apply_root_constraints(self, frames, constraints): + for frame_idx, p in enumerate(constraints): + if p is not None: + frames[frame_idx][:3] = p + + def generate_root_constraint_for_one_foot(self, frame, c): + pelvis = self.skeleton.skeleton_model["joints"]["pelvis"] + pelvis_pos = self.skeleton.nodes[pelvis].get_global_position(frame) + target_length = np.linalg.norm(c.position - pelvis_pos) + limb_length = self.get_limb_length(c.joint_name) + if target_length < limb_length: + return frame[:3] # no change + new_root_pos = (c.position + normalize(root_pos - c.position) * limb_length) + if np.linalg.norm(self.skeleton.nodes[pelvis].offset) > 0.0: + root_m = self.skeleton.nodes[self.skeleton.root].get_global_matrix(frame)[:3,:3] + new_root_pos -= np.dot(root_m, self.skeleton.nodes[pelvis].offset) + else: + new_root_pos -= self.skeleton.nodes[pelvis].offset + return new_root_pos + + def generate_root_constraint_for_two_feet(self, frame, constraint1, constraint2, limb_length_offset=0.0): + """ Set the root position to the projection on the intersection of two spheres """ + + pelvis = self.skeleton.skeleton_model["joints"]["pelvis"] + m = self.skeleton.nodes[pelvis].get_global_matrix(frame) + p = m[:3, 3] + #print("root position", root, p) + t1 = np.linalg.norm(constraint1.position - p) + t2 = np.linalg.norm(constraint2.position - p) + + c1 = constraint1.position + r1 = self.get_limb_length(constraint1.joint_name) + limb_length_offset + #p1 = c1 + r1 * normalize(p-c1) + c2 = constraint2.position + r2 = self.get_limb_length(constraint2.joint_name) + limb_length_offset + #(r1, r2, t1,t2) + #p2 = c2 + r2 * normalize(p-c2) + if t1 < r1 and t2 < r2: + return None + #print("adapt root for two constraints", constraint1.position, r1, constraint2.position, r2) + new_root_pos = project_on_intersection_circle(p, c1, r1, c2, r2) + if np.linalg.norm(self.skeleton.nodes[pelvis].offset) > 0.0: + root_m = self.skeleton.nodes[self.skeleton.root].get_global_matrix(frame)[:3,:3] + new_root_pos -= np.dot(root_m, self.skeleton.nodes[pelvis].offset) + else: + new_root_pos -= self.skeleton.nodes[pelvis].offset + return new_root_pos + + def get_limb_length(self, joint_name): + limb_length = np.linalg.norm(self.skeleton.nodes[joint_name].offset) + limb_length += np.linalg.norm(self.skeleton.nodes[joint_name].parent.offset) + return limb_length + + def apply_analytical_ik(self, frames): + n_frames = len(frames) + for frame_idx, constraints in self._constraints.items(): + if 0 <= frame_idx < n_frames and len(constraints)> 0: + self.apply_analytical_ik_on_frame(frames[frame_idx], constraints) + + def apply_analytical_ik_on_frame(self, frame, constraints): + for c in constraints: + if c.joint_name in self.ik_chains: + frame = self.ik_chains[c.joint_name].apply(frame, c.position, c.orientation) + else: + print("could not find ik chain definition for ", c.joint_name) + frame = self._ik.modify_frame(frame, constraints) + return frame + + def apply_orientation_constraints_on_frame(self, frame, constraints): + for c in constraints: + self.ik_chains[c.joint_name].set_end_effector_rotation2(frame, c.orientation) + return frame + + def blend_at_transitions(self, frames): + for frame_range, joint_names in self._blend_ranges.items(): + start = frame_range[0] + end = frame_range[1] + self._blend_around_frame_range(frames, start, end, joint_names) + return frames + + def shift_root_to_ground(self, frames, scene_interface): + for idx, frame in enumerate(frames): + x = frames[idx][0] + z = frames[idx][2] + target_ground_height = scene_interface.get_height(x, z) + root_pos = self.get_projected_root_pos(frames[idx]) + shift = target_ground_height - root_pos[1] + frames[idx][1] += shift + + def get_projected_root_pos(self, frame): + pelvis = self.skeleton.skeleton_model["joints"]["pelvis"] + ground_pos = self.skeleton.nodes[pelvis].get_global_position(frame) + if np.linalg.norm(self.skeleton.nodes[pelvis].offset) > 0.0: + root_m = self.skeleton.nodes[self.skeleton.root].get_global_matrix(frame)[:3,:3] + ground_pos -= np.dot(root_m, self.skeleton.nodes[pelvis].offset) + else: + ground_pos -= self.skeleton.nodes[pelvis].offset + return ground_pos diff --git a/build/lib/anim_utils/motion_editing/numerical_ik_exp.py b/build/lib/anim_utils/motion_editing/numerical_ik_exp.py new file mode 100644 index 0000000..2f9e727 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/numerical_ik_exp.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" Numerical IK using exponential displacement map based on Section 5.1 of [1] + +Use current configuration as q_0 +Find v such that (q_i = q_0_i exp(v_i) for 0 self.success_threshold and iter_counter < self.max_retries: + r = minimize(self._objective, guess, args=(self.skeleton, reference, constraints, self.joint_weights), + method=self.ik_settings["optimization_method"], + tol=self.ik_settings["tolerance"], + options=self._optimization_options) + exp_frame = r["x"] + exp_frame[:9] = 0 + error = self._objective(exp_frame, self.skeleton, reference, constraints, self.joint_weights) + iter_counter += 1 + + return exp_frame + + def modify_frame(self, reference, constraints): + exp_frame = self.run(reference, constraints) + d = convert_exp_frame_to_quat_frame(self.skeleton, exp_frame) + q_frame = add_quat_frames(self.skeleton, reference, d) + return q_frame diff --git a/build/lib/anim_utils/motion_editing/numerical_ik_quat.py b/build/lib/anim_utils/motion_editing/numerical_ik_quat.py new file mode 100644 index 0000000..c5d7786 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/numerical_ik_quat.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. + +import time +import numpy as np +from scipy.optimize import minimize +from ..utilities.log import write_log, write_message_to_log, LOG_MODE_DEBUG + + +def obj_inverse_kinematics(s, data): + pose, free_joints, target_joint, target_position, target_direction = data + pose.set_channel_values(s, free_joints) + if target_direction is not None: + parent_joint = pose.get_parent_joint(target_joint) + pose.point_in_direction(parent_joint, target_direction) + position = pose.evaluate_position(target_joint) + d = position - target_position + return np.dot(d, d) + + +class NumericalInverseKinematicsQuat(object): + def __init__(self, skeleton_pose_model, ik_settings): + self._ik_settings = ik_settings + self.verbose = False + self.pose = skeleton_pose_model + self.success_threshold = self._ik_settings["success_threshold"] + self.max_retries = self._ik_settings["max_retries"] + self.optimize_orientation = self._ik_settings["optimize_orientation"] + + def _run_optimization(self, objective, initial_guess, data, cons=None): + return minimize(objective, initial_guess, args=(data,), + method=self._ik_settings["optimization_method"],#"SLSQP",#best result using L-BFGS-B + constraints=cons, tol=self._ik_settings["tolerance"], + options={'maxiter': self._ik_settings["max_iterations"], 'disp': self.verbose})#,'eps':1.0 + + def modify_pose(self, joint_name, target, direction=None): + error = 0.0 + if joint_name in list(self.pose.free_joints_map.keys()): + free_joints = self.pose.free_joints_map[joint_name] + error = self._modify_using_optimization(joint_name, target, free_joints, direction) + return error + + def modify_pose_general(self, constraint): + free_joints = constraint.free_joints + initial_guess = self._extract_free_parameters(free_joints) + data = constraint.data(self, free_joints) + if self.verbose: + self.pose.set_channel_values(initial_guess, free_joints) + p = self.pose.evaluate_position(constraint.joint_name) + write_message_to_log( + "Start optimization for joint " + constraint.joint_name + " " + str(len(initial_guess)) + + " " + str(len(free_joints)) + " " + str(p), LOG_MODE_DEBUG) + cons = None + #start = time.time() + error = np.inf + iter_counter = 0 + result = None + while error > self.success_threshold and iter_counter < self.max_retries: + result = self._run_optimization(constraint.evaluate, initial_guess, data, cons) + error = constraint.evaluate(result["x"], data) + iter_counter += 1 + # write_log("finished optimization in",time.time()-start,"seconds with error", error)#,result["x"].tolist(), initial_guess.tolist() + if result is not None: + self.pose.set_channel_values(result["x"], free_joints) + return error + + def _extract_free_parameters(self, free_joints): + """get parameters of joints from reference frame + """ + parameters = list() + for joint_name in free_joints: + parameters += self.pose.extract_parameters(joint_name).tolist() + return np.asarray(parameters) + + def _extract_free_parameter_indices(self, free_joints): + """get parameter indices of joints from reference frame + """ + indices = {} + for joint_name in free_joints: + indices[joint_name] = list(range(*self.pose.extract_parameters_indices(joint_name))) + return indices + + def _modify_using_optimization(self, target_joint, target_position, free_joints, target_direction=None): + initial_guess = self._extract_free_parameters(free_joints) + data = self.pose, free_joints, target_joint, target_position, target_direction + if self.verbose: + write_message_to_log("Start optimization for joint " + target_joint + " " + str(len(initial_guess)) + + " " + str(len(free_joints)), LOG_MODE_DEBUG) + start = time.time() + cons = None#self.pose.generate_constraints(free_joints) + result = self._run_optimization(obj_inverse_kinematics, initial_guess, data, cons) + position = self.pose.evaluate_position(target_joint) + error = np.linalg.norm(position-target_position) + if self.verbose: + write_message_to_log("Finished optimization in " + str(time.time()-start) + " seconds with error " + str(error), LOG_MODE_DEBUG) #,result["x"].tolist(), initial_guess.tolist() + self.pose.set_channel_values(result["x"], free_joints) + return error + + def optimize_joint(self, objective, target_joint, target_position, target_orientation, free_joint): + initial_guess = self.pose.extract_parameters(free_joint) # self._extract_free_parameters([free_joint]) + data = self.pose, [free_joint], target_joint, target_position, None + result = self._run_optimization(objective, initial_guess, data) + # apply constraints here + self.pose.apply_bounds(free_joint) + return result["x"] + + def evaluate_delta(self, parameters, target_joint, target_position, free_joints): + self.pose.set_channel_values(parameters, free_joints) + position = self.pose.evaluate_position(target_joint) + d = position - target_position + return np.dot(d, d) diff --git a/build/lib/anim_utils/motion_editing/skeleton_pose_model.py b/build/lib/anim_utils/motion_editing/skeleton_pose_model.py new file mode 100644 index 0000000..777bc5d --- /dev/null +++ b/build/lib/anim_utils/motion_editing/skeleton_pose_model.py @@ -0,0 +1,291 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from transformations import quaternion_matrix, quaternion_from_matrix +from ..animation_data.quaternion_frame import convert_quaternion_frames_to_euler_frames, euler_to_quaternion, quaternion_to_euler +from ..animation_data.utils import quaternion_from_vector_to_vector +from ..animation_data.constants import LEN_QUAT, LEN_ROOT_POS + + +def convert_euler_to_quat(euler_frame, joints): + quat_frame = euler_frame[:3].tolist() + offset = 3 + step = 3 + for joint in joints: + e = euler_frame[offset:offset+step] + q = euler_to_quaternion(e) + quat_frame += list(q) + offset += step + return np.array(quat_frame) + + +class SkeletonPoseModel(object): + def __init__(self, skeleton, use_euler=False): + self.channels = skeleton.get_channels() + pose_parameters = skeleton.reference_frame + self.skeleton = skeleton + self.use_euler = use_euler + if self.use_euler: + self.pose_parameters = convert_quaternion_frames_to_euler_frames([pose_parameters])[0]#change to euler + else: + self.pose_parameters = pose_parameters + self.n_channels = {} + self.channels_start = {} + self.types = {} + self.modelled_joints = [] + channel_idx = 0 + for joint, ch in list(self.channels.items()): + self.channels_start[joint] = channel_idx + self.n_channels[joint] = len(ch) + if len(ch) == LEN_QUAT: + self.types[joint] = "rot" + elif len(ch) == LEN_ROOT_POS + LEN_QUAT: + self.types[joint] = "trans" + else: + self.types[joint] = "rot" + channel_idx += self.n_channels[joint] + if self.n_channels[joint] > 0: + self.modelled_joints.append(joint) + self.free_joints_map = skeleton.free_joints_map + if "head" in skeleton.skeleton_model: + self.head_joint = skeleton.skeleton_model["joints"]["head"] + if "neck" in skeleton.skeleton_model: + self.neck_joint = skeleton.skeleton_model["joints"]["neck"] + if "cos_map" in skeleton.skeleton_model: + hand_joint = skeleton.skeleton_model["joints"]["right_wrist"] + if hand_joint in skeleton.skeleton_model["cos_map"]: + cos = skeleton.skeleton_model["cos_map"][hand_joint] + self.relative_hand_dir = np.array(list(cos["y"]) + [0]) + self.relative_hand_cross = np.array(list(cos["x"]) + [0]) + up_vec = list(np.cross(cos["y"], cos["x"])) + self.relative_hand_up = np.array(up_vec + [0]) + else: + self.relative_hand_dir = np.array([1.0, 0.0, 0.0, 0.0]) + self.relative_hand_cross = np.array([0.0,1.0,0.0, 0.0]) + self.relative_hand_up = np.array([0.0, 0.0, 1.0, 0.0]) + if "relative_head_dir" in skeleton.skeleton_model.keys(): + vec = list(skeleton.skeleton_model["relative_head_dir"]) + self.relative_head_dir = np.array(vec+[0]) + else: + self.relative_head_dir = np.array([0.0, 0.0, 1.0, 0.0]) + + self.bounds = dict()#skeleton.bounds + + def set_pose_parameters(self, pose_parameters): + self.pose_parameters = pose_parameters + + def set_channel_values(self, parameters, free_joints): + p_idx = 0 + for joint_name in free_joints: + n_channels = self.n_channels[joint_name] + p_end = p_idx + n_channels + f_start = self.channels_start[joint_name] + f_end = f_start + n_channels + #print f_start, f_end + self.pose_parameters[f_start:f_end] = parameters[p_idx:p_end] + p_idx += n_channels + return self.pose_parameters + + def extract_parameters_indices(self, joint_name): + f_start = self.channels_start[joint_name] + f_end = f_start + self.n_channels[joint_name] + return f_start, f_end + + def extract_parameters(self, joint_name): + f_start, f_end = self.extract_parameters_indices(joint_name) + return self.pose_parameters[f_start: f_end] + + def get_vector(self): + if self.use_euler: + return convert_euler_to_quat(self.pose_parameters, self.modelled_joints)#convert to quat + else: + return self.pose_parameters + + def evaluate_position_with_cache(self, target_joint, free_joints): + for joint in free_joints: + self.skeleton.nodes[joint].get_global_position(self.pose_parameters, use_cache=True) + return self.skeleton.nodes[target_joint].get_global_position(self.pose_parameters, use_cache=True)#get_vector() + + def evaluate_position(self, target_joint): + return self.skeleton.nodes[target_joint].get_global_position(self.pose_parameters) + + def evaluate_orientation(self, target_joint): + return self.skeleton.nodes[target_joint].get_global_orientation_quaternion(self.pose_parameters) + + def evaluate_matrix(self, target_joint): + return self.skeleton.nodes[target_joint].get_global_matrix(self.pose_parameters) + + def apply_bounds(self, free_joint): + if free_joint in list(self.bounds.keys()): + euler_angles = self.get_euler_angles(free_joint) + for bound in self.bounds[free_joint]: + self.apply_bound_on_joint(euler_angles, bound) + + if self.use_euler: + self.set_channel_values(euler_angles, [free_joint]) + else: + q = euler_to_quaternion(euler_angles) + self.set_channel_values(q, [free_joint]) + return + + def apply_bound_on_joint(self, euler_angles, bound): + if "min" in list(bound.keys()): + euler_angles[bound["dim"]] = max(euler_angles[bound["dim"]],bound["min"]) + if "max" in list(bound.keys()): + euler_angles[bound["dim"]] = min(euler_angles[bound["dim"]],bound["max"]) + + def get_euler_angles(self, joint): + if self.use_euler: + euler_angles = self.extract_parameters(joint) + else: + q = self.extract_parameters(joint) + euler_angles = quaternion_to_euler(q) + return euler_angles + + def generate_constraints(self, free_joints): + """ TODO add bounds on axis components of the quaternion according to + Inverse Kinematics with Dual-Quaternions, Exponential-Maps, and Joint Limits by Ben Kenwright + or try out euler based ik + """ + cons = [] + idx = 0 + for joint_name in free_joints: + if joint_name in list(self.bounds.keys()): + start = idx + for bound in self.bounds[joint_name]: + if "min" in list(bound.keys()): + cons.append(({"type": 'ineq', "fun": lambda x: x[start+bound["dim"]]-bound["min"]})) + if "max" in list(bound.keys()): + cons.append(({"type": 'ineq', "fun": lambda x: bound["max"]-x[start+bound["dim"]]})) + idx += self.n_channels[joint_name] + return tuple(cons) + + def clear_cache(self): + self.skeleton.clear_cached_global_matrices() + + def lookat(self, point): + head_position = self.evaluate_position(self.head_joint) + target_dir = point - head_position + target_dir /= np.linalg.norm(target_dir) + head_direction = self.get_joint_direction(self.head_joint, self.relative_head_dir) + delta_q = quaternion_from_vector_to_vector(head_direction, target_dir) + delta_matrix = quaternion_matrix(delta_q) + + #delta*parent*old_local = parent*new_local + #inv_parent*delta*parent*old_local = new_local + parent_m = self.skeleton.nodes[self.neck_joint].get_global_matrix(self.pose_parameters, use_cache=False) + old_local = np.dot(parent_m, self.skeleton.nodes[self.head_joint].get_local_matrix(self.pose_parameters)) + m = np.dot(delta_matrix, old_local) + new_local = np.dot(np.linalg.inv(parent_m),m) + new_local_q = quaternion_from_matrix(new_local) + self.set_channel_values(new_local_q, [self.head_joint]) + + def set_hand_orientation_using_direction_vectors(self, joint_name, orientation): + m = quaternion_matrix(orientation) + target_dir = np.dot(m, self.relative_hand_dir) + target_dir /= np.linalg.norm(target_dir) + cross_dir = np.dot(m, self.relative_hand_cross) + cross_dir /= np.linalg.norm(cross_dir) + self.point_in_direction(joint_name, target_dir[:3], cross_dir[:3]) + + def set_hand_orientation(self, joint_name, orientation): + #print "set hand orientation" + m = quaternion_matrix(orientation) + parent_joint_name = self.get_parent_joint(joint_name) + parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) + local_m = np.dot(np.linalg.inv(parent_m), m) + q = quaternion_from_matrix(local_m) + self.set_channel_values(q, [joint_name]) + + + def point_in_direction(self, joint_name, target_dir, target_cross=None): + parent_joint_name = self.get_parent_joint(joint_name) + joint_direction = self.get_joint_direction(joint_name, self.relative_hand_dir) + delta_q = quaternion_from_vector_to_vector(joint_direction, target_dir) + delta_matrix = quaternion_matrix(delta_q) + #delta*parent*old_local = parent*new_local + #inv_parent*delta*parent*old_local = new_local + parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) + old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) + m = np.dot(delta_matrix, old_local) + new_local = np.dot(np.linalg.inv(parent_m),m) + new_local_q = quaternion_from_matrix(new_local) + self.set_channel_values(new_local_q, [joint_name]) + + #rotate around orientation vector + if target_cross is not None: + joint_cross = self.get_joint_direction(joint_name, self.relative_hand_cross) + delta_q = quaternion_from_vector_to_vector(joint_cross, target_cross) + delta_matrix = quaternion_matrix(delta_q) + parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) + old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) + m = np.dot(delta_matrix, old_local) + new_local = np.dot(np.linalg.inv(parent_m),m) + new_local_q = quaternion_from_matrix(new_local) + self.point_hand_cross_dir_in_direction(joint_name, target_cross, parent_joint_name) + + def point_hand_cross_dir_in_direction(self,joint_name,target_cross,parent_joint_name): + joint_cross = self.get_joint_direction(joint_name, self.relative_hand_cross) + delta_q = quaternion_from_vector_to_vector(joint_cross, target_cross) + delta_matrix = quaternion_matrix(delta_q) + parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) + old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) + m = np.dot(delta_matrix, old_local) + new_local = np.dot(np.linalg.inv(parent_m), m) + new_local_q = quaternion_from_matrix(new_local) + self.set_channel_values(new_local_q, [joint_name]) + + def point_hand_up_dir_in_direction(self, joint_name, target_up, parent_joint_name): + joint_cross = self.get_joint_direction(joint_name, self.relative_hand_up) + delta_q = quaternion_from_vector_to_vector(joint_cross, target_up) + delta_matrix = quaternion_matrix(delta_q) + parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) + old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) + m = np.dot(delta_matrix, old_local) + new_local = np.dot(np.linalg.inv(parent_m), m) + new_local_q = quaternion_from_matrix(new_local) + self.set_channel_values(new_local_q, [joint_name]) + + def set_joint_orientation(self, joint_name, target_q): + global_q = self.skeleton.nodes[joint_name].get_global_orientation_quaternion(self.pose_parameters, use_cache=False) + global_m = quaternion_matrix(global_q) + target_m = quaternion_matrix(target_q) + delta_m = np.linalg.inv(global_m)*target_m + local_m = self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters) + new_local_m = np.dot(delta_m, local_m) + new_local_q = quaternion_from_matrix(new_local_m) + self.set_channel_values(new_local_q, [joint_name]) + + def get_joint_direction(self, joint_name, ref_vector): + q = self.evaluate_orientation(joint_name) + q /= np.linalg.norm(q) + rotation_matrix = quaternion_matrix(q) + vec = np.dot(rotation_matrix, ref_vector)[:3] + return vec/np.linalg.norm(vec) + + def get_parent_joint(self, joint_name): + if joint_name not in list(self.skeleton.parent_dict.keys()): + return None + return self.skeleton.parent_dict[joint_name] + + diff --git a/build/lib/anim_utils/motion_editing/utils.py b/build/lib/anim_utils/motion_editing/utils.py new file mode 100644 index 0000000..a18f918 --- /dev/null +++ b/build/lib/anim_utils/motion_editing/utils.py @@ -0,0 +1,597 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +import math +from matplotlib import pyplot as plt +import json +from transformations import quaternion_multiply, quaternion_inverse, quaternion_matrix, quaternion_from_matrix, euler_from_quaternion +from scipy.interpolate import UnivariateSpline +#from anim_utils.animation_data.constants import DEFAULT_ROTATION_ORDER + +DEFAULT_ROTATION_ORDER = ['Xrotation','Yrotation','Zrotation'] + +def normalize(v): + return v/np.linalg.norm(v) + + +def quaternion_from_axis_angle(axis, angle): + q = [1,0,0,0] + if np.linalg.norm(axis) > 0: + q[1] = axis[0] * math.sin(angle / 2) + q[2] = axis[1] * math.sin(angle / 2) + q[3] = axis[2] * math.sin(angle / 2) + q[0] = math.cos(angle / 2) + q = normalize(q) + return q + + +def exp_map_to_quaternion(e): + angle = np.linalg.norm(e) + if angle > 0: + axis = e / angle + q = quaternion_from_axis_angle(axis, angle) + else: + q = [1, 0, 0, 0] + return q + + +def convert_exp_frame_to_quat_frame(skeleton, e): + src_offset = 0 + dest_offset = 0 + n_joints = len(skeleton.animated_joints) + q = np.zeros(n_joints*4) + for node in skeleton.animated_joints: + e_i = e[src_offset:src_offset+3] + q[dest_offset:dest_offset+4] = exp_map_to_quaternion(e_i) + src_offset += 3 + dest_offset += 4 + return q + + +def add_quat_frames(skeleton, q_frame1, q_frame2, dest_offset=3): + src_offset = 0 + new_quat_frame = np.zeros(len(q_frame1)) + new_quat_frame[:3] = q_frame1[:3] + for node in skeleton.animated_joints: + new_q = quaternion_multiply(q_frame1[dest_offset:dest_offset + 4], q_frame2[src_offset:src_offset + 4]) + new_quat_frame[dest_offset:dest_offset+4] = new_q + dest_offset += 4 + src_offset += 4 + return new_quat_frame + + +def get_3d_rotation_between_vectors(a, b): + v = np.cross(a, b) + s = np.linalg.norm(v) + if s ==0: + return np.eye(3) + c = np.dot(a,b) + v_x = np.array([[0, -v[2], v[1]], + [v[2], 0, -v[0]], + [-v[1], v[0], 0]]) + v_x_2 = np.dot(v_x,v_x) + r = np.eye(3) + v_x + (v_x_2* (1-c/s**2)) + return r + + +def normalize_quaternion(q): + return quaternion_inverse(q) / np.dot(q, q) + + +def get_average_joint_position(skeleton, frames, joint_name, start_frame, end_frame): + end_frame = min(end_frame, frames.shape[0]) + temp_positions = [] + for idx in range(start_frame, end_frame): + frame = frames[idx] + pos = skeleton.nodes[joint_name].get_global_position(frame) + temp_positions.append(pos) + return np.mean(temp_positions, axis=0) + + +def get_average_joint_direction(skeleton, frames, joint_name, child_joint_name, start_frame, end_frame,ground_height=0): + temp_dirs = [] + for idx in range(start_frame, end_frame): + frame = frames[idx] + pos1 = skeleton.nodes[joint_name].get_global_position(frame) + pos2 = skeleton.nodes[child_joint_name].get_global_position(frame) + #pos2[1] = ground_height + joint_dir = pos2 - pos1 + joint_dir /= np.linalg.norm(joint_dir) + temp_dirs.append(joint_dir) + return np.mean(temp_dirs, axis=0) + +def get_average_direction_from_target(skeleton, frames, target_pos, child_joint_name, start_frame, end_frame,ground_height=0): + temp_dirs = [] + for idx in range(start_frame, end_frame): + frame = frames[idx] + pos2 = skeleton.nodes[child_joint_name].get_global_position(frame) + pos2[1] = ground_height + joint_dir = pos2 - target_pos + joint_dir /= np.linalg.norm(joint_dir) + temp_dirs.append(joint_dir) + return np.mean(temp_dirs, axis=0) + + +def to_local_cos(skeleton, node_name, frame, q): + # bring into parent coordinate system + pm = skeleton.nodes[node_name].get_global_matrix(frame)[:3,:3] + #pm[:3, 3] = [0, 0, 0] + inv_pm = np.linalg.inv(pm) + r = quaternion_matrix(q)[:3,:3] + lr = np.dot(inv_pm, r)[:3,:3] + q = quaternion_from_matrix(lr) + return q + +def get_dir_on_plane(x, n): + axb = np.cross(x,n) + d = np.cross(n, normalize(axb)) + d = normalize(d) + return d + +def project2(x,n): + """ get direction on plane based on cross product and then project onto the direction """ + d = get_dir_on_plane(x, n) + return project_on_line(x, d) + +def project_vec3(x, n): + """" project vector on normal of plane and then substract from vector to get projection on plane """ + w = project_on_line(x, n) + v = x-w + return v + +def project(x, n): + """ http://www.euclideanspace.com/maths/geometry/elements/plane/lineOnPlane/""" + l = np.linalg.norm(x) + a = normalize(x) + b = normalize(n) + axb = np.cross(a,b) + bxaxb = np.cross(b, axb) + return l * bxaxb + +def project_on_line(x, v): + """https://en.wikipedia.org/wiki/Scalar_projection""" + s = np.dot(x, v) / np.dot(v, v) + return s * v + +def project_onto_plane(x, n): + """https://stackoverflow.com/questions/17915475/how-may-i-project-vectors-onto-a-plane-defined-by-its-orthogonal-vector-in-pytho""" + nl = np.linalg.norm(n) + d = np.dot(x, n) / nl + p = [d * normalize(n)[i] for i in range(len(n))] + return [x[i] - p[i] for i in range(len(x))] + + +def project_vec_on_plane(vec, n): + """https://math.stackexchange.com/questions/633181/formula-to-project-a-vector-onto-a-plane""" + n = normalize(n) + d = np.dot(vec, n) + return vec - np.dot(d, n) + + +def distance_from_point_to_line(p1, p2, vec): + proj = p2+project_on_line(p1, vec) + return np.linalg.norm(proj - p1) + + +def limb_projection(p1, center, n): + #s1 = np.dot(p1, n) / np.dot(p1, p1) + #proj_p1 = p1 - s1*n + + #s2 = np.dot(p2, n) / np.dot(p2, p2) + #proj_p2 = p2 - s2 * n + proj_p1 = project_vec3(p1, n) + proj_center = project_vec3(center, n) + + d = np.linalg.norm(proj_p1-proj_center) + return d + + +def plot_line(ax, start, end,label=None, color=None): + x = start[0], end[0] + y = start[1], end[1] + ax.plot(x, y, label=label, color=color) + +def convert_to_foot_positions(joint_heights): + + n_frames = len(list(joint_heights.items())[0][1][0]) + print(n_frames) + foot_positions = [] + for f in range(n_frames): + foot_positions.append(dict()) + for joint, data in list(joint_heights.items()): + ps, yv, ya = data + for frame_idx, p in enumerate(ps): + foot_positions[frame_idx].update({joint: p}) + return foot_positions + +def plot_foot_positions(ax, foot_positions, bodies,step_size=5): + for f, data in enumerate(foot_positions): + if f%step_size != 0: + continue + for body in [list(bodies.values())[0]]: + start_j = body["start"] + end_j = body["end"] + start = f, data[start_j][1] + end = f+5, data[end_j][1] + plot_line(ax, start, end, color="k") + + +def get_vertical_acceleration(skeleton, frames, joint_name): + """ https://stackoverflow.com/questions/40226357/second-derivative-in-python-scipy-numpy-pandas + """ + ps = [] + for frame in frames: + p = skeleton.nodes[joint_name].get_global_position(frame) + ps.append(p) + ps = np.array(ps) + x = np.linspace(0, len(frames), len(frames)) + ys = np.array(ps[:, 1]) + y_spl = UnivariateSpline(x, ys, s=0, k=4) + velocity = y_spl.derivative(n=1) + acceleration = y_spl.derivative(n=2) + return ps, velocity(x), acceleration(x) + + +def quaternion_to_axis_angle(q): + """http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/ + + """ + a = 2* math.acos(q[0]) + x = q[1] / math.sqrt(1-q[0]*q[0]) + y = q[2] / math.sqrt(1-q[0]*q[0]) + z = q[3] / math.sqrt(1-q[0]*q[0]) + return normalize([x,y,z]),a + +def get_delta_quaternion(q1,q2): + return quaternion_multiply(quaternion_inverse(q1), q2) + + +def get_angular_velocity(skeleton, frames, joint): + """ http://answers.unity3d.com/questions/49082/rotation-quaternion-to-angular-velocity.html + """ + idx = skeleton.animated_joints.index(joint) * 4 + 3 + angular_velocity = [[0,0,0]] + prev_q = frames[0, idx:idx + 4] + for frame_idx, frame in enumerate(frames[1:]): + q = frames[frame_idx, idx:idx+4] + q_delta = get_delta_quaternion(prev_q, q) + axis, angle = quaternion_to_axis_angle(q_delta) + a = axis * angle + angular_velocity.append(a) + prev_q = q + return np.array(angular_velocity) + + +def get_angular_velocities(skeleton, frames, joints): + anglular_velocity = dict() + for joint in joints: + anglular_velocity[joint] = get_angular_velocity(skeleton, frames, joint) + return anglular_velocity + + +def plot_joint_heights(joint_heights, ground_height=0, frame_range=(None,None)): + plt.figure(1) + ax = plt.subplot(111) + n_frames = 0 + for joint, data in list(joint_heights.items()): + ps, yv, ya = data + if frame_range == (None, None): + start, end = 0, len(ps) + else: + start, end = frame_range + n_frames = end- start + x = np.linspace(start,end, n_frames) + plt.plot(x, ps[start:end,1], label=joint) + plot_line(ax, (start, ground_height),(end, ground_height), "ground") + foot_positions = convert_to_foot_positions(joint_heights) + bodies = {"left":{"start":"LeftHeel", "end": "LeftToeBase"}, "right":{"start":"RightHeel", "end": "RightToeBase"}} + #plot_foot_positions(ax, foot_positions, bodies) + plt.legend() + plt.show(True) + + +def plot_angular_velocities(angular_velocities, frame_range=(None,None)): + plt.figure(1) + ax = plt.subplot(111) + n_frames = 0 + for joint, data in list(angular_velocities.items()): + if frame_range == (None, None): + start, end = 0, len(data) + else: + start, end = frame_range + n_frames = end- start + x = np.linspace(start,end, n_frames) + v = list(map(np.linalg.norm, data[start:end])) + plt.plot(x, np.rad2deg(v), label=joint) + plt.legend() + plt.show(True) + + +def export_constraints(constraints, file_path): + unique_dict = dict() + for frame_idx in constraints: + for c in constraints[frame_idx]: + key = tuple(c.position) + unique_dict[key] = None + + points = [] + for p in list(unique_dict.keys()): + points.append(p) + data = dict() + data["points"] = points + with open(file_path, "w") as out: + json.dump(data, out) + +def plot_constraints(constraints, ground_height=0): + colors ={"RightFoot":"r", "LeftFoot":"g"} + plt.figure(1) + joint_constraints = dict() + ax = plt.subplot(111) + + for frame_idx in constraints: + for c in constraints[frame_idx]: + if c.joint_name not in list(joint_constraints.keys()): + joint_constraints[c.joint_name] = [] + joint_constraints[c.joint_name].append(c.position) + for joint_name in list(joint_constraints.keys()): + temp = np.array(joint_constraints[joint_name]) + y = temp[:, 1] + n_frames = len(y) + x = np.linspace(0, n_frames, n_frames) + + ax.scatter(x,y, label=joint_name, c=colors[joint_name]) + + plot_line(ax, (0, ground_height), (n_frames, ground_height), "ground") + plt.legend() + plt.show(True) + + +def get_random_color(): + random_color = np.random.rand(3, ) + if np.sum(random_color) < 0.5: + random_color += np.array([0, 0, 1]) + return random_color.tolist() + + +def convert_ground_contacts_to_annotation(ground_contacts, joints, n_frames): + data = dict() + data["color_map"] = {j : get_random_color() for j in joints} + data["semantic_annotation"] = dict() + for idx in range(n_frames): + if idx in ground_contacts: + for label in ground_contacts[idx]: + if label not in data["semantic_annotation"]: + data["semantic_annotation"][label] = [] + data["semantic_annotation"][label].append(idx) + return data + + +def save_ground_contact_annotation(ground_contacts, joints, n_frames, filename): + data = convert_ground_contacts_to_annotation(ground_contacts, joints, n_frames) + with open(filename, "w") as out: + json.dump(data, out) + + +def load_ground_contact_annotation(filename, n_frames): + ground_contacts = [[] for f in range(n_frames)] + with open(filename, "r") as in_file: + annotation_data = json.load(in_file) + semantic_annotation = annotation_data["semantic_annotation"] + for label in list(semantic_annotation.keys()): + for idx in semantic_annotation[label]: + ground_contacts[idx].append(label) + + return ground_contacts + + +def save_ground_contact_annotation_merge_labels(ground_contacts, n_frames, left_foot, right_foot, filename): + data = dict() + contact_label = "contact" + no_contact_label = "no_contact" + data["color_map"] = {left_foot: [1,0,0], + right_foot: [0,1,0], + contact_label: [0,0,1], + no_contact_label: [1,1,1]} + data["frame_annotation"] = [] + for idx in range(n_frames): + if left_foot in ground_contacts[idx] and right_foot in ground_contacts[idx]: + annotation = contact_label + elif left_foot in ground_contacts[idx]: + annotation = left_foot + elif right_foot in ground_contacts[idx]: + annotation = right_foot + else: + annotation = no_contact_label + + data["frame_annotation"].append(annotation) + with open(filename, "w") as out: + json.dump(data, out) + +def get_intersection_circle(center1, radius1, center2, radius2): + """ Calculate the projection on the intersection of two spheres defined by two constraints on the ankles and the leg lengths + + http://mrl.snu.ac.kr/Papers/tog2001.pdf + """ + delta = center2 - center1 + d = np.linalg.norm(delta) + c_n = delta / d # normal + + # radius (eq 27) + r1_sq = radius1 * radius1 + r2_sq = radius2 * radius2 + d_sq = d * d + nom = r1_sq - r2_sq + d_sq + c_r_sq = r1_sq - ((nom * nom) / (4 * d_sq)) + if c_r_sq < 0: # no intersection + print("no intersection", c_r_sq) + return + c_r = math.sqrt(c_r_sq) + + # center (eq 29) + x = (r1_sq - r2_sq + d_sq) / (2 * d) + c_c = center1 + x * c_n + return c_c, c_r, c_n + + +def get_intersection_circle2(center1, radius1, center2, radius2): + """ src: http://mathworld.wolfram.com/Sphere-SphereIntersection.html + """ + delta = center2 - center1 + d = np.linalg.norm(delta) + c_n = delta / d # normal + + R = radius1 + r = radius2 + + # translation of the circle center along the normal (eq 5) + x = (d*d - r*r + R*R)/(2*d) + c_c = center1 + x * c_n + + # radius of the sphere (eq 9) + sq = (-d+r-R)*(-d-r+R)*(-d+r+R)*(d+r+R) + c_r = (1/(2*d)) * math.sqrt(sq) + return c_c, c_r, c_n + +def project_point_onto_plane(point, point_on_plane, normal): + """ calculate the projection on the intersection of two spheres defined by two constraints on the ankles and the leg lengths + + http://mrl.snu.ac.kr/Papers/tog2001.pdf + """ + h = np.dot(np.dot(normal, point_on_plane - point), normal) + pp = point + h + return pp + +def project_on_intersection_circle(p, center1, radius1, center2, radius2): + """ calculate the projection on the intersection of two spheres defined by two constraints on the ankles and the leg lengths + + http://mrl.snu.ac.kr/Papers/tog2001.pdf + """ + + c_c, c_r, c_n = get_intersection_circle(center1, radius1, center2, radius2) + + # project root position onto plane on which the circle lies + pp = project_point_onto_plane(p, c_c, c_n) + + # project projected point onto circle + delta = normalize(pp - c_c) + p_c = c_c + delta * c_r + + # set the root position to the projection on the intersection + return p_c + +def smooth_root_positions(positions, window): + h_window = int(window/2) + smoothed_positions = [] + n_pos = len(positions) + for idx, p in enumerate(positions): + start = max(idx-h_window, 0) + end = min(idx + h_window, n_pos) + #print start, end, positions[start:end] + avg_p = np.average(positions[start:end], axis=0) + smoothed_positions.append(avg_p) + return smoothed_positions + + + +def move_to_ground(skeleton, frames, ground_height, foot_joints, start_frame=0, n_frames=5): + minimum_height = guess_ground_height(skeleton, frames, start_frame, n_frames, foot_joints) + for f in frames: + f[:3] += [0, ground_height-minimum_height, 0] + return frames + + +def get_limb_length(skeleton, joint_name, offset=1): + limb_length = np.linalg.norm(skeleton.nodes[joint_name].offset) + limb_length += np.linalg.norm(skeleton.nodes[joint_name].parent.offset) + return limb_length + offset + + +def global_position_to_root_translation(skeleton, frame, joint_name, p): + """ determine necessary root translation to achieve a global position""" + + tframe = np.array(frame) + tframe[:3] = [0,0,0] + parent_joint = skeleton.nodes[joint_name].parent.node_name + parent_m = skeleton.nodes[parent_joint].get_global_matrix(tframe, use_cache=False) + old_global = np.dot(parent_m, skeleton.nodes[joint_name].get_local_matrix(tframe)) + return p - old_global[:3,3] + + +def generate_root_constraint_for_one_foot(skeleton, frame, c): + root = skeleton.aligning_root_node + root_pos = skeleton.nodes[root].get_global_position(frame) + target_length = np.linalg.norm(c.position - root_pos) + limb_length = get_limb_length(skeleton, c.joint_name) + if target_length >= limb_length: + new_root_pos = (c.position + normalize(root_pos - c.position) * limb_length) + #print "one constraint on ", c.joint_name, "- before", root_pos, "after", new_root_pos + return global_position_to_root_translation(skeleton, frame, root, new_root_pos) + + else: + print("no change") + + +def generate_root_constraint_for_two_feet(skeleton, frame, constraint1, constraint2, length_offset=1.0): + """ Set the root position to the projection on the intersection of two spheres """ + root = skeleton.aligning_root_node + # root = self.skeleton.root + p = skeleton.nodes[root].get_global_position(frame) + offset = skeleton.nodes[root].get_global_position(skeleton.identity_frame) + t1 = np.linalg.norm(constraint1.position - p) + t2 = np.linalg.norm(constraint2.position - p) + + c1 = constraint1.position + r1 = get_limb_length(skeleton, constraint1.joint_name)- length_offset + # p1 = c1 + r1 * normalize(p-c1) + c2 = constraint2.position + r2 = get_limb_length(skeleton, constraint2.joint_name) - length_offset + # p2 = c2 + r2 * normalize(p-c2) + if r1 > t1 and r2 > t2: + #print ("no root constraint", t1,t2, r1, r2) + return None + #print ("adapt root for two constraints", t1, t2, r1, r2) + p_c = project_on_intersection_circle(p, c1, r1, c2, r2) + p = global_position_to_root_translation(skeleton, frame, root, p_c) + tframe = np.array(frame) + tframe[:3] = p + new_p = skeleton.nodes[root].get_global_position(tframe) + return p#p_c - offset + + +def smooth_root_translation_at_end(frames, d, window): + root_pos = frames[d, :3] + start_idx = d-window + start = frames[start_idx, :3] + end = root_pos + for i in range(window): + t = float(i) / (window) + frames[start_idx + i, :3] = start * (1 - t) + end * t + + +def smooth_root_translation_at_start(frames, d, window): + start = frames[d, :3] + start_idx = d+window + end = frames[start_idx, :3] + for i in range(window): + t = float(i) / (window) + frames[d + i, :3] = start * (1 - t) + end * t diff --git a/build/lib/anim_utils/retargeting/__init__.py b/build/lib/anim_utils/retargeting/__init__.py new file mode 100644 index 0000000..ceecb03 --- /dev/null +++ b/build/lib/anim_utils/retargeting/__init__.py @@ -0,0 +1,5 @@ +from .analytical import retarget_from_src_to_target, Retargeting, generate_joint_map +from .constrained_retargeting import retarget_from_src_to_target as retarget_from_src_to_target_constrained +from .point_cloud_retargeting import retarget_from_point_cloud_to_target +from .constants import ROCKETBOX_TO_GAME_ENGINE_MAP, ADDITIONAL_ROTATION_MAP,GAME_ENGINE_TO_ROCKETBOX_MAP, ROCKETBOX_ROOT_OFFSET +from .point_cloud_retargeting import PointCloudRetargeting diff --git a/build/lib/anim_utils/retargeting/analytical.py b/build/lib/anim_utils/retargeting/analytical.py new file mode 100644 index 0000000..f3dc1fb --- /dev/null +++ b/build/lib/anim_utils/retargeting/analytical.py @@ -0,0 +1,408 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" +Functions for retargeting based on the paper "Using an Intermediate Skeleton and Inverse Kinematics for Motion Retargeting" +by Monzani et al. +See: http://www.vis.uni-stuttgart.de/plain/vdl/vdl_upload/91_35_retargeting%20monzani00using.pdf +""" +import numpy as np +import math +from transformations import quaternion_matrix, quaternion_multiply, quaternion_about_axis, quaternion_from_matrix +from .constants import OPENGL_UP_AXIS +from .utils import normalize, align_axis, find_rotation_between_vectors, align_root_translation, to_local_cos, get_quaternion_rotation_by_name, apply_additional_rotation_on_frames, project_vector_on_axis, quaternion_from_vector_to_vector +from ..animation_data.skeleton_models import JOINT_CHILD_MAP + + +def create_local_cos_map_using_child_map(skeleton, up_vector, x_vector, child_map=None): + joint_cos_map = dict() + for j in list(skeleton.nodes.keys()): + joint_cos_map[j] = dict() + joint_cos_map[j]["y"] = up_vector + joint_cos_map[j]["x"] = x_vector + + if j == skeleton.root: + joint_cos_map[j]["x"] = (-np.array(x_vector)).tolist() + else: + o = np.array([0, 0, 0]) + if child_map is not None and j in child_map: + child_name = child_map[j] + node = skeleton.nodes[child_name] + o = np.array(node.offset) + elif len(skeleton.nodes[j].children) > 0: + node = skeleton.nodes[j].children[0] + o = np.array(node.offset) + o = normalize(o) + if sum(o * o) > 0: + joint_cos_map[j]["y"] = o + return joint_cos_map + + +def create_local_cos_map(skeleton, up_vector, x_vector): + joint_cos_map = dict() + for j in list(skeleton.nodes.keys()): + joint_cos_map[j] = dict() + joint_cos_map[j]["y"] = up_vector + joint_cos_map[j]["x"] = x_vector + if j == skeleton.root: + joint_cos_map[j]["x"] = (-np.array(x_vector)).tolist() + return joint_cos_map + + +def get_body_x_axis(skeleton): + rh = skeleton.skeleton_model["joints"]["right_hip"] + lh = skeleton.skeleton_model["joints"]["left_hip"] + return get_body_axis(skeleton, rh, lh) + +def get_body_y_axis(skeleton): + a = skeleton.skeleton_model["joints"]["pelvis"] + b = skeleton.skeleton_model["joints"]["head"] + return get_body_axis(skeleton, a,b) + +def get_quaternion_to_axis(skeleton, joint_a, joint_b, axis): + ident_f = skeleton.identity_frame + ap = skeleton.nodes[joint_a].get_global_position(ident_f) + bp = skeleton.nodes[joint_b].get_global_position(ident_f) + delta = bp - ap + delta /= np.linalg.norm(delta) + return quaternion_from_vector_to_vector(axis, delta) + + +def get_body_axis(skeleton, joint_a, joint_b, project=True): + ident_f = skeleton.identity_frame + ap = skeleton.nodes[joint_a].get_global_position(ident_f) + bp = skeleton.nodes[joint_b].get_global_position(ident_f) + delta = bp - ap + m = np.linalg.norm(delta) + if m != 0: + delta /= m + if project: + projection = project_vector_on_axis(delta) + return projection / np.linalg.norm(projection) + else: + return delta + else: + return None + +def rotate_axes(cos, q): + m = quaternion_matrix(q)[:3, :3] + for key, a in list(cos.items()): + cos[key] = np.dot(m, a) + cos[key] = normalize(cos[key]) + return cos + +def get_child_joint(skeleton, inv_joint_map, node_name): + """ Warning output is random if there are more than one child joints + and the value is not specified in the JOINT_CHILD_MAP """ + child_node = None + if len(skeleton.nodes[node_name].children) > 0: + child_node = skeleton.nodes[node_name].children[-1] + if node_name in inv_joint_map: + joint_name = inv_joint_map[node_name] + while joint_name in JOINT_CHILD_MAP: + child_joint_name = JOINT_CHILD_MAP[joint_name] + + # check if child joint is mapped + joint_key = None + if child_joint_name in skeleton.skeleton_model["joints"]: + joint_key = skeleton.skeleton_model["joints"][child_joint_name] + + if joint_key is not None and joint_key in skeleton.nodes: # return child joint + child_node = skeleton.nodes[joint_key] + return child_node + else: #keep traversing until end of child map is reached + if child_joint_name in JOINT_CHILD_MAP: + joint_name = JOINT_CHILD_MAP[child_joint_name] + else: + break + return child_node + +def create_local_cos_map_from_skeleton_axes_with_map(skeleton, flip=1.0, project=True): + body_x_axis = get_body_x_axis(skeleton)*flip + body_y_axis = get_body_y_axis(skeleton) + if np.dot(body_y_axis, body_x_axis) != 0: + body_x_axis = [1,0,0] + if np.dot(body_y_axis, body_x_axis) != 0: + body_x_axis = [0,1,0] + if np.dot(body_y_axis, body_x_axis) != 0: + body_x_axis = [0,0,1] + print("body x axis", body_x_axis) + print("body y axis", body_y_axis) + inv_joint_map = dict((v,k) for k, v in skeleton.skeleton_model["joints"].items()) + joint_cos_map = dict() + for j in list(skeleton.nodes.keys()): + joint_cos_map[j] = dict() + joint_cos_map[j]["y"] = body_y_axis + joint_cos_map[j]["x"] = body_x_axis + + node = skeleton.nodes[j] + child_node = get_child_joint(skeleton, inv_joint_map, node.node_name) + if child_node is None: + continue + + y_axis = get_body_axis(skeleton, j, child_node.node_name, project) + if y_axis is not None: + joint_cos_map[j]["y"] = y_axis + #check if the new y axis is similar to the x axis + z_vector = np.cross(y_axis, joint_cos_map[j]["x"]) + if np.linalg.norm(z_vector) == 0.0: + joint_cos_map[j]["x"] = body_y_axis * -np.sum(joint_cos_map[j]["y"]) + #check for angle and rotate + q = get_quaternion_to_axis(skeleton, j, child_node.node_name, y_axis) + rotate_axes(joint_cos_map[j], q) + else: + joint_cos_map[j]["y"] = None + joint_cos_map[j]["x"] = None + return joint_cos_map + + +def align_root_joint(axes, global_src_x_vec, max_iter_count=10): + # handle special case for the root joint + # apply only the y axis rotation of the Hip to the Game_engine node + not_aligned = True + q = [1, 0, 0, 0] + iter_count = 0 + while not_aligned: + qx, axes = align_axis(axes, "x", global_src_x_vec) # first find rotation to align x axis + q = quaternion_multiply(qx, q) + q = normalize(q) + qy, axes = align_axis(axes, "y", OPENGL_UP_AXIS) # then add a rotation to let the y axis point up + q = quaternion_multiply(qy, q) + q = normalize(q) + dot_y = np.dot(axes["y"], OPENGL_UP_AXIS) + dot_y = min(1, max(dot_y, -1)) + a_y = math.acos(dot_y) + dot_x = np.dot(axes["x"], global_src_x_vec) + dot_x = min(1, max(dot_x, -1)) + a_x = math.acos(dot_x) + iter_count += 1 + not_aligned = a_y > 0.1 or a_x > 0.1 and iter_count < max_iter_count + return q + +def align_joint(local_target_axes, global_src_up_vec, global_src_x_vec): + # first align the twist axis + q, axes = align_axis(local_target_axes, "y", global_src_up_vec) + q = normalize(q) + # then align the swing axis + qx, axes = align_axis(axes, "x", global_src_x_vec) + q = quaternion_multiply(qx, q) + q = normalize(q) + + # handle cases when twist axis alignment was lost + dot = np.dot(axes["y"], global_src_up_vec) + if dot <= -1: + q180 = quaternion_about_axis(np.deg2rad(180), global_src_x_vec) + q180 = normalize(q180) + q = quaternion_multiply(q180, q) + q = normalize(q) + elif abs(dot) != 1.0: + qy, axes = align_axis(axes, "y", global_src_up_vec) + q = quaternion_multiply(qy, q) + q = normalize(q) + return q + + +def find_rotation_analytically(new_skeleton, joint_name, global_src_up_vec, global_src_x_vec, frame, joint_cos_map, apply_spine_fix=False, apply_root_fix=False, max_iter_count=10): + local_target_axes = joint_cos_map[joint_name] + if joint_name == new_skeleton.root and apply_root_fix: + q = align_root_joint(local_target_axes, global_src_x_vec, max_iter_count) + else: + q = align_joint(local_target_axes, global_src_up_vec, global_src_x_vec) + return to_local_cos(new_skeleton, joint_name, frame, q) + + +def create_correction_map(target_skeleton,target_to_src_joint_map, src_cos_map, target_cos_map): + correction_map = dict() + for target_name in target_to_src_joint_map: + src_name = target_to_src_joint_map[target_name] + if src_name in src_cos_map and target_name is not None and target_name in target_cos_map: + src_zero_vector_y = src_cos_map[src_name]["y"] + target_zero_vector_y = target_cos_map[target_name]["y"] + src_zero_vector_x = src_cos_map[src_name]["x"] + target_zero_vector_x = target_cos_map[target_name]["x"] + if target_zero_vector_y is not None and src_zero_vector_y is not None: + q = quaternion_from_vector_to_vector(target_zero_vector_y, src_zero_vector_y) + q = normalize(q) + + m = quaternion_matrix(q)[:3, :3] + target_zero_vector_x = normalize(np.dot(m, target_zero_vector_x)) + qx = quaternion_from_vector_to_vector(target_zero_vector_x, src_zero_vector_x) + q = quaternion_multiply(qx, q) + q = normalize(q) + correction_map[target_name] = q + return correction_map + + +class Retargeting(object): + def __init__(self, src_skeleton, target_skeleton, target_to_src_joint_map, scale_factor=1.0, additional_rotation_map=None, constant_offset=None, place_on_ground=False, force_root_translation=False, ground_height=0): + self.src_skeleton = src_skeleton + self.target_skeleton = target_skeleton + self.target_to_src_joint_map = target_to_src_joint_map + self.src_to_target_joint_map = {v: k for k, v in list(self.target_to_src_joint_map.items())} + self.scale_factor = scale_factor + self.n_params = len(self.target_skeleton.animated_joints) * 4 + 3 + self.ground_height = ground_height + self.rotation_offsets = additional_rotation_map + self.src_inv_joint_map = dict((v,k) for k, v in src_skeleton.skeleton_model["joints"].items()) + self.src_child_map = dict() + for src_name in self.src_skeleton.animated_joints: + src_child = get_child_joint(self.src_skeleton, self.src_inv_joint_map, src_name) + if src_child is not None: + self.src_child_map[src_name] = src_child.node_name + else: + self.src_child_map[src_name] = None + self.target_cos_map = create_local_cos_map_from_skeleton_axes_with_map(self.target_skeleton) + self.src_cos_map = create_local_cos_map_from_skeleton_axes_with_map(self.src_skeleton) + + if "cos_map" in target_skeleton.skeleton_model: + self.target_cos_map.update(target_skeleton.skeleton_model["cos_map"]) + if "cos_map" in src_skeleton.skeleton_model: + self.src_cos_map.update(src_skeleton.skeleton_model["cos_map"]) + self.correction_map = dict() + + spine_joints = ["pelvis","spine", "spine_1","spine_2"] + target_joint_map = self.target_skeleton.skeleton_model["joints"] + self.target_spine_joints =[target_joint_map[j] for j in spine_joints if j in target_joint_map] + + self.correction_map = create_correction_map(self.target_skeleton, self.src_to_target_joint_map, self.src_cos_map, self.target_cos_map) + self.constant_offset = constant_offset + self.place_on_ground = place_on_ground + self.force_root_translation = force_root_translation + self.apply_spine_fix = "neck" in target_joint_map and self.src_skeleton.animated_joints != self.target_skeleton.animated_joints + if "root" in target_joint_map: + self.apply_root_fix = self.target_skeleton.skeleton_model["joints"]["root"] is not None # aligns root up axis with src skeleton up axis + # make sure the src root joint in the target is not None + target_root = self.target_skeleton.skeleton_model["joints"]["root"] + if self.apply_root_fix and target_to_src_joint_map[target_root] is None: + target_to_src_joint_map[target_root] = self.src_skeleton.root + else: + self.apply_root_fix = False + if scale_factor <= 0: + self.auto_scale_factor() + + def auto_scale_factor(self): + """ estimate scale from leg length by gemlongman """ + target_hip_h = self.target_skeleton.get_body_hip2foot_height() + src_hip_h = self.src_skeleton.get_body_hip2foot_height() + self.scale_factor = target_hip_h / src_hip_h + print("debug scale_factor :" + str(target_hip_h)+ " / " +str(src_hip_h) + " = " +str(self.scale_factor)) + + def rotate_bone(self, src_name, target_name, src_frame, target_frame, guess): + q = guess + src_x_axis = self.src_cos_map[src_name]["x"] + src_up_axis = self.src_cos_map[src_name]["y"] + if self.src_cos_map[src_name]["y"] is not None and self.target_cos_map[target_name]["y"] is not None: + global_m = self.src_skeleton.nodes[src_name].get_global_matrix(src_frame)[:3, :3] + global_src_up_vec = normalize(np.dot(global_m, src_up_axis)) + global_src_x_vec = normalize(np.dot(global_m, src_x_axis)) + apply_spine_fix = self.apply_spine_fix and target_name in self.target_spine_joints + q = find_rotation_analytically(self.target_skeleton, target_name, global_src_up_vec, global_src_x_vec, target_frame, self.target_cos_map, apply_spine_fix, self.apply_root_fix) + return q + + def rotate_bone_fast(self, src_name, target_name, src_frame, target_frame, quess): + q = quess + src_child_name = self.src_skeleton.nodes[src_name].children[0].node_name + if src_child_name in self.src_to_target_joint_map: + m = self.src_skeleton.nodes[src_name].get_global_matrix(src_frame)[:3, :3] + gq = quaternion_from_matrix(m) + correction_q = self.correction_map[target_name] + q = quaternion_multiply(gq, correction_q) + q = normalize(q) + q = to_local_cos(self.target_skeleton, target_name, target_frame, q) + return q + + def retarget_frame(self, src_frame, ref_frame): + target_frame = np.zeros(self.n_params) + # copy the root translation assuming the rocketbox skeleton with static offset on the hips is used as source + target_frame[0] = src_frame[0] * self.scale_factor + target_frame[1] = src_frame[1] * self.scale_factor + target_frame[2] = src_frame[2] * self.scale_factor + + if self.constant_offset is not None: + target_frame[:3] += self.constant_offset + animated_joints = self.target_skeleton.animated_joints + target_offset = 3 + + for target_name in animated_joints: + q = get_quaternion_rotation_by_name(target_name, self.target_skeleton.reference_frame, self.target_skeleton, root_offset=3) + + if target_name in self.target_to_src_joint_map.keys(): + + src_name = self.target_to_src_joint_map[target_name] + if src_name is not None and len(self.src_skeleton.nodes[src_name].children)>0: + q = self.rotate_bone(src_name, target_name, src_frame, target_frame, q) + + if ref_frame is not None: + # align quaternion to the reference frame to allow interpolation + # http://physicsforgames.blogspot.de/2010/02/quaternions.html + ref_q = ref_frame[target_offset:target_offset + 4] + if np.dot(ref_q, q) < 0: + q = -q + target_frame[target_offset:target_offset + 4] = q + target_offset += 4 + + # apply offset on the root taking the orientation into account + if self.force_root_translation: + aligning_root = self.target_skeleton.skeleton_model["joints"]["pelvis"] + target_frame = align_root_translation(self.target_skeleton, target_frame, src_frame, aligning_root, self.scale_factor) + return target_frame + + def run(self, src_frames, frame_range): + n_frames = len(src_frames) + target_frames = [] + if n_frames > 0: + if frame_range is None: + frame_range = (0, n_frames) + if self.rotation_offsets is not None: + src_frames = apply_additional_rotation_on_frames(self.src_skeleton.animated_joints, src_frames, self.rotation_offsets) + + ref_frame = None + for idx, src_frame in enumerate(src_frames[frame_range[0]:frame_range[1]]): + target_frame = self.retarget_frame(src_frame, ref_frame) + if ref_frame is None: + ref_frame = target_frame + target_frames.append(target_frame) + target_frames = np.array(target_frames) + if self.place_on_ground: + delta = target_frames[0][1] - self.ground_height + target_frames[:,1] -= delta + return target_frames + + +def generate_joint_map(src_model, target_model, joint_filter=None): + joint_map = dict() + for j in src_model["joints"]: + if joint_filter is not None and j not in joint_filter: + continue + if j in target_model["joints"]: + src = src_model["joints"][j] + target = target_model["joints"][j] + joint_map[target] = src + return joint_map + + +def retarget_from_src_to_target(src_skeleton, target_skeleton, src_frames, joint_map=None, additional_rotation_map=None, scale_factor=1.0, frame_range=None, place_on_ground=False, joint_filter=None, force_root_translation=False): + if joint_map is None: + joint_map = generate_joint_map(src_skeleton.skeleton_model, target_skeleton.skeleton_model, joint_filter) + print('joint map', joint_map) + retargeting = Retargeting(src_skeleton, target_skeleton, joint_map, scale_factor, additional_rotation_map=additional_rotation_map, place_on_ground=place_on_ground, force_root_translation=force_root_translation) + return retargeting.run(src_frames, frame_range) diff --git a/build/lib/anim_utils/retargeting/constants.py b/build/lib/anim_utils/retargeting/constants.py new file mode 100644 index 0000000..408a040 --- /dev/null +++ b/build/lib/anim_utils/retargeting/constants.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +GAME_ENGINE_REFERENCE_POSE_EULER = [0.202552772072, -0.3393745422363281, 10.18097736938018, 0.0, 0.0, 88.15288821532792, -3.3291626376861925, 172.40743933061506, 90.48198857145417, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.267224765943357, 5.461144951918523, -108.06852912064531, -15.717336936646204, 0.749500429122681, -31.810810127019366, 5.749795741186075, -0.64655017163842, -43.79621907038145, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 26.628020277759394, 18.180233818114445, 89.72419760530946, 18.24367060730651, 1.5799727651772104, 39.37862756278345, 45.669771502815834, 0.494263941559835, 19.71385918379141, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 31.738570778606658, -0.035796158863762605, -10.010293103153826, 0.0, 0.0, 0.0, 520.8293416407181, -6.305803932868075, -1.875562438841992, 23.726055821805346, 0.0010593260744296063, 3.267962297354599, -60.93853290197474, 0.0020840827755293063, -2.8705207369072694, 0.0, 0.0, 0.0, -158.31965133452601, -12.378967235699056, 6.357392524527775, 19.81125436520809, -0.03971871449276927, -11.895292807406602, -70.75282007667651, -1.2148004469780682, 20.150610072602195, 0.0, 0.0, 0.0] +GAME_ENGINE_T_POSE_QUAT = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.5, 0.5, 0.5, 0.5, -0.4034116551104901, 0.5807400765583645, 0.40341165511049004, 0.5807400765583645, 0.9718569538669156, 0.23557177509311245, -1.3791773619254053e-32, -9.140441879201479e-33, 0.9842093673640157, -0.17700825176506327, -5.454487879647522e-34, 3.0328292674444404e-33, 0.9988215725658287, 0.04853314513943063, -5.451814187658488e-11, -6.046242580699848e-10, 0.7247803814040166, 0.012592769755452584, 0.06644647837285525, -0.6856527447575631, 0.9983567056586047, 0.03270386259217816, -0.025876101002818737, -0.03930360078850377, 0.9965682409037466, 0.05203248433861332, -0.057026576671593956, 0.029871915718325807, 0.9297560338521583, 0.0938946447178257, 0.3530751373573903, -0.045557223234931374, 0.983733142437575, -0.0773187561935722, 0.14664502687446831, -0.06918200997050479, 0.9992691595380858, -0.015669716666339383, -0.028769659982579642, -0.019695518275262152, 0.9975311344163887, 0.04375497165440451, 0.034215414983264525, -0.04297026533543685, 0.9536776598189488, -0.04860775743712403, 0.2797821416180022, -0.09928826874727571, 0.9987647232093224, 0.005804170391239798, -0.03763753948846192, 0.03191794009533443, 0.9966582124862393, 0.051895568999548565, 0.046770662050906374, -0.042329216544453346, 0.9329213679196267, -0.08913572545954243, 0.33917282714358277, -0.08169661592258975, 0.9995615741071782, 0.009141850996134456, -0.017560293698507947, 0.022016407835221498, 0.9998819715691583, -0.0037191151629644803, 0.010434642587798942, -0.010645625742176174, 0.9322424364818923, -0.04990228518632508, 0.342604219994313, -0.1051482286944295, 0.9980915335828159, 0.005996155911584098, -0.04281223678247725, 0.044095907817706795, 0.9963697981737442, 0.045136674083357205, 0.05368016564496531, -0.048252935208484254, 0.2349924837443164, 0.31132145224589375, 0.8367206208319933, 0.38439054180573773, 0.7909589011799936, 0.46413884444997205, 0.3410569180890096, -0.20649292564252283, 0.9802741605131092, 0.09191251147798253, 0.14965372781082692, 0.09065551398812742, 0.7247803805906335, 0.012592769950269537, -0.06644647823624146, 0.6856527456270242, 0.9983567056586047, 0.03270386259217816, 0.02587610100281874, 0.039303600788503784, 0.9965682518976952, 0.05203243726125053, 0.05702650678920791, -0.029871764354427382, 0.9297559857385495, 0.09389467503083726, -0.3530751148595373, 0.045558317035600863, 0.9837333505577588, -0.0773206378561847, -0.14664363717997042, 0.06917989329673528, 0.9992693467177038, -0.015665213954855695, 0.028764041362077865, 0.019697809691502192, 0.9975314823914712, 0.04375194186430184, -0.03421120658988061, 0.042968623024728494, 0.9536775069845468, -0.04860738335400616, -0.2797828498945051, 0.09928792403976064, 0.9987646449635478, 0.005801503439721487, 0.03763947021552947, -0.03191859662597178, 0.9966579624324041, 0.051897847903993356, -0.046772431757685674, 0.04233035471733049, 0.9329213062387378, -0.0891352715094854, -0.3391729808717024, 0.08169717734010994, 0.9995615367687105, 0.009137951896895397, 0.01756132082424807, -0.022018902302604802, 0.9998819504532211, -0.0037172268673331425, -0.010435123687251087, 0.010647796763214817, 0.9322427205484806, -0.049904462569957994, -0.34260361428243297, 0.10514665035361338, 0.9980915983630915, 0.005999462058971182, 0.04281045823362956, -0.04409571858853277, 0.9963701280372368, 0.04513257610822263, -0.0536778650865031, 0.048252516293463533, -0.23499072279120342, -0.3113794298572072, 0.8366855880933993, 0.3844209119450591, 0.7910190999603561, 0.4640967281746974, -0.34091221681567874, 0.206595911918094, 0.9802667852924755, 0.09194228474474263, -0.14970309938761828, -0.09062355081331422, 0.9778768812145672, 0.20918127350714558, 2.5771780613672396e-10, 5.811305966263625e-10, 0.9909380491927413, -0.1343197031789609, -5.066775135883771e-11, -6.8679137736776675e-12, -0.1336980695398881, 0.966385580820868, 0.04173833379190176, -0.2155960270392212, 0.9725347805290746, 0.061775087466743615, 0.22395896183352518, -0.014224016457752058, 0.8338287133786118, -0.5510358060089089, 0.022043010546568757, -0.02456263274824042, 0.9633980641899633, -0.2673236757692123, 0.018056317099312928, 0.00872878577337811, -0.1336980711986006, 0.9663855803373587, -0.04173831577523998, 0.21559603166581406, 0.9725347764612302, 0.06177510013852306, -0.2239589750418668, 0.014224031586612895, 0.8338286703052938, -0.551035871807389, -0.022043023480577864, 0.0245626072356164, 0.963398245692903, -0.26732302190877494, -0.018056289743723242, -0.008728834635171634] +ROCKETBOX_TO_GAME_ENGINE_MAP = dict() +ROCKETBOX_TO_GAME_ENGINE_MAP["Hips"] = "Game_engine" +ROCKETBOX_TO_GAME_ENGINE_MAP["Hips"] = "pelvis" +ROCKETBOX_TO_GAME_ENGINE_MAP["Spine"] = "spine_01" +ROCKETBOX_TO_GAME_ENGINE_MAP["Spine_1"] = "spine_02" +ROCKETBOX_TO_GAME_ENGINE_MAP["LeftShoulder"] = "clavicle_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["RightShoulder"] = "clavicle_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["LeftArm"] = "upperarm_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["RightArm"] = "upperarm_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["LeftForeArm"] = "lowerarm_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["RightForeArm"] = "lowerarm_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["LeftHand"] = "hand_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["RightHand"] = "hand_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["LeftUpLeg"] = "thigh_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["RightUpLeg"] = "thigh_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["LeftLeg"] = "calf_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["RightLeg"] = "calf_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["LeftFoot"] = "foot_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["RightFoot"] = "foot_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_L_Toe0"] = "ball_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_R_Toe0"] = "ball_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_L_Finger3"] = "middle_01_l" +ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_R_Finger3"] = "middle_01_r" +ROCKETBOX_TO_GAME_ENGINE_MAP["Head"] = "neck_01" +GAME_ENGINE_TO_ROCKETBOX_MAP = {v:k for k,v in list(ROCKETBOX_TO_GAME_ENGINE_MAP.items())} +ADDITIONAL_ROTATION_MAP = dict() +ADDITIONAL_ROTATION_MAP["LeftShoulder"] = [0, 0, -20] +ADDITIONAL_ROTATION_MAP["LeftArm"] = [0, 0, 20] +ADDITIONAL_ROTATION_MAP["RightShoulder"] = [0, 0, 20] +ADDITIONAL_ROTATION_MAP["RightArm"] = [0, 0, -20] +OPENGL_UP_AXIS = np.array([0, 1, 0]) +ROCKETBOX_ROOT_OFFSET = np.array([0, 100.949997, 0]) +EXTRA_ROOT_NAME = "Root" +ROOT_JOINT = "Hips" +ROOT_CHILDREN = ["Spine", "LeftUpLeg","RightUpLeg"] +EXTREMITIES = ["RightUpLeg", "LeftUpLeg", "RightLeg", "LeftLeg", "RightArm", "LeftArm", "RightForeArm", "LeftForeArm"] +GAME_ENGINE_ROOT_JOINT = ROCKETBOX_TO_GAME_ENGINE_MAP[ROOT_JOINT] +GAME_ENGINE_ROOT_CHILDREN = ["spine_01", "clavicle_l", "clavicle_r"]#[ROCKETBOX_TO_GAME_ENGINE_MAP[k] for k in ROOT_CHILDREN] +GAME_ENGINE_EXTREMITIES = [ROCKETBOX_TO_GAME_ENGINE_MAP[k] for k in EXTREMITIES] +GAME_ENGINE_SPINE_OFFSET_LIST = ["pelvis", "spine_01"] # list of bones that have an additional rotation offset +AXES = [[1,0,0], [0,1,0], [0,0,1]] + diff --git a/build/lib/anim_utils/retargeting/constrained_retargeting.py b/build/lib/anim_utils/retargeting/constrained_retargeting.py new file mode 100644 index 0000000..b52796e --- /dev/null +++ b/build/lib/anim_utils/retargeting/constrained_retargeting.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +from transformations import quaternion_from_matrix, quaternion_matrix +from .analytical import Retargeting, generate_joint_map, apply_additional_rotation_on_frames +from ..motion_editing.hybrit_ik import HybritIK + +IK_SETTINGS = { + "tolerance": 0.05, + "optimization_method": "L-BFGS-B", + "max_iterations": 1000, + "interpolation_window": 120, + "transition_window": 60, + "use_euler_representation": False, + "solving_method": "unconstrained", + "activate_look_at": True, + "max_retries": 5, + "success_threshold": 5.0, + "optimize_orientation": True, + "elementary_action_max_iterations": 5, + "elementary_action_optimization_eps": 1.0, + "adapt_hands_during_carry_both": True, + "constrain_place_orientation": False, +} +CONSTRAINED_JOINTS = ["left_wrist","right_wrist", "left_ankle", "right_ankle", "neck"] + + +class KeyframeConstraint(object): + def __init__(self, frame_idx, joint_name, position, orientation=None, look_at=False): + self.frame_idx = frame_idx + self.joint_name = joint_name + self.position = position + self.orientation = orientation + self.look_at = look_at + + def evaluate(self, skeleton, frame): + if self.orientation is not None: + parent_joint = skeleton.nodes[self.joint_name].parent + if parent_joint is not None: + m = quaternion_matrix(self.orientation) + parent_m = parent_joint.get_global_matrix(frame, use_cache=False) + local_m = np.dot(np.linalg.inv(parent_m), m) + q = quaternion_from_matrix(local_m) + idx = skeleton.animated_joints.index(parent_joint.node_name) + # idx = skeleton.nodes[c.joint_name].quaternion_frame_index * 4 + frame[idx:idx + 4] = q + d = self.position - skeleton.nodes[self.joint_name].get_global_position(frame) + return np.dot(d, d) + + +class ConstrainedRetargeting(Retargeting): + def __init__(self, src_skeleton, target_skeleton, target_to_src_joint_map, scale_factor=1.0, additional_rotation_map=None, constant_offset=None, place_on_ground=False, ground_height=0): + Retargeting.__init__(self, src_skeleton, target_skeleton, target_to_src_joint_map, scale_factor, additional_rotation_map, constant_offset, place_on_ground, ground_height) + src_joint_map = src_skeleton.skeleton_model["joints"] + self.constrained_joints = [src_joint_map[j] for j in CONSTRAINED_JOINTS] + self.ik = HybritIK(target_skeleton, IK_SETTINGS) + target_joint_map = target_skeleton.skeleton_model["joints"] + self.ik.add_analytical_ik(target_joint_map["left_wrist"], target_joint_map["left_elbow"], target_joint_map["left_shoulder"], [1,0,0],[0,0,1]) + self.ik.add_analytical_ik(target_joint_map["right_wrist"], target_joint_map["right_elbow"], target_joint_map["right_shoulder"], [1, 0, 0], [0, 0, 1]) + self.ik.add_analytical_ik(target_joint_map["right_ankle"], target_joint_map["right_knee"], target_joint_map["right_hip"], [1, 0, 0], [0, 0, 1]) + self.ik.add_analytical_ik(target_joint_map["left_ankle"], target_joint_map["left_knee"], target_joint_map["left_hip"], [1, 0, 0], [0, 0, 1]) + + def generate_ik_constraints(self, frame): + constraints = [] + for j in self.constrained_joints: + p = self.src_skeleton.nodes[j].get_global_position(frame) + c = KeyframeConstraint(0,j, p) + constraints.append(c) + return constraints + + def run(self, src_frames, frame_range): + n_frames = len(src_frames) + target_frames = [] + if n_frames > 0: + if frame_range is None: + frame_range = (0, n_frames) + + if self.additional_rotation_map is not None: + src_frames = apply_additional_rotation_on_frames(self.src_skeleton.animated_joints, src_frames, self.additional_rotation_map) + + ref_frame = None + for idx, src_frame in enumerate(src_frames[frame_range[0]:frame_range[1]]): + print("retarget frame", idx) + ik_constraints = self.generate_ik_constraints(src_frame) + target_frame = self.retarget_frame(src_frame, ref_frame) + target_frame = self.ik.modify_frame(target_frame, ik_constraints) + if ref_frame is None: + ref_frame = target_frame + target_frames.append(target_frame) + target_frames = np.array(target_frames) + if self.place_on_ground: + delta = target_frames[0][1] - self.ground_height + target_frames[:,1] -= delta + return target_frames + + +def retarget_from_src_to_target(src_skeleton, target_skeleton, src_frames, joint_map=None, + additional_rotation_map=None, scale_factor=1.0, frame_range=None, + place_on_ground=False): + if joint_map is None: + joint_map = generate_joint_map(src_skeleton.skeleton_model, target_skeleton.skeleton_model) + retargeting = ConstrainedRetargeting(src_skeleton, target_skeleton, joint_map, scale_factor, + additional_rotation_map=additional_rotation_map, place_on_ground=place_on_ground) + return retargeting.run(src_frames, frame_range) diff --git a/build/lib/anim_utils/retargeting/fast_point_cloud_retargeting.py b/build/lib/anim_utils/retargeting/fast_point_cloud_retargeting.py new file mode 100644 index 0000000..041c0e7 --- /dev/null +++ b/build/lib/anim_utils/retargeting/fast_point_cloud_retargeting.py @@ -0,0 +1,331 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" +Functions for retargeting based on the paper "Using an Intermediate Skeleton and Inverse Kinematics for Motion Retargeting" +by Monzani et al. +See: http://www.vis.uni-stuttgart.de/plain/vdl/vdl_upload/91_35_retargeting%20monzani00using.pdf +""" +import numpy as np +import math +from .constants import OPENGL_UP_AXIS, GAME_ENGINE_SPINE_OFFSET_LIST +from .utils import normalize, align_axis, find_rotation_between_vectors, align_root_translation, to_local_cos, get_quaternion_rotation_by_name, apply_additional_rotation_on_frames, project_vector_on_axis, quaternion_from_vector_to_vector +from transformations import quaternion_matrix, quaternion_multiply, quaternion_about_axis, quaternion_from_matrix +from .point_cloud_retargeting import create_local_cos_map_from_skeleton_axes_with_map, get_parent_map, get_children_map, JOINT_CHILD_MAP + + + + +def apply_manual_fixes(joint_cos_map, joints): + for j in joints: + if j in joint_cos_map: + joint_cos_map[j]["x"] *= -1 + + +def get_child_joint2(skeleton_model, inv_joint_map, node_name, src_children_map): + """ Warning output is random if there are more than one child joints + and the value is not specified in the JOINT_CHILD_MAP """ + child_name = None + if node_name in src_children_map and len(src_children_map[node_name]) > 0: + child_name = src_children_map[node_name][-1] + if node_name in inv_joint_map: + joint_name = inv_joint_map[node_name] + while joint_name in JOINT_CHILD_MAP: + _child_joint_name = JOINT_CHILD_MAP[joint_name] + + # check if child joint is mapped + joint_key = None + if _child_joint_name in skeleton_model["joints"]: + joint_key = skeleton_model["joints"][_child_joint_name] + + if joint_key is not None: # return child joint + child_name = joint_key + return child_name + else: #keep traversing until end of child map is reached + if _child_joint_name in JOINT_CHILD_MAP: + joint_name = JOINT_CHILD_MAP[_child_joint_name] + #print(joint_name) + else: + break + return child_name + + +class FastPointCloudRetargeting(object): + def __init__(self, src_skeleton, src_joints, target_skeleton, target_to_src_joint_map, scale_factor=1.0, additional_rotation_map=None, constant_offset=None, place_on_ground=False, ground_height=0): + self.src_skeleton = src_skeleton + self.src_joints = src_joints + self.src_model = src_skeleton.skeleton_model + self.target_skeleton = target_skeleton + self.target_to_src_joint_map = target_to_src_joint_map + self.src_to_target_joint_map = {v: k for k, v in list(self.target_to_src_joint_map.items())} + print("src to traget map", self.src_to_target_joint_map) + print("target to src map", self.target_to_src_joint_map) + self.scale_factor = scale_factor + self.n_params = len(self.target_skeleton.animated_joints) * 4 + 3 + self.ground_height = ground_height + self.additional_rotation_map = additional_rotation_map + self.src_inv_joint_map = dict((v,k) for k, v in src_skeleton.skeleton_model["joints"].items()) + + self.target_cos_map = create_local_cos_map_from_skeleton_axes_with_map(self.target_skeleton) + self.src_cos_map = create_local_cos_map_from_skeleton_axes_with_map(self.src_skeleton, flip=1.0, project=True) + + if "cos_map" in target_skeleton.skeleton_model: + self.target_cos_map.update(target_skeleton.skeleton_model["cos_map"]) + if "x_cos_fixes" in target_skeleton.skeleton_model: + apply_manual_fixes(self.target_cos_map, target_skeleton.skeleton_model["x_cos_fixes"]) + if "cos_map" in src_skeleton.skeleton_model: + self.src_cos_map.update(src_skeleton.skeleton_model["cos_map"]) + if "x_cos_fixes" in src_skeleton.skeleton_model: + apply_manual_fixes(self.src_cos_map, src_skeleton.skeleton_model["x_cos_fixes"]) + self.correction_map = dict() + self.create_correction_map() + self.constant_offset = constant_offset + self.place_on_ground = place_on_ground + self.apply_spine_fix = self.src_skeleton.animated_joints != self.target_skeleton.animated_joints + + self.src_child_map = dict() + self.src_parent_map = get_parent_map(src_joints) + src_children_map = get_children_map(src_joints) + for src_name in self.src_joints: + src_child = get_child_joint2(self.src_model, self.src_inv_joint_map, src_name, src_children_map) + if src_child is not None: + self.src_parent_map[src_child] = src_name + self.src_child_map[src_name] = src_child + else: + self.src_child_map[src_name] = None + # print("ch",self.src_child_map) + # for j in ["pelvis", "spine", "spine_1", "spine_2"]: + # if j in target_joints: + src_joint_map = self.src_model["joints"] + for j in ["neck", "spine_2", "spine_1", "spine"]: + if j in src_joint_map: + self.src_parent_map["spine_03"] = "pelvis" + self.src_child_map[src_joint_map["pelvis"]] = src_joint_map["neck"] # "pelvis" "neck_01" + + self.constant_offset = constant_offset + self.place_on_ground = place_on_ground + self.temp_frame_data = dict() + + self.target_cos_map = create_local_cos_map_from_skeleton_axes_with_map(self.target_skeleton) + if "cos_map" in target_skeleton.skeleton_model: + self.target_cos_map.update(target_skeleton.skeleton_model["cos_map"]) + if "x_cos_fixes" in target_skeleton.skeleton_model: + apply_manual_fixes(self.target_cos_map, target_skeleton.skeleton_model["x_cos_fixes"]) + + target_joints = self.target_skeleton.skeleton_model["joints"] + self.target_spine_joints = [target_joints[j] for j in ["neck", "spine_2", "spine_1", "spine"] if + j in target_joints] # ["spine_03", "neck_01"] + self.target_ball_joints = [target_joints[j] for j in ["left_shoulder", "right_shoulder", "left_hip", "right_hip"] if + j in target_joints] # ["thigh_r", "thigh_l", "upperarm_r", "upperarm_l"] + self.target_ankle_joints = [target_joints[j] for j in ["left_ankle", "right_ankle"] if j in target_joints] + self.clavicle_joints = [target_joints[j] for j in ["right_clavicle", "left_clavicle"] if j in target_joints] + if "neck" in target_joints: + self.target_neck_joint = target_joints["neck"] + else: + self.target_neck_joint = None + + def create_correction_map(self): + self.correction_map = dict() + joint_map = self.target_skeleton.skeleton_model["joints"] + for target_name in self.target_to_src_joint_map: + src_name = self.target_to_src_joint_map[target_name] + if src_name in self.src_cos_map and target_name is not None: + src_zero_vector_y = self.src_cos_map[src_name]["y"] + target_zero_vector_y = self.target_cos_map[target_name]["y"] + src_zero_vector_x = self.src_cos_map[src_name]["x"] + target_zero_vector_x = self.target_cos_map[target_name]["x"] + if target_zero_vector_y is not None and src_zero_vector_y is not None: + q = quaternion_from_vector_to_vector(target_zero_vector_y, src_zero_vector_y) + q = normalize(q) + + if target_name in [joint_map["pelvis"], joint_map["spine"], joint_map["spine_1"]] and False:#,, joint_map["spine_2"] + # add offset rotation to spine based on an upright reference pose + m = quaternion_matrix(q)[:3, :3] + v = normalize(np.dot(m, target_zero_vector_y)) + node = self.target_skeleton.nodes[target_name] + t_pose_global_m = node.get_global_matrix(self.target_skeleton.reference_frame)[:3, :3] + global_original = np.dot(t_pose_global_m, v) + global_original = normalize(global_original) + qoffset = find_rotation_between_vectors(OPENGL_UP_AXIS, global_original) + q = quaternion_multiply(qoffset, q) + q = normalize(q) + + m = quaternion_matrix(q)[:3, :3] + target_zero_vector_x = normalize(np.dot(m, target_zero_vector_x)) + qx = quaternion_from_vector_to_vector(target_zero_vector_x, src_zero_vector_x) + q = quaternion_multiply(qx, q) + q = normalize(q) + self.correction_map[target_name] = q + + def rotate_bone(self, src_name, target_name, src_frame, target_frame, quess): + q = quess + #src_child_name = self.src_skeleton.nodes[src_name].children[0].node_name + #print("in", src_child_name in self.src_to_target_joint_map) + if target_name in self.correction_map: + m = self.src_skeleton.nodes[src_name].get_global_matrix(src_frame)[:3, :3] + gq = quaternion_from_matrix(m) + correction_q = self.correction_map[target_name] + q = quaternion_multiply(gq, correction_q) + q = normalize(q) + q = to_local_cos(self.target_skeleton, target_name, target_frame, q) + return q + + def get_rotation_from_pc(self, src_name, target_name, src_pc, src_frame): + child_name = self.src_child_map[src_name] + global_src_up_vec, global_src_x_vec = self.estimate_src_joint_cos(src_name, child_name, target_name, src_pc) + if "Spine" in src_name: + print("axis",src_name, global_src_up_vec, global_src_up_vec) + q = [1, 0, 0, 0] + local_target_axes = self.src_cos_map[src_name] + qy, axes = align_axis(local_target_axes, "y", global_src_up_vec) + q = quaternion_multiply(qy, q) + q = normalize(q) + if global_src_x_vec is not None: + qx, axes = align_axis(axes, "x", global_src_x_vec) + q = quaternion_multiply(qx, q) + q = normalize(q) + return to_local_cos(self.src_skeleton, src_name, src_frame, q) + + def convert_pc_to_frame(self, src_pc, ref_frame): + src_frame = np.zeros(ref_frame.shape) + src_frame[:3] = np.array(src_frame[0]) + src_frame_offset = 3 + animated_joints = self.src_skeleton.animated_joints + for src_name in animated_joints: + q = get_quaternion_rotation_by_name(src_name, self.src_skeleton.reference_frame, + self.src_skeleton, + root_offset=3) + if src_name in self.src_joints.keys(): + src_child_name = self.src_child_map[src_name] + if src_child_name in self.src_joints.keys() and src_name in self.src_to_target_joint_map.keys(): + target_name = self.src_to_target_joint_map[src_name] + q = self.get_rotation_from_pc(src_name, target_name, src_pc, src_frame) + else: + print("ignore pc", src_name, src_name in self.src_to_target_joint_map.keys()) + if ref_frame is not None: + q = q if np.dot(ref_frame[src_frame_offset:src_frame_offset + 4], q) >= 0 else -q + src_frame[src_frame_offset:src_frame_offset + 4] = q + src_frame_offset += 4 + #print("src", src_frame[3:7]) + return src_frame + + def retarget_frame(self, src_pc, ref_frame): + print("fast retargeting") + self.temp_frame_data = dict() + src_frame = self.convert_pc_to_frame(src_pc, ref_frame) + target_frame = np.zeros(self.n_params) + # copy the root translation assuming the rocketbox skeleton with static offset on the hips is used as source + target_frame[:3] = np.array(src_frame[:3]) * self.scale_factor + + if self.constant_offset is not None: + target_frame[:3] += self.constant_offset + animated_joints = self.target_skeleton.animated_joints + target_offset = 3 + self.src_spine_joints = ["Spine", "Spine1"] + for target_name in animated_joints: + q = get_quaternion_rotation_by_name(target_name, self.target_skeleton.reference_frame, self.target_skeleton, root_offset=3) + if target_name in self.target_to_src_joint_map.keys(): + src_name = self.target_to_src_joint_map[target_name] + if src_name is not None and len(self.src_skeleton.nodes[src_name].children)>0 and src_name not in self.src_spine_joints: + q = self.rotate_bone(src_name, target_name, src_frame, target_frame, q) + print("rotate", src_name, target_name) + else: + print("ignore", src_name,target_name) + if ref_frame is not None and False: + # align quaternion to the reference frame to allow interpolation + # http://physicsforgames.blogspot.de/2010/02/quaternions.html + ref_q = ref_frame[target_offset:target_offset + 4] + if np.dot(ref_q, q) < 0: + q = -q + target_frame[target_offset:target_offset + 4] = q + target_offset += 4 + + return target_frame + + + def estimate_src_joint_cos(self, src_name, child_name, target_name, src_frame): + joint_idx = self.src_joints[src_name]["index"] + child_idx = self.src_joints[child_name]["index"] + global_src_up_vec = src_frame[child_idx] - src_frame[joint_idx] + global_src_up_vec /= np.linalg.norm(global_src_up_vec) + self.temp_frame_data[src_name] = global_src_up_vec + if target_name == self.target_skeleton.skeleton_model["joints"]["pelvis"]: + left_hip = self.src_model["joints"]["left_hip"] + right_hip = self.src_model["joints"]["right_hip"] + left_hip_idx = self.src_joints[left_hip]["index"] + right_hip_idx = self.src_joints[right_hip]["index"] + global_src_x_vec = src_frame[left_hip_idx] - src_frame[right_hip_idx] + global_src_x_vec /= np.linalg.norm(global_src_x_vec) + elif target_name in self.target_spine_joints or target_name == "CC_Base_Waist": # find x vector from shoulders + left_shoulder = self.src_model["joints"]["left_shoulder"] + right_shoulder = self.src_model["joints"]["right_shoulder"] + left_shoulder_idx = self.src_joints[left_shoulder]["index"] + right_shoulder_idx = self.src_joints[right_shoulder]["index"] + global_src_x_vec = src_frame[left_shoulder_idx] - src_frame[right_shoulder_idx] + global_src_x_vec /= np.linalg.norm(global_src_x_vec) + elif target_name in self.target_ball_joints: # use x vector of child + child_child_name = self.src_child_map[child_name] + child_child_idx = self.src_joints[child_child_name]["index"] + child_global_src_up_vec = src_frame[child_child_idx] - src_frame[child_idx] + child_global_src_up_vec /= np.linalg.norm(child_global_src_up_vec) + + global_src_x_vec = np.cross(global_src_up_vec, child_global_src_up_vec) + + global_src_x_vec /= np.linalg.norm(global_src_x_vec) + + else: # find x vector by cross product with parent + global_src_x_vec = None + if src_name in self.src_parent_map: + parent_joint = self.src_parent_map[src_name] + # print("estimate cos", src_name, target_name) + # if target_name in self.clavicle_joints: + # parent_joint = self.target_neck_joint + # print("set parent for", target_name, "to", self.target_neck_joint) + if parent_joint in self.temp_frame_data: + global_parent_up_vector = self.temp_frame_data[parent_joint] + global_src_x_vec = np.cross(global_src_up_vec, global_parent_up_vector) + global_src_x_vec /= np.linalg.norm(global_src_x_vec) + # print("apply",src_name, parent_joint, global_src_x_vec) + # if target_name in ["calf_l", "calf_r","thigh_r","thigh_l", "spine_03","neck_01","lowerarm_r","lowerarm_l"]: + if target_name not in self.target_ankle_joints: + global_src_x_vec = - global_src_x_vec + # global_src_x_vec = None + # if global_src_x_vec is None: + # print("did not find vector", target_name, parent_joint, self.target_skeleton.root) + # else: + # print("ignore", target_name) + + return global_src_up_vec, global_src_x_vec + +def generate_joint_map(src_model, target_model, joint_filter=None): + joint_map = dict() + for j in src_model["joints"]: + if joint_filter is not None and j not in joint_filter: + continue + if j in target_model["joints"]: + src = src_model["joints"][j] + target = target_model["joints"][j] + joint_map[target] = src + return joint_map + + + diff --git a/build/lib/anim_utils/retargeting/point_cloud_retargeting.py b/build/lib/anim_utils/retargeting/point_cloud_retargeting.py new file mode 100644 index 0000000..67ec882 --- /dev/null +++ b/build/lib/anim_utils/retargeting/point_cloud_retargeting.py @@ -0,0 +1,523 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" +Functions for retargeting based on the paper "Using an Intermediate Skeleton and Inverse Kinematics for Motion Retargeting" +by Monzani et al. +See: http://www.vis.uni-stuttgart.de/plain/vdl/vdl_upload/91_35_retargeting%20monzani00using.pdf +""" +import numpy as np +import math +from .constants import OPENGL_UP_AXIS, GAME_ENGINE_SPINE_OFFSET_LIST +from transformations import quaternion_matrix, quaternion_multiply, quaternion_about_axis, quaternion_inverse, quaternion_from_matrix +from .utils import normalize, align_axis, find_rotation_between_vectors, align_root_translation, to_local_cos, get_quaternion_rotation_by_name, apply_additional_rotation_on_frames, project_vector_on_axis, quaternion_from_vector_to_vector +from ..animation_data.skeleton_models import JOINT_CHILD_MAP +from .analytical import create_local_cos_map_from_skeleton_axes_with_map + +JOINT_CHILD_MAP = dict() +JOINT_CHILD_MAP["root"] = "pelvis" +JOINT_CHILD_MAP["pelvis"] = "spine_2" +JOINT_CHILD_MAP["spine_2"] = "neck" +JOINT_CHILD_MAP["neck"] = "head" +JOINT_CHILD_MAP["left_clavicle"] = "left_shoulder" +JOINT_CHILD_MAP["left_shoulder"] = "left_elbow" +JOINT_CHILD_MAP["left_elbow"] = "left_wrist" +JOINT_CHILD_MAP["left_wrist"] = "left_finger" +JOINT_CHILD_MAP["right_clavicle"] = "right_shoulder" +JOINT_CHILD_MAP["right_shoulder"] = "right_elbow" +JOINT_CHILD_MAP["right_elbow"] = "right_wrist" +JOINT_CHILD_MAP["right_wrist"] = "right_finger" +JOINT_CHILD_MAP["left_hip"] = "left_knee" +JOINT_CHILD_MAP["left_knee"] = "left_ankle" +JOINT_CHILD_MAP["right_elbow"] = "right_wrist" +JOINT_CHILD_MAP["right_hip"] = "right_knee" +JOINT_CHILD_MAP["right_knee"] = "right_ankle" +JOINT_CHILD_MAP["left_ankle"] = "left_toe" +JOINT_CHILD_MAP["right_ankle"] = "right_toe" + + +def estimate_correction(target_zero_vector_y, target_zero_vector_x, src_zero_vector_y, src_zero_vector_x): + q = quaternion_from_vector_to_vector(target_zero_vector_y, src_zero_vector_y) + q = normalize(q) + m = quaternion_matrix(q)[:3, :3] + target_zero_vector_x = normalize(np.dot(m, target_zero_vector_x)) + qx = quaternion_from_vector_to_vector(target_zero_vector_x, src_zero_vector_x) + q = quaternion_multiply(qx, q) + q = normalize(q) + return q + +def create_correction_map(target_skeleton, target_to_src_joint_map, target_cos_map, src_cos_map): + correction_map = dict() + joint_map = target_skeleton.skeleton_model["joints"] + for target_name in target_to_src_joint_map: + src_name = target_to_src_joint_map[target_name] + if src_name in src_cos_map and target_name is not None: + src_zero_vector_y = src_cos_map[src_name]["y"] + target_zero_vector_y = target_cos_map[target_name]["y"] + src_zero_vector_x = src_cos_map[src_name]["x"] + target_zero_vector_x = target_cos_map[target_name]["x"] + if target_zero_vector_y is not None and src_zero_vector_y is not None: + q = estimate_correction(target_zero_vector_y, target_zero_vector_x, src_zero_vector_y, src_zero_vector_x) + correction_map[target_name] = q + return correction_map + + +def get_quaternion_to_axis(skeleton, joint_a, joint_b, axis): + ident_f = skeleton.identity_frame + ap = skeleton.nodes[joint_a].get_global_position(ident_f) + bp = skeleton.nodes[joint_b].get_global_position(ident_f) + delta = bp - ap + delta /= np.linalg.norm(delta) + return quaternion_from_vector_to_vector(axis, delta) + + +def rotate_axes2(cos, q): + m = quaternion_matrix(q)[:3, :3] + aligned_axes = dict() + for key, a in list(cos.items()): + aligned_axes[key] = np.dot(m, a) + aligned_axes[key] = normalize(aligned_axes[key]) + return aligned_axes + + +def get_child_joint(skeleton_model, inv_joint_map, node_name, src_children_map): + """ Warning output is random if there are more than one child joints + and the value is not specified in the JOINT_CHILD_MAP """ + child_name = None + if node_name in src_children_map and len(src_children_map[node_name]) > 0: + child_name = src_children_map[node_name][-1] + if node_name in inv_joint_map: + joint_name = inv_joint_map[node_name] + while joint_name in JOINT_CHILD_MAP: + _child_joint_name = JOINT_CHILD_MAP[joint_name] + + # check if child joint is mapped + joint_key = None + if _child_joint_name in skeleton_model["joints"]: + joint_key = skeleton_model["joints"][_child_joint_name] + + if joint_key is not None: # return child joint + child_name = joint_key + return child_name + else: #keep traversing until end of child map is reached + if _child_joint_name in JOINT_CHILD_MAP: + joint_name = JOINT_CHILD_MAP[_child_joint_name] + #print(joint_name) + else: + break + return child_name + +def rotate_axes_in_place(cos, q): + m = quaternion_matrix(q)[:3, :3] + for key, a in list(cos.items()): + cos[key] = np.dot(m, a) + cos[key] = normalize(cos[key]) + return cos + +def align_axis_in_place(axes, key, new_vec): + q = quaternion_from_vector_to_vector(axes[key], new_vec) + aligned_axes = rotate_axes_in_place(axes, q) + return q, aligned_axes + + +def to_local_cos_fast(skeleton, node_name, frame, q): + # bring into parent coordinate system + pm = np.array(skeleton.nodes[node_name].get_global_matrix(frame, use_cache=True)[:3,:3]) + inv_p = quaternion_inverse(quaternion_from_matrix(pm)) + return quaternion_multiply(inv_p, q) + +def align_root_joint(new_skeleton, free_joint_name, axes, global_src_up_vec, global_src_x_vec,joint_cos_map, max_iter_count=10): + # handle special case for the root joint + # apply only the y axis rotation of the Hip to the Game_engine node + q = [1, 0, 0, 0] + #apply first time + qx, axes = align_axis_in_place(axes, "x", global_src_x_vec) # first find rotation to align x axis + q = quaternion_multiply(qx, q) + q = normalize(q) + + qy, axes = align_axis_in_place(axes, "y", global_src_up_vec) # then add a rotation to let the y axis point up + q = quaternion_multiply(qy, q) + q = normalize(q) + + #apply second time + qx, axes = align_axis_in_place(axes, "x", global_src_x_vec) # first find rotation to align x axis + q = quaternion_multiply(qx, q) + q = normalize(q) + qy, axes = align_axis_in_place(axes, "y", global_src_up_vec) # then add a rotation to let the y axis point up + q = quaternion_multiply(qy, q) + q = normalize(q) + + # print("handle special case for pelvis") + # handle special case of applying the x axis rotation of the Hip to the pelvis + node = new_skeleton.nodes[free_joint_name] + t_pose_global_m = node.get_global_matrix(new_skeleton.reference_frame)[:3, :3] + global_original = np.dot(t_pose_global_m, joint_cos_map[free_joint_name]["y"]) + global_original = normalize(global_original) + qoffset = find_rotation_between_vectors(OPENGL_UP_AXIS, global_original) + q = quaternion_multiply(q, qoffset) + q = normalize(q) + return q + +def align_joint(local_target_axes, up_vec, x_vec): + q = [1, 0, 0, 0] + qy, axes = align_axis_in_place(local_target_axes, "y", up_vec) + q = quaternion_multiply(qy, q) + q = normalize(q) + + # then align the twisting angles + if x_vec is not None: + qx, axes = align_axis_in_place(axes, "x", x_vec) + q = quaternion_multiply(qx, q) + q = normalize(q) + # print("set twist angle", free_joint_name, twist_angle) + return q + +def find_rotation_analytically(new_skeleton, free_joint_name, target, frame, joint_cos_map, is_root=False, max_iter_count=10, twist_angle=None): + global_src_up_vec = target[0] + if twist_angle is None: + global_src_x_vec = target[1] + else: + global_src_x_vec = None + local_target_axes = dict(joint_cos_map[free_joint_name]) + + if is_root: + q = align_root_joint(new_skeleton, free_joint_name, local_target_axes, global_src_up_vec,global_src_x_vec, joint_cos_map, max_iter_count) + + else: + # first align the bone vectors + q = [1, 0, 0, 0] + qy, axes = align_axis_in_place(local_target_axes, "y", global_src_up_vec) + q = quaternion_multiply(qy, q) + q = normalize(q) + + # then align the twisting angles + if global_src_x_vec is not None: + qx, axes = align_axis_in_place(axes, "x", global_src_x_vec) + q = quaternion_multiply(qx, q) + q = normalize(q) + + #if "FK" in free_joint_name: + # q = to_local_cos(new_skeleton, free_joint_name, frame, q) + if new_skeleton.nodes[free_joint_name].parent is not None: + #if "upLeg" in free_joint_name: # it does not work for the legs for some reason + # q = to_local_cos(new_skeleton, new_skeleton.nodes[free_joint_name].parent.node_name, frame, q) + #else: + q = to_local_cos(new_skeleton, new_skeleton.nodes[free_joint_name].parent.node_name, frame, q) + + if twist_angle is not None: + # separate rotation + local_twist_axis = np.array(joint_cos_map[free_joint_name]["y"]) + swing_q, twist_q = swing_twist_decomposition(q, local_twist_axis) + # replace + twist_q = quaternion_about_axis(-twist_angle, local_twist_axis) + q = quaternion_multiply(swing_q, twist_q) + q = normalize(q) + return q + + +def find_rotation_analytically_with_guess(new_skeleton, free_joint_name, target, frame, joint_cos_map, prev_global_q, is_root=False, max_iter_count = 10): + global_src_up_vec = target[0] + global_src_x_vec = target[1] + local_target_axes = joint_cos_map[free_joint_name] + rotated_axes = rotate_axes2(local_target_axes, prev_global_q) + #print("rotate",local_target_axes, rotated_axes, prev_global_q) + #print("") + if is_root: + q = align_root_joint(new_skeleton, free_joint_name, rotated_axes, global_src_up_vec,global_src_x_vec, joint_cos_map, max_iter_count) + else: + q = align_joint(rotated_axes, global_src_up_vec, global_src_x_vec) + q = quaternion_multiply(q, prev_global_q) + q = normalize(q) + return to_local_cos_fast(new_skeleton, free_joint_name, frame, q) + + +def get_parent_map(joints): + """Returns a dict of node names to their parent node's name""" + parent_dict = dict() + for joint_name in joints.keys(): + parent_dict[joint_name] = joints[joint_name]['parent'] + return parent_dict + + +def get_children_map(joints): + """Returns a dict of node names to a list of children names""" + child_dict = dict() + for joint_name in joints.keys(): + parent_name = joints[joint_name]['parent'] + if parent_name not in child_dict: + child_dict[parent_name] = list() + child_dict[parent_name].append(joint_name) + return child_dict + +def swing_twist_decomposition(q, twist_axis): + """ code by janis sprenger based on + Dobrowsolski 2015 Swing-twist decomposition in Clifford algebra. https://arxiv.org/abs/1506.05481 + """ + #q = normalize(q) + #twist_axis = np.array((q * offset))[0] + projection = np.dot(twist_axis, np.array([q[1], q[2], q[3]])) * twist_axis + twist_q = np.array([q[0], projection[0], projection[1],projection[2]]) + if np.linalg.norm(twist_q) == 0: + twist_q = np.array([1,0,0,0]) + twist_q = normalize(twist_q) + swing_q = quaternion_multiply(q, quaternion_inverse(twist_q))#q * quaternion_inverse(twist) + return swing_q, twist_q + + +class PointCloudRetargeting(object): + def __init__(self, src_joints, src_model, target_skeleton, target_to_src_joint_map, scale_factor=1.0, additional_rotation=None, constant_offset=None, place_on_ground=False, ground_height=0): + self.src_joints = src_joints + self.src_model = src_model + self.target_skeleton = target_skeleton + self.target_to_src_joint_map = target_to_src_joint_map + if target_skeleton.skeleton_model["joints"]["pelvis"] is not None: + self.target_skeleton_root = target_skeleton.skeleton_model["joints"]["pelvis"] + else: + self.target_skeleton_root = target_skeleton.root + + #FIXME: enable spine during retargeting + for j in [ "spine_1", "spine"]:#"spine_2", + k = self.target_skeleton.skeleton_model["joints"][j] + self.target_to_src_joint_map[k] = None + + self.src_to_target_joint_map = {v: k for k, v in list(self.target_to_src_joint_map.items())} + self.scale_factor = scale_factor + self.n_params = len(self.target_skeleton.animated_joints) * 4 + 3 + self.ground_height = ground_height + self.additional_rotation = additional_rotation + self.src_inv_joint_map = dict((v,k) for k, v in src_model["joints"].items()) + self.src_child_map = dict() + self.src_parent_map = get_parent_map(src_joints) + src_children_map = get_children_map(src_joints) + for src_name in self.src_joints: + src_child = get_child_joint(self.src_model, self.src_inv_joint_map, src_name, src_children_map) + if src_child is not None: + self.src_parent_map[src_child] = src_name + self.src_child_map[src_name] = src_child + else: + self.src_child_map[src_name] = None + #print("ch",self.src_child_map) + #for j in ["pelvis", "spine", "spine_1", "spine_2"]: + # if j in target_joints: + src_joint_map = self.src_model["joints"] + for j in ["neck", "spine_2", "spine_1", "spine"]: + if j in src_joint_map: + self.src_parent_map["spine_03"] = "pelvis" + #self.src_child_map[src_joint_map["pelvis"]] = [ "left_eye", "right_eye", "left_ear", "right_ear"]#src_joint_map["neck"]#"pelvis" "neck_01" + + self.constant_offset = constant_offset + self.place_on_ground = place_on_ground + self.current_frame_data = dict() + self.prev_frame_data = dict() + self.frame_idx = 0 + self.target_cos_map = create_local_cos_map_from_skeleton_axes_with_map(self.target_skeleton) + if "cos_map" in target_skeleton.skeleton_model: + self.target_cos_map.update(target_skeleton.skeleton_model["cos_map"]) + + target_joints = self.target_skeleton.skeleton_model["joints"] + self.target_spine_joints = [target_joints[j] for j in ["neck", "spine_2", "spine_1", "spine"] if j in target_joints]#["spine_03", "neck_01"] + self.target_ball_joints = [target_joints[j] for j in ["left_shoulder", "right_shoulder", "left_hip", "right_hip"] if j in target_joints]# ["thigh_r", "thigh_l", "upperarm_r", "upperarm_l"] + self.target_ankle_joints = [target_joints[j] for j in ["left_ankle", "right_ankle"] if j in target_joints] + self.clavicle_joints = [target_joints[j] for j in ["right_clavicle", "left_clavicle"] if j in target_joints] + self.twist_angle_joints = [target_joints[j] for j in ["right_knee","left_knee","left_shoulder", "right_shoulder", "right_clavicle", "left_clavicle","right_elbow", "left_elbow","left_wrist", "right_wrist"] if j in target_joints] + if "neck" in target_joints: + self.target_neck_joint = target_joints["neck"] + else: + self.target_neck_joint = None + + left_hip = self.src_model["joints"]["left_hip"] + right_hip = self.src_model["joints"]["right_hip"] + self.left_hip_idx = self.src_joints[left_hip]["index"] + self.right_hip_idx = self.src_joints[right_hip]["index"] + left_shoulder = self.src_model["joints"]["left_shoulder"] + right_shoulder = self.src_model["joints"]["right_shoulder"] + self.left_shoulder_idx = self.src_joints[left_shoulder]["index"] + self.right_shoulder_idx = self.src_joints[right_shoulder]["index"] + self.ref_rotation = dict() + for target_name in self.target_skeleton.animated_joints: + self.ref_rotation[target_name] = get_quaternion_rotation_by_name(target_name, self.target_skeleton.reference_frame, + self.target_skeleton, root_offset=3) + + + def estimate_src_joint_cos(self, src_name, child_name, target_name, src_frame): + joint_idx = self.src_joints[src_name]["index"] + child_idx = self.src_joints[child_name]["index"] + if isinstance(child_idx, list): + child_pos = np.mean([src_frame[idx] for idx in child_idx]) + else: + child_pos = src_frame[joint_idx] + global_src_up_vec = src_frame[child_idx] - child_pos + global_src_up_vec /= np.linalg.norm(global_src_up_vec) + if target_name == self.target_skeleton.skeleton_model["joints"]["pelvis"]: + global_src_x_vec = src_frame[self.left_hip_idx] - src_frame[self.right_hip_idx] + global_src_x_vec /= np.linalg.norm(global_src_x_vec) + elif target_name in self.target_spine_joints or target_name == "CC_Base_Waist": # find x vector from shoulders + global_src_x_vec = src_frame[self.left_shoulder_idx] - src_frame[self.right_shoulder_idx] + global_src_x_vec /= np.linalg.norm(global_src_x_vec) + elif target_name in self.target_ball_joints: # use x vector of child + child_child_name = self.src_child_map[child_name] + child_child_idx = self.src_joints[child_child_name]["index"] + child_global_src_up_vec = src_frame[child_child_idx] - src_frame[child_idx] + child_global_src_up_vec /= np.linalg.norm(child_global_src_up_vec) + angle = abs(math.acos(np.dot(global_src_up_vec, child_global_src_up_vec))) + if angle < 0.01 and src_name in self.prev_frame_data:#np.linalg.norm(delta) < 0.2 + print("use stored at ", self.frame_idx,"for", src_name, "for child") + global_src_up_vec = self.prev_frame_data[src_name][0] + global_src_x_vec = self.prev_frame_data[src_name][1] + else: + global_src_x_vec = np.cross(global_src_up_vec, child_global_src_up_vec) + norm = np.linalg.norm(global_src_x_vec) + global_src_x_vec /= norm + else: # find x vector by cross product with parent + global_src_x_vec = None + if src_name in self.src_parent_map: + parent_joint = self.src_parent_map[src_name] + if parent_joint in self.current_frame_data: + global_parent_up_vector = self.current_frame_data[parent_joint][0] + global_src_x_vec = np.cross(global_src_up_vec, global_parent_up_vector) + norm = np.linalg.norm(global_src_x_vec) + global_src_x_vec /= norm + if target_name not in self.target_ankle_joints: + global_src_x_vec = -global_src_x_vec + return global_src_up_vec, global_src_x_vec + + def get_src_cos_from_joint_map(self, src_name, target_name, src_frame): + src_cos = None + if src_name not in self.src_child_map.keys() or self.src_child_map[src_name] is None: + return src_cos + if self.src_child_map[src_name] in self.src_to_target_joint_map:# and or target_name =="neck_01" or target_name.startswith("hand") + child_name = self.src_child_map[src_name] + if child_name not in self.src_joints.keys(): + return src_cos + src_cos = self.estimate_src_joint_cos(src_name, child_name, target_name, src_frame) + self.current_frame_data[src_name] = src_cos + + return src_cos + + + def get_src_cos_from_multiple_points(self, src_frame, cos_def): + up_start = np.mean([src_frame[self.src_joints[name]["index"]] for name in cos_def["up_start"]], axis=0) + up_end = np.mean([src_frame[self.src_joints[name]["index"]] for name in cos_def["up_end"]], axis=0) + global_src_up_vec = up_end - up_start + x_start = np.mean([src_frame[self.src_joints[name]["index"]] for name in cos_def["x_start"]], axis=0) + x_end = np.mean([src_frame[self.src_joints[name]["index"]] for name in cos_def["x_end"]], axis=0) + global_src_x_vec = x_end - x_start + global_src_up_vec /= np.linalg.norm(global_src_up_vec) + global_src_x_vec /= np.linalg.norm(global_src_x_vec) + return global_src_up_vec, global_src_x_vec + + + def retarget_frame(self, src_frame, prev_frame, pose_angles=None): + + target_joints = self.target_skeleton.skeleton_model["joints"] + joint_map = dict() + for k, v in target_joints.items(): + joint_map[v] = k + self.target_skeleton.clear_cached_global_matrices() + target_frame = np.zeros(self.n_params) + self.prev_frame_data = self.current_frame_data + self.current_frame_data = dict() + + # copy the root translation assuming the rocketbox skeleton with static offset on the hips is used as source + #target_frame[:3] = np.array(src_frame[0]) * self.scale_factor + target_frame[:3] = src_frame[self.left_hip_idx] + (src_frame[self.right_hip_idx] - src_frame[self.left_hip_idx])/2 + target_frame[:3] *= self.scale_factor + if self.constant_offset is not None: + target_frame[:3] += self.constant_offset + target_offset = 3 + for target_name in self.target_skeleton.animated_joints: + q = self.ref_rotation[target_name] + src_cos = None + default_name = None + if target_name in joint_map: + default_name = joint_map[target_name] + if default_name in self.src_model["cos_defs"]: + src_cos = self.get_src_cos_from_multiple_points(src_frame, self.src_model["cos_defs"][default_name]) + elif target_name in self.target_to_src_joint_map: + src_name = self.target_to_src_joint_map[target_name] + if src_name is not None and src_name in self.src_joints: + src_cos = self.get_src_cos_from_joint_map(src_name, target_name, src_frame) + if src_cos is not None and src_cos[1] is not None: + twist_angle = None + if pose_angles is not None and self.target_skeleton.nodes[target_name].parent is not None and target_name in self.twist_angle_joints: + joint_idx = self.src_joints[src_name]["index"] + twist_angle = pose_angles[joint_idx][0] + is_root = target_name == self.target_skeleton_root + q = find_rotation_analytically(self.target_skeleton, target_name, src_cos, target_frame, self.target_cos_map, is_root=is_root, twist_angle=twist_angle) + q = q/np.linalg.norm(q) + + #if ref_frame is not None: + # q = q if np.dot(ref_frame[target_offset:target_offset + 4], q) >= 0 else -q + + if prev_frame is not None: + prev_q = normalize(prev_frame[target_offset:target_offset + 4]) + if np.dot(q, prev_q) < 0: + q = -q + inv_q = normalize(quaternion_inverse(q)) + delta_q = normalize(quaternion_multiply(inv_q, prev_q)) + theta = 2 * np.arccos(delta_q[0]) + if abs(theta) > np.pi: + print("keep", self.frame_idx, theta,src_name, q, prev_q) + q = prev_q + target_frame[target_offset:target_offset + 4] = q + target_offset += 4 + return target_frame + + def run(self, src_frames, frame_range): + n_frames = len(src_frames) + target_frames = [] + self.frame_idx = 0 + if n_frames > 0: + if frame_range is None: + frame_range = (0, n_frames) + if self.additional_rotation is not None: + src_frames = apply_additional_rotation_on_frames(self.src_skeleton.animated_joints, src_frames, self.additional_rotation) + + prev_frame = None + for idx, src_frame in enumerate(src_frames[frame_range[0]:frame_range[1]]): + target_frame = self.retarget_frame(src_frame, prev_frame) + prev_frame = target_frame + target_frames.append(target_frame) + self.frame_idx += 1 + target_frames = np.array(target_frames) + if self.place_on_ground: + delta = target_frames[0][1] - self.ground_height + target_frames[:, 1] -= delta + return target_frames + + +def generate_joint_map(src_model, target_model): + joint_map = dict() + for j in src_model["joints"]: + if j in target_model["joints"]: + src = src_model["joints"][j] + target = target_model["joints"][j] + joint_map[target] = src + return joint_map + + +def retarget_from_point_cloud_to_target(src_joints, src_model, target_skeleton, src_frames, joint_map=None, additional_rotation=None, scale_factor=1.0, frame_range=None, place_on_ground=False): + if joint_map is None: + joint_map = generate_joint_map(src_model, target_skeleton.skeleton_model) + constant_offset = -np.array(target_skeleton.nodes[target_skeleton.root].offset) + retargeting = PointCloudRetargeting(src_joints, src_model, target_skeleton, joint_map, scale_factor, additional_rotation=additional_rotation, place_on_ground=place_on_ground, constant_offset=constant_offset) + return retargeting.run(src_frames, frame_range) diff --git a/build/lib/anim_utils/retargeting/utils.py b/build/lib/anim_utils/retargeting/utils.py new file mode 100644 index 0000000..41ebc27 --- /dev/null +++ b/build/lib/anim_utils/retargeting/utils.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +import numpy as np +import math +from transformations import quaternion_from_matrix, euler_matrix, quaternion_matrix, quaternion_multiply, euler_from_quaternion, quaternion_from_euler, quaternion_inverse, euler_from_matrix +from ..animation_data.utils import quaternion_from_vector_to_vector + +AXES = [[1,0,0],[0,1,0],[0,0,1], [-1,0,0],[0,-1,0],[0,0,-1]] + + +def get_angle_between_vectors(v1, v2): + q = quaternion_from_vector_to_vector(v1, v2) + v = q[1:] + sin_theta = np.linalg.norm(v) + sin_theta = min(sin_theta, 1.0) + abs_angle = 2 * math.asin(sin_theta) + return abs_angle + + +def project_vector_on_axis(v): + min_idx = -1 + min_angle = np.inf + for idx, a in enumerate(AXES): + angle = get_angle_between_vectors(v, a) + if angle < min_angle: + min_angle = angle + min_idx = idx + length = np.linalg.norm(v) + return np.array(AXES[min_idx]) * length + + +def align_root_translation(target_skeleton, target_frame, src_frame, root_node="pelvis", src_scale_factor=1.0): + target_pos = target_skeleton.nodes[root_node].get_global_position(target_frame) + target_pos = np.array([target_pos[0], target_pos[2]]) + src_pos = np.array([src_frame[0], src_frame[2]])*src_scale_factor + delta = src_pos - target_pos + target_frame[0] += delta[0] + target_frame[2] += delta[1] + return target_frame + + +def align_quaternion_frames(target_skeleton, frames): + """align quaternions for blending + src: http://physicsforgames.blogspot.de/2010/02/quaternions.html + """ + ref_frame = None + new_frames = [] + for frame in frames: + if ref_frame is None: + ref_frame = frame + else: + offset = 3 + for joint in target_skeleton.animated_joints: + q = frame[offset:offset + 4] + ref_q = ref_frame[offset:offset + 4] + dot = np.dot(ref_q, q) + if dot < 0: + frame[offset:offset + 4] = -q + offset += 4 + new_frames.append(frame) + return new_frames + + +def get_coordinate_system_axes(skeleton, joint_name, frame, axes): + global_m = skeleton.nodes[joint_name].get_global_matrix(frame)[:3,:3] + dirs = [] + for axis in axes: + direction = np.dot(global_m, axis) + direction /= np.linalg.norm(direction) + dirs.append(direction) + return np.array(dirs) + + +def rotate_axes(axes, q): + m = quaternion_matrix(q)[:3, :3] + aligned_axes = dict() + for key, a in axes.items(): + aligned_axes[key] = np.dot(m, a) + aligned_axes[key] = normalize(aligned_axes[key]) + return aligned_axes + + +def align_axis(axes, key, new_vec): + q = quaternion_from_vector_to_vector(axes[key], new_vec) + aligned_axes = rotate_axes(axes, q) + return q, aligned_axes + + +def get_quaternion_rotation_by_name(joint_name, frame, skeleton, root_offset=3): + assert joint_name in skeleton.animated_joints + joint_index = skeleton.animated_joints.index(joint_name) + return frame[joint_index * 4 + root_offset : (joint_index + 1) * 4 + root_offset] + + +def normalize(v): + return v/np.linalg.norm(v) + + +def filter_dofs(q, fixed_dims): + e = list(euler_from_quaternion(q)) + for d in fixed_dims: + e[d] = 0 + q = quaternion_from_euler(*e) + return q + + +def quaternion_from_axis_angle(axis, angle): + q = [1,0,0,0] + q[1] = axis[0] * math.sin(angle / 2) + q[2] = axis[1] * math.sin(angle / 2) + q[3] = axis[2] * math.sin(angle / 2) + q[0] = math.cos(angle / 2) + return normalize(q) + + +def find_rotation_between_vectors(a, b): + """http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another""" + if np.array_equiv(a, b): + return [1, 0, 0, 0] + + axis = normalize(np.cross(a, b)) + dot = np.dot(a, b) + if dot >= 1.0: + return [1, 0, 0, 0] + angle = math.acos(dot) + q = quaternion_from_axis_angle(axis, angle) + return q + + +def to_local_cos_old(skeleton, node_name, frame, q): + # bring into parent coordinate system + pm = skeleton.nodes[node_name].get_global_matrix(frame)#[:3,:3] + inv_pm = np.linalg.inv(pm) + r = quaternion_matrix(q)[:3,:3] + lr = np.dot(inv_pm, r)[:3,:3] + q = quaternion_from_matrix(lr) + return q + + +def to_local_cos(skeleton, node_name, frame, q): + # bring into parent coordinate system + pm = skeleton.nodes[node_name].get_global_matrix(frame)#[:3,:3] + inv_p = quaternion_inverse(quaternion_from_matrix(pm)) + normalize(inv_p) + return quaternion_multiply(inv_p, q) + + +def to_global_cos(skeleton, node_name, frame, q): + pm = skeleton.nodes[node_name].get_global_matrix(frame)#[:3,:3] + r = quaternion_matrix(q)[:3, :3] + lr = np.dot(pm, r)[:3, :3] + q = quaternion_from_matrix(lr) + return q + + +def apply_additional_rotation_on_frames(animated_joints, frames, rotation_offsets): + new_frames = [] + for frame in frames: + new_frame = frame[:] + for idx, name in enumerate(animated_joints): + if name in rotation_offsets: + euler = np.radians(rotation_offsets[name]) + additional_q = quaternion_from_euler(*euler) + offset = idx * 4 + 3 + q = new_frame[offset:offset + 4] + new_frame[offset:offset + 4] = quaternion_multiply(q, additional_q) + + new_frames.append(new_frame) + return new_frames diff --git a/build/lib/anim_utils/utilities/__init__.py b/build/lib/anim_utils/utilities/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/build/lib/anim_utils/utilities/custom_math.py b/build/lib/anim_utils/utilities/custom_math.py new file mode 100644 index 0000000..be3f9ec --- /dev/null +++ b/build/lib/anim_utils/utilities/custom_math.py @@ -0,0 +1,651 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +# encoding: UTF-8 +__author__ = 'hadu01' + +import numpy as np +import math +from transformations.transformations import _AXES2TUPLE, _TUPLE2AXES, _NEXT_AXIS, quaternion_from_euler +LEN_CARTESIAN = 3 +LEN_QUATERNION = 4 + + +def diff_quat(q0, q1): + """ + Args: + q0, q1: (qw, qx, qy, qz) + + Returns: + float: angle in radians + """ + w0, x0, y0, z0 = q0 + w1, x1, y1, z1 = q1 + d1 = w1*w1+x1*x1+y1*y1+z1*z1 + + w = x1 * x0 + y1 * y0 + z1 * z0 + w1 * w0 + x = -x1 * w0 - y1 * z0 + z1 * y0 + w1 * x0 + y = x1 * z0 - y1 * w0 - z1 * x0 + w1 * y0 + z = -x1 * y0 + y1 * x0 - z1 * w0 + w1 * z0 + return (w, x, y, z) + + +def euler_matrix_jac(ai, aj, ak, axes='rxyz', der_axis='x'): + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i + parity] + k = _NEXT_AXIS[i - parity + 1] + + if frame: + ai, ak = ak, ai + if parity: + ai, aj, ak = -ai, -aj, -ak + si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) + ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) + cc, cs = ci * ck, ci * sk + sc, ss = si * ck, si * sk + M = np.zeros([4, 4]) + # rotation matrix + # if repetition: + # M[i, i] = cj + # M[i, j] = sj * si + # M[i, k] = sj * ci + # M[j, i] = sj * sk + # M[j, j] = -cj * ss + cc + # M[j, k] = -cj * cs - sc + # M[k, i] = -sj * ck + # M[k, j] = cj * sc + cs + # M[k, k] = cj * cc - ss + # else: + # M[i, i] = cj * ck + # M[i, j] = sj * sc - cs + # M[i, k] = sj * cc + ss + # M[j, i] = cj * sk + # M[j, j] = sj * ss + cc + # M[j, k] = sj * cs - sc + # M[k, i] = -sj + # M[k, j] = cj * si + # M[k, k] = cj * ci + ''' + first order derivatives: + for ai: + d(si)/d(ai) = ci, d(sj)/d(ai) = 0, d(sk)/d(ai) = 0 + d(ci)/d(ai) = -si, d(cj)/d(ai) = 0, d(ck)/d(ai) = 0 + d(cc)/d(ai) = -si * ck = -sc, d(cs)/d(ai) = -si * sk = -ss + d(sc)/d(ai) = ci * ck = cc, d(ss)/d(ai) = ci * sk = cs + + for aj: + d(si)/d(aj) = 0, d(sj)/d(aj) = cj, d(sk)/d(aj) = 0 + d(ci)/d(aj) = 0, d(cj)/d(aj) = -sj, d(ck)/d(aj) = 0 + d(cc)/d(aj) = 0, d(cs)/d(aj) = 0 + d(sc)/d(aj) = 0, d(ss)/d(aj) = 0 + + for ak: + d(si)/d(ak) = 0, d(sj)/d(ak) = 0, d(sk)/d(ak) = ck + d(ci)/d(ak) = 0, d(cj)/d(ak) = 0, d(ck)/d(ak) = -sk + d(cc)/d(ak) = -ci * sk = -cs, d(cs)/d(ak) = ci * ck = cc + d(sc)/d(ak) = -si * sk = -ss, d(ss)/d(ak) = si * ck = sc + ''' + if repetition: + if der_axis == 'x': + M[i, i] = 0 + M[i, j] = sj * ci + M[i, k] = -sj * si + M[j, i] = 0 + M[j, j] = -cj * cc + sc + M[j, k] = cj * ss - cc + M[k, i] = 0 + M[k, j] = cj * cc - ss + M[k, k] = -cj * sc - cs + elif der_axis == 'y': + M[i, i] = -sj + M[i, j] = cj * si + M[i, k] = cj * ci + M[j, i] = cj * sk + M[j, j] = sj * ss + M[j, k] = sj * cs + M[k, i] = -cj * ck + M[k, j] = -sj * sc + M[k, k] = -sj * cc + elif der_axis == 'z': + M[i, i] = 0 + M[i, j] = 0 + M[i, k] = 0 + M[j, i] = sj * ck + M[j, j] = -cj * sc - cs + M[j, k] = -cj * cc + ss + M[k, i] = sj * sk + M[k, j] = -cj * ss + cc + M[k, k] = -cj * cs - sc + else: + raise ValueError('Unknown axis type!') + else: + if der_axis == 'x': + if not frame: + M[i, i] = 0 + M[i, j] = sj * cc + ss + M[i, k] = -sj * sc + cs + M[j, i] = 0 + M[j, j] = sj * cs - sc + M[j, k] = -sj * ss - cc + M[k, i] = 0 + M[k, j] = cj * ci + M[k, k] = -cj * si + # if not parity: + # M[i, i] = 0 + # M[i, j] = sj * cc + ss + # M[i, k] = -sj * sc + cs + # M[j, i] = 0 + # M[j, j] = sj * cs - sc + # M[j, k] = -sj * ss - cc + # M[k, i] = 0 + # M[k, j] = cj * ci + # M[k, k] = -cj * si + # else: + # M[i, i] = 0 + # M[i, j] = -sj * cc + ss + # M[i, k] = -sj * sc - cs + # M[j, i] = 0 + # M[j, j] = -sj * cs - sc + # M[j, k] = -sj * ss + cc + # M[k, i] = 0 + # M[k, j] = -cj * ci + # M[k, k] = -cj * si + else: + M[i, i] = -cj * sk + M[i, j] = -sj * ss - cc + M[i, k] = -sj * cs + sc + M[j, i] = cj * ck + M[j, j] = sj * sc - cs + M[j, k] = sj * cc + ss + M[k, i] = 0 + M[k, j] = 0 + M[k, k] = 0 + # if not parity: + # M[i, i] = -cj * sk + # M[i, j] = -sj * ss - cc + # M[i, k] = -sj * cs + sc + # M[j, i] = cj * ck + # M[j, j] = sj * sc - cs + # M[j, k] = sj * cc + ss + # M[k, i] = 0 + # M[k, j] = 0 + # M[k, k] = 0 + # else: + # M[i, i] = -cj * sk + # M[i, j] = -sj * ss + cc + # M[i, k] = -sj * cs - sc + # M[j, i] = -cj * ck + # M[j, j] = -sj * sc - cs + # M[j, k] = -sj * cc + ss + # M[k, i] = 0 + # M[k, j] = 0 + # M[k, k] = 0 + elif der_axis == 'y': + M[i, i] = -sj * ck + M[i, j] = cj * sc + M[i, k] = cj * cc + M[j, i] = -sj * sk + M[j, j] = cj * ss + M[j, k] = cj * cs + M[k, i] = -cj + M[k, j] = -sj * si + M[k, k] = -sj * ci + # if not parity: + # M[i, i] = -sj * ck + # M[i, j] = cj * sc + # M[i, k] = cj * cc + # M[j, i] = -sj * sk + # M[j, j] = cj * ss + # M[j, k] = cj * cs + # M[k, i] = -cj + # M[k, j] = -sj * si + # M[k, k] = -sj * ci + # else: + # M[i, i] = -sj * ck + # M[i, j] = -cj * sc + # M[i, k] = -cj * cc + # M[j, i] = -sj * sk + # M[j, j] = -cj * ss + # M[j, k] = -cj * cs + # M[k, i] = cj + # M[k, j] = -sj * si + # M[k, k] = -sj * ci + elif der_axis == 'z': + if not frame: + M[i, i] = -cj * sk + M[i, j] = -sj * ss - cc + M[i, k] = -sj * cs + sc + M[j, i] = cj * ck + M[j, j] = sj * sc - cs + M[j, k] = sj * cc + ss + M[k, i] = 0 + M[k, j] = 0 + M[k, k] = 0 + # if not parity: + # M[i, i] = -cj * sk + # M[i, j] = -sj * ss - cc + # M[i, k] = -sj * cs + sc + # M[j, i] = cj * ck + # M[j, j] = sj * sc - cs + # M[j, k] = sj * cc + ss + # M[k, i] = 0 + # M[k, j] = 0 + # M[k, k] = 0 + # else: + # M[i, i] = -cj * sk + # M[i, j] = -sj * ss + cc + # M[i, k] = -sj * cs - sc + # M[j, i] = -cj * ck + # M[j, j] = -sj * sc - cs + # M[j, k] = -sj * cc + ss + # M[k, i] = 0 + # M[k, j] = 0 + # M[k, k] = 0 + else: + M[i, i] = 0 + M[i, j] = sj * cc + ss + M[i, k] = -sj * sc + cs + M[j, i] = 0 + M[j, j] = sj * cs - sc + M[j, k] = -sj * ss - cc + M[k, i] = 0 + M[k, j] = cj * ci + M[k, k] = -cj * si + # if not parity: + # M[i, i] = 0 + # M[i, j] = sj * cc + ss + # M[i, k] = -sj * sc + cs + # M[j, i] = 0 + # M[j, j] = sj * cs - sc + # M[j, k] = -sj * ss - cc + # M[k, i] = 0 + # M[k, j] = cj * ci + # M[k, k] = -cj * si + # else: + # M[i, i] = 0 + # M[i, j] = -sj * cc + ss + # M[i, k] = -sj * sc - cs + # M[j, i] = 0 + # M[j, j] = -sj * cs - sc + # M[j, k] = -sj * ss + cc + # M[k, i] = 0 + # M[k, j] = -cj * ci + # M[k, k] = -cj * si + else: + raise ValueError('Unknown axis type!') + if parity: + M = -M + return M + + +def quaternion_inv(q): + """ + Inverse of quaternion q + Args: + q: (qw, qx, qy, qz) + """ + w, x, y, z = q + d = w*w + x*x + y*y + z*z + q_inv = (w/d, -x/d, -y/d, -z/d) + return q_inv + + +def normalize_quaternion(q): + q = np.asarray(q) + normalized_q = q/np.linalg.norm(q) + return normalized_q + + +def error_measure_3d_mat(raw_data, + reconstructed_data): + ''' + Compute the mean squared error bewteen original data and reconstructed data + The data matrix is array3D: n_samples * n_frames * n_dims + ''' + raw_data = np.asarray(raw_data) + reconstructed_data = np.asarray(reconstructed_data) + assert raw_data.shape == reconstructed_data.shape + diff = raw_data - reconstructed_data + n_samples, n_frames, n_dim = diff.shape + err = 0 + for i in range(n_samples): + for j in range(n_frames): + err += np.linalg.norm(diff[i, j]) + err = err/(n_samples * n_frames) + return err + + +def err_quat_data(raw_data, + reconstructed_data): + raw_data = np.asarray(raw_data) + reconstructed_data = np.asarray(reconstructed_data) + assert raw_data.shape == reconstructed_data.shape + diff = raw_data - reconstructed_data + n_samples, n_frames, n_dim = diff.shape + err = 0 + for i in range(n_samples): + for j in range(n_frames): + err += np.linalg.norm(diff[i, j][3:]) + + err = err/(n_samples * n_frames) + return err + + +def quat_to_expmap(q): + """ + map quaternion to exponential map + :param q: [qw, qx, qy, qz] + :return: + """ + q = q/np.linalg.norm(q) + theta = 2 * np.arccos(q[0]) + if theta < 1e-3: + auxillary = 1/(0.5 + theta ** 2 / 48) + else: + auxillary = theta / np.sin(0.5*theta) + vx = q[1] * auxillary + vy = q[2] * auxillary + vz = q[3] * auxillary + return np.array([vx, vy, vz]) + +def euler_to_expmap(euler_angles): + ''' + convert euler angles to exponential map + :param euler_angles: degree + :return: + ''' + euler_angles_rad = np.deg2rad(euler_angles) + quat = quaternion_from_euler(*euler_angles) + return quat_to_expmap(quat) + + +def angle_axis_from_quaternion(q): + q = q/np.linalg.norm(q) + theta = 2 * np.arccos(q[0]) + if theta < 1e-3: + axis = np.array([0, 0, 0]) + else: + x = q[1] / np.sin(0.5 * theta) + y = q[2] / np.sin(0.5 * theta) + z = q[3] / np.sin(0.5 * theta) + axis = np.array([x, y, z]) + return theta, axis + + +def quat_to_logmap(q): + q = q/np.linalg.norm(q) + theta = 2 * np.arccos(q[0]) + if theta < 1e-3: + auxillary = 1/(0.5 + theta ** 2 / 48) + else: + auxillary = theta / np.sin(0.5*theta) + vx = q[1] * auxillary + vy = q[2] * auxillary + vz = q[3] * auxillary + return np.array([vx, vy, vz]) + + +def expmap_to_quat(exp_map): + theta = np.linalg.norm(exp_map) + print('theta is: ', theta) + if np.abs(theta) < 1e-3: + auxillary = 0.5 + theta ** 2 / 48 + else: + auxillary = np.sin(0.5*theta) / theta + qw = np.cos(theta/2) + qx = exp_map[0]*auxillary + qy = exp_map[1]*auxillary + qz = exp_map[2]*auxillary + return [qw, qx, qy, qz] + + +def logmap_to_quat(v): + theta = np.linalg.norm(v) + if theta < 1e-3: + auxillary = 0.5 + theta ** 2 / 48 + else: + auxillary = np.sin(0.5*theta) / theta + qw = np.cos(theta/2) + qx = v[0]*auxillary + qy = v[1]*auxillary + qz = v[2]*auxillary + return [qw, qx, qy, qz] + + +def areQuatClose(quat1, quat2): + dot = np.dot(quat1, quat2) + if dot < 0.0: + return False + else: + return True + + +def cartesian_splines_distance(raw_splines, reconstructed_splines, skeleton, weighted_error=True): + """ + Calculate the Euclidean distance between motion represented as Cartesian splines + :param raw_splines: Cartesian spline coefficience matrix + :param reconstructed_splines: spline coefficience matrix + :param skeleton: + :param weighted_error: + :return: + """ + raw_splines = np.asarray(raw_splines) + reconstructed_splines = np.asarray(reconstructed_splines) + n_samples, n_basis, n_dims = raw_splines.shape + assert n_dims%LEN_CARTESIAN == 0 + n_joints = n_dims/LEN_CARTESIAN + if not weighted_error: + return error_measure_3d_mat(raw_splines, reconstructed_splines)/n_joints + else: + joint_weights = skeleton.joint_weights[:-4] # ignore the last two tool joints + weight_vector = np.ones(n_dims) + for i in range(n_joints): + weight_vector[i*LEN_QUATERNION: (i+1)*LEN_QUATERNION] *= joint_weights[i] + weight_mat = np.diag(weight_vector) + return error_measure_3d_mat(np.dot(raw_splines, weight_mat), + np.dot(reconstructed_splines, weight_mat))/n_joints + +def angle_between_vectors(v1, v2): + """ + Compute angle (in radians) between two 3d vectors (vq, v2) + :param v1: (x, y, z) + :param v2: (x, y, z) + :return: + """ + v1 = np.asarray(v1) + v2 = np.asarray(v2) + assert len(v1) == 3 + assert len(v2) == 3 + theta = np.arccos(np.dot(v1, v2)/(np.linalg.norm(v1) * np.linalg.norm(v2))) + return theta + + +def angle_between_2d_vector(v1, v2): + v1 = np.asarray(v1) + v2 = np.asarray(v2) + theta = np.arccos(np.dot(v1, v2)/np.linalg.norm(v1) * np.linalg.norm(v2)) + return theta + + + +def find_local_maximum(signal, neighbor_size): + ''' + find local maximums of a signal in a given beighbor size + :param signal: + :param neighbor_size: + :return: + ''' + # mirror the signal on the boundary with k = stepsize/2 + signal_len = len(signal) + localMax = [] + k = int(np.floor(neighbor_size/2)) + extendedSignal = np.zeros(signal_len + 2*k) + extendedSignal[k: -k] = signal + for i in range(k): + extendedSignal[i] = signal[0] + extendedSignal[-i-1] = signal[-1] + for j in range(0, signal_len): + searchArea = extendedSignal[j: j+neighbor_size] + maximum = np.max(searchArea) + if maximum == extendedSignal[j+k]: + localMax.append(j) + + return localMax + + +def find_local_minimum(signal, neighbor_size): + ''' + find local maximums of a signal in a given beighbor size + :param signal: + :param neighbor_size: + :return: + ''' + # mirror the signal on the boundary with k = stepsize/2 + signal_len = len(signal) + localMin = [] + k = int(np.floor(neighbor_size/2)) + extendedSignal = np.zeros(signal_len + 2*k) + for i in range(signal_len): + extendedSignal[i+k] = signal[i] + for i in range(k): + extendedSignal[i] = signal[0] + extendedSignal[-i-1] = signal[-1] + for j in range(1, signal_len-1): + searchArea = extendedSignal[j: j+neighbor_size] + minimum = np.min(searchArea) + if minimum == extendedSignal[j+k]: + localMin.append(j) + + return localMin + + +def linear_weights_curve(window_size): + ''' + create a linear weight curve, the starting and ending points are zero, and the peak is in the middle + :param window_size: + :return: + ''' + middle_point = window_size/2.0 + weights = np.zeros(window_size) + for i in range(0, int(np.floor(middle_point))+1): + weights[i] = (1.0/middle_point) * i + for i in range(int(np.ceil(middle_point)), window_size): + weights[i] = -(1.0/middle_point) * i + 2.0 + return weights + + +def sin_weights_curve(window_size): + ''' + create a sine weight curve, the starting and ending points are zero, and the peak is in the middle + :param window_size: + :return: + ''' + return np.sin(np.arange(0, window_size) * np.pi / (window_size - 1)) + + +def power_weights_curve(window_size): + ''' + create a power function weight curve, the starting and ending points are zero, and the peak is in the middle + :param window_size: + :return: + ''' + middle_point = window_size/2.0 + weights = np.zeros(window_size) + a = (1.0/middle_point) ** 2 + for i in range(0, int(np.floor(middle_point))+1): + weights[i] = a * (i**2) + for i in range(int(np.ceil(middle_point)), window_size): + weights[i] = a * (i-window_size + 1) ** 2 + return weights + + +def tdot_mat(mat, out=None): + ''' + a copy of implementation of M*M_T from GPy + :param mat: + :param out: + :return: + ''' + from scipy.linalg import blas + """returns np.dot(mat, mat.T), but faster for large 2D arrays of doubles.""" + if (mat.dtype != 'float64') or (len(mat.shape) != 2): + return np.dot(mat, mat.T) + nn = mat.shape[0] + if out is None: + out = np.zeros((nn, nn)) + else: + assert(out.dtype == 'float64') + assert(out.shape == (nn, nn)) + # FIXME: should allow non-contiguous out, and copy output into it: + assert(8 in out.strides) + # zeroing needed because of dumb way I copy across triangular answer + out[:] = 0.0 + + # # Call to DSYRK from BLAS + mat = np.asfortranarray(mat) + out = blas.dsyrk(alpha=1.0, a=mat, beta=0.0, c=out, overwrite_c=1, + trans=0, lower=0) + + symmetrify(out, upper=True) + return np.ascontiguousarray(out) + + +def symmetrify(A, upper=False): + """ + Take the square matrix A and make it symmetrical by copting elements from + the lower half to the upper + + works IN PLACE. + + note: tries to use cython, falls back to a slower numpy version + """ + _symmetrify_numpy(A, upper) + + +def _symmetrify_numpy(A, upper=False): + triu = np.triu_indices_from(A,k=1) + if upper: + A.T[triu] = A[triu] + else: + A[triu] = A.T[triu] + + +def quaternion_from_vectors(vec1, vec2): + vec1 = vec1/np.linalg.norm(vec1) + vec2 = vec2/np.linalg.norm(vec2) + inner_prod = np.dot(vec1, vec2) + cross_prod = np.cross(vec1, vec2) + cross_prod = cross_prod / np.linalg.norm(cross_prod) + theta = np.arccos(inner_prod) + sin_half = np.sin(theta/2.0) + w = np.cos(theta/2.0) + print(w) + x = cross_prod[0] * sin_half + y = cross_prod[1] * sin_half + z = cross_prod[2] * sin_half + return np.array([w, x, y, z]) diff --git a/build/lib/anim_utils/utilities/io_helper_functions.py b/build/lib/anim_utils/utilities/io_helper_functions.py new file mode 100644 index 0000000..d615fb0 --- /dev/null +++ b/build/lib/anim_utils/utilities/io_helper_functions.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +""" +Created on Wed Feb 04 12:56:39 2015 + +@author: Han Du, Martin Manns, Erik Herrmann +""" +import os +import sys +import glob +import json +import collections +import math +import numpy as np +from datetime import datetime +from ..animation_data.bvh import BVHWriter, BVHReader + + +def load_json_file(filename, use_ordered_dict=True): + """ Load a dictionary from a file + + Parameters + ---------- + * filename: string + \tThe path to the saved json file. + * use_ordered_dict: bool + \tIf set to True dicts are read as OrderedDicts. + """ + tmp = None + with open(filename, 'r') as infile: + if use_ordered_dict: + tmp = json.JSONDecoder( + object_pairs_hook=collections.OrderedDict).decode( + infile.read()) + else: + tmp = json.load(infile) + infile.close() + return tmp + + +def write_to_json_file(filename, serializable, indent=4): + with open(filename, 'w') as outfile: + tmp = json.dumps(serializable, indent=4) + outfile.write(tmp) + outfile.close() + + +def gen_file_paths(directory, mask='*mm.json'): + """Generator of input file paths + + Parameters + ---------- + + * dir: String + \tPath of input folder, in which the input files reside + * mask: String, defaults to '*.bvh' + \tMask pattern for file search + + """ + + if not directory.endswith(os.sep): + directory += os.sep + + for filepath in glob.glob(directory + mask): + yield filepath + + +def clean_path(path): + path = path.replace('/', os.sep).replace('\\', os.sep) + if os.sep == '\\' and '\\\\?\\' not in path: + # fix for Windows 260 char limit + relative_levels = len([directory for directory in path.split(os.sep) + if directory == '..']) + cwd = [ + directory for directory in os.getcwd().split( + os.sep)] if ':' not in path else [] + path = '\\\\?\\' + os.sep.join(cwd[:len(cwd) - relative_levels] + [ + directory for directory in path.split(os.sep) if directory != ''][relative_levels:]) + return path + + +def load_bvh_files_from_folder(data_folder): + bvhfiles = glob.glob(os.path.join(data_folder, '*.bvh')) + # order filenames alphabetically + tmp = {} + for item in bvhfiles: + filename = os.path.split(item)[-1] + bvhreader = BVHReader(item) + tmp[filename] = bvhreader.frames + motion_data = collections.OrderedDict(sorted(tmp.items())) + return motion_data diff --git a/build/lib/anim_utils/utilities/log.py b/build/lib/anim_utils/utilities/log.py new file mode 100644 index 0000000..b8a44c2 --- /dev/null +++ b/build/lib/anim_utils/utilities/log.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python +# +# Copyright 2019 DFKI GmbH. +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the +# "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to the +# following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN +# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE +# USE OR OTHER DEALINGS IN THE SOFTWARE. +LOG_MODE_ERROR = -1 +LOG_MODE_INFO = 1 +LOG_MODE_DEBUG = 2 + +_lines = [] +_active = True +_mode = LOG_MODE_INFO + +def activate(): + global _active + _active = True + + +def deactivate(): + global _active + _active = False + +def set_log_mode(mode): + global _mode + _mode = mode + +def write_log(*args): + global _active + global _lines + if _active: + line = " ".join(map(str, args)) + print(line) + _lines.append(line) + + +def write_message_to_log(message, mode=LOG_MODE_INFO): + global _active + global _lines + if _active and _mode >= mode: + print(message) + _lines.append(message) + + +def save_log(filename): + global _lines + with open(filename, "wb") as outfile: + for l in _lines: + outfile.write(l+"\n") + + +def clear_log(): + global _lines + _lines = [] diff --git a/dist/anim_utils-0.1-py3.10.egg b/dist/anim_utils-0.1-py3.10.egg new file mode 100644 index 0000000..0ed1477 Binary files /dev/null and b/dist/anim_utils-0.1-py3.10.egg differ diff --git a/setup.py b/setup.py index 70285a6..5110ae9 100644 --- a/setup.py +++ b/setup.py @@ -24,6 +24,6 @@ license='MIT', keywords="skeleton animation data retargeting", packages=find_packages(exclude=("examples",)), - python_requires=">=3.5.*, <4", + python_requires=">=3.5, <4", install_requires=requirements, )