├── blender └── fbx2bvh.py ├── model └── LMM │ ├── test.py │ ├── LMM.md │ ├── preprocessing.py │ ├── stepper.py │ ├── projector.py │ ├── decompressor.py │ ├── train.py │ └── setting.py ├── test ├── inverse_kinematics.py ├── calc_matching_time.py ├── bvh_viewer.py ├── plotting_cspace.py ├── plotting_global.py ├── character_controller.py ├── path_following.py └── plotting.py ├── figs ├── icon.png ├── cat_icon.png ├── ccd_ik.png ├── fabrik.png ├── sim_motion.gif ├── two_bone_ik.png └── sim_motion_sidestep.gif ├── configs ├── skel_smpl_neutral.npz └── DeepLearning │ └── LMM.toml ├── update.md ├── anitaichi ├── transform │ ├── ti_vec.py │ ├── vec.py │ ├── ti_quat.py │ └── quat.py └── animation │ ├── skel.py │ ├── anim.py │ └── anim_loader │ └── bvh.py ├── data └── data.md ├── anim ├── keyframe.py ├── blend.py ├── pose.py ├── aistpp.py ├── inverse_kinematics │ ├── jacobi_ik.py │ ├── ccd_ik.py │ ├── fabrik.py │ └── two_bone_ik.py ├── amass.py ├── motion_matching │ ├── database.py │ └── mm.py ├── smpl.py ├── skel.py ├── bvh.py └── animation.py ├── util ├── load.py ├── dualquat.py └── quat.py ├── todo.md ├── LICENSE ├── .vscode └── settings.json ├── .gitignore └── README.md /blender/fbx2bvh.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /model/LMM/test.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /test/inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /figs/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/figs/icon.png -------------------------------------------------------------------------------- /figs/cat_icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/figs/cat_icon.png -------------------------------------------------------------------------------- /figs/ccd_ik.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/figs/ccd_ik.png -------------------------------------------------------------------------------- /figs/fabrik.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/figs/fabrik.png -------------------------------------------------------------------------------- /figs/sim_motion.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/figs/sim_motion.gif -------------------------------------------------------------------------------- /figs/two_bone_ik.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/figs/two_bone_ik.png -------------------------------------------------------------------------------- /configs/skel_smpl_neutral.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/configs/skel_smpl_neutral.npz -------------------------------------------------------------------------------- /figs/sim_motion_sidestep.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KosukeFukazawa/CharacterAnimationTools/HEAD/figs/sim_motion_sidestep.gif -------------------------------------------------------------------------------- /update.md: -------------------------------------------------------------------------------- 1 | ## Update 1. 2 | 1. Add reading and writing of positions by bvh. 3 | 2. Specifies forward and vertical direction. 4 | 3. Add foot contact detection. -------------------------------------------------------------------------------- /model/LMM/LMM.md: -------------------------------------------------------------------------------- 1 |
2 | 3 | # Learned Motion Matching 4 | 5 | [**original repo**](https://github.com/orangeduck/Motion-Matching) 6 | [**paper**](https://dl.acm.org/doi/abs/10.1145/3386569.3392440) 7 | 8 |
9 | 10 | 11 | Unofficial JAX imprementation of LMM. If you want to train, simply run 12 | ```python 13 | python model/LMM/train.py 14 | ``` 15 | If you want to change experimental settings, please change settings on [`configs/DeepLearning/LMM.toml`](../../configs/DeepLearning/LMM.toml). -------------------------------------------------------------------------------- /model/LMM/preprocessing.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from pathlib import Path 4 | import pickle 5 | 6 | from anim import bvh 7 | from anim.animation import Animation 8 | from anim.motion_matching.mm import create_matching_features 9 | 10 | def preprocess_motion_data(BASEPATH: Path, cfg, save_dir: Path): 11 | dataset_dir = BASEPATH / cfg.dir 12 | anims = [] 13 | for file, start, end in zip(cfg.files, cfg.starts, cfg.ends): 14 | anims.append(bvh.load(dataset_dir / file, start, end)) 15 | -------------------------------------------------------------------------------- /anitaichi/transform/ti_vec.py: -------------------------------------------------------------------------------- 1 | import taichi as ti 2 | import taichi.math as tm 3 | 4 | @ti.func 5 | def cross(v, w): 6 | out0 = v.y * w.z - v.z * w.y 7 | out1 = v.z * w.x - v.x * w.z 8 | out2 = v.x * w.y - v.y * w.x 9 | return ti.Vector([out0, out1, out2]) 10 | 11 | @ti.func 12 | def dot(v, w): 13 | return ti.Vector([v.x * w.x, v.y * w.y, v.z * w.z]) 14 | 15 | @ti.func 16 | def length(v): 17 | return tm.sqrt(v.x * v.x + v.y + v.y + v.z + v.z) 18 | 19 | @ti.func 20 | def normalize(v): 21 | return tm.normalize(v) -------------------------------------------------------------------------------- /model/LMM/stepper.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import jax.numpy as jnp 4 | from flax import linen as nn 5 | 6 | class Stepper(nn.Module): 7 | """A simple MLP network.""" 8 | output_size: int 9 | hidden_size: int = 512 10 | 11 | @nn.compact 12 | def __call__(self, x): 13 | nbatch, nwindow = x.shape[:2] 14 | x = x.reshape([nbatch * nwindow, -1]) 15 | for _ in range(3): 16 | x = nn.Dense(self.hidden_size)(x) 17 | x = nn.elu(x) 18 | x = nn.Dense(self.output_size)(x) 19 | 20 | return x.reshape([nbatch, nwindow, -1]) -------------------------------------------------------------------------------- /data/data.md: -------------------------------------------------------------------------------- 1 | # data 2 | Please place your data here. 3 | In my case, I create symbolic links for datasets as shown below. 4 | 5 | ``` 6 | data/ 7 | |_ aistpp/ 8 | |_ amass/ 9 | |_ lafan1/ 10 | |_ smpl/ 11 | |_ female/ 12 | |_ model.pkl 13 | |_ male/ 14 | |_ model.pkl 15 | |_ neutral/ 16 | |_ model.pkl 17 | |_smplh/ 18 | |_ female/ 19 | |_ model.npz 20 | |_ male/ 21 | |_ model.npz 22 | |_ neutral/ 23 | |_ model.npz 24 | |_smplx/ 25 | |_ female/ 26 | |_ model.npz 27 | |_ male/ 28 | |_ model.npz 29 | |_ neutral/ 30 | |_ model.npz 31 | ``` -------------------------------------------------------------------------------- /model/LMM/projector.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import jax.numpy as jnp 4 | from flax import linen as nn 5 | 6 | class Projector(nn.Module): 7 | """A simple MLP network.""" 8 | output_size: int 9 | hidden_size: int = 512 10 | 11 | @nn.compact 12 | def __call__(self, x): 13 | nbatch, nwindow = x.shape[:2] 14 | x = x.reshape([nbatch * nwindow, -1]) 15 | for _ in range(3): 16 | x = nn.Dense(self.hidden_size)(x) 17 | x = nn.elu(x) 18 | x = nn.Dense(self.output_size)(x) 19 | 20 | return x.reshape([nbatch, nwindow, -1]) 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /anim/keyframe.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | import numpy as np 5 | 6 | from anim.skel import Skel 7 | 8 | @dataclass 9 | class KeyFrame: 10 | frame: int 11 | joint: str | int 12 | rotation: np.ndarray = None 13 | position: np.ndarray = None 14 | interp: str = "linear" # interpolation method. 15 | 16 | 17 | class KeyFrameAnimation: 18 | def __init__( 19 | self, 20 | skel: Skel, 21 | keys: list[KeyFrame], 22 | fps: int, 23 | anim_name: str=None, 24 | positions: np.ndarray=None, 25 | ) -> None: 26 | self.skel = skel 27 | # TBD -------------------------------------------------------------------------------- /anitaichi/transform/vec.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | 5 | # Calculate cross object of two 3D vectors. 6 | def cross(a, b): 7 | return np.concatenate([ 8 | a[...,1:2]*b[...,2:3] - a[...,2:3]*b[...,1:2], 9 | a[...,2:3]*b[...,0:1] - a[...,0:1]*b[...,2:3], 10 | a[...,0:1]*b[...,1:2] - a[...,1:2]*b[...,0:1]], axis=-1) 11 | 12 | def dot(a, b, keepdims=False): 13 | return np.sum(a*b, axis=-1, keepdims=keepdims) 14 | 15 | def length(v, keepdims=False): 16 | return np.linalg.norm(v, axis=-1, keepdims=keepdims) 17 | 18 | def normalize(v): 19 | lengths = length(v, keepdims=True) 20 | lengths = np.where(lengths==0, 1e-10, lengths) # avoid 0 divide 21 | return v / lengths 22 | 23 | -------------------------------------------------------------------------------- /util/load.py: -------------------------------------------------------------------------------- 1 | """ 2 | load.py 3 | """ 4 | from __future__ import annotations 5 | 6 | from pathlib import Path 7 | import toml, yaml, pickle, json 8 | from easydict import EasyDict 9 | 10 | def toml_load(path: Path): 11 | with open(path, "r") as f: 12 | cfg = toml.load(f) 13 | cfg = EasyDict(cfg) 14 | return cfg 15 | 16 | def yaml_load(path: Path): 17 | with open(path, "r") as f: 18 | cfg = yaml.load(f, Loader=yaml.Loader) 19 | cfg = EasyDict(cfg) 20 | return cfg 21 | 22 | def pickle_load(path: Path, encoding: str="ASCII"): 23 | with open(path, "rb") as f: 24 | cfg = pickle.load(f, encoding=encoding) 25 | return cfg 26 | 27 | def json_load(path: Path): 28 | with open(path, "r") as f: 29 | cfg = json.loads(f.read()) 30 | cfg = EasyDict(cfg) 31 | return cfg -------------------------------------------------------------------------------- /todo.md: -------------------------------------------------------------------------------- 1 | # TODO Works 2 | ## Motion Matching 3 | - [x] 実装したマッチングアルゴリズムが実時間上で動くことを確認(複数回) - 現状1回あたり0.33s 4 | - [ ] FAISSを用いたGPUマッチングの実装 - faiss-gpuがAnacondaからインストールできない？ 5 | - [ ] [`anim/pose.py`](anim/pose.py)に`Pose`クラスを実装。関節位置などを保存する入れ子とする。 6 | - [ ] 予め与えられたカーブからのアニメーション作成(オフライン、事前にsimulation boneの位置と向きを配列(T, pos + dir)に保存し、それに沿うようなアニメーションを作成する) 7 | - [ ] キーボード入力に対応 8 | - [ ] シミュレーションの作成(キーボードに対応させてsimulation boneを移動、回転) 9 | - [ ] 将来の軌道カーブの作成(キーボード入力から作成) 10 | - [ ] グラフィックスインターフェイスの用意(PyOpenGL or Taichi?) 11 | - [ ] InertializationやIKを無視した、単なるポーズ再生の切り替えで動くことを確認(リアルタイム) 12 | - [ ] ダンピングアルゴリズムの実装([`anim/blend.py`](anim/blend.py)) 13 | - [ ] FootIKを[`Animation`](anim/animation.py)に取り入れる 14 | - [ ] FootIKや Inertializationによる自然なアニメーションの作成(リアルタイム) 15 | - [ ] AMASSデータも使えるようにする 16 | 17 | ## その他 18 | - [ ] Motion Blendの完全な実装 19 | - [ ] Motion Graphsの作成(MMの機能を一部コピー) 20 | - [ ] FullbodyIKを[`Animation`](anim/animation.py)に取り入れる(Jacobian and FABRIK)。 21 | - [ ] Learned Motion Matchingの追実装 22 | - [ ] Crowd animationの方策を検討 23 | - [ ] LBSの実装 24 | -------------------------------------------------------------------------------- /model/LMM/decompressor.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import jax.numpy as jnp 4 | from flax import linen as nn 5 | 6 | class Compressor(nn.Module): 7 | """A simple MLP network.""" 8 | output_size: int 9 | hidden_size: int = 512 10 | 11 | @nn.compact 12 | def __call__(self, x): 13 | nbatch, nwindow = x.shape[:2] 14 | x = x.reshape([nbatch * nwindow, -1]) 15 | for _ in range(3): 16 | x = nn.Dense(self.hidden_size)(x) 17 | x = nn.elu(x) 18 | x = nn.Dense(self.output_size)(x) 19 | 20 | return x.reshape([nbatch, nwindow, -1]) 21 | 22 | 23 | class Decompressor(nn.Module): 24 | output_size: int 25 | hidden_size: int = 512 26 | 27 | @nn.compact 28 | def __call__(self, x): 29 | nbatch, nwindow = x.shape[:2] 30 | x = x.reshape([nbatch * nwindow, -1]) 31 | x = nn.Dense(self.hidden_size)(x) 32 | x = nn.elu(x) 33 | x = nn.Dense(self.output_size)(x) 34 | 35 | return x.reshape([nbatch, nwindow, -1]) 36 | 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Kosuke Fukazawa 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /anim/blend.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | 5 | from anim.animation import Animation 6 | from util import quat 7 | 8 | # ===================== 9 | # Basic interpolation 10 | # ===================== 11 | # Linear Interpolation (LERP) of objects. 12 | def lerp(x, y, t): 13 | return (1 - t) * x + t * y 14 | 15 | # LERP for quaternions. 16 | def quat_lerp(x: np.ndarray, y: np.ndarray, t: float) -> np.ndarray: 17 | assert x.shape == y.shape, "Two quats must be the same shape." 18 | return quat.normalize(lerp(x, y, t)) 19 | 20 | # Spherical linear interpolation (SLERP) for quaternions. 21 | def slerp(x: np.ndarray, y: np.ndarray, t: float) -> np.ndarray: 22 | assert x.shape == y.shape, "Two quats must be the same shape." 23 | if t == 0: 24 | return x 25 | elif t == 1: 26 | return y 27 | if quat.dot(x, y) < 0: 28 | y = - y 29 | ca = quat.dot(x, y) 30 | theta = np.arccos(np.clip(ca, 0, 1)) 31 | r = quat.normalize(y - x * ca) 32 | return x * np.cos(theta * t) + r * np.sin(theta * t) 33 | 34 | 35 | # ========= 36 | # Damping 37 | # ========= 38 | 39 | # predict `t` -------------------------------------------------------------------------------- /anim/pose.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | from util import quat 5 | from anim.skel import Skel 6 | 7 | class Pose: 8 | def __init__( 9 | self, 10 | skel: Skel, 11 | quats: np.ndarray, 12 | root_pos: np.ndarray, 13 | ) -> None: 14 | self.skel = skel 15 | self.quats = quats #[J,] 16 | self.root_pos = root_pos # [3,] 17 | 18 | def forward_kinematics(self): 19 | if not hasattr(self, "lpos"): 20 | offsets = self.skel.offsets 21 | parents = self.skel.parents 22 | offsets[0] = self.root_pos 23 | else: 24 | offsets = self.lpos 25 | return quat.fk(self.quats, offsets, parents) 26 | 27 | def set_local_positions(self): 28 | offsets = self.skel.offsets 29 | offsets[0] = self.root_pos 30 | self.lpos = offsets 31 | 32 | def set_global_positions(self): 33 | _, self.global_positions = self.forward_kinematics() 34 | 35 | def set_global_rotations(self): 36 | self.global_rotations, _ = self.forward_kinematics() 37 | 38 | def set_gpos_and_grot(self): 39 | self.global_rotations, self.global_positions = \ 40 | self.forward_kinematics() -------------------------------------------------------------------------------- /test/calc_matching_time.py: -------------------------------------------------------------------------------- 1 | # Calculate time for motion matching. 2 | import sys 3 | from pathlib import Path 4 | import timeit 5 | 6 | BASEPATH = Path(__file__).resolve().parent.parent 7 | sys.path.append(str(BASEPATH)) 8 | 9 | from anim import bvh 10 | from anim.motion_matching.database import Database 11 | from anim.motion_matching.mm import create_matching_database, motion_matching_search 12 | 13 | def create_database(bvh_dir: Path, files: list, starts: list, ends: list) -> Database: 14 | if isinstance(bvh_dir, str): 15 | bvh_dir = Path(bvh_dir) 16 | anims = [] 17 | for file, start, end in zip(files, starts, ends): 18 | anim = bvh.load(bvh_dir / file, start, end) 19 | anims.append(anim) 20 | return Database(anims) 21 | 22 | bvh_dir = BASEPATH / "data/lafan1" 23 | 24 | files = ["pushAndStumble1_subject5.bvh", "run1_subject5.bvh", "walk1_subject5.bvh"] 25 | starts = [194, 90, 80] 26 | ends = [351, 7086, 7791] 27 | 28 | matching_method = "kd-tree" 29 | 30 | db = create_database(bvh_dir, files, starts, ends) 31 | mdb = create_matching_database( 32 | db=db, method=matching_method, 33 | w_pos_foot=0.75, w_vel_foot=1, w_vel_hips=1, w_traj_pos=1, w_traj_dir=1.5, 34 | ignore_end=20, dense_bound_size=16, sparse_bound_size=64, 35 | ) 36 | 37 | cur_idx = 100 38 | query = mdb.features[mdb.indices.index(1320)] 39 | 40 | iter = 100 41 | score = timeit.timeit( 42 | "motion_matching_search(cur_idx, matching_method, mdb, query, False)", 43 | globals=globals(), 44 | number=iter, 45 | ) 46 | print(score / iter) -------------------------------------------------------------------------------- /anitaichi/transform/ti_quat.py: -------------------------------------------------------------------------------- 1 | import taichi as ti 2 | import taichi.math as tm 3 | 4 | from anitaichi.transform import ti_vec 5 | 6 | @ti.func 7 | def eye(): 8 | return ti.Vector([1.0, 0.0, 0.0, 0.0]) 9 | 10 | @ti.func 11 | def normalize(q): 12 | return tm.normalize(q) 13 | 14 | @ti.func 15 | def inv(q): 16 | return ti.Vector([-q[0], q[1], q[2], q[3]]) 17 | 18 | @ti.func 19 | def abs(q): 20 | return -q if q[0] < 0.0 else q 21 | 22 | @ti.func 23 | def mul(x, y): 24 | qout0 = y[0] * x[0] - y[1] * x[1] - y[2] * x[2] - y[3] * x[3] 25 | qout1 = y[0] * x[1] + y[1] * x[0] - y[2] * x[3] + y[3] * x[2] 26 | qout2 = y[0] * x[2] + y[1] * x[3] + y[2] * x[0] - y[3] * x[1] 27 | qout3 = y[0] * x[3] - y[1] * x[2] + y[2] * x[1] + y[3] * x[0] 28 | return ti.Vector([qout0, qout1, qout2, qout3]) 29 | 30 | @ti.func 31 | def mul_vec3(q, v): 32 | v_quat = ti.Vector([q[1], q[2], q[3]]) 33 | t = 2.0 * ti_vec.cross(v_quat, v) 34 | return v + q[0] * t + ti_vec.cross(v_quat, t) 35 | 36 | @ti.func 37 | def from_angle_and_axis(angle, axis): 38 | c = tm.cos(angle / 2.0) 39 | s = tm.sin(angle / 2.0) 40 | return ti.Vector([c, s * axis.x, s * axis.y, s * axis.z]) 41 | 42 | @ti.func 43 | def from_euler(euler, order): 44 | axis = { 45 | "x": ti.Vector([1.0, 0.0, 0.0]), 46 | "y": ti.Vector([0.0, 1.0, 0.0]), 47 | "z": ti.Vector([0.0, 0.0, 1.0]) 48 | } 49 | q0 = from_angle_and_axis(euler[0], axis[order[0]]) 50 | q1 = from_angle_and_axis(euler[1], axis[order[1]]) 51 | q2 = from_angle_and_axis(euler[2], axis[order[2]]) 52 | 53 | return mul(q0, mul(q1, q2)) 54 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | // "python.linting.mypyEnabled": true, 3 | 4 | "files.associations": { 5 | "__bit_reference": "cpp", 6 | "__bits": "cpp", 7 | "__config": "cpp", 8 | "__debug": "cpp", 9 | "__errc": "cpp", 10 | "__locale": "cpp", 11 | "__mutex_base": "cpp", 12 | "__nullptr": "cpp", 13 | "__split_buffer": "cpp", 14 | "__string": "cpp", 15 | "__threading_support": "cpp", 16 | "__tuple": "cpp", 17 | "atomic": "cpp", 18 | "bitset": "cpp", 19 | "cctype": "cpp", 20 | "chrono": "cpp", 21 | "clocale": "cpp", 22 | "compare": "cpp", 23 | "concepts": "cpp", 24 | "cstdarg": "cpp", 25 | "cstddef": "cpp", 26 | "cstdint": "cpp", 27 | "cstdio": "cpp", 28 | "cstdlib": "cpp", 29 | "cstring": "cpp", 30 | "ctime": "cpp", 31 | "cwchar": "cpp", 32 | "cwctype": "cpp", 33 | "exception": "cpp", 34 | "initializer_list": "cpp", 35 | "ios": "cpp", 36 | "iosfwd": "cpp", 37 | "iostream": "cpp", 38 | "istream": "cpp", 39 | "limits": "cpp", 40 | "locale": "cpp", 41 | "memory": "cpp", 42 | "mutex": "cpp", 43 | "new": "cpp", 44 | "ostream": "cpp", 45 | "ratio": "cpp", 46 | "stdexcept": "cpp", 47 | "streambuf": "cpp", 48 | "string": "cpp", 49 | "string_view": "cpp", 50 | "system_error": "cpp", 51 | "tuple": "cpp", 52 | "type_traits": "cpp", 53 | "typeinfo": "cpp", 54 | "variant": "cpp", 55 | "vector": "cpp", 56 | "array": "cpp", 57 | "list": "cpp" 58 | } 59 | } -------------------------------------------------------------------------------- /test/bvh_viewer.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | 6 | import numpy as np 7 | import taichi as ti 8 | 9 | BASEPATH = Path(__file__).resolve().parent.parent 10 | sys.path.append(str(BASEPATH)) 11 | 12 | from anitaichi.animation.anim_loader import bvh 13 | 14 | @ti.kernel 15 | def get_frame( 16 | gposs: ti.types.ndarray(), 17 | cur_pose: ti.template(), 18 | frame: int 19 | ): 20 | for i in cur_pose: 21 | cur_pose[i] = gposs[frame, i] / 100 22 | 23 | def update_camera(camera, anim, frame): 24 | root_pos = anim.trans[frame] / 100 25 | camera_pos = root_pos + np.array([0, 0, -10]) 26 | 27 | camera.position(*camera_pos) 28 | camera.lookat(*root_pos) 29 | scene.set_camera(camera) 30 | 31 | if __name__ == "__main__": 32 | # Starting taichi on CUDA. 33 | ti.init(arch=ti.cuda) 34 | 35 | window = ti.ui.Window("bvh viewer", (1024, 1024), vsync=True) 36 | gui = window.get_gui() 37 | canvas = window.get_canvas() 38 | canvas.set_background_color((1, 1, 1)) 39 | scene = ti.ui.Scene() 40 | camera = ti.ui.Camera() 41 | 42 | bvh_file = BASEPATH / "data/aiming1_subject1.bvh" 43 | anim = bvh.load(bvh_file) 44 | anim.ti_fk() 45 | cur_pose = ti.Vector.field(3, dtype=float, shape=len(anim.skel)) 46 | frame = 0 47 | 48 | while window.running: 49 | get_frame(anim.global_positions_field, cur_pose, frame) 50 | scene.particles(cur_pose, radius=0.05, color=(1.0, 0.0, 0.0)) 51 | scene.ambient_light((0.5, 0.5, 0.5)) 52 | update_camera(camera, anim, frame) 53 | 54 | frame += 1 55 | if frame == len(anim): 56 | frame = 0 57 | 58 | canvas.scene(scene) 59 | window.show() -------------------------------------------------------------------------------- /anitaichi/animation/skel.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | import taichi as ti 5 | 6 | class Joint: 7 | def __init__(self, name: str, index: int, parent: int, offset: list[float], dof:int =None, root: bool=None): 8 | self.name = name 9 | self.index = index 10 | self.parent = parent 11 | self.offset = offset 12 | if dof != None: 13 | self.dof = dof 14 | else: self.dof = 6 if (parent==-1) else 3 15 | if root != None: 16 | self.root = root 17 | self.root: bool = (parent==-1) 18 | 19 | class Skel: 20 | def __init__(self, joints: list[Joint], skel_name: str=None): 21 | self.joints = joints 22 | self.num_joints = len(joints) 23 | self.skel_name = skel_name 24 | 25 | def __len__(self): 26 | return self.num_joints 27 | 28 | # ================== 29 | # Read only property 30 | # ================== 31 | @property 32 | def offsets(self) -> np.ndarray: # For numpy array 33 | offsets = [] 34 | for j in self.joints: 35 | offsets.append(j.offset) 36 | return np.array(offsets) 37 | 38 | @property 39 | def offsets_field(self): # For Taichi field 40 | offsets = ti.Vector.field(3, dtype=float, shape=(self.num_joints,)) 41 | for i in range(len(self.joints)): 42 | offsets[i] = self.joints[i].offset 43 | return offsets 44 | 45 | @property 46 | def parents(self) -> list[int]: 47 | return [j.parent for j in self.joints] 48 | 49 | @property 50 | def parents_field(self): 51 | parents_idx_field = ti.field(int, shape=len(self.joints)) 52 | parents_idx = [joint.parent for joint in self.joints] 53 | for i in range(len(self.joints)): 54 | parents_idx_field[i] = parents_idx[i] 55 | return parents_idx_field -------------------------------------------------------------------------------- /model/LMM/train.py: -------------------------------------------------------------------------------- 1 | """ 2 | train.py 3 | You can define your configulation file like below. 4 | ``` 5 | python train.py configs/DeepLearning/LMM.toml 6 | ``` 7 | """ 8 | from __future__ import annotations 9 | 10 | import sys 11 | from pathlib import Path 12 | import numpy as np 13 | import jax.numpy as jnp 14 | from jax import jit, partial, random 15 | from flax import linen as nn 16 | from flax.training import train_state 17 | import optax 18 | 19 | BASEPATH = Path(__file__).resolve().parent.parent.parent 20 | sys.path.append(str(BASEPATH)) 21 | 22 | from model.LMM.setting import Config 23 | from model.LMM.decompressor import Compressor, Decompressor 24 | from model.LMM.stepper import Stepper 25 | from model.LMM.projector import Projector 26 | 27 | def main(cfg: Config): 28 | ckpt_dir = cfg.ckpt_dir 29 | 30 | match cfg.setting.train_model: 31 | case "all": 32 | sys.stdout.write("Train models: decompressor, stepper, projector\n") 33 | train_decompressor(cfg.setting.decompressor, ckpt_dir) 34 | train_stepper(cfg.setting.stepper, ckpt_dir) 35 | train_projector(cfg.setting.projector, ckpt_dir) 36 | case "decompressor": 37 | sys.stdout.write("Train model: decompressor\n") 38 | train_decompressor(cfg.setting.decompressor, ckpt_dir) 39 | case "stepper": 40 | sys.stdout.write("Train model: stepper\n") 41 | train_stepper(cfg.setting.stepper, ckpt_dir) 42 | case "projector": 43 | sys.stdout.write("Train model: projector\n") 44 | train_projector(cfg.setting.projector, ckpt_dir) 45 | case _: 46 | raise ValueError("Invalid value. Please check LMM.toml.") 47 | 48 | def train_decompressor(cfg, ckpt_dir): 49 | pass 50 | 51 | def train_stepper(cfg, ckpt_dir): 52 | pass 53 | 54 | def train_projector(cfg, ckpt_dir): 55 | pass 56 | 57 | if __name__ == "__main__": 58 | setting_path = sys.argv[1] 59 | cfg = Config(BASEPATH, setting_path) 60 | 61 | main(cfg) -------------------------------------------------------------------------------- /util/dualquat.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | from util import quat 5 | 6 | def eye(shape: list[int], dtype=np.float32) -> np.ndarray: 7 | return np.ones(list(shape) + [8], dtype=dtype) * \ 8 | np.asarray([1, 0, 0, 0, 0, 0, 0, 0], dtype=dtype) 9 | 10 | def normalize(dq: np.ndarray, eps=1e-8) -> np.ndarray: 11 | mag = quat.length(dq[...,:4]) 12 | mag = np.where(mag==0, eps, mag) 13 | return dq / mag[None] 14 | 15 | def abs(dq: np.ndarray) -> np.ndarray: 16 | real = np.where(dq[...,0:1] > 0.0, dq[...,:4], -dq[...,:4]) 17 | dual = np.where(dq[...,4:5] > 0.0, dq[...,4:], -dq[...,4:]) 18 | return np.concatenate([real, dual], axis=-1) 19 | 20 | def inv(dq: np.ndarray) -> np.ndarray: 21 | real = quat.inv(dq[...,:4]) 22 | dual = -quat.mul_inv(quat.inv_mul(dq[...,:4], dq[...,4:]), dq[...,:4]) 23 | return np.concatenate([real, dual], axis=-1) 24 | 25 | def mul(x: np.ndarray, y: np.ndarray) -> np.ndarray: 26 | real = quat.mul(x[...,:4], y[...,:4]) 27 | dual = quat.mul(x[...,:4], y[...,4:]) + quat.mul(x[...,4:], y[...,:4]) 28 | return np.concatenate([real, dual], axis=-1) 29 | 30 | def from_trans(trans: np.ndarray) -> np.ndarray: 31 | dual = np.zeros(trans.shape[:-1] + (4,)) 32 | dual[...,1:] = trans * 0.5 33 | return np.concatenate([quat.eye(trans.shape[:,-1]), dual], axis=-1) 34 | 35 | def from_rot(rot: np.ndarray) -> np.ndarray: 36 | return np.concatenate([rot, np.zeros(rot.shape[:-1] + (4,))], axis=-1) 37 | 38 | def from_rot_and_trans(rot: np.ndarray, trans: np.ndarray) -> np.ndarray: 39 | rot = quat.normalize(rot) 40 | dual = np.zeros(trans.shape[:-1] + (4,)) 41 | dual[...,1:] = trans 42 | dual = quat.mul(dual, rot) * 0.5 43 | return np.concatenate([rot, dual], axis=-1) 44 | 45 | def to_trans(dq: np.ndarray) -> np.ndarray: 46 | return 2 * quat.mul_inv(dq[...,4:], dq[...,:4])[...,1:] 47 | 48 | def to_rot(dq: np.ndarray) -> np.ndarray: 49 | return dq[...,:4] 50 | 51 | def fk(dq: np.ndarray, parents: list[int]) -> np.ndarray: 52 | gdq = [dq[...,:1,:]] 53 | for i in range(1, len(parents)): 54 | gdq.append(mul(gdq[parents[i]], dq[...,i:i+1,:])) 55 | return np.concatenate(gdq, axis=-2) -------------------------------------------------------------------------------- /configs/DeepLearning/LMM.toml: -------------------------------------------------------------------------------- 1 | # Common settings 2 | exp_name = "original" 3 | 4 | # device settings 5 | device = "cuda" 6 | gpus = [0] 7 | 8 | # which model do you train? ["all", "decompressor", "stepper", "projector"] 9 | train_model = "all" 10 | 11 | # if you need preprocess (if false, load from processed_dir) 12 | need_preprocess = true 13 | # processed data dir 14 | processed_dir = "model/LMM/preprocessed" 15 | processed_file_name = "dataset.pkl" 16 | 17 | # saved files settings 18 | checkpoint_dir = "model/LMM/checkpoints" 19 | save_config_dir = "model/LMM/configs" 20 | 21 | # random seed 22 | seed = 0 23 | 24 | # Dataset settings 25 | [dataset] 26 | # dataset dir 27 | dir = "data/lafan1" 28 | files = ["pushAndStumble1_subject5.bvh", "run1_subject5.bvh", "walk1_subject5.bvh"] 29 | starts = [194, 90, 80] 30 | ends = [351, 7086, 7791] 31 | ignore_end = 20 32 | # matching features weights. (for decompressor) 33 | # TBD: Specify feature that is used. 34 | # [w_pos_foot, w_vel_foot, w_vel_hips, w_traj_pos, w_traj_dir] 35 | feat_weights = [0.75, 1.0, 1.0, 1.0, 1.5] 36 | [dataset.train] 37 | [dataset.val] 38 | [dataset.test] 39 | 40 | # Model settings 41 | [decompressor] 42 | model = "mlp" 43 | [decompressor.train] 44 | max_iter = 300000 45 | log_freq = 100 46 | save_freq = 50000 47 | 48 | num_epochs = 10 49 | batch_size = 32 50 | [decompressor.train.loss] 51 | losses = ["mse"] 52 | weights = [1.0] 53 | [decompressor.train.optim] 54 | method = "radam" 55 | lr = 0.001 56 | momentum = 0.9 57 | betas = [0.5, 0.999] 58 | 59 | [stepper] 60 | model = "mlp" 61 | [stepper.train] 62 | max_iter = 300000 63 | log_freq = 100 64 | save_freq = 50000 65 | [stepper.train.loss] 66 | losses = ["mse"] 67 | weights = [1.0] 68 | [stepper.train.optim] 69 | method = "radam" 70 | lr = 0.00003 71 | betas = [0.5, 0.999] 72 | 73 | [projector] 74 | model = "mlp" 75 | [projector.train] 76 | max_iter = 300000 77 | log_freq = 100 78 | save_freq = 50000 79 | [projector.train.loss] 80 | losses = ["mse"] 81 | weights = [1.0] 82 | [projector.train.optim] 83 | method = "radam" 84 | lr = 0.00003 85 | betas = [0.5, 0.999] 86 | -------------------------------------------------------------------------------- /anim/aistpp.py: -------------------------------------------------------------------------------- 1 | """ 2 | aistpp.py 3 | AIST++ : 4 | format: ***.pkl (pickle file) 5 | Parameters : 6 | "smpl_loss" (float): I don't know how to use it. 7 | "smpl_poses" (np.ndarray): SMPL pose paramters (rotations). shape: (num_frame, num_J * 3 = 72). 8 | "smpl_scaling" (float): Scaling parameters of the skeleton. 9 | "smpl_trans" (np.ndarray): Root translations. shape: (num_frame, 3). 10 | """ 11 | # loading AIST++ data. 12 | 13 | from __future__ import annotations 14 | 15 | from pathlib import Path 16 | import numpy as np 17 | from anim.skel import Skel 18 | from anim.animation import Animation 19 | from anim.smpl import load_model, calc_skel_offsets, SMPL_JOINT_NAMES 20 | from util import quat 21 | from util.load import pickle_load 22 | 23 | def load( 24 | aistpp_motion_path: Path, 25 | skel: Skel=None, 26 | smpl_path: Path=Path("data/smpl/neutral/model.pkl"), 27 | scale: float=100.0, 28 | fps: int=30, 29 | ) -> Animation: 30 | 31 | NUM_JOINTS = 24 32 | NUM_BETAS = 10 33 | 34 | # Load a motion. 35 | if isinstance(aistpp_motion_path, str): 36 | aistpp_motion_path = Path(aistpp_motion_path) 37 | name = aistpp_motion_path.stem 38 | aistpp_dict = pickle_load(aistpp_motion_path) 39 | poses = aistpp_dict["smpl_poses"] 40 | num_frames = len(poses) 41 | poses = poses.reshape(num_frames, -1, 3) 42 | quats = quat.from_axis_angle(poses) 43 | trans = aistpp_dict["smpl_trans"] / aistpp_dict["smpl_scaling"] 44 | 45 | if skel == None: 46 | # Load a Skel. 47 | smpl_dict = load_model(smpl_path) 48 | parents = list(map(int, smpl_dict["kintree_table"][0][:NUM_JOINTS])) 49 | parents[0] = -1 50 | J_regressor = smpl_dict["J_regressor"] 51 | shapedirs = smpl_dict["shapedirs"] 52 | v_template = smpl_dict["v_template"] 53 | betas = np.zeros([NUM_BETAS]) 54 | J_positions = calc_skel_offsets(betas, J_regressor, shapedirs, v_template)[:NUM_JOINTS] * scale 55 | root_offset = J_positions[0] 56 | offsets = J_positions - J_positions[parents] 57 | offsets[0] = root_offset 58 | names = SMPL_JOINT_NAMES[:NUM_JOINTS] 59 | skel = Skel.from_names_parents_offsets(names, parents, offsets) 60 | 61 | # We need to apply smpl root offsets to translations. 62 | trans += root_offset 63 | 64 | return Animation(skel=skel, quats=quats, trans=trans, fps=fps, anim_name=name,) -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | # others 132 | .DS_Store 133 | data/* 134 | !data/data.md 135 | 136 | # under construction 137 | anim/SE3/* 138 | anim/SO3/* 139 | makefile 140 | *.ipynb -------------------------------------------------------------------------------- /test/plotting_cspace.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | import numpy as np 6 | import matplotlib.ticker as ticker 7 | import matplotlib.pyplot as plt 8 | from matplotlib.animation import FuncAnimation 9 | 10 | BASEPATH = Path(__file__).resolve().parent.parent 11 | sys.path.append(str(BASEPATH)) 12 | 13 | from anim import bvh 14 | 15 | if __name__ == "__main__": 16 | bvh_folder = BASEPATH / "data" 17 | bvh_file = "sim_motion.bvh" 18 | start, end = None, None 19 | elev, azim = 120, -90 20 | save_fig = False 21 | 22 | anim = bvh.load(bvh_folder / bvh_file, start, end) 23 | poss = anim.cpos / 100 24 | parents = anim.parents 25 | 26 | future_20_poss = anim.future_traj_poss(20, cspace=True) 27 | future_20_dirs = anim.future_traj_dirs(20, cspace=True) 28 | 29 | fig = plt.figure(figsize=(7, 7)) 30 | ax = fig.add_subplot(111, projection="3d") 31 | 32 | def update(index: int): 33 | poses = poss[index] 34 | 35 | future_20_pos = future_20_poss[index] / 100 36 | future_20_dir = future_20_dirs[index] 37 | 38 | ax.cla() 39 | ax.set_title("character") 40 | ax.grid(axis="y") 41 | ax.set_yticklabels([]) 42 | ax.yaxis.pane.set_facecolor("gray") 43 | ax.yaxis.set_major_locator(ticker.NullLocator()) 44 | ax.yaxis.set_minor_locator(ticker.NullLocator()) 45 | ax.view_init(elev=elev, azim=azim) 46 | ax.set_xlim3d(-3, 3) 47 | ax.set_ylim3d(0, 3) 48 | ax.set_zlim3d(-3, 3) 49 | ax.set_box_aspect((2, 1, 2)) 50 | 51 | ax.scatter(poses[:, 0], poses[:, 1], poses[:, 2], c="red", label="joints") 52 | for i, parent in enumerate(parents): 53 | if parent != -1: 54 | ax.plot( 55 | [poses[parent,0], poses[i,0]], 56 | [poses[parent,1], poses[i,1]], 57 | [poses[parent,2], poses[i,2]], 58 | c="red", alpha=0.8 59 | ) 60 | ax.plot( 61 | [future_20_pos[0], future_20_pos[0]+future_20_dir[0]], 62 | [0, 0], 63 | [future_20_pos[1], future_20_pos[1]+future_20_dir[1]], 64 | label="20 future direction", c="green" 65 | ) 66 | ax.legend() 67 | fig.suptitle("frame: {}".format(index+1)) 68 | sys.stdout.write("processed frame: {}\n".format(index+1)) 69 | ani = FuncAnimation(fig, update, frames=np.arange(len(anim)), interval=1000/anim.fps, repeat=True) 70 | 71 | if save_fig: 72 | fig_path = (BASEPATH / "figs" / bvh_file).with_suffix(".gif") 73 | ani.save(fig_path) 74 | else: 75 | plt.show() -------------------------------------------------------------------------------- /anim/inverse_kinematics/jacobi_ik.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | ROOT = Path(__file__).resolve().parent.parent.parent 6 | sys.path.append(str(ROOT)) 7 | 8 | import copy 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | from util import quat 12 | from anim.skel import Joint, Skel 13 | from anim.animation import Animation 14 | 15 | def simple_jacobi_ik( 16 | anim: Animation, 17 | tgt_pos: np.ndarray, 18 | iter: int=10, 19 | ) -> Animation: 20 | return 21 | 22 | if __name__=="__main__": 23 | """使用例""" 24 | 25 | # 簡単なJointの定義 26 | joints = [] 27 | joints.append(Joint(name="ROOT", index=0, parent=-1, offset=np.zeros(3), root=True, dof=6)) 28 | joints.append(Joint(name="J1", index=1, parent= 0, offset=np.array([1, 0, 0]))) 29 | joints.append(Joint(name="J2", index=2, parent= 1, offset=np.array([1, 0, 0]))) 30 | joints.append(Joint(name="J3", index=3, parent= 2, offset=np.array([1, 0, 0]))) 31 | # Root J1 J2 J3 32 | # *----------*----------*----------* 33 | # (0, 0, 0) (1, 0, 0) (2, 0, 0) (3, 0, 0) 34 | 35 | 36 | skel = Skel(joints=joints, skel_name="sample_skel") 37 | anim = Animation.no_animation(skel, num_frame=2, anim_name="sample_anim") 38 | 39 | # target positions for first frame and second frame. 40 | tgts = np.array([ 41 | [1, 1, 1], 42 | [1, 2, 1]] 43 | ) 44 | 45 | new_anim = simple_jacobi_ik(anim, tgts) 46 | 47 | fig = plt.figure(figsize=(11, 7)) 48 | 49 | # first frame 50 | ax1 = fig.add_subplot(121, projection="3d") 51 | ax1.set_xlim3d(-2, 2) 52 | ax1.set_ylim3d(-2, 2) 53 | ax1.set_zlim3d(-2, 2) 54 | ax1.set_box_aspect((1,1,1)) 55 | 56 | orig = anim.gpos[0] 57 | poss = new_anim.gpos[0] 58 | tgt = tgts[0] 59 | 60 | ax1.plot(poss[:,0], poss[:,1], poss[:,2]) 61 | ax1.scatter(poss[:,0], poss[:,1], poss[:,2], label="calc") 62 | ax1.plot(orig[:,0], orig[:,1], orig[:,2]) 63 | ax1.scatter(orig[:,0], orig[:,1], orig[:,2], label="orig") 64 | ax1.scatter(tgt[0], tgt[1], tgt[2], label="target") 65 | ax1.legend() 66 | 67 | # second frame 68 | ax2 = fig.add_subplot(122, projection="3d") 69 | ax2.set_xlim3d(-2, 2) 70 | ax2.set_ylim3d(-2, 2) 71 | ax2.set_zlim3d(-2, 2) 72 | ax2.set_box_aspect((1,1,1)) 73 | 74 | orig = anim.gpos[1] 75 | poss = new_anim.gpos[1] 76 | tgt = tgts[1] 77 | 78 | ax2.plot(poss[:,0], poss[:,1], poss[:,2]) 79 | ax2.scatter(poss[:,0], poss[:,1], poss[:,2], label="calc") 80 | ax2.plot(orig[:,0], orig[:,1], orig[:,2]) 81 | ax2.scatter(orig[:,0], orig[:,1], orig[:,2], label="orig") 82 | ax2.scatter(tgt[0], tgt[1], tgt[2], label="target") 83 | ax2.legend() 84 | 85 | plt.show() -------------------------------------------------------------------------------- /model/LMM/setting.py: -------------------------------------------------------------------------------- 1 | """ 2 | setting.py 3 | Device setting etc.. 4 | """ 5 | from __future__ import annotations 6 | 7 | import sys 8 | from pathlib import Path 9 | import shutil 10 | 11 | from util.load import pickle_load, toml_load 12 | from model.LMM.preprocessing import preprocess_motion_data 13 | 14 | class Config: 15 | def __init__(self, BASEPATH: Path, setting_path: str) -> None: 16 | # Load a setting file. 17 | try: 18 | if (BASEPATH / setting_path).exists(): 19 | setting_path = BASEPATH / setting_path 20 | setting = toml_load(setting_path) 21 | else: 22 | setting = toml_load(setting_path) 23 | except: 24 | setting_path = BASEPATH / "configs/DeepLearning/LMM.toml" 25 | setting = toml_load(setting_path) 26 | sys.stdout.write(f"Config file: {str(setting_path)}\n") 27 | 28 | self.setting = setting 29 | self.BASEPATH = BASEPATH 30 | self.seed = setting.seed 31 | 32 | # Device settings (gpu or cpu) 33 | self.device = setting.device 34 | # GPU settings. 35 | if self.device == "gpu" or "cuda": 36 | if isinstance(setting.gpus, list): 37 | # if you use multi gpus 38 | if len(setting.gpus) > 1: 39 | self.gpus = setting.gpus 40 | self.multi_gpus_setting() 41 | else: 42 | # if you use single gpu 43 | self.gpus = setting.gpus[0] 44 | self.single_gpu_setting() 45 | else: 46 | self.gpus = setting.gpus 47 | self.single_gpu_setting() 48 | 49 | # Dataset settings. 50 | # if you need preprocess 51 | if setting.need_preprocess: 52 | save_dir = BASEPATH / setting.processed_dir 53 | if not save_dir.exists(): 54 | save_dir.mkdir(parents=True) 55 | sys.stdout.write(f"Create preprocessed folder at: {str(save_dir)}\n") 56 | self.dataset = preprocess_motion_data(BASEPATH, setting.dataset, save_dir / setting.processed_file_name) 57 | else: 58 | self.dataset = pickle_load(save_dir / setting.processed_file_name) 59 | 60 | # Save experiment settings. 61 | ckpt_dir = BASEPATH / setting.checkpoint_dir / setting.exp_name 62 | # TBD: if exists, load checkpoints and resume the experiment. 63 | if not ckpt_dir.exists(): 64 | ckpt_dir.mkdir(parents=True) 65 | sys.stdout.write(f"Create checkpoint folder at: {str(ckpt_dir)}\n") 66 | self.ckpt_dir = ckpt_dir 67 | 68 | save_cfg_dir = BASEPATH / setting.save_config_dir 69 | save_cfg_path = save_cfg_dir / f"{setting.exp_name}.toml" 70 | if not save_cfg_dir.exists(): 71 | save_cfg_dir.mkdir(parents=True) 72 | sys.stdout.write(f"Create config folder at: {str(save_cfg_dir)}\n") 73 | shutil.copy(str(setting_path), str(save_cfg_path)) 74 | 75 | def single_gpu_setting(self): 76 | pass 77 | 78 | def multi_gpus_setting(self): 79 | pass -------------------------------------------------------------------------------- /test/plotting_global.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | import numpy as np 6 | import matplotlib.ticker as ticker 7 | import matplotlib.pyplot as plt 8 | from matplotlib.animation import FuncAnimation 9 | 10 | BASEPATH = Path(__file__).resolve().parent.parent 11 | sys.path.append(str(BASEPATH)) 12 | 13 | from anim import bvh 14 | 15 | if __name__ == "__main__": 16 | bvh_folder = BASEPATH / "data" 17 | bvh_file = "sim_motion.bvh" 18 | start, end = None, None 19 | elev, azim = 120, -90 20 | save_fig = False 21 | 22 | anim = bvh.load(bvh_folder / bvh_file, start, end) 23 | poss = anim.gpos / 100 24 | parents = anim.parents 25 | 26 | proj_root_poss = anim.proj_root_pos(remove_vertical=True) 27 | root_dirs = anim.root_direction(remove_vertical=True) 28 | future_20_poss = anim.future_traj_poss(20, cspace=False) 29 | future_20_dirs = anim.future_traj_dirs(20, cspace=False) 30 | 31 | fig = plt.figure(figsize=(7, 7)) 32 | ax = fig.add_subplot(111, projection="3d") 33 | 34 | def update(index: int): 35 | poses = poss[index] 36 | 37 | # root direction 38 | proj_root_pos = proj_root_poss[index] / 100 39 | root_dir = root_dirs[index] 40 | 41 | # future root direction 42 | future_20_pos = future_20_poss[index] / 100 43 | future_20_dir = future_20_dirs[index] 44 | 45 | # plot settings 46 | ax.cla() 47 | ax.set_title("global") 48 | ax.grid(axis="y") 49 | ax.set_yticklabels([]) 50 | ax.yaxis.pane.set_facecolor("gray") 51 | ax.yaxis.set_major_locator(ticker.NullLocator()) 52 | ax.yaxis.set_minor_locator(ticker.NullLocator()) 53 | ax.view_init(elev=elev, azim=azim) 54 | ax.set_xlim3d(proj_root_pos[0]-3, proj_root_pos[0]+3) 55 | ax.set_ylim3d(0, 3) 56 | ax.set_zlim3d(proj_root_pos[1]-3, proj_root_pos[1]+3) 57 | ax.set_box_aspect((2, 1, 2)) 58 | 59 | ax.scatter(poses[:, 0], poses[:, 1], poses[:, 2], c="red", label="joints") 60 | for i, parent in enumerate(parents): 61 | if parent != -1: 62 | ax.plot( 63 | [poses[parent,0], poses[i,0]], 64 | [poses[parent,1], poses[i,1]], 65 | [poses[parent,2], poses[i,2]], 66 | c="red", alpha=0.8 67 | ) 68 | ax.plot( 69 | [proj_root_pos[0], proj_root_pos[0]+root_dir[0]], 70 | [0, 0], 71 | [proj_root_pos[1], proj_root_pos[1]+root_dir[1]], 72 | label="root direction", c="blue" 73 | ) 74 | ax.plot( 75 | [future_20_pos[0], future_20_pos[0]+future_20_dir[0]], 76 | [0, 0], 77 | [future_20_pos[1], future_20_pos[1]+future_20_dir[1]], 78 | label="20 future direction", c="green" 79 | ) 80 | ax.legend() 81 | fig.suptitle("frame: {}".format(index+1)) 82 | sys.stdout.write("Processed frame: {}\n".format(index+1)) 83 | ani = FuncAnimation(fig, update, frames=np.arange(len(anim)), interval=1000/anim.fps, repeat=True) 84 | 85 | if save_fig: 86 | fig_path = (BASEPATH / "figs" / bvh_file).with_suffix(".gif") 87 | ani.save(fig_path) 88 | sys.stdout.write("Saved figure as " + str(fig_path) + "\n") 89 | else: 90 | plt.show() -------------------------------------------------------------------------------- /test/character_controller.py: -------------------------------------------------------------------------------- 1 | # TBD 2 | 3 | import sys 4 | from pathlib import Path 5 | import pickle 6 | 7 | BASEPATH = Path(__file__).resolve().parent.parent 8 | sys.path.append(str(BASEPATH)) 9 | 10 | from anim import bvh 11 | from anim.motion_matching.database import Database 12 | from anim.motion_matching.mm import create_matching_database, motion_matching_search 13 | 14 | def create_database(bvh_dir: Path, files: list, starts: list, ends: list) -> Database: 15 | if isinstance(bvh_dir, str): 16 | bvh_dir = Path(bvh_dir) 17 | anims = [] 18 | for file, start, end in zip(files, starts, ends): 19 | anim = bvh.load(bvh_dir / file, start, end) 20 | anims.append(anim) 21 | return Database(anims) 22 | 23 | def main(): 24 | # init configs (this will replace to a yaml file). 25 | matching_method = "aabb" 26 | ignore_end = 20 27 | 28 | bvh_dir = BASEPATH / "data/lafan1" 29 | files = ["pushAndStumble1_subject5.bvh", "run1_subject5.bvh", "walk1_subject5.bvh"] 30 | starts = [194, 90, 80] 31 | ends = [351, 7086, 7791] 32 | 33 | load_database = False 34 | save_database = True 35 | 36 | search_time = 0.1 37 | search_timer = search_time 38 | 39 | desired_velocity_change_threshold = 50.0 40 | desired_rotation_change_threshold = 50.0 41 | desired_gait = 0.0 42 | desired_gait_velocity = 0.0 43 | 44 | simulation_velocity_halflife = 0.27 45 | simulation_rotation_halflife = 0.27 46 | 47 | simulation_run_fwrd_speed = 4.0 48 | simulation_run_side_speed = 3.0 49 | simulation_run_back_speed = 2.5 50 | simulation_walk_fwrd_speed = 1.75 51 | simulation_walk_side_speed = 1.5 52 | simulation_walk_back_speed = 1.25 53 | 54 | dt = 1 / 60 55 | 56 | 57 | # Create `Database` and `MatchingDatabase`. 58 | db = create_database(bvh_dir, files, starts, ends) 59 | mdb = create_matching_database( 60 | db=db, method=matching_method, 61 | w_pos_foot=0.75, w_vel_foot=1, w_vel_hips=1, w_traj_pos=1, w_traj_dir=1.5, 62 | dense_bound_size=16, sparse_bound_size=64, 63 | ) 64 | if save_database: 65 | with open(BASEPATH / "data/motion_matching/db.pkl", "wb") as f: 66 | pickle.dump(db, f) 67 | with open(BASEPATH / "data/motion_matching/db.pkl", "wb") as f: 68 | pickle.dump(mdb, f) 69 | 70 | # Initialize parameters 71 | frame_idx = 0 72 | 73 | # Init the pose 74 | 75 | # Init the simulation (future direction etc.) 76 | 77 | # Init the camera 78 | 79 | 80 | 81 | 82 | 83 | 84 | """ここ以下は繰り返し""" 85 | 86 | # Generate query for motion matching. 87 | query = None 88 | 89 | # Check if we reached the end of the current anim. 90 | if frame_idx in db.ends: 91 | end_of_anim = True 92 | cur_idx = -1 93 | else: 94 | end_of_anim = False 95 | cur_idx = frame_idx 96 | 97 | if search_timer <= 0 or end_of_anim: 98 | # Motion Matching Search! 99 | cur_idx = motion_matching_search(cur_idx, matching_method, db, mdb, query, ignore_end) 100 | 101 | if cur_idx != frame_idx: 102 | frame_idx = cur_idx 103 | 104 | search_time = search_timer 105 | 106 | search_timer -= dt 107 | frame_idx += 1 108 | 109 | # Update the next pose 110 | 111 | # Update the simulation 112 | 113 | # Update the camera 114 | 115 | # Drawing 116 | 117 | 118 | if __name__ == "__main__": 119 | main() 120 | -------------------------------------------------------------------------------- /anim/amass.py: -------------------------------------------------------------------------------- 1 | """ 2 | amass.py 3 | AMASS : 4 | Format: ***.npz (numpy binary file) 5 | Parameters: 6 | "trans" (np.ndarray): Root translations. shape: (num_frames, 3). 7 | "gender" (np.ndarray): Gender name. "male", "female", "neutral". 8 | "mocap_framerate" (np.ndarray): Fps of mocap data. 9 | "betas" (np.ndarray): PCA based body shape parameters. shape: (num_betas=16). 10 | "dmpls" (np.ndarray): Dynamic body parameters of DMPL. We do not use. Unused for skeleton. shape: (num_frames, num_dmpls=8). 11 | "poses" (np.ndarray): SMPLH pose parameters (rotations). shape: (num_frames, num_J * 3 = 156). 12 | """ 13 | # loading amass data. 14 | 15 | from __future__ import annotations 16 | 17 | from pathlib import Path 18 | import numpy as np 19 | from anim.skel import Skel 20 | from anim.animation import Animation 21 | from anim.smpl import load_model, calc_skel_offsets, SMPL_JOINT_NAMES, SMPLH_JOINT_NAMES 22 | from util import quat 23 | 24 | def load( 25 | amass_motion_path: Path, 26 | skel: Skel=None, 27 | smplh_path: Path=Path("data/smplh/neutral/model.npz"), 28 | remove_betas: bool=False, 29 | gender: str=None, 30 | num_betas: int=16, 31 | scale: float=100.0, 32 | load_hand=True, 33 | ) -> Animation: 34 | """ 35 | args: 36 | amass_motion_file (Path): Path to the AMASS motion file. 37 | smplh_path (Path): Optional. Path to the SMPLH model. 38 | remove_betas (bool): remove beta parameters from AMASS. 39 | gender (str): Gender of SMPLH model. 40 | num_betas (int): Number of betas to use. Defaults to 16. 41 | num_dmpls (int): Number of dmpl parameters to use. Defaults to 8. 42 | scale (float): Scaling paramter of the skeleton. Defaults to 100.0. 43 | load_hand (bool): Whether to use hand joints. Defaults to True. 44 | load_dmpl (bool): Whether to use DMPL. Defaults to False. 45 | Return: 46 | anim (Animation) 47 | """ 48 | if load_hand: 49 | NUM_JOINTS = 52 50 | names = SMPLH_JOINT_NAMES[:NUM_JOINTS] 51 | else: 52 | NUM_JOINTS = 24 53 | names = SMPL_JOINT_NAMES[:NUM_JOINTS] 54 | 55 | if not isinstance(amass_motion_path, Path): 56 | amass_motion_path = Path(amass_motion_path) 57 | if not isinstance(smplh_path, Path): 58 | smplh_path = Path(smplh_path) 59 | 60 | 61 | trans_quat = quat.mul(quat.from_angle_axis(-np.pi / 2, [0, 1, 0]), quat.from_angle_axis(-np.pi / 2, [1, 0, 0])) 62 | 63 | # Load AMASS info. 64 | amass_dict = np.load(amass_motion_path, allow_pickle=True) 65 | if gender == None: gender = str(amass_dict["gender"]) 66 | fps = int(amass_dict["mocap_framerate"]) 67 | if remove_betas: betas = np.zeros([num_betas]) 68 | else: betas = amass_dict["betas"][:num_betas] 69 | axangles = amass_dict["poses"] 70 | num_frames = len(axangles) 71 | axangles = axangles.reshape([num_frames, -1, 3])[:,:NUM_JOINTS] 72 | quats = quat.from_axis_angle(axangles) 73 | root_rot = quat.mul(trans_quat[None], quats[:,0]) 74 | quats[:,0] = root_rot 75 | 76 | if skel == None: 77 | # Load SMPL parmeters. 78 | smplh_dict = load_model(smplh_path, gender) 79 | parents = smplh_dict["kintree_table"][0][:NUM_JOINTS] 80 | parents[0] = -1 81 | J_regressor = smplh_dict["J_regressor"] 82 | shapedirs = smplh_dict["shapedirs"] 83 | v_template = smplh_dict["v_template"] 84 | 85 | J_positions = calc_skel_offsets(betas, J_regressor, shapedirs, v_template)[:NUM_JOINTS] * scale 86 | root_offset = J_positions[0] 87 | offsets = J_positions - J_positions[parents] 88 | offsets[0] = root_offset 89 | skel = Skel.from_names_parents_offsets(names, parents, offsets, skel_name="SMPLH") 90 | 91 | root_pos = skel.offsets[0][None].repeat(len(quats), axis=0) 92 | trans = amass_dict["trans"] * scale + root_pos 93 | trans = trans @ quat.to_xform(trans_quat).T 94 | anim = Animation(skel=skel, quats=quats, trans=trans, fps=fps, anim_name=amass_motion_path.stem) 95 | 96 | return anim -------------------------------------------------------------------------------- /anim/inverse_kinematics/ccd_ik.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | ROOT = Path(__file__).resolve().parent.parent.parent 6 | sys.path.append(str(ROOT)) 7 | 8 | import copy 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | from util import quat 12 | from anim.skel import Joint, Skel 13 | from anim.animation import Animation 14 | 15 | def normalize_vector(vector: np.ndarray): 16 | norm = np.linalg.norm(vector, axis=-1, keepdims=True) 17 | norm = np.where(norm==0, 1e-10, norm) 18 | return vector / norm 19 | 20 | # まずはEnd effectorのみの実装(Jointの末端が一つのみ) 21 | def simple_ccd_ik( 22 | anim: Animation, 23 | tgt_pos: np.ndarray, 24 | iter: int=10, 25 | ) -> Animation: 26 | anim_ = copy.deepcopy(anim) 27 | 28 | for i in range(iter): 29 | # except end effector 30 | for parent in reversed(anim_.parents): 31 | if parent == -1: 32 | continue 33 | cur_quat = anim_.quats[:,parent] # (T, 4) 34 | 35 | # 現在のJointを基準としたeffectorの位置(global座標系) 36 | rel_effpos = anim_.gpos[:,-1] - anim_.gpos[:,parent] 37 | # rel_effpos = quat.mul_vec(quat.inv(cur_quat), rel_effpos) 38 | eff_vec = normalize_vector(rel_effpos) 39 | 40 | # 現在のJointを基準としたtargetの位置(global座標系) 41 | rel_tgtpos = tgt_pos - anim.gpos[:,parent] 42 | # rel_tgtpos = quat.mul_vec(quat.inv(cur_quat), rel_tgtpos) 43 | tgt_vec = normalize_vector(rel_tgtpos) 44 | 45 | rel_quat = quat.normalize(quat.between(eff_vec, tgt_vec)) 46 | anim_.quats[:,parent] = quat.abs(quat.normalize(quat.mul(rel_quat, cur_quat))) 47 | 48 | return anim_ 49 | 50 | if __name__=="__main__": 51 | """使用例""" 52 | 53 | # 簡単なJointの定義 54 | joints = [] 55 | joints.append(Joint(name="ROOT", parent=-1, offset=np.zeros(3), root=True, dof=6)) 56 | joints.append(Joint(name="J1", parent= 0, offset=np.array([1, 0, 0]))) 57 | joints.append(Joint(name="J2", parent= 1, offset=np.array([1, 0, 0]))) 58 | joints.append(Joint(name="J3", parent= 2, offset=np.array([1, 0, 0]))) 59 | # Root J1 J2 J3 60 | # *----------*----------*----------* 61 | # (0, 0, 0) (1, 0, 0) (2, 0, 0) (3, 0, 0) 62 | 63 | 64 | skel = Skel(joints=joints, skel_name="sample_skel") 65 | anim = Animation.no_animation(skel, num_frame=2, anim_name="sample_anim") 66 | 67 | # first and second frame 68 | target_position = np.array([[1, 1, 1], [1, 2, 1]]) 69 | 70 | new_anim = simple_ccd_ik(anim, target_position) 71 | 72 | fig = plt.figure(figsize=(11, 7)) 73 | 74 | # first frame 75 | ax1 = fig.add_subplot(121, projection="3d") 76 | ax1.set_xlim3d(-2, 2) 77 | ax1.set_ylim3d(-2, 2) 78 | ax1.set_zlim3d(-2, 2) 79 | ax1.set_box_aspect((1,1,1)) 80 | 81 | orig = anim.gpos[0] 82 | poss = new_anim.gpos[0] 83 | tgt = target_position[0] 84 | 85 | ax1.plot(orig[:,0], orig[:,1], orig[:,2], alpha=0.4) 86 | ax1.scatter(orig[:,0], orig[:,1], orig[:,2], alpha=0.4, label="orig") 87 | ax1.plot(poss[:,0], poss[:,1], poss[:,2]) 88 | ax1.scatter(poss[:,0], poss[:,1], poss[:,2], label="calc") 89 | ax1.scatter(tgt[0], tgt[1], tgt[2], s=50, marker="*", label="target") 90 | ax1.legend() 91 | 92 | # second frame 93 | ax2 = fig.add_subplot(122, projection="3d") 94 | ax2.set_xlim3d(-2, 2) 95 | ax2.set_ylim3d(-2, 2) 96 | ax2.set_zlim3d(-2, 2) 97 | ax2.set_box_aspect((1,1,1)) 98 | 99 | orig = anim.gpos[1] 100 | poss = new_anim.gpos[1] 101 | tgt = target_position[1] 102 | 103 | ax2.plot(orig[:,0], orig[:,1], orig[:,2], alpha=0.4) 104 | ax2.scatter(orig[:,0], orig[:,1], orig[:,2], alpha=0.4, label="orig") 105 | ax2.plot(poss[:,0], poss[:,1], poss[:,2]) 106 | ax2.scatter(poss[:,0], poss[:,1], poss[:,2], label="calc") 107 | ax2.scatter(tgt[0], tgt[1], tgt[2], s=50, marker="*", label="target") 108 | ax2.legend() 109 | 110 | plt.show() -------------------------------------------------------------------------------- /anim/inverse_kinematics/fabrik.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | ROOT = Path(__file__).resolve().parent.parent.parent 6 | sys.path.append(str(ROOT)) 7 | 8 | import copy 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | from util import quat 12 | from anim.skel import Joint, Skel 13 | from anim.animation import Animation 14 | 15 | def normalize_vector(vector: np.ndarray): 16 | norm = np.linalg.norm(vector, axis=-1, keepdims=True) 17 | norm = np.where(norm==0, 1e-10, norm) 18 | return vector / norm 19 | 20 | def backward_reaching( 21 | anim: Animation, 22 | tgt_pos: np.ndarray, 23 | ) -> Animation: 24 | 25 | joint_idxs = anim.skel.indices 26 | target = tgt_pos.copy() # (T, 3) 27 | cur_gposs = anim.gpos.copy() 28 | bone_lengths = anim.skel.bone_lengths 29 | for j, parent in zip(reversed(joint_idxs), reversed(anim.parents)): 30 | if parent == -1: 31 | continue 32 | cur_quat = anim.quats[:,parent] # (T, 4) 33 | 34 | # 現在のJointを基準とした更新前の子Joint位置(global座標系) 35 | rel_chpos = cur_gposs[:,j] - cur_gposs[:,parent] 36 | ch_vec = normalize_vector(rel_chpos) 37 | 38 | # 現在のJointを基準とした更新後の子Joint位置(global座標系) 39 | rel_tgtpos = target - cur_gposs[:, parent] 40 | tgt_vec = normalize_vector(rel_tgtpos) 41 | 42 | rel_quat = quat.normalize(quat.between(ch_vec, tgt_vec)) 43 | anim.quats[:,parent] = quat.abs(quat.normalize(quat.mul(rel_quat, cur_quat))) 44 | 45 | # 親がROOTのとき、offsetを変更 46 | if parent == 0: 47 | anim.trans = target - tgt_vec * bone_lengths[j] 48 | else: 49 | # targetの更新 50 | target = target - tgt_vec * bone_lengths[j] 51 | 52 | return anim 53 | 54 | def forward_reaching( 55 | anim: Animation, 56 | base: np.ndarray, 57 | ) -> Animation: 58 | 59 | joint_idxs = np.arange(len(anim.skel)) 60 | target = base.copy() # (T, 3) 61 | past_gposs = anim.gpos.copy() 62 | 63 | for j, parent in zip(joint_idxs, anim.parents): 64 | if parent == -1: 65 | continue 66 | elif parent == 0: 67 | anim.trans = target 68 | continue 69 | cur_quat = anim.quats[:,parent] # (T, 4) 70 | 71 | # 過去の親 -> 過去の子のJoint位置 72 | past_chpos = past_gposs[:,j] - past_gposs[:,parent] 73 | pa_vec = normalize_vector(past_chpos) 74 | 75 | # 現在の親 -> 過去の子のJoint位置 76 | cur_chpos = past_gposs[:,j] - anim.gpos[:,parent] 77 | cur_vec = normalize_vector(cur_chpos) 78 | 79 | rel_quat = quat.normalize(quat.between(pa_vec, cur_vec)) 80 | anim.quats[:,parent] = quat.abs(quat.normalize(quat.mul(rel_quat, cur_quat))) 81 | 82 | return anim 83 | 84 | # まずはEnd effectorのみの実装(Jointの末端が一つのみ) 85 | def simple_fabrik( 86 | anim: Animation, 87 | tgt_pos: np.ndarray, 88 | iter: int=10, 89 | ) -> Animation: 90 | 91 | anim_ = copy.deepcopy(anim) 92 | # define base(root) positions. 93 | base = anim.trans.copy() # (T, 3) 94 | 95 | for i in range(iter): 96 | anim_ = backward_reaching(anim_, tgt_pos) 97 | anim_ = forward_reaching(anim_, base) 98 | 99 | return anim_ 100 | 101 | if __name__=="__main__": 102 | """使用例""" 103 | 104 | # 簡単なJointの定義 105 | joints = [] 106 | joints.append(Joint(name="ROOT", parent=-1, offset=np.zeros(3), root=True, dof=6)) 107 | joints.append(Joint(name="J1", parent= 0, offset=np.array([1, 0, 0]))) 108 | joints.append(Joint(name="J2", parent= 1, offset=np.array([1, 0, 0]))) 109 | joints.append(Joint(name="J3", parent= 2, offset=np.array([1, 0, 0]))) 110 | # Root J1 J2 J3 111 | # *----------*----------*----------* 112 | # (0, 0, 0) (1, 0, 0) (2, 0, 0) (3, 0, 0) 113 | 114 | 115 | skel = Skel(joints=joints, skel_name="sample_skel") 116 | anim = Animation.no_animation(skel, num_frame=2, anim_name="sample_anim") 117 | 118 | # target positions for first frame and second frame. 119 | tgts = np.array([ 120 | [1, 1, 1], 121 | [1, 2, 1]] 122 | ) 123 | 124 | new_anim = simple_fabrik(anim, tgts) 125 | 126 | fig = plt.figure(figsize=(11, 7)) 127 | 128 | # first frame 129 | ax1 = fig.add_subplot(121, projection="3d") 130 | ax1.set_xlim3d(-2, 2) 131 | ax1.set_ylim3d(-2, 2) 132 | ax1.set_zlim3d(-2, 2) 133 | ax1.set_box_aspect((1,1,1)) 134 | 135 | orig = anim.gpos[0] 136 | poss = new_anim.gpos[0] 137 | tgt = tgts[0] 138 | 139 | ax1.plot(orig[:,0], orig[:,1], orig[:,2], alpha=0.4) 140 | ax1.scatter(orig[:,0], orig[:,1], orig[:,2], alpha=0.4, label="orig") 141 | ax1.plot(poss[:,0], poss[:,1], poss[:,2]) 142 | ax1.scatter(poss[:,0], poss[:,1], poss[:,2], label="calc") 143 | ax1.scatter(tgt[0], tgt[1], tgt[2], s=50, marker="*", label="target") 144 | ax1.legend() 145 | 146 | # second frame 147 | ax2 = fig.add_subplot(122, projection="3d") 148 | ax2.set_xlim3d(-2, 2) 149 | ax2.set_ylim3d(-2, 2) 150 | ax2.set_zlim3d(-2, 2) 151 | ax2.set_box_aspect((1,1,1)) 152 | 153 | orig = anim.gpos[1] 154 | poss = new_anim.gpos[1] 155 | tgt = tgts[1] 156 | 157 | ax2.plot(orig[:,0], orig[:,1], orig[:,2], alpha=0.4) 158 | ax2.scatter(orig[:,0], orig[:,1], orig[:,2], alpha=0.4, label="orig") 159 | ax2.plot(poss[:,0], poss[:,1], poss[:,2]) 160 | ax2.scatter(poss[:,0], poss[:,1], poss[:,2], label="calc") 161 | ax2.scatter(tgt[0], tgt[1], tgt[2], s=50, marker="*", label="target") 162 | ax2.legend() 163 | 164 | plt.savefig("demo.png") -------------------------------------------------------------------------------- /anim/motion_matching/database.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | import numpy as np 5 | from scipy.spatial import KDTree 6 | from util import quat 7 | from anim.skel import Skel 8 | from anim.animation import Animation 9 | 10 | class Database(Animation): 11 | def __init__( 12 | self, 13 | anims: list[Animation]=None, 14 | skel: Skel=None, 15 | quats: np.ndarray=None, 16 | trans: np.ndarray=None, 17 | fps: int=None, 18 | starts: list[int]=None, 19 | names: list[str]=None, 20 | db_name: str=None, 21 | ) -> None: 22 | self.name = db_name 23 | 24 | if anims is not None: 25 | self.skel = anims[0].skel # all `Skel` must be the same. 26 | self.quats = np.concatenate([anim.quats for anim in anims], axis=0) 27 | self.trans = np.concatenate([anim.trans for anim in anims], axis=0) 28 | self.fps = anims[0].fps # all fps must be the same. 29 | self.starts = [] 30 | self.ends = [] 31 | self.names = [] 32 | cur_frame = 0 33 | for anim in anims: 34 | self.starts.append(cur_frame) 35 | cur_frame += len(anim) 36 | self.ends.append(cur_frame-1) 37 | self.names.append(anim.name) 38 | else: 39 | self.skel = skel 40 | self.quats = quats 41 | self.trans = trans 42 | self.fps = fps 43 | self.starts = starts 44 | self.names = names 45 | 46 | self.num_anim = len(self.starts) 47 | 48 | def __len__(self) -> int: return len(self.trans) 49 | 50 | def __add__(self, other: Database) -> Database: 51 | assert isinstance(other, Database) 52 | starts: list[int] = self.starts.copy() 53 | ends: list[int] = self.ends.copy() 54 | names: list[str] = self.names.copy() 55 | starts.extend(list(map(lambda x: x+len(self), other.starts))) 56 | ends.extend(list(map(lambda x: x+len(self), other.ends))) 57 | names.extend(other.names) 58 | quats: np.ndarray = np.concatenate([self.quats, other.quats], axis=0) 59 | trans: np.ndarray = np.concatenate([self.trans, other.trans], axis=0) 60 | 61 | return Database( 62 | skel = self.skel, 63 | quats=quats, 64 | trans=trans, 65 | fps=self.fps, 66 | starts=starts, 67 | names=names, 68 | db_name=self.name, 69 | ) 70 | 71 | def cat(self, other: Database) -> None: 72 | assert isinstance(other, Database) 73 | self.starts.extend(list(map(lambda x: x+len(self), other.starts))) 74 | self.ends.extend(list(map(lambda x: x+len(self), other.ends))) 75 | self.names.extend(other.names) 76 | self.quats = np.concatenate([self.quats, other.quats], axis=0) 77 | self.trans = np.concatenate([self.trans, other.trans], axis=0) 78 | 79 | # ============ 80 | # Velocity 81 | # ============ 82 | @property 83 | def gposvel(self) -> np.ndarray: 84 | gpos : np.ndarray = self.gpos 85 | gpvel: np.ndarray = np.zeros_like(gpos) 86 | gpvel[1:] = (gpos[1:] - gpos[:-1]) * self.fps # relative position from previous frame. 87 | for start in self.starts: 88 | gpvel[start] = gpvel[start+1] - (gpvel[start+3] - gpvel[start+2]) 89 | return gpvel 90 | 91 | @property 92 | def cposvel(self) -> np.ndarray: 93 | cpos : np.ndarray = self.cpos 94 | cpvel: np.ndarray = np.zeros_like(cpos) 95 | cpvel[1:] = (cpos[1:] - cpos[:-1]) * self.fps # relative position from previous frame. 96 | for start in self.starts: 97 | cpvel[start] = cpvel[start+1] - (cpvel[start+3] - cpvel[start+2]) 98 | return cpvel 99 | 100 | @property 101 | def lrotvel(self) -> np.ndarray: 102 | """Calculate rotation velocities with rotation vector style.""" 103 | 104 | lrot : np.ndarray = self.quats.copy() 105 | lrvel: np.ndarray = np.zeros_like(self.lpos) 106 | lrvel[1:] = quat.to_axis_angle( 107 | quat.abs( 108 | quat.mul(lrot[1:], quat.inv(lrot[:-1]))) 109 | ) * self.fps 110 | for start in self.starts: 111 | lrvel[start] = lrvel[start+1] - (lrvel[start+3] - lrvel[start+2]) 112 | return lrvel 113 | 114 | # =================== 115 | # Future trajectory 116 | # =================== 117 | def clamp_future_idxs(self, offset: int) -> np.ndarray: 118 | """Function to calculate the frame array for `offset` frame ahead. 119 | If `offset` frame ahead does not exist, 120 | return the last frame in each animation. 121 | """ 122 | idxs = np.arange(len(self)) + offset 123 | for end in self.ends: 124 | idxs = np.where((idxs > end) & (idxs <= end + offset), end, idxs) 125 | return idxs 126 | 127 | @dataclass 128 | class MatchingDatabase: 129 | features: np.ndarray # [T, num_features] 130 | means: np.ndarray # [num_features] 131 | stds: np.ndarray # [num_features] 132 | indices: list[int] 133 | 134 | # The elements belows are used only AABB. 135 | dense_bound_size: int = None 136 | dense_bound_mins: np.ndarray = None # [num_bounds, num_features] 137 | dense_bound_maxs: np.ndarray = None # [num_bounds, num_features] 138 | sparse_bound_size: int = None 139 | sparse_bound_mins: np.ndarray = None # [num_bounds, num_features] 140 | sparse_bound_maxs: np.ndarray = None # [num_bounds, num_features] 141 | 142 | # kd-tree 143 | kdtree: KDTree = None 144 | 145 | def __len__(self): 146 | return len(self.features) -------------------------------------------------------------------------------- /test/path_following.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | import pickle 6 | import numpy as np 7 | 8 | BASEPATH = Path(__file__).resolve().parent.parent 9 | sys.path.append(str(BASEPATH)) 10 | 11 | from util.load import pickle_load 12 | from util import quat 13 | from anim import bvh 14 | from anim.animation import Animation 15 | from anim.motion_matching.database import Database 16 | from anim.motion_matching.mm import create_matching_database, motion_matching_search 17 | 18 | def create_database(bvh_dir: Path, files: list, starts: list, ends: list) -> Database: 19 | if isinstance(bvh_dir, str): 20 | bvh_dir = Path(bvh_dir) 21 | anims = [] 22 | for file, start, end in zip(files, starts, ends): 23 | anim = bvh.load(bvh_dir / file, start, end) 24 | anims.append(anim) 25 | return Database(anims) 26 | 27 | def create_query(db: Database, idx: int, traj_feats: np.ndarray) -> np.ndarray: 28 | left_foot_idx = db.skel.names.index("LeftFoot") 29 | right_foot_idx = db.skel.names.index("RightFoot") 30 | hips_idx = db.skel.names.index("Hips") 31 | 32 | fpos = db.cpos[idx, [left_foot_idx, right_foot_idx]].ravel() 33 | vels = db.cposvel[idx,[left_foot_idx, right_foot_idx, hips_idx]].ravel() 34 | return np.concatenate([fpos, vels, traj_feats]) 35 | 36 | def create_circle_traj(): 37 | # Create path (circle) 38 | t = np.linspace(0, np.pi * 2, 100) 39 | x = 500 * np.cos(t) 40 | z = 500 * np.sin(t) 41 | dir_x = -z 42 | dir_z = x 43 | norm = np.sqrt(dir_x ** 2 + dir_z ** 2) 44 | dir_x = dir_x / norm 45 | dir_z = dir_z / norm 46 | x -= 500 47 | 48 | return np.array([ 49 | x[20], z[20], x[40], z[40], x[60], z[60], 50 | dir_x[20], dir_z[20], dir_x[40], dir_z[40], dir_x[60], dir_z[60] 51 | ]) 52 | 53 | 54 | def create_animation_from_idxs(db: Database, anim_frames: list[int]) -> Animation: 55 | quats = [] 56 | trans = [] 57 | t = np.linspace(0, np.pi * 2, 100) 58 | x = 500 * np.cos(t) - 500 59 | z = 500 * np.sin(t) 60 | 61 | for i, frame in enumerate(anim_frames): 62 | c_root_rot, root_pos = db.croot(frame) 63 | rot = db.quats[frame] 64 | i = i % 100 65 | rot[0] = quat.mul(quat.from_angle_axis(t[i], [0, 1, 0]), c_root_rot) 66 | root_pos[0] = x[i] 67 | root_pos[2] = z[i] 68 | 69 | quats.append(rot) 70 | trans.append(root_pos) 71 | 72 | quats = np.array(quats) 73 | trans = np.array(trans) 74 | return Animation(db.skel, quats, trans, db.fps) 75 | 76 | def main(): 77 | matching_method = "kd-tree" 78 | 79 | bvh_dir = BASEPATH / "data/lafan1" 80 | files = ["pushAndStumble1_subject5.bvh", "run1_subject5.bvh", "walk1_subject5.bvh"] 81 | starts = [194, 90, 80] 82 | ends = [351, 7086, 7791] 83 | 84 | load_database = False 85 | save_database = True 86 | 87 | search_time = 5 88 | search_timer = 0 89 | 90 | # Create `Database` and `MatchingDatabase`. 91 | if load_database: 92 | db = pickle_load(BASEPATH / "data/db.pkl") 93 | mdb = pickle_load(BASEPATH / "data/mdb.pkl") 94 | else: 95 | db = create_database(bvh_dir, files, starts, ends) 96 | mdb = create_matching_database( 97 | db=db, method=matching_method, 98 | w_pos_foot=0.75, w_vel_foot=1, w_vel_hips=1, w_traj_pos=1, w_traj_dir=1.5, 99 | ignore_end=20, dense_bound_size=16, sparse_bound_size=64, 100 | ) 101 | 102 | if save_database: 103 | with open(BASEPATH / "data/db.pkl", "wb") as f: 104 | pickle.dump(db, f) 105 | with open(BASEPATH / "data/mdb.pkl", "wb") as f: 106 | pickle.dump(mdb, f) 107 | 108 | # Initialize parameters 109 | frame_idx = 0 110 | frame_sum = 0 111 | animation_length = 120 # frame 112 | anim_frames = [] # animated frame will apend here. 113 | 114 | # We define the path that moves forward 3m every 20 frames. 115 | # 20, 40, 60 frames ahead. 116 | # path_poss = [ 200, 0, 400, 0, 600, 0 ] 117 | # path_dirs = [ 0, 1, 0, 1, 0, 1 ] 118 | # traj_feats = np.concatenate([path_poss, path_dirs]) 119 | 120 | # Create path (circle) 121 | traj_feats = create_circle_traj() 122 | 123 | # initial query 124 | query = create_query(db, frame_idx, traj_feats) 125 | 126 | # Animation loop 127 | while frame_sum < animation_length: 128 | # Check if we reached the end of the current anim. 129 | if frame_idx in db.ends: 130 | end_of_anim = True 131 | cur_idx = -1 132 | else: 133 | end_of_anim = False 134 | cur_idx = frame_idx 135 | 136 | if search_timer <= 0 or end_of_anim: 137 | # Motion Matching Search! 138 | cur_idx = motion_matching_search(cur_idx, matching_method, mdb, query) 139 | 140 | if cur_idx != frame_idx: 141 | frame_idx = cur_idx 142 | 143 | search_timer = search_time 144 | 145 | # update frame 146 | search_timer -= 1 147 | frame_idx += 1 148 | frame_sum += 1 149 | anim_frames.append(frame_idx) 150 | 151 | # Create new query 152 | query = create_query(db, frame_idx, traj_feats) 153 | 154 | sys.stdout.write("Processed frame: {}\n".format(frame_sum)) 155 | 156 | # Save animation. 157 | print("Creating animation..") 158 | sim_anim = create_animation_from_idxs(db, anim_frames) 159 | bvh.save(BASEPATH / "data/sim_motion.bvh", sim_anim) 160 | print("saved at {}".format(str(BASEPATH / "data/sim_motion.bvh"))) 161 | 162 | if __name__ == "__main__": 163 | main() 164 | -------------------------------------------------------------------------------- /test/plotting.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | import numpy as np 6 | from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg 7 | import matplotlib.ticker as ticker 8 | import matplotlib.pyplot as plt 9 | from matplotlib.animation import FuncAnimation 10 | import tkinter as tk 11 | 12 | BASEPATH = Path(__file__).resolve().parent.parent 13 | sys.path.append(str(BASEPATH)) 14 | 15 | from anim import bvh 16 | from anim.motion_matching.database import Database 17 | from anim.motion_matching.mm import create_matching_database 18 | 19 | if __name__ == "__main__": 20 | bvh_folder = BASEPATH / "data" 21 | bvh_file = "sim_motion.bvh" 22 | start = 90 23 | # end = 7086 24 | end = 2090 25 | save_fig = True 26 | 27 | anim = bvh.load(bvh_folder / bvh_file, start, end) 28 | db = Database([anim]) 29 | mdb = create_matching_database( 30 | db=db, method="brute-force", 31 | w_pos_foot=0.75, w_vel_foot=1, w_vel_hips=1, w_traj_pos=1, w_traj_dir=1.5, 32 | ignore_end=20, dense_bound_size=16, sparse_bound_size=64, 33 | ) 34 | bar_lim = np.max(np.abs(mdb.features)) 35 | gposs = db.gpos / 100 36 | cposs = db.cpos / 100 37 | parents = db.parents 38 | 39 | proj_root_poss = db.proj_root_pos(remove_vertical=True) 40 | root_dirs = db.root_direction(remove_vertical=True) 41 | future_20_gposs = db.future_traj_poss(20, cspace=False) 42 | future_20_gdirs = db.future_traj_dirs(20, cspace=False) 43 | future_20_cposs = db.future_traj_poss(20, cspace=True) 44 | future_20_cdirs = db.future_traj_dirs(20, cspace=True) 45 | 46 | fig = plt.figure(figsize=(7, 7)) 47 | axg = fig.add_subplot(221, projection="3d") 48 | axc = fig.add_subplot(223, projection="3d") 49 | axf = fig.add_subplot(122) 50 | 51 | def update(index: int): 52 | gposes = gposs[index] 53 | cposes = cposs[index] 54 | features = mdb.features[index] 55 | 56 | proj_root_pos = proj_root_poss[index] / 100 57 | root_dir = root_dirs[index] 58 | 59 | future_20_gpos = future_20_gposs[index] / 100 60 | future_20_gdir = future_20_gdirs[index] 61 | future_20_cpos = future_20_cposs[index] / 100 62 | future_20_cdir = future_20_cdirs[index] 63 | 64 | # global space viewer settings 65 | axg.cla() 66 | axg.set_title("global") 67 | axg.grid(axis="y") 68 | axg.set_yticklabels([]) 69 | axg.yaxis.pane.set_facecolor("gray") 70 | axg.yaxis.set_major_locator(ticker.NullLocator()) 71 | axg.yaxis.set_minor_locator(ticker.NullLocator()) 72 | axg.view_init(elev=120, azim=-90) 73 | axg.set_xlim3d(proj_root_pos[0]-3, proj_root_pos[0]+3) 74 | axg.set_ylim3d(0, 3) 75 | axg.set_zlim3d(proj_root_pos[1]-3, proj_root_pos[1]+3) 76 | axg.set_box_aspect((1, 0.5, 1)) 77 | axg.scatter(gposes[:, 0], gposes[:, 1], gposes[:, 2], c="red", s=3, label="joints") 78 | for i, parent in enumerate(parents): 79 | if parent != -1: 80 | axg.plot( 81 | [gposes[parent,0], gposes[i,0]], 82 | [gposes[parent,1], gposes[i,1]], 83 | [gposes[parent,2], gposes[i,2]], 84 | c="red", alpha=0.8 85 | ) 86 | axg.plot( 87 | [proj_root_pos[0], proj_root_pos[0]+root_dir[0]], 88 | [0, 0], 89 | [proj_root_pos[1], proj_root_pos[1]+root_dir[1]], 90 | label="root direction", c="blue" 91 | ) 92 | axg.plot( 93 | [future_20_gpos[0], future_20_gpos[0]+future_20_gdir[0]], 94 | [0, 0], 95 | [future_20_gpos[1], future_20_gpos[1]+future_20_gdir[1]], 96 | label="future direction 20", c="green" 97 | ) 98 | axg.legend(bbox_to_anchor=(0, 1), loc="upper left", borderaxespad=0) 99 | 100 | # character space viewer settings 101 | axc.cla() 102 | axc.set_title("character") 103 | axc.grid(axis="y") 104 | axc.set_yticklabels([]) 105 | axc.yaxis.pane.set_facecolor("gray") 106 | axc.yaxis.set_major_locator(ticker.NullLocator()) 107 | axc.yaxis.set_minor_locator(ticker.NullLocator()) 108 | axc.view_init(elev=120, azim=-90) 109 | axc.set_xlim3d(-3, 3) 110 | axc.set_ylim3d(0, 3) 111 | axc.set_zlim3d(-3, 3) 112 | axc.set_box_aspect((1,0.5,1)) 113 | axc.scatter(cposes[:, 0], cposes[:, 1], cposes[:, 2], c="red", s=3, label="joints") 114 | for i, parent in enumerate(parents): 115 | if parent != -1: 116 | axc.plot( 117 | [cposes[parent,0], cposes[i,0]], 118 | [cposes[parent,1], cposes[i,1]], 119 | [cposes[parent,2], cposes[i,2]], 120 | c="red", alpha=0.8 121 | ) 122 | axc.plot([0, 0], [0, 0], [0, 1], label="root direction", c="blue") 123 | axc.plot( 124 | [future_20_cpos[0], future_20_cpos[0]+future_20_cdir[0]], 125 | [0, 0], 126 | [future_20_cpos[1], future_20_cpos[1]+future_20_cdir[1]], 127 | label="future direction 20", c="green" 128 | ) 129 | 130 | # features bar settings 131 | axf.cla() 132 | axf.set_title("matching features") 133 | axf.grid(axis="x") 134 | axf.set_xlim(-bar_lim, bar_lim) 135 | axf.barh(np.arange(len(features)), features) 136 | 137 | fig.suptitle("frame: {}".format(index+1)) 138 | sys.stdout.write("processed frame: {}\n".format(index+1)) 139 | 140 | ani = FuncAnimation(fig, update, frames=np.arange(len(db)), interval=1000/anim.fps, repeat=False) 141 | 142 | if save_fig: 143 | fig_path = (BASEPATH / "figs" / bvh_file).with_suffix(".gif") 144 | ani.save(fig_path) 145 | sys.stdout.write("Saved figure as " + str(fig_path) + "\n") 146 | else: 147 | plt.show() -------------------------------------------------------------------------------- /anitaichi/animation/anim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import taichi as ti 3 | from anitaichi.animation.skel import Skel 4 | from anitaichi.transform import ti_quat, quat 5 | 6 | def forward_kinematics_rotations( 7 | parents: list[int], # (J) 8 | rots: np.ndarray, # (T, J, 4) or (J, 4) 9 | ): 10 | grots = [] 11 | for i, pa in enumerate(parents): 12 | if pa == -1: 13 | grots.append(rots[...,i:i+1,:]) 14 | else: 15 | grots.append(quat.mul(grots[pa], rots[...,i:i+1,:])) 16 | return np.concatenate(grots, axis=-2) 17 | 18 | # Forward Kinematics using numpy. 19 | # (Since it is difficult to parallelize each bone, use this for Pose FK.) 20 | def forward_kinematics( 21 | offsets: np.ndarray, # (J, 3) 22 | parents: list[int], # (J) 23 | grots: np.ndarray, # (T, J, 4) or (J, 4) 24 | trans: np.ndarray # (T, 3) or (3) 25 | ) -> np.ndarray: 26 | gposs = [] 27 | offsets = offsets[None].repeat(len(grots), axis=0) 28 | for i, pa in enumerate(parents): 29 | if pa == -1: 30 | gposs.append(trans[..., None, :]) 31 | else: 32 | gposs.append(quat.mul_vec(grots[..., pa:pa+1, :], offsets[..., i:i+1, :]) + gposs[pa]) 33 | return np.concatenate(gposs, axis=-2) 34 | 35 | # Forward kinematics using taichi. 36 | # (Parallelization in the time direction.) 37 | @ti.kernel 38 | def ti_forward_kinematics( 39 | parents: ti.template(), # (J) 40 | offsets: ti.template(), # (J) * 3 41 | lrots: ti.template(), # (T, J) * 4 42 | grots: ti.template(), # (T, J) * 4 43 | gposs: ti.template(), # (T, J) * 3 44 | ): 45 | # parallelize time dimention. 46 | for i in ti.ndrange(grots.shape[0]): 47 | for j in ti.ndrange(grots.shape[1]): 48 | if parents[j] == -1: 49 | grots[i, j] = lrots[i, j] 50 | gposs[i, j] = offsets[j] 51 | else: 52 | grots[i, j] = ti_quat.mul(grots[i, parents[j]], lrots[i, j]) 53 | gposs[i, j] = ti_quat.mul_vec3(grots[i, parents[j]], offsets[j]) + gposs[i, parents[j]] 54 | 55 | # @ti.kernel 56 | # def calc_local_positions( 57 | # parents: ti.template(), # (J) 58 | # offsets: ti.template(), # (J) * 3 59 | # grots: ti.template(), # (T, J) * 4 60 | # lposs: ti.template() # (T, J) * 3 61 | # ): 62 | # for i, j in grots: 63 | # if parents[j] == -1: 64 | # lposs[i, j] = offsets[j] 65 | # else: 66 | # lposs[i, j] = ti_quat.mul_vec3(grots[i, j], offsets[j]) 67 | 68 | # Convert numpy array to taichi vector field. 69 | def convert_to_vector_field(array: np.ndarray): 70 | shape, dim_vector = array.shape[:-1], array.shape[-1] 71 | field = ti.Vector.field(dim_vector, dtype=float, shape=shape) 72 | field.from_numpy(array) 73 | return field 74 | 75 | 76 | class Pose: 77 | def __init__( 78 | self, 79 | skel: Skel, 80 | rots: np.ndarray, 81 | root_pos: np.ndarray, 82 | ) -> None: 83 | self.skel = skel 84 | self.rots = rots 85 | self.root_pos = root_pos 86 | 87 | @property 88 | def local_rotations(self): 89 | return self.rots 90 | 91 | @property 92 | def global_rotations(self): 93 | parents = self.skel.parents 94 | lrots = self.rots 95 | grots = forward_kinematics_rotations(parents, lrots) 96 | return grots 97 | 98 | @property 99 | def global_positions(self): 100 | offsets = self.skel.offsets 101 | parents = self.skel.parents 102 | grots = self.global_rotations 103 | trans = self.root_pos 104 | gposs = forward_kinematics(offsets, parents, grots, trans) 105 | return gposs 106 | 107 | 108 | class Animation: 109 | def __init__( 110 | self, 111 | skel: Skel, 112 | rots: np.ndarray, 113 | trans: np.ndarray, 114 | fps: int, 115 | anim_name: str, 116 | ) -> None: 117 | self.skel = skel 118 | self.rots = rots 119 | self.trans = trans 120 | 121 | self.fps = fps 122 | self.anim_name = anim_name 123 | 124 | def __len__(self): 125 | return len(self.rots) 126 | 127 | @property 128 | def local_rotations(self): 129 | return self.rots 130 | 131 | @property 132 | def global_rotations(self): 133 | parents = self.skel.parents 134 | lrots = self.rots 135 | grots = forward_kinematics_rotations(parents, lrots) 136 | return grots 137 | 138 | # @property 139 | # def local_positions_field(self): 140 | # parents = self.skel.parents_field 141 | # offsets = self.skel.offsets_field 142 | # grots = self.global_rotations 143 | # lposs = ti.Vector.field(3, dtype=float, shape=grots.shape) 144 | # calc_local_positions(parents, offsets, grots, lposs) 145 | # return lposs 146 | 147 | @property 148 | def global_positions(self): 149 | offsets = self.skel.offsets 150 | parents = self.skel.parents 151 | grots = self.global_rotations 152 | trans = self.trans 153 | gposs = forward_kinematics(offsets, parents, grots, trans) 154 | return gposs 155 | 156 | def ti_fk(self): 157 | parents = self.skel.parents_field # (J) 158 | offsets = self.skel.offsets_field # (J) * 3 159 | lrots = convert_to_vector_field(self.local_rotations) # (T, J) * 4 160 | grots = ti.Vector.field(4, dtype=float, shape=lrots.shape) # (T, J) * 4 161 | gposs = ti.Vector.field(3, dtype=float, shape=lrots.shape) # (T, J) * 3 162 | ti_forward_kinematics(parents, offsets, lrots, grots, gposs) 163 | self.global_rotations_field = grots 164 | self.global_positions_field = gposs 165 | 166 | def to_pose(self, frame_num: int) -> Pose: 167 | skel = self.skel 168 | rots = self.rots[frame_num] 169 | root_pos = self.trans[frame_num] 170 | return Pose(skel, rots, root_pos) 171 | 172 | def to_poses(self, frames: slice) -> list[Pose]: 173 | if isinstance(frames, int): 174 | return self.to_pose(frames) 175 | skel = self.skel 176 | anim_rots = self.rots[frames] 177 | anim_trans = self.trans[frames] 178 | poses = [] 179 | for rots, root_pos in zip(anim_rots, anim_trans): 180 | poses.append(Pose(skel, rots, root_pos)) 181 | return poses 182 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | 3 | cat 4 | 5 |

