├── .gitignore ├── .travis.yml ├── LICENSE ├── README.rst ├── assets ├── viz_ik.png └── viz_structure.png ├── pyproject.toml ├── tests ├── __init__.py ├── test_core.py ├── test_optimizer.py ├── test_solver.py ├── test_visualizer.py └── utils.py └── tinyik ├── __init__.py ├── component.py ├── core.py ├── optimizer.py ├── solver.py └── visualizer.py /.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 | # poetry 132 | poetry.lock 133 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | python: 3 | - 3.5 4 | - 3.6 5 | - 3.7 6 | - 3.8 7 | before_install: 8 | - sudo apt-get install gfortran libopenblas-dev liblapack-dev 9 | - pip install poetry 10 | install: 11 | - poetry install 12 | script: 13 | - poetry run flake8 14 | - poetry run pytest 15 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 lanius 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 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | tinyik 2 | ====== 3 | 4 | tinyik is a simple and naive inverse kinematics solver. 5 | 6 | It defines the actuator as a set of links and revolute joints from an origin. Here is the example of a robot arm that consists of two joints that rotate around z-axis and two links of 1.0 length along x-axis: 7 | 8 | .. code-block:: python 9 | 10 | >>> import tinyik 11 | >>> arm = tinyik.Actuator(['z', [1., 0., 0.], 'z', [1., 0., 0.]]) 12 | 13 | Since the joint angles are zero by default, the end-effector position is at (2.0, 0, 0): 14 | 15 | .. code-block:: python 16 | 17 | >>> arm.angles 18 | array([ 0., 0.]) 19 | >>> arm.ee 20 | array([ 2., 0., 0.]) 21 | 22 | Sets the joint angles to 30 and 60 degrees to calculate a new position of the end-effector: 23 | 24 | .. code-block:: python 25 | 26 | >>> import numpy as np 27 | >>> arm.angles = [np.pi / 6, np.pi / 3] # or np.deg2rad([30, 60]) 28 | >>> arm.ee 29 | array([ 0.8660254, 1.5 , 0. ]) 30 | 31 | Sets a position of the end-effector to calculate the joint angles: 32 | 33 | .. code-block:: python 34 | 35 | >>> arm.ee = [2 / np.sqrt(2), 2 / np.sqrt(2), 0.] 36 | >>> arm.angles 37 | array([ 7.85398147e-01, 3.23715739e-08]) 38 | >>> np.round(np.rad2deg(arm.angles)) 39 | array([ 45., 0.]) 40 | 41 | Optionally, it has the visualization feature. Passes the actuator to it to visualize its structure: 42 | 43 | .. code-block:: python 44 | 45 | >>> leg = tinyik.Actuator([[.3, .0, .0], 'z', [.3, .0, .0], 'x', [.0, -.5, .0], 'x', [.0, -.5, .0]]) 46 | >>> leg.angles = np.deg2rad([30, 45, -90]) 47 | >>> tinyik.visualize(leg) 48 | 49 | .. image:: https://raw.githubusercontent.com/lanius/tinyik/master/assets/viz_structure.png 50 | 51 | Passes with the target position, can compare before and after the IK. The gray links are before IK and the white links are after it. The red sphere is the target position: 52 | 53 | .. code-block:: python 54 | 55 | >>> tinyik.visualize(leg, target=[.8, .0, .8]) 56 | 57 | .. image:: https://raw.githubusercontent.com/lanius/tinyik/master/assets/viz_ik.png 58 | 59 | Installation 60 | ------------ 61 | 62 | .. code-block:: console 63 | 64 | $ pip install tinyik 65 | 66 | With the visualization feature: 67 | 68 | .. code-block:: console 69 | 70 | $ pip install tinyik[viz] 71 | -------------------------------------------------------------------------------- /assets/viz_ik.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lanius/tinyik/849224214ba1c669b2409c3ad8012ba583e36258/assets/viz_ik.png -------------------------------------------------------------------------------- /assets/viz_structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lanius/tinyik/849224214ba1c669b2409c3ad8012ba583e36258/assets/viz_structure.png -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "tinyik" 3 | version = "2.2.1" 4 | description = "A tiny inverse kinematics solver" 5 | license = "MIT" 6 | authors = ["lanius "] 7 | readme = "README.rst" 8 | repository = "https://github.com/lanius/tinyik" 9 | 10 | [tool.poetry.dependencies] 11 | python = "^3.5" 12 | autograd = "*" 13 | scipy = "*" 14 | open3d = {version = "^0.9.0", optional = true} 15 | 16 | [tool.poetry.dev-dependencies] 17 | pytest = "*" 18 | flake8 = "*" 19 | 20 | [tool.poetry.extras] 21 | viz = ["open3d"] 22 | 23 | [build-system] 24 | requires = ["poetry>=0.12"] 25 | build-backend = "poetry.masonry.api" 26 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lanius/tinyik/849224214ba1c669b2409c3ad8012ba583e36258/tests/__init__.py -------------------------------------------------------------------------------- /tests/test_core.py: -------------------------------------------------------------------------------- 1 | from tinyik import Actuator 2 | 3 | from .utils import x, y, z, theta, approx_eq 4 | 5 | 6 | def test_actuator_instantiation(): 7 | two_joints_arm = Actuator(['z', 1., 'z', 1.]) 8 | assert len(two_joints_arm.angles) == 2 9 | assert all(two_joints_arm.angles == 0.) 10 | assert all(two_joints_arm.ee == [2., 0., 0.]) 11 | 12 | three_joints_arm = Actuator(['x', 1., 'y', 1., 'z', 1.]) 13 | assert len(three_joints_arm.angles) == 3 14 | assert all(three_joints_arm.angles == 0.) 15 | assert all(three_joints_arm.ee == [3., 0., 0.]) 16 | 17 | y_axis_dir_arm = Actuator(['z', [0., 1., 0.], 'z', [0., 1., 0.]]) 18 | assert all(y_axis_dir_arm.ee == [0., 2., 0.]) 19 | 20 | 21 | def test_actuator_angles(): 22 | arm = Actuator(['z', 1., 'y', 1.]) 23 | arm.angles = [theta, theta] 24 | assert approx_eq(arm.angles, [theta, theta]) 25 | assert approx_eq(arm.ee, [x, y, -z]) 26 | 27 | 28 | def test_actuator_ee(): 29 | arm = Actuator(['z', 1., 'y', 1.]) 30 | arm.ee = [x, -y, z] 31 | assert approx_eq(arm.ee, [x, -y, z]) 32 | assert approx_eq(arm.angles, [-theta, -theta]) 33 | 34 | 35 | def test_multiple_actuator_ee(): 36 | first_arm = Actuator( 37 | ['y', [0., 0., 0.01], 'x', [0., 0., 1.11], 'x', [0., 0., 1.225]]) 38 | second_arm = Actuator( 39 | ['x', [0., 0., 1.175], 'x', [0., 0., 1.225], 'x', [0., 0., .1]]) 40 | angles = [0, -1, 2.5] 41 | first_arm.angles = angles[:] 42 | first_arm.ee = first_arm.ee + [0., 0., 0.] 43 | assert approx_eq(first_arm.angles, angles) 44 | first_arm.ee = first_arm.ee + [0., 0., 0.] 45 | assert approx_eq(first_arm.angles, angles) 46 | assert approx_eq(second_arm.angles, [0., 0., 0.]) 47 | -------------------------------------------------------------------------------- /tests/test_optimizer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from tinyik import ( 4 | Link, Joint, 5 | FKSolver, IKSolver, 6 | NewtonOptimizer, 7 | SteepestDescentOptimizer, 8 | ConjugateGradientOptimizer, 9 | ScipyOptimizer, ScipySmoothOptimizer 10 | ) 11 | 12 | from .utils import x, y, z, theta, approx_eq 13 | 14 | 15 | def build_ik_solver(optimizer_instance): 16 | fk = FKSolver([ 17 | Joint('z'), Link([1., 0., 0.]), Joint('y'), Link([1., 0., 0.]) 18 | ]) 19 | return IKSolver(fk, optimizer_instance) 20 | 21 | 22 | def test_inverse_kinematics_with_newton(): 23 | ik = build_ik_solver(NewtonOptimizer()) 24 | assert approx_eq(ik.solve([theta, theta], [2., 0., 0.]), [0., 0.]) 25 | assert approx_eq(ik.solve([0., 0.], [x, y, -z]), [theta, theta]) 26 | assert approx_eq(ik.solve([0., 0.], [x, -y, z]), [-theta, -theta]) 27 | 28 | 29 | def test_inverse_kinematics_with_steepest_descent(): 30 | ik = build_ik_solver(SteepestDescentOptimizer(maxiter=100, alpha=0.1)) 31 | assert approx_eq(ik.solve([theta, theta], [2., 0., 0.]), [0., 0.]) 32 | assert approx_eq(ik.solve([0., 0.], [x, y, -z]), [theta, theta]) 33 | assert approx_eq(ik.solve([0., 0.], [x, -y, z]), [-theta, -theta]) 34 | 35 | 36 | def test_inverse_kinematics_with_conjugate_gradient(): 37 | ik = build_ik_solver(ConjugateGradientOptimizer()) 38 | assert approx_eq(ik.solve([theta, theta], [2., 0., 0.]), [0., 0.]) 39 | assert approx_eq(ik.solve([0., 0.], [x, y, -z]), [theta, theta]) 40 | assert approx_eq(ik.solve([0., 0.], [x, -y, z]), [-theta, -theta]) 41 | 42 | 43 | def test_inverse_kinematics_with_scipy(): 44 | ik = build_ik_solver(ScipyOptimizer()) 45 | assert approx_eq(ik.solve([theta, theta], [2., 0., 0.]), [0., 0.]) 46 | assert approx_eq(ik.solve([0., 0.], [x, y, -z]), [theta, theta]) 47 | assert approx_eq(ik.solve([0., 0.], [x, -y, z]), [-theta, -theta]) 48 | 49 | 50 | def test_inverse_kinematics_with_scipy_smooth(): 51 | ik = build_ik_solver(ScipySmoothOptimizer(smooth_factor=0.)) 52 | assert approx_eq(ik.solve([theta, theta], [2., 0., 0.]), [0., 0.]) 53 | assert approx_eq(ik.solve([0., 0.], [x, y, -z]), [theta, theta]) 54 | assert approx_eq(ik.solve([0., 0.], [x, -y, z]), [-theta, -theta]) 55 | 56 | def approx_roughly_eq(a, b): 57 | return all(np.round(a, 1) == np.round(b, 1)) 58 | 59 | ik = build_ik_solver(ScipySmoothOptimizer()) 60 | assert approx_roughly_eq(ik.solve([theta, theta], [2., 0., 0.]), [0., 0.]) 61 | assert approx_roughly_eq(ik.solve([0., 0.], [x, y, -z]), [theta, theta]) 62 | assert approx_roughly_eq(ik.solve([0., 0.], [x, -y, z]), [-theta, -theta]) 63 | -------------------------------------------------------------------------------- /tests/test_solver.py: -------------------------------------------------------------------------------- 1 | from tinyik import Link, Joint, FKSolver, CCDFKSolver, CCDIKSolver 2 | 3 | from .utils import x, y, z, theta, approx_eq 4 | 5 | 6 | components = [Joint('z'), Link([1., 0., 0.]), Joint('y'), Link([1., 0., 0.])] 7 | predicted = [2., 0., 0.] 8 | 9 | 10 | def test_fk(): 11 | fk = FKSolver(components) 12 | assert all(fk.solve([0., 0.]) == predicted) 13 | 14 | assert approx_eq(fk.solve([theta, theta]), [x, y, -z]) 15 | assert approx_eq(fk.solve([-theta, -theta]), [x, -y, z]) 16 | 17 | 18 | def test_ccd_fk(): 19 | fk = CCDFKSolver(components) 20 | assert all(fk.solve([0., 0.]) == predicted) 21 | 22 | assert approx_eq(fk.solve([theta, theta]), [x, y, -z]) 23 | assert approx_eq(fk.solve([-theta, -theta]), [x, -y, z]) 24 | 25 | 26 | def test_ccd_ik(): 27 | fk = CCDFKSolver(components) 28 | ik = CCDIKSolver(fk) 29 | assert approx_eq(ik.solve([0., 0.], [x, y, -z]), [theta, theta]) 30 | assert approx_eq(ik.solve([0., 0.], [x, -y, z]), [-theta, -theta]) 31 | -------------------------------------------------------------------------------- /tests/test_visualizer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import tinyik 4 | 5 | 6 | tokens = [ 7 | [.3, .0, .0], 'z', [.3, .0, .0], 'x', [.0, -.5, .0], 'x', [.0, -.5, .0]] 8 | 9 | 10 | def visualize(): 11 | leg = tinyik.Actuator(tokens) 12 | leg.angles = np.deg2rad([30, 45, -90]) 13 | tinyik.visualize(leg) 14 | 15 | 16 | def visualize_with_target(): 17 | leg = tinyik.Actuator(tokens) 18 | leg.angles = np.deg2rad([30, 45, -90]) 19 | tinyik.visualize(leg, target=[.8, .0, .8]) 20 | 21 | 22 | large_tokens = [ 23 | [85., 80., 0.], 24 | 'z', 25 | [500., 0., 0.], 26 | 'z', 27 | [0., -500., 0.], 28 | ] 29 | 30 | 31 | def large_visualize(): 32 | arm = tinyik.Actuator(large_tokens) 33 | tinyik.visualize(arm, radius=15.) 34 | 35 | 36 | def large_visualize_with_target(): 37 | arm = tinyik.Actuator(large_tokens) 38 | tinyik.visualize(arm, target=[400., -300., 0.], radius=15.) 39 | 40 | 41 | def visualize_with_z_axis(): 42 | arm = tinyik.Actuator([ 43 | 'z', [0, 0, 180.7], 'y', [-612.7, 0, 0], 'y', [-571.55, 0, 0], 44 | 'y', [0, -174.15, 0], 'z', [0, 0, -119.85], 'y', [0, -116.55, 0]]) 45 | tinyik.visualize(arm, radius=10.) 46 | 47 | 48 | if __name__ == '__main__': 49 | visualize() 50 | visualize_with_target() 51 | large_visualize() 52 | large_visualize_with_target() 53 | visualize_with_z_axis() 54 | -------------------------------------------------------------------------------- /tests/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | x = (3. + (2. * np.sqrt(3.))) / 4. 5 | y = (2. + np.sqrt(3.)) / 4. 6 | z = .5 7 | 8 | theta = np.pi / 6 9 | 10 | 11 | def approx_eq(a, b): 12 | return all(np.round(a, 5) == np.round(b, 5)) 13 | -------------------------------------------------------------------------------- /tinyik/__init__.py: -------------------------------------------------------------------------------- 1 | """A simple and naive inverse kinematics solver.""" 2 | 3 | from .core import Actuator 4 | from .component import Link, Joint 5 | from .solver import FKSolver, IKSolver, CCDFKSolver, CCDIKSolver 6 | from .optimizer import ( 7 | NewtonOptimizer, SteepestDescentOptimizer, ConjugateGradientOptimizer, 8 | ScipyOptimizer, ScipySmoothOptimizer 9 | ) 10 | from .visualizer import visualize 11 | 12 | 13 | __all__ = ( 14 | 'Actuator', 15 | 'Link', 'Joint', 16 | 'FKSolver', 'IKSolver', 'CCDFKSolver', 'CCDIKSolver', 17 | 'NewtonOptimizer', 'SteepestDescentOptimizer', 18 | 'ConjugateGradientOptimizer', 19 | 'ScipyOptimizer', 'ScipySmoothOptimizer', 20 | 'visualize' 21 | ) 22 | -------------------------------------------------------------------------------- /tinyik/component.py: -------------------------------------------------------------------------------- 1 | """Components for an actuator.""" 2 | 3 | import autograd.numpy as np 4 | 5 | 6 | class Link(object): 7 | """Represents a link.""" 8 | 9 | def __init__(self, coord): 10 | """Create a link from a specified coordinate.""" 11 | self.coord = coord 12 | 13 | def matrix(self, _): 14 | """Return translation matrix in homogeneous coordinates.""" 15 | x, y, z = self.coord 16 | return np.array([ 17 | [1., 0., 0., x], 18 | [0., 1., 0., y], 19 | [0., 0., 1., z], 20 | [0., 0., 0., 1.] 21 | ]) 22 | 23 | 24 | class Joint(object): 25 | """Represents a revolute joint.""" 26 | 27 | def __init__(self, axis): 28 | """Create a revolute joint from a specified axis.""" 29 | self.axis = axis 30 | 31 | def matrix(self, angle): 32 | """Return rotation matrix in homogeneous coordinates.""" 33 | _rot_mat = { 34 | 'x': self._x_rot, 35 | 'y': self._y_rot, 36 | 'z': self._z_rot 37 | } 38 | return _rot_mat[self.axis](angle) 39 | 40 | def _x_rot(self, angle): 41 | return np.array([ 42 | [1., 0., 0., 0.], 43 | [0., np.cos(angle), -np.sin(angle), 0.], 44 | [0., np.sin(angle), np.cos(angle), 0.], 45 | [0., 0., 0., 1.] 46 | ]) 47 | 48 | def _y_rot(self, angle): 49 | return np.array([ 50 | [np.cos(angle), 0., np.sin(angle), 0.], 51 | [0., 1., 0., 0.], 52 | [-np.sin(angle), 0., np.cos(angle), 0.], 53 | [0., 0., 0., 1.] 54 | ]) 55 | 56 | def _z_rot(self, angle): 57 | return np.array([ 58 | [np.cos(angle), -np.sin(angle), 0., 0.], 59 | [np.sin(angle), np.cos(angle), 0., 0.], 60 | [0., 0., 1., 0.], 61 | [0., 0., 0., 1.] 62 | ]) 63 | -------------------------------------------------------------------------------- /tinyik/core.py: -------------------------------------------------------------------------------- 1 | """Core features.""" 2 | 3 | from numbers import Number 4 | 5 | import autograd.numpy as np 6 | 7 | from .component import Link, Joint 8 | from .solver import FKSolver, IKSolver 9 | from .optimizer import ScipyOptimizer 10 | 11 | 12 | class Actuator(object): 13 | """Represents an actuator as a set of links and revolute joints.""" 14 | 15 | def __init__(self, tokens, optimizer=None): 16 | """Create an actuator from specified link lengths and joint axes.""" 17 | components = [] 18 | for t in tokens: 19 | if isinstance(t, Number): 20 | components.append(Link([t, 0., 0.])) 21 | elif isinstance(t, list) or isinstance(t, np.ndarray): 22 | components.append(Link(t)) 23 | elif isinstance(t, str) and t in {'x', 'y', 'z'}: 24 | components.append(Joint(t)) 25 | else: 26 | raise ValueError( 27 | 'the arguments need to be ' 28 | 'link length or joint axis: {}'.format(t) 29 | ) 30 | 31 | self.fk = FKSolver(components) 32 | self.ik = IKSolver( 33 | self.fk, ScipyOptimizer() if optimizer is None else optimizer) 34 | 35 | self.angles = [0.] * len( 36 | [c for c in components if isinstance(c, Joint)] 37 | ) 38 | self.components = components 39 | 40 | @property 41 | def angles(self): 42 | """The joint angles.""" 43 | return self._angles 44 | 45 | @angles.setter 46 | def angles(self, angles): 47 | self._angles = np.array(angles) 48 | 49 | @property 50 | def ee(self): 51 | """The end-effector position.""" 52 | return self.fk.solve(self.angles) 53 | 54 | @ee.setter 55 | def ee(self, position): 56 | self.angles = self.ik.solve(self.angles, position) 57 | -------------------------------------------------------------------------------- /tinyik/optimizer.py: -------------------------------------------------------------------------------- 1 | """Optimizers.""" 2 | 3 | import autograd.numpy as np 4 | import autograd 5 | import scipy.optimize 6 | 7 | 8 | class NewtonOptimizer(object): 9 | """An optimizer based on Newton's method.""" 10 | 11 | def __init__(self, tol=1.48e-08, maxiter=50): 12 | """Generate an optimizer from an objective function.""" 13 | self.tol = tol 14 | self.maxiter = maxiter 15 | 16 | def prepare(self, f): 17 | """Accept an objective function for optimization.""" 18 | self.g = autograd.grad(f) 19 | self.h = autograd.hessian(f) 20 | 21 | def optimize(self, x0, target): 22 | """Calculate an optimum argument of an objective function.""" 23 | x = x0 24 | for _ in range(self.maxiter): 25 | delta = np.linalg.solve(self.h(x, target), -self.g(x, target)) 26 | x = x + delta 27 | if np.linalg.norm(delta) < self.tol: 28 | break 29 | return x 30 | 31 | 32 | class SteepestDescentOptimizer(object): 33 | """An optimizer based on steepest descent method.""" 34 | 35 | def __init__(self, tol=1.48e-08, maxiter=50, alpha=1): 36 | """Generate an optimizer from an objective function.""" 37 | self.tol = tol 38 | self.maxiter = maxiter 39 | self.alpha = alpha 40 | 41 | def prepare(self, f): 42 | """Accept an objective function for optimization.""" 43 | self.g = autograd.grad(f) 44 | 45 | def optimize(self, x0, target): 46 | """Calculate an optimum argument of an objective function.""" 47 | x = x0 48 | for _ in range(self.maxiter): 49 | delta = self.alpha * self.g(x, target) 50 | x = x - delta 51 | if np.linalg.norm(delta) < self.tol: 52 | break 53 | return x 54 | 55 | 56 | class ConjugateGradientOptimizer(object): 57 | """An optimizer based on conjugate gradient method.""" 58 | 59 | def __init__(self, tol=1.48e-08, maxiter=50): 60 | """Generate an optimizer from an objective function.""" 61 | self.tol = tol 62 | self.maxiter = maxiter 63 | 64 | def prepare(self, f): 65 | """Accept an objective function for optimization.""" 66 | self.g = autograd.grad(f) 67 | self.h = autograd.hessian(f) 68 | 69 | def optimize(self, x0, target): 70 | """Calculate an optimum argument of an objective function.""" 71 | x = x0 72 | for i in range(self.maxiter): 73 | g = self.g(x, target) 74 | h = self.h(x, target) 75 | if i == 0: 76 | alpha = 0 77 | m = g 78 | else: 79 | alpha = - np.dot(m, np.dot(h, g)) / np.dot(m, np.dot(h, m)) 80 | m = g + np.dot(alpha, m) 81 | t = - np.dot(m, g) / np.dot(m, np.dot(h, m)) 82 | delta = np.dot(t, m) 83 | x = x + delta 84 | if np.linalg.norm(delta) < self.tol: 85 | break 86 | return x 87 | 88 | 89 | class ScipyOptimizer(object): 90 | """An optimizer based on scipy.optimize.minimize.""" 91 | 92 | def __init__(self, **optimizer_opt): 93 | """Generate an optimizer from an objective function.""" 94 | for k, v in [ # default values 95 | ('method', 'BFGS'), 96 | ('tol', 1.48e-08), 97 | ('options', {'maxiter': 50})]: 98 | if k not in optimizer_opt: 99 | optimizer_opt[k] = v 100 | self.optimizer_opt = optimizer_opt 101 | 102 | def prepare(self, f): 103 | """Accept an objective function for optimization.""" 104 | self.f = f 105 | 106 | def optimize(self, angles0, target): 107 | """Calculate an optimum argument of an objective function.""" 108 | def new_objective(angles): 109 | return self.f(angles, target) 110 | 111 | return scipy.optimize.minimize( 112 | new_objective, 113 | angles0, 114 | **self.optimizer_opt).x 115 | 116 | 117 | class ScipySmoothOptimizer(ScipyOptimizer): 118 | """A smooth optimizer based on scipy.optimize.minimize.""" 119 | 120 | def __init__(self, smooth_factor=.1, **optimizer_opt): 121 | """Generate an optimizer from an objective function.""" 122 | self.smooth_factor = smooth_factor 123 | for k, v in [ # default values 124 | ('method', 'L-BFGS-B'), 125 | ('bounds', None)]: 126 | if k not in optimizer_opt: 127 | optimizer_opt[k] = v 128 | super(ScipySmoothOptimizer, self).__init__(**optimizer_opt) 129 | 130 | def optimize(self, angles0, target): 131 | """Calculate an optimum argument of an objective function.""" 132 | def new_objective(angles): 133 | a = angles - angles0 134 | if isinstance(self.smooth_factor, (np.ndarray, list)): 135 | if len(a) == len(self.smooth_factor): 136 | return (self.f(angles, target) + 137 | np.sum(self.smooth_factor * np.power(a, 2))) 138 | else: 139 | raise ValueError('len(smooth_factor) != number of joints') 140 | else: 141 | return (self.f(angles, target) + 142 | self.smooth_factor * np.sum(np.power(a, 2))) 143 | 144 | return scipy.optimize.minimize( 145 | new_objective, 146 | angles0, 147 | **self.optimizer_opt).x 148 | -------------------------------------------------------------------------------- /tinyik/solver.py: -------------------------------------------------------------------------------- 1 | """Solvers.""" 2 | 3 | from functools import reduce 4 | import sys 5 | 6 | import autograd.numpy as np 7 | 8 | from .component import Joint 9 | 10 | 11 | class FKSolver(object): 12 | """A forward kinematics solver.""" 13 | 14 | def __init__(self, components): 15 | """Generate a FK solver from link and joint instances.""" 16 | joint_indexes = [ 17 | i for i, c in enumerate(components) if isinstance(c, Joint) 18 | ] 19 | 20 | def matrices(angles): 21 | joints = dict(zip(joint_indexes, angles)) 22 | a = [joints.get(i, None) for i in range(len(components))] 23 | return [c.matrix(a[i]) for i, c in enumerate(components)] 24 | 25 | self._matrices = matrices 26 | 27 | def solve(self, angles): 28 | """Calculate a position of the end-effector and return it.""" 29 | return reduce( 30 | lambda a, m: np.dot(m, a), 31 | reversed(self._matrices(angles)), 32 | np.array([0., 0., 0., 1.]) 33 | )[:3] 34 | 35 | 36 | class IKSolver(object): 37 | """An inverse kinematics solver.""" 38 | 39 | def __init__(self, fk_solver, optimizer): 40 | """Generate an IK solver from a FK solver instance.""" 41 | def distance_squared(angles, target): 42 | x = target - fk_solver.solve(angles) 43 | return np.sum(np.power(x, 2)) 44 | 45 | optimizer.prepare(distance_squared) 46 | self.optimizer = optimizer 47 | 48 | def solve(self, angles0, target): 49 | """Calculate joint angles and returns it.""" 50 | return self.optimizer.optimize(np.array(angles0), target) 51 | 52 | 53 | class CCDFKSolver(object): 54 | 55 | def __init__(self, components): 56 | joint_indexes = [ 57 | i for i, c in enumerate(components) if isinstance(c, Joint) 58 | ] 59 | 60 | def matrices(angles): 61 | joints = dict(zip(joint_indexes, angles)) 62 | a = [joints.get(i, None) for i in range(len(components))] 63 | return [c.matrix(a[i]) for i, c in enumerate(components)] 64 | 65 | self._matrices = matrices 66 | self.components = components 67 | self.joint_indexes = joint_indexes 68 | 69 | def solve(self, angles, p=None, index=None): 70 | if p is None: 71 | p = [0., 0., 0., 1.] 72 | if index is None: 73 | index = len(self.components) - 1 74 | return reduce( 75 | lambda a, m: np.dot(m, a), 76 | reversed(self._matrices(angles)[:index + 1]), 77 | np.array(p) 78 | )[:3] 79 | 80 | 81 | class CCDIKSolver(object): 82 | 83 | def __init__(self, fk_solver, tol=1.48e-08, maxiter=50): 84 | self._fk_solver = fk_solver 85 | self.tol = tol 86 | self.maxiter = maxiter 87 | 88 | def solve(self, angles0, target): 89 | joint_indexes = list(reversed(self._fk_solver.joint_indexes)) 90 | angles = angles0[:] 91 | pmap = { 92 | 'x': [1., 0., 0., 0.], 93 | 'y': [0., 1., 0., 0.], 94 | 'z': [0., 0., 1., 0.]} 95 | prev_dist = sys.float_info.max 96 | for _ in range(self.maxiter): 97 | for i, idx in enumerate(joint_indexes): 98 | axis = self._fk_solver.solve( 99 | angles, 100 | p=pmap[self._fk_solver.components[idx].axis], 101 | index=idx) 102 | pj = self._fk_solver.solve(angles, index=idx) 103 | ee = self._fk_solver.solve(angles) 104 | eorp = self.p_on_rot_plane(ee, pj, axis) 105 | torp = self.p_on_rot_plane(target, pj, axis) 106 | ve = (eorp - pj) / np.linalg.norm(eorp - pj) 107 | vt = (torp - pj) / np.linalg.norm(torp - pj) 108 | a = np.arccos(np.dot(vt, ve)) 109 | sign = 1 if np.dot(axis, np.cross(ve, vt)) > 0 else -1 110 | angles[-(i + 1)] += (a * sign) 111 | 112 | ee = self._fk_solver.solve(angles) 113 | dist = np.linalg.norm(target - ee) 114 | delta = prev_dist - dist 115 | if delta < self.tol: 116 | break 117 | prev_dist = dist 118 | 119 | return angles 120 | 121 | def p_on_rot_plane(self, p, joint_pos, joint_axis): 122 | ua = joint_axis / np.linalg.norm(joint_axis) 123 | return p - (np.dot(p - joint_pos, ua) * ua) 124 | -------------------------------------------------------------------------------- /tinyik/visualizer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | try: 4 | import open3d as o3d # the extra feature 5 | except ImportError: 6 | pass 7 | 8 | 9 | def translate(p): 10 | x, y, z = p 11 | return np.array([ 12 | [1., 0., 0., x], 13 | [0., 1., 0., y], 14 | [0., 0., 1., z], 15 | [0., 0., 0., 1.] 16 | ]) 17 | 18 | 19 | def rotate(axis, angle): 20 | x, y, z = axis 21 | return np.array([ # Rodrigues 22 | [ 23 | np.cos(angle) + (x**2 * (1 - np.cos(angle))), 24 | (x * y * (1 - np.cos(angle))) - (z * np.sin(angle)), 25 | (x * z * (1 - np.cos(angle))) + (y * np.sin(angle)), 26 | 0. 27 | ], [ 28 | (y * x * (1 - np.cos(angle))) + (z * np.sin(angle)), 29 | np.cos(angle) + (y**2 * (1 - np.cos(angle))), 30 | (y * z * (1 - np.cos(angle))) - (x * np.sin(angle)), 31 | 0. 32 | ], [ 33 | (z * x * (1 - np.cos(angle))) - (y * np.sin(angle)), 34 | (z * y * (1 - np.cos(angle))) + (x * np.sin(angle)), 35 | np.cos(angle) + (z**2 * (1 - np.cos(angle))), 36 | 0. 37 | ], [0., 0., 0., 1.] 38 | ]) 39 | 40 | 41 | def create_sphere(p, radius, color=None): 42 | if color is None: 43 | color = [.8, .8, .8] 44 | geo = o3d.geometry.TriangleMesh.create_sphere(radius=radius) 45 | geo.compute_vertex_normals() 46 | geo.paint_uniform_color(color) 47 | geo.transform(translate(p)) 48 | return geo 49 | 50 | 51 | class GeoComponent: 52 | 53 | child = None 54 | radius = .1 55 | 56 | def tip(self, link_color=None): 57 | return create_sphere( 58 | [0., 0., 0.], 59 | radius=self.radius*2, 60 | color=[.8, .8, .8] if link_color is None else link_color) 61 | 62 | def geo(self, mat=None, link_color=None): 63 | geo = self.base_geo(link_color) 64 | if mat is not None: 65 | geo.transform(mat) 66 | mat = mat @ self.mat() 67 | else: 68 | mat = self.mat() 69 | if self.child is None: 70 | return [geo] + [self.tip(link_color).transform(mat)] 71 | else: 72 | return [geo] + self.child.geo(mat, link_color) 73 | 74 | 75 | class Link(GeoComponent): 76 | 77 | def __init__(self, c, radius): 78 | self.c = c 79 | self.radius = radius 80 | 81 | def base_geo(self, link_color=None): 82 | norm = np.linalg.norm(self.c.coord) 83 | 84 | geo = o3d.geometry.TriangleMesh.create_cylinder( 85 | radius=self.radius, height=norm) 86 | geo.compute_vertex_normals() 87 | geo.paint_uniform_color( 88 | [.8, .8, .8] if link_color is None else link_color) 89 | 90 | # Calculate transformation matrix for cylinder 91 | # With help from https://stackoverflow.com/a/59829173 92 | def get_cross_prod_mat(vector): 93 | return np.array([ 94 | [0, -vector[2], vector[1]], 95 | [vector[2], 0, -vector[0]], 96 | [-vector[1], vector[0], 0], 97 | ]) 98 | cylinder_dir_unit_vector = self.c.coord / norm 99 | 100 | # Unit vector for "up" direction 101 | z_unit_vector = np.array([0, 0, 1]) 102 | z_rotation_mat = get_cross_prod_mat(z_unit_vector) 103 | 104 | z_c_vec = np.matmul(z_rotation_mat, cylinder_dir_unit_vector) 105 | z_c_vec_mat = get_cross_prod_mat(z_c_vec) 106 | 107 | # Added np.abs to ensure that unit vector that is aligned with any 108 | # axis in the negative direction does not 109 | rotation_mat = np.eye(3, 3) + z_c_vec_mat + np.matmul( 110 | z_c_vec_mat, z_c_vec_mat)/( 111 | (1 + np.abs(np.dot(z_unit_vector, cylinder_dir_unit_vector)))) 112 | 113 | cylinder_transform_mat = np.vstack((np.hstack( 114 | (rotation_mat, np.transpose( 115 | np.array([self.c.coord])/2))), np.array([0, 0, 0, 1]))) 116 | 117 | geo.transform(cylinder_transform_mat) 118 | return geo 119 | 120 | def mat(self): 121 | return self.c.matrix(None) 122 | 123 | 124 | class Joint(GeoComponent): 125 | 126 | def __init__(self, c, radius): 127 | self.c = c 128 | self.radius = radius 129 | self.angle = 0. 130 | 131 | def base_geo(self, _=None): 132 | geo = o3d.geometry.TriangleMesh.create_cylinder( 133 | radius=self.radius*2, height=self.radius*4) 134 | geo.compute_vertex_normals() 135 | geo.paint_uniform_color([.2, .2, .9]) 136 | rx = { 137 | 'x': [0., 1., 0.], 138 | 'y': [1., 0., 0.], 139 | 'z': [0., 0., 1.], 140 | } 141 | geo.transform(rotate(rx[self.c.axis], np.pi / 2)) 142 | return geo 143 | 144 | def mat(self): 145 | return self.c.matrix(self.angle) 146 | 147 | 148 | def build_geos(actuator, target=None, radius=.05): 149 | root = None 150 | p = None 151 | joints = [] 152 | for c in actuator.components: 153 | if hasattr(c, 'axis'): 154 | gc = Joint(c, radius) 155 | joints.append(gc) 156 | else: 157 | gc = Link(c, radius) 158 | 159 | if root is None: 160 | root = gc 161 | p = gc 162 | else: 163 | p.child = gc 164 | p = gc 165 | 166 | for j, a in zip(joints, actuator.angles): 167 | j.angle = a 168 | 169 | if target: 170 | geos = root.geo(link_color=[.5, .5, .5]) 171 | actuator.ee = target 172 | for j, a in zip(joints, actuator.angles): 173 | j.angle = a 174 | geos += root.geo() 175 | geos += [create_sphere(target, radius=radius*2.4, color=[.8, .2, .2])] 176 | else: 177 | geos = root.geo() 178 | 179 | return geos 180 | 181 | 182 | def visualize(actuator, target=None, radius=.05): 183 | geos = build_geos(actuator, target, radius) 184 | o3d.visualization.draw_geometries( 185 | geos, window_name='tinyik vizualizer', width=640, height=480) 186 | --------------------------------------------------------------------------------