├── .editorconfig ├── Makefile ├── pyproject.toml ├── LICENSE ├── .gitignore ├── tests └── test_pyfabrik.py ├── README.md └── pyfabrik └── __init__.py /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*] 4 | indent_style = space 5 | indent_size = 4 6 | end_of_line = lf 7 | charset = utf-8 8 | trim_trailing_whitespace = true 9 | insert_final_newline = true 10 | 11 | [*.py] 12 | indent_style = space 13 | indent_size = 4 14 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | test: 2 | poetry run pytest tests 3 | 4 | build: 5 | poetry run poetry build 6 | 7 | publish: 8 | poetry run poetry publish 9 | 10 | check: 11 | poetry run mypy pyfabrik --ignore-missing-imports 12 | poetry run mypy tests --ignore-missing-imports 13 | poetry run black --check pyfabrik 14 | poetry run black --check tests 15 | 16 | format: 17 | poetry run black pyfabrik 18 | poetry run black tests 19 | 20 | fmt: format 21 | 22 | clean: 23 | find . -name '__pycache__' -exec rm -rv {} \; 24 | find . -name '*.py[co]' -exec rm -rv {} \; 25 | rm -fr dist 26 | rm -fr pyfabrik.egg-info 27 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "pyfabrik" 3 | version = "0.5.0-dev1" 4 | description = "Python 3 implementation of FABRIK (Forward And Backward Reaching Inverse Kinematics) algorithm." 5 | authors = [ 6 | { name = "Saša Savić", email = "sasa@savic.one" } 7 | ] 8 | license = { text = "MIT" } 9 | readme = "README.md" 10 | classifiers = [ 11 | "Programming Language :: Python :: 3 :: Only", 12 | "Programming Language :: Python :: 3.6", 13 | "Operating System :: OS Independent", 14 | "License :: OSI Approved :: MIT License" 15 | ] 16 | requires-python = ">=3.6" 17 | 18 | dependencies = [ 19 | "vectormath>=0.2.2" 20 | ] 21 | 22 | [project.urls] 23 | homepage = "https://github.com/saleone/pyfabrik" 24 | repository = "https://github.com/saleone/pyfabrik" 25 | 26 | [dependency-groups] 27 | dev = [ 28 | "ipython>=7.10", 29 | "pytest>=5.3", 30 | "mypy>=0.761.0", 31 | "black>=18.3a0" 32 | ] 33 | 34 | [build-system] 35 | requires = ["setuptools", "wheel", "uv"] 36 | build-backend = "setuptools.build_meta" 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Saša Savić 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 | 23 | -------------------------------------------------------------------------------- /.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 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | 106 | # coc-nvim 107 | .vim/ 108 | -------------------------------------------------------------------------------- /tests/test_pyfabrik.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import pytest 4 | from vectormath import Vector2 5 | from vectormath import Vector3 6 | from pyfabrik import Fabrik2D 7 | from pyfabrik import Fabrik3D 8 | from pyfabrik import Fabrik 9 | 10 | 11 | def test_default_fabrik_class_is_2d_solver(): 12 | assert Fabrik is Fabrik2D 13 | 14 | 15 | def test_2d_correctly_moves_the_joints(): 16 | poss = [Vector2(0, 0), Vector2(10, 0), Vector2(20, 0)] 17 | fab = Fabrik2D(poss, 0.01) 18 | 19 | assert fab.move_to(Vector2(20, 0)) == 0 20 | assert fab.angles_deg == [0.0, 0.0] 21 | print(fab.angles_deg) 22 | 23 | assert fab.move_to(Vector2(60, 60)) == 249 24 | assert fab.angles_deg == [43.187653094161064, 3.622882738369357] 25 | print(fab.angles_deg) 26 | 27 | assert fab.move_to(Vector2(0, 20)) == 250 28 | assert fab.angles_deg == [88.19119752090381, 3.6158044811401675] 29 | print(fab.angles_deg) 30 | 31 | assert fab.move_to(Vector2(0, 10)) == 5 32 | assert fab.angles_deg == [30.05682734132901, 119.97158632933548] 33 | print(fab.angles_deg) 34 | 35 | 36 | def test_3d_correctly_moves_in_2d_space(): 37 | poss = [Vector3(0, 0, 0), Vector3(10, 0, 0), Vector3(20, 0, 0)] 38 | fab = Fabrik3D(poss, 0.01) 39 | 40 | assert fab.move_to(Vector3(20, 0, 0)) == 0 41 | assert fab.angles_deg == [0.0, 0.0] 42 | print(fab.angles_deg) 43 | 44 | assert fab.move_to(Vector3(60, 60, 0)) == 249 45 | assert fab.angles_deg == [43.187653094161064, 3.622882738369357] 46 | print(fab.angles_deg) 47 | 48 | assert fab.move_to(Vector3(0, 20, 0)) == 250 49 | assert fab.angles_deg == [88.19119752090381, 3.6158044811401675] 50 | print(fab.angles_deg) 51 | 52 | assert fab.move_to(Vector3(0, 10, 0)) == 5 53 | assert fab.angles_deg == [30.05682734132901, 119.97158632933548] 54 | print(fab.angles_deg) 55 | 56 | 57 | def test_value_error_raised_when_joints_overlap(): 58 | poss = [Vector3(0, 0, 0), Vector3(10, 0, 0), Vector3(10, 0, 0)] 59 | with pytest.raises(ValueError) as exinfo: 60 | _ = Fabrik3D(poss, 0.01) 61 | assert str(exinfo.value) == "link lengths must be > 0" 62 | 63 | 64 | @pytest.mark.parametrize("tolerance", [-1.0, 0.0]) 65 | def test_value_error_raised_when_tolerance_isnt_positive(tolerance): 66 | poss = [Vector3(0, 0, 0), Vector3(10, 0, 0), Vector3(10, 0, 0)] 67 | with pytest.raises(ValueError) as exinfo: 68 | fab = Fabrik3D(poss, tolerance) 69 | assert str(exinfo.value) == "tolerance must be > 0" 70 | 71 | 72 | if __name__ == "__main__": 73 | import timeit 74 | 75 | times = timeit.Timer("test_main()", setup="from __main__ import test_main").repeat( 76 | 1, number=1 77 | ) 78 | 79 | times = [time / 100 for time in times] 80 | print("finished in {}s:".format(sum(times))) 81 | for i, time in enumerate(times): 82 | print("\t {} - {}".format(i + 1, time)) 83 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pyfabrik 2 | 3 | ![Badge showing number of total downloads from PyPI.](https://pepy.tech/badge/pyfabrik) 4 | 5 | ![Badge showing number of monthly downloads from PyPI.](https://pepy.tech/badge/pyfabrik/month) 6 | 7 | ![Badge showing that code has been formated with Black formatter.](https://img.shields.io/badge/code%20style-black-000000.svg) 8 | 9 | Python 3 implementation of 10 | [FABRIK](http://www.andreasaristidou.com/FABRIK.html) (Forward And 11 | Backward Reaching Inverse Kinematics). 12 | ## Installation 13 | 14 | pip install pyfabrik 15 | 16 | ## Usage 17 | 18 | **NOTE: API is still very unstable (until the 1.0 release). Suggestions are welcome.** 19 | 20 | ```python 21 | 22 | import pyfabrik 23 | from vectormath import Vector3 24 | 25 | initial_joint_positions = [Vector3(0, 0, 0), Vector3(10, 0, 0), Vector3(20, 0, 0)] 26 | tolerance = 0.01 27 | 28 | # Initialize the Fabrik class (Fabrik, Fabrik2D or Fabrik3D) 29 | fab = pyfabrik.Fabrik3D(initial_joint_positions, tolerance) 30 | 31 | fab.move_to(Vector3(20, 0, 0)) 32 | fab.angles_deg # Holds [0.0, 0.0, 0.0] 33 | 34 | fab.move_to(Vector3(60, 60, 0)) # Return 249 as number of iterations executed 35 | fab.angles_deg # Holds [43.187653094161064, 3.622882738369357, 0.0] 36 | ``` 37 | 38 | 39 | ## Goal 40 | ![Inverse kinematics example with human skeleton.](http://www.andreasaristidou.com/publications/images/FABRIC_gif_1.gif) 41 | 42 | ## Roadmap 43 | 44 | - [x] Basic 2D (flat chain) 45 | - [x] Basic 3D (flat chain) 46 | - [ ] 3D testing sandbox 47 | - [ ] Basic 2D joint movement restrictions 48 | - [ ] Basic 3D joint movement restrictions 49 | - [ ] Complex chain support 2D 50 | - [ ] Complex chain support 3D 51 | 52 | ## Contributing 53 | 54 | __All contributions are appreciated.__ 55 | 56 | Read the paper [paper](http://www.andreasaristidou.com/publications/papers/FABRIK.pdf). 57 | 58 | FABRIKs [homepage](http://www.andreasaristidou.com/FABRIK.html) has links to other implementations. 59 | 60 | ## [License](./LICENSE) 61 | 62 | MIT License 63 | 64 | Copyright (c) 2020 Saša Savić 65 | 66 | Permission is hereby granted, free of charge, to any person obtaining a copy 67 | of this software and associated documentation files (the "Software"), to deal 68 | in the Software without restriction, including without limitation the rights 69 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 70 | copies of the Software, and to permit persons to whom the Software is 71 | furnished to do so, subject to the following conditions: 72 | 73 | The above copyright notice and this permission notice shall be included in all 74 | copies or substantial portions of the Software. 75 | 76 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 77 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 78 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 79 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 80 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 81 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 82 | SOFTWARE. 83 | -------------------------------------------------------------------------------- /pyfabrik/__init__.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | from vectormath import Vector2 4 | from vectormath import Vector3 5 | 6 | 7 | class FabrikBase: 8 | joints: list[Vector2 | Vector3] 9 | 10 | def __init__( 11 | self, joint_positions: list[Vector2 | Vector3], tolerance: float 12 | ) -> None: 13 | # Tolerance is measured as distance (no negative values) and 14 | # when tolerance is 0 solver won't be able to finish. 15 | if tolerance <= 0: 16 | raise ValueError("tolerance must be > 0") 17 | self.tolerance: float = tolerance 18 | link_lengths = [] 19 | 20 | joint_a = joint_positions[0] 21 | for joint_b in joint_positions[1:]: 22 | link_lengths.append((joint_a - joint_b).length) 23 | joint_a = joint_b 24 | 25 | if any([ll <= 0 for ll in link_lengths]): 26 | raise ValueError("link lengths must be > 0") 27 | 28 | self.lengths: list[float] = link_lengths 29 | self.max_len: float = sum(link_lengths) 30 | 31 | # Calculate initial angles 32 | self._has_moved = True 33 | self._angles: list[float] = [] 34 | _ = self.angles 35 | 36 | @property 37 | def angles(self) -> list[float]: 38 | # Only calculate angles if chain moved. 39 | if not self._has_moved: 40 | return self._angles 41 | 42 | print(self.joints) 43 | angles = [math.atan2(self.joints[1].y, self.joints[1].x)] 44 | 45 | prev_angle: float = angles[0] 46 | for i in range(2, len(self.joints)): 47 | p = self.joints[i] - self.joints[i - 1] 48 | abs_angle: float = math.atan2(p.y, p.x) 49 | angles.append(abs_angle - prev_angle) 50 | prev_angle = abs_angle 51 | 52 | self.has_moved = False 53 | self._angles = angles 54 | return self._angles 55 | 56 | def solvable(self, target: Vector2 | Vector3) -> bool: 57 | return self.max_len >= target.length 58 | 59 | @property 60 | def angles_deg(self) -> list[float]: 61 | return [math.degrees(val) for val in self.angles] 62 | 63 | def move_to(self, target: Vector2 | Vector3, try_to_reach: bool = True) -> int: 64 | if not self.solvable(target): 65 | if not try_to_reach: 66 | return 0 67 | target = target.as_length(self.max_len) 68 | return self._iterate(target) 69 | 70 | def _iterate(self, target: Vector2 | Vector3) -> int: 71 | iteration: int = 0 72 | initial_position: Vector2 | Vector3 = self.joints[0] 73 | last: int = len(self.joints) - 1 74 | 75 | while (self.joints[-1] - target).length > self.tolerance: 76 | iteration += 1 77 | 78 | self.joints[-1] = target 79 | for i in reversed(range(0, last)): 80 | next, current = self.joints[i + 1], self.joints[i] 81 | len_share = self.lengths[i] / (next - current).length 82 | self.joints[i] = (1 - len_share) * next + len_share * current 83 | 84 | self.joints[0] = initial_position 85 | for i in range(0, last): 86 | next, current = self.joints[i + 1], self.joints[i] 87 | len_share = self.lengths[i] / (next - current).length 88 | self.joints[i + 1] = (1 - len_share) * current + len_share * next 89 | return iteration 90 | 91 | 92 | class Fabrik2D(FabrikBase): 93 | def __init__(self, joint_positions: list[Vector2], tolerance: float = 0.0) -> None: 94 | self.joints: list[Vector2] = joint_positions 95 | print(self.joints) 96 | super().__init__(joint_positions, tolerance) 97 | 98 | def move_to(self, target: Vector2, try_to_reach: bool = True) -> int: 99 | return super().move_to(target, try_to_reach) 100 | 101 | def solvable(self, target: Vector2) -> bool: 102 | return super().solvable(target) 103 | 104 | 105 | class Fabrik3D(FabrikBase): 106 | def __init__(self, joint_positions: list[Vector3], tolerance: float = 0.0) -> None: 107 | self.joints: list[Vector3] = joint_positions 108 | super().__init__(joint_positions, tolerance) 109 | 110 | def solvable(self, target: Vector3) -> bool: 111 | return super().solvable(target) 112 | 113 | def move_to(self, target: Vector3, try_to_reach: bool = True) -> int: 114 | return super().move_to(target, try_to_reach) 115 | 116 | 117 | Fabrik = Fabrik2D 118 | 119 | __all__ = ["Fabrik", "Fabrik2D", "Fabrik3D"] 120 | --------------------------------------------------------------------------------