6 | 7 |

8 | CAT: Character Animation Tools for Python 9 |

10 | 11 | This repository includes scripts for Character Animation. 12 | All the code is written entirely in python. 13 | 14 | It is useful for pre-processing and post-processing motions in **Deep Learning**. 15 | It will be also useful for create character animations. 16 | 17 | ## :star: Requirements 18 | I tested on python3.10 (for match-case syntax). 19 | 20 | ### Package 21 | * NumPy 22 | * SciPy 23 | * matplotlib 24 | * chumpy (if you use vanilla SMPL for AIST++). 25 | * easydict 26 | 33 | 34 | ### Motion Data 35 | Some of the scripts in this repository need motion data below. 36 | Please download them and place them in [`data/`](data) or link them as symbolic links at [`data/`](data). For more information please see [`data/data.md`](data/data.md). 37 | * [Ubisoft La Forge Animation Dataset (LAFAN1)](https://github.com/ubisoft/ubisoft-laforge-animation-dataset) 38 | * [SMPL](https://smpl.is.tue.mpg.de/) 39 | * [SMPL+H](https://mano.is.tue.mpg.de/) 40 | * [SMPL-X](https://smpl-x.is.tue.mpg.de/) 41 | * [AMASS](https://amass.is.tue.mpg.de/) 42 | * [AIST++](https://google.github.io/aistplusplus_dataset/factsfigures.html) 43 | 44 | 45 | ## :question: How to use? 46 | 47 | ### 1. Load and Save Animation 48 |
49 | open 50 | 51 | #### 1.1 Load Animation from [bvh](https://research.cs.wisc.edu/graphics/Courses/cs-838-1999/Jeff/BVH.html) file. 52 | ```Python 53 | from anim import bvh 54 | from anim.animation import Animation 55 | anim_bvh: Animation = bvh.load(filepath="data/**.bvh") 56 | ``` 57 | 58 | #### 1.2 Load Animation from [AIST++](https://google.github.io/aistplusplus_dataset/factsfigures.html). 59 | You need to install chumpy to use vanilla SMPL model. 60 | ```Python 61 | from anim import aistpp 62 | anim: Animation = aistpp.load( 63 | aistpp_motion_path="data/aistpp/**.pkl", 64 | smpl_path="data/smpl/neutral/model.pkl" 65 | ) 66 | ``` 67 | 68 | #### 1.3 Load Animation from [AMASS](https://amass.is.tue.mpg.de/). 69 | I recommend you to download extended SMPL+H model (16 beta components). 70 | ```Python 71 | from anim import amass 72 | anim: Animation = amass.load( 73 | amass_motion_path="data/amass/**.npz", 74 | smplh_path="data/smplh/neutral/model.npz" 75 | ) 76 | ``` 77 | 78 | #### 1.4 Save as bvh. 79 | You can convert SMPL based motion files (AIST++, AMASS) to BVH files. 80 | ```Python 81 | from anim import bvh 82 | from anim.animation import Animation 83 | 84 | ... 85 | 86 | anim: Animation 87 | bvh.save( 88 | filepath="data/***.bvh", 89 | anim=anim 90 | ) 91 | 92 | ``` 93 | 94 | 98 | 99 |
100 | 101 | 102 | ### 2. Get motion features 103 |
104 | open 105 | 106 | #### 2.1 Get positions (global, root-centric, character space). 107 | ```Python 108 | import numpy as np 109 | from anim.animation import Animation 110 | 111 | ... 112 | 113 | anim: Animation 114 | global_positions: np.ndarray = anim.gpos 115 | rcentric_positions: np.ndarray = anim.rtpos 116 | cspace_positions: np.ndarray = anim.cpos 117 | ``` 118 | 119 | #### 2.2 Get velocities(positions, rotations). 120 | ```Python 121 | anim: Animation 122 | pos_velocities: np.ndarray = anim.gposvel 123 | rot_velocities: np.ndarray = anim.lrotvel 124 | ``` 125 | 126 | #### 2.3 Get mirrored Animation. 127 | (**caution**: Skel offsets must be symmetric.) 128 | ```Python 129 | anim: Animation 130 | anim_M: Animation = anim.mirror() 131 | ``` 132 |
133 | 134 | 135 | ### 3. Inverse Kinematics 136 |
137 | open 138 | 139 | #### 3.1 Two bone IK 140 | Analytical method of foot IK example (define heels positon and knees forward vector). 141 | ```bash 142 | python anim/inverse_kinematics/two_bone_ik.py 143 | ``` 144 | ![two_bone_ik](figs/two_bone_ik.png) 145 | 146 | #### 3.2 CCD-IK 147 | Simple demo. 148 | ```bash 149 | python anim/inverse_kinematics/ccd_ik.py 150 | ``` 151 | ![ccd_ik](figs/ccd_ik.png) 152 | 153 | #### 3.3 FABRIK 154 | Simple demo. 155 | ```bash 156 | python anim/inverse_kinematics/fabrik.py 157 | ``` 158 | ![fabrik](figs/fabrik.png) 159 | 160 | 164 | 165 |
166 | 167 | 168 | ### 4. Motion Blending 169 |
170 | open 171 | 172 | #### 4.1 Linear blending for pose. 173 | TBD 174 | 175 |
176 | 177 | 178 | ### 5. Motion Matching 179 |
180 | open 181 | 182 | #### 5.1 Character Control by predefined trajectories 183 | 184 | ```python 185 | python test/path_following.py 186 | ``` 187 | ![sim_motion](figs/sim_motion.gif) 188 | ![sim_sidestep](figs/sim_motion_sidestep.gif) 189 | 190 | 196 | 197 |
198 | 199 | 267 | 268 | 269 | 272 | 273 | 274 | ## :eyes: Notification 275 | * [`util/quat.py`](util/quat.py) inspired by [Motion-Matching](https://github.com/orangeduck/Motion-Matching). 276 | * This repository is MIT licensed, but some datasets requires a separate license. Please check them. 277 | 278 | ## :speech_balloon: Contact 279 | This repository is under construction. 280 | Feel free to contact me on [issue](https://github.com/KosukeFukazawa/CharacterAnimationTools/issues). 281 | 282 | ## :books:License 283 | This code is distributed under an [MIT LICENSE](LICENSE). -------------------------------------------------------------------------------- /anim/inverse_kinematics/two_bone_ik.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | from pathlib import Path 5 | ROOT = Path(__file__).resolve().parent.parent.parent 6 | sys.path.append(str(ROOT)) 7 | 8 | import copy 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | from util import quat 12 | from anim.skel import Joint, Skel 13 | from anim.animation import Animation 14 | 15 | def normalize(tensor: np.ndarray, axis:int=-1) -> np.ndarray: 16 | """ Returns a tensor normalized in the `axis` dimension. 17 | args: 18 | tensor -> tensor / norm(tensor, axis=-axis) 19 | axis: dimemsion. 20 | """ 21 | norm = np.linalg.norm(tensor, axis=axis, keepdims=True) 22 | norm = np.where(norm==0, 1e-10, norm) 23 | return tensor / norm 24 | 25 | 26 | def two_bone_ik( 27 | anim: Animation, 28 | base_joint: int, 29 | middle_joint: int, 30 | eff_joint: int, 31 | tgt_pos: np.ndarray, 32 | fwd: np.ndarray, 33 | max_length_buffer: float=1e-3, 34 | ) -> Animation: 35 | """Simple two bone IK method. This algorithm can be solved analytically. 36 | args: 37 | anim: Animation 38 | base_joint: index of base (fixed) joint. 39 | middle_joint: index of middle joint. 40 | eff_joint: index of effector joint. 41 | tgt_pos: (T, 3), target positions for effector. 42 | fwd: (T, 3), forward vectors for middle joint. 43 | max_length_buffer: エラー回避用 44 | """ 45 | anim = copy.deepcopy(anim) 46 | tgt = tgt_pos.copy() 47 | 48 | # 連結チェック 49 | assert anim.parents[eff_joint] == middle_joint and \ 50 | anim.parents[middle_joint] == base_joint 51 | 52 | # 骨の最大の伸びを取得 53 | lbm: float = np.linalg.norm(anim.offsets[middle_joint], axis=-1) 54 | lme: float = np.linalg.norm(anim.offsets[eff_joint], axis=-1) 55 | max_length: float = lbm + lme - max_length_buffer 56 | 57 | bt_poss = tgt - anim.gpos[:, base_joint] # (T, 3) 58 | bt_lengths = np.linalg.norm(bt_poss, axis=-1) # (T) 59 | # targetが骨の届かない位置に存在するフレームを取得 60 | error_frame = np.where(bt_lengths > max_length) 61 | # base -> effectorの長さ1のベクトル 62 | bt_vec = normalize(bt_poss) # (T, 3) 63 | # 届かないフレームのみ更新 64 | tgt[error_frame] = anim.gpos[error_frame, base_joint] + \ 65 | bt_vec[error_frame] * max_length 66 | 67 | # get normal vector for axis of rotations. 68 | axis_rot = np.cross(bt_vec, fwd, axis=-1) 69 | axis_rot = normalize(axis_rot) # (T, 3) 70 | 71 | # get the relative positions. 72 | be_poss = anim.gpos[:, eff_joint] - anim.gpos[:, base_joint] 73 | be_vec = normalize(be_poss) # (T, 3) 74 | bm_poss = anim.gpos[:, middle_joint] - anim.gpos[:, base_joint] 75 | bm_vec = normalize(bm_poss) # (T, 3) 76 | me_poss = anim.gpos[:, eff_joint] - anim.gpos[:, middle_joint] 77 | me_vec = normalize(me_poss) # (T, 3) 78 | 79 | # get the current interior angles of the base and middle joints. 80 | # einsum calculate dot product. 81 | be_bm_0 = np.arccos(np.clip(np.einsum("ij,ij->i", be_vec, bm_vec), -1, 1)) # (T,) 82 | mb_me_0 = np.arccos(np.clip(np.einsum("ij,ij->i", -bm_vec, me_vec), -1, 1)) # (T,) 83 | 84 | # get the new interior angles of the base and middle joints. 85 | lbt = np.linalg.norm(tgt - anim.gpos[:, base_joint], axis=-1) 86 | be_bm_1 = np.arccos(np.clip((lbm * lbm + lbt * lbt - lme * lme)/(2 * lbm * lbt) , -1, 1)) # (T,) 87 | mb_me_1 = np.arccos(np.clip((lbm * lbm + lme * lme - lbt * lbt)/(2 * lbm * lme) , -1, 1)) # (T,) 88 | 89 | r0 = quat.from_angle_axis(be_bm_1 - be_bm_0, axis_rot) 90 | r1 = quat.from_angle_axis(mb_me_1 - mb_me_0, axis_rot) 91 | 92 | # effectorをtarget方向へ向けるための回転 93 | r2 = quat.from_angle_axis( 94 | np.arccos(np.clip(np.einsum("ij,ij->i", be_vec, bt_vec), -1, 1)), 95 | normalize(np.cross(be_vec, bt_vec)) 96 | ) 97 | 98 | anim.quats[:, base_joint] = quat.mul(r2, quat.mul(r0, anim.quats[:, base_joint])) 99 | anim.quats[:, middle_joint] = quat.mul(r1, anim.quats[:, middle_joint]) 100 | 101 | return anim 102 | 103 | if __name__=="__main__": 104 | """使用例""" 105 | 106 | # 簡単なJointの定義 107 | joints = [] 108 | joints.append(Joint(name="ROOT", parent=-1, offset=np.array([0, 2, 0]), root=True, dof=6)) 109 | joints.append(Joint(name="J1", parent= 0, offset=np.array([0, -1, 0.1]))) 110 | joints.append(Joint(name="J2", parent= 1, offset=np.array([0, -1, -0.1]))) 111 | joints.append(Joint(name="J3", parent= 2, offset=np.array([0, 0, 0.2]))) 112 | 113 | skel = Skel(joints=joints, skel_name="sample_skel") 114 | anim = Animation.no_animation(skel, num_frame=2, anim_name="sample_anim") 115 | anim.trans = np.array([ 116 | [0, 2, 0], 117 | [0, 2, 0] 118 | ]) 119 | 120 | # target positions for first frame and second frame. 121 | tgts = np.array([ 122 | [0, 1, 0], 123 | [0.2, 0.8, 0.2] 124 | ]) 125 | 126 | fwd = np.array([ 127 | [0.2, 0, 1], 128 | [0.4, 0.2, 0.5] 129 | ]) 130 | 131 | fwd_ = normalize(fwd) * 0.2 132 | 133 | new_anim = two_bone_ik( 134 | anim=anim, 135 | base_joint=0, 136 | middle_joint=1, 137 | eff_joint=2, 138 | tgt_pos=tgts, 139 | fwd=fwd) 140 | 141 | fig = plt.figure(figsize=(11, 7)) 142 | 143 | # first frame 144 | ax1 = fig.add_subplot(121, projection="3d") 145 | ax1.set_xlim3d(-1, 1) 146 | ax1.set_ylim3d(-1, 1) 147 | ax1.set_zlim3d( 0, 2) 148 | ax1.set_box_aspect((1,1,1)) 149 | 150 | orig = anim.gpos[0] 151 | poss = new_anim.gpos[0] 152 | tgt = tgts[0] 153 | 154 | ax1.plot(orig[:,0], -orig[:,2], orig[:,1], alpha=0.4) 155 | ax1.scatter(orig[:,0], -orig[:,2], orig[:,1], label="orig", alpha=0.4) 156 | ax1.plot(poss[:,0], -poss[:,2], poss[:,1]) 157 | ax1.scatter(poss[:,0], -poss[:,2], poss[:,1], label="calc") 158 | ax1.plot( 159 | [poss[1,0], poss[1,0]+fwd_[0, 0]], 160 | [-poss[1,2], -poss[1,2]-fwd_[0, 2]], 161 | [poss[1,1], poss[1,1]+fwd_[0, 1]], label="fwd") 162 | ax1.scatter(tgt[0], -tgt[2], tgt[1], s=80, color="red", marker="*", label="target") 163 | ax1.legend() 164 | 165 | # second frame 166 | ax2 = fig.add_subplot(122, projection="3d") 167 | ax2.set_xlim3d(-1, 1) 168 | ax2.set_ylim3d(-1, 1) 169 | ax2.set_zlim3d(0, 2) 170 | ax2.set_box_aspect((1,1,1)) 171 | 172 | orig = anim.gpos[1] 173 | poss = new_anim.gpos[1] 174 | tgt = tgts[1] 175 | 176 | ax2.plot(orig[:,0], -orig[:,2], orig[:,1], alpha=0.4) 177 | ax2.scatter(orig[:,0], -orig[:,2], orig[:,1], label="orig", alpha=0.4) 178 | ax2.plot(poss[:,0], -poss[:,2], poss[:,1]) 179 | ax2.scatter(poss[:,0], -poss[:,2], poss[:,1], label="calc") 180 | ax2.plot( 181 | [poss[1,0], poss[1,0]+fwd_[1, 0]], 182 | [-poss[1,2], -poss[1,2]-fwd_[1, 2]], 183 | [poss[1,1], poss[1,1]+fwd_[1, 1]], label="fwd") 184 | ax2.scatter(tgt[0], -tgt[2], tgt[1], s=80, color="red", marker="*", label="target") 185 | ax2.legend() 186 | 187 | plt.show() -------------------------------------------------------------------------------- /anim/smpl.py: -------------------------------------------------------------------------------- 1 | """ 2 | smpl.py 3 | 4 | JOINT_NAMES, SMPLH_JOINT_NAMES, SMPL_JOINT_NAMES are from \ 5 | https://github.com/vchoutas/smplx/blob/main/smplx/joint_names.py 6 | """ 7 | 8 | from __future__ import annotations 9 | 10 | from pathlib import Path 11 | import numpy as np 12 | from util.load import pickle_load 13 | 14 | JOINT_NAMES = [ 15 | "pelvis", 16 | "left_hip", 17 | "right_hip", 18 | "spine1", 19 | "left_knee", 20 | "right_knee", 21 | "spine2", 22 | "left_ankle", 23 | "right_ankle", 24 | "spine3", 25 | "left_foot", 26 | "right_foot", 27 | "neck", 28 | "left_collar", 29 | "right_collar", 30 | "head", 31 | "left_shoulder", 32 | "right_shoulder", 33 | "left_elbow", 34 | "right_elbow", 35 | "left_wrist", 36 | "right_wrist", 37 | "jaw", 38 | "left_eye_smplhf", 39 | "right_eye_smplhf", 40 | "left_index1", 41 | "left_index2", 42 | "left_index3", 43 | "left_middle1", 44 | "left_middle2", 45 | "left_middle3", 46 | "left_pinky1", 47 | "left_pinky2", 48 | "left_pinky3", 49 | "left_ring1", 50 | "left_ring2", 51 | "left_ring3", 52 | "left_thumb1", 53 | "left_thumb2", 54 | "left_thumb3", 55 | "right_index1", 56 | "right_index2", 57 | "right_index3", 58 | "right_middle1", 59 | "right_middle2", 60 | "right_middle3", 61 | "right_pinky1", 62 | "right_pinky2", 63 | "right_pinky3", 64 | "right_ring1", 65 | "right_ring2", 66 | "right_ring3", 67 | "right_thumb1", 68 | "right_thumb2", 69 | "right_thumb3", 70 | "nose", 71 | "right_eye", 72 | "left_eye", 73 | "right_ear", 74 | "left_ear", 75 | "left_big_toe", 76 | "left_small_toe", 77 | "left_heel", 78 | "right_big_toe", 79 | "right_small_toe", 80 | "right_heel", 81 | "left_thumb", 82 | "left_index", 83 | "left_middle", 84 | "left_ring", 85 | "left_pinky", 86 | "right_thumb", 87 | "right_index", 88 | "right_middle", 89 | "right_ring", 90 | "right_pinky", 91 | "right_eye_brow1", 92 | "right_eye_brow2", 93 | "right_eye_brow3", 94 | "right_eye_brow4", 95 | "right_eye_brow5", 96 | "left_eye_brow5", 97 | "left_eye_brow4", 98 | "left_eye_brow3", 99 | "left_eye_brow2", 100 | "left_eye_brow1", 101 | "nose1", 102 | "nose2", 103 | "nose3", 104 | "nose4", 105 | "right_nose_2", 106 | "right_nose_1", 107 | "nose_middle", 108 | "left_nose_1", 109 | "left_nose_2", 110 | "right_eye1", 111 | "right_eye2", 112 | "right_eye3", 113 | "right_eye4", 114 | "right_eye5", 115 | "right_eye6", 116 | "left_eye4", 117 | "left_eye3", 118 | "left_eye2", 119 | "left_eye1", 120 | "left_eye6", 121 | "left_eye5", 122 | "right_mouth_1", 123 | "right_mouth_2", 124 | "right_mouth_3", 125 | "mouth_top", 126 | "left_mouth_3", 127 | "left_mouth_2", 128 | "left_mouth_1", 129 | "left_mouth_5", # 59 in OpenPose output 130 | "left_mouth_4", # 58 in OpenPose output 131 | "mouth_bottom", 132 | "right_mouth_4", 133 | "right_mouth_5", 134 | "right_lip_1", 135 | "right_lip_2", 136 | "lip_top", 137 | "left_lip_2", 138 | "left_lip_1", 139 | "left_lip_3", 140 | "lip_bottom", 141 | "right_lip_3", 142 | # Face contour 143 | "right_contour_1", 144 | "right_contour_2", 145 | "right_contour_3", 146 | "right_contour_4", 147 | "right_contour_5", 148 | "right_contour_6", 149 | "right_contour_7", 150 | "right_contour_8", 151 | "contour_middle", 152 | "left_contour_8", 153 | "left_contour_7", 154 | "left_contour_6", 155 | "left_contour_5", 156 | "left_contour_4", 157 | "left_contour_3", 158 | "left_contour_2", 159 | "left_contour_1", 160 | ] 161 | 162 | 163 | SMPLH_JOINT_NAMES = [ 164 | "pelvis", 165 | "left_hip", 166 | "right_hip", 167 | "spine1", 168 | "left_knee", 169 | "right_knee", 170 | "spine2", 171 | "left_ankle", 172 | "right_ankle", 173 | "spine3", 174 | "left_foot", 175 | "right_foot", 176 | "neck", 177 | "left_collar", 178 | "right_collar", 179 | "head", 180 | "left_shoulder", 181 | "right_shoulder", 182 | "left_elbow", 183 | "right_elbow", 184 | "left_wrist", 185 | "right_wrist", 186 | "left_index1", 187 | "left_index2", 188 | "left_index3", 189 | "left_middle1", 190 | "left_middle2", 191 | "left_middle3", 192 | "left_pinky1", 193 | "left_pinky2", 194 | "left_pinky3", 195 | "left_ring1", 196 | "left_ring2", 197 | "left_ring3", 198 | "left_thumb1", 199 | "left_thumb2", 200 | "left_thumb3", 201 | "right_index1", 202 | "right_index2", 203 | "right_index3", 204 | "right_middle1", 205 | "right_middle2", 206 | "right_middle3", 207 | "right_pinky1", 208 | "right_pinky2", 209 | "right_pinky3", 210 | "right_ring1", 211 | "right_ring2", 212 | "right_ring3", 213 | "right_thumb1", 214 | "right_thumb2", 215 | "right_thumb3", 216 | "nose", 217 | "right_eye", 218 | "left_eye", 219 | "right_ear", 220 | "left_ear", 221 | "left_big_toe", 222 | "left_small_toe", 223 | "left_heel", 224 | "right_big_toe", 225 | "right_small_toe", 226 | "right_heel", 227 | "left_thumb", 228 | "left_index", 229 | "left_middle", 230 | "left_ring", 231 | "left_pinky", 232 | "right_thumb", 233 | "right_index", 234 | "right_middle", 235 | "right_ring", 236 | "right_pinky", 237 | ] 238 | 239 | SMPL_JOINT_NAMES = [ 240 | "pelvis", 241 | "left_hip", 242 | "right_hip", 243 | "spine1", 244 | "left_knee", 245 | "right_knee", 246 | "spine2", 247 | "left_ankle", 248 | "right_ankle", 249 | "spine3", 250 | "left_foot", 251 | "right_foot", 252 | "neck", 253 | "left_collar", 254 | "right_collar", 255 | "head", 256 | "left_shoulder", 257 | "right_shoulder", 258 | "left_elbow", 259 | "right_elbow", 260 | "left_wrist", 261 | "right_wrist", 262 | "left_hand", 263 | "right_hand", 264 | ] 265 | 266 | def load_model(model_path: Path, gender: str=None): 267 | if isinstance(model_path, str): 268 | model_path = Path(model_path) 269 | if model_path.suffix == "": 270 | model_path = model_path / gender / "model.npz" 271 | match model_path.suffix: 272 | case ".npz": 273 | model_dict = np.load(model_path, allow_pickle=True) 274 | case ".pkl": 275 | try: 276 | model_dict = pickle_load(model_path) 277 | except: 278 | model_dict = pickle_load(model_path, encoding="latin1") 279 | case _ : 280 | ValueError("This file is not supported.") 281 | return model_dict 282 | 283 | def calc_skel_offsets( 284 | betas: np.ndarray, 285 | J_regressor: np.ndarray, 286 | shapedirs: np.ndarray, 287 | v_template: np.ndarray, 288 | ) -> np.ndarray: 289 | """ 290 | args: 291 | betas (np.ndarray): body shape parameters of SMPL. shape: (num_betas). 292 | J_regressor (np.ndarray): Joint regressor from SMPL vertices. shape: (num_J, num_vertices). 293 | shapedirs (np.ndarray): Matrix to convert from PCA (betas) to SMPL blendshapes. shape: (num_vertices, 3, num_betas). 294 | v_template (np.ndarray): Template body vertices of SMPL. shape: (num_vertices, 3). 295 | return: 296 | joints (np.ndarray): Offset joint positons in 3D space (absolute coordinates). shape: (num_J, 3). 297 | """ 298 | num_betas = min(len(betas), shapedirs.shape[-1]) 299 | betas, shapedirs = betas[:num_betas], shapedirs[..., :num_betas] 300 | blendshape = shapedirs @ betas 301 | vertices = v_template + blendshape 302 | joints = J_regressor @ vertices 303 | 304 | return joints 305 | 306 | -------------------------------------------------------------------------------- /anim/skel.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | import numpy as np 5 | 6 | @dataclass 7 | class Joint: 8 | name: str 9 | parent: int 10 | offset: np.ndarray 11 | root: bool = False 12 | dof: int = 3 13 | 14 | def axis_to_vector(axis: str): 15 | match axis: 16 | case "x": 17 | return [1, 0, 0] 18 | case "y": 19 | return [0, 1, 0] 20 | case "z": 21 | return [0, 0, 1] 22 | case "-x": 23 | return [-1, 0, 0] 24 | case "-y": 25 | return [0, -1, 0] 26 | case "-z": 27 | return [0, 0, -1] 28 | case _: 29 | raise ValueError 30 | 31 | class Skel: 32 | def __init__( 33 | self, 34 | joints: list[Joint], 35 | skel_name: str="skeleton", 36 | rest_forward: list[int]=[0, 0, 1], 37 | rest_vertical: list[int]=[0, 1, 0], 38 | forward_axis: str="z", 39 | vertical_axis: str="y", 40 | ) -> None: 41 | """Class for skeleton offset definition. 42 | 43 | Args: 44 | joints (list[Joint]): list of Joints. 45 | skel_name (str, optional): name of skeleton. Defaults to "skeleton". 46 | rest_forward (list[int], optional): forward direction of the rest pose. Defaults to [0, 0, 1]. 47 | rest_vertical (list[int], optional): vertical direction of the rest pose. Defaults to [0, 1, 0]. 48 | forward_axis (str, optional): forward axis of the coodinates. Defaults to "z". 49 | vertical_axis (str, optional): vertical axis of the coodinates. Defaults to "y". 50 | """ 51 | self.skel_name = skel_name 52 | self.joints = joints 53 | self.rest_forward = rest_forward 54 | self.rest_vertical = rest_vertical 55 | self.forward_axis = forward_axis 56 | self.forward = axis_to_vector(forward_axis) 57 | self.vertical_axis = vertical_axis 58 | self.vertical = axis_to_vector(vertical_axis) 59 | 60 | def __len__(self) -> int: 61 | return len(self.joints) 62 | 63 | def __getitem__(self, index: int | slice | str) -> Joint | list[Joint]: 64 | return self.get_joint(index) 65 | 66 | @property 67 | def indices(self) -> list[int]: 68 | return [idx for idx in range(len(self))] 69 | 70 | @property 71 | def parents(self) -> list[int]: 72 | """Get list of all joint's parent indices.""" 73 | return [j.parent for j in self.joints] 74 | 75 | @property 76 | def children(self) -> dict[int: list[int]]: 77 | children_dict = {} 78 | for i in range(len(self)): 79 | children_dict[i] = [] 80 | for i, parent in enumerate(self.parents): 81 | if parent == -1: 82 | continue 83 | children_dict[parent].append(i) 84 | return children_dict 85 | 86 | @property 87 | def names(self) -> list[str]: 88 | """Get all joints names.""" 89 | return [j.name for j in self.joints] 90 | 91 | @property 92 | def offsets(self) -> np.ndarray: 93 | """Offset coordinates of all joints (np.ndarray).""" 94 | offsets: list[np.ndarray] = [] 95 | for joint in self.joints: 96 | offsets.append(joint.offset) 97 | return np.array(offsets) 98 | 99 | @property 100 | def dofs(self) -> list[int]: 101 | return [j.dof for j in self.joints] 102 | 103 | @property 104 | def joint_depths(self) -> list[int]: 105 | """Get hierarchical distance between joints to ROOT.""" 106 | def get_depth(joint_idx: int, cur_depth: int=0): 107 | parent_idx = self.parents[joint_idx] 108 | if parent_idx != -1: 109 | depth = cur_depth + 1 110 | cur_depth = get_depth(parent_idx, depth) 111 | return cur_depth 112 | return [get_depth(idx) for idx in self.indices] 113 | 114 | @property 115 | def bone_lengths(self) -> np.ndarray: 116 | lengths = np.linalg.norm(self.offsets, axis=-1) 117 | lengths[0] = 0 118 | return lengths 119 | 120 | def get_joint(self, index: int | slice | str) -> Joint | list[Joint]: 121 | """Get Joint from index or slice or joint name.""" 122 | if isinstance(index, str): 123 | index: int = self.get_index_from_jname(index) 124 | return self.joints[index] 125 | 126 | def get_index_from_jname(self, jname: str) -> int: 127 | """Get joint index from joint name.""" 128 | jname_list = self.names 129 | return jname_list.index(jname) 130 | 131 | def get_children( 132 | self, 133 | index: int | str, 134 | return_idx: bool = False 135 | ) -> list[Joint] | list[int]: 136 | """Get list of children joints or children indices. 137 | args: 138 | index: index of joint or name of joint. 139 | return_idx: if True, return joint index. 140 | return: 141 | list of joints or list of indices (if return_idx). 142 | """ 143 | if isinstance(index, str): 144 | index: int = self.get_index_from_jname(index) 145 | 146 | children_idx = self.children[index] 147 | if return_idx: 148 | return children_idx 149 | else: 150 | children = [] 151 | for child_idx in children_idx: 152 | children.append(self.joints[child_idx]) 153 | return children 154 | 155 | def get_parent( 156 | self, 157 | index: int | str, 158 | return_idx: bool = False 159 | ) -> Joint | int | None: 160 | """Get parent joint or parent index. 161 | args: 162 | index (int | str): index of joint or name of joint. 163 | return_idx (bool): if True, return joint index. 164 | return: 165 | None : if parent doesn't exists. 166 | index (int): if return_idx = True. 167 | joint (Joint): if return_idx = False. 168 | """ 169 | if isinstance(index, str): 170 | _index: int = self.get_index_from_jname(index) 171 | elif isinstance(index, int): 172 | _index: int = index 173 | 174 | if return_idx: return self.parents[_index] 175 | elif self.parents[_index] == -1: 176 | return None 177 | else: 178 | return self.joints[self.parents[_index]] 179 | 180 | @staticmethod 181 | def from_names_parents_offsets( 182 | names: list[str], 183 | parents: list[int], 184 | offsets: np.ndarray, 185 | skel_name: str="skeleton", 186 | rest_forward: list[int]=[0, 0, 1], 187 | rest_vertical: list[int] = [0, 1, 0], 188 | forward_axis: str = "z", 189 | vertical_axis: str = "y" 190 | ) -> Skel: 191 | """Construct new Skel from names, parents, offsets. 192 | 193 | args: 194 | names(list[str]) : list of joints names. 195 | parents(list[int]) : list of joints parents indices. 196 | offsets(np.ndarray): joint offset relative coordinates from parent joints. 197 | skel_name (str, optional): name of skeleton. Defaults to "skeleton". 198 | rest_forward (list[int], optional): forward direction of the rest pose. Defaults to [0, 0, 1]. 199 | rest_vertical (list[int], optional): vertical direction of the rest pose. Defaults to [0, 1, 0]. 200 | forward_axis (str, optional): forward axis of the coodinates. Defaults to "z". 201 | vertical_axis (str, optional): vertical axis of the coodinates. Defaults to "y". 202 | return: 203 | Skel 204 | """ 205 | ln, lp, lo = len(names), len(parents), len(offsets) 206 | assert len(set([ln, lp, lo])) == 1 207 | joints = [] 208 | for name, parent, offset in zip(names, parents, offsets): 209 | dof = 6 if parent == -1 else 3 210 | joints.append(Joint(name, parent, offset, (parent==-1), dof)) 211 | return Skel(joints, skel_name, rest_forward, rest_vertical, forward_axis, vertical_axis) -------------------------------------------------------------------------------- /anitaichi/animation/anim_loader/bvh.py: -------------------------------------------------------------------------------- 1 | """ 2 | bvh.py 3 | """ 4 | # loading and writing a biovision hierarchy data (BVH). 5 | 6 | from __future__ import annotations 7 | 8 | from io import TextIOWrapper 9 | from pathlib import Path 10 | import logging 11 | import numpy as np 12 | 13 | from anitaichi.animation.anim import Animation 14 | from anitaichi.animation.skel import Skel, Joint 15 | from anitaichi.transform import quat 16 | 17 | def load( 18 | filepath: Path | str, 19 | start: int=None, 20 | end: int=None, 21 | order: str=None, 22 | load_skel: bool=True, 23 | load_pose: bool=True, 24 | skel: Skel=None, 25 | skel_name: str=None, 26 | ) -> Skel | Animation: 27 | 28 | if not load_skel and not load_pose: 29 | logging.info("Either load_skel or load_pose must be specified.") 30 | raise ValueError 31 | 32 | if isinstance(filepath, str): 33 | filepath = Path(filepath) 34 | 35 | # List bvh file by each row (line) and each word (column). 36 | with open(filepath, "r") as f: 37 | lines: list[str] = [line.strip() for line in f if line != ""] 38 | motion_idx: int = lines.index("MOTION") 39 | lines: list[str | list[str]] = \ 40 | list(map(lambda x: x.split(), lines)) 41 | f.close() 42 | 43 | # Load HIERARCHY term. 44 | if load_skel: 45 | skel, order = load_hierarchy( 46 | lines = lines[:motion_idx], 47 | skel_name=skel_name, 48 | ) 49 | 50 | # Load MOTION term. 51 | if load_pose: 52 | assert not skel is None, "You need to load skeleton or define skeleton." 53 | name = filepath.name.split(".")[0] 54 | 55 | fps, trans, quats = load_motion( 56 | lines = lines[motion_idx:], 57 | start = start, 58 | end = end, 59 | order = order, 60 | skel = skel 61 | ) 62 | return Animation( 63 | skel=skel, 64 | rots=quats, 65 | trans=trans, 66 | fps=fps, 67 | anim_name=name, 68 | ) 69 | 70 | return skel 71 | 72 | def load_hierarchy( 73 | lines: list[str | list[str]], 74 | skel_name: str, 75 | ) -> Skel: 76 | 77 | channelmap: dict[str, str] = { 78 | "Xrotation" : "x", 79 | "Yrotation" : "y", 80 | "Zrotation" : "z", 81 | } 82 | 83 | stacks: list[int] = [-1] 84 | parents: list[int] = [] 85 | name_list: list[str] = [] 86 | idx = 0 87 | joints: list[Joint] = [] 88 | depth:int = 0 89 | end_site: bool = False 90 | 91 | for line in lines: 92 | if "ROOT" in line or "JOINT" in line: 93 | parents.append(stacks[-1]) 94 | stacks.append(len(parents) - 1) 95 | name_list.append(line[1]) 96 | 97 | elif "OFFSET" in line: 98 | if not end_site: 99 | offset = list(map(float, line[1:])) 100 | joints.append(Joint( 101 | name = name_list[-1], 102 | index = idx, 103 | parent = parents[-1], 104 | offset = offset, 105 | )) 106 | idx += 1 107 | 108 | elif "CHANNELS" in line: 109 | dof = int(line[1]) 110 | if dof == 3: 111 | order = "".join([channelmap[p] for p in line[2:2+3]]) 112 | elif dof == 6: 113 | order = "".join([channelmap[p] for p in line[5:5+3]]) 114 | joints[-1].dof = dof 115 | 116 | elif "End" in line: 117 | end_site = True 118 | stacks.append(len(parents) - 1) 119 | 120 | elif "{" in line: 121 | depth += 1 122 | 123 | elif "}" in line: 124 | depth -= 1 125 | end_site = False 126 | stacks.pop() 127 | 128 | assert depth == 0, "Brackets are not closed." 129 | 130 | skel = Skel(joints=joints, skel_name=skel_name) 131 | return skel, order 132 | 133 | def load_motion( 134 | lines: list[list[str]], 135 | order: str, 136 | skel: Skel, 137 | start: int=None, 138 | end: int=None, 139 | ) -> tuple[int, np.ndarray, np.ndarray]: 140 | 141 | fps: int = round(1 / float(lines[2][2])) 142 | lines: list[list[str]] = lines[3:] 143 | 144 | np_lines = np.array( 145 | list(map(lambda line: list(map(float, line)), lines)) 146 | ) # T × dim_J(J × 3 + 3 or J × 6) matrix. 147 | np_lines = np_lines[start:end] 148 | 149 | dofs = [] 150 | eangles = [] 151 | poss = [] 152 | ckpt = 0 153 | for joint in skel.joints: 154 | dof = joint.dof 155 | dofs.append(dof) 156 | if dof == 3: 157 | eangle = np_lines[:, ckpt:ckpt+3] 158 | eangles.append(eangle[:, None]) 159 | ckpt += 3 160 | elif dof == 6: 161 | pos = np_lines[:, ckpt:ckpt+3] # [T, 3] 162 | eangle = np_lines[:, ckpt+3:ckpt+6] 163 | poss.append(pos[:, None]) # [T, 1, 3] 164 | eangles.append(eangle[:, None]) # [T, 1, 3] 165 | ckpt += 6 166 | 167 | assert sum(dofs) == np_lines.shape[1], \ 168 | "Skel and Motion are not compatible." 169 | 170 | poss = np.concatenate(poss, axis=1) 171 | eangles = np.concatenate(eangles, axis=1) 172 | 173 | trans = poss[:, 0] 174 | quats = quat.unroll(quat.from_euler(eangles, order)) 175 | 176 | return fps, trans, quats 177 | 178 | def save( 179 | filepath: Path | str, 180 | anim: Animation, 181 | order: str="zyx", 182 | ) -> bool: 183 | 184 | skel = anim.skel 185 | trans = anim.trans.to_numpy() 186 | quats = anim.rots.to_numpy() 187 | fps = anim.fps 188 | 189 | with open(filepath, "w") as f: 190 | # write hierarchy data. 191 | f.write("HIERARCHY\n") 192 | index_order = save_hierarchy(f, skel, 0, order, 0) 193 | 194 | # write motion data. 195 | f.write("MOTION\n") 196 | f.write("Frames: %d\n" % len(trans)) 197 | f.write("Frame Time: %f\n" % (1.0 / fps)) 198 | save_motion(f, trans, quats, order, index_order) 199 | f.close() 200 | 201 | def save_hierarchy( 202 | f: TextIOWrapper, 203 | skel: Skel, 204 | index: int, 205 | order: str, 206 | depth: int, 207 | ) -> list[int]: 208 | 209 | def order2xyzrotation(order: str) -> str: 210 | cmap: dict[str, str] = { 211 | "x" : "Xrotation", 212 | "y" : "Yrotation", 213 | "z" : "Zrotation", 214 | } 215 | return "%s %s %s" % (cmap[order[0]], cmap[order[1]], cmap[order[2]]) 216 | 217 | joint = skel[index] 218 | index_order = [index] 219 | if joint.root: 220 | f.write("\t" * depth + "ROOT %s\n" % joint.name) 221 | else: 222 | f.write("\t" * depth + "JOINT %s\n" % joint.name) 223 | f.write("\t" * depth + "{\n") 224 | depth += 1 225 | offset = joint.offset 226 | f.write("\t" * depth + \ 227 | "OFFSET %f %f %f\n" % (offset[0], offset[1], offset[2])) 228 | 229 | if joint.root: 230 | f.write("\t" * depth + \ 231 | "CHANNELS 6 Xposition Yposition Zposition %s\n"\ 232 | % order2xyzrotation(order)) 233 | else: 234 | f.write("\t" * depth + "CHANNELS 3 %s\n"\ 235 | % order2xyzrotation(order)) 236 | 237 | children_idxs = skel.get_children(index, return_idx=True) 238 | for child_idx in children_idxs: 239 | ch_index_order = save_hierarchy(f, skel, child_idx, order, depth) 240 | index_order.extend(ch_index_order) 241 | if children_idxs == []: 242 | f.write("\t" * depth + "End Site\n") 243 | f.write("\t" * depth + "{\n") 244 | f.write("\t" * (depth + 1) + "OFFSET %f %f %f\n" \ 245 | % (0, 0, 0)) 246 | f.write("\t" * depth + "}\n") 247 | 248 | depth -= 1 249 | f.write("\t" * depth + "}\n") 250 | return index_order 251 | 252 | def save_motion( 253 | f: TextIOWrapper, 254 | trans: np.ndarray, 255 | quats: np.ndarray, 256 | order: str, 257 | index_order: list[int], 258 | ) -> None: 259 | 260 | def write_position_rotation( 261 | pos: np.ndarray, 262 | rot: np.ndarray, 263 | ) -> str: 264 | pos, rot = pos.tolist(), rot.tolist() 265 | return "%f %f %f %f %f %f " \ 266 | % (pos[0], pos[1], pos[2], rot[0], rot[1], rot[2]) 267 | 268 | def write_rotation(rot: np.ndarray) -> str: 269 | rot = rot.tolist() 270 | return "%f %f %f " %(rot[0], rot[1], rot[2]) 271 | 272 | eangles = np.rad2deg(quat.to_euler(quats, order)) # (T, J, 3) 273 | for i in range(len(trans)): 274 | for j in index_order: 275 | if j == 0: 276 | f.write( 277 | "%s" % write_position_rotation(trans[i], eangles[i, j]) 278 | ) 279 | else: 280 | f.write( 281 | "%s" % write_rotation(eangles[i, j]) 282 | ) 283 | f.write("\n") -------------------------------------------------------------------------------- /anim/bvh.py: -------------------------------------------------------------------------------- 1 | """ 2 | bvh.py 3 | """ 4 | # loading and writing a biovision hierarchy data (BVH). 5 | 6 | from __future__ import annotations 7 | 8 | from io import TextIOWrapper 9 | from pathlib import Path 10 | import logging 11 | import numpy as np 12 | from anim.skel import Joint, Skel 13 | from anim.animation import Animation 14 | from util import quat 15 | 16 | def load( 17 | filepath: Path | str, 18 | start: int=None, 19 | end: int=None, 20 | order: str=None, 21 | rest_forward: list[int] = [0, 0, 1], 22 | rest_vertical: list[int] = [0, 1, 0], 23 | forward_axis: str = "z", 24 | vertical_axis: str = "y", 25 | load_skel: bool=True, 26 | load_pose: bool=True, 27 | skel: Skel=None, 28 | skel_name: str=None, 29 | ) -> Skel | Animation: 30 | 31 | if not load_skel and not load_pose: 32 | logging.info("Either `load_skel` or `load_pose` must be specified.") 33 | raise ValueError 34 | 35 | if isinstance(filepath, str): 36 | filepath = Path(filepath) 37 | 38 | # List bvh file by each row (line) and each word (column). 39 | with open(filepath, "r") as f: 40 | lines: list[str] = [line.strip() for line in f if line != ""] 41 | motion_idx: int = lines.index("MOTION") 42 | lines: list[str | list[str]] = \ 43 | list(map(lambda x: x.split(), lines)) 44 | f.close() 45 | 46 | # Load HIERARCHY term. 47 | if load_skel: 48 | skel, order = load_hierarchy( 49 | lines = lines[:motion_idx], 50 | skel_name=skel_name, 51 | rest_forward=rest_forward, 52 | rest_vertical=rest_vertical, 53 | forward_axis=forward_axis, 54 | vertical_axis=vertical_axis 55 | ) 56 | 57 | # Load MOTION term. 58 | if load_pose: 59 | assert not skel is None, "You need to load `Skel` or define `Skel`." 60 | name = filepath.name.split(".")[0] 61 | 62 | fps, trans, quats, poss = load_motion( 63 | lines = lines[motion_idx:], 64 | start = start, 65 | end = end, 66 | order = order, 67 | skel = skel 68 | ) 69 | anim = Animation(skel, quats, trans, fps=fps, anim_name=name) 70 | positions = anim.gpos # [T, J, 3] 71 | for joint_idx, pos in poss.items(): 72 | positions[:, joint_idx] = pos 73 | anim.positions = positions 74 | return anim 75 | 76 | return skel 77 | 78 | def load_hierarchy( 79 | lines: list[str | list[str]], 80 | skel_name: str, 81 | rest_forward: list[int], 82 | rest_vertical: list[int], 83 | forward_axis: str, 84 | vertical_axis: str, 85 | ) -> Skel: 86 | 87 | channelmap: dict[str, str] = { 88 | "Xrotation" : "x", 89 | "Yrotation" : "y", 90 | "Zrotation" : "z", 91 | } 92 | 93 | stacks: list[int] = [-1] 94 | parents: list[int] = [] 95 | name_list: list[str] = [] 96 | joints: list[Joint] = [] 97 | depth:int = 0 98 | end_site: bool = False 99 | 100 | for line in lines: 101 | if "ROOT" in line or "JOINT" in line: 102 | parents.append(stacks[-1]) 103 | stacks.append(len(parents) - 1) 104 | name_list.append(line[1]) 105 | 106 | elif "OFFSET" in line: 107 | if not end_site: 108 | offset = np.array(list(map(float, line[1:]))) 109 | joints.append(Joint( 110 | name = name_list[-1], 111 | parent = parents[-1], 112 | offset = offset, 113 | root = (parents[-1]==-1), 114 | )) 115 | 116 | elif "CHANNELS" in line: 117 | dof = int(line[1]) 118 | if dof == 3: 119 | order = "".join([channelmap[p] for p in line[2:2+3]]) 120 | elif dof == 6: 121 | order = "".join([channelmap[p] for p in line[5:5+3]]) 122 | joints[-1].dof = dof 123 | 124 | elif "End" in line: 125 | end_site = True 126 | stacks.append(len(parents) - 1) 127 | 128 | elif "{" in line: 129 | depth += 1 130 | 131 | elif "}" in line: 132 | depth -= 1 133 | end_site = False 134 | stacks.pop() 135 | 136 | assert depth == 0, "Brackets are not closed." 137 | 138 | skel = Skel(joints, skel_name, rest_forward, rest_vertical, forward_axis, vertical_axis) 139 | return skel, order 140 | 141 | def load_motion( 142 | lines: list[list[str]], 143 | order: str, 144 | skel: Skel, 145 | start: int=None, 146 | end: int=None, 147 | ) -> tuple[int, np.ndarray, np.ndarray, dict[int: np.ndarray]]: 148 | 149 | fps: int = round(1 / float(lines[2][2])) 150 | lines: list[list[str]] = lines[3:] 151 | 152 | np_lines = np.array( 153 | list(map(lambda line: list(map(float, line)), lines)) 154 | ) # T × dim_J matrix 155 | np_lines = np_lines[start:end] 156 | 157 | eangles = [] 158 | poss = {} 159 | ckpt = 0 160 | joint_idx = 0 161 | for dof in skel.dofs: 162 | if dof == 3: 163 | eangle = np_lines[:, ckpt:ckpt+3] 164 | eangles.append(eangle[:, None]) 165 | ckpt += 3 166 | joint_idx += 1 167 | elif dof == 6: 168 | pos = np_lines[:, ckpt:ckpt+3] # [T, 3] 169 | eangle = np_lines[:, ckpt+3:ckpt+6] 170 | poss[joint_idx] = pos 171 | eangles.append(eangle[:, None]) 172 | ckpt += 6 173 | joint_idx += 1 174 | 175 | assert sum(skel.dofs) == np_lines.shape[1], \ 176 | "Skel and Motion are not compatible." 177 | 178 | eangles = np.concatenate(eangles, axis=1) 179 | 180 | trans = poss[0] 181 | quats = quat.unroll(quat.from_euler(eangles, order)) 182 | 183 | return fps, trans, quats, poss 184 | 185 | def save( 186 | filepath: Path | str, 187 | anim: Animation, 188 | order: str="zyx", 189 | save_pos: bool=False, 190 | ) -> bool: 191 | 192 | skel = anim.skel 193 | trans = anim.trans 194 | quats = anim.quats 195 | positions = anim.positions 196 | fps = anim.fps 197 | 198 | with open(filepath, "w") as f: 199 | # write hierarchy data. 200 | f.write("HIERARCHY\n") 201 | index_order = save_hierarchy(f, skel, 0, order, 0, save_pos) 202 | 203 | # write motion data. 204 | f.write("MOTION\n") 205 | f.write("Frames: %d\n" % len(trans)) 206 | f.write("Frame Time: %f\n" % (1.0 / fps)) 207 | save_motion(f, trans, quats, positions, save_pos, order, index_order) 208 | f.close() 209 | 210 | def save_hierarchy( 211 | f: TextIOWrapper, 212 | skel: Skel, 213 | index: int, 214 | order: str, 215 | depth: int, 216 | save_pos: bool, 217 | ) -> list[int]: 218 | 219 | def order2xyzrotation(order: str) -> str: 220 | cmap: dict[str, str] = { 221 | "x" : "Xrotation", 222 | "y" : "Yrotation", 223 | "z" : "Zrotation", 224 | } 225 | return "%s %s %s" % (cmap[order[0]], cmap[order[1]], cmap[order[2]]) 226 | 227 | joint = skel[index] 228 | index_order = [index] 229 | if joint.root: 230 | f.write("\t" * depth + "ROOT %s\n" % joint.name) 231 | else: 232 | f.write("\t" * depth + "JOINT %s\n" % joint.name) 233 | f.write("\t" * depth + "{\n") 234 | depth += 1 235 | offset = joint.offset 236 | f.write("\t" * depth + \ 237 | "OFFSET %f %f %f\n" % (offset[0], offset[1], offset[2])) 238 | 239 | if joint.root or save_pos: 240 | f.write("\t" * depth + \ 241 | "CHANNELS 6 Xposition Yposition Zposition %s\n"\ 242 | % order2xyzrotation(order)) 243 | else: 244 | f.write("\t" * depth + "CHANNELS 3 %s\n"\ 245 | % order2xyzrotation(order)) 246 | 247 | children_idxs = skel.get_children(index, return_idx=True) 248 | for child_idx in children_idxs: 249 | ch_index_order = save_hierarchy(f, skel, child_idx, order, depth, save_pos) 250 | index_order.extend(ch_index_order) 251 | if children_idxs == []: 252 | f.write("\t" * depth + "End Site\n") 253 | f.write("\t" * depth + "{\n") 254 | f.write("\t" * (depth + 1) + "OFFSET %f %f %f\n" \ 255 | % (0, 0, 0)) 256 | f.write("\t" * depth + "}\n") 257 | 258 | depth -= 1 259 | f.write("\t" * depth + "}\n") 260 | return index_order 261 | 262 | def save_motion( 263 | f: TextIOWrapper, 264 | trans: np.ndarray, 265 | quats: np.ndarray, 266 | positions: np.ndarray, 267 | save_pos: bool, 268 | order: str, 269 | index_order: list[int], 270 | ) -> None: 271 | 272 | def write_position_rotation( 273 | pos: np.ndarray, 274 | rot: np.ndarray, 275 | ) -> str: 276 | pos, rot = pos.tolist(), rot.tolist() 277 | return "%f %f %f %f %f %f " \ 278 | % (pos[0], pos[1], pos[2], rot[0], rot[1], rot[2]) 279 | 280 | def write_rotation(rot: np.ndarray) -> str: 281 | rot = rot.tolist() 282 | return "%f %f %f " %(rot[0], rot[1], rot[2]) 283 | 284 | eangles = np.rad2deg(quat.to_euler(quats, order)) # (T, J, 3) 285 | for i in range(len(trans)): 286 | for j in index_order: 287 | if j == 0: 288 | f.write( 289 | "%s" % write_position_rotation(trans[i], eangles[i, j]) 290 | ) 291 | elif save_pos: 292 | f.write( 293 | "%s" % write_position_rotation(positions[i, j], eangles[i, j]) 294 | ) 295 | else: 296 | f.write( 297 | "%s" % write_rotation(eangles[i, j]) 298 | ) 299 | f.write("\n") -------------------------------------------------------------------------------- /anim/motion_matching/mm.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import sys 4 | import numpy as np 5 | from scipy.spatial import KDTree 6 | # import taichi as ti 7 | 8 | from anim.motion_matching.database import Database, MatchingDatabase 9 | 10 | def normalize_features(features: np.ndarray, weights: np.ndarray, axis=0) -> tuple: 11 | """Normalize function for matching features. (features: [T, num_features])""" 12 | means = np.mean(features, axis=axis) # [num_feat] 13 | stds = np.std(features, axis=axis) # [num_feat] 14 | scales = np.where(stds==0, 1e-10, stds / weights) 15 | norms = (features - means[None]) / scales[None] 16 | return norms, means, scales 17 | 18 | def normalize_query(query: np.ndarray, means:np.ndarray, stds:np.ndarray) -> np.ndarray: 19 | """Normalize function for `query`. 20 | `means` and `stds` are calcurated by normalize_features(). 21 | """ 22 | return (query - means) / stds 23 | 24 | def calc_box_distance( 25 | best_cost: float, 26 | query: np.ndarray, # [n_features] 27 | box_mins: np.ndarray, # [n_features] 28 | box_maxs: np.ndarray, # [n_features] 29 | ) -> tuple[float, bool]: 30 | """Calculate distance between `query` to an AABB.""" 31 | cost = 0.0 32 | smaller = True 33 | for i, feat in enumerate(query): 34 | cost += np.square(feat - np.clip(feat, box_mins[i], box_maxs[i])) 35 | if cost >= best_cost: 36 | smaller = False 37 | break 38 | return cost, smaller 39 | 40 | def create_position_features(db: Database) -> np.ndarray: 41 | left_foot_idx = db.skel.names.index("LeftFoot") 42 | right_foot_idx = db.skel.names.index("RightFoot") 43 | features = db.cpos[:, [left_foot_idx, right_foot_idx]].reshape(len(db), -1) # (T, 2 * 3) 44 | return features 45 | 46 | def create_velocity_features(db: Database) -> np.ndarray: 47 | left_foot_idx = db.skel.names.index("LeftFoot") 48 | right_foot_idx = db.skel.names.index("RightFoot") 49 | hips_idx = db.skel.names.index("Hips") 50 | features = db.cposvel[:, [left_foot_idx, right_foot_idx, hips_idx]].reshape(len(db), -1) # (T, 3 * 3) 51 | return features 52 | 53 | def create_traj_features(db: Database) -> np.ndarray: 54 | traj_pos0 = db.future_traj_poss(20, remove_vertical=True) 55 | traj_pos1 = db.future_traj_poss(40, remove_vertical=True) 56 | traj_pos2 = db.future_traj_poss(60, remove_vertical=True) 57 | pos_features = np.concatenate([traj_pos0, traj_pos1, traj_pos2], axis=-1) # [T, 2*3] 58 | 59 | traj_dir0 = db.future_traj_dirs(20, remove_vertical=True) 60 | traj_dir1 = db.future_traj_dirs(40, remove_vertical=True) 61 | traj_dir2 = db.future_traj_dirs(60, remove_vertical=True) 62 | dir_features = np.concatenate([traj_dir0, traj_dir1, traj_dir2], axis=-1) # [T, 2*3] 63 | 64 | features = np.concatenate([pos_features, dir_features], axis=-1) # [T, 2*3 + 2*3] 65 | return features 66 | 67 | def create_matching_features( 68 | db: Database, 69 | w_pos_foot: float, 70 | w_vel_foot: float, 71 | w_vel_hips: float, 72 | w_traj_pos: float, 73 | w_traj_dir: float, 74 | ignore_end: int 75 | ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: 76 | """Create a `MatchingDatabase` to find the best frame with motion matching. 77 | Each feature is calculated with `create_***_features()` above, 78 | and this function integrates them. 79 | """ 80 | pos_feats = create_position_features(db) 81 | vel_feats = create_velocity_features(db) 82 | traj_feats = create_traj_features(db) 83 | features = np.concatenate([pos_feats, vel_feats, traj_feats], axis=-1) 84 | 85 | # remove ignore index from mdb and create a list that ties mdb indices to db indices. 86 | indices = [] 87 | rmv_starts = [] 88 | for i, (start, end) in enumerate(zip(db.starts, db.ends)): 89 | rmv_starts.append(end - ignore_end * (i + 1) + 1) 90 | 91 | for j in range(start, end - ignore_end + 1): 92 | indices.append(j) 93 | for st in rmv_starts: 94 | np.delete(features, slice(st, st+ignore_end), axis=0) 95 | 96 | # normalize 97 | weights = np.array( 98 | [w_pos_foot] * 6 + 99 | [w_vel_foot] * 6 + 100 | [w_vel_hips] * 3 + 101 | [w_traj_pos] * 6 + 102 | [w_traj_dir] * 6 103 | ) 104 | fnorms, means, stds = normalize_features(features, weights) 105 | 106 | return fnorms, means, stds, indices 107 | 108 | def create_matching_aabb(features: np.ndarray, size: int | None) -> tuple[np.ndarray, np.ndarray]: 109 | """Create Axis-Aligned Bounding Boxes (AABB) on frame dim. 110 | This will speed up the Nearest Neighbour search. 111 | based on https://dl.acm.org/doi/abs/10.1145/3386569.3392440. 112 | """ 113 | num_frames = len(features) 114 | bound_mins = [] 115 | bound_maxs = [] 116 | 117 | num_bounds = (num_frames - 1) // size + 1 # advance 118 | frame_bound_ends = [(i + 1) * size for i in range(num_bounds)] 119 | frame_bound_ends[-1] = num_frames 120 | cur_idx = 0 121 | for end in frame_bound_ends: 122 | bound_mins.append(np.min(features[cur_idx:end], axis=0, keepdims=True)) # [1, num_features] 123 | bound_maxs.append(np.max(features[cur_idx:end], axis=0, keepdims=True)) 124 | 125 | np_bound_mins = np.concatenate(bound_mins, axis=0) # [num_bounds, num_features] 126 | np_bound_maxs = np.concatenate(bound_maxs, axis=0) 127 | return np_bound_mins, np_bound_maxs 128 | 129 | def create_matching_database( 130 | db: Database, 131 | method: str, 132 | w_pos_foot: float, 133 | w_vel_foot: float, 134 | w_vel_hips: float, 135 | w_traj_pos: float, 136 | w_traj_dir: float, 137 | ignore_end: int, 138 | dense_bound_size: int = None, 139 | sparse_bound_size: int = None, 140 | ) -> MatchingDatabase: 141 | """Create a simple `MatchingDatabase`.""" 142 | fnorms, means, stds, indices = create_matching_features( 143 | db, w_pos_foot, w_vel_foot, w_vel_hips, w_traj_pos, w_traj_dir, ignore_end 144 | ) 145 | if method == "aabb": 146 | dense_bound_mins, dense_bound_maxs = create_matching_aabb(fnorms, dense_bound_size) 147 | sparse_bound_mins, sparse_bound_maxs = create_matching_aabb(fnorms, sparse_bound_size) 148 | return MatchingDatabase( 149 | fnorms, means, stds, indices, 150 | dense_bound_size, dense_bound_mins, dense_bound_maxs, 151 | sparse_bound_size, sparse_bound_mins, sparse_bound_maxs, 152 | ) 153 | 154 | elif method == "kd-tree": 155 | kdtree = KDTree(fnorms) 156 | return MatchingDatabase(fnorms, means, stds, indices, kdtree=kdtree) 157 | 158 | elif method in ["brute-force", "faiss"]: 159 | return MatchingDatabase(fnorms, means, stds, indices) 160 | 161 | else: 162 | raise ValueError("{} is not in methods.".format(method)) 163 | 164 | # def create_cost_matrix( 165 | # features: np.ndarray, # [num_frames, num_features] 166 | # method: str="taichi", 167 | # ): 168 | # num_frames, num_features = features.shape 169 | # if method == "taichi": 170 | # ti_features = ti.Vector.field(n=num_features, dtype=float, shape=(num_frames)) 171 | # ti_features.from_numpy(features) 172 | # cost_mat = ti.field(float, shape=(num_frames, num_frames)) 173 | # cost_mat.fill(0) 174 | # _create_cost_matrix(ti_features, cost_mat, num_features) 175 | # return cost_mat.to_numpy() 176 | 177 | # @ti.kernel 178 | # def _create_cost_matrix( 179 | # features: ti.template(), 180 | # cost_mat: ti.template(), 181 | # num_features: int 182 | # ): 183 | # for i, j in cost_mat: 184 | # for k in ti.static(range(num_features)): 185 | # cost_mat[i, j] += (features[i][k] - features[j][k]) ** 2 186 | 187 | def motion_matching_search( 188 | cur_idx: int, 189 | method: str, # ["brute-force", "aabb", "kd-tree", "faiss"] 190 | mdb: MatchingDatabase, 191 | query: np.ndarray, # [num_features] 192 | norm_query: bool=True # If we need normalize query. 193 | ): 194 | """Search best index in `MatchingDatabase`. 195 | This function is called when 196 | * motion can be updated (every N frames), 197 | * motion should be updated (when the animation ends, `cur_idx`==-1), 198 | etc. 199 | """ 200 | if norm_query: 201 | qnorm = normalize_query(query, mdb.means, mdb.stds) # [num_features] 202 | else: 203 | qnorm = query 204 | 205 | if not cur_idx == -1: # can be updated 206 | cur_idx = mdb.indices.index(cur_idx) # db index -> mdb index 207 | # mdb.features[cur_idx] will be replaced to controlled character's features. 208 | best_cost: float = np.sum(np.square(mdb.features[cur_idx] - qnorm)) 209 | else: best_cost: float = sys.float_info.max # should be updated 210 | 211 | if method == "brute-force": 212 | return brute_force_search(mdb, qnorm, cur_idx, best_cost) 213 | elif method == "aabb": 214 | return aabb_search(mdb, qnorm, cur_idx, best_cost) 215 | elif method == "kd-tree": 216 | return kd_search(mdb, qnorm, cur_idx, best_cost) 217 | elif method == "faiss": 218 | return faiss_search(mdb, qnorm, cur_idx, best_cost) 219 | 220 | def brute_force_search( 221 | mdb: MatchingDatabase, 222 | qnorm: np.ndarray, # [num_features] 223 | cur_idx: int, 224 | best_cost: float, 225 | ) -> int: 226 | costs = np.sum(np.square(mdb.features - qnorm[None].repeat(len(mdb), axis=0)), axis=-1) 227 | cur_cost = np.min(costs) 228 | if cur_cost < best_cost: 229 | return mdb.indices[np.argmin(costs)] # mdb index -> db index 230 | else: 231 | return mdb.indices[cur_idx] 232 | 233 | def aabb_search( 234 | mdb: MatchingDatabase, 235 | qnorm: np.ndarray, # [num_features] 236 | cur_idx: int, 237 | best_cost: float, 238 | ) -> int: 239 | i = 0 240 | best_idx = cur_idx 241 | # for all sparse boxes 242 | while i < len(mdb): 243 | sparse_box_idx = i // mdb.sparse_bound_size # box index 244 | i_sparse_next = (sparse_box_idx + 1) * mdb.sparse_bound_size # frame index on next sparce box 245 | # Calculate distance between query to sparse bounding box. 246 | cur_cost, smaller = calc_box_distance( 247 | best_cost, qnorm, 248 | mdb.dense_bound_mins[sparse_box_idx], 249 | mdb.sparse_bound_maxs[sparse_box_idx], 250 | ) 251 | 252 | if not smaller: 253 | i = i_sparse_next 254 | continue 255 | 256 | # for all dense boxes 257 | while i < len(mdb) and i < i_sparse_next: 258 | dense_box_idx = i // mdb.dense_bound_size # box index 259 | i_dense_next = (dense_box_idx + 1) * mdb.dense_bound_size # frame index 260 | 261 | # Calculate distance to a dense bounding box. 262 | cur_cost, smaller = calc_box_distance( 263 | best_cost, qnorm, 264 | mdb.sparse_bound_mins[sparse_box_idx], 265 | mdb.sparse_bound_maxs[sparse_box_idx] 266 | ) 267 | 268 | if not smaller: 269 | i = i_dense_next 270 | continue 271 | 272 | # Calculate distance to a feature inside box. 273 | while i < len(mdb) and i < i_dense_next: 274 | cur_cost = np.sum(np.square(qnorm - mdb.features[i])) 275 | if cur_cost < best_cost: 276 | best_idx = i 277 | best_cost = cur_cost 278 | 279 | i += 1 280 | return mdb.indices[best_idx] # mdb index -> db index 281 | 282 | def kd_search( 283 | mdb: MatchingDatabase, 284 | qnorm: np.ndarray, 285 | cur_idx: int, 286 | best_cost: float, 287 | ) -> int: 288 | dist, index = mdb.kdtree.query(qnorm, k=1) 289 | if dist < best_cost: 290 | return mdb.indices[index] # mdb index -> db index 291 | else: 292 | return mdb.indices[cur_idx] 293 | 294 | def faiss_search( 295 | mdb: MatchingDatabase, 296 | qnorm: np.ndarray, 297 | cur_idx: int, 298 | best_cost: float, 299 | ) -> int: 300 | return -------------------------------------------------------------------------------- /util/quat.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | 5 | # Calculate cross object of two 3D vectors. 6 | def _fast_cross(a, b): 7 | return np.concatenate([ 8 | a[...,1:2]*b[...,2:3] - a[...,2:3]*b[...,1:2], 9 | a[...,2:3]*b[...,0:1] - a[...,0:1]*b[...,2:3], 10 | a[...,0:1]*b[...,1:2] - a[...,1:2]*b[...,0:1]], axis=-1) 11 | 12 | # Make origin quaternions (No rotations). 13 | def eye(shape, dtype=np.float32): 14 | return np.ones(list(shape) + [4], dtype=dtype) * np.asarray([1, 0, 0, 0], dtype=dtype) 15 | 16 | # Return norm of quaternions. 17 | def length(x): 18 | return np.sqrt(np.sum(x * x, axis=-1)) 19 | 20 | # Make unit quaternions. 21 | def normalize(x, eps=1e-8): 22 | return x / (length(x)[...,None] + eps) 23 | 24 | def abs(x): 25 | return np.where(x[...,0:1] > 0.0, x, -x) 26 | 27 | # Calculate inverse rotations. 28 | def inv(q): 29 | return np.array([1, -1, -1, -1], dtype=np.float32) * q 30 | 31 | # Calculate the dot product of two quaternions. 32 | def dot(x, y): 33 | return np.sum(x * y, axis=-1)[...,None] if x.ndim > 1 else np.sum(x * y, axis=-1) 34 | 35 | # Multiply two quaternions (return rotations). 36 | def mul(x, y): 37 | x0, x1, x2, x3 = x[..., 0:1], x[..., 1:2], x[..., 2:3], x[..., 3:4] 38 | y0, y1, y2, y3 = y[..., 0:1], y[..., 1:2], y[..., 2:3], y[..., 3:4] 39 | 40 | return np.concatenate([ 41 | y0 * x0 - y1 * x1 - y2 * x2 - y3 * x3, 42 | y0 * x1 + y1 * x0 - y2 * x3 + y3 * x2, 43 | y0 * x2 + y1 * x3 + y2 * x0 - y3 * x1, 44 | y0 * x3 - y1 * x2 + y2 * x1 + y3 * x0], axis=-1) 45 | 46 | def inv_mul(x, y): 47 | return mul(inv(x), y) 48 | 49 | def mul_inv(x, y): 50 | return mul(x, inv(y)) 51 | 52 | # Multiply quaternions and vectors (return vectors). 53 | def mul_vec(q, x): 54 | t = 2.0 * _fast_cross(q[..., 1:], x) 55 | return x + q[..., 0][..., None] * t + _fast_cross(q[..., 1:], t) 56 | 57 | def inv_mul_vec(q, x): 58 | return mul_vec(inv(q), x) 59 | 60 | def unroll(x): 61 | y = x.copy() 62 | for i in range(1, len(x)): 63 | d0 = np.sum( y[i] * y[i-1], axis=-1) 64 | d1 = np.sum(-y[i] * y[i-1], axis=-1) 65 | y[i][d0 < d1] = -y[i][d0 < d1] 66 | return y 67 | 68 | # Calculate quaternions between two unit 3D vectors (x to y). 69 | def between(x, y): 70 | return np.concatenate([ 71 | np.sqrt(np.sum(x*x, axis=-1) * np.sum(y*y, axis=-1))[...,None] + 72 | np.sum(x * y, axis=-1)[...,None], 73 | _fast_cross(x, y)], axis=-1) 74 | 75 | def log(x, eps=1e-5): 76 | length = np.sqrt(np.sum(np.square(x[...,1:]), axis=-1))[...,None] 77 | halfangle = np.where(length < eps, np.ones_like(length), np.arctan2(length, x[...,0:1]) / length) 78 | return halfangle * x[...,1:] 79 | 80 | def exp(x, eps=1e-5): 81 | halfangle = np.sqrt(np.sum(np.square(x), axis=-1))[...,None] 82 | c = np.where(halfangle < eps, np.ones_like(halfangle), np.cos(halfangle)) 83 | s = np.where(halfangle < eps, np.ones_like(halfangle), np.sinc(halfangle / np.pi)) 84 | return np.concatenate([c, s * x], axis=-1) 85 | 86 | # Calculate global space rotations and positions from local space. 87 | def fk(lrot, lpos, parents): 88 | 89 | gp, gr = [lpos[...,:1,:]], [lrot[...,:1,:]] 90 | for i in range(1, len(parents)): 91 | gp.append(mul_vec(gr[parents[i]], lpos[...,i:i+1,:]) + gp[parents[i]]) 92 | gr.append(mul (gr[parents[i]], lrot[...,i:i+1,:])) 93 | 94 | return np.concatenate(gr, axis=-2), np.concatenate(gp, axis=-2) 95 | 96 | def fk_rot(lrot, parents): 97 | 98 | gr = [lrot[...,:1,:]] 99 | for i in range(1, len(parents)): 100 | gr.append(mul(gr[parents[i]], lrot[...,i:i+1,:])) 101 | 102 | return np.concatenate(gr, axis=-2) 103 | 104 | def fk_vel(lrot, lpos, lvel, lang, parents): 105 | 106 | gp, gr, gv, ga = [lpos[...,:1,:]], [lrot[...,:1,:]], [lvel[...,:1,:]], [lang[...,:1,:]] 107 | for i in range(1, len(parents)): 108 | gp.append(mul_vec(gr[parents[i]], lpos[...,i:i+1,:]) + gp[parents[i]]) 109 | gr.append(mul (gr[parents[i]], lrot[...,i:i+1,:])) 110 | gv.append(mul_vec(gr[parents[i]], lvel[...,i:i+1,:]) + 111 | _fast_cross(ga[parents[i]], mul_vec(gr[parents[i]], lpos[...,i:i+1,:])) + 112 | gv[parents[i]]) 113 | ga.append(mul_vec(gr[parents[i]], lang[...,i:i+1,:]) + ga[parents[i]]) 114 | 115 | return ( 116 | np.concatenate(gr, axis=-2), 117 | np.concatenate(gp, axis=-2), 118 | np.concatenate(gv, axis=-2), 119 | np.concatenate(ga, axis=-2)) 120 | 121 | # Calculate local space rotations and positions from global space. 122 | def ik(grot, gpos, parents): 123 | 124 | return ( 125 | np.concatenate([ 126 | grot[...,:1,:], 127 | mul(inv(grot[...,parents[1:],:]), grot[...,1:,:]), 128 | ], axis=-2), 129 | np.concatenate([ 130 | gpos[...,:1,:], 131 | mul_vec( 132 | inv(grot[...,parents[1:],:]), 133 | gpos[...,1:,:] - gpos[...,parents[1:],:]), 134 | ], axis=-2)) 135 | 136 | def ik_rot(grot, parents): 137 | 138 | return np.concatenate([grot[...,:1,:], mul(inv(grot[...,parents[1:],:]), grot[...,1:,:]), 139 | ], axis=-2) 140 | 141 | # ================================================================ 142 | # Conversion from quaternions to other rotation representations. 143 | # ================================================================ 144 | # Calculate axis-angle from quaternions. 145 | # This function is based on ACTOR 146 | # https://github.com/Mathux/ACTOR/blob/d3b0afe674e01fa2b65c89784816c3435df0a9a5/src/utils/rotation_conversions.py#L481 147 | def to_axis_angle(x, eps=1e-5): 148 | norm = np.linalg.norm(x[...,1:],axis=-1,keepdims=True) 149 | half_angle = np.arctan2(norm, x[...,:1]) 150 | angle = 2 * half_angle 151 | small_angle = np.abs(angle) < eps 152 | sin_half_angle_over_angle = np.empty_like(angle) 153 | sin_half_angle_over_angle[~small_angle] = ( 154 | np.sin(half_angle[~small_angle]) / angle[~small_angle] 155 | ) 156 | # for x small, sin(x/2) is about x/2 - (x/2)^3/6 157 | # so sin(x/2)/x is about 1/2 - (x*x)/48 158 | sin_half_angle_over_angle[small_angle] = ( 159 | 0.5 - (angle[small_angle] * angle[small_angle]) / 48 160 | ) 161 | return x[..., 1:] / sin_half_angle_over_angle 162 | 163 | # Calculate euler angles from quaternions.(!Under construction.) 164 | def to_euler(x, order='zyx'): 165 | x0, x1, x2, x3 = x[..., 0:1], x[..., 1:2], x[..., 2:3], x[..., 3:4] 166 | 167 | if order == 'zyx': 168 | return np.concatenate([ 169 | np.arctan2(2.0 * (x0 * x3 + x1 * x2), 1.0 - 2.0 * (x2 * x2 + x3 * x3)), 170 | np.arcsin(np.clip(2.0 * (x0 * x2 - x3 * x1), -1.0, 1.0)), 171 | np.arctan2(2.0 * (x0 * x1 + x2 * x3), 1.0 - 2.0 * (x1 * x1 + x2 * x2)), 172 | ], axis=-1) 173 | elif order == 'xzy': 174 | return np.concatenate([ 175 | np.arctan2(2.0 * (x1 * x0 - x2 * x3), -x1 * x1 + x2 * x2 - x3 * x3 + x0 * x0), 176 | np.arctan2(2.0 * (x2 * x0 - x1 * x3), x1 * x1 - x2 * x2 - x3 * x3 + x0 * x0), 177 | np.arcsin(np.clip(2.0 * (x1 * x2 + x3 * x0), -1.0, 1.0)) 178 | ], axis=-1) 179 | else: 180 | raise NotImplementedError('Cannot convert to ordering %s' % order) 181 | 182 | # Calculate rotation matrix from quaternions. 183 | def to_xform(x): 184 | 185 | qw, qx, qy, qz = x[...,0:1], x[...,1:2], x[...,2:3], x[...,3:4] 186 | 187 | x2, y2, z2 = qx + qx, qy + qy, qz + qz 188 | xx, yy, wx = qx * x2, qy * y2, qw * x2 189 | xy, yz, wy = qx * y2, qy * z2, qw * y2 190 | xz, zz, wz = qx * z2, qz * z2, qw * z2 191 | 192 | return np.concatenate([ 193 | np.concatenate([1.0 - (yy + zz), xy - wz, xz + wy], axis=-1)[...,None,:], 194 | np.concatenate([xy + wz, 1.0 - (xx + zz), yz - wx], axis=-1)[...,None,:], 195 | np.concatenate([xz - wy, yz + wx, 1.0 - (xx + yy)], axis=-1)[...,None,:], 196 | ], axis=-2) 197 | 198 | # Calculate 6d orthogonal rotation representation (ortho6d) from quaternions. 199 | # https://github.com/papagina/RotationContinuity 200 | def to_xform_xy(x): 201 | 202 | qw, qx, qy, qz = x[...,0:1], x[...,1:2], x[...,2:3], x[...,3:4] 203 | 204 | x2, y2, z2 = qx + qx, qy + qy, qz + qz 205 | xx, yy, wx = qx * x2, qy * y2, qw * x2 206 | xy, yz, wy = qx * y2, qy * z2, qw * y2 207 | xz, zz, wz = qx * z2, qz * z2, qw * z2 208 | 209 | return np.concatenate([ 210 | np.concatenate([1.0 - (yy + zz), xy - wz], axis=-1)[...,None,:], 211 | np.concatenate([xy + wz, 1.0 - (xx + zz)], axis=-1)[...,None,:], 212 | np.concatenate([xz - wy, yz + wx], axis=-1)[...,None,:], 213 | ], axis=-2) 214 | 215 | # Calculate scaled angle axis from quaternions. 216 | def to_scaled_angle_axis(x, eps=1e-5): 217 | return 2.0 * log(x, eps) 218 | 219 | 220 | # ================================================================ 221 | # Conversion from other rotation representations to quaternions. 222 | # ================================================================ 223 | 224 | # Calculate quaternions from axis and angle. 225 | def from_angle_axis(angle, axis): 226 | c = np.cos(angle / 2.0)[..., None] 227 | s = np.sin(angle / 2.0)[..., None] 228 | q = np.concatenate([c, s * axis], axis=-1) 229 | return q 230 | 231 | # Calculate quaternions from axis-angle. 232 | def from_axis_angle(rots, eps=1e-5): 233 | angle = np.linalg.norm(rots, axis=-1) 234 | angle = np.where(angle==0, eps, angle) 235 | axis = rots / angle[...,None] 236 | return from_angle_axis(angle, axis) 237 | 238 | # Calculate quaternions from euler angles. 239 | def from_euler(e, order='zyx', mode="degree"): 240 | if mode=="degree": 241 | e = np.deg2rad(e) 242 | axis = { 243 | 'x': np.asarray([1, 0, 0], dtype=np.float32), 244 | 'y': np.asarray([0, 1, 0], dtype=np.float32), 245 | 'z': np.asarray([0, 0, 1], dtype=np.float32)} 246 | 247 | q0 = from_angle_axis(e[..., 0], axis[order[0]]) 248 | q1 = from_angle_axis(e[..., 1], axis[order[1]]) 249 | q2 = from_angle_axis(e[..., 2], axis[order[2]]) 250 | 251 | return mul(q0, mul(q1, q2)) 252 | 253 | # Calculate quaternions from rotation matrix. 254 | def from_xform(ts): 255 | 256 | return normalize( 257 | np.where((ts[...,2,2] < 0.0)[...,None], 258 | np.where((ts[...,0,0] > ts[...,1,1])[...,None], 259 | np.concatenate([ 260 | (ts[...,2,1]-ts[...,1,2])[...,None], 261 | (1.0 + ts[...,0,0] - ts[...,1,1] - ts[...,2,2])[...,None], 262 | (ts[...,1,0]+ts[...,0,1])[...,None], 263 | (ts[...,0,2]+ts[...,2,0])[...,None]], axis=-1), 264 | np.concatenate([ 265 | (ts[...,0,2]-ts[...,2,0])[...,None], 266 | (ts[...,1,0]+ts[...,0,1])[...,None], 267 | (1.0 - ts[...,0,0] + ts[...,1,1] - ts[...,2,2])[...,None], 268 | (ts[...,2,1]+ts[...,1,2])[...,None]], axis=-1)), 269 | np.where((ts[...,0,0] < -ts[...,1,1])[...,None], 270 | np.concatenate([ 271 | (ts[...,1,0]-ts[...,0,1])[...,None], 272 | (ts[...,0,2]+ts[...,2,0])[...,None], 273 | (ts[...,2,1]+ts[...,1,2])[...,None], 274 | (1.0 - ts[...,0,0] - ts[...,1,1] + ts[...,2,2])[...,None]], axis=-1), 275 | np.concatenate([ 276 | (1.0 + ts[...,0,0] + ts[...,1,1] + ts[...,2,2])[...,None], 277 | (ts[...,2,1]-ts[...,1,2])[...,None], 278 | (ts[...,0,2]-ts[...,2,0])[...,None], 279 | (ts[...,1,0]-ts[...,0,1])[...,None]], axis=-1)))) 280 | 281 | # Calculate quaternions from ortho6d. 282 | def from_xform_xy(x): 283 | 284 | c2 = _fast_cross(x[...,0], x[...,1]) 285 | c2 = c2 / np.sqrt(np.sum(np.square(c2), axis=-1))[...,None] 286 | c1 = _fast_cross(c2, x[...,0]) 287 | c1 = c1 / np.sqrt(np.sum(np.square(c1), axis=-1))[...,None] 288 | c0 = x[...,0] 289 | 290 | return from_xform(np.concatenate([ 291 | c0[...,None], 292 | c1[...,None], 293 | c2[...,None]], axis=-1)) 294 | 295 | # Calculate quaternions from scaled angle axis. 296 | def from_scaled_angle_axis(x, eps=1e-5): 297 | return exp(x / 2.0, eps) -------------------------------------------------------------------------------- /anitaichi/transform/quat.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | 5 | from anitaichi.transform import vec 6 | 7 | # Make origin quaternions (No rotations). 8 | def eye(shape, dtype=np.float32): 9 | return np.ones(list(shape) + [4], dtype=dtype) * np.asarray([1, 0, 0, 0], dtype=dtype) 10 | 11 | # Return norm of quaternions. 12 | def length(x): 13 | return np.sqrt(np.sum(x * x, axis=-1)) 14 | 15 | # Make unit quaternions. 16 | def normalize(x, eps=1e-8): 17 | return x / (length(x)[...,None] + eps) 18 | 19 | def abs(x): 20 | return np.where(x[...,0:1] > 0.0, x, -x) 21 | 22 | # Calculate inverse rotations. 23 | def inv(q): 24 | return np.array([1, -1, -1, -1], dtype=np.float32) * q 25 | 26 | # Calculate the dot product of two quaternions. 27 | def dot(x, y): 28 | return np.sum(x * y, axis=-1)[...,None] if x.ndim > 1 else np.sum(x * y, axis=-1) 29 | 30 | # Multiply two quaternions (return rotations). 31 | def mul(x, y): 32 | x0, x1, x2, x3 = x[..., 0:1], x[..., 1:2], x[..., 2:3], x[..., 3:4] 33 | y0, y1, y2, y3 = y[..., 0:1], y[..., 1:2], y[..., 2:3], y[..., 3:4] 34 | 35 | return np.concatenate([ 36 | y0 * x0 - y1 * x1 - y2 * x2 - y3 * x3, 37 | y0 * x1 + y1 * x0 - y2 * x3 + y3 * x2, 38 | y0 * x2 + y1 * x3 + y2 * x0 - y3 * x1, 39 | y0 * x3 - y1 * x2 + y2 * x1 + y3 * x0], axis=-1) 40 | 41 | def inv_mul(x, y): 42 | return mul(inv(x), y) 43 | 44 | def mul_inv(x, y): 45 | return mul(x, inv(y)) 46 | 47 | # Multiply quaternions and vectors (return vectors). 48 | def mul_vec(q, x): 49 | t = 2.0 * vec.cross(q[..., 1:], x) 50 | return x + q[..., 0][..., None] * t + vec.cross(q[..., 1:], t) 51 | 52 | def inv_mul_vec(q, x): 53 | return mul_vec(inv(q), x) 54 | 55 | def unroll(x): 56 | y = x.copy() 57 | for i in range(1, len(x)): 58 | d0 = np.sum( y[i] * y[i-1], axis=-1) 59 | d1 = np.sum(-y[i] * y[i-1], axis=-1) 60 | y[i][d0 < d1] = -y[i][d0 < d1] 61 | return y 62 | 63 | # Calculate quaternions between two unit 3D vectors (x to y). 64 | def between(x, y): 65 | return np.concatenate([ 66 | np.sqrt(np.sum(x*x, axis=-1) * np.sum(y*y, axis=-1))[...,None] + 67 | np.sum(x * y, axis=-1)[...,None], 68 | vec.cross(x, y)], axis=-1) 69 | 70 | def log(x, eps=1e-5): 71 | length = np.sqrt(np.sum(np.square(x[...,1:]), axis=-1))[...,None] 72 | halfangle = np.where(length < eps, np.ones_like(length), np.arctan2(length, x[...,0:1]) / length) 73 | return halfangle * x[...,1:] 74 | 75 | def exp(x, eps=1e-5): 76 | halfangle = np.sqrt(np.sum(np.square(x), axis=-1))[...,None] 77 | c = np.where(halfangle < eps, np.ones_like(halfangle), np.cos(halfangle)) 78 | s = np.where(halfangle < eps, np.ones_like(halfangle), np.sinc(halfangle / np.pi)) 79 | return np.concatenate([c, s * x], axis=-1) 80 | 81 | # Calculate global space rotations and positions from local space. 82 | def fk(lrot, lpos, parents): 83 | 84 | gp, gr = [lpos[...,:1,:]], [lrot[...,:1,:]] 85 | for i in range(1, len(parents)): 86 | gp.append(mul_vec(gr[parents[i]], lpos[...,i:i+1,:]) + gp[parents[i]]) 87 | gr.append(mul (gr[parents[i]], lrot[...,i:i+1,:])) 88 | 89 | return np.concatenate(gr, axis=-2), np.concatenate(gp, axis=-2) 90 | 91 | def fk_rot(lrot, parents): 92 | 93 | gr = [lrot[...,:1,:]] 94 | for i in range(1, len(parents)): 95 | gr.append(mul(gr[parents[i]], lrot[...,i:i+1,:])) 96 | 97 | return np.concatenate(gr, axis=-2) 98 | 99 | # Calculate root centric space rotations and positions. 100 | def fk_centric(lrot, lpos, parents): 101 | rrot, rpos = lrot.copy(), lpos.copy() 102 | rrot[:,0,:] = eye([rrot.shape[0]]) 103 | rpos[:,0,:] = 0 104 | return fk(rrot, rpos, parents) 105 | 106 | # Calculate local space rotations and positions from global space. 107 | def ik(grot, gpos, parents): 108 | 109 | return ( 110 | np.concatenate([ 111 | grot[...,:1,:], 112 | mul(inv(grot[...,parents[1:],:]), grot[...,1:,:]), 113 | ], axis=-2), 114 | np.concatenate([ 115 | gpos[...,:1,:], 116 | mul_vec( 117 | inv(grot[...,parents[1:],:]), 118 | gpos[...,1:,:] - gpos[...,parents[1:],:]), 119 | ], axis=-2)) 120 | 121 | def ik_rot(grot, parents): 122 | 123 | return np.concatenate([grot[...,:1,:], mul(inv(grot[...,parents[1:],:]), grot[...,1:,:]), 124 | ], axis=-2) 125 | 126 | def fk_vel(lrot, lpos, lvel, lang, parents): 127 | 128 | gp, gr, gv, ga = [lpos[...,:1,:]], [lrot[...,:1,:]], [lvel[...,:1,:]], [lang[...,:1,:]] 129 | for i in range(1, len(parents)): 130 | gp.append(mul_vec(gr[parents[i]], lpos[...,i:i+1,:]) + gp[parents[i]]) 131 | gr.append(mul (gr[parents[i]], lrot[...,i:i+1,:])) 132 | gv.append(mul_vec(gr[parents[i]], lvel[...,i:i+1,:]) + 133 | vec.cross(ga[parents[i]], mul_vec(gr[parents[i]], lpos[...,i:i+1,:])) + 134 | gv[parents[i]]) 135 | ga.append(mul_vec(gr[parents[i]], lang[...,i:i+1,:]) + ga[parents[i]]) 136 | 137 | return ( 138 | np.concatenate(gr, axis=-2), 139 | np.concatenate(gp, axis=-2), 140 | np.concatenate(gv, axis=-2), 141 | np.concatenate(ga, axis=-2)) 142 | 143 | # Linear Interpolation of two vectors 144 | def lerp(x, y, t): 145 | return (1 - t) * x + t * y 146 | 147 | # LERP of quaternions 148 | def quat_lerp(x, y, t): 149 | return normalize(lerp(x, y, t)) 150 | 151 | # Spherical linear interpolation of quaternions 152 | def slerp(x, y, t): 153 | if t == 0: 154 | return x 155 | elif t == 1: 156 | return y 157 | 158 | if dot(x, y) < 0: 159 | y = - y 160 | ca = dot(x, y) 161 | theta = np.arccos(np.clip(ca, 0, 1)) 162 | 163 | r = normalize(y - x * ca) 164 | 165 | return x * np.cos(theta * t) + r * np.sin(theta * t) 166 | 167 | 168 | ################################################################ 169 | # Conversion from quaternions to other rotation representations. 170 | ################################################################ 171 | 172 | # Calculate axis-angle from quaternions. 173 | # This function is based on ACTOR 174 | # https://github.com/Mathux/ACTOR/blob/d3b0afe674e01fa2b65c89784816c3435df0a9a5/src/utils/rotation_conversions.py#L481 175 | def to_axis_angle(x, eps=1e-5): 176 | norm = np.linalg.norm(x[...,1:],axis=-1,keepdims=True) 177 | half_angle = np.arctan2(norm, x[...,:1]) 178 | angle = 2 * half_angle 179 | small_angle = np.abs(angle) < eps 180 | sin_half_angle_over_angle = np.empty_like(angle) 181 | sin_half_angle_over_angle[~small_angle] = ( 182 | np.sin(half_angle[~small_angle]) / angle[~small_angle] 183 | ) 184 | # for x small, sin(x/2) is about x/2 - (x/2)^3/6 185 | # so sin(x/2)/x is about 1/2 - (x*x)/48 186 | sin_half_angle_over_angle[small_angle] = ( 187 | 0.5 - (angle[small_angle] * angle[small_angle]) / 48 188 | ) 189 | return x[..., 1:] / sin_half_angle_over_angle 190 | 191 | # Calculate euler angles from quaternions.(!Under construction.) 192 | def to_euler(x, order='zyx'): 193 | x0, x1, x2, x3 = x[..., 0:1], x[..., 1:2], x[..., 2:3], x[..., 3:4] 194 | 195 | if order == 'zyx': 196 | return np.concatenate([ 197 | np.arctan2(2.0 * (x0 * x3 + x1 * x2), 1.0 - 2.0 * (x2 * x2 + x3 * x3)), 198 | np.arcsin(np.clip(2.0 * (x0 * x2 - x3 * x1), -1.0, 1.0)), 199 | np.arctan2(2.0 * (x0 * x1 + x2 * x3), 1.0 - 2.0 * (x1 * x1 + x2 * x2)), 200 | ], axis=-1) 201 | elif order == 'xzy': 202 | return np.concatenate([ 203 | np.arctan2(2.0 * (x1 * x0 - x2 * x3), -x1 * x1 + x2 * x2 - x3 * x3 + x0 * x0), 204 | np.arctan2(2.0 * (x2 * x0 - x1 * x3), x1 * x1 - x2 * x2 - x3 * x3 + x0 * x0), 205 | np.arcsin(np.clip(2.0 * (x1 * x2 + x3 * x0), -1.0, 1.0)) 206 | ], axis=-1) 207 | else: 208 | raise NotImplementedError('Cannot convert to ordering %s' % order) 209 | 210 | # Calculate rotation matrix from quaternions. 211 | def to_xform(x): 212 | 213 | qw, qx, qy, qz = x[...,0:1], x[...,1:2], x[...,2:3], x[...,3:4] 214 | 215 | x2, y2, z2 = qx + qx, qy + qy, qz + qz 216 | xx, yy, wx = qx * x2, qy * y2, qw * x2 217 | xy, yz, wy = qx * y2, qy * z2, qw * y2 218 | xz, zz, wz = qx * z2, qz * z2, qw * z2 219 | 220 | return np.concatenate([ 221 | np.concatenate([1.0 - (yy + zz), xy - wz, xz + wy], axis=-1)[...,None,:], 222 | np.concatenate([xy + wz, 1.0 - (xx + zz), yz - wx], axis=-1)[...,None,:], 223 | np.concatenate([xz - wy, yz + wx, 1.0 - (xx + yy)], axis=-1)[...,None,:], 224 | ], axis=-2) 225 | 226 | # Calculate 6d orthogonal rotation representation (ortho6d) from quaternions. 227 | # https://github.com/papagina/RotationContinuity 228 | def to_xform_xy(x): 229 | 230 | qw, qx, qy, qz = x[...,0:1], x[...,1:2], x[...,2:3], x[...,3:4] 231 | 232 | x2, y2, z2 = qx + qx, qy + qy, qz + qz 233 | xx, yy, wx = qx * x2, qy * y2, qw * x2 234 | xy, yz, wy = qx * y2, qy * z2, qw * y2 235 | xz, zz, wz = qx * z2, qz * z2, qw * z2 236 | 237 | return np.concatenate([ 238 | np.concatenate([1.0 - (yy + zz), xy - wz], axis=-1)[...,None,:], 239 | np.concatenate([xy + wz, 1.0 - (xx + zz)], axis=-1)[...,None,:], 240 | np.concatenate([xz - wy, yz + wx], axis=-1)[...,None,:], 241 | ], axis=-2) 242 | 243 | # Calculate scaled angle axis from quaternions. 244 | def to_scaled_angle_axis(x, eps=1e-5): 245 | return 2.0 * log(x, eps) 246 | 247 | 248 | ################################################################ 249 | # Conversion from other rotation representations to quaternions. 250 | ################################################################ 251 | 252 | # Calculate quaternions from axis and angle. 253 | def from_angle_axis(angle, axis): 254 | c = np.cos(angle / 2.0)[..., None] 255 | s = np.sin(angle / 2.0)[..., None] 256 | q = np.concatenate([c, s * axis], axis=-1) 257 | return q 258 | 259 | # Calculate quaternions from axis-angle. 260 | def from_axis_angle(rots): 261 | angle = np.linalg.norm(rots, axis=-1) 262 | axis = rots / angle[...,None] 263 | return from_angle_axis(angle, axis) 264 | 265 | # Calculate quaternions from euler angles. 266 | def from_euler(e, order='zyx', mode="degree"): 267 | if mode=="degree": 268 | e = np.deg2rad(e) 269 | axis = { 270 | 'x': np.asarray([1, 0, 0], dtype=np.float32), 271 | 'y': np.asarray([0, 1, 0], dtype=np.float32), 272 | 'z': np.asarray([0, 0, 1], dtype=np.float32)} 273 | 274 | q0 = from_angle_axis(e[..., 0], axis[order[0]]) 275 | q1 = from_angle_axis(e[..., 1], axis[order[1]]) 276 | q2 = from_angle_axis(e[..., 2], axis[order[2]]) 277 | 278 | return mul(q0, mul(q1, q2)) 279 | 280 | # Calculate quaternions from rotation matrix. 281 | def from_xform(ts): 282 | 283 | return normalize( 284 | np.where((ts[...,2,2] < 0.0)[...,None], 285 | np.where((ts[...,0,0] > ts[...,1,1])[...,None], 286 | np.concatenate([ 287 | (ts[...,2,1]-ts[...,1,2])[...,None], 288 | (1.0 + ts[...,0,0] - ts[...,1,1] - ts[...,2,2])[...,None], 289 | (ts[...,1,0]+ts[...,0,1])[...,None], 290 | (ts[...,0,2]+ts[...,2,0])[...,None]], axis=-1), 291 | np.concatenate([ 292 | (ts[...,0,2]-ts[...,2,0])[...,None], 293 | (ts[...,1,0]+ts[...,0,1])[...,None], 294 | (1.0 - ts[...,0,0] + ts[...,1,1] - ts[...,2,2])[...,None], 295 | (ts[...,2,1]+ts[...,1,2])[...,None]], axis=-1)), 296 | np.where((ts[...,0,0] < -ts[...,1,1])[...,None], 297 | np.concatenate([ 298 | (ts[...,1,0]-ts[...,0,1])[...,None], 299 | (ts[...,0,2]+ts[...,2,0])[...,None], 300 | (ts[...,2,1]+ts[...,1,2])[...,None], 301 | (1.0 - ts[...,0,0] - ts[...,1,1] + ts[...,2,2])[...,None]], axis=-1), 302 | np.concatenate([ 303 | (1.0 + ts[...,0,0] + ts[...,1,1] + ts[...,2,2])[...,None], 304 | (ts[...,2,1]-ts[...,1,2])[...,None], 305 | (ts[...,0,2]-ts[...,2,0])[...,None], 306 | (ts[...,1,0]-ts[...,0,1])[...,None]], axis=-1)))) 307 | 308 | # Calculate quaternions from ortho6d. 309 | def from_xform_xy(x): 310 | 311 | c2 = vec.cross(x[...,0], x[...,1]) 312 | c2 = c2 / np.sqrt(np.sum(np.square(c2), axis=-1))[...,None] 313 | c1 = vec.cross(c2, x[...,0]) 314 | c1 = c1 / np.sqrt(np.sum(np.square(c1), axis=-1))[...,None] 315 | c0 = x[...,0] 316 | 317 | return from_xform(np.concatenate([ 318 | c0[...,None], 319 | c1[...,None], 320 | c2[...,None]], axis=-1)) 321 | 322 | # Calculate quaternions from scaled angle axis. 323 | def from_scaled_angle_axis(x, eps=1e-5): 324 | return exp(x / 2.0, eps) -------------------------------------------------------------------------------- /anim/animation.py: -------------------------------------------------------------------------------- 1 | """ 2 | animation.py 3 | """ 4 | # Basic animation class. Can use for 5 | # deep-learning (pre/post)processing 6 | # and visualization etc. 7 | 8 | from __future__ import annotations 9 | 10 | import numpy as np 11 | import scipy.ndimage as ndimage 12 | from util import quat 13 | from util import dualquat as dq 14 | from anim.skel import Skel 15 | 16 | 17 | class Animation: 18 | def __init__( 19 | self, 20 | skel: Skel, 21 | quats: np.ndarray, 22 | trans: np.ndarray, 23 | positions: np.ndarray = None, 24 | fps: int=30, 25 | anim_name: str="animation", 26 | ) -> None: 27 | """ Class for Motions representations. 28 | 29 | Args: 30 | skel (Skel): Skelton definition. 31 | quats (np.ndarray): Joints rotations. shape: (T, J, 4) 32 | trans (np.ndarray): Root transitions. shape: (T, 3) 33 | positions (np.ndarray): Joints positions. shape: (T, J, 3). Defaults to None. 34 | fps (int): Frame per seconds for animation. Defaults to 30. 35 | anim_name (str, optional): Name of animation. Defaults to "animation". 36 | """ 37 | self.skel = skel 38 | self.quats = quats 39 | self.trans = trans 40 | self.positions = positions 41 | self.fps = fps 42 | self.name = anim_name 43 | 44 | def __len__(self) -> int: return len(self.trans) 45 | 46 | def __add__(self, other: Animation) -> Animation: 47 | quats: np.ndarray = np.concatenate([self.quats, other.quats], axis=0) 48 | trans: np.ndarray = np.concatenate([self.trans, other.trans], axis=0) 49 | return Animation( 50 | skel=self.skel, 51 | quats=quats, 52 | trans=trans, 53 | fps=self.fps, 54 | anim_name=self.name, 55 | ) 56 | 57 | def __getitem__(self, index: int | slice) -> Animation: 58 | if isinstance(index, int): 59 | if index == -1: 60 | index = len(self) - 1 61 | return Animation( 62 | skel=self.skel, 63 | quats=self.quats[index:index+1], 64 | trans=self.trans[index:index+1], 65 | fps=self.fps, 66 | anim_name=self.name, 67 | ) 68 | elif isinstance(index, slice): 69 | return Animation( 70 | skel=self.skel, 71 | quats=self.quats[index], 72 | trans=self.trans[index], 73 | fps=self.fps, 74 | anim_name=self.name, 75 | ) 76 | else: 77 | raise TypeError 78 | 79 | def cat(self, other: Animation) -> None: 80 | assert isinstance(other, Animation) 81 | 82 | self.quats = np.concatenate( 83 | [self.quats, other.quats], axis=0) 84 | self.trans = np.concatenate( 85 | [self.trans, other.trans], axis=0 86 | ) 87 | 88 | @property 89 | def parents(self) -> list[int]: 90 | return self.skel.parents 91 | 92 | @property 93 | def joint_names(self) -> list[str]: 94 | return self.skel.names 95 | 96 | @property 97 | def offsets(self) -> np.ndarray: 98 | return self.skel.offsets 99 | 100 | # ===================== 101 | # Rotation conversion 102 | # ===================== 103 | @property 104 | def grot(self) -> np.ndarray: 105 | """global rotations(quaternions) for each joints. shape: (T, J, 4)""" 106 | return quat.fk_rot(lrot=self.quats, parents=self.parents) 107 | 108 | @property 109 | def crot(self) -> np.ndarray: 110 | """Projected root(simulation bone) centric global rotations.""" 111 | 112 | # Root rotations relative to the forward vector. 113 | root_rot = self.proj_root_rot 114 | # Root positions projected on the ground (y=0). 115 | root_pos = self.proj_root_pos() 116 | # Make relative to simulation bone. 117 | lrot = self.quats.copy() 118 | lpos = self.lpos.copy() 119 | lrot[:, 0] = quat.inv_mul(root_rot, lrot[:, 0]) 120 | lpos[:, 0] = quat.inv_mul_vec(root_rot, lpos[:, 0] - root_pos) 121 | return quat.fk_rot(lrot, self.parents) 122 | 123 | @property 124 | def axangs(self) -> np.ndarray: 125 | """axis-angle rotation representations. shape: (T, J, 3)""" 126 | return quat.to_axis_angle(self.quats) 127 | 128 | @property 129 | def xforms(self) -> np.ndarray: 130 | """rotation matrix. shape: (T, J, 3, 3)""" 131 | return quat.to_xform(self.quats) 132 | 133 | @property 134 | def ortho6ds(self) -> np.ndarray: 135 | return quat.to_xform_xy(self.quats) 136 | 137 | @property 138 | def sw_tws(self) -> tuple[np.ndarray, np.ndarray]: 139 | """Twist-Swing decomposition. 140 | This function is based on HybrIK. 141 | Remove root rotations. 142 | quat.mul(swings, twists) reproduce original rotations. 143 | return: 144 | twists: (T, J-1, 4), except ROOT. 145 | swings: (T, J-1, 4) 146 | """ 147 | 148 | rots: np.ndarray = self.quats.copy() 149 | pos: np.ndarray = self.lpos.copy() 150 | 151 | children: np.ndarray = \ 152 | -np.ones(len(self.parents), dtype=int) 153 | for i in range(1, len(self.parents)): 154 | children[self.parents[i]] = i 155 | 156 | swings: list[np.ndarray] = [] 157 | twists: list[np.ndarray] = [] 158 | 159 | for i in range(1, len(self.parents)): 160 | # ルートに最も近いJointはtwistのみ / only twist for nearest joint to root. 161 | if children[i] < 0: 162 | swings.append(quat.eye([len(self), 1])) 163 | twists.append(rots[:, i:i+1]) 164 | continue 165 | 166 | u: np.ndarray = pos[:, children[i]:children[i]+1] 167 | rot: np.ndarray = rots[:, i:i+1] 168 | u = u / np.linalg.norm(u, axis=-1, keepdims=True) 169 | v: np.ndarray = quat.mul_vec(rot, u) 170 | 171 | swing: np.ndarray = quat.normalize(quat.between(u, v)) 172 | swings.append(swing) 173 | twist: np.ndarray = quat.inv_mul(swing, rot) 174 | twists.append(twist) 175 | 176 | swings_np: np.ndarray = np.concatenate(swings, axis=1) 177 | twists_np: np.ndarray = np.concatenate(twists, axis=1) 178 | 179 | return swings_np, twists_np 180 | 181 | # ================== 182 | # Position from FK 183 | # ================== 184 | def set_positions_from_fk(self) -> None: 185 | self.positions = self.gpos 186 | 187 | @property 188 | def lpos(self) -> np.ndarray: 189 | lpos: np.ndarray = self.offsets[None].repeat(len(self), axis=0) 190 | lpos[:,0] = self.trans 191 | return lpos 192 | 193 | @property 194 | def gpos(self) -> np.ndarray: 195 | """Global space positions.""" 196 | 197 | _, gpos = quat.fk( 198 | lrot=self.quats, 199 | lpos=self.lpos, 200 | parents=self.parents 201 | ) 202 | return gpos 203 | 204 | @property 205 | def rtpos(self) -> np.ndarray: 206 | """Root-centric local positions.""" 207 | 208 | lrots: np.ndarray = self.quats.copy() 209 | lposs: np.ndarray = self.lpos.copy() 210 | # ROOT to zero. 211 | lrots[:,0] = quat.eye([len(self)]) 212 | lposs[:,0] = np.zeros([len(self), 3]) 213 | 214 | _, rtpos = quat.fk( 215 | lrot=lrots, 216 | lpos=lposs, 217 | parents=self.parents 218 | ) 219 | return rtpos 220 | 221 | @property 222 | def cpos(self) -> np.ndarray: 223 | """Projected root(simulation bone) centric positions.""" 224 | lrot = self.quats.copy() 225 | lpos = self.lpos 226 | c_root_rot, c_root_pos = self.croot() 227 | lrot[:, 0] = c_root_rot 228 | lpos[:, 0] = c_root_pos 229 | 230 | crot, cpos = quat.fk(lrot, lpos, self.parents) 231 | return cpos 232 | 233 | def croot(self, idx: int=None) -> tuple[np.ndarray, np.ndarray]: 234 | """return character space info. 235 | return: 236 | crot: character space root rotations. [T, 4] 237 | cpos: character space root positions. [T, 3] 238 | """ 239 | # Root rotations relative to the forward vector. 240 | root_rot = self.proj_root_rot 241 | # Root positions projected on the ground (y=0). 242 | root_pos = self.proj_root_pos() 243 | # Make relative to simulation bone. 244 | crot = quat.inv_mul(root_rot, self.quats[:, 0]) 245 | cpos = quat.inv_mul_vec(root_rot, self.trans - root_pos) 246 | if idx is not None: 247 | return crot[idx], cpos[idx] 248 | else: return crot, cpos 249 | 250 | # ============ 251 | # Velocity 252 | # ============ 253 | @property 254 | def gposvel(self) -> np.ndarray: 255 | gpos : np.ndarray = self.gpos 256 | gpvel: np.ndarray = np.zeros_like(gpos) 257 | gpvel[1:] = (gpos[1:] - gpos[:-1]) * self.fps # relative position from previous frame. 258 | gpvel[0] = gpvel[1] - (gpvel[3] - gpvel[2]) 259 | return gpvel 260 | 261 | @property 262 | def cposvel(self) -> np.ndarray: 263 | cpos : np.ndarray = self.cpos 264 | cpvel: np.ndarray = np.zeros_like(cpos) 265 | cpvel[1:] = (cpos[1:] - cpos[:-1]) * self.fps # relative position from previous frame. 266 | cpvel[0] = cpvel[1] - (cpvel[3] - cpvel[2]) 267 | return cpvel 268 | 269 | @property 270 | def lrotvel(self) -> np.ndarray: 271 | """Calculate rotation velocities with 272 | rotation vector style.""" 273 | 274 | lrot : np.ndarray = self.quats.copy() 275 | lrvel: np.ndarray = np.zeros_like(self.lpos) 276 | lrvel[1:] = quat.to_axis_angle(quat.abs(quat.mul(lrot[1:], quat.inv(lrot[:-1])))) * self.fps 277 | lrvel[0] = lrvel[1] - (lrvel[3] - lrvel[2]) 278 | return lrvel 279 | 280 | # ============== 281 | # 4x4 matrix 282 | # ============== 283 | @property 284 | def local_transform(self) -> np.ndarray: 285 | xforms: np.ndarray = self.xforms 286 | offsets: np.ndarray = self.offsets 287 | transform: np.ndarray = np.zeros(xforms.shape[:-2] + (4, 4,)) 288 | transform[..., :3, :3] = xforms 289 | transform[..., :3, 3] = offsets 290 | transform[..., 3, 3] = 1 291 | return transform 292 | 293 | @property 294 | def global_transform(self) -> np.ndarray: 295 | ltrans: np.ndarray = self.local_transform.copy() 296 | parents: list[int] = self.parents 297 | 298 | gtrans = [ltrans[...,:1,:,:]] 299 | for i in range(1, len(parents)): 300 | gtrans.append(np.matmul(gtrans[parents[i]],ltrans[...,i:i+1,:,:])) 301 | 302 | return np.concatenate(gtrans, axis=-3) 303 | 304 | # ================== 305 | # dual quaternions 306 | # ================== 307 | @property 308 | def local_dualquat(self) -> np.ndarray: 309 | return dq.from_rot_and_trans(self.quats, self.lpos) 310 | 311 | @property 312 | def global_dualquat(self) -> np.ndarray: 313 | return dq.fk(self.local_dualquat, self.parents) 314 | 315 | # ============= 316 | # trajectory 317 | # ============= 318 | def proj_root_pos(self, remove_vertical: bool=False) -> np.ndarray: 319 | """Root position projected on the ground (world space). 320 | return: 321 | Projected bone positons as ndarray of shape [len(self), 3] or [len(self), 2](remove_vertical). 322 | """ 323 | vertical = self.skel.vertical 324 | if remove_vertical: 325 | settle_ax = [] 326 | for i, ax in enumerate(vertical): 327 | if ax == 0: 328 | settle_ax.append(i) 329 | return self.trans[..., settle_ax] 330 | else: 331 | return self.trans * np.array([abs(abs(ax) - 1) for ax in vertical]) 332 | 333 | @property 334 | def proj_root_rot(self) -> np.ndarray: 335 | # root rotations relative to the forward on the ground. [len(self), 4] 336 | forward = self.skel.forward 337 | return quat.normalize( 338 | quat.between(np.array(forward), self.root_direction()) 339 | ) 340 | 341 | def root_direction(self, remove_vertical: bool=False) -> np.ndarray: 342 | """Forward orientation vectors on the ground (world space). 343 | return: 344 | Forward vectors as ndarray of shape [..., 3] or [..., 2](remove_vertical). 345 | """ 346 | # Calculate forward vectors except vertical axis. 347 | vertical = self.skel.vertical 348 | rt_rots = self.quats[..., 0, :] 349 | forwards = np.zeros(shape=rt_rots.shape[:-1] + (3,)) 350 | forwards[...,] = self.skel.rest_forward 351 | rt_fwd = quat.mul_vec(rt_rots, forwards) * np.array([abs(abs(ax) - 1) for ax in vertical]) # [T, 3] 352 | # Normalize vectors. 353 | norm_rt_fwd = rt_fwd / np.linalg.norm(rt_fwd, axis=-1, keepdims=True) 354 | if remove_vertical: 355 | settle_ax = [] 356 | for i, ax in enumerate(vertical): 357 | if ax == 0: 358 | settle_ax.append(i) 359 | norm_rt_fwd = norm_rt_fwd[..., settle_ax] 360 | return norm_rt_fwd 361 | 362 | # =================== 363 | # Future trajectory 364 | # =================== 365 | def future_traj_poss(self, frame: int, remove_vertical: bool=True, cspace=True) -> np.ndarray: 366 | """Calculate future trajectory positions on simulation bone. 367 | Args: 368 | frame (int): how many ahead frame to see. 369 | remove_vertical (bool, optional): remove vertical axis positions. Defaults to True. 370 | cspace (bool, optional): use local character space. Defaults to True. 371 | Returns: 372 | np.ndarray: future trajectories positions. shape=(len(self), 3) or (len(self), 2) 373 | """ 374 | idxs = self.clamp_future_idxs(frame) 375 | proj_root_pos = self.proj_root_pos() 376 | if cspace: 377 | traj_pos = quat.inv_mul_vec(self.proj_root_rot, proj_root_pos[idxs] - proj_root_pos) 378 | else: 379 | traj_pos = proj_root_pos[idxs] 380 | 381 | if remove_vertical: 382 | vertical = self.skel.vertical 383 | settle_ax = [] 384 | for i, ax in enumerate(vertical): 385 | if ax == 0: 386 | settle_ax.append(i) 387 | return traj_pos[..., settle_ax] 388 | else: 389 | return traj_pos 390 | 391 | def future_traj_dirs(self, frame: int, remove_vertical: bool=True, cspace=True) -> np.ndarray: 392 | """Calculate future trajectory directions on simulation bone (local character space). 393 | Args: 394 | frame (int): how many ahead frame to see. 395 | remove_vertical (bool, optional): remove vertical axis. Defaults to True. 396 | cspace (bool, optional): use local character space. Defaults to True. 397 | Returns: 398 | np.ndarray: future trajectories directions. shape=(len(self), 3) or (len(self), 2) 399 | """ 400 | idxs = self.clamp_future_idxs(frame) 401 | root_directions = self.root_direction() 402 | if cspace: 403 | traj_dir = quat.inv_mul_vec(self.proj_root_rot, root_directions[idxs]) 404 | else: 405 | traj_dir = root_directions[idxs] 406 | 407 | if remove_vertical: 408 | vertical = self.skel.vertical 409 | settle_ax = [] 410 | for i, ax in enumerate(vertical): 411 | if ax == 0: 412 | settle_ax.append(i) 413 | return traj_dir[..., settle_ax] 414 | else: 415 | return traj_dir 416 | 417 | def clamp_future_idxs(self, offset: int) -> np.ndarray: 418 | """Function to calculate the frame array for `offset` frame ahead. 419 | If `offset` frame ahead does not exist, 420 | return the last frame. 421 | """ 422 | idxs = np.arange(len(self)) + offset 423 | idxs[-(offset + 1):] = idxs[-(offset + 1)] 424 | return idxs 425 | 426 | # ===================== 427 | # Other functions 428 | # ===================== 429 | def calc_foot_contact( 430 | self, 431 | method: str="velocity", 432 | threshold: float=0.15, 433 | left_foot_name: str="LeftToe", 434 | right_foot_name: str="RightToe", 435 | ) -> np.ndarray: 436 | if method == "velocity": 437 | contact_vel = np.linalg.norm( 438 | self.gposvel[:, 439 | [self.joint_names.index(left_foot_name), 440 | self.joint_names.index(right_foot_name)] 441 | ], axis=-1 442 | ) 443 | contacts = contact_vel < threshold 444 | elif method == "position": 445 | # vertical axis position for each frame. 446 | settle_idx = 0 447 | inverse = False # Is the negative direction vertical? 448 | for i, ax in enumerate(self.skel.vertical): 449 | if ax == 1: 450 | settle_idx = i 451 | if ax == -1: 452 | settle_idx = i 453 | inverse = True 454 | contact_pos = self.gpos[:, 455 | [self.joint_names.index(left_foot_name), 456 | self.joint_names.index(right_foot_name)], settle_idx] 457 | if inverse: 458 | contact_pos *= -1 459 | contacts = contact_pos < threshold 460 | else: 461 | raise ValueError("unknown value selected on `method`.") 462 | for ci in range(contacts.shape[1]): 463 | contacts[:, ci] = ndimage.median_filter( 464 | contacts[:, ci], 465 | size=6, 466 | mode="nearest" 467 | ) 468 | return contacts 469 | 470 | def mirror(self, dataset: str=None) -> Animation: 471 | vertical = self.skel.vertical 472 | forward = self.skel.forward 473 | mirror_axis = [] 474 | for vert_ax, fwd_ax in zip(vertical, forward): 475 | if abs(vert_ax) == 1 or abs(fwd_ax) == 1: 476 | mirror_axis.append(1) 477 | else: 478 | mirror_axis.append(-1) 479 | if dataset == "lafan1": 480 | quatM, lposM = animation_mirror( 481 | lrot=self.quats, 482 | lpos=self.lpos, 483 | names=self.joint_names, 484 | parents=self.parents 485 | ) 486 | transM = lposM[:, 0] 487 | else: 488 | quatM, transM = mirror_rot_trans( 489 | lrot=self.quats, 490 | trans=self.trans, 491 | names=self.joint_names, 492 | parents=self.parents, 493 | mirror_axis=mirror_axis, 494 | ) 495 | return Animation( 496 | skel=self.skel, 497 | quats=quatM, 498 | trans=transM, 499 | fps=self.fps, 500 | anim_name=self.name+"_M", 501 | ) 502 | 503 | @staticmethod 504 | def no_animation( 505 | skel: Skel, 506 | fps: int=30, 507 | num_frame: int=1, 508 | anim_name: str="animation", 509 | ) -> Animation: 510 | """Create a unit animation (no rotation, no transition) from Skel""" 511 | quats = quat.eye([num_frame, len(skel)]) 512 | trans = np.zeros([num_frame, 3]) 513 | return Animation(skel, quats, trans, None, fps, anim_name) 514 | 515 | 516 | def mirror_rot_trans( 517 | lrot: np.ndarray, 518 | trans: np.ndarray, 519 | names: list[str], 520 | parents: np.ndarray | list[int], 521 | mirror_axis: list[int] 522 | ) -> tuple[np.ndarray, np.ndarray]: 523 | 524 | lower_names = [n.lower() for n in names] 525 | joints_mirror: np.ndarray = np.array([( 526 | lower_names.index("left"+n[5:]) if n.startswith("right") else 527 | lower_names.index("right"+n[4:]) if n.startswith("left") else 528 | lower_names.index(n)) for n in lower_names]) 529 | 530 | mirror_pos: np.ndarray = np.array(mirror_axis) 531 | mirror_rot: np.ndarray = np.array([1,] + [-ax for ax in mirror_axis]) 532 | grot: np.ndarray = quat.fk_rot(lrot, parents) 533 | trans_mirror: np.ndarray = mirror_pos * trans 534 | grot_mirror: np.ndarray = mirror_rot * grot[:,joints_mirror] 535 | 536 | return quat.ik_rot(grot_mirror, parents), trans_mirror 537 | 538 | # for LAFAN1 dataset. This function from 539 | # https://github.com/orangeduck/Motion-Matching/blob/5e57a1352812494cca83396785cfc4985df9170c/resources/generate_database.py#L15 540 | def animation_mirror(lrot, lpos, names, parents): 541 | 542 | joints_mirror = np.array([( 543 | names.index('Left'+n[5:]) if n.startswith('Right') else ( 544 | names.index('Right'+n[4:]) if n.startswith('Left') else 545 | names.index(n))) for n in names]) 546 | 547 | mirror_pos = np.array([-1, 1, 1]) 548 | mirror_rot = np.array([[-1, -1, 1], [1, 1, -1], [1, 1, -1]]) 549 | 550 | grot, gpos = quat.fk(lrot, lpos, parents) 551 | 552 | gpos_mirror = mirror_pos * gpos[:,joints_mirror] 553 | grot_mirror = quat.from_xform(mirror_rot * quat.to_xform(grot[:,joints_mirror])) 554 | 555 | return quat.ik(grot_mirror, gpos_mirror, parents) --------------------------------------------------------------------------------