├── .gitignore ├── LICENSE ├── README.md ├── anim_utils ├── __init__.py ├── animation_data │ ├── __init__.py │ ├── acclaim.py │ ├── bvh.py │ ├── constants.py │ ├── fbx │ │ ├── __init__.py │ │ ├── fbx_export.py │ │ └── fbx_import.py │ ├── joint_constraints.py │ ├── motion_blending.py │ ├── motion_concatenation.py │ ├── motion_distance.py │ ├── motion_state.py │ ├── motion_vector.py │ ├── quaternion_frame.py │ ├── skeleton.py │ ├── skeleton_builder.py │ ├── skeleton_models.py │ ├── skeleton_node.py │ └── utils.py ├── motion_editing │ ├── __init__.py │ ├── analytical_inverse_kinematics.py │ ├── coordinate_cyclic_descent.py │ ├── cubic_motion_spline.py │ ├── fabrik_chain.py │ ├── fabrik_node.py │ ├── footplant_constraint_generator.py │ ├── hybrit_ik.py │ ├── ik_constraints.py │ ├── ik_constraints_builder.py │ ├── motion_editing.py │ ├── motion_filtering.py │ ├── motion_grounding.py │ ├── numerical_ik_exp.py │ ├── numerical_ik_quat.py │ ├── skeleton_pose_model.py │ └── utils.py ├── retargeting │ ├── __init__.py │ ├── analytical.py │ ├── constants.py │ ├── constrained_retargeting.py │ ├── fast_point_cloud_retargeting.py │ ├── point_cloud_retargeting.py │ └── utils.py └── utilities │ ├── __init__.py │ ├── custom_math.py │ ├── io_helper_functions.py │ └── log.py ├── examples ├── amc_to_bvh.py ├── data │ ├── bvh │ │ ├── custom_skeleton_walk_example.bvh │ │ └── mh_cmu_skeleton_pose.bvh │ └── models │ │ ├── custom.json │ │ └── mh_cmu.json ├── run_retargeting.bat └── run_retargeting.py ├── requirements.txt └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.pickle 3 | *.spyderproject 4 | *.spyderworkspace 5 | *.idea 6 | *.html 7 | *.index 8 | *.meta 9 | *.data-00000-of-00001 10 | *.gz 11 | *.bin 12 | .vscode/* 13 | tornado_test* 14 | texdoc* 15 | checkpoint -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The code is licensed under the terms of the MIT license, reproduced below. 2 | 3 | = = = = = 4 | MIT license 5 | 6 | Copyright (c) 2019 DFKI GmbH 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy 9 | of this software and associated documentation files (the "Software"), to deal 10 | in the Software without restriction, including without limitation the rights 11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | copies of the Software, and to permit persons to whom the Software is 13 | furnished to do so, subject to the following conditions: 14 | 15 | The above copyright notice and this permission notice shall be included in all 16 | copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | SOFTWARE. 25 | 26 | = = = = = 27 | 28 | Contributions by Daimler AG in the following files are also licensed under terms of the MIT license: 29 | anim_utils/animation_data/bvh.py 30 | anim_utils/animation_data/utils.py 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Skeleton Animation Utilities 2 | 3 | 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/. 4 | 5 | ## Installation 6 | 7 | Clone the repository and install the package with editable flag or use the follwing command: 8 | ```bat 9 | pip install git+https://github.com/eherr/anim_utils 10 | ``` 11 | The optional FBX IO requires the [Python FBX SDK](https://www.autodesk.com/developer-network/platform-technologies/fbx-sdk-2020-3) 12 | 13 | ## Example 14 | 15 | ```python 16 | from anim_utils.animation_data import BVHReader, MotionVector, SkeletonBuilder 17 | 18 | bvh = BVHReader("example.bvh") 19 | mv = MotionVector() 20 | mv.from_bvh_reader(bvh) 21 | skeleton = SkeletonBuilder().load_from_bvh(bvh) 22 | point_clouds = [] 23 | for frame in mv.frames: 24 | point_cloud = [] 25 | for j in skeleton.animated_joints: 26 | p = skeleton.nodes[j].get_global_position(frame) 27 | point_cloud.append(p) 28 | point_clouds.append(point_cloud) 29 | 30 | ``` 31 | 32 | A retargeting script can be found in the example directory. 33 | 34 | ## Developers 35 | 36 | Erik Herrmann1, Han Du1, Martin Manns2, Markus Mauer2 37 | 38 | 1DFKI GmbH 39 | 2Daimler AG 40 | 41 | 42 | ## License 43 | Copyright (c) 2019 DFKI GmbH. 44 | MIT License, see the LICENSE file. 45 | 46 | Contributions by Daimler AG in the following files are also licensed under terms of the MIT license: 47 | anim_utils/animation_data/bvh.py 48 | anim_utils/animation_data/utils.py 49 | 50 | Each file contains a copyright notice. 51 | -------------------------------------------------------------------------------- /anim_utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eherr/anim_utils/0788e8a2256c873567e39c903843070e249ba94c/anim_utils/__init__.py -------------------------------------------------------------------------------- /anim_utils/animation_data/__init__.py: -------------------------------------------------------------------------------- 1 | from .motion_vector import MotionVector 2 | from .motion_concatenation import align_quaternion_frames, transform_euler_frames, transform_quaternion_frames 3 | from .bvh import BVHReader, BVHWriter 4 | from .acclaim import parse_asf_file, parse_amc_file 5 | from .skeleton import Skeleton 6 | from .skeleton_builder import SkeletonBuilder 7 | from .skeleton_node import SKELETON_NODE_TYPE_ROOT, SKELETON_NODE_TYPE_JOINT, SKELETON_NODE_TYPE_END_SITE, SkeletonRootNode, SkeletonJointNode, SkeletonEndSiteNode 8 | from .skeleton_models import SKELETON_MODELS 9 | from .constants import * 10 | -------------------------------------------------------------------------------- /anim_utils/animation_data/acclaim.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | """ parser of asf and amc formats 24 | https://research.cs.wisc.edu/graphics/Courses/cs-838-1999/Jeff/ASF-AMC.html 25 | """ 26 | import sys 27 | import math 28 | import numpy as np 29 | from transformations import euler_matrix 30 | 31 | DEFAULT_FRAME_TIME = 1.0/120 32 | 33 | def asf_to_bvh_channels(dof): 34 | channels = [] 35 | for d in dof: 36 | type_str = "" 37 | if d[0].lower() == "r": 38 | type_str = "rotation" 39 | elif d[0].lower() == "t": 40 | type_str = "translation" 41 | axis_str = d[-1].upper() 42 | channels.append(axis_str + type_str) 43 | return channels 44 | 45 | def rotate_around_x(alpha): 46 | #Note vectors represent columns 47 | cx = math.cos(alpha) 48 | sx = math.sin(alpha) 49 | m = np.array([[1.0, 0.0, 0.0, 0.0], 50 | [0.0, cx , sx,0.0], 51 | [0.0, -sx, cx,0.0], 52 | [0.0,0.0,0.0,1.0]],np.float32) 53 | return m.T 54 | 55 | 56 | def rotate_around_y(beta): 57 | #Note vectors represent columns 58 | cy = math.cos(beta) 59 | sy = math.sin(beta) 60 | m = np.array([[cy,0.0,-sy ,0.0], 61 | [0.0,1.0,0.0,0.0], 62 | [sy, 0.0, cy,0.0], 63 | [0.0,0.0,0.0,1.0]],np.float32) 64 | return m.T 65 | 66 | 67 | def rotate_around_z(gamma): 68 | #Note vectors represent columns 69 | cz = math.cos(gamma) 70 | sz = math.sin(gamma) 71 | m = np.array([[cz, sz,0.0,0.0], 72 | [-sz, cz,0.0,0.0], 73 | [0.0,0.0,1.0,0.0], 74 | [0.0,0.0,0.0,1.0]],np.float32) 75 | return m.T 76 | 77 | 78 | AXES = "rxyz" 79 | def create_euler_matrix(angles, order): 80 | m = np.eye(4, dtype=np.float32) 81 | for idx, d in enumerate(order): 82 | a = np.radians(angles[idx]) 83 | d = d[0].lower() 84 | local_rot = np.eye(4) 85 | if d =="x": 86 | local_rot = euler_matrix(a,0,0, AXES) 87 | elif d =="y": 88 | local_rot = euler_matrix(0,a,0, AXES) 89 | elif d =="z": 90 | local_rot = euler_matrix(0,0,a, AXES) 91 | m = np.dot(local_rot, m) 92 | return m 93 | 94 | def create_euler_matrix2(angles, order): 95 | m = np.eye(4, dtype=np.float32) 96 | for idx, d in enumerate(order): 97 | a = np.radians(angles[idx]) 98 | d = d[0].lower() 99 | local_rot = np.eye(4) 100 | if d =="x": 101 | local_rot = rotate_around_x(a) 102 | elif d =="y": 103 | local_rot = rotate_around_y(a) 104 | elif d =="z": 105 | local_rot = rotate_around_z(a) 106 | m = np.dot(local_rot, m) 107 | return m 108 | 109 | def create_coordinate_transform_matrices(data): 110 | """ FROM https://research.cs.wisc.edu/graphics/Courses/cs-838-1999/Jeff/ASF-AMC.html 111 | Precalculate some the transformation for each segment from the axis 112 | """ 113 | angles = data["root"]["orientation"] 114 | order = data["root"]["axis"] 115 | order = asf_to_bvh_channels(order) 116 | c = create_euler_matrix(angles, order) 117 | data["root"]["coordinate_transform"] = c 118 | data["root"]["inv_coordinate_transform"] = np.linalg.inv(c) 119 | 120 | for key in data["bones"]: 121 | if "axis" in data["bones"][key]: 122 | angles, order = data["bones"][key]["axis"] 123 | order = asf_to_bvh_channels(order) 124 | c = create_euler_matrix(angles, order) 125 | data["bones"][key]["coordinate_transform"] = c 126 | data["bones"][key]["inv_coordinate_transform"] = np.linalg.inv(c) 127 | return data 128 | 129 | def set_parents(data): 130 | for parent in data["children"]: 131 | for c in data["children"][parent]: 132 | data["bones"][c]["parent"] = parent 133 | return data 134 | 135 | def convert_bones_to_joints(data): 136 | """ ASF stores bones that need to be converted into joints the internal skeleton class 137 | based on the BVH format 138 | additionally helper bones for the root need to be removed 139 | """ 140 | data = set_parents(data) 141 | for b in data["bones"]: 142 | if b == "root": 143 | continue 144 | # add offset to parent 145 | parent = data["bones"][b]["parent"] 146 | if parent in data["bones"] and "direction" in data["bones"][parent]: 147 | offset = np.array(data["bones"][parent]["direction"]) 148 | offset *= float(data["bones"][parent]["length"]) 149 | data["bones"][b]["offset"] = offset 150 | 151 | # remove helper bones for hips without dofs 152 | new_root_children = [] 153 | old_root_children = [] 154 | for c in data["children"]["root"]: 155 | if "dof" in data["bones"][c] and len(data["bones"][c]["dof"])>0: 156 | new_root_children.append(c) 157 | else: # replace bones without dofs with their children 158 | old_root_children.append(c) 159 | for cc in data["children"][c]: 160 | data["bones"][cc]["parent"] = "root" 161 | new_root_children.append(cc) 162 | data["children"]["root"] = new_root_children 163 | for c in old_root_children: 164 | del data["bones"][c] 165 | return data 166 | 167 | def parse_asf_file(filepath): 168 | with open(filepath, "rt") as in_file: 169 | lines = in_file.readlines() 170 | data = dict() 171 | data["bones"] = dict() 172 | idx = 0 173 | while idx < len(lines): 174 | next_line = lines[idx].lstrip() 175 | if next_line.startswith(":root"): 176 | idx += 1 177 | data["root"], idx = read_root_data(lines, idx) 178 | idx -=1 179 | if next_line.startswith(":name"): 180 | data["name"] = next_line.split(" ")[1] 181 | idx+=1 182 | elif next_line.startswith(":bonedata"): 183 | idx+=1 184 | next_line = lines[idx].lstrip() 185 | while not next_line.startswith(":hierarchy") and idx+1 < len(lines): 186 | node, idx = read_bone_data(lines, idx) 187 | if "name" in node: 188 | name = node["name"] 189 | data["bones"][name] = node 190 | if idx < len(lines): 191 | next_line = lines[idx].lstrip() 192 | elif next_line.startswith(":hierarchy"): 193 | data["children"], idx = read_hierarchy(lines, idx) 194 | else: 195 | idx+=1 196 | data = create_coordinate_transform_matrices(data) 197 | data = convert_bones_to_joints(data) 198 | return data 199 | 200 | def read_root_data(lines, idx): 201 | data = dict() 202 | #print("start root", idx) 203 | next_line = lines[idx].strip() 204 | while not next_line.startswith(":bonedata") and idx+1 < len(lines): 205 | values = next_line.split(" ") 206 | if len(values) > 0: 207 | key = values[0] 208 | if key == "position": 209 | data["position"] = [values[1], values[2], values[3]] 210 | elif key == "orientation": 211 | data["orientation"] = [float(values[1]),float(values[2]), float(values[3])] 212 | elif key == "axis": 213 | data["axis"] = values[1] 214 | elif key == "order": 215 | data["order"] = [v for v in values if v != "order"] 216 | #elif key == "limits": 217 | # data[key] = [values[1], values[2], values[3]] 218 | if idx+1 < len(lines): 219 | idx+=1 220 | next_line = lines[idx].strip() # remove empty lines 221 | 222 | #print("end root", idx, next_line) 223 | return data, idx 224 | 225 | def read_bone_data(lines, idx): 226 | idx +=1 #skip begin 227 | data = dict() 228 | #print("start bone", idx) 229 | next_line = lines[idx].strip() 230 | while not next_line.startswith("end") and idx+1 < len(lines): 231 | values = next_line.split(" ") 232 | values = [v for v in values if v != ""] 233 | if len(values) > 0: 234 | key = values[0] 235 | if key == "id": 236 | data["id"] = values[1] 237 | elif key == "name": 238 | data["name"] = values[1] 239 | elif key == "direction": 240 | direction = np.array([float(v) for v in values if v != "direction"]) 241 | direction /= np.linalg.norm(direction) 242 | data["direction"] = direction.tolist() 243 | elif key == "length": 244 | data["length"] = float(values[1]) 245 | elif key == "axis": 246 | data["axis"] = [float(values[1]), float(values[2]), float(values[3])], values[4] 247 | elif key == "dof": 248 | data["dof"] = [v for v in values if v != "dof"] 249 | #elif key == "limits": 250 | # data[key] = [values[1], values[2], values[3]] 251 | if idx+1 < len(lines): 252 | idx+=1 253 | next_line = lines[idx].strip() # remove empty lines 254 | idx +=1 #skip end 255 | #print("end", idx, lines[idx]) 256 | return data, idx 257 | 258 | 259 | def read_hierarchy(lines, idx): 260 | print("found hierarchy") 261 | idx +=1 #skip begin 262 | child_dict = dict() 263 | next_line = lines[idx].strip() 264 | while not next_line.startswith("end"): 265 | values = next_line.split(" ") 266 | if len(values) > 1: 267 | child_dict[values[0]] = values[1:] 268 | idx+=1 269 | next_line = lines[idx].strip() # remove empty lines 270 | idx +=1 #skip end 271 | return child_dict, idx 272 | 273 | def parse_amc_file(filepath): 274 | with open(filepath, "rt") as in_file: 275 | lines = in_file.readlines() 276 | frames = [] 277 | current_frame = None 278 | idx = 0 279 | while idx < len(lines): 280 | next_line = lines[idx].strip() 281 | values = next_line.split(" ") 282 | if len(values) == 1: 283 | if values[0].isdigit(): 284 | if current_frame is not None: 285 | frames.append(current_frame) 286 | current_frame = dict() 287 | elif len(values) > 1 and current_frame is not None: 288 | key = values[0] 289 | current_frame[key] = [float(v) for v in values if v != key] 290 | idx+=1 291 | return frames 292 | -------------------------------------------------------------------------------- /anim_utils/animation_data/constants.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | DEFAULT_ROTATION_ORDER = ['Xrotation','Yrotation','Zrotation'] 24 | DEFAULT_SMOOTHING_WINDOW_SIZE = 20 25 | LEN_QUAT = 4 26 | LEN_ROOT_POS = 3 27 | LEN_EULER = 3 28 | ROTATION_TYPE_EULER = 0 29 | ROTATION_TYPE_QUATERNION = 1 30 | LEN_ROOT = 3 -------------------------------------------------------------------------------- /anim_utils/animation_data/fbx/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | from anim_utils.animation_data.motion_vector import MotionVector 24 | from anim_utils.animation_data.skeleton_builder import SkeletonBuilder 25 | 26 | has_fbx = True 27 | try: 28 | import FbxCommon 29 | except ImportError as e: 30 | has_fbx = False 31 | print("Info: Disable FBX IO due to missing library") 32 | pass 33 | 34 | 35 | if has_fbx: 36 | from .fbx_import import load_fbx_file 37 | from .fbx_export import export_motion_vector_to_fbx_file 38 | else: 39 | def load_fbx_file(file_path): 40 | raise NotImplementedError 41 | 42 | def export_motion_vector_to_fbx_file(skeleton, motion_vector, out_file_name): 43 | raise NotImplementedError 44 | 45 | 46 | 47 | def load_skeleton_and_animations_from_fbx(file_path): 48 | skeleton_def, animations = load_fbx_file(file_path) 49 | skeleton = SkeletonBuilder().load_from_fbx_data(skeleton_def) 50 | anim_names = list(animations.keys()) 51 | motion_vectors = dict() 52 | if len(anim_names) > 0: 53 | anim_name = anim_names[0] 54 | mv = MotionVector() 55 | mv.from_fbx(animations[anim_name], skeleton.animated_joints) 56 | motion_vectors[anim_name] = mv 57 | return skeleton, motion_vectors 58 | 59 | -------------------------------------------------------------------------------- /anim_utils/animation_data/fbx/fbx_export.py: -------------------------------------------------------------------------------- 1 | """ 2 | Code to export an animated skeleton to an FBX file based on a skeleton and a motion vector. 3 | The code is based on Example01 of the FBX SDK samples. 4 | """ 5 | 6 | from transformations import euler_from_quaternion 7 | import numpy as np 8 | import FbxCommon 9 | from fbx import * 10 | 11 | 12 | def create_scene(sdk_manager, scene, skeleton, frames, frame_time): 13 | info = FbxDocumentInfo.Create(sdk_manager, "SceneInfo") 14 | info.mTitle = "MotionExportScene" 15 | scene.SetSceneInfo(info) 16 | root_node = create_skeleton(sdk_manager, "", skeleton) 17 | scene.GetRootNode().LclTranslation.Set(FbxDouble3(0, 0, 0)) 18 | scene.GetRootNode().AddChild(root_node) 19 | set_rest_pose(sdk_manager, scene, root_node, skeleton) 20 | set_animation_curves(scene, root_node, skeleton, frames, frame_time) 21 | return root_node 22 | 23 | 24 | def create_skeleton(sdk_manager, name, skeleton): 25 | root_node = create_root_node(sdk_manager, name, skeleton) 26 | return root_node 27 | 28 | def create_root_node(sdk_manager, name, skeleton): 29 | skeleton_root = create_skeleton_nodes_recursively(sdk_manager, name, skeleton, skeleton.root) 30 | node_type = FbxSkeleton.eRoot 31 | node_attribute = FbxSkeleton.Create(sdk_manager, name) 32 | node_attribute.SetSkeletonType(node_type) 33 | skeleton_root.SetNodeAttribute(node_attribute) 34 | return skeleton_root 35 | 36 | def create_root_node2(sdk_manager, name, skeleton): 37 | node_type = FbxSkeleton.eRoot 38 | node_attribute = FbxSkeleton.Create(sdk_manager, name) 39 | node_attribute.SetSkeletonType(node_type) 40 | extra_root_node = FbxNode.Create(sdk_manager, "Root") 41 | extra_root_node.SetNodeAttribute(node_attribute) 42 | skeleton_root = create_skeleton_nodes_recursively(sdk_manager, name, skeleton, skeleton.root) 43 | extra_root_node.AddChild(skeleton_root) 44 | return extra_root_node 45 | 46 | def create_skeleton_nodes_recursively(sdk_manager, skeleton_name, skeleton, node_name): 47 | node = skeleton.nodes[node_name] 48 | name = skeleton_name + node_name 49 | skeleton_node_attribute = FbxSkeleton.Create(sdk_manager, skeleton_name) 50 | node_type = FbxSkeleton.eLimbNode 51 | skeleton_node_attribute.SetSkeletonType(node_type) 52 | skeleton_node = FbxNode.Create(sdk_manager, name) 53 | skeleton_node.SetNodeAttribute(skeleton_node_attribute) 54 | t = FbxDouble3(node.offset[0], node.offset[1], node.offset[2]) 55 | skeleton_node.LclTranslation.Set(t) 56 | for c_node in node.children: 57 | c_name = c_node.node_name 58 | c_node = create_skeleton_nodes_recursively(sdk_manager, skeleton_name, skeleton, c_name) 59 | skeleton_node.AddChild(c_node) 60 | return skeleton_node 61 | 62 | 63 | def set_rest_pose_recursively(pose, fbx_node, skeleton): 64 | name = fbx_node.GetName() 65 | if name in skeleton.nodes.keys(): 66 | node = skeleton.nodes[name] 67 | t = node.offset 68 | 69 | l_t = FbxVector4(t[0], t[1], t[2]) 70 | l_t = FbxVector4(0,0,0) 71 | l_r = FbxVector4(0.0, 0.0, 0.0) 72 | l_s = FbxVector4(1.0, 1.0, 1.0) 73 | 74 | transform = FbxMatrix() 75 | transform.SetTRS(l_t, l_r, l_s) 76 | pose.Add(fbx_node, transform, True)# maybe set this to false 77 | n_children = fbx_node.GetChildCount() 78 | for idx in range(n_children): 79 | c_node = fbx_node.GetChild(idx) 80 | set_rest_pose_recursively(pose, c_node, skeleton) 81 | 82 | 83 | def set_rest_pose(sdk_manager, scene, root_node, skeleton): 84 | pose = FbxPose.Create(sdk_manager, "RestPose") 85 | set_rest_pose_recursively(pose, root_node, skeleton) 86 | scene.AddPose(pose) 87 | 88 | 89 | def create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, dimension, dim_idx): 90 | t = FbxTime() 91 | curve = fbx_node.LclTranslation.GetCurve(anim_layer, dimension, True) 92 | curve.KeyModifyBegin() 93 | for idx, frame in enumerate(euler_frames): 94 | t.SetSecondDouble(idx * frame_time) 95 | key_index = curve.KeyAdd(t)[0] 96 | curve.KeySetValue(key_index, frame[dim_idx]) 97 | curve.KeySetInterpolation(key_index, FbxAnimCurveDef.eInterpolationConstant) 98 | curve.KeyModifyEnd() 99 | 100 | 101 | def create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, dimension, dim_idx): 102 | t = FbxTime() 103 | curve = fbx_node.LclRotation.GetCurve(anim_layer, dimension, True) 104 | curve.KeyModifyBegin() 105 | for idx, frame in enumerate(euler_frames): 106 | t.SetSecondDouble(idx * frame_time) 107 | key_index = curve.KeyAdd(t)[0] 108 | curve.KeySetValue(key_index, frame[dim_idx]) 109 | curve.KeySetInterpolation(key_index, FbxAnimCurveDef.eInterpolationConstant) 110 | curve.KeyModifyEnd() 111 | 112 | 113 | def create_translation_curves(fbx_node, anim_layer, euler_frames, frame_time): 114 | create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, "X", 0) 115 | create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, "Y", 1) 116 | create_translation_curve(fbx_node, anim_layer, euler_frames, frame_time, "Z", 2) 117 | 118 | 119 | def create_rotation_curves(fbx_node, anim_layer, skeleton, euler_frames, frame_time): 120 | node_name = fbx_node.GetName() 121 | if node_name not in skeleton.animated_joints: 122 | return 123 | node_idx = skeleton.animated_joints.index(node_name) 124 | offset = node_idx * 3 + 3 125 | create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, "X", offset) 126 | create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, "Y", offset+1) 127 | create_euler_curve(fbx_node, anim_layer, euler_frames, frame_time, "Z", offset+2) 128 | 129 | 130 | def add_rotation_curves_recursively(fbx_node, anim_layer, skeleton, euler_frames, frame_time): 131 | create_rotation_curves(fbx_node, anim_layer, skeleton, euler_frames, frame_time) 132 | n_children = fbx_node.GetChildCount() 133 | for idx in range(n_children): 134 | c_node = fbx_node.GetChild(idx) 135 | add_rotation_curves_recursively(c_node, anim_layer, skeleton, euler_frames, frame_time) 136 | 137 | 138 | def convert_quaternion_to_euler_frame(skeleton, frame): 139 | n_dims = len(skeleton.animated_joints) * 3 + 3 140 | euler_frame = np.zeros(n_dims) 141 | euler_frame[:3] = frame[:3] 142 | target_offset = 3 143 | src_offset = 3 144 | for idx, node in enumerate(skeleton.animated_joints): 145 | q = frame[src_offset:src_offset + 4] 146 | e = euler_from_quaternion(q) 147 | euler_frame[target_offset:target_offset + 3] = np.degrees(e) 148 | target_offset += 3 149 | src_offset += 4 150 | return euler_frame 151 | 152 | def set_animation_curves(scene, root_node, skeleton, frames, frame_time): 153 | 154 | # convert frames from quaternion to euler 155 | euler_frames = [] 156 | for frame in frames: 157 | euler_frame = convert_quaternion_to_euler_frame(skeleton, frame) 158 | euler_frames.append(euler_frame) 159 | 160 | anim_stack_name = "default" 161 | anim_stack = FbxAnimStack.Create(scene, anim_stack_name) 162 | anim_layer = FbxAnimLayer.Create(scene, "Base Layer") 163 | anim_stack.AddMember(anim_layer) 164 | 165 | create_translation_curves(root_node, anim_layer, euler_frames, frame_time) 166 | add_rotation_curves_recursively(root_node, anim_layer, skeleton, euler_frames, frame_time) 167 | 168 | 169 | def export_motion_vector_to_fbx_file(skeleton, motion_vector, out_file_name, fbx_axis_system=FbxAxisSystem.MayaZUp): 170 | """ Save a scene with a single skeleton and animation curves for the root and joints 171 | """ 172 | sdk_manager, scene = FbxCommon.InitializeSdkObjects() 173 | root_node = create_scene(sdk_manager, scene, skeleton, motion_vector.frames, motion_vector.frame_time) 174 | fbx_axis_system.MayaZUp.ConvertScene(scene) 175 | #sdk_manager.GetIOSettings().SetBoolProp(EXP_FBX_EMBEDDED, True) 176 | save_scene(sdk_manager, scene, out_file_name, pEmbedMedia=False) 177 | print("finished") 178 | 179 | sdk_manager.Destroy() 180 | 181 | 182 | def save_scene(pSdkManager, pScene, pFilename, pFileFormat = -1, pEmbedMedia = False): 183 | """ copied from fbx common 184 | changed settings to a Blender compatible version according to 185 | http://stackoverflow.com/questions/29833986/fbx-sdk-exporting-into-older-fbx-file-format 186 | """ 187 | 188 | lExporter = FbxExporter.Create(pSdkManager, "") 189 | lExporter.SetFileExportVersion("FBX201200", FbxSceneRenamer.eFBX_TO_FBX) 190 | 191 | if pFileFormat < 0 or pFileFormat >= pSdkManager.GetIOPluginRegistry().GetWriterFormatCount(): 192 | pFileFormat = pSdkManager.GetIOPluginRegistry().GetNativeWriterFormat() 193 | if not pEmbedMedia: 194 | lFormatCount = pSdkManager.GetIOPluginRegistry().GetWriterFormatCount() 195 | for lFormatIndex in range(lFormatCount): 196 | if pSdkManager.GetIOPluginRegistry().WriterIsFBX(lFormatIndex): 197 | lDesc = pSdkManager.GetIOPluginRegistry().GetWriterFormatDescription(lFormatIndex) 198 | if "binary" in lDesc: 199 | pFileFormat = lFormatIndex 200 | break 201 | 202 | if not pSdkManager.GetIOSettings(): 203 | ios = FbxIOSettings.Create(pSdkManager, IOSROOT) 204 | pSdkManager.SetIOSettings(ios) 205 | 206 | pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_MATERIAL, True) 207 | pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_TEXTURE, True) 208 | pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_EMBEDDED, pEmbedMedia) 209 | pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_SHAPE, True) 210 | pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_GOBO, True) 211 | pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_ANIMATION, True) 212 | pSdkManager.GetIOSettings().SetBoolProp(EXP_FBX_GLOBAL_SETTINGS, True) 213 | 214 | result = lExporter.Initialize(pFilename, pFileFormat, pSdkManager.GetIOSettings()) 215 | if result == True: 216 | result = lExporter.Export(pScene) 217 | 218 | lExporter.Destroy() 219 | return result -------------------------------------------------------------------------------- /anim_utils/animation_data/fbx/fbx_import.py: -------------------------------------------------------------------------------- 1 | import FbxCommon 2 | from fbx import * 3 | import numpy as np 4 | import collections 5 | from transformations import quaternion_matrix, euler_from_quaternion, quaternion_from_euler 6 | from anim_utils.animation_data.skeleton_node import SKELETON_NODE_TYPE_ROOT,SKELETON_NODE_TYPE_JOINT, SKELETON_NODE_TYPE_END_SITE 7 | 8 | def load_fbx_file(file_path): 9 | importer = FBXImporter(file_path) 10 | importer.load() 11 | importer.destroy() 12 | return importer.skeleton, importer.animations 13 | 14 | 15 | class FBXImporter(object): 16 | def __init__(self, file_path): 17 | self.skeleton = None 18 | self.skeleton_root_node = None 19 | self.skinning_data = dict() 20 | self.animations = dict() 21 | self.mesh_list = [] 22 | 23 | (self.sdk_manager, self.fbx_scene) = FbxCommon.InitializeSdkObjects() 24 | FbxCommon.LoadScene(self.sdk_manager, self.fbx_scene, file_path) 25 | FbxAxisSystem.OpenGL.ConvertScene(self.fbx_scene) 26 | 27 | def destroy(self): 28 | self.sdk_manager.Destroy() 29 | 30 | def load(self): 31 | self.skeleton = None 32 | self.skinning_data = dict() 33 | self.animations = dict() 34 | self.mesh_list = [] 35 | root_node = self.fbx_scene.GetRootNode() 36 | self.parseFBXNodeHierarchy(root_node, 0) 37 | if self.skeleton_root_node is not None: 38 | self.animations = self.read_animations(self.skeleton_root_node) 39 | 40 | 41 | def parseFBXNodeHierarchy(self, fbx_node, depth): 42 | 43 | n_attributes = fbx_node.GetNodeAttributeCount() 44 | for idx in range(n_attributes): 45 | attribute = fbx_node.GetNodeAttributeByIndex(idx) 46 | if self.skeleton is None and attribute.GetAttributeType() == FbxNodeAttribute.eSkeleton: 47 | self.skeleton_root_node = fbx_node 48 | self.skeleton = self.create_skeleton(fbx_node) 49 | 50 | for idx in range(fbx_node.GetChildCount()): 51 | self.parseFBXNodeHierarchy(fbx_node.GetChild(idx), depth + 1) 52 | 53 | 54 | def create_skeleton(self, node): 55 | current_time = FbxTime() 56 | current_time.SetFrame(0, FbxTime.eFrames24) 57 | scale = node.EvaluateGlobalTransform().GetS()[0] 58 | def add_child_node_recursively(skeleton, fbx_node): 59 | node_name = fbx_node.GetName() 60 | node_idx = len(skeleton["animated_joints"]) 61 | localTransform = fbx_node.EvaluateLocalTransform(current_time) 62 | #lT = localTransform.GetT() 63 | o = fbx_node.LclTranslation.Get() 64 | offset = scale*np.array([o[0], o[1], o[2]]) 65 | q = localTransform.GetQ() 66 | rotation = np.array([q[3],q[0], q[1], q[2]]) 67 | 68 | node = {"name": node_name, 69 | "children": [], 70 | "channels": [], 71 | "offset": offset, 72 | "rotation": rotation, 73 | "fixed": True, 74 | "index": -1, 75 | "quaternion_frame_index": -1, 76 | "node_type": SKELETON_NODE_TYPE_JOINT} 77 | n_children = fbx_node.GetChildCount() 78 | if n_children > 0: 79 | node["channels"] = ["Xrotation", "Yrotation", "Zrotation"] 80 | node["index"] = node_idx 81 | node["quaternion_frame_index"] = node_idx 82 | node["fixed"] = False 83 | skeleton["animated_joints"].append(node_name) 84 | for idx in range(n_children): 85 | c_node = add_child_node_recursively(skeleton, fbx_node.GetChild(idx)) 86 | node["children"].append(c_node["name"]) 87 | skeleton["nodes"][node_name] = node 88 | return node 89 | 90 | o = node.LclTranslation.Get() 91 | offset = scale*np.array([o[0], o[1], o[2]]) 92 | e = node.LclRotation.Get() 93 | rotation = quaternion_from_euler(*e, axes='sxyz') 94 | root_name = node.GetName() 95 | skeleton = dict() 96 | skeleton["animated_joints"] = [root_name] 97 | skeleton["node_names"] = dict() 98 | skeleton["nodes"] = collections.OrderedDict() 99 | skeleton["frame_time"] = 0.013889 100 | root_node = {"name": root_name, 101 | "children": [], 102 | "channels": ["Xposition", "Yposition", "Zposition", 103 | "Xrotation", "Yrotation", "Zrotation"], 104 | "offset": offset, 105 | "rotation": rotation, 106 | "fixed": False, 107 | "index": 0, 108 | "quaternion_frame_index": 0, 109 | "inv_bind_pose": np.eye(4), 110 | "node_type": SKELETON_NODE_TYPE_ROOT} 111 | 112 | skeleton["nodes"][root_name] = root_node 113 | for idx in range(node.GetChildCount()): 114 | c_node = add_child_node_recursively(skeleton, node.GetChild(idx)) 115 | root_node["children"].append(c_node["name"]) 116 | 117 | skeleton["root"] = root_name 118 | print('animated_joints', len(skeleton["animated_joints"])) 119 | return skeleton 120 | 121 | 122 | def read_animations(self, temp_node): 123 | """src: http://gamedev.stackexchange.com/questions/59419/c-fbx-animation-importer-using-the-fbx-sdk 124 | """ 125 | anims = dict() 126 | count = self.fbx_scene.GetSrcObjectCount(FbxCriteria.ObjectType(FbxAnimStack.ClassId)) 127 | for idx in range(count): 128 | anim = dict() 129 | anim_stack = self.fbx_scene.GetSrcObject(FbxCriteria.ObjectType(FbxAnimStack.ClassId), idx) 130 | self.fbx_scene.SetCurrentAnimationStack(anim_stack) 131 | anim_name = anim_stack.GetName() 132 | anim_layer = anim_stack.GetMember(FbxCriteria.ObjectType(FbxAnimLayer.ClassId), 0) 133 | mLocalTimeSpan = anim_stack.GetLocalTimeSpan() 134 | start = mLocalTimeSpan.GetStart() 135 | end = mLocalTimeSpan.GetStop() 136 | anim["n_frames"] = end.GetFrameCount(FbxTime.eFrames24) - start.GetFrameCount(FbxTime.eFrames24) + 1 137 | anim["duration"] = end.GetSecondCount() - start.GetSecondCount() 138 | anim["frame_time"] = anim["duration"]/anim["n_frames"] # 0.013889 139 | anim["curves"] = collections.OrderedDict() 140 | print("found animation", anim_name, anim["n_frames"], anim["duration"]) 141 | is_root = True 142 | nodes = [] 143 | while temp_node is not None: 144 | name = temp_node.GetName() 145 | if has_curve(anim_layer, temp_node): 146 | if is_root: 147 | anim["curves"][name] = get_global_anim_curve(temp_node, start, end) 148 | is_root = False 149 | else: 150 | anim["curves"][name] = get_local_anim_curve(temp_node, start, end) 151 | for i in range(temp_node.GetChildCount()): 152 | nodes.append(temp_node.GetChild(i)) 153 | if len(nodes) > 0: 154 | temp_node = nodes.pop(0) 155 | else: 156 | temp_node = None 157 | anims[anim_name] = anim 158 | return anims 159 | 160 | def get_local_anim_curve(node, start, end): 161 | curve= [] 162 | current_t = FbxTime() 163 | for frame_idx in range(start.GetFrameCount(FbxTime.eFrames24), end.GetFrameCount(FbxTime.eFrames24)): 164 | current_t.SetFrame(frame_idx, FbxTime.eFrames24) 165 | local_transform = node.EvaluateLocalTransform(current_t) 166 | q = local_transform.GetQ() 167 | t = local_transform.GetT() 168 | transform = {"rotation": [q[3], q[0], q[1], q[2]], 169 | "translation": [t[0], t[1], t[2]]} 170 | curve.append(transform) 171 | return curve 172 | 173 | def get_global_anim_curve(node, start, end): 174 | curve= [] 175 | current_t = FbxTime() 176 | for frame_idx in range(start.GetFrameCount(FbxTime.eFrames24), end.GetFrameCount(FbxTime.eFrames24)): 177 | current_t.SetFrame(frame_idx, FbxTime.eFrames24) 178 | local_transform = node.EvaluateGlobalTransform(current_t) 179 | q = local_transform.GetQ() 180 | t = local_transform.GetT() 181 | transform = {"rotation": [q[3], q[0], q[1], q[2]], 182 | "translation": [t[0], t[1], t[2]]} 183 | curve.append(transform) 184 | return curve 185 | 186 | def has_curve(anim_layer, node): 187 | translation = node.LclTranslation.GetCurve(anim_layer, "X") 188 | rotation = node.LclRotation.GetCurve(anim_layer, "X") 189 | return rotation is not None or translation is not None 190 | 191 | 192 | def FBXMatrixToNumpy(m): 193 | q = m.GetQ() 194 | t = m.GetT() 195 | m = quaternion_matrix([q[0], q[1], q[2], q[3]]) 196 | m[:3,3] = t[0], t[1],t[2] 197 | return m 198 | 199 | -------------------------------------------------------------------------------- /anim_utils/animation_data/motion_distance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | from math import sqrt 25 | from transformations import euler_matrix 26 | from .motion_concatenation import _align_point_clouds_2D 27 | 28 | 29 | def _point_cloud_distance(a, b): 30 | """ Calculates the distance between two point clouds with equal length and 31 | corresponding indices 32 | """ 33 | assert len(a) == len(b) 34 | distance = 0 35 | n_points = len(a) 36 | for i in range(n_points): 37 | d = [a[i][0] - b[i][0], a[i][1] - b[i][1], a[i][2] - b[i][2]] 38 | distance += sqrt(d[0] ** 2 + d[1] ** 2 + d[2] ** 2) 39 | return distance / n_points 40 | 41 | 42 | def convert_quat_frame_to_point_cloud(skeleton, frame, joints=None): 43 | points = [] 44 | if joints is None: 45 | joints = [k for k, n in list(skeleton.nodes.items()) if len(n.children) > 0] 46 | for j in joints: 47 | p = skeleton.nodes[j].get_global_position(frame) 48 | points.append(p) 49 | return points 50 | 51 | 52 | def _transform_point_cloud(point_cloud, theta, offset_x, offset_z): 53 | """ Transforms points in a point cloud by a rotation around the y axis and a translation 54 | along x and z 55 | """ 56 | transformed_point_cloud = [] 57 | for p in point_cloud: 58 | if p is not None: 59 | m = euler_matrix(0,np.radians(theta), 0) 60 | m[0, 3] = offset_x 61 | m[2, 3] = offset_z 62 | p = p.tolist() 63 | new_point = np.dot(m, p + [1])[:3] 64 | transformed_point_cloud.append(new_point) 65 | return transformed_point_cloud 66 | 67 | 68 | def _transform_invariant_point_cloud_distance(pa,pb, weights=None): 69 | if weights is None: 70 | weights = [1.0] * len(pa) 71 | theta, offset_x, offset_z = _align_point_clouds_2D(pa, pb, weights) 72 | t_b = _transform_point_cloud(pb, theta, offset_x, offset_z) 73 | distance = _point_cloud_distance(pa, t_b) 74 | return distance 75 | 76 | 77 | def frame_distance_measure(skeleton, a, b, weights=None): 78 | """ Converts frames into point clouds and calculates 79 | the distance between the aligned point clouds. 80 | 81 | Parameters 82 | --------- 83 | *skeleton: Skeleton 84 | \t skeleton used for forward kinematics 85 | *a: np.ndarray 86 | \t the parameters of a single frame representing a skeleton pose. 87 | *b: np.ndarray 88 | \t the parameters of a single frame representing a skeleton pose. 89 | *weights: list of floats 90 | \t weights giving the importance of points during the alignment and distance 91 | 92 | Returns 93 | ------- 94 | Distance representing similarity of the poses. 95 | """ 96 | assert len(a) == len(b) 97 | pa = convert_quat_frame_to_point_cloud(skeleton, a) 98 | pb = convert_quat_frame_to_point_cloud(skeleton, b) 99 | return _transform_invariant_point_cloud_distance(pa,pb, weights) 100 | -------------------------------------------------------------------------------- /anim_utils/animation_data/motion_state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import collections 24 | import numpy as np 25 | from transformations import quaternion_inverse, quaternion_conjugate, quaternion_multiply, quaternion_matrix 26 | 27 | def get_random_color(): 28 | random_color = np.random.rand(3, ) 29 | if np.sum(random_color) < 0.5: 30 | random_color += np.array([0, 0, 1]) 31 | return random_color.tolist() 32 | 33 | 34 | class MotionStateInterface(object): 35 | def update(self, dt): 36 | pass 37 | 38 | def get_pose(self, frame_idx=None): 39 | pass 40 | 41 | def reset(self): 42 | pass 43 | 44 | def get_frames(self): 45 | pass 46 | 47 | def get_n_frames(self): 48 | pass 49 | 50 | def get_frame_time(self): 51 | pass 52 | 53 | def set_frame_idx(self, frame_idx): 54 | pass 55 | 56 | def set_color_annotation_from_labels(self, labels, colors): 57 | pass 58 | 59 | def replace_current_frame(self, frame): 60 | pass 61 | 62 | def replace_frames(self, new_frames): 63 | pass 64 | 65 | def set_color_annotation(self, semantic_annotation, color_map): 66 | pass 67 | 68 | def set_time_function(self, time_function): 69 | pass 70 | 71 | def set_color_annotation_legacy(self, annotation, color_map): 72 | pass 73 | 74 | def set_random_color_annotation(self): 75 | pass 76 | 77 | def get_current_frame_idx(self): 78 | pass 79 | 80 | def get_current_annotation(self): 81 | return None 82 | 83 | def get_n_annotations(self): 84 | return 0 85 | 86 | def get_semantic_annotation(self): 87 | return list() 88 | 89 | def set_time(self, t): 90 | return 91 | 92 | 93 | class MotionState(MotionStateInterface): 94 | """ Wrapper of a MotionVector to add functions for keeping track of the current time and frame index as well as semantic information. 95 | """ 96 | def __init__(self, mv): 97 | self.mv = mv 98 | self.time = 0 99 | self.frame_idx = 0 100 | self.play = False 101 | self._frame_annotation = None 102 | self.label_color_map = dict() 103 | self._semantic_annotation = collections.OrderedDict() 104 | self.events = dict() 105 | self.n_annotations = 0 106 | self.hold_frames = list() # sorted list 107 | self.paused = False 108 | self.interpolate = False 109 | self.tick = None 110 | self._time_function = None 111 | 112 | def update(self, dt): 113 | if self.play and not self.paused: 114 | self.time += dt 115 | self.frame_idx = int(self.time / self.mv.frame_time) 116 | if len(self.hold_frames) > 0: 117 | #print("hold frame", self.frame_idx,self.hold_frame, self.mv.n_frames) 118 | if self.frame_idx >= self.hold_frames[0]: 119 | self.paused = True 120 | self.frame_idx = self.hold_frames[0] 121 | self.hold_frames = self.hold_frames[1:] # remove entry 122 | if self.tick is not None: 123 | self.tick.update(dt) 124 | if self.frame_idx >= self.mv.n_frames: 125 | self.reset() 126 | return True 127 | return False 128 | 129 | 130 | def get_pose(self, frame_idx=None): 131 | if self.interpolate: 132 | return self.get_pose_interpolate(frame_idx) 133 | else: 134 | if frame_idx is None: 135 | return self.mv.frames[self.frame_idx] 136 | else: 137 | return self.mv.frames[frame_idx] 138 | 139 | def get_pose_interpolate(self, frame_idx=None): 140 | #FIXME interpolation causes jump in translation at start and end 141 | if frame_idx is None: 142 | start_frame_idx = int(self.time / self.mv.frame_time) 143 | end_frame_idx = start_frame_idx+1 144 | t = (self.time % self.mv.frame_time)/ self.mv.frame_time 145 | if end_frame_idx >= self.mv.n_frames: 146 | end_frame_idx = start_frame_idx 147 | t = 0 148 | #print("get pose", self.time, start_frame_idx, t, end_frame_idx) 149 | return self.mv.interpolate(start_frame_idx, end_frame_idx, t) 150 | #return self.mv.frames[self.frame_idx] 151 | else: 152 | return self.mv.frames[frame_idx] 153 | 154 | def reset(self): 155 | self.time = 0 156 | self.frame_idx = 0 157 | self.paused = False 158 | if self.tick is not None: 159 | self.tick.reset() 160 | 161 | def get_frames(self): 162 | return self.mv.frames 163 | 164 | def get_n_frames(self): 165 | return self.mv.n_frames 166 | 167 | def get_frame_time(self): 168 | return self.mv.frame_time 169 | 170 | def get_current_annotation(self): 171 | return self._frame_annotation[self.frame_idx] 172 | 173 | 174 | def get_current_annotation_label(self): 175 | if self._frame_annotation is not None and 0 <= self.frame_idx < len(self._frame_annotation): 176 | return self._frame_annotation[self.frame_idx]["label"] 177 | else: 178 | return "" 179 | 180 | def set_frame_idx(self, frame_idx): 181 | self.frame_idx = frame_idx 182 | self.time = self.get_frame_time() * self.frame_idx 183 | 184 | def set_color_annotation_from_labels(self, labels, colors=None): 185 | 186 | if colors is None: 187 | def getRGBfromI(RGBint): 188 | """ https://stackoverflow.com/questions/33124347/convert-integers-to-rgb-values-and-back-with-python 189 | """ 190 | blue = RGBint & 255 191 | green = (RGBint >> 8) & 255 192 | red = (RGBint >> 16) & 255 193 | return red, green, blue 194 | n_classes = int(max(labels)+1) 195 | step_size = 255255255 / n_classes 196 | self.label_color_map = dict() 197 | for n in range(n_classes): 198 | l = int(n*step_size) 199 | self.label_color_map[str(n)] = np.array(getRGBfromI(l), dtype=float)/255 200 | else: 201 | n_classes = len(colors) 202 | for n in range(n_classes): 203 | self.label_color_map[str(n)] = colors[n] 204 | 205 | self._semantic_annotation = collections.OrderedDict() 206 | for n in range(n_classes): 207 | self._semantic_annotation[str(n)] = [] 208 | self.n_annotations = len(labels) 209 | self._frame_annotation = [] 210 | for idx, label in enumerate(labels): 211 | label = str(int(label)) 212 | self._frame_annotation.append({"label": label, "color": self.label_color_map[label]}) 213 | self._semantic_annotation[label].append(idx) 214 | 215 | def replace_current_frame(self, frame): 216 | if 0 <= self.frame_idx < self.mv.n_frames: 217 | self.mv.frames[self.frame_idx] = frame 218 | 219 | def replace_frames(self, new_frames): 220 | self.time = 0 221 | self.frame_idx = 0 222 | self.mv.frames = new_frames 223 | self.mv.n_frames = len(new_frames) 224 | 225 | def set_color_annotation(self, semantic_annotation, color_map): 226 | self._semantic_annotation = semantic_annotation 227 | self.label_color_map = color_map 228 | self.n_annotations = self.mv.n_frames 229 | self._frame_annotation = [] 230 | for idx in range(self.n_annotations): 231 | self._frame_annotation.append({"label": "none", "color": [1,1,1]}) 232 | for label in semantic_annotation.keys(): 233 | if label in color_map.keys(): 234 | entry = {"label": label, "color": color_map[label]} 235 | if len(semantic_annotation[label]) > 0 and type(semantic_annotation[label][0]) == list: 236 | for indices in semantic_annotation[label]: 237 | for i in indices: 238 | if i < self.n_annotations: 239 | self._frame_annotation[i] = entry 240 | else: 241 | for idx in semantic_annotation[label]: 242 | if idx < self.n_annotations: 243 | self._frame_annotation[idx] = entry 244 | else: 245 | print("warning", idx, self.n_annotations) 246 | 247 | 248 | def set_time_function(self, time_function): 249 | self._time_function = time_function 250 | 251 | def set_color_annotation_legacy(self, annotation, color_map): 252 | self._semantic_annotation = collections.OrderedDict() 253 | self.label_color_map = color_map 254 | self.n_annotations = len(annotation) 255 | self._frame_annotation = [] 256 | for idx, label in enumerate(annotation): 257 | 258 | self._frame_annotation.append({"label": label, 259 | "color": color_map[label]}) 260 | 261 | if label not in list(self._semantic_annotation.keys()): 262 | self._semantic_annotation[label] = [] 263 | self._semantic_annotation[label].append(idx) 264 | 265 | def set_random_color_annotation(self): 266 | self._frame_annotation = [] 267 | for idx in range(self.mv.n_frames): 268 | self._frame_annotation.append({"color": get_random_color()}) 269 | self.n_annotations = self.mv.n_frames 270 | 271 | def get_current_frame_idx(self): 272 | return self.frame_idx 273 | 274 | def get_n_annotations(self): 275 | return self.n_annotations 276 | 277 | def get_semantic_annotation(self): 278 | return list(self._semantic_annotation.items()) 279 | 280 | def get_label_color_map(self): 281 | return self.label_color_map 282 | 283 | def apply_delta_frame(self, skeleton, delta_frame): 284 | self.mv.apply_delta_frame(skeleton, delta_frame) 285 | 286 | def set_time(self, t): 287 | self.frame_idx = int(t / self.get_frame_time()) 288 | self.frame_idx = min(self.frame_idx, self.mv.n_frames-1) 289 | #self.time = self.frame_idx*self.get_frame_time() 290 | self.time = t 291 | 292 | def set_position(self, position): 293 | delta = position - self.mv.frames[self.frame_idx, :3] 294 | self.mv.frames[:, :3] += delta 295 | 296 | def set_orientation(self, orientation): 297 | print("rotate frames") 298 | current_q = self.mv.frames[self.frame_idx, 3:7] 299 | delta_q = quaternion_multiply(orientation, quaternion_inverse(current_q)) 300 | delta_q /= np.linalg.norm(delta_q) 301 | delta_m = quaternion_matrix(delta_q)[:3, :3] 302 | for idx in range(self.mv.n_frames): 303 | old_t = self.mv.frames[idx, :3] 304 | old_q = self.mv.frames[idx, 3:7] 305 | self.mv.frames[idx, :3] = np.dot(delta_m, old_t)[:3] 306 | self.mv.frames[idx, 3:7] = quaternion_multiply(delta_q, old_q) 307 | #self.mv.frames[i, 3:7] = quaternion_multiply(delta_q, old_q) 308 | 309 | def set_ticker(self, tick): 310 | self.tick = tick 311 | 312 | class MotionSplineState(MotionStateInterface): 313 | def __init__(self, spline, frame_time): 314 | self.spline = spline 315 | self.time = 0.0 316 | self.frame_idx = 0.0 317 | self.frame_time = frame_time 318 | self.events = dict() 319 | 320 | def update(self, dt): 321 | self.time += dt 322 | self.frame_idx = self.time / self.frame_time 323 | if self.frame_idx >= self.spline.get_domain()[-1]: 324 | self.reset() 325 | return True 326 | else: 327 | return False 328 | 329 | def get_pose(self, frame_idx=None): 330 | if frame_idx is None: 331 | return self.spline.evaluate(self.frame_idx) 332 | else: 333 | return self.spline.evaluate(frame_idx) 334 | 335 | def reset(self): 336 | self.time = 0 337 | 338 | def get_frames(self): 339 | return self.spline.get_motion_vector() 340 | 341 | def get_last_frame(self): 342 | return self.spline.evaluate(self.spline.get_domain()[-1]) 343 | 344 | def get_n_frames(self): 345 | return self.spline.get_domain()[-1] 346 | 347 | def get_frame_time(self): 348 | return self.frame_time 349 | 350 | def set_time(self, t): 351 | self.time = t 352 | 353 | -------------------------------------------------------------------------------- /anim_utils/animation_data/quaternion_frame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | """ 24 | Created on Fri Nov 24 14:10:21 2014 25 | 26 | @author: Erik Herrmann, Han Du 27 | 28 | """ 29 | import collections 30 | import numpy as np 31 | from transformations import euler_matrix, quaternion_from_matrix, quaternion_matrix, euler_from_matrix 32 | from .utils import rotation_order_to_string 33 | from .constants import DEFAULT_ROTATION_ORDER, LEN_EULER, LEN_QUAT, LEN_ROOT_POS 34 | 35 | 36 | 37 | 38 | def check_quat(test_quat, ref_quat): 39 | """check locomotion_synthesis_test quat needs to be filpped or not 40 | """ 41 | test_quat = np.asarray(test_quat) 42 | ref_quat = np.asarray(ref_quat) 43 | dot = np.dot(test_quat, ref_quat) 44 | if dot < 0: 45 | test_quat = - test_quat 46 | return test_quat.tolist() 47 | 48 | def euler_to_quaternion(euler_angles, rotation_order=DEFAULT_ROTATION_ORDER,filter_values=True): 49 | """Convert euler angles to quaternion vector [qw, qx, qy, qz] 50 | Parameters 51 | ---------- 52 | * euler_angles: list of floats 53 | \tA list of ordered euler angles in degress 54 | * rotation_order: Iteratable 55 | \t a list that specifies the rotation axis corresponding to the values in euler_angles 56 | * filter_values: Bool 57 | \t enforce a unique rotation representation 58 | 59 | """ 60 | assert len(euler_angles) == 3, ('The length of euler angles should be 3!') 61 | euler_angles = np.deg2rad(euler_angles) 62 | rotmat = euler_matrix(*euler_angles, rotation_order_to_string(rotation_order)) 63 | # convert rotation matrix R into quaternion vector (qw, qx, qy, qz) 64 | quat = quaternion_from_matrix(rotmat) 65 | # filter the quaternion see 66 | # http://physicsforgames.blogspot.de/2010/02/quaternions.html 67 | if filter_values: 68 | dot = np.sum(quat) 69 | if dot < 0: 70 | quat = -quat 71 | return [quat[0], quat[1], quat[2], quat[3]] 72 | 73 | 74 | def quaternion_to_euler(quat, rotation_order=DEFAULT_ROTATION_ORDER): 75 | """ 76 | Parameters 77 | ---------- 78 | * q: list of floats 79 | \tQuaternion vector with form: [qw, qx, qy, qz] 80 | 81 | Return 82 | ------ 83 | * euler_angles: list 84 | \tEuler angles in degree with specified order 85 | """ 86 | quat = np.asarray(quat) 87 | quat = quat / np.linalg.norm(quat) 88 | rotmat_quat = quaternion_matrix(quat) 89 | rotation_order_str = rotation_order_to_string(rotation_order) 90 | euler_angles = np.rad2deg(euler_from_matrix(rotmat_quat, rotation_order_str)) 91 | return euler_angles 92 | 93 | 94 | def convert_euler_to_quaternion_frame(bvh_data, e_frame, filter_values=True, animated_joints=None): 95 | """Convert a BVH frame into an ordered dict of quaternions for each skeleton node 96 | Parameters 97 | ---------- 98 | * bvh_data: BVHData 99 | \t Contains skeleton information 100 | * frame_vector: np.ndarray 101 | \t animation keyframe frame represented by Euler angles 102 | * filter_values: Bool 103 | \t enforce a unique rotation representation 104 | 105 | Returns: 106 | ---------- 107 | * quat_frame: OrderedDict that contains a quaternion for each joint 108 | """ 109 | if animated_joints is None: 110 | animated_joints = list(bvh_data.node_names.keys()) 111 | n_dims = len(animated_joints)*4 112 | quat_frame = np.zeros((n_dims)) 113 | offset = 0 114 | for node_name in animated_joints: 115 | if bvh_data.get_node_channels(node_name) is not None: 116 | angles, order = bvh_data.get_node_angles(node_name, e_frame) 117 | q = euler_to_quaternion(angles, order, filter_values) 118 | quat_frame[offset:offset+4] = q 119 | offset +=4 120 | return quat_frame 121 | 122 | 123 | def convert_euler_frames_to_quaternion_frames(bvh_data, euler_frames, filter_values=True, animated_joints=None): 124 | """ 125 | Parameters 126 | ---------- 127 | * bvh_data: BVHData 128 | \t Contains skeleton information 129 | * euler_frames: np.ndarray 130 | \t list of euler frames 131 | Returns: 132 | ---------- 133 | * quater_frames: a list of quaternion frames 134 | """ 135 | if animated_joints is None: 136 | animated_joints = list(bvh_data.node_names.keys()) 137 | quat_frames = [] 138 | prev_frame = None 139 | for e_frame in euler_frames: 140 | q_frame = convert_euler_to_quaternion_frame(bvh_data, e_frame, filter_values, animated_joints=animated_joints) 141 | o = 0 142 | if prev_frame is not None and filter_values: 143 | for joint_name in animated_joints: 144 | q = check_quat(q_frame[o:o+4], prev_frame[o:o+4]) 145 | q_frame[o:o+4] = q 146 | o+=4 147 | prev_frame = q_frame 148 | q_frame = np.concatenate([e_frame[:3],q_frame]) 149 | quat_frames.append(q_frame) 150 | return quat_frames 151 | 152 | 153 | 154 | def convert_quaternion_frames_to_euler_frames(quat_frames): 155 | """Returns an nparray of Euler frames 156 | 157 | Parameters 158 | ---------- 159 | * quat_frames: List of quaternion frames 160 | \tQuaternion frames that shall be converted to Euler frames 161 | 162 | Returns 163 | ------- 164 | * euler_frames: numpy array 165 | \tEuler frames 166 | """ 167 | n_frames = len(quat_frames) 168 | assert n_frames > 0 169 | n_joints = int((len(quat_frames[0])-LEN_ROOT_POS) /LEN_QUAT) 170 | n_params = n_joints*LEN_EULER + LEN_ROOT_POS 171 | euler_frames = np.zeros((n_frames, n_params)) 172 | for frame_idx, quat_frame in enumerate(quat_frames): 173 | euler_frames[frame_idx,:LEN_ROOT_POS] = quat_frame[:LEN_ROOT_POS] 174 | src = LEN_ROOT_POS 175 | dst = LEN_ROOT_POS 176 | for i in range(n_joints): 177 | q = quat_frame[src:src+LEN_QUAT] 178 | euler_frames[frame_idx, dst:dst+LEN_EULER] = quaternion_to_euler(q) 179 | dst += LEN_EULER 180 | src += LEN_QUAT 181 | return euler_frames 182 | -------------------------------------------------------------------------------- /anim_utils/animation_data/skeleton_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | from transformations import quaternion_matrix, euler_matrix, euler_from_matrix, quaternion_from_matrix, quaternion_about_axis 25 | from .constants import ROTATION_TYPE_EULER, ROTATION_TYPE_QUATERNION 26 | from .utils import rotation_order_to_string 27 | 28 | SKELETON_NODE_TYPE_ROOT = 0 29 | SKELETON_NODE_TYPE_JOINT = 1 30 | SKELETON_NODE_TYPE_END_SITE = 2 31 | ORIGIN = point = np.array([0, 0, 0, 1]) 32 | 33 | 34 | class SkeletonNodeBase(object): 35 | def __init__(self, node_name, channels, parent=None, level=0, channel_indices=None): 36 | self.parent = parent 37 | self.node_name = node_name 38 | self.channels = channels 39 | self.level = level 40 | self.children = [] 41 | self.index = -1 42 | self.quaternion_frame_index = -1 43 | self.euler_frame_index = -1 44 | self.node_type = None 45 | self.offset = [0.0, 0.0, 0.0] 46 | self.rotation = np.array([1.0, 0.0, 0.0, 0.0]) 47 | self.euler_angles = np.array([0.0, 0.0, 0.0]) 48 | self.translation = np.array([0.0, 0.0, 0.0]) 49 | self.fixed = True 50 | self.cached_global_matrix = None 51 | self.joint_constraint = None 52 | self.format_meta_data = dict() # store format specific data 53 | self.stiffness = 0 54 | self.rotation_order = [c for c in self.channels if "rotation" in c] 55 | self._rotation_order_str = rotation_order_to_string(self.rotation_order) 56 | self.channel_indices = channel_indices 57 | self.translation_order = [c for c in self.channels if "position" in c] 58 | if self.channel_indices is not None: 59 | self.rotation_channel_indices = [self.channel_indices[self.channels.index(r)] for r in self.rotation_order] 60 | self.translation_channel_indices = [self.channel_indices[self.channels.index(t)] for t in self.translation_order] 61 | 62 | def clear_cached_global_matrix(self): 63 | self.cached_global_matrix = None 64 | 65 | def get_global_position(self, quaternion_frame, use_cache=False): 66 | global_matrix = self.get_global_matrix(quaternion_frame, use_cache) 67 | point = np.dot(global_matrix, ORIGIN) 68 | return point[:3] 69 | 70 | def get_global_position_from_euler(self, euler_frame, use_cache=False): 71 | global_matrix = self.get_global_matrix_from_euler_frame(euler_frame, use_cache) 72 | point = np.dot(global_matrix, ORIGIN) 73 | return point[:3] 74 | 75 | def get_global_orientation_quaternion(self, quaternion_frame, use_cache=False): 76 | global_matrix = self.get_global_matrix(quaternion_frame, use_cache) 77 | return quaternion_from_matrix(global_matrix) 78 | 79 | def get_global_orientation_euler(self, quaternion_frame, use_cache=False): 80 | global_matrix = self.get_global_matrix(quaternion_frame, use_cache) 81 | return euler_from_matrix(global_matrix) 82 | 83 | def get_global_matrix(self, quaternion_frame, use_cache=False): 84 | if self.cached_global_matrix is not None and use_cache: 85 | return self.cached_global_matrix 86 | else: 87 | if self.parent is not None: 88 | parent_matrix = self.parent.get_global_matrix(quaternion_frame, use_cache) 89 | self.cached_global_matrix = np.dot(parent_matrix, self.get_local_matrix(quaternion_frame)) 90 | return self.cached_global_matrix 91 | else: 92 | self.cached_global_matrix = self.get_local_matrix(quaternion_frame) 93 | return self.cached_global_matrix 94 | 95 | def get_global_matrix_from_euler_frame(self, euler_frame, use_cache=False): 96 | if self.parent is not None: 97 | parent_matrix = self.parent.get_global_matrix_from_euler_frame(euler_frame) 98 | self.cached_global_matrix = np.dot(parent_matrix, self.get_local_matrix_from_euler(euler_frame)) 99 | return self.cached_global_matrix 100 | else: 101 | self.cached_global_matrix = self.get_local_matrix_from_euler(euler_frame) 102 | return self.cached_global_matrix 103 | 104 | def get_global_matrix_from_anim_frame(self, frame, use_cache=False): 105 | if self.cached_global_matrix is not None and use_cache: 106 | return self.cached_global_matrix 107 | else: 108 | if self.parent is not None: 109 | parent_matrix = self.parent.get_global_matrix2(frame, use_cache) 110 | self.cached_global_matrix = np.dot(parent_matrix, frame[self.node_name]) 111 | return self.cached_global_matrix 112 | else: 113 | self.cached_global_matrix = frame[self.node_name] 114 | return self.cached_global_matrix 115 | 116 | def get_local_matrix(self, quaternion_frame): 117 | pass 118 | 119 | def get_local_matrix_from_euler(self, euler_frame): 120 | pass 121 | 122 | def get_frame_parameters(self, frame, rotation_type): 123 | pass 124 | 125 | def get_number_of_frame_parameters(self, rotation_type): 126 | pass 127 | 128 | def to_unity_format(self, joints, scale=1.0, joint_name_map=None): 129 | joint_desc = dict() 130 | joint_desc["name"] = self.node_name 131 | if type(self.offset) == list: 132 | offset = list(self.offset) 133 | else: 134 | offset = self.offset.tolist() 135 | offset[0] *= -scale 136 | offset[1] *= scale 137 | offset[2] *= scale 138 | joint_desc["offset"] = offset 139 | 140 | if type(self.rotation) == list: 141 | rotation = list(self.rotation) 142 | else: 143 | rotation = self.rotation.tolist() 144 | rotation[0] *= -scale 145 | rotation[1] *= -scale 146 | rotation[2] *= scale 147 | rotation[3] *= scale 148 | joint_desc["rotation"] = rotation 149 | if joint_name_map is not None: 150 | if self.node_name in joint_name_map: 151 | joint_desc["targetName"] = joint_name_map[self.node_name] 152 | else: 153 | joint_desc["targetName"] = "none" 154 | else: 155 | joint_desc["targetName"] = self.node_name 156 | joint_desc["children"] = [] 157 | joints.append(joint_desc) 158 | for c in self.children: 159 | joint_desc["children"].append(c.node_name) 160 | c.to_unity_format(joints, scale, joint_name_map=joint_name_map) 161 | 162 | def get_fk_chain_list(self): 163 | pass 164 | 165 | def get_parent_name(self, animated_joint_list): 166 | ''' 167 | :return: string, the name of parent node. If parent node is None, return None 168 | ''' 169 | if self.parent is None: 170 | return None 171 | else: 172 | if animated_joint_list is None: 173 | return self.parent.node_name 174 | else: 175 | if self.parent.node_name in animated_joint_list: 176 | return self.parent.node_name 177 | else: 178 | return self.parent.get_parent_name(animated_joint_list) 179 | 180 | 181 | class SkeletonRootNode(SkeletonNodeBase): 182 | def __init__(self, node_name, channels, parent=None, level=0, channel_indices=None): 183 | super(SkeletonRootNode, self).__init__(node_name, channels, parent, level, channel_indices) 184 | self.node_type = SKELETON_NODE_TYPE_ROOT 185 | 186 | def get_local_matrix(self, quaternion_frame): 187 | local_matrix = quaternion_matrix(quaternion_frame[3:7]) 188 | local_matrix[:3, 3] = [t + o for t, o in zip(quaternion_frame[:3], self.offset)] 189 | return local_matrix 190 | 191 | def get_local_matrix_from_euler(self, euler_frame): 192 | if not self.rotation_order is None and not self.rotation_channel_indices is None: 193 | local_matrix = euler_matrix(*euler_frame[self.rotation_channel_indices], self._rotation_order_str) 194 | else: 195 | raise ValueError("no rotation order!") 196 | if len(self.translation_channel_indices)> 0: 197 | local_matrix[:3, 3] = [t + o for t, o in zip(euler_frame[self.translation_channel_indices], self.offset)] 198 | else: 199 | local_matrix[:3, 3] = self.offset 200 | return local_matrix 201 | 202 | def get_frame_parameters(self, frame, rotation_type): 203 | if not self.fixed: 204 | return frame[:7].tolist() 205 | else: 206 | return [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] 207 | 208 | def get_euler_frame_parameters(self, euler_frame): 209 | if not self.fixed: 210 | return euler_frame[:6].tolist() 211 | else: 212 | return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 213 | 214 | def get_number_of_frame_parameters(self, rotation_type): 215 | if rotation_type == ROTATION_TYPE_QUATERNION: 216 | return 7 217 | else: 218 | return 6 219 | 220 | def get_fk_chain_list(self): 221 | return [self.node_name] 222 | 223 | class SkeletonJointNode(SkeletonNodeBase): 224 | def __init__(self, node_name, channels, parent=None, level=0, channel_indices=None): 225 | super(SkeletonJointNode, self).__init__(node_name, channels, parent, level, channel_indices) 226 | self.node_type = SKELETON_NODE_TYPE_JOINT 227 | 228 | def get_local_matrix(self, quaternion_frame): 229 | if not self.fixed: 230 | frame_index = self.quaternion_frame_index * 4 + 3 231 | m = quaternion_matrix(quaternion_frame[frame_index: frame_index + 4]) 232 | else: 233 | m = quaternion_matrix(self.rotation) 234 | m[:3, 3] = self.offset 235 | return m 236 | 237 | def get_local_matrix_from_euler(self, euler_frame): 238 | if not self.fixed: 239 | if not self.rotation_order is None: 240 | local_matrix = euler_matrix(*euler_frame[self.rotation_channel_indices], self._rotation_order_str) 241 | else: 242 | raise ValueError("no rotation order!") 243 | else: 244 | local_matrix = euler_matrix(*np.radians(self.euler_angles), axes='rxyz') 245 | # local_matrix[:3, 3] = self.offset 246 | if self.translation_channel_indices != []: 247 | local_matrix[:3, 3] = [t + o for t, o in zip(euler_frame[self.translation_channel_indices], self.offset)] 248 | else: 249 | local_matrix[:3, 3] = self.offset 250 | return local_matrix 251 | 252 | def get_frame_parameters(self, frame, rotation_type): 253 | if rotation_type == ROTATION_TYPE_QUATERNION: 254 | if not self.fixed: 255 | frame_index = self.quaternion_frame_index * 4 + 3 256 | return frame[frame_index:frame_index+4].tolist() 257 | else: 258 | return self.rotation 259 | else: 260 | if not self.fixed: 261 | frame_index = self.quaternion_frame_index * 3 + 3 262 | return frame[frame_index:frame_index+3].tolist() 263 | else: 264 | return [0.0, 0.0, 0.0] 265 | 266 | def get_number_of_frame_parameters(self, rotation_type): 267 | if rotation_type == ROTATION_TYPE_QUATERNION: 268 | return 4 269 | else: 270 | return 3 271 | 272 | def get_fk_chain_list(self): 273 | fk_chain = [self.node_name] 274 | if self.parent is not None: 275 | fk_chain += self.parent.get_fk_chain_list() 276 | return fk_chain 277 | 278 | class SkeletonEndSiteNode(SkeletonNodeBase): 279 | def __init__(self, node_name, channels, parent=None, level=0): 280 | super(SkeletonEndSiteNode, self).__init__(node_name, channels, parent, level) 281 | self.node_type = SKELETON_NODE_TYPE_END_SITE 282 | 283 | def get_local_matrix(self, quaternion_frame): 284 | local_matrix = np.identity(4) 285 | local_matrix[:3, 3] = self.offset 286 | return local_matrix 287 | 288 | def get_local_matrix_from_euler(self, euler_frame): 289 | local_matrix = np.identity(4) 290 | local_matrix[:3, 3] = self.offset 291 | return local_matrix 292 | 293 | def get_frame_parameters(self, frame, rotation_type): 294 | return None 295 | 296 | def get_number_of_frame_parameters(self, rotation_type): 297 | return 0 298 | 299 | def get_fk_chain_list(self): 300 | fk_chain = [] 301 | if self.parent is not None: 302 | fk_chain += self.parent.get_fk_chain_list() 303 | return fk_chain 304 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/__init__.py: -------------------------------------------------------------------------------- 1 | from .motion_editing import MotionEditing 2 | from .motion_grounding import MotionGrounding, add_heels_to_skeleton 3 | from .footplant_constraint_generator import FootplantConstraintGenerator 4 | from .utils import get_average_joint_position, get_average_joint_direction 5 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/analytical_inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | """ Analytical IK for arms and legs based on Section 5.3 of [1] and Section 4.4 of [2] 24 | 25 | [1] Lee, Jehee, and Sung Yong Shin. "A hierarchical approach to interactive motion editing for human-like figures." 26 | Proceedings of the 26th annual conference on Computer graphics and interactive techniques. 1999. 27 | [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. 28 | 29 | """ 30 | import math 31 | import numpy as np 32 | import scipy.integrate as integrate 33 | from transformations import quaternion_multiply, quaternion_about_axis, quaternion_matrix, quaternion_from_matrix, quaternion_inverse 34 | 35 | 36 | def normalize(v): 37 | return v/np.linalg.norm(v) 38 | 39 | 40 | def quaternion_from_axis_angle(axis, angle): 41 | q = [1,0,0,0] 42 | q[1] = axis[0] * math.sin(angle / 2) 43 | q[2] = axis[1] * math.sin(angle / 2) 44 | q[3] = axis[2] * math.sin(angle / 2) 45 | q[0] = math.cos(angle / 2) 46 | return normalize(q) 47 | 48 | 49 | def find_rotation_between_vectors(a, b): 50 | """http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another""" 51 | if np.array_equiv(a, b): 52 | return [1, 0, 0, 0] 53 | 54 | axis = normalize(np.cross(a, b)) 55 | dot = np.dot(a, b) 56 | if dot >= 1.0: 57 | return [1, 0, 0, 0] 58 | angle = math.acos(dot) 59 | q = quaternion_from_axis_angle(axis, angle) 60 | return q 61 | 62 | def calculate_angle(upper_limb, lower_limb, ru, rl, target_length): 63 | upper_limb_sq = upper_limb * upper_limb 64 | lower_limb_sq = lower_limb * lower_limb 65 | ru_sq = ru * ru 66 | rl_sq = rl * rl 67 | lusq_rusq = upper_limb_sq - ru_sq 68 | lusq_rusq = max(0, lusq_rusq) 69 | llsq_rlsq = lower_limb_sq - rl_sq 70 | llsq_rlsq = max(0, llsq_rlsq) 71 | temp = upper_limb_sq + rl_sq 72 | temp += 2 * math.sqrt(lusq_rusq) * math.sqrt(llsq_rlsq) 73 | temp += - (target_length*target_length) 74 | temp /= 2 * ru * rl 75 | print("temp",temp) 76 | temp = max(-1, temp) 77 | return math.acos(temp) 78 | 79 | 80 | def calculate_angle2(upper_limb,lower_limb,target_length): 81 | """ get angle between upper and lower limb based on desired length 82 | https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html""" 83 | a = upper_limb 84 | b = lower_limb 85 | c = target_length 86 | temp = (a*a + b*b - c*c) / (2 * a * b) 87 | temp = min(1, temp) 88 | temp = max(-1, temp) 89 | angle = math.acos(temp) 90 | return angle 91 | 92 | 93 | def damp_angle(orig_angle, target_angle, p=0.1*np.pi, a=0.01): 94 | """ src: Kovar et al. [2] Section 4.4. eq. 10 and 11""" 95 | def func(x): 96 | if x < p: 97 | return 1.0 98 | elif p <= x <= np.pi: 99 | return a * ((x-p)/(np.pi-p)) 100 | else: 101 | return 0.0 102 | res = integrate.quad(func, orig_angle, target_angle) 103 | return orig_angle + res[0] 104 | 105 | 106 | def to_local_coordinate_system(skeleton, frame, joint_name, q): 107 | """ given a global rotation concatenate it with an existing local rotation and bring it to the local coordinate system""" 108 | # delta*parent*old_local = parent*new_local 109 | # inv_parent*delta*parent*old_local = new_local 110 | if skeleton.nodes[joint_name].parent is not None: 111 | global_m = quaternion_matrix(q) 112 | parent_joint = skeleton.nodes[joint_name].parent.node_name 113 | parent_m = skeleton.nodes[parent_joint].get_global_matrix(frame, use_cache=False) 114 | old_global = np.dot(parent_m, skeleton.nodes[joint_name].get_local_matrix(frame)) 115 | new_global = np.dot(global_m, old_global) 116 | new_local = np.dot(np.linalg.inv(parent_m), new_global) 117 | return quaternion_from_matrix(new_local) 118 | else: 119 | return q 120 | 121 | 122 | 123 | 124 | def calculate_limb_joint_rotation(skeleton, root, joint, end_effector, local_joint_axis, frame, target_position): 125 | """ find angle so the distance from root to end effector is equal to the distance from the root to the target""" 126 | root_pos = skeleton.nodes[root].get_global_position(frame) 127 | joint_pos = skeleton.nodes[joint].get_global_position(frame) 128 | end_effector_pos = skeleton.nodes[end_effector].get_global_position(frame) 129 | 130 | upper_limb = np.linalg.norm(root_pos - joint_pos) 131 | lower_limb = np.linalg.norm(joint_pos - end_effector_pos) 132 | target_length = np.linalg.norm(root_pos - target_position) 133 | #current_length = np.linalg.norm(root_pos - end_effector_pos) 134 | #current_angle = calculate_angle2(upper_limb, lower_limb, current_length) 135 | target_angle = calculate_angle2(upper_limb, lower_limb, target_length) 136 | 137 | joint_delta_angle = np.pi - target_angle 138 | joint_delta_q = quaternion_about_axis(joint_delta_angle, local_joint_axis) 139 | joint_delta_q = normalize(joint_delta_q) 140 | return joint_delta_q 141 | 142 | 143 | def calculate_limb_root_rotation(skeleton, root, end_effector, frame, target_position): 144 | """ find angle between the vectors end_effector - root and target- root """ 145 | 146 | # align vectors 147 | root_pos = skeleton.nodes[root].get_global_position(frame) 148 | end_effector_pos = skeleton.nodes[end_effector].get_global_position(frame) 149 | src_delta = end_effector_pos - root_pos 150 | src_dir = src_delta / np.linalg.norm(src_delta) 151 | 152 | target_delta = target_position - root_pos 153 | target_dir = target_delta / np.linalg.norm(target_delta) 154 | 155 | root_delta_q = find_rotation_between_vectors(src_dir, target_dir) 156 | root_delta_q = normalize(root_delta_q) 157 | 158 | return to_local_coordinate_system(skeleton, frame, root, root_delta_q) 159 | 160 | 161 | class AnalyticalLimbIK(object): 162 | def __init__(self, skeleton, limb_root, limb_joint, end_effector, joint_axis, local_end_effector_dir, damp_angle=None, damp_factor=0.01): 163 | self.skeleton = skeleton 164 | self.limb_root = limb_root 165 | self.limb_joint = limb_joint 166 | self.end_effector = end_effector 167 | self.local_joint_axis = joint_axis 168 | self.local_end_effector_dir = local_end_effector_dir 169 | joint_idx = self.skeleton.animated_joints.index(self.limb_joint) * 4 + 3 170 | self.joint_indices = [joint_idx, joint_idx + 1, joint_idx + 2, joint_idx + 3] 171 | root_idx = self.skeleton.animated_joints.index(self.limb_root) * 4 + 3 172 | self.root_indices = [root_idx, root_idx + 1, root_idx + 2, root_idx + 3] 173 | end_effector_idx = self.skeleton.animated_joints.index(self.end_effector) * 4 + 3 174 | self.end_effector_indices = [end_effector_idx, end_effector_idx + 1, end_effector_idx + 2, end_effector_idx + 3] 175 | self.damp_angle = damp_angle 176 | self.damp_factor = damp_factor 177 | 178 | @classmethod 179 | def init_from_dict(cls, skeleton, joint_name, data, damp_angle=None, damp_factor=None): 180 | limb_root = data["root"] 181 | limb_joint = data["joint"] 182 | joint_axis = data["joint_axis"] 183 | end_effector_dir = data["end_effector_dir"] 184 | return AnalyticalLimbIK(skeleton, limb_root, limb_joint, joint_name, joint_axis, end_effector_dir, damp_angle, damp_factor) 185 | 186 | def calculate_limb_joint_rotation(self, frame, target_position): 187 | """ find angle so the distance from root to end effector is equal to the distance from the root to the target""" 188 | root_pos = self.skeleton.nodes[self.limb_root].get_global_position(frame) 189 | joint_pos = self.skeleton.nodes[self.limb_joint].get_global_position(frame) 190 | end_effector_pos = self.skeleton.nodes[self.end_effector].get_global_position(frame) 191 | 192 | upper_limb = np.linalg.norm(root_pos - joint_pos) 193 | lower_limb = np.linalg.norm(joint_pos - end_effector_pos) 194 | target_length = np.linalg.norm(root_pos - target_position) 195 | current_length = np.linalg.norm(root_pos - end_effector_pos) 196 | current_angle = calculate_angle2(upper_limb, lower_limb, current_length) 197 | target_angle = calculate_angle2(upper_limb, lower_limb, target_length) 198 | if self.damp_angle is not None: 199 | target_angle = damp_angle(current_angle, target_angle, self.damp_angle, self.damp_factor) 200 | #if abs(target_angle - np.pi) < self.min_angle: 201 | # target_angle -= self.min_angle 202 | joint_delta_angle = np.pi - target_angle 203 | joint_delta_q = quaternion_about_axis(joint_delta_angle, self.local_joint_axis) 204 | joint_delta_q = normalize(joint_delta_q) 205 | frame[self.joint_indices] = joint_delta_q 206 | return joint_delta_q 207 | 208 | def calculate_limb_root_rotation(self, frame, target_position): 209 | """ find angle between the vectors end_effector - root and target- root """ 210 | 211 | # align vectors 212 | root_pos = self.skeleton.nodes[self.limb_root].get_global_position(frame) 213 | end_effector_pos = self.skeleton.nodes[self.end_effector].get_global_position(frame) 214 | src_delta = end_effector_pos - root_pos 215 | src_dir = src_delta / np.linalg.norm(src_delta) 216 | 217 | target_delta = target_position - root_pos 218 | target_dir = target_delta / np.linalg.norm(target_delta) 219 | 220 | root_delta_q = find_rotation_between_vectors(src_dir, target_dir) 221 | root_delta_q = normalize(root_delta_q) 222 | 223 | frame[self.root_indices] = self._to_local_coordinate_system(frame, self.limb_root, root_delta_q) 224 | 225 | def _to_local_coordinate_system(self, frame, joint_name, q): 226 | """ given a global rotation concatenate it with an existing local rotation and bring it to the local coordinate system""" 227 | # delta*parent*old_local = parent*new_local 228 | # inv_parent*delta*parent*old_local = new_local 229 | global_m = quaternion_matrix(q) 230 | parent_joint = self.skeleton.nodes[joint_name].parent.node_name 231 | parent_m = self.skeleton.nodes[parent_joint].get_global_matrix(frame, use_cache=False) 232 | old_global = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(frame)) 233 | new_global = np.dot(global_m, old_global) 234 | new_local = np.dot(np.linalg.inv(parent_m), new_global) 235 | return quaternion_from_matrix(new_local) 236 | 237 | def calculate_end_effector_rotation(self, frame, target_dir): 238 | src_dir = self.get_joint_dir(frame, self.end_effector) 239 | global_delta_q = find_rotation_between_vectors(src_dir, target_dir) 240 | new_local_q = self._to_local_coordinate_system(frame, self.end_effector, global_delta_q) 241 | frame[self.end_effector_indices] = new_local_q 242 | 243 | def set_end_effector_rotation(self, frame, target_orientation): 244 | #print "set orientation", target_orientation 245 | q = self.get_global_joint_orientation(self.end_effector, frame) 246 | delta_orientation = quaternion_multiply(target_orientation, quaternion_inverse(q)) 247 | new_local_q = self._to_local_coordinate_system(frame, self.end_effector, delta_orientation) 248 | frame[self.end_effector_indices] = new_local_q 249 | 250 | def get_global_joint_orientation(self, joint_name, frame): 251 | m = self.skeleton.nodes[joint_name].get_global_matrix(frame) 252 | m[:3, 3] = [0, 0, 0] 253 | return normalize(quaternion_from_matrix(m)) 254 | 255 | def get_joint_dir(self, frame, joint_name): 256 | pos1 = self.skeleton.nodes[joint_name].get_global_position(frame) 257 | pos2 = self.skeleton.nodes[joint_name].children[0].get_global_position(frame) 258 | return normalize(pos2 - pos1) 259 | 260 | 261 | def apply(self, frame, position, orientation): 262 | if position is not None: 263 | # 1 calculate joint angle based on the distance to target position 264 | self.calculate_limb_joint_rotation(frame, position) 265 | 266 | # 2 calculate limb root rotation to align the end effector with the target position 267 | self.calculate_limb_root_rotation(frame, position) 268 | 269 | # 3 orient end effector 270 | if orientation is not None: 271 | self.set_end_effector_rotation2(frame, orientation) 272 | return frame 273 | 274 | 275 | def set_end_effector_rotation2(self, frame, target_orientation): 276 | #print("set", target_orientation) 277 | new_local_q = self.to_local_cos2(self.end_effector, frame, target_orientation) 278 | frame[self.end_effector_indices] = new_local_q 279 | 280 | def to_local_cos2(self, joint_name, frame, q): 281 | # bring into parent coordinate system 282 | parent_joint = self.skeleton.nodes[joint_name].parent.node_name 283 | pm = self.skeleton.nodes[parent_joint].get_global_matrix(frame)#[:3, :3] 284 | inv_p = quaternion_inverse(quaternion_from_matrix(pm)) 285 | normalize(inv_p) 286 | return quaternion_multiply(inv_p, q) 287 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/cubic_motion_spline.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | from scipy.interpolate import interp1d 25 | from transformations import quaternion_multiply, quaternion_inverse 26 | 27 | 28 | def get_quaternion_delta(a, b): 29 | return quaternion_multiply(quaternion_inverse(b), a) 30 | 31 | 32 | def add_frames(skeleton, a, b): 33 | """ returns c = a + b""" 34 | #print("add frames", len(a), len(b)) 35 | c = np.zeros(len(a)) 36 | c[:3] = a[:3] + b[:3] 37 | for idx, j in enumerate(skeleton.animated_joints): 38 | o = idx * 4 + 3 39 | q_a = a[o:o + 4] 40 | q_b = b[o:o + 4] 41 | #print(q_a,q_b) 42 | q_prod = quaternion_multiply(q_a, q_b) 43 | c[o:o + 4] = q_prod / np.linalg.norm(q_prod) 44 | return c 45 | 46 | 47 | def substract_frames(skeleton, a, b): 48 | """ returns c = a - b""" 49 | c = np.zeros(len(a)) 50 | c[:3] = a[:3] - b[:3] 51 | for idx, j in enumerate(skeleton.animated_joints): 52 | o = idx*4 + 3 53 | q_a = a[o:o+4] 54 | q_b = b[o:o+4] 55 | q_delta = get_quaternion_delta(q_a, q_b) 56 | c[o:o+4] = q_delta / np.linalg.norm(q_delta) 57 | return c 58 | 59 | 60 | def generate_knots(n_frames, n_knots): 61 | """https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.LSQUnivariateSpline.html""" 62 | knots = np.linspace(0, n_frames - 1, n_knots) 63 | return knots 64 | 65 | 66 | class CubicMotionSpline(object): 67 | def __init__(self, skeleton, splines, n_dims): 68 | self.skeleton = skeleton 69 | self.n_dims = n_dims 70 | self.splines = splines 71 | 72 | @classmethod 73 | def fit_frames(cls, skeleton, times, frames, kind='cubic'): 74 | if type(frames) is np.ndarray: 75 | n_frames, n_dims = frames.shape 76 | else: 77 | n_dims = 1 78 | splines = [] 79 | for d in range(n_dims): 80 | s = interp1d(times, frames[:, d], kind=kind) 81 | splines.append(s) 82 | return CubicMotionSpline(skeleton, splines, n_dims) 83 | 84 | def evaluate(self, t): 85 | return np.asarray([s(t) for s in self.splines]).T 86 | 87 | def get_displacement_map(self, frames): 88 | d = [] 89 | for idx, f in enumerate(frames): 90 | s_f = self.evaluate(idx) 91 | d.append(substract_frames(self.skeleton, f, s_f)) 92 | return np.array(d) 93 | 94 | def to_frames(self, times): 95 | frames = [] 96 | for t in times: 97 | frame = self.evaluate(t) 98 | frames.append(frame) 99 | return frames 100 | 101 | def add_spline_discrete(self, other, times): 102 | new_frames = [] 103 | for t in times: 104 | f = add_frames(self.skeleton, self.evaluate(t), other.evaluate(t)) 105 | new_frames.append(f) 106 | new_frames = np.array(new_frames) 107 | splines = [] 108 | for d in range(self.n_dims): 109 | s = interp1d(times, new_frames[:, d], kind='cubic') 110 | splines.append(s) 111 | self.splines = splines 112 | 113 | def plot(self, x): 114 | import matplotlib.pyplot as plt 115 | for s in self.splines: 116 | plt.plot(x, s(x)) 117 | plt.show() 118 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/fabrik_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | from .fabrik_chain import FABRIKChain, to_local_cos, quaternion_from_vector_to_vector, FABRIKBone, ROOT_OFFSET 25 | 26 | 27 | def get_child_offset(skeleton, node, child_node): 28 | actual_offset = np.array([0, 0, 0], np.float) 29 | while node is not None and skeleton.nodes[node].children[0].node_name != child_node: 30 | local_offset = np.array(skeleton.nodes[node].children[0].offset) 31 | local_offset_len = np.linalg.norm(local_offset) 32 | if local_offset_len > 0: 33 | local_offset /= local_offset_len 34 | actual_offset = actual_offset + local_offset 35 | node = skeleton.nodes[node].children[0].node_name 36 | if len(skeleton.nodes[node].children) < 1: 37 | node = None 38 | else: 39 | node = None 40 | return actual_offset 41 | 42 | 43 | def get_global_joint_parameters_from_positions(skeleton, positions, next_nodes): 44 | n_parameters = len(skeleton.animated_joints)*4+3 45 | frame = np.zeros(n_parameters) 46 | frame[:3] = positions[skeleton.root] 47 | o = 3 48 | print(skeleton.animated_joints) 49 | print(next_nodes.keys()) 50 | print(positions.keys()) 51 | for node in skeleton.animated_joints: 52 | next_node = next_nodes[node] 53 | if next_node in positions: 54 | next_offset = np.array(skeleton.nodes[next_node].offset) 55 | next_offset /= np.linalg.norm(next_offset) 56 | 57 | # 1. sum over offsets of static nodes 58 | local_offset = get_child_offset(skeleton, node, next_node) 59 | #print(node, local_offset) 60 | actual_offset = next_offset + local_offset 61 | actual_offset /= np.linalg.norm(actual_offset) # actual_offset = [0.5, 0.5,0] 62 | 63 | 64 | dir_to_child = positions[next_node] - positions[node] 65 | dir_to_child /= np.linalg.norm(dir_to_child) 66 | 67 | # 2. get global rotation 68 | global_q = quaternion_from_vector_to_vector(actual_offset, dir_to_child) 69 | frame[o:o + 4] = global_q 70 | else: 71 | #print("ignore", node, next_node) 72 | frame[o:o + 4] = [1,0,0,0] 73 | o += 4 74 | return frame 75 | 76 | 77 | def global_to_local_frame(skeleton, global_frame): 78 | n_parameters = len(skeleton.animated_joints)*4+3 79 | frame = np.zeros(n_parameters) 80 | o = 3 81 | for node in skeleton.animated_joints: 82 | frame[o:o + 4] = to_local_cos(skeleton, node, frame, global_frame[o:o + 4]) 83 | 84 | o += 4 85 | 86 | return frame 87 | 88 | 89 | class FABRIKNode(object): 90 | def __init__(self, skeleton, root, parent_chain=None, tolerance=0.01, max_iter=100, root_offset=ROOT_OFFSET): 91 | self.root_pos = np.array([0,0,0], dtype=np.float) 92 | self.skeleton = skeleton 93 | self.n_parameters = len(self.skeleton.animated_joints)*4+3 94 | self.tolerance = tolerance 95 | self.max_iter = max_iter 96 | self.parent_chain = parent_chain 97 | self.root = root 98 | self.root_offset = root_offset 99 | self.child_nodes = list() 100 | # self.target = None 101 | # self.position = None 102 | self.is_leaf = True 103 | self.construct_from_skeleton(skeleton, root, tolerance, max_iter) 104 | 105 | def construct_from_skeleton(self, skeleton, root, tolerance, max_iter): 106 | """ there need to be two end effectors""" 107 | children = [c.node_name for c in skeleton.nodes[root].children] 108 | for c in children: 109 | self.is_leaf = False 110 | current_node = c 111 | node_order = [self.root] 112 | while current_node is not None: 113 | n_children = len(skeleton.nodes[current_node].children) 114 | if n_children == 1: # append to chain 115 | child_node = skeleton.nodes[current_node].children[0].node_name 116 | # only add list to joints 117 | if not skeleton.nodes[current_node].fixed: 118 | node_order.append(current_node)# skip fixed nodes 119 | current_node = child_node 120 | else: # stop chain # split up by adding child nodes 121 | if n_children > 0: 122 | node_order.append(current_node) 123 | bones = dict() 124 | for idx, node in enumerate(node_order): 125 | child_node = None 126 | if idx+1 < len(node_order): 127 | child_node = node_order[idx + 1] 128 | bones[node] = FABRIKBone(node, child_node) 129 | if idx == 0 and self.parent_chain is None : 130 | bones[node].is_root = True 131 | else: 132 | bones[node].is_root = False 133 | parent_chain = FABRIKChain(skeleton, bones, node_order) 134 | print("construct node at",self.root , current_node, node_order) 135 | node = FABRIKNode(skeleton, current_node, parent_chain, tolerance, max_iter) 136 | self.child_nodes.append(node) 137 | current_node = None 138 | 139 | def backward_stage(self): 140 | for c in self.child_nodes: 141 | c.backward_stage() # calculate backward update of children of child to start at the end effectors 142 | 143 | if not self.is_leaf: 144 | positions = [c.get_chain_root_position() for c in self.child_nodes] 145 | t = np.mean(positions, axis=0) 146 | #print("centroid",t) 147 | #print("no leaf") 148 | else: 149 | t = self.parent_chain.target 150 | #print("leaf") 151 | 152 | if self.parent_chain is not None: 153 | self.parent_chain.target = t 154 | if self.target_is_reachable(): 155 | self.parent_chain.backward() 156 | else: 157 | print("unreachable") 158 | # if unreachable orient joints to target 159 | self.parent_chain.orient_to_target() 160 | 161 | def target_is_reachable(self): 162 | return self.parent_chain.target_is_reachable() 163 | 164 | def forward_stage(self): 165 | # calculate forward update of parent chain 166 | if self.parent_chain is not None: 167 | #if self.parent_chain.target_is_reachable(): 168 | self.parent_chain.forward() 169 | #else: 170 | # self.parent_chain.orient_to_target() 171 | 172 | # calculate forward update of children of child 173 | for c in self.child_nodes: 174 | if self.parent_chain is not None: 175 | c.parent_chain.root_pos = self.parent_chain.get_end_effector_position() 176 | else: 177 | c.parent_chain.root_pos = self.root_pos 178 | c.forward_stage() 179 | 180 | def get_chain_root_position(self): 181 | root_node = self.parent_chain.node_order[0] 182 | return self.parent_chain.bones[root_node].position 183 | 184 | 185 | def set_positions_from_frame(self, frame, parent_length): 186 | if self.parent_chain is not None: 187 | self.parent_chain.set_positions_from_frame(frame, parent_length) 188 | parent_length += self.parent_chain.chain_length 189 | for c in self.child_nodes: 190 | c.set_positions_from_frame(frame, parent_length) 191 | 192 | def get_error(self): 193 | error = 0 194 | for c in self.child_nodes: 195 | error += c.get_error() 196 | if self.is_leaf: 197 | if self.parent_chain.target_is_reachable(): 198 | error += self.parent_chain.get_error() 199 | else: 200 | error += 0 201 | return error 202 | 203 | def solve(self): 204 | iter = 0 205 | distance = self.get_error() 206 | while distance > self.tolerance and iter < self.max_iter: 207 | self.backward_stage() 208 | self.forward_stage() 209 | distance = self.get_error() 210 | iter += 1 211 | n_out_of_reach = self.targets_out_of_reach() 212 | if n_out_of_reach > 0: 213 | self.orient_to_targets() 214 | print("solved", distance, n_out_of_reach) 215 | self.print_end_effectors() 216 | 217 | def print_end_effectors(self): 218 | n_targets = 0 219 | for c in self.child_nodes: 220 | if c.is_leaf: 221 | print(c.root, c.parent_chain.target, c.parent_chain.get_end_effector_position()) 222 | else: 223 | c.print_end_effectors() 224 | return n_targets 225 | 226 | def targets_out_of_reach(self): 227 | n_targets = 0 228 | for c in self.child_nodes: 229 | if c.is_leaf and not c.parent_chain.target_is_reachable(): 230 | n_targets+=1 231 | else: 232 | n_targets += c.targets_out_of_reach() 233 | return n_targets 234 | 235 | def orient_to_targets(self): 236 | for c in self.child_nodes: 237 | if c.is_leaf and not c.parent_chain.target_is_reachable(): 238 | c.parent_chain.orient_to_target() 239 | else: 240 | c.orient_to_targets() 241 | 242 | def get_joint_parameters(self, frame): 243 | #print("get joint parameters") 244 | for c in self.child_nodes: 245 | print("from", c.parent_chain.node_order[0], "to", c.parent_chain.node_order[-1]) 246 | global_frame = c.parent_chain.get_joint_parameters_global() 247 | src = 3 248 | if self.parent_chain is None: 249 | animated_joints = c.parent_chain.node_order 250 | else: 251 | animated_joints = c.parent_chain.node_order[1:] 252 | for j in animated_joints: #ignore root rotation 253 | dest = self.skeleton.animated_joints.index(j)*4 +3 254 | frame[dest:dest+4] = to_local_cos(self.skeleton, j, frame, global_frame[src:src+4]) 255 | src += 4 256 | c.get_joint_parameters(frame) 257 | return frame 258 | 259 | def set_targets(self, targets): 260 | #print("set targets",targets) 261 | if self.is_leaf: 262 | if self.root in targets: 263 | self.target = targets[self.root] 264 | self.parent_chain.target = self.target 265 | #print("set target",self.root) 266 | else: 267 | self.target = self.parent_chain.get_end_effector_position() # keep the initial position as target for unconstrained leafs 268 | self.parent_chain.target = self.target 269 | print("set end effector position") 270 | else: 271 | for c in self.child_nodes: 272 | c.set_targets(targets) 273 | 274 | def run(self, orig_frame, targets): 275 | self.set_positions_from_frame(orig_frame, 0) 276 | self.set_targets(targets) 277 | self.solve() 278 | if False: 279 | frame = np.zeros(self.n_parameters) 280 | self.get_joint_parameters(frame) 281 | else: 282 | joint_pos = dict() 283 | self.get_global_positions(joint_pos) 284 | next_nodes = dict() 285 | self.get_next_nodes(next_nodes) 286 | frame = get_global_joint_parameters_from_positions(self.skeleton, joint_pos, next_nodes) 287 | frame = global_to_local_frame(self.skeleton, frame) 288 | #print("done") 289 | #self.print_frame_parameters(frame) 290 | return frame 291 | 292 | def get_next_nodes(self, next_nodes): 293 | if self.parent_chain is not None: 294 | self.parent_chain.get_next_nodes(next_nodes) 295 | for c in self.child_nodes: 296 | c.get_next_nodes(next_nodes) 297 | 298 | def print_frame_parameters(self, frame): 299 | idx = 3 300 | for n in self.skeleton.nodes: 301 | if len(self.skeleton.nodes[n].children) > 0: 302 | print(n, frame[idx: idx+4]) 303 | idx +=4 304 | return 305 | 306 | def draw(self, m,v,p, l): 307 | if self.parent_chain is not None: 308 | self.parent_chain.draw(m,v,p,l) 309 | for c in self.child_nodes: 310 | c.draw(m, v, p, l) 311 | 312 | def get_global_positions(self, positions): 313 | if self.parent_chain is not None: 314 | positions.update(self.parent_chain.get_global_positions()) 315 | for c in self.child_nodes: 316 | c.get_global_positions(positions) -------------------------------------------------------------------------------- /anim_utils/motion_editing/hybrit_ik.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | """ Hybrit IK using numerical IK based on an exponential displacement map and analytical IK 24 | based on Section 5.1 of [1] 25 | 26 | Use current configuration as q_0 27 | Find v such that (q_i = q_0_i exp(v_i) for 0 self.success_threshold and iter_counter < self.max_retries: 88 | r = minimize(self._objective, guess, args=(self.skeleton, reference, constraints, self.joint_weights, self.analytical_ik), 89 | method=self.ik_settings["optimization_method"], 90 | tol=self.ik_settings["tolerance"], 91 | options=self._optimization_options) 92 | guess = r["x"] 93 | error = self._objective(guess, self.skeleton, reference, constraints, self.joint_weights, self.analytical_ik) 94 | iter_counter += 1 95 | 96 | exp_frame = guess 97 | return exp_frame 98 | 99 | def modify_frame(self, reference, constraints): 100 | exp_frame = self.run(reference, constraints) 101 | d = convert_exp_frame_to_quat_frame(self.skeleton, exp_frame) 102 | q_frame = add_quat_frames(self.skeleton, reference, d) 103 | for c in constraints: 104 | if c.joint_name in self.analytical_ik: 105 | q_frame = self.analytical_ik[c.joint_name].apply(q_frame, c.position, c.orientation) 106 | return q_frame 107 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/ik_constraints.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | 25 | SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION = "keyframe_position" 26 | SPATIAL_CONSTRAINT_TYPE_TWO_HAND_POSITION = "keyframe_two_hands" 27 | SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION = "keyframe_relative_position" 28 | SPATIAL_CONSTRAINT_TYPE_KEYFRAME_LOOK_AT = "keyframe_look_at" 29 | SUPPORTED_CONSTRAINT_TYPES = [SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION, 30 | SPATIAL_CONSTRAINT_TYPE_TWO_HAND_POSITION, 31 | SPATIAL_CONSTRAINT_TYPE_KEYFRAME_LOOK_AT, 32 | SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION] 33 | class IKConstraint(object): 34 | @staticmethod 35 | def evaluate(params, data): 36 | pass 37 | 38 | 39 | class JointIKConstraint(IKConstraint): 40 | 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): 41 | self.joint_name = joint_name 42 | self.position = position 43 | self.orientation = orientation 44 | self.keyframe = keyframe 45 | self.frame_range = frame_range 46 | self.free_joints = free_joints 47 | self.step_idx = step_idx 48 | self.look_at = look_at 49 | self.optimize = optimize 50 | self.offset = offset 51 | self.look_at_pos = look_at_pos 52 | # set in case it is a relative constraint 53 | self.relative_parent_joint_name = None # joint the offsets points from to the target 54 | self.relative_offset = None 55 | 56 | # tool orientation constraint 57 | self.src_tool_cos = None # tool coordinate system 58 | self.dest_tool_cos = None # target direction 59 | 60 | # set a fk chain root to reduce the degrees of freedom 61 | self.fk_chain_root = None 62 | 63 | 64 | @staticmethod 65 | def evaluate(parameters, data): 66 | pose, free_joints, target_joint, target_position, target_orientation, offset = data 67 | pose.set_channel_values(parameters, free_joints) 68 | #parent_joint = pose.get_parent_joint(target_joint) 69 | #pose.apply_bounds_on_joint(parent_joint) 70 | if target_orientation is not None: 71 | parent_joint = pose.get_parent_joint(target_joint) 72 | if parent_joint is not None: 73 | pose.set_hand_orientation(parent_joint, target_orientation) 74 | #pose.apply_bounds_on_joint(parent_joint) 75 | if offset is not None: 76 | m = pose.evaluate_matrix(target_joint) 77 | p = np.dot(m, offset)[:3] 78 | d = target_position - p 79 | else: 80 | d = pose.evaluate_position(target_joint) - target_position 81 | return np.dot(d, d) 82 | 83 | def data(self, ik, free_joints=None): 84 | if free_joints is None: 85 | free_joints = self.free_joints 86 | if ik.optimize_orientation: 87 | orientation = self.orientation 88 | else: 89 | orientation = None 90 | return ik.pose, free_joints, self.joint_name, self.position, orientation, self.offset 91 | 92 | def get_joint_names(self): 93 | return [self.joint_name] 94 | 95 | class RelativeJointIKConstraint(IKConstraint): 96 | def __init__(self, ref_joint_name, target_joint_name, rel_position, keyframe, free_joints, step_idx=-1, frame_range=None): 97 | self.ref_joint_name = ref_joint_name 98 | self.target_joint_name = target_joint_name 99 | self.rel_position = rel_position 100 | self.keyframe = keyframe 101 | self.frame_range = frame_range 102 | self.free_joints = free_joints 103 | self.step_idx = step_idx 104 | 105 | @staticmethod 106 | def evaluate(parameters, data): 107 | pose, free_joints, ref_joint, target_joint, target_delta = data 108 | pose.set_channel_values(parameters, free_joints) 109 | ref_matrix = pose.skeleton.nodes[ref_joint].get_global_matrix(pose.get_vector()) 110 | target = np.dot(ref_matrix, target_delta)[:3] 111 | d = pose.evaluate_position(target_joint) - target 112 | return np.dot(d, d) 113 | 114 | def data(self, ik, free_joints=None): 115 | if free_joints is None: 116 | free_joints = self.free_joints 117 | return ik.pose, free_joints, self.ref_joint_name, self.target_joint_name, self.rel_position 118 | 119 | def get_joint_names(self): 120 | return [self.target_joint_name] 121 | 122 | 123 | class TwoJointIKConstraint(IKConstraint): 124 | def __init__(self, joint_names, target_positions, target_center, target_delta, target_direction, keyframe, free_joints): 125 | self.joint_names = joint_names 126 | self.target_positions = target_positions 127 | self.target_center = target_center 128 | self.target_delta = target_delta 129 | self.target_direction = target_direction 130 | self.keyframe = keyframe 131 | self.free_joints = free_joints 132 | 133 | @staticmethod 134 | def evaluate(parameters, data): 135 | pose, free_joints, joint_names, target_positions, target_center, target_delta, target_direction = data 136 | pose.set_channel_values(parameters, free_joints) 137 | left = pose.evaluate_position(joint_names[0]) 138 | right = pose.evaluate_position(joint_names[1]) 139 | delta_vector = right - left 140 | residual_vector = [0.0, 0.0, 0.0] 141 | #get distance to center 142 | residual_vector[0] = np.linalg.norm(target_center - (left + 0.5 * delta_vector)) 143 | #get difference to distance between hands 144 | delta = np.linalg.norm(delta_vector) 145 | residual_vector[1] = abs(target_delta - delta) 146 | #print "difference", residual_vector[1] 147 | #get difference of global orientation 148 | direction = delta_vector/delta 149 | 150 | residual_vector[2] = abs(target_direction[0] - direction[0]) + \ 151 | abs(target_direction[1] - direction[1]) + \ 152 | abs(target_direction[2] - direction[2]) 153 | residual_vector[2] *= 10.0 154 | 155 | #print (target_center, (left + 0.5 * delta_vector), left, right) 156 | #error = np.linalg.norm(left-target_positions[0]) + np.linalg.norm(right-target_positions[1]) 157 | return sum(residual_vector)#error#residual_vector[0]#sum(residual_vector) 158 | 159 | def data(self, ik, free_joints=None): 160 | if free_joints is None: 161 | free_joints = self.free_joints 162 | return ik.pose, free_joints, self.joint_names, self.target_positions, self.target_center, self.target_delta, self.target_direction 163 | 164 | def get_joint_names(self): 165 | return self.joint_names 166 | 167 | 168 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/ik_constraints_builder.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import collections 24 | from copy import deepcopy 25 | import numpy as np 26 | from transformations import quaternion_from_matrix 27 | from anim_utils.motion_editing.motion_editing import KeyframeConstraint 28 | from .ik_constraints import SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION, SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION, SUPPORTED_CONSTRAINT_TYPES 29 | 30 | class IKConstraintsBuilder(object): 31 | def __init__(self, skeleton, force_orientation=False): 32 | self.skeleton = skeleton 33 | self.force_orientation = force_orientation 34 | 35 | def convert_to_ik_constraints(self, constraints, frame_offset=0, time_function=None, constrain_orientation=True): 36 | ik_constraints = collections.OrderedDict() 37 | for c in constraints: 38 | if c.constraint_type not in SUPPORTED_CONSTRAINT_TYPES or "generated" in c.semantic_annotation.keys(): 39 | print("skip unsupported constraint") 40 | continue 41 | start_frame_idx = self.get_global_frame_idx(c.canonical_keyframe, frame_offset, time_function) 42 | if c.canonical_end_keyframe is not None: 43 | print("apply ik constraint on region") 44 | end_frame_idx = self.get_global_frame_idx(c.canonical_end_keyframe, frame_offset, time_function) 45 | else: 46 | print("no end keyframe defined") 47 | end_frame_idx = start_frame_idx+1 48 | 49 | for frame_idx in range(start_frame_idx, end_frame_idx): 50 | ik_constraint = self.convert_mg_constraint_to_ik_constraint(frame_idx, c, constrain_orientation) 51 | 52 | ik_constraint.src_tool_cos = c.src_tool_cos 53 | ik_constraint.dest_tool_cos = c.dest_tool_cos 54 | 55 | ik_constraint.orientation = None 56 | if c.orientation is not None: 57 | if c.constrain_orientation_in_region: 58 | if start_frame_idx < frame_idx and frame_idx < end_frame_idx -1: 59 | ik_constraint.inside_region_orientation = True 60 | elif frame_idx == start_frame_idx: 61 | ik_constraint.inside_region_orientation = True 62 | ik_constraint.keep_orientation = True 63 | elif frame_idx == start_frame_idx: 64 | ik_constraint.orientation = c.orientation 65 | 66 | 67 | ik_constraint.position = None 68 | if c.position is not None: 69 | if c.constrain_position_in_region: 70 | ik_constraint.inside_region_position = False 71 | if start_frame_idx < frame_idx and frame_idx < end_frame_idx -1: 72 | ik_constraint.inside_region_position = True 73 | elif frame_idx == end_frame_idx-1: 74 | ik_constraint.end_of_region = True 75 | elif frame_idx == start_frame_idx: 76 | ik_constraint.position = c.position 77 | 78 | if ik_constraint is not None: 79 | if frame_idx not in ik_constraints: 80 | ik_constraints[frame_idx] = dict() 81 | if c.joint_name not in ik_constraints[frame_idx]: 82 | ik_constraints[frame_idx][c.joint_name] = [] 83 | ik_constraints[frame_idx][c.joint_name] = ik_constraint 84 | 85 | return ik_constraints 86 | 87 | def get_global_frame_idx(self, mp_frame_idx, frame_offset, time_function): 88 | if time_function is not None: 89 | frame_idx = frame_offset + int(time_function[mp_frame_idx]) + 1 90 | else: 91 | frame_idx = frame_offset + int(mp_frame_idx) 92 | return frame_idx 93 | 94 | def convert_mg_constraint_to_ik_constraint(self, frame_idx, mg_constraint, constrain_orientation=False): 95 | if mg_constraint.constraint_type == SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION: 96 | ik_constraint = self._create_keyframe_ik_constraint(mg_constraint, frame_idx, constrain_orientation, look_at=mg_constraint.look_at) 97 | elif mg_constraint.constraint_type == SPATIAL_CONSTRAINT_TYPE_KEYFRAME_RELATIVE_POSITION: 98 | ik_constraint = KeyframeConstraint(frame_idx, mg_constraint.joint_name, mg_constraint.position, None, mg_constraint.look_at, mg_constraint.offset) 99 | else: 100 | ik_constraint = None 101 | return ik_constraint 102 | 103 | def _create_keyframe_ik_constraint(self, constraint, keyframe, constrain_orientation, look_at): 104 | if constrain_orientation: 105 | orientation = constraint.orientation 106 | else: 107 | orientation = None 108 | return KeyframeConstraint(keyframe, constraint.joint_name, constraint.position, orientation, look_at) 109 | 110 | def generate_relative_constraint(self, keyframe, frame, joint_name, relative_joint_name): 111 | joint_pos = self.skeleton.nodes[joint_name].get_global_position(frame) 112 | rel_joint_pos = self.skeleton.nodes[relative_joint_name].get_global_position(frame) 113 | #create a keyframe constraint but indicate that it is a relative constraint 114 | ik_constraint = KeyframeConstraint(keyframe, joint_name, None, None, None) 115 | ik_constraint.relative_parent_joint_name = relative_joint_name 116 | ik_constraint.relative_offset = joint_pos - rel_joint_pos 117 | return ik_constraint 118 | 119 | def generate_mirror_constraint(self, keyframe, ref_frame, frame, mirror_joint_name): 120 | """ generate a constraint on the mirror joint with the similar offset to the root as in the reference frame""" 121 | ref_mirror_joint_pos = self.skeleton.nodes[mirror_joint_name].get_global_position(ref_frame) 122 | ref_root_joint_pos = self.skeleton.nodes[self.skeleton.root].get_global_position(ref_frame) 123 | ref_offset_from_root = ref_mirror_joint_pos-ref_root_joint_pos 124 | 125 | target_root_joint_pos = self.skeleton.nodes[self.skeleton.root].get_global_position(frame) 126 | mirror_joint_pos = ref_offset_from_root + target_root_joint_pos 127 | print("generate mirror constraint on",mirror_joint_name, mirror_joint_pos, ref_mirror_joint_pos) 128 | #create a keyframe constraint and set the original position as constraint 129 | ik_constraint = KeyframeConstraint(keyframe, mirror_joint_name, mirror_joint_pos, None, None) 130 | return ik_constraint 131 | 132 | 133 | def generate_orientation_constraint_from_frame(self, frame, joint_name): 134 | if joint_name in self.skeleton.nodes: 135 | m = self.skeleton.nodes[joint_name].get_global_matrix(frame)#[:3,:3] 136 | return quaternion_from_matrix(m) 137 | 138 | def convert_to_ik_constraints_with_relative(self, frames, constraints, frame_offset=0, time_function=None, constrain_orientation=True): 139 | """ Create an orderered dictionary containing for each constrained frame, a KeyframeConstraint for the constrained joint""" 140 | ik_constraints = collections.OrderedDict() 141 | for c in constraints: 142 | if c.constraint_type not in SUPPORTED_CONSTRAINT_TYPES or "generated" in c.semantic_annotation.keys(): 143 | print("skip unsupported constraint") 144 | continue 145 | start_frame_idx = self.get_global_frame_idx(c.canonical_keyframe, frame_offset, time_function) 146 | if c.canonical_end_keyframe is not None: 147 | print("apply ik constraint on region") 148 | end_frame_idx = self.get_global_frame_idx(c.canonical_end_keyframe, frame_offset, time_function) 149 | else: 150 | print("no end keyframe defined") 151 | end_frame_idx = start_frame_idx + 1 152 | if c.orientation is None and self.force_orientation: 153 | c.orientation = self.generate_orientation_constraint_from_frame(frames[start_frame_idx], c.joint_name) 154 | 155 | base_ik_constraint = self.convert_mg_constraint_to_ik_constraint(start_frame_idx, c, constrain_orientation) 156 | 157 | relative_ik_constraint = None 158 | if base_ik_constraint is None: 159 | continue 160 | base_ik_constraint.src_tool_cos = c.src_tool_cos 161 | base_ik_constraint.dest_tool_cos = c.dest_tool_cos 162 | for frame_idx in range(start_frame_idx, end_frame_idx): 163 | ik_constraint = deepcopy(base_ik_constraint) 164 | if frame_idx not in ik_constraints: 165 | ik_constraints[frame_idx] = dict() 166 | 167 | ik_constraint.orientation = None 168 | if c.orientation is not None and constrain_orientation: 169 | if c.constrain_orientation_in_region: 170 | ik_constraint.orientation = c.orientation 171 | if start_frame_idx < frame_idx < end_frame_idx -1: 172 | ik_constraint.inside_region_orientation = True 173 | elif frame_idx == start_frame_idx: 174 | ik_constraint.inside_region_orientation = True 175 | ik_constraint.keep_orientation = True 176 | elif frame_idx == start_frame_idx: 177 | ik_constraint.orientation = c.orientation 178 | 179 | ik_constraint.position = None 180 | if c.position is not None: 181 | if c.constrain_position_in_region and c.relative_joint_name is None:# ignore region if it is relative 182 | ik_constraint.inside_region_position = False 183 | ik_constraint.position = c.position 184 | if start_frame_idx < frame_idx < end_frame_idx -1: 185 | ik_constraint.inside_region_position = True 186 | elif frame_idx == end_frame_idx-1: 187 | ik_constraint.end_of_region = True 188 | elif frame_idx == start_frame_idx: 189 | ik_constraint.position = c.position 190 | # overwrite with relative constraint if larger than start frame 191 | if c.relative_joint_name is not None and frame_idx == start_frame_idx: 192 | relative_ik_constraint = self.generate_relative_constraint(frame_idx, frames[frame_idx], 193 | c.joint_name, 194 | c.relative_joint_name) 195 | relative_ik_constraint.orientation = ik_constraint.orientation 196 | ik_constraints[frame_idx][c.joint_name] = ik_constraint 197 | 198 | elif relative_ik_constraint is not None and frame_idx > start_frame_idx: 199 | relative_ik_constraint.frame_idx = frame_idx 200 | ik_constraints[frame_idx][c.joint_name] = relative_ik_constraint 201 | else: 202 | ik_constraints[frame_idx][c.joint_name] = ik_constraint 203 | 204 | # add also a mirror constraint 205 | if c.mirror_joint_name is not None: 206 | _ik_constraint = self.generate_mirror_constraint(frame_idx, frames[0], frames[frame_idx], c.mirror_joint_name) 207 | ik_constraints[frame_idx][c.mirror_joint_name] = _ik_constraint 208 | 209 | # add also a parent constraint 210 | if c.constrained_parent is not None and c.vector_to_parent is not None: 211 | print("generate parent constraint", c.vector_to_parent) 212 | # "get length of vector and calculate constraint position" 213 | a = self.skeleton.nodes[c.joint_name].get_global_position(frames[0]) 214 | b = self.skeleton.nodes[c.constrained_parent].get_global_position(frames[0]) 215 | bone_length = np.linalg.norm(b-a) 216 | c.vector_to_parent /= np.linalg.norm(c.vector_to_parent) 217 | parent_pos = c.position + c.vector_to_parent * bone_length 218 | 219 | #create a keyframe constraint but indicate that it is a relative constraint 220 | _ik_constraint = KeyframeConstraint(frame_idx, c.constrained_parent, parent_pos, None, None) 221 | 222 | 223 | #remove degrees of freedom of the child constraint 224 | ik_constraints[frame_idx][c.joint_name].fk_chain_root = c.constrained_parent 225 | 226 | #change order so that the parent constraint is applied first 227 | # TODO order constraints based on joint order in fk chain 228 | reversed_ordered_dict = collections.OrderedDict() 229 | reversed_ordered_dict[c.constrained_parent] = _ik_constraint 230 | for k, v in ik_constraints[frame_idx].items(): 231 | reversed_ordered_dict[k] = v 232 | ik_constraints[frame_idx] = reversed_ordered_dict 233 | return ik_constraints 234 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/motion_filtering.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | from ..motion_vector import MotionVector 25 | 26 | 27 | def smooth(x, window_len=11,window='hanning'): 28 | """smooth the data using a window with requested size. 29 | http://scipy-cookbook.readthedocs.io/items/SignalSmooth.html 30 | 31 | This method is based on the convolution of a scaled window with the signal. 32 | The signal is prepared by introducing reflected copies of the signal 33 | (with the window size) in both ends so that transient parts are minimized 34 | in the begining and end part of the output signal. 35 | 36 | input: 37 | x: the input signal 38 | window_len: the dimension of the smoothing window; should be an odd integer 39 | window: the type of window from 'flat', 'hanning', 'hamming', 'bartlett', 'blackman' 40 | flat window will produce a moving average smoothing. 41 | 42 | output: 43 | the smoothed signal 44 | 45 | example: 46 | 47 | t=linspace(-2,2,0.1) 48 | x=sin(t)+randn(len(t))*0.1 49 | y=smooth(x) 50 | 51 | see also: 52 | 53 | numpy.hanning, numpy.hamming, numpy.bartlett, numpy.blackman, numpy.convolve 54 | scipy.signal.lfilter 55 | 56 | TODO: the window parameter could be the window itself if an array instead of a string 57 | NOTE: length(output) != length(input), to correct this: return y[(window_len/2-1):-(window_len/2)] instead of just y. 58 | """ 59 | 60 | if x.ndim != 1: 61 | raise (ValueError, "smooth only accepts 1 dimension arrays.") 62 | 63 | if x.size < window_len: 64 | raise (ValueError, "Input vector needs to be bigger than window size.") 65 | 66 | if window_len<3: 67 | return x 68 | 69 | if not window in ['flat', 'hanning', 'hamming', 'bartlett', 'blackman']: 70 | raise(ValueError, "Window is on of 'flat', 'hanning', 'hamming', 'bartlett', 'blackman'") 71 | 72 | 73 | s=np.r_[x[window_len-1:0:-1],x,x[-2:-window_len-1:-1]] 74 | #print(len(s)) 75 | if window == 'flat': #moving average 76 | w=np.ones(window_len,'d') 77 | else: 78 | w=eval('np.'+window+'(window_len)') 79 | 80 | y=np.convolve(w/w.sum(),s,mode='valid') 81 | return y 82 | 83 | 84 | def low_pass(src, alpha = 0.5): 85 | """ http://blog.thomnichols.org/2011/08/smoothing-sensor-data-with-a-low-pass-filter 86 | https://en.wikipedia.org/wiki/Low-pass_filter 87 | """ 88 | 89 | n_frames = len(src.frames) 90 | n_dims = len(src.frames[0]) 91 | new_frames = [src.frames[0]] 92 | for i in range(1, n_frames): 93 | new_f = [] 94 | for j in range(n_dims): 95 | v = new_frames[i-1][j] + alpha * (src.frames[i][j] - new_frames[i-1][j]) 96 | print("filter", v, src.frames[i][j]) 97 | new_f.append(v) 98 | new_frames.append(new_f) 99 | 100 | target_motion = MotionVector() 101 | target_motion.skeleton = src.skeleton 102 | target_motion.frames = np.array(new_frames) 103 | target_motion.n_frames = len(new_frames) 104 | return target_motion 105 | 106 | 107 | def moving_average(src, window=4): 108 | """ https://www.wavemetrics.com/products/igorpro/dataanalysis/signalprocessing/smoothing.htm#MovingAverage 109 | """ 110 | n_frames = len(src.frames) 111 | n_dims = len(src.frames[0]) 112 | new_frames = np.zeros(src.frames.shape) 113 | new_frames[0,:] = src.frames[0,:] 114 | hw = int(window/2) 115 | for i in range(1, n_frames): 116 | for j in range(n_dims): 117 | start = max(0, i-hw) 118 | end = min(n_frames-1, i+hw) 119 | w = end-start 120 | #print(i, j, start, end, w, n_frames, n_dims) 121 | #print("filter", v, src.frames[i, j]) 122 | new_frames[i, j] = np.sum(src.frames[start:end, j])/w 123 | target_motion = MotionVector() 124 | target_motion.skeleton = src.skeleton 125 | target_motion.frames = np.array(new_frames) 126 | print("new shape", target_motion.frames.shape) 127 | target_motion.n_frames = len(new_frames) 128 | return target_motion -------------------------------------------------------------------------------- /anim_utils/motion_editing/numerical_ik_exp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | """ Numerical IK using exponential displacement map based on Section 5.1 of [1] 24 | 25 | Use current configuration as q_0 26 | Find v such that (q_i = q_0_i exp(v_i) for 0 self.success_threshold and iter_counter < self.max_retries: 81 | r = minimize(self._objective, guess, args=(self.skeleton, reference, constraints, self.joint_weights), 82 | method=self.ik_settings["optimization_method"], 83 | tol=self.ik_settings["tolerance"], 84 | options=self._optimization_options) 85 | exp_frame = r["x"] 86 | exp_frame[:9] = 0 87 | error = self._objective(exp_frame, self.skeleton, reference, constraints, self.joint_weights) 88 | iter_counter += 1 89 | 90 | return exp_frame 91 | 92 | def modify_frame(self, reference, constraints): 93 | exp_frame = self.run(reference, constraints) 94 | d = convert_exp_frame_to_quat_frame(self.skeleton, exp_frame) 95 | q_frame = add_quat_frames(self.skeleton, reference, d) 96 | return q_frame 97 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/numerical_ik_quat.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | import time 25 | import numpy as np 26 | from scipy.optimize import minimize 27 | from ..utilities.log import write_log, write_message_to_log, LOG_MODE_DEBUG 28 | 29 | 30 | def obj_inverse_kinematics(s, data): 31 | pose, free_joints, target_joint, target_position, target_direction = data 32 | pose.set_channel_values(s, free_joints) 33 | if target_direction is not None: 34 | parent_joint = pose.get_parent_joint(target_joint) 35 | pose.point_in_direction(parent_joint, target_direction) 36 | position = pose.evaluate_position(target_joint) 37 | d = position - target_position 38 | return np.dot(d, d) 39 | 40 | 41 | class NumericalInverseKinematicsQuat(object): 42 | def __init__(self, skeleton_pose_model, ik_settings): 43 | self._ik_settings = ik_settings 44 | self.verbose = False 45 | self.pose = skeleton_pose_model 46 | self.success_threshold = self._ik_settings["success_threshold"] 47 | self.max_retries = self._ik_settings["max_retries"] 48 | self.optimize_orientation = self._ik_settings["optimize_orientation"] 49 | 50 | def _run_optimization(self, objective, initial_guess, data, cons=None): 51 | return minimize(objective, initial_guess, args=(data,), 52 | method=self._ik_settings["optimization_method"],#"SLSQP",#best result using L-BFGS-B 53 | constraints=cons, tol=self._ik_settings["tolerance"], 54 | options={'maxiter': self._ik_settings["max_iterations"], 'disp': self.verbose})#,'eps':1.0 55 | 56 | def modify_pose(self, joint_name, target, direction=None): 57 | error = 0.0 58 | if joint_name in list(self.pose.free_joints_map.keys()): 59 | free_joints = self.pose.free_joints_map[joint_name] 60 | error = self._modify_using_optimization(joint_name, target, free_joints, direction) 61 | return error 62 | 63 | def modify_pose_general(self, constraint): 64 | free_joints = constraint.free_joints 65 | initial_guess = self._extract_free_parameters(free_joints) 66 | data = constraint.data(self, free_joints) 67 | if self.verbose: 68 | self.pose.set_channel_values(initial_guess, free_joints) 69 | p = self.pose.evaluate_position(constraint.joint_name) 70 | write_message_to_log( 71 | "Start optimization for joint " + constraint.joint_name + " " + str(len(initial_guess)) 72 | + " " + str(len(free_joints)) + " " + str(p), LOG_MODE_DEBUG) 73 | cons = None 74 | #start = time.time() 75 | error = np.inf 76 | iter_counter = 0 77 | result = None 78 | while error > self.success_threshold and iter_counter < self.max_retries: 79 | result = self._run_optimization(constraint.evaluate, initial_guess, data, cons) 80 | error = constraint.evaluate(result["x"], data) 81 | iter_counter += 1 82 | # write_log("finished optimization in",time.time()-start,"seconds with error", error)#,result["x"].tolist(), initial_guess.tolist() 83 | if result is not None: 84 | self.pose.set_channel_values(result["x"], free_joints) 85 | return error 86 | 87 | def _extract_free_parameters(self, free_joints): 88 | """get parameters of joints from reference frame 89 | """ 90 | parameters = list() 91 | for joint_name in free_joints: 92 | parameters += self.pose.extract_parameters(joint_name).tolist() 93 | return np.asarray(parameters) 94 | 95 | def _extract_free_parameter_indices(self, free_joints): 96 | """get parameter indices of joints from reference frame 97 | """ 98 | indices = {} 99 | for joint_name in free_joints: 100 | indices[joint_name] = list(range(*self.pose.extract_parameters_indices(joint_name))) 101 | return indices 102 | 103 | def _modify_using_optimization(self, target_joint, target_position, free_joints, target_direction=None): 104 | initial_guess = self._extract_free_parameters(free_joints) 105 | data = self.pose, free_joints, target_joint, target_position, target_direction 106 | if self.verbose: 107 | write_message_to_log("Start optimization for joint " + target_joint + " " + str(len(initial_guess)) 108 | + " " + str(len(free_joints)), LOG_MODE_DEBUG) 109 | start = time.time() 110 | cons = None#self.pose.generate_constraints(free_joints) 111 | result = self._run_optimization(obj_inverse_kinematics, initial_guess, data, cons) 112 | position = self.pose.evaluate_position(target_joint) 113 | error = np.linalg.norm(position-target_position) 114 | if self.verbose: 115 | 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() 116 | self.pose.set_channel_values(result["x"], free_joints) 117 | return error 118 | 119 | def optimize_joint(self, objective, target_joint, target_position, target_orientation, free_joint): 120 | initial_guess = self.pose.extract_parameters(free_joint) # self._extract_free_parameters([free_joint]) 121 | data = self.pose, [free_joint], target_joint, target_position, None 122 | result = self._run_optimization(objective, initial_guess, data) 123 | # apply constraints here 124 | self.pose.apply_bounds(free_joint) 125 | return result["x"] 126 | 127 | def evaluate_delta(self, parameters, target_joint, target_position, free_joints): 128 | self.pose.set_channel_values(parameters, free_joints) 129 | position = self.pose.evaluate_position(target_joint) 130 | d = position - target_position 131 | return np.dot(d, d) 132 | -------------------------------------------------------------------------------- /anim_utils/motion_editing/skeleton_pose_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | from transformations import quaternion_matrix, quaternion_from_matrix 25 | from ..animation_data.quaternion_frame import convert_quaternion_frames_to_euler_frames, euler_to_quaternion, quaternion_to_euler 26 | from ..animation_data.utils import quaternion_from_vector_to_vector 27 | from ..animation_data.constants import LEN_QUAT, LEN_ROOT_POS 28 | 29 | 30 | def convert_euler_to_quat(euler_frame, joints): 31 | quat_frame = euler_frame[:3].tolist() 32 | offset = 3 33 | step = 3 34 | for joint in joints: 35 | e = euler_frame[offset:offset+step] 36 | q = euler_to_quaternion(e) 37 | quat_frame += list(q) 38 | offset += step 39 | return np.array(quat_frame) 40 | 41 | 42 | class SkeletonPoseModel(object): 43 | def __init__(self, skeleton, use_euler=False): 44 | self.channels = skeleton.get_channels() 45 | pose_parameters = skeleton.reference_frame 46 | self.skeleton = skeleton 47 | self.use_euler = use_euler 48 | if self.use_euler: 49 | self.pose_parameters = convert_quaternion_frames_to_euler_frames([pose_parameters])[0]#change to euler 50 | else: 51 | self.pose_parameters = pose_parameters 52 | self.n_channels = {} 53 | self.channels_start = {} 54 | self.types = {} 55 | self.modelled_joints = [] 56 | channel_idx = 0 57 | for joint, ch in list(self.channels.items()): 58 | self.channels_start[joint] = channel_idx 59 | self.n_channels[joint] = len(ch) 60 | if len(ch) == LEN_QUAT: 61 | self.types[joint] = "rot" 62 | elif len(ch) == LEN_ROOT_POS + LEN_QUAT: 63 | self.types[joint] = "trans" 64 | else: 65 | self.types[joint] = "rot" 66 | channel_idx += self.n_channels[joint] 67 | if self.n_channels[joint] > 0: 68 | self.modelled_joints.append(joint) 69 | self.free_joints_map = skeleton.free_joints_map 70 | if "head" in skeleton.skeleton_model: 71 | self.head_joint = skeleton.skeleton_model["joints"]["head"] 72 | if "neck" in skeleton.skeleton_model: 73 | self.neck_joint = skeleton.skeleton_model["joints"]["neck"] 74 | if "cos_map" in skeleton.skeleton_model: 75 | hand_joint = skeleton.skeleton_model["joints"]["right_wrist"] 76 | if hand_joint in skeleton.skeleton_model["cos_map"]: 77 | cos = skeleton.skeleton_model["cos_map"][hand_joint] 78 | self.relative_hand_dir = np.array(list(cos["y"]) + [0]) 79 | self.relative_hand_cross = np.array(list(cos["x"]) + [0]) 80 | up_vec = list(np.cross(cos["y"], cos["x"])) 81 | self.relative_hand_up = np.array(up_vec + [0]) 82 | else: 83 | self.relative_hand_dir = np.array([1.0, 0.0, 0.0, 0.0]) 84 | self.relative_hand_cross = np.array([0.0,1.0,0.0, 0.0]) 85 | self.relative_hand_up = np.array([0.0, 0.0, 1.0, 0.0]) 86 | if "relative_head_dir" in skeleton.skeleton_model.keys(): 87 | vec = list(skeleton.skeleton_model["relative_head_dir"]) 88 | self.relative_head_dir = np.array(vec+[0]) 89 | else: 90 | self.relative_head_dir = np.array([0.0, 0.0, 1.0, 0.0]) 91 | 92 | self.bounds = dict()#skeleton.bounds 93 | 94 | def set_pose_parameters(self, pose_parameters): 95 | self.pose_parameters = pose_parameters 96 | 97 | def set_channel_values(self, parameters, free_joints): 98 | p_idx = 0 99 | for joint_name in free_joints: 100 | n_channels = self.n_channels[joint_name] 101 | p_end = p_idx + n_channels 102 | f_start = self.channels_start[joint_name] 103 | f_end = f_start + n_channels 104 | #print f_start, f_end 105 | self.pose_parameters[f_start:f_end] = parameters[p_idx:p_end] 106 | p_idx += n_channels 107 | return self.pose_parameters 108 | 109 | def extract_parameters_indices(self, joint_name): 110 | f_start = self.channels_start[joint_name] 111 | f_end = f_start + self.n_channels[joint_name] 112 | return f_start, f_end 113 | 114 | def extract_parameters(self, joint_name): 115 | f_start, f_end = self.extract_parameters_indices(joint_name) 116 | return self.pose_parameters[f_start: f_end] 117 | 118 | def get_vector(self): 119 | if self.use_euler: 120 | return convert_euler_to_quat(self.pose_parameters, self.modelled_joints)#convert to quat 121 | else: 122 | return self.pose_parameters 123 | 124 | def evaluate_position_with_cache(self, target_joint, free_joints): 125 | for joint in free_joints: 126 | self.skeleton.nodes[joint].get_global_position(self.pose_parameters, use_cache=True) 127 | return self.skeleton.nodes[target_joint].get_global_position(self.pose_parameters, use_cache=True)#get_vector() 128 | 129 | def evaluate_position(self, target_joint): 130 | return self.skeleton.nodes[target_joint].get_global_position(self.pose_parameters) 131 | 132 | def evaluate_orientation(self, target_joint): 133 | return self.skeleton.nodes[target_joint].get_global_orientation_quaternion(self.pose_parameters) 134 | 135 | def evaluate_matrix(self, target_joint): 136 | return self.skeleton.nodes[target_joint].get_global_matrix(self.pose_parameters) 137 | 138 | def apply_bounds(self, free_joint): 139 | if free_joint in list(self.bounds.keys()): 140 | euler_angles = self.get_euler_angles(free_joint) 141 | for bound in self.bounds[free_joint]: 142 | self.apply_bound_on_joint(euler_angles, bound) 143 | 144 | if self.use_euler: 145 | self.set_channel_values(euler_angles, [free_joint]) 146 | else: 147 | q = euler_to_quaternion(euler_angles) 148 | self.set_channel_values(q, [free_joint]) 149 | return 150 | 151 | def apply_bound_on_joint(self, euler_angles, bound): 152 | if "min" in list(bound.keys()): 153 | euler_angles[bound["dim"]] = max(euler_angles[bound["dim"]],bound["min"]) 154 | if "max" in list(bound.keys()): 155 | euler_angles[bound["dim"]] = min(euler_angles[bound["dim"]],bound["max"]) 156 | 157 | def get_euler_angles(self, joint): 158 | if self.use_euler: 159 | euler_angles = self.extract_parameters(joint) 160 | else: 161 | q = self.extract_parameters(joint) 162 | euler_angles = quaternion_to_euler(q) 163 | return euler_angles 164 | 165 | def generate_constraints(self, free_joints): 166 | """ TODO add bounds on axis components of the quaternion according to 167 | Inverse Kinematics with Dual-Quaternions, Exponential-Maps, and Joint Limits by Ben Kenwright 168 | or try out euler based ik 169 | """ 170 | cons = [] 171 | idx = 0 172 | for joint_name in free_joints: 173 | if joint_name in list(self.bounds.keys()): 174 | start = idx 175 | for bound in self.bounds[joint_name]: 176 | if "min" in list(bound.keys()): 177 | cons.append(({"type": 'ineq', "fun": lambda x: x[start+bound["dim"]]-bound["min"]})) 178 | if "max" in list(bound.keys()): 179 | cons.append(({"type": 'ineq', "fun": lambda x: bound["max"]-x[start+bound["dim"]]})) 180 | idx += self.n_channels[joint_name] 181 | return tuple(cons) 182 | 183 | def clear_cache(self): 184 | self.skeleton.clear_cached_global_matrices() 185 | 186 | def lookat(self, point): 187 | head_position = self.evaluate_position(self.head_joint) 188 | target_dir = point - head_position 189 | target_dir /= np.linalg.norm(target_dir) 190 | head_direction = self.get_joint_direction(self.head_joint, self.relative_head_dir) 191 | delta_q = quaternion_from_vector_to_vector(head_direction, target_dir) 192 | delta_matrix = quaternion_matrix(delta_q) 193 | 194 | #delta*parent*old_local = parent*new_local 195 | #inv_parent*delta*parent*old_local = new_local 196 | parent_m = self.skeleton.nodes[self.neck_joint].get_global_matrix(self.pose_parameters, use_cache=False) 197 | old_local = np.dot(parent_m, self.skeleton.nodes[self.head_joint].get_local_matrix(self.pose_parameters)) 198 | m = np.dot(delta_matrix, old_local) 199 | new_local = np.dot(np.linalg.inv(parent_m),m) 200 | new_local_q = quaternion_from_matrix(new_local) 201 | self.set_channel_values(new_local_q, [self.head_joint]) 202 | 203 | def set_hand_orientation_using_direction_vectors(self, joint_name, orientation): 204 | m = quaternion_matrix(orientation) 205 | target_dir = np.dot(m, self.relative_hand_dir) 206 | target_dir /= np.linalg.norm(target_dir) 207 | cross_dir = np.dot(m, self.relative_hand_cross) 208 | cross_dir /= np.linalg.norm(cross_dir) 209 | self.point_in_direction(joint_name, target_dir[:3], cross_dir[:3]) 210 | 211 | def set_hand_orientation(self, joint_name, orientation): 212 | #print "set hand orientation" 213 | m = quaternion_matrix(orientation) 214 | parent_joint_name = self.get_parent_joint(joint_name) 215 | parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) 216 | local_m = np.dot(np.linalg.inv(parent_m), m) 217 | q = quaternion_from_matrix(local_m) 218 | self.set_channel_values(q, [joint_name]) 219 | 220 | 221 | def point_in_direction(self, joint_name, target_dir, target_cross=None): 222 | parent_joint_name = self.get_parent_joint(joint_name) 223 | joint_direction = self.get_joint_direction(joint_name, self.relative_hand_dir) 224 | delta_q = quaternion_from_vector_to_vector(joint_direction, target_dir) 225 | delta_matrix = quaternion_matrix(delta_q) 226 | #delta*parent*old_local = parent*new_local 227 | #inv_parent*delta*parent*old_local = new_local 228 | parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) 229 | old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) 230 | m = np.dot(delta_matrix, old_local) 231 | new_local = np.dot(np.linalg.inv(parent_m),m) 232 | new_local_q = quaternion_from_matrix(new_local) 233 | self.set_channel_values(new_local_q, [joint_name]) 234 | 235 | #rotate around orientation vector 236 | if target_cross is not None: 237 | joint_cross = self.get_joint_direction(joint_name, self.relative_hand_cross) 238 | delta_q = quaternion_from_vector_to_vector(joint_cross, target_cross) 239 | delta_matrix = quaternion_matrix(delta_q) 240 | parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) 241 | old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) 242 | m = np.dot(delta_matrix, old_local) 243 | new_local = np.dot(np.linalg.inv(parent_m),m) 244 | new_local_q = quaternion_from_matrix(new_local) 245 | self.point_hand_cross_dir_in_direction(joint_name, target_cross, parent_joint_name) 246 | 247 | def point_hand_cross_dir_in_direction(self,joint_name,target_cross,parent_joint_name): 248 | joint_cross = self.get_joint_direction(joint_name, self.relative_hand_cross) 249 | delta_q = quaternion_from_vector_to_vector(joint_cross, target_cross) 250 | delta_matrix = quaternion_matrix(delta_q) 251 | parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) 252 | old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) 253 | m = np.dot(delta_matrix, old_local) 254 | new_local = np.dot(np.linalg.inv(parent_m), m) 255 | new_local_q = quaternion_from_matrix(new_local) 256 | self.set_channel_values(new_local_q, [joint_name]) 257 | 258 | def point_hand_up_dir_in_direction(self, joint_name, target_up, parent_joint_name): 259 | joint_cross = self.get_joint_direction(joint_name, self.relative_hand_up) 260 | delta_q = quaternion_from_vector_to_vector(joint_cross, target_up) 261 | delta_matrix = quaternion_matrix(delta_q) 262 | parent_m = self.skeleton.nodes[parent_joint_name].get_global_matrix(self.pose_parameters, use_cache=False) 263 | old_local = np.dot(parent_m, self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters)) 264 | m = np.dot(delta_matrix, old_local) 265 | new_local = np.dot(np.linalg.inv(parent_m), m) 266 | new_local_q = quaternion_from_matrix(new_local) 267 | self.set_channel_values(new_local_q, [joint_name]) 268 | 269 | def set_joint_orientation(self, joint_name, target_q): 270 | global_q = self.skeleton.nodes[joint_name].get_global_orientation_quaternion(self.pose_parameters, use_cache=False) 271 | global_m = quaternion_matrix(global_q) 272 | target_m = quaternion_matrix(target_q) 273 | delta_m = np.linalg.inv(global_m)*target_m 274 | local_m = self.skeleton.nodes[joint_name].get_local_matrix(self.pose_parameters) 275 | new_local_m = np.dot(delta_m, local_m) 276 | new_local_q = quaternion_from_matrix(new_local_m) 277 | self.set_channel_values(new_local_q, [joint_name]) 278 | 279 | def get_joint_direction(self, joint_name, ref_vector): 280 | q = self.evaluate_orientation(joint_name) 281 | q /= np.linalg.norm(q) 282 | rotation_matrix = quaternion_matrix(q) 283 | vec = np.dot(rotation_matrix, ref_vector)[:3] 284 | return vec/np.linalg.norm(vec) 285 | 286 | def get_parent_joint(self, joint_name): 287 | if joint_name not in list(self.skeleton.parent_dict.keys()): 288 | return None 289 | return self.skeleton.parent_dict[joint_name] 290 | 291 | 292 | -------------------------------------------------------------------------------- /anim_utils/retargeting/__init__.py: -------------------------------------------------------------------------------- 1 | from .analytical import retarget_from_src_to_target, Retargeting, generate_joint_map 2 | from .constrained_retargeting import retarget_from_src_to_target as retarget_from_src_to_target_constrained 3 | from .point_cloud_retargeting import retarget_from_point_cloud_to_target 4 | from .constants import ROCKETBOX_TO_GAME_ENGINE_MAP, ADDITIONAL_ROTATION_MAP,GAME_ENGINE_TO_ROCKETBOX_MAP, ROCKETBOX_ROOT_OFFSET 5 | from .point_cloud_retargeting import PointCloudRetargeting 6 | -------------------------------------------------------------------------------- /anim_utils/retargeting/constants.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | 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] 25 | 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] 26 | ROCKETBOX_TO_GAME_ENGINE_MAP = dict() 27 | ROCKETBOX_TO_GAME_ENGINE_MAP["Hips"] = "Game_engine" 28 | ROCKETBOX_TO_GAME_ENGINE_MAP["Hips"] = "pelvis" 29 | ROCKETBOX_TO_GAME_ENGINE_MAP["Spine"] = "spine_01" 30 | ROCKETBOX_TO_GAME_ENGINE_MAP["Spine_1"] = "spine_02" 31 | ROCKETBOX_TO_GAME_ENGINE_MAP["LeftShoulder"] = "clavicle_l" 32 | ROCKETBOX_TO_GAME_ENGINE_MAP["RightShoulder"] = "clavicle_r" 33 | ROCKETBOX_TO_GAME_ENGINE_MAP["LeftArm"] = "upperarm_l" 34 | ROCKETBOX_TO_GAME_ENGINE_MAP["RightArm"] = "upperarm_r" 35 | ROCKETBOX_TO_GAME_ENGINE_MAP["LeftForeArm"] = "lowerarm_l" 36 | ROCKETBOX_TO_GAME_ENGINE_MAP["RightForeArm"] = "lowerarm_r" 37 | ROCKETBOX_TO_GAME_ENGINE_MAP["LeftHand"] = "hand_l" 38 | ROCKETBOX_TO_GAME_ENGINE_MAP["RightHand"] = "hand_r" 39 | ROCKETBOX_TO_GAME_ENGINE_MAP["LeftUpLeg"] = "thigh_l" 40 | ROCKETBOX_TO_GAME_ENGINE_MAP["RightUpLeg"] = "thigh_r" 41 | ROCKETBOX_TO_GAME_ENGINE_MAP["LeftLeg"] = "calf_l" 42 | ROCKETBOX_TO_GAME_ENGINE_MAP["RightLeg"] = "calf_r" 43 | ROCKETBOX_TO_GAME_ENGINE_MAP["LeftFoot"] = "foot_l" 44 | ROCKETBOX_TO_GAME_ENGINE_MAP["RightFoot"] = "foot_r" 45 | ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_L_Toe0"] = "ball_l" 46 | ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_R_Toe0"] = "ball_r" 47 | ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_L_Finger3"] = "middle_01_l" 48 | ROCKETBOX_TO_GAME_ENGINE_MAP["Bip01_R_Finger3"] = "middle_01_r" 49 | ROCKETBOX_TO_GAME_ENGINE_MAP["Head"] = "neck_01" 50 | GAME_ENGINE_TO_ROCKETBOX_MAP = {v:k for k,v in list(ROCKETBOX_TO_GAME_ENGINE_MAP.items())} 51 | ADDITIONAL_ROTATION_MAP = dict() 52 | ADDITIONAL_ROTATION_MAP["LeftShoulder"] = [0, 0, -20] 53 | ADDITIONAL_ROTATION_MAP["LeftArm"] = [0, 0, 20] 54 | ADDITIONAL_ROTATION_MAP["RightShoulder"] = [0, 0, 20] 55 | ADDITIONAL_ROTATION_MAP["RightArm"] = [0, 0, -20] 56 | OPENGL_UP_AXIS = np.array([0, 1, 0]) 57 | ROCKETBOX_ROOT_OFFSET = np.array([0, 100.949997, 0]) 58 | EXTRA_ROOT_NAME = "Root" 59 | ROOT_JOINT = "Hips" 60 | ROOT_CHILDREN = ["Spine", "LeftUpLeg","RightUpLeg"] 61 | EXTREMITIES = ["RightUpLeg", "LeftUpLeg", "RightLeg", "LeftLeg", "RightArm", "LeftArm", "RightForeArm", "LeftForeArm"] 62 | GAME_ENGINE_ROOT_JOINT = ROCKETBOX_TO_GAME_ENGINE_MAP[ROOT_JOINT] 63 | GAME_ENGINE_ROOT_CHILDREN = ["spine_01", "clavicle_l", "clavicle_r"]#[ROCKETBOX_TO_GAME_ENGINE_MAP[k] for k in ROOT_CHILDREN] 64 | GAME_ENGINE_EXTREMITIES = [ROCKETBOX_TO_GAME_ENGINE_MAP[k] for k in EXTREMITIES] 65 | GAME_ENGINE_SPINE_OFFSET_LIST = ["pelvis", "spine_01"] # list of bones that have an additional rotation offset 66 | AXES = [[1,0,0], [0,1,0], [0,0,1]] 67 | 68 | -------------------------------------------------------------------------------- /anim_utils/retargeting/constrained_retargeting.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | from transformations import quaternion_from_matrix, quaternion_matrix 25 | from .analytical import Retargeting, generate_joint_map, apply_additional_rotation_on_frames 26 | from ..motion_editing.hybrit_ik import HybritIK 27 | 28 | IK_SETTINGS = { 29 | "tolerance": 0.05, 30 | "optimization_method": "L-BFGS-B", 31 | "max_iterations": 1000, 32 | "interpolation_window": 120, 33 | "transition_window": 60, 34 | "use_euler_representation": False, 35 | "solving_method": "unconstrained", 36 | "activate_look_at": True, 37 | "max_retries": 5, 38 | "success_threshold": 5.0, 39 | "optimize_orientation": True, 40 | "elementary_action_max_iterations": 5, 41 | "elementary_action_optimization_eps": 1.0, 42 | "adapt_hands_during_carry_both": True, 43 | "constrain_place_orientation": False, 44 | } 45 | CONSTRAINED_JOINTS = ["left_wrist","right_wrist", "left_ankle", "right_ankle", "neck"] 46 | 47 | 48 | class KeyframeConstraint(object): 49 | def __init__(self, frame_idx, joint_name, position, orientation=None, look_at=False): 50 | self.frame_idx = frame_idx 51 | self.joint_name = joint_name 52 | self.position = position 53 | self.orientation = orientation 54 | self.look_at = look_at 55 | 56 | def evaluate(self, skeleton, frame): 57 | if self.orientation is not None: 58 | parent_joint = skeleton.nodes[self.joint_name].parent 59 | if parent_joint is not None: 60 | m = quaternion_matrix(self.orientation) 61 | parent_m = parent_joint.get_global_matrix(frame, use_cache=False) 62 | local_m = np.dot(np.linalg.inv(parent_m), m) 63 | q = quaternion_from_matrix(local_m) 64 | idx = skeleton.animated_joints.index(parent_joint.node_name) 65 | # idx = skeleton.nodes[c.joint_name].quaternion_frame_index * 4 66 | frame[idx:idx + 4] = q 67 | d = self.position - skeleton.nodes[self.joint_name].get_global_position(frame) 68 | return np.dot(d, d) 69 | 70 | 71 | class ConstrainedRetargeting(Retargeting): 72 | 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): 73 | Retargeting.__init__(self, src_skeleton, target_skeleton, target_to_src_joint_map, scale_factor, additional_rotation_map, constant_offset, place_on_ground, ground_height) 74 | src_joint_map = src_skeleton.skeleton_model["joints"] 75 | self.constrained_joints = [src_joint_map[j] for j in CONSTRAINED_JOINTS] 76 | self.ik = HybritIK(target_skeleton, IK_SETTINGS) 77 | target_joint_map = target_skeleton.skeleton_model["joints"] 78 | 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]) 79 | 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]) 80 | 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]) 81 | 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]) 82 | 83 | def generate_ik_constraints(self, frame): 84 | constraints = [] 85 | for j in self.constrained_joints: 86 | p = self.src_skeleton.nodes[j].get_global_position(frame) 87 | c = KeyframeConstraint(0,j, p) 88 | constraints.append(c) 89 | return constraints 90 | 91 | def run(self, src_frames, frame_range): 92 | n_frames = len(src_frames) 93 | target_frames = [] 94 | if n_frames > 0: 95 | if frame_range is None: 96 | frame_range = (0, n_frames) 97 | 98 | if self.additional_rotation_map is not None: 99 | src_frames = apply_additional_rotation_on_frames(self.src_skeleton.animated_joints, src_frames, self.additional_rotation_map) 100 | 101 | ref_frame = None 102 | for idx, src_frame in enumerate(src_frames[frame_range[0]:frame_range[1]]): 103 | print("retarget frame", idx) 104 | ik_constraints = self.generate_ik_constraints(src_frame) 105 | target_frame = self.retarget_frame(src_frame, ref_frame) 106 | target_frame = self.ik.modify_frame(target_frame, ik_constraints) 107 | if ref_frame is None: 108 | ref_frame = target_frame 109 | target_frames.append(target_frame) 110 | target_frames = np.array(target_frames) 111 | if self.place_on_ground: 112 | delta = target_frames[0][1] - self.ground_height 113 | target_frames[:,1] -= delta 114 | return target_frames 115 | 116 | 117 | def retarget_from_src_to_target(src_skeleton, target_skeleton, src_frames, joint_map=None, 118 | additional_rotation_map=None, scale_factor=1.0, frame_range=None, 119 | place_on_ground=False): 120 | if joint_map is None: 121 | joint_map = generate_joint_map(src_skeleton.skeleton_model, target_skeleton.skeleton_model) 122 | retargeting = ConstrainedRetargeting(src_skeleton, target_skeleton, joint_map, scale_factor, 123 | additional_rotation_map=additional_rotation_map, place_on_ground=place_on_ground) 124 | return retargeting.run(src_frames, frame_range) 125 | -------------------------------------------------------------------------------- /anim_utils/retargeting/utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | import numpy as np 24 | import math 25 | from transformations import quaternion_from_matrix, euler_matrix, quaternion_matrix, quaternion_multiply, euler_from_quaternion, quaternion_from_euler, quaternion_inverse, euler_from_matrix 26 | from ..animation_data.utils import quaternion_from_vector_to_vector 27 | 28 | AXES = [[1,0,0],[0,1,0],[0,0,1], [-1,0,0],[0,-1,0],[0,0,-1]] 29 | 30 | 31 | def get_angle_between_vectors(v1, v2): 32 | q = quaternion_from_vector_to_vector(v1, v2) 33 | v = q[1:] 34 | sin_theta = np.linalg.norm(v) 35 | sin_theta = min(sin_theta, 1.0) 36 | abs_angle = 2 * math.asin(sin_theta) 37 | return abs_angle 38 | 39 | 40 | def project_vector_on_axis(v): 41 | min_idx = -1 42 | min_angle = np.inf 43 | for idx, a in enumerate(AXES): 44 | angle = get_angle_between_vectors(v, a) 45 | if angle < min_angle: 46 | min_angle = angle 47 | min_idx = idx 48 | length = np.linalg.norm(v) 49 | return np.array(AXES[min_idx]) * length 50 | 51 | 52 | def align_root_translation(target_skeleton, target_frame, src_frame, root_node="pelvis", src_scale_factor=1.0): 53 | target_pos = target_skeleton.nodes[root_node].get_global_position(target_frame) 54 | target_pos = np.array([target_pos[0], target_pos[2]]) 55 | src_pos = np.array([src_frame[0], src_frame[2]])*src_scale_factor 56 | delta = src_pos - target_pos 57 | target_frame[0] += delta[0] 58 | target_frame[2] += delta[1] 59 | return target_frame 60 | 61 | 62 | def align_quaternion_frames(target_skeleton, frames): 63 | """align quaternions for blending 64 | src: http://physicsforgames.blogspot.de/2010/02/quaternions.html 65 | """ 66 | ref_frame = None 67 | new_frames = [] 68 | for frame in frames: 69 | if ref_frame is None: 70 | ref_frame = frame 71 | else: 72 | offset = 3 73 | for joint in target_skeleton.animated_joints: 74 | q = frame[offset:offset + 4] 75 | ref_q = ref_frame[offset:offset + 4] 76 | dot = np.dot(ref_q, q) 77 | if dot < 0: 78 | frame[offset:offset + 4] = -q 79 | offset += 4 80 | new_frames.append(frame) 81 | return new_frames 82 | 83 | 84 | def get_coordinate_system_axes(skeleton, joint_name, frame, axes): 85 | global_m = skeleton.nodes[joint_name].get_global_matrix(frame)[:3,:3] 86 | dirs = [] 87 | for axis in axes: 88 | direction = np.dot(global_m, axis) 89 | direction /= np.linalg.norm(direction) 90 | dirs.append(direction) 91 | return np.array(dirs) 92 | 93 | 94 | def rotate_axes(axes, q): 95 | m = quaternion_matrix(q)[:3, :3] 96 | aligned_axes = dict() 97 | for key, a in axes.items(): 98 | aligned_axes[key] = np.dot(m, a) 99 | aligned_axes[key] = normalize(aligned_axes[key]) 100 | return aligned_axes 101 | 102 | 103 | def align_axis(axes, key, new_vec): 104 | q = quaternion_from_vector_to_vector(axes[key], new_vec) 105 | aligned_axes = rotate_axes(axes, q) 106 | return q, aligned_axes 107 | 108 | 109 | def get_quaternion_rotation_by_name(joint_name, frame, skeleton, root_offset=3): 110 | assert joint_name in skeleton.animated_joints 111 | joint_index = skeleton.animated_joints.index(joint_name) 112 | return frame[joint_index * 4 + root_offset : (joint_index + 1) * 4 + root_offset] 113 | 114 | 115 | def normalize(v): 116 | return v/np.linalg.norm(v) 117 | 118 | 119 | def filter_dofs(q, fixed_dims): 120 | e = list(euler_from_quaternion(q)) 121 | for d in fixed_dims: 122 | e[d] = 0 123 | q = quaternion_from_euler(*e) 124 | return q 125 | 126 | 127 | def quaternion_from_axis_angle(axis, angle): 128 | q = [1,0,0,0] 129 | q[1] = axis[0] * math.sin(angle / 2) 130 | q[2] = axis[1] * math.sin(angle / 2) 131 | q[3] = axis[2] * math.sin(angle / 2) 132 | q[0] = math.cos(angle / 2) 133 | return normalize(q) 134 | 135 | 136 | def find_rotation_between_vectors(a, b): 137 | """http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another""" 138 | if np.array_equiv(a, b): 139 | return [1, 0, 0, 0] 140 | 141 | axis = normalize(np.cross(a, b)) 142 | dot = np.dot(a, b) 143 | if dot >= 1.0: 144 | return [1, 0, 0, 0] 145 | angle = math.acos(dot) 146 | q = quaternion_from_axis_angle(axis, angle) 147 | return q 148 | 149 | 150 | def to_local_cos_old(skeleton, node_name, frame, q): 151 | # bring into parent coordinate system 152 | pm = skeleton.nodes[node_name].get_global_matrix(frame)#[:3,:3] 153 | inv_pm = np.linalg.inv(pm) 154 | r = quaternion_matrix(q)[:3,:3] 155 | lr = np.dot(inv_pm, r)[:3,:3] 156 | q = quaternion_from_matrix(lr) 157 | return q 158 | 159 | 160 | def to_local_cos(skeleton, node_name, frame, q): 161 | # bring into parent coordinate system 162 | pm = skeleton.nodes[node_name].get_global_matrix(frame)#[:3,:3] 163 | inv_p = quaternion_inverse(quaternion_from_matrix(pm)) 164 | normalize(inv_p) 165 | return quaternion_multiply(inv_p, q) 166 | 167 | 168 | def to_global_cos(skeleton, node_name, frame, q): 169 | pm = skeleton.nodes[node_name].get_global_matrix(frame)#[:3,:3] 170 | r = quaternion_matrix(q)[:3, :3] 171 | lr = np.dot(pm, r)[:3, :3] 172 | q = quaternion_from_matrix(lr) 173 | return q 174 | 175 | 176 | def apply_additional_rotation_on_frames(animated_joints, frames, rotation_offsets): 177 | new_frames = [] 178 | for frame in frames: 179 | new_frame = frame[:] 180 | for idx, name in enumerate(animated_joints): 181 | if name in rotation_offsets: 182 | euler = np.radians(rotation_offsets[name]) 183 | additional_q = quaternion_from_euler(*euler) 184 | offset = idx * 4 + 3 185 | q = new_frame[offset:offset + 4] 186 | new_frame[offset:offset + 4] = quaternion_multiply(q, additional_q) 187 | 188 | new_frames.append(new_frame) 189 | return new_frames 190 | -------------------------------------------------------------------------------- /anim_utils/utilities/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eherr/anim_utils/0788e8a2256c873567e39c903843070e249ba94c/anim_utils/utilities/__init__.py -------------------------------------------------------------------------------- /anim_utils/utilities/io_helper_functions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | """ 24 | Created on Wed Feb 04 12:56:39 2015 25 | 26 | @author: Han Du, Martin Manns, Erik Herrmann 27 | """ 28 | import os 29 | import sys 30 | import glob 31 | import json 32 | import collections 33 | import math 34 | import numpy as np 35 | from datetime import datetime 36 | from ..animation_data.bvh import BVHWriter, BVHReader 37 | 38 | 39 | def load_json_file(filename, use_ordered_dict=True): 40 | """ Load a dictionary from a file 41 | 42 | Parameters 43 | ---------- 44 | * filename: string 45 | \tThe path to the saved json file. 46 | * use_ordered_dict: bool 47 | \tIf set to True dicts are read as OrderedDicts. 48 | """ 49 | tmp = None 50 | with open(filename, 'r') as infile: 51 | if use_ordered_dict: 52 | tmp = json.JSONDecoder( 53 | object_pairs_hook=collections.OrderedDict).decode( 54 | infile.read()) 55 | else: 56 | tmp = json.load(infile) 57 | infile.close() 58 | return tmp 59 | 60 | 61 | def write_to_json_file(filename, serializable, indent=4): 62 | with open(filename, 'w') as outfile: 63 | tmp = json.dumps(serializable, indent=4) 64 | outfile.write(tmp) 65 | outfile.close() 66 | 67 | 68 | def gen_file_paths(directory, mask='*mm.json'): 69 | """Generator of input file paths 70 | 71 | Parameters 72 | ---------- 73 | 74 | * dir: String 75 | \tPath of input folder, in which the input files reside 76 | * mask: String, defaults to '*.bvh' 77 | \tMask pattern for file search 78 | 79 | """ 80 | 81 | if not directory.endswith(os.sep): 82 | directory += os.sep 83 | 84 | for filepath in glob.glob(directory + mask): 85 | yield filepath 86 | 87 | 88 | def clean_path(path): 89 | path = path.replace('/', os.sep).replace('\\', os.sep) 90 | if os.sep == '\\' and '\\\\?\\' not in path: 91 | # fix for Windows 260 char limit 92 | relative_levels = len([directory for directory in path.split(os.sep) 93 | if directory == '..']) 94 | cwd = [ 95 | directory for directory in os.getcwd().split( 96 | os.sep)] if ':' not in path else [] 97 | path = '\\\\?\\' + os.sep.join(cwd[:len(cwd) - relative_levels] + [ 98 | directory for directory in path.split(os.sep) if directory != ''][relative_levels:]) 99 | return path 100 | 101 | 102 | def load_bvh_files_from_folder(data_folder): 103 | bvhfiles = glob.glob(os.path.join(data_folder, '*.bvh')) 104 | # order filenames alphabetically 105 | tmp = {} 106 | for item in bvhfiles: 107 | filename = os.path.split(item)[-1] 108 | bvhreader = BVHReader(item) 109 | tmp[filename] = bvhreader.frames 110 | motion_data = collections.OrderedDict(sorted(tmp.items())) 111 | return motion_data 112 | -------------------------------------------------------------------------------- /anim_utils/utilities/log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2019 DFKI GmbH. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the 7 | # "Software"), to deal in the Software without restriction, including 8 | # without limitation the rights to use, copy, modify, merge, publish, 9 | # distribute, sublicense, and/or sell copies of the Software, and to permit 10 | # persons to whom the Software is furnished to do so, subject to the 11 | # following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included 14 | # in all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN 19 | # NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 20 | # DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 21 | # OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 22 | # USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | LOG_MODE_ERROR = -1 24 | LOG_MODE_INFO = 1 25 | LOG_MODE_DEBUG = 2 26 | 27 | _lines = [] 28 | _active = True 29 | _mode = LOG_MODE_INFO 30 | 31 | def activate(): 32 | global _active 33 | _active = True 34 | 35 | 36 | def deactivate(): 37 | global _active 38 | _active = False 39 | 40 | def set_log_mode(mode): 41 | global _mode 42 | _mode = mode 43 | 44 | def write_log(*args): 45 | global _active 46 | global _lines 47 | if _active: 48 | line = " ".join(map(str, args)) 49 | print(line) 50 | _lines.append(line) 51 | 52 | 53 | def write_message_to_log(message, mode=LOG_MODE_INFO): 54 | global _active 55 | global _lines 56 | if _active and _mode >= mode: 57 | print(message) 58 | _lines.append(message) 59 | 60 | 61 | def save_log(filename): 62 | global _lines 63 | with open(filename, "wb") as outfile: 64 | for l in _lines: 65 | outfile.write(l+"\n") 66 | 67 | 68 | def clear_log(): 69 | global _lines 70 | _lines = [] 71 | -------------------------------------------------------------------------------- /examples/amc_to_bvh.py: -------------------------------------------------------------------------------- 1 | import os 2 | import argparse 3 | from pathlib import Path 4 | from anim_utils.animation_data import MotionVector, SkeletonBuilder 5 | from anim_utils.animation_data.acclaim import parse_asf_file, parse_amc_file 6 | from anim_utils.animation_data.bvh import write_euler_frames_to_bvh_file, convert_quaternion_to_euler_frames 7 | 8 | 9 | def load_skeleton(filename): 10 | asf = parse_asf_file(filename) 11 | skeleton = SkeletonBuilder().load_from_asf_data(asf) 12 | return skeleton 13 | 14 | 15 | def load_motion(filename, skeleton): 16 | amc_frames = parse_amc_file(filename) 17 | mv = MotionVector() 18 | mv.from_amc_data(skeleton, amc_frames) 19 | return mv 20 | 21 | def main(motion_dir,out_dir): 22 | amc_files = [] 23 | skeleton_file = None 24 | p = Path(motion_dir) 25 | for filename in p.iterdir(): 26 | if filename.suffix == ".amc": 27 | amc_files.append(filename) 28 | elif filename.suffix == ".asf": 29 | skeleton_file = filename 30 | if skeleton_file is None: 31 | return 32 | skeleton = load_skeleton(skeleton_file) 33 | os.makedirs(out_dir, exist_ok=True) 34 | for filename in amc_files: 35 | src_motion = load_motion(str(filename), skeleton) 36 | outfilename = out_dir + os.sep+filename.stem + ".bvh" 37 | frame_data = convert_quaternion_to_euler_frames(skeleton, src_motion.frames) 38 | print("write", outfilename) 39 | write_euler_frames_to_bvh_file(outfilename, skeleton, frame_data, src_motion.frame_time) 40 | 41 | if __name__ == "__main__": 42 | parser = argparse.ArgumentParser(description='Export.') 43 | parser.add_argument('motion_dir', nargs='?', help='directory of amc files') 44 | parser.add_argument('out_dir', nargs='?', help='out directory') 45 | args = parser.parse_args() 46 | if args.motion_dir is not None and args.out_dir is not None: 47 | main(args.motion_dir, args.out_dir) 48 | 49 | -------------------------------------------------------------------------------- /examples/data/bvh/mh_cmu_skeleton_pose.bvh: -------------------------------------------------------------------------------- 1 | HIERARCHY 2 | ROOT Hips 3 | { 4 | OFFSET 0.0 82.7767562866211 -3.42842698097229 5 | CHANNELS 6 Xposition Yposition Zposition Xrotation Yrotation Zrotation 6 | JOINT LHipJoint 7 | { 8 | OFFSET 0.0 0.0 0.0 9 | CHANNELS 3 Xrotation Yrotation Zrotation 10 | JOINT LeftUpLeg 11 | { 12 | OFFSET 1.3833840739607695e-08 9.52159583568573 6.661737828039804e-08 13 | CHANNELS 3 Xrotation Yrotation Zrotation 14 | JOINT LeftLeg 15 | { 16 | OFFSET -5.707910588625964e-08 37.121901512145996 1.8232933385320393e-07 17 | CHANNELS 3 Xrotation Yrotation Zrotation 18 | JOINT LeftFoot 19 | { 20 | OFFSET -2.8894096004705716e-08 37.903292179107666 6.046882994326097e-09 21 | CHANNELS 3 Xrotation Yrotation Zrotation 22 | JOINT LeftToeBase 23 | { 24 | OFFSET 2.6527413599097827e-08 13.004454374313354 -1.205770683299079e-07 25 | CHANNELS 3 Xrotation Yrotation Zrotation 26 | End Site 27 | { 28 | OFFSET 0.0 0.0 0.0 29 | } 30 | } 31 | } 32 | } 33 | } 34 | } 35 | JOINT LowerBack 36 | { 37 | OFFSET 0.0 8.534448742866516 -1.7118563278017973e-06 38 | CHANNELS 3 Xrotation Yrotation Zrotation 39 | JOINT Spine 40 | { 41 | OFFSET -6.293440239720671e-16 15.672223567962646 -5.30786294916652e-07 42 | CHANNELS 3 Xrotation Yrotation Zrotation 43 | JOINT Spine1 44 | { 45 | OFFSET -8.787226335679009e-16 15.124907493591309 1.9850716626024223e-08 46 | CHANNELS 3 Xrotation Yrotation Zrotation 47 | JOINT LeftShoulder 48 | { 49 | OFFSET 2.2699464857578278 7.339215278625488 1.5681582689285278 50 | CHANNELS 3 Xrotation Yrotation Zrotation 51 | JOINT LeftArm 52 | { 53 | OFFSET -2.069970861384718e-09 13.603790998458862 1.58716789711022e-10 54 | CHANNELS 3 Xrotation Yrotation Zrotation 55 | JOINT LeftForeArm 56 | { 57 | OFFSET 1.2936460791479476e-08 23.163669109344482 -2.6309954215264497e-08 58 | CHANNELS 3 Xrotation Yrotation Zrotation 59 | JOINT LeftHand 60 | { 61 | OFFSET 4.768468215843313e-06 21.078219413757324 8.336330736824493e-08 62 | CHANNELS 3 Xrotation Yrotation Zrotation 63 | JOINT LThumb 64 | { 65 | OFFSET 0.26190444827079773 3.5369667410850525 1.84941366314888 66 | CHANNELS 3 Xrotation Yrotation Zrotation 67 | End Site 68 | { 69 | OFFSET 0.0 0.0 0.0 70 | } 71 | } 72 | JOINT LeftFingerBase 73 | { 74 | OFFSET -4.723999609268503e-06 2.8848156332969666 4.762068073205228e-07 75 | CHANNELS 3 Xrotation Yrotation Zrotation 76 | JOINT LeftHandFinger1 77 | { 78 | OFFSET -2.75353812639878e-06 6.790903806686401 -3.88968601328088e-06 79 | CHANNELS 3 Xrotation Yrotation Zrotation 80 | End Site 81 | { 82 | OFFSET 0.0 0.0 0.0 83 | } 84 | } 85 | } 86 | } 87 | } 88 | } 89 | } 90 | JOINT Neck 91 | { 92 | OFFSET 0.0 14.310184717178345 -1.513166836275559e-07 93 | CHANNELS 3 Xrotation Yrotation Zrotation 94 | JOINT Neck1 95 | { 96 | OFFSET -5.1608641744361507e-14 5.616583824157715 5.539393832521e-08 97 | CHANNELS 3 Xrotation Yrotation Zrotation 98 | JOINT Head 99 | { 100 | OFFSET -1.4334930811821245e-12 5.616592764854431 -1.838854331026596e-06 101 | CHANNELS 3 Xrotation Yrotation Zrotation 102 | End Site 103 | { 104 | OFFSET 0.0 0.0 0.0 105 | } 106 | } 107 | } 108 | } 109 | JOINT RightShoulder 110 | { 111 | OFFSET -2.2699464857578278 7.339215278625488 1.5681582689285278 112 | CHANNELS 3 Xrotation Yrotation Zrotation 113 | JOINT RightArm 114 | { 115 | OFFSET 3.627677713780031e-08 13.603789806365967 1.1437942953884672e-09 116 | CHANNELS 3 Xrotation Yrotation Zrotation 117 | JOINT RightForeArm 118 | { 119 | OFFSET -2.7195401486324045e-08 23.16366672515869 -4.628752048319029e-08 120 | CHANNELS 3 Xrotation Yrotation Zrotation 121 | JOINT RightHand 122 | { 123 | OFFSET -1.327116194715927e-10 21.07820987701416 -1.3437972956609201e-08 124 | CHANNELS 3 Xrotation Yrotation Zrotation 125 | JOINT RThumb 126 | { 127 | OFFSET -0.26191262528300285 3.5369661450386047 1.849413514137268 128 | CHANNELS 3 Xrotation Yrotation Zrotation 129 | End Site 130 | { 131 | OFFSET 0.0 0.0 0.0 132 | } 133 | } 134 | JOINT RightFingerBase 135 | { 136 | OFFSET 4.731079172870523e-06 2.8848156332969666 4.4467967086347926e-07 137 | CHANNELS 3 Xrotation Yrotation Zrotation 138 | JOINT RightHandFinger1 139 | { 140 | OFFSET -2.6848201173379493e-06 6.790900230407715 3.930656191641901e-06 141 | CHANNELS 3 Xrotation Yrotation Zrotation 142 | End Site 143 | { 144 | OFFSET 0.0 0.0 0.0 145 | } 146 | } 147 | } 148 | } 149 | } 150 | } 151 | } 152 | } 153 | } 154 | } 155 | JOINT RHipJoint 156 | { 157 | OFFSET 0.0 0.0 0.0 158 | CHANNELS 3 Xrotation Yrotation Zrotation 159 | JOINT RightUpLeg 160 | { 161 | OFFSET -1.3833840739607695e-08 9.52159583568573 6.661737828039804e-08 162 | CHANNELS 3 Xrotation Yrotation Zrotation 163 | JOINT RightLeg 164 | { 165 | OFFSET -6.34770724872169e-08 37.121896743774414 2.5993788455025424e-07 166 | CHANNELS 3 Xrotation Yrotation Zrotation 167 | JOINT RightFoot 168 | { 169 | OFFSET 8.486544800234697e-09 37.90329694747925 -9.579663284853268e-08 170 | CHANNELS 3 Xrotation Yrotation Zrotation 171 | JOINT RightToeBase 172 | { 173 | OFFSET -6.957762366255338e-08 13.004448413848877 3.9166149434777253e-07 174 | CHANNELS 3 Xrotation Yrotation Zrotation 175 | End Site 176 | { 177 | OFFSET 0.0 0.0 0.0 178 | } 179 | } 180 | } 181 | } 182 | } 183 | } 184 | } 185 | 186 | MOTION 187 | Frames: 1 188 | Frame Time: 0.03333333333333333 189 | 0.0 0.0 0.0 -32.27821509955131 -0.0 -0.0 103.6654985504939 -5.371501992054073 -95.67629348130087 -163.07202685185752 75.4812052619931 -115.06387844004911 7.412837889086095 -0.6280809269458 1.2700263869782151 -71.91479551052174 3.782538074189336 -3.9907707566457336 -10.713498986625499 -0.29240576983295 -5.377361800435618 44.73105986928389 -1.2314678747238603e-09 -1.7024022899486208e-08 -11.924406944865304 -2.312625006985577e-09 3.455116446109691e-08 -5.502807242876798 1.6915838854038996e-09 -1.7558764259232047e-08 3.6165050097203166 -0.20083336510609334 -83.50405683770225 -0.9972586380965407 1.5072289843522337 -7.084814470980371 -0.5977967683724057 0.6474372798120615 0.7425440025521939 11.895972353808386 -6.388887317337185 -5.553647179504365 -153.3799885734933 10.576451540141349 -128.86234784536776 -1.1818968360983708 62.45487062337334 14.7109920491596 -6.437667231188217 -4.102374448447604 -25.071374075972823 26.755459580027633 5.338051885040417e-05 -7.933376847963722e-14 -1.395235020646198e-05 -9.804588378174592e-05 -1.2497638333798033e-11 -24.785457571891207 4.055096874577285e-05 1.8724688990796483e-05 3.6165050097203166 0.20083336510609334 83.50405683770225 -0.9972585375704511 -1.5072289727033144 7.084838335189063 -0.5978009502044934 -0.6474413307452646 -0.7425805998183256 11.895937837151305 6.388849727936702 5.553659978823617 -153.37972521347135 -10.578499503264693 128.85454077895406 -1.181683179040518 -62.45489950191313 -14.710836136827515 -6.437787447350281 4.102433203878838 25.07137816219318 103.6654985504939 5.371501992054073 95.67629348130087 -163.07202372707337 -75.48120845540153 115.06388499885017 7.412840473811435 0.6280807516378462 -1.2700319507716342 -71.91482890982178 -3.782542213898917 3.9907902010979814 -10.713132487873903 0.29241052268169754 5.377299312458633 190 | -------------------------------------------------------------------------------- /examples/data/models/mh_cmu.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "mh_cmu", 3 | "model": { 4 | "flip_x_axis": false, 5 | "foot_joints": [], 6 | "joints": { 7 | "right_clavicle": "RightShoulder", 8 | "right_wrist": "RightHand", 9 | "spine_1": "Spine", 10 | "left_knee": "LeftLeg", 11 | "right_hip": "RightUpLeg", 12 | "right_ankle": "RightFoot", 13 | "left_hip": "LeftUpLeg", 14 | "left_elbow": "LeftForeArm", 15 | "left_wrist": "LeftHand", 16 | "right_heel": null, 17 | "right_elbow": "RightForeArm", 18 | "left_heel": null, 19 | "neck": "Neck", 20 | "head": "Head", 21 | "left_ankle": "LeftFoot", 22 | "left_finger": "LeftHandFinger1", 23 | "left_shoulder": "LeftArm", 24 | "left_clavicle": "LeftShoulder", 25 | "right_finger": "RightHandFinger1", 26 | "pelvis": "Hips", 27 | "left_toe": "LeftToeBase", 28 | "spine_2": "Spine1", 29 | "right_shoulder": "RightArm", 30 | "right_knee": "RightLeg", 31 | "spine": "LowerBack", 32 | "root": null, 33 | "right_toe": "RightToeBase" 34 | }, 35 | "joint_constraints": {}, 36 | "foot_correction": { 37 | "y": 3, 38 | "x": -22 39 | }, 40 | "cos_map": { 41 | "RightToeBase_EndSite": { 42 | "y": [ 43 | 0.0, 44 | 1.0, 45 | 0.0 46 | ], 47 | "x": [ 48 | 1.0, 49 | 0.0, 50 | 0.0 51 | ] 52 | }, 53 | "RightToeBase": { 54 | "y": null, 55 | "x": null 56 | }, 57 | "LHipJoint": { 58 | "y": [ 59 | 4.362543993070427e-08, 60 | 0.9999999999999893, 61 | 1.3958775673323336e-07 62 | ], 63 | "x": [ 64 | 0.9999999999999991, 65 | -4.362543993070427e-08, 66 | -3.0447886482137315e-15 67 | ] 68 | }, 69 | "LeftHandFinger1_EndSite": { 70 | "y": [ 71 | 0.0, 72 | 1.0, 73 | 0.0 74 | ], 75 | "x": [ 76 | 1.0, 77 | 0.0, 78 | 0.0 79 | ] 80 | }, 81 | "LThumb_EndSite": { 82 | "y": [ 83 | 0.0, 84 | 1.0, 85 | 0.0 86 | ], 87 | "x": [ 88 | 1.0, 89 | 0.0, 90 | 0.0 91 | ] 92 | }, 93 | "RightHand": { 94 | "y": [ 95 | -5.232931096677443e-07, 96 | 0.99999999999986, 97 | -7.98291379250472e-08 98 | ], 99 | "x": [ 100 | 0.9999999999998631, 101 | 5.232931096677442e-07, 102 | -2.0887018913448053e-14 103 | ] 104 | }, 105 | "RightForeArm": { 106 | "y": [ 107 | 7.607400531515146e-08, 108 | 0.9999999999999956, 109 | 5.6498992024948e-08 110 | ], 111 | "x": [ 112 | 0.9999999999999971, 113 | -7.607400531515146e-08, 114 | -2.1490523098033015e-15 115 | ] 116 | }, 117 | "RightArm": { 118 | "y": [ 119 | 1.8625181741016823e-07, 120 | 0.9999999999999827, 121 | 9.084774498292877e-09 122 | ], 123 | "x": [ 124 | 0.9999999999999827, 125 | -1.8625181741016823e-07, 126 | -8.460278805343062e-16 127 | ] 128 | }, 129 | "RightFoot": { 130 | "y": [ 131 | -0.03741687788744992, 132 | 0.937149335139786, 133 | -0.3469165618649718 134 | ], 135 | "x": [ 136 | 0.9992772768043815, 137 | 0.03741687807554929, 138 | -0.006700843043897071 139 | ] 140 | }, 141 | "LeftToeBase_EndSite": { 142 | "y": [ 143 | 0.0, 144 | 1.0, 145 | 0.0 146 | ], 147 | "x": [ 148 | 1.0, 149 | 0.0, 150 | 0.0 151 | ] 152 | }, 153 | "LeftUpLeg": { 154 | "y": [ 155 | -3.678742363776819e-09, 156 | 1.0, 157 | 9.673151481344277e-10 158 | ], 159 | "x": [ 160 | 1.0, 161 | 3.678742363776819e-09, 162 | 1.7792516072825837e-18 163 | ] 164 | }, 165 | "RightHandFinger1": { 166 | "y": null, 167 | "x": null 168 | }, 169 | "Head_EndSite": { 170 | "y": [ 171 | 0.0, 172 | 1.0, 173 | 0.0 174 | ], 175 | "x": [ 176 | 1.0, 177 | 0.0, 178 | 0.0 179 | ] 180 | }, 181 | "LowerBack": { 182 | "y": [ 183 | 1.4608782157652188e-16, 184 | 0.9999999999999997, 185 | -2.7267598804179754e-08 186 | ], 187 | "x": [ 188 | 1.0, 189 | -1.4608782157652188e-16, 190 | 1.991732054462597e-24 191 | ] 192 | }, 193 | "RightHandFinger1_EndSite": { 194 | "y": [ 195 | 0.0, 196 | 1.0, 197 | 0.0 198 | ], 199 | "x": [ 200 | 1.0, 201 | 0.0, 202 | 0.0 203 | ] 204 | }, 205 | "RightLeg": { 206 | "y": [ 207 | -1.2521853546460741e-09, 208 | 1.0, 209 | -6.957585597929516e-10 210 | ], 211 | "x": [ 212 | 1.0, 213 | 1.2521853546460741e-09, 214 | -4.356093394711894e-19 215 | ] 216 | }, 217 | "LThumb": { 218 | "y": null, 219 | "x": null 220 | }, 221 | "LeftHandFinger1": { 222 | "y": null, 223 | "x": null 224 | }, 225 | "Head": { 226 | "y": null, 227 | "x": null 228 | }, 229 | "LeftToeBase": { 230 | "y": null, 231 | "x": null 232 | }, 233 | "Spine1": { 234 | "y": [ 235 | -1.500203549097681e-17, 236 | 0.9999999999999994, 237 | 3.478619031568733e-08 238 | ], 239 | "x": [ 240 | 1.0, 241 | 1.500203549097681e-17, 242 | 2.609318308559076e-25 243 | ] 244 | }, 245 | "LeftFoot": { 246 | "y": [ 247 | 0.037416903931746584, 248 | 0.9371491146576466, 249 | -0.346917154658791 250 | ], 251 | "x": [ 252 | 0.9992772757160086, 253 | -0.03741690411984657, 254 | 0.006700859920823079 255 | ] 256 | }, 257 | "Hips": { 258 | "y": [ 259 | -1.3970473314446654e-16, 260 | 0.9999999999999983, 261 | 5.866850720536422e-08 262 | ], 263 | "x": [ 264 | 1.0, 265 | 1.3970473314446652e-16, 266 | 4.0981340715548134e-24 267 | ] 268 | }, 269 | "LeftLeg": { 270 | "y": [ 271 | 1.2521853546460741e-09, 272 | 1.0, 273 | -6.957585597929516e-10 274 | ], 275 | "x": [ 276 | 1.0, 277 | -1.2521853546460741e-09, 278 | 4.356093394711894e-19 279 | ] 280 | }, 281 | "RightFingerBase": { 282 | "y": [ 283 | -2.5207580723192456e-07, 284 | 0.9999999999998989, 285 | 3.7263532598889865e-07 286 | ], 287 | "x": [ 288 | 0.9999999999999682, 289 | 2.5207580723192456e-07, 290 | 4.696617530089386e-14 291 | ] 292 | }, 293 | "LeftArm": { 294 | "y": [ 295 | -1.8625181741016823e-07, 296 | 0.9999999999999827, 297 | 9.084774498292877e-09 298 | ], 299 | "x": [ 300 | 0.9999999999999827, 301 | 1.8625181741016823e-07, 302 | 8.460278805343062e-16 303 | ] 304 | }, 305 | "LeftFingerBase": { 306 | "y": [ 307 | 2.5207580723192456e-07, 308 | 0.9999999999998989, 309 | 3.7263532598889865e-07 310 | ], 311 | "x": [ 312 | 0.9999999999999682, 313 | -2.5207580723192456e-07, 314 | -4.696617530089386e-14 315 | ] 316 | }, 317 | "Neck1": { 318 | "y": [ 319 | -3.4285399249251175e-16, 320 | 0.9999999999999504, 321 | 3.152734813720523e-07 322 | ], 323 | "x": [ 324 | 1.0, 325 | 3.428539924925117e-16, 326 | 5.404638590771215e-23 327 | ] 328 | }, 329 | "RThumb_EndSite": { 330 | "y": [ 331 | 0.0, 332 | 1.0, 333 | 0.0 334 | ], 335 | "x": [ 336 | 1.0, 337 | 0.0, 338 | 0.0 339 | ] 340 | }, 341 | "LeftShoulder": { 342 | "y": [ 343 | -8.627202448266553e-09, 344 | 1.0, 345 | 9.33536241132448e-10 346 | ], 347 | "x": [ 348 | 1.0, 349 | 8.627202448266553e-09, 350 | 4.0269030725217046e-18 351 | ] 352 | }, 353 | "LeftHand": { 354 | "y": [ 355 | 5.232931096677443e-07, 356 | 0.99999999999986, 357 | -7.98291379250472e-08 358 | ], 359 | "x": [ 360 | 0.9999999999998631, 361 | -5.232931096677442e-07, 362 | 2.0887018913448053e-14 363 | ] 364 | }, 365 | "Neck": { 366 | "y": [ 367 | -1.6329503983607474e-16, 368 | 0.9999999999999876, 369 | 1.5789258841609454e-07 370 | ], 371 | "x": [ 372 | 1.0, 373 | 1.6329503983607474e-16, 374 | 1.2891538257613636e-23 375 | ] 376 | }, 377 | "Spine": { 378 | "y": [ 379 | 4.431044720000334e-19, 380 | 1.0, 381 | -7.257104130473665e-10 382 | ], 383 | "x": [ 384 | 1.0, 385 | -4.431044720000334e-19, 386 | 1.6078276469913974e-28 387 | ] 388 | }, 389 | "LeftForeArm": { 390 | "y": [ 391 | -7.607400531515146e-08, 392 | 0.9999999999999956, 393 | 5.6498992024948e-08 394 | ], 395 | "x": [ 396 | 0.9999999999999971, 397 | 7.607400531515146e-08, 398 | 2.1490523098033015e-15 399 | ] 400 | }, 401 | "RightUpLeg": { 402 | "y": [ 403 | 3.678742363776819e-09, 404 | 1.0, 405 | 9.673151481344277e-10 406 | ], 407 | "x": [ 408 | 1.0, 409 | -3.678742363776819e-09, 410 | -1.7792516072825837e-18 411 | ] 412 | }, 413 | "RThumb": { 414 | "y": null, 415 | "x": null 416 | }, 417 | "RightShoulder": { 418 | "y": [ 419 | 8.627202448266553e-09, 420 | 1.0, 421 | 9.33536241132448e-10 422 | ], 423 | "x": [ 424 | 1.0, 425 | -8.627202448266553e-09, 426 | -4.0269030725217046e-18 427 | ] 428 | }, 429 | "RHipJoint": { 430 | "y": [ 431 | -4.35328545937373e-08, 432 | 0.9999999999999893, 433 | 1.3954151977193214e-07 434 | ], 435 | "x": [ 436 | 0.9999999999999991, 437 | 4.35328545937373e-08, 438 | 3.037320345010336e-15 439 | ] 440 | } 441 | }, 442 | "name": "mh_cmu" 443 | } 444 | } -------------------------------------------------------------------------------- /examples/run_retargeting.bat: -------------------------------------------------------------------------------- 1 | python run_retargeting.py data\bvh\mh_cmu_skeleton_pose.bvh mh_cmu data\bvh\custom_skeleton_walk_example.bvh custom out.bvh -------------------------------------------------------------------------------- /examples/run_retargeting.py: -------------------------------------------------------------------------------- 1 | import json 2 | import os 3 | import argparse 4 | from pathlib import Path 5 | from anim_utils.animation_data import BVHReader, MotionVector, SkeletonBuilder 6 | from anim_utils.retargeting.analytical import Retargeting, generate_joint_map 7 | from anim_utils.animation_data.bvh import write_euler_frames_to_bvh_file, convert_quaternion_to_euler_frames 8 | 9 | MODEL_DATA_PATH = "data"+os.sep+"models" 10 | 11 | def load_json_file(path): 12 | with open(path, "rt") as in_file: 13 | return json.load(in_file) 14 | 15 | def load_skeleton_model(skeleton_type): 16 | skeleton_model = dict() 17 | path = MODEL_DATA_PATH + os.sep+skeleton_type+".json" 18 | if os.path.isfile(path): 19 | data = load_json_file(path) 20 | skeleton_model = data["model"] 21 | else: 22 | print("Error: model unknown", path) 23 | return skeleton_model 24 | 25 | 26 | def load_skeleton(path, skeleton_type=None): 27 | bvh = BVHReader(path) 28 | skeleton = SkeletonBuilder().load_from_bvh(bvh) 29 | skeleton.skeleton_model = load_skeleton_model(skeleton_type) 30 | return skeleton 31 | 32 | 33 | def load_motion(path, skeleton_type=None): 34 | bvh = BVHReader(path) 35 | mv = MotionVector() 36 | mv.from_bvh_reader(bvh) 37 | skeleton = SkeletonBuilder().load_from_bvh(bvh) 38 | skeleton.skeleton_model = load_skeleton_model(skeleton_type) 39 | return skeleton, mv 40 | 41 | 42 | 43 | def main(src_motion_dir, src_skeleton_type, dest_skeleton, dest_skeleton_type, out_dir, auto_scale=False, place_on_ground=False): 44 | 45 | dest_skeleton = load_skeleton(dest_skeleton, dest_skeleton_type) 46 | min_p, max_p = dest_skeleton.get_bounding_box() 47 | dest_height = (max_p[1] - min_p[1]) 48 | os.makedirs(out_dir, exist_ok=True) 49 | p = Path(src_motion_dir) 50 | for filename in p.iterdir(): 51 | if filename.suffix != ".bvh": 52 | continue 53 | ground_height = 5.5 -1.8 + 0.4 #0#85 54 | ground_height = 5.5 -1.8 + 0.2 #0#85 55 | ground_height *= 0.01 56 | src_skeleton, src_motion = load_motion(filename, src_skeleton_type) 57 | src_scale = 1.0 58 | if auto_scale: 59 | min_p, max_p = src_skeleton.get_bounding_box() 60 | src_height = (max_p[1] - min_p[1]) 61 | src_scale = dest_height / src_height 62 | 63 | joint_map = generate_joint_map(src_skeleton.skeleton_model, dest_skeleton.skeleton_model) 64 | retargeting = Retargeting(src_skeleton, dest_skeleton, joint_map,src_scale, additional_rotation_map=None, place_on_ground=place_on_ground, ground_height=ground_height) 65 | 66 | new_frames = retargeting.run(src_motion.frames, frame_range=None) 67 | frame_data = convert_quaternion_to_euler_frames(dest_skeleton, new_frames) 68 | outfilename = out_dir + os.sep+filename.stem + ".bvh" 69 | print("write", outfilename, auto_scale, place_on_ground) 70 | write_euler_frames_to_bvh_file(outfilename, dest_skeleton, frame_data, src_motion.frame_time) 71 | 72 | 73 | 74 | 75 | 76 | if __name__ == "__main__": 77 | parser = argparse.ArgumentParser(description='Run retargeting.') 78 | parser.add_argument('dest_skeleton', nargs='?', help='BVH filename') 79 | parser.add_argument('dest_skeleton_type', nargs='?', help='skeleton model name') 80 | parser.add_argument('src_motion_dir', nargs='?', help='src BVH directory') 81 | parser.add_argument('src_skeleton_type', nargs='?', help='skeleton model name') 82 | parser.add_argument('out_dir', nargs='?', help='output BVH directory') 83 | parser.add_argument('--auto_scale', default=False, dest='auto_scale', action='store_true') 84 | parser.add_argument('--place_on_ground', default=False, dest='place_on_ground', action='store_true') 85 | args = parser.parse_args() 86 | if args.src_motion_dir is not None and args.dest_skeleton is not None and args.out_dir is not None: 87 | print(args.auto_scale) 88 | print(args.place_on_ground) 89 | main(args.src_motion_dir, args.src_skeleton_type, args.dest_skeleton, args.dest_skeleton_type, args.out_dir, bool(args.auto_scale), bool(args.place_on_ground)) 90 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | scipy 3 | matplotlib 4 | transformations 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from setuptools import setup, find_packages 4 | from os import path 5 | from io import open 6 | 7 | here = path.abspath(path.dirname(__file__)) 8 | 9 | with open(path.join(here, "README.md"), encoding="utf-8") as f: 10 | long_description = f.read() 11 | 12 | requirements = [] 13 | with open(path.join(here, "requirements.txt"), "r") as infile: 14 | requirements = [line for line in infile.read().split("\n") if line] 15 | 16 | setup( 17 | name="anim_utils", 18 | version="0.1", 19 | description="Skeleton Animation Utilities.", 20 | long_description=long_description, 21 | long_description_content_type="text/markdown", 22 | url="https://github.com/eherr/anim_utils", 23 | author="DFKI GmbH", 24 | license='MIT', 25 | keywords="skeleton animation data retargeting", 26 | packages=find_packages(exclude=("examples",)), 27 | python_requires=">=3.5.*, <4", 28 | install_requires=requirements, 29 | ) 30 | --------------------------------------------------------------------------------