From af85082a487ba3d13c9313c6bb80cd397f0ac94a Mon Sep 17 00:00:00 2001 From: Daniel Date: Sun, 12 May 2024 12:57:42 +0200 Subject: [PATCH 01/20] Add inverse kinematics implementation --- poetry.lock | 120 ++++++++++++++++++++++++++++++++++++++----- pyproject.toml | 1 + src/ktree/k_types.py | 30 +++++++++++ src/ktree/ktree.py | 84 +++++++++++++++++++++++++++--- test/test_types.py | 74 +++++++++++++++++++++++++- 5 files changed, 288 insertions(+), 21 deletions(-) diff --git a/poetry.lock b/poetry.lock index 6989a6b..1ebfa96 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1,4 +1,4 @@ -# This file is automatically @generated by Poetry 1.7.1 and should not be changed by hand. +# This file is automatically @generated by Poetry 1.5.1 and should not be changed by hand. [[package]] name = "annotated-types" @@ -479,6 +479,75 @@ files = [ {file = "packaging-23.2.tar.gz", hash = "sha256:048fb0e9405036518eaaf48a55953c750c11e1a1b68e0dd1a9d62ed0c092cfc5"}, ] +[[package]] +name = "pandas" +version = "2.2.2" +description = "Powerful data structures for data analysis, time series, and statistics" +optional = false +python-versions = ">=3.9" +files = [ + {file = "pandas-2.2.2-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:90c6fca2acf139569e74e8781709dccb6fe25940488755716d1d354d6bc58bce"}, + {file = "pandas-2.2.2-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:c7adfc142dac335d8c1e0dcbd37eb8617eac386596eb9e1a1b77791cf2498238"}, + {file = "pandas-2.2.2-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:4abfe0be0d7221be4f12552995e58723c7422c80a659da13ca382697de830c08"}, + {file = "pandas-2.2.2-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8635c16bf3d99040fdf3ca3db669a7250ddf49c55dc4aa8fe0ae0fa8d6dcc1f0"}, + {file = "pandas-2.2.2-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:40ae1dffb3967a52203105a077415a86044a2bea011b5f321c6aa64b379a3f51"}, + {file = "pandas-2.2.2-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:8e5a0b00e1e56a842f922e7fae8ae4077aee4af0acb5ae3622bd4b4c30aedf99"}, + {file = "pandas-2.2.2-cp310-cp310-win_amd64.whl", hash = "sha256:ddf818e4e6c7c6f4f7c8a12709696d193976b591cc7dc50588d3d1a6b5dc8772"}, + {file = "pandas-2.2.2-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:696039430f7a562b74fa45f540aca068ea85fa34c244d0deee539cb6d70aa288"}, + {file = "pandas-2.2.2-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:8e90497254aacacbc4ea6ae5e7a8cd75629d6ad2b30025a4a8b09aa4faf55151"}, + {file = "pandas-2.2.2-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:58b84b91b0b9f4bafac2a0ac55002280c094dfc6402402332c0913a59654ab2b"}, + {file = "pandas-2.2.2-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6d2123dc9ad6a814bcdea0f099885276b31b24f7edf40f6cdbc0912672e22eee"}, + {file = "pandas-2.2.2-cp311-cp311-musllinux_1_1_aarch64.whl", hash = "sha256:2925720037f06e89af896c70bca73459d7e6a4be96f9de79e2d440bd499fe0db"}, + {file = "pandas-2.2.2-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:0cace394b6ea70c01ca1595f839cf193df35d1575986e484ad35c4aeae7266c1"}, + {file = "pandas-2.2.2-cp311-cp311-win_amd64.whl", hash = "sha256:873d13d177501a28b2756375d59816c365e42ed8417b41665f346289adc68d24"}, + {file = "pandas-2.2.2-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:9dfde2a0ddef507a631dc9dc4af6a9489d5e2e740e226ad426a05cabfbd7c8ef"}, + {file = "pandas-2.2.2-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:e9b79011ff7a0f4b1d6da6a61aa1aa604fb312d6647de5bad20013682d1429ce"}, + {file = "pandas-2.2.2-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1cb51fe389360f3b5a4d57dbd2848a5f033350336ca3b340d1c53a1fad33bcad"}, + {file = "pandas-2.2.2-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:eee3a87076c0756de40b05c5e9a6069c035ba43e8dd71c379e68cab2c20f16ad"}, + {file = "pandas-2.2.2-cp312-cp312-musllinux_1_1_aarch64.whl", hash = "sha256:3e374f59e440d4ab45ca2fffde54b81ac3834cf5ae2cdfa69c90bc03bde04d76"}, + {file = "pandas-2.2.2-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:43498c0bdb43d55cb162cdc8c06fac328ccb5d2eabe3cadeb3529ae6f0517c32"}, + {file = "pandas-2.2.2-cp312-cp312-win_amd64.whl", hash = "sha256:d187d355ecec3629624fccb01d104da7d7f391db0311145817525281e2804d23"}, + {file = "pandas-2.2.2-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:0ca6377b8fca51815f382bd0b697a0814c8bda55115678cbc94c30aacbb6eff2"}, + {file = "pandas-2.2.2-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:9057e6aa78a584bc93a13f0a9bf7e753a5e9770a30b4d758b8d5f2a62a9433cd"}, + {file = "pandas-2.2.2-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:001910ad31abc7bf06f49dcc903755d2f7f3a9186c0c040b827e522e9cef0863"}, + {file = "pandas-2.2.2-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:66b479b0bd07204e37583c191535505410daa8df638fd8e75ae1b383851fe921"}, + {file = "pandas-2.2.2-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:a77e9d1c386196879aa5eb712e77461aaee433e54c68cf253053a73b7e49c33a"}, + {file = "pandas-2.2.2-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:92fd6b027924a7e178ac202cfbe25e53368db90d56872d20ffae94b96c7acc57"}, + {file = "pandas-2.2.2-cp39-cp39-win_amd64.whl", hash = "sha256:640cef9aa381b60e296db324337a554aeeb883ead99dc8f6c18e81a93942f5f4"}, + {file = "pandas-2.2.2.tar.gz", hash = "sha256:9e79019aba43cb4fda9e4d983f8e88ca0373adbb697ae9c6c43093218de28b54"}, +] + +[package.dependencies] +numpy = {version = ">=1.22.4", markers = "python_version < \"3.11\""} +python-dateutil = ">=2.8.2" +pytz = ">=2020.1" +tzdata = ">=2022.7" + +[package.extras] +all = ["PyQt5 (>=5.15.9)", "SQLAlchemy (>=2.0.0)", "adbc-driver-postgresql (>=0.8.0)", "adbc-driver-sqlite (>=0.8.0)", "beautifulsoup4 (>=4.11.2)", "bottleneck (>=1.3.6)", "dataframe-api-compat (>=0.1.7)", "fastparquet (>=2022.12.0)", "fsspec (>=2022.11.0)", "gcsfs (>=2022.11.0)", "html5lib (>=1.1)", "hypothesis (>=6.46.1)", "jinja2 (>=3.1.2)", "lxml (>=4.9.2)", "matplotlib (>=3.6.3)", "numba (>=0.56.4)", "numexpr (>=2.8.4)", "odfpy (>=1.4.1)", "openpyxl (>=3.1.0)", "pandas-gbq (>=0.19.0)", "psycopg2 (>=2.9.6)", "pyarrow (>=10.0.1)", "pymysql (>=1.0.2)", "pyreadstat (>=1.2.0)", "pytest (>=7.3.2)", "pytest-xdist (>=2.2.0)", "python-calamine (>=0.1.7)", "pyxlsb (>=1.0.10)", "qtpy (>=2.3.0)", "s3fs (>=2022.11.0)", "scipy (>=1.10.0)", "tables (>=3.8.0)", "tabulate (>=0.9.0)", "xarray (>=2022.12.0)", "xlrd (>=2.0.1)", "xlsxwriter (>=3.0.5)", "zstandard (>=0.19.0)"] +aws = ["s3fs (>=2022.11.0)"] +clipboard = ["PyQt5 (>=5.15.9)", "qtpy (>=2.3.0)"] +compression = ["zstandard (>=0.19.0)"] +computation = ["scipy (>=1.10.0)", "xarray (>=2022.12.0)"] +consortium-standard = ["dataframe-api-compat (>=0.1.7)"] +excel = ["odfpy (>=1.4.1)", "openpyxl (>=3.1.0)", "python-calamine (>=0.1.7)", "pyxlsb (>=1.0.10)", "xlrd (>=2.0.1)", "xlsxwriter (>=3.0.5)"] +feather = ["pyarrow (>=10.0.1)"] +fss = ["fsspec (>=2022.11.0)"] +gcp = ["gcsfs (>=2022.11.0)", "pandas-gbq (>=0.19.0)"] +hdf5 = ["tables (>=3.8.0)"] +html = ["beautifulsoup4 (>=4.11.2)", "html5lib (>=1.1)", "lxml (>=4.9.2)"] +mysql = ["SQLAlchemy (>=2.0.0)", "pymysql (>=1.0.2)"] +output-formatting = ["jinja2 (>=3.1.2)", "tabulate (>=0.9.0)"] +parquet = ["pyarrow (>=10.0.1)"] +performance = ["bottleneck (>=1.3.6)", "numba (>=0.56.4)", "numexpr (>=2.8.4)"] +plot = ["matplotlib (>=3.6.3)"] +postgresql = ["SQLAlchemy (>=2.0.0)", "adbc-driver-postgresql (>=0.8.0)", "psycopg2 (>=2.9.6)"] +pyarrow = ["pyarrow (>=10.0.1)"] +spss = ["pyreadstat (>=1.2.0)"] +sql-other = ["SQLAlchemy (>=2.0.0)", "adbc-driver-postgresql (>=0.8.0)", "adbc-driver-sqlite (>=0.8.0)"] +test = ["hypothesis (>=6.46.1)", "pytest (>=7.3.2)", "pytest-xdist (>=2.2.0)"] +xml = ["lxml (>=4.9.2)"] + [[package]] name = "parso" version = "0.8.3" @@ -798,6 +867,31 @@ files = [ loguru = "*" pytest = "*" +[[package]] +name = "python-dateutil" +version = "2.9.0.post0" +description = "Extensions to the standard Python datetime module" +optional = false +python-versions = "!=3.0.*,!=3.1.*,!=3.2.*,>=2.7" +files = [ + {file = "python-dateutil-2.9.0.post0.tar.gz", hash = "sha256:37dd54208da7e1cd875388217d5e00ebd4179249f90fb72437e91a35459a0ad3"}, + {file = "python_dateutil-2.9.0.post0-py2.py3-none-any.whl", hash = "sha256:a8b2bc7bffae282281c8140a97d3aa9c14da0b136dfe83f850eea9a5f7470427"}, +] + +[package.dependencies] +six = ">=1.5" + +[[package]] +name = "pytz" +version = "2024.1" +description = "World timezone definitions, modern and historical" +optional = false +python-versions = "*" +files = [ + {file = "pytz-2024.1-py2.py3-none-any.whl", hash = "sha256:328171f4e3623139da4983451950b28e95ac706e13f3f2630a879749e7a8b319"}, + {file = "pytz-2024.1.tar.gz", hash = "sha256:2a29735ea9c18baf14b448846bde5a48030ed267578472d8955cd0e7443a9812"}, +] + [[package]] name = "pyyaml" version = "6.0.1" @@ -810,7 +904,6 @@ files = [ {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:69b023b2b4daa7548bcfbd4aa3da05b3a74b772db9e23b982788168117739938"}, {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:81e0b275a9ecc9c0c0c07b4b90ba548307583c125f54d5b6946cfee6360c733d"}, {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ba336e390cd8e4d1739f42dfe9bb83a3cc2e80f567d8805e11b46f4a943f5515"}, - {file = "PyYAML-6.0.1-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:326c013efe8048858a6d312ddd31d56e468118ad4cdeda36c719bf5bb6192290"}, {file = "PyYAML-6.0.1-cp310-cp310-win32.whl", hash = "sha256:bd4af7373a854424dabd882decdc5579653d7868b8fb26dc7d0e99f823aa5924"}, {file = "PyYAML-6.0.1-cp310-cp310-win_amd64.whl", hash = "sha256:fd1592b3fdf65fff2ad0004b5e363300ef59ced41c2e6b3a99d4089fa8c5435d"}, {file = "PyYAML-6.0.1-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:6965a7bc3cf88e5a1c3bd2e0b5c22f8d677dc88a455344035f03399034eb3007"}, @@ -818,16 +911,8 @@ files = [ {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:42f8152b8dbc4fe7d96729ec2b99c7097d656dc1213a3229ca5383f973a5ed6d"}, {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:062582fca9fabdd2c8b54a3ef1c978d786e0f6b3a1510e0ac93ef59e0ddae2bc"}, {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d2b04aac4d386b172d5b9692e2d2da8de7bfb6c387fa4f801fbf6fb2e6ba4673"}, - {file = "PyYAML-6.0.1-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:e7d73685e87afe9f3b36c799222440d6cf362062f78be1013661b00c5c6f678b"}, {file = "PyYAML-6.0.1-cp311-cp311-win32.whl", hash = "sha256:1635fd110e8d85d55237ab316b5b011de701ea0f29d07611174a1b42f1444741"}, {file = "PyYAML-6.0.1-cp311-cp311-win_amd64.whl", hash = "sha256:bf07ee2fef7014951eeb99f56f39c9bb4af143d8aa3c21b1677805985307da34"}, - {file = "PyYAML-6.0.1-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:855fb52b0dc35af121542a76b9a84f8d1cd886ea97c84703eaa6d88e37a2ad28"}, - {file = "PyYAML-6.0.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:40df9b996c2b73138957fe23a16a4f0ba614f4c0efce1e9406a184b6d07fa3a9"}, - {file = "PyYAML-6.0.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a08c6f0fe150303c1c6b71ebcd7213c2858041a7e01975da3a99aed1e7a378ef"}, - {file = "PyYAML-6.0.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6c22bec3fbe2524cde73d7ada88f6566758a8f7227bfbf93a408a9d86bcc12a0"}, - {file = "PyYAML-6.0.1-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:8d4e9c88387b0f5c7d5f281e55304de64cf7f9c0021a3525bd3b1c542da3b0e4"}, - {file = "PyYAML-6.0.1-cp312-cp312-win32.whl", hash = "sha256:d483d2cdf104e7c9fa60c544d92981f12ad66a457afae824d146093b8c294c54"}, - {file = "PyYAML-6.0.1-cp312-cp312-win_amd64.whl", hash = "sha256:0d3304d8c0adc42be59c5f8a4d9e3d7379e6955ad754aa9d6ab7a398b59dd1df"}, {file = "PyYAML-6.0.1-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:50550eb667afee136e9a77d6dc71ae76a44df8b3e51e41b77f6de2932bfe0f47"}, {file = "PyYAML-6.0.1-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1fe35611261b29bd1de0070f0b2f47cb6ff71fa6595c077e42bd0c419fa27b98"}, {file = "PyYAML-6.0.1-cp36-cp36m-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:704219a11b772aea0d8ecd7058d0082713c3562b4e271b849ad7dc4a5c90c13c"}, @@ -844,7 +929,6 @@ files = [ {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a0cd17c15d3bb3fa06978b4e8958dcdc6e0174ccea823003a106c7d4d7899ac5"}, {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:28c119d996beec18c05208a8bd78cbe4007878c6dd15091efb73a30e90539696"}, {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:7e07cbde391ba96ab58e532ff4803f79c4129397514e1413a7dc761ccd755735"}, - {file = "PyYAML-6.0.1-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:49a183be227561de579b4a36efbb21b3eab9651dd81b1858589f796549873dd6"}, {file = "PyYAML-6.0.1-cp38-cp38-win32.whl", hash = "sha256:184c5108a2aca3c5b3d3bf9395d50893a7ab82a38004c8f61c258d4428e80206"}, {file = "PyYAML-6.0.1-cp38-cp38-win_amd64.whl", hash = "sha256:1e2722cc9fbb45d9b87631ac70924c11d3a401b2d7f410cc0e3bbf249f2dca62"}, {file = "PyYAML-6.0.1-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:9eb6caa9a297fc2c2fb8862bc5370d0303ddba53ba97e71f08023b6cd73d16a8"}, @@ -852,7 +936,6 @@ files = [ {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:5773183b6446b2c99bb77e77595dd486303b4faab2b086e7b17bc6bef28865f6"}, {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:b786eecbdf8499b9ca1d697215862083bd6d2a99965554781d0d8d1ad31e13a0"}, {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:bc1bf2925a1ecd43da378f4db9e4f799775d6367bdb94671027b73b393a7c42c"}, - {file = "PyYAML-6.0.1-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:04ac92ad1925b2cff1db0cfebffb6ffc43457495c9b3c39d3fcae417d7125dc5"}, {file = "PyYAML-6.0.1-cp39-cp39-win32.whl", hash = "sha256:faca3bdcf85b2fc05d06ff3fbc1f83e1391b3e724afa3feba7d13eeab355484c"}, {file = "PyYAML-6.0.1-cp39-cp39-win_amd64.whl", hash = "sha256:510c9deebc5c0225e8c96813043e62b680ba2f9c50a08d3724c7f28a747d1486"}, {file = "PyYAML-6.0.1.tar.gz", hash = "sha256:bfdf460b1736c775f2ba9f6a92bca30bc2095067b8a9d77876d1fad6cc3b4a43"}, @@ -978,6 +1061,17 @@ files = [ {file = "typing_extensions-4.8.0.tar.gz", hash = "sha256:df8e4339e9cb77357558cbdbceca33c303714cf861d1eef15e1070055ae8b7ef"}, ] +[[package]] +name = "tzdata" +version = "2024.1" +description = "Provider of IANA time zone data" +optional = false +python-versions = ">=2" +files = [ + {file = "tzdata-2024.1-py2.py3-none-any.whl", hash = "sha256:9068bc196136463f5245e51efda838afa15aaeca9903f49050dfa2679db4d252"}, + {file = "tzdata-2024.1.tar.gz", hash = "sha256:2674120f8d891909751c38abcdfd386ac0a5a1127954fbc332af6b5ceae07efd"}, +] + [[package]] name = "wcwidth" version = "0.2.9" @@ -1006,4 +1100,4 @@ dev = ["black (>=19.3b0)", "pytest (>=4.6.2)"] [metadata] lock-version = "2.0" python-versions = ">=3.10,<3.11" -content-hash = "b8d43a3c96e324091972c12f4812b686ca0dc3f57d881874307dfc5f8b36c1c5" +content-hash = "d1e0fc459941b2caf2b803df8e6e2427cb4cc71dacec1d55aee713413e7c6625" diff --git a/pyproject.toml b/pyproject.toml index 4f6cd20..e815821 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -15,6 +15,7 @@ types-pyyaml = "^6.0.12.11" pydantic = "^2.3.0" ipython = "^8.14.0" networkx = "^3.1" +pandas = "^2.2.2" [[tool.poetry.source]] name = "rocsys" diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index d160157..2bb158c 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -1,5 +1,6 @@ import numpy as np from enum import Enum +from loguru import logger from numpy.typing import NDArray from pydantic import ( BaseModel, @@ -272,12 +273,15 @@ class Joint(BaseModel): axis: JointAxis | None = Field( default=None, description="If `type` is other than FIXED, axis of rotation or translation (x, y or z)" ) + value: float | None = Field(default=None, description="Value of the joint in SI Units (meters or radians)") @model_validator(mode="after") # type: ignore[misc] def _axis_validator(self) -> "Joint": match self.type: case JointType.FIXED | JointType.SPATIAL: self.axis = None + self.value = None + return self @computed_field # type: ignore[misc] @@ -318,6 +322,27 @@ class Transformation(BaseModel): ) joint: Joint = Field(default=Joint(), description="Joint connecting parent and child") + @model_validator(mode="after") # type: ignore[misc] + def _joint_validator(self) -> "Transformation": + match self.joint.type: + case JointType.REVOLUTE: + match self.joint.axis: + case JointAxis.X: + self.pose.rotation.rx = self.joint.value + case JointAxis.Y: + self.pose.rotation.ry = self.joint.value + case JointAxis.Z: + self.pose.rotation.rz = self.joint.value + case JointType.PRISMATIC: + match self.joint.axis: + case JointAxis.X: + self.pose.translation.x = self.joint.value + case JointAxis.Y: + self.pose.translation.y = self.joint.value + case JointAxis.Z: + self.pose.translation.z = self.joint.value + return self + @field_validator("pose", mode="before") @classmethod def _pose_validator(cls, v: Pose | NDArray[np.float_] | list[float] | dict[str, float]) -> Pose: @@ -378,6 +403,11 @@ def inv(self) -> "Transformation": pose=Pose(translation=Vector(vector=new_pos), rotation=new_rot), parent=self.child, child=self.parent, + joint=Joint( + type=self.joint.type, + axis=self.joint.axis, + value=None if self.joint.value is None else -self.joint.value, + ), ) def reset_rotation(self) -> "Transformation": diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 54c5b00..1d47c55 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -1,5 +1,6 @@ import networkx as nx import numpy as np +import pandas as pd from ktree.k_types import JointType, Pose, Transformation, Vector from ktree.models import KinematicsConfig from loguru import logger @@ -16,8 +17,8 @@ def model_post_init(self, __context: Any) -> None: logger.info("Initializing kinematic chain") """Create kinematic chain based on parsed configuration""" self._k_chain = nx.DiGraph() - self._joints = [t for t in self.config.transformations if t.joint.type != JointType.FIXED] - self._n_actuated_joints = len(self._joints) + self._actuated_joints = [t for t in self.config.transformations if t.joint.type != JointType.FIXED] + self._n_actuated_joints = len(self._actuated_joints) logger.debug("Kinematic chain nodes".upper()) for transformation in self.config.transformations: @@ -106,6 +107,9 @@ def update_transformation( def get_transformation(self, parent: str, child: str) -> Transformation: """Get pose of child relative to parent""" + if parent == child: + return Transformation(parent=parent, child=child) + sp = cast(list, nx.shortest_path(self._k_chain, source=parent, target=child)) all_paths = list(nx.all_simple_paths(self._k_chain, source=parent, target=child)) @@ -131,8 +135,8 @@ def _get_jacobian(self) -> NDArray: end_effector = self.get_transformation(parent=self.config.base, child=self.config.end_effector) - for col_j, joint_j in enumerate(self._joints): - joint_wt = self.get_transformation(parent=self.config.base, child=joint_j.parent) + for col_j, joint_j in enumerate(self._actuated_joints): + joint_in_world = self.get_transformation(parent=self.config.base, child=joint_j.child) if joint_j.joint.vector is None: raise ValueError( f"Actuated Joint transformation {joint_j.parent.upper()} - {joint_j.child.upper()} has no axis." @@ -140,10 +144,12 @@ def _get_jacobian(self) -> NDArray: ) match joint_j.joint.type: case JointType.PRISMATIC: - jacobian[:3, col_j] = joint_wt.pose.rotation * joint_j.joint.vector + jacobian[:3, col_j] = joint_in_world.pose.rotation * joint_j.joint.vector case JointType.REVOLUTE: - a_i = cast(Vector, joint_wt.pose.rotation * joint_j.joint.vector) - jacobian[:3, col_j] = (a_i @ (end_effector.pose.translation - joint_wt.pose.translation)).vector + a_i = cast(Vector, joint_in_world.pose.rotation * joint_j.joint.vector) + jacobian[:3, col_j] = ( + a_i @ (end_effector.pose.translation - joint_in_world.pose.translation) + ).vector jacobian[3:, col_j] = a_i.vector case JointType.FIXED: ValueError(f"Joint type {joint_j.joint.type} not accepted") @@ -171,6 +177,70 @@ def _get_jacobian(self) -> NDArray: return jacobian_rpy @ jacobian + def update_joints_from_list(self, joint_values: NDArray | list[float], mm_deg: bool = False) -> None: + """Update joint values""" + if isinstance(joint_values, list): + joint_values = np.array(joint_values) + + if joint_values.shape != (self._n_actuated_joints,): + raise ValueError( + f"Invalid joint values shape. Expected ({self._n_actuated_joints},) got {joint_values.shape}" + ) + + for transformation, joint_value in zip(self._actuated_joints, joint_values): + if transformation.joint.type == JointType.REVOLUTE: + transformation.joint.value = np.radians(joint_value) if mm_deg else joint_value + elif transformation.joint.type == JointType.PRISMATIC: + transformation.joint.value = 0.001 * joint_value if mm_deg else joint_value + self.update_transformation(transformation=transformation) + + def get_joint_values(self) -> NDArray: + """Get joint values""" + return np.array([t.joint.value for t in self._actuated_joints]) + + def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: + """Inverse kinematics using Jacobian""" + target_effector.child = "target_effector" + start_pose = self.get_transformation(parent=self.config.base, child=self.config.end_effector) + # delta_pose = start_pose.inv() * target_effector + delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) + pose_tol = np.array([1e-6] * 6) + ITERATIONS = 1000 + iter = 0 + dx = delta_pose / 100 + iterations = [] + while True: + if all(abs(dx) < pose_tol): + break + if iter > ITERATIONS: + break + dq = np.linalg.pinv(self._get_jacobian()) @ dx + self.update_joints_from_list(self.get_joint_values() + dq) + current_effector = self.get_transformation(parent=self.config.base, child=self.config.end_effector) + iterations.append(self._iteration_row()) + logger.debug(current_effector) + # logger.debug(current_effector.inv() * target_effector) + dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 100 + logger.debug(dx) + + iter += 1 + + logger.info(f"Converged in {iter} iterations") + index = pd.MultiIndex.from_tuples( + [ + (f"{self.config.base}_in_{transformation.child}", coordinate) + for transformation in self.config.transformations + for coordinate in ["x", "y", "z", "rx", "ry", "rz"] + ] + ) + return pd.DataFrame(np.array(iterations).reshape(-1, len(self.config.transformations) * 6), columns=index) + + def _iteration_row(self) -> list: + return [ + self.get_transformation(parent=self.config.base, child=transformation.child).pose.to_list() + for transformation in self.config.transformations + ] + def _add_transformation(self, transformation: Transformation) -> None: self._k_chain.add_edge(transformation.parent, transformation.child, T=transformation) self._k_chain.add_edge(transformation.child, transformation.parent, T=transformation.inv()) diff --git a/test/test_types.py b/test/test_types.py index 1cf6365..1fcb145 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -2,7 +2,15 @@ import pytest import random import yaml -from ktree.k_types import Pose, Rotation, Transformation, Vector +from ktree.k_types import ( + Joint, + JointAxis, + JointType, + Pose, + Rotation, + Transformation, + Vector, +) from ktree.ktree import KinematicsTree from ktree.models import KinematicsConfig from loguru import logger @@ -175,3 +183,67 @@ def test_multiple_paths_warning(caplog) -> None: # type: ignore # noqa: F811 kt.get_transformation(parent="yaskawa_base", child="cam") assert "Multiple paths" in caplog.text + + +def test_jacobian() -> None: + kc = KinematicsConfig.parse(Path("./test/2dof.yaml")) + kt = KinematicsTree(config=kc) + kt._get_jacobian() + pass + + +def test_joint_update() -> None: + kt = KinematicsTree( + config=KinematicsConfig( + base_frame="base", + end_effector_frame="end_effector", + kinematics_chain=[ + Transformation( + parent="base", + child="joint1", + pose=Pose().from_list([0.0, 0.0, 0.0, 0.0, 0.0, 10.0], mm_deg=True), + joint=Joint(type=JointType.REVOLUTE, axis=JointAxis.Z, value=20.0), + ), + ], + ) + ) + assert kt.get_transformation(parent="base", child="joint1").joint.value == 20.0 + assert kt.get_transformation(parent="joint1", child="base").joint.value == -20.0 + t1 = Transformation( + parent="base", + child="joint1", + pose=Pose().from_list([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], mm_deg=True), + joint=Joint(type=JointType.REVOLUTE, axis=JointAxis.Z, value=np.radians(30.0)), + ) + kt.update_transformation(t1) + assert kt.get_transformation(parent="base", child="joint1").joint.value == np.radians(30.0) + + kt._actuated_joints[0].joint.value = np.radians(40.0) + kt.update_transformation(transformation=kt._actuated_joints[0]) + assert kt.get_transformation(parent="base", child="joint1").joint.value == np.radians(40.0) + + +def test_2dof() -> None: + kc = KinematicsConfig.parse(Path("./test/2dof.yaml")) + kt = KinematicsTree(config=kc) + logger.info(kt.get_transformation(parent="base", child="link1")) + assert kt._actuated_joints[0].joint.value == 10.0 + kt.update_joints_from_list([30.0, 40.0], mm_deg=True) + assert kt.get_transformation(parent="base", child="link1").joint.value == np.radians(30.0) + assert kt.get_transformation(parent="link1", child="link2").joint.value == np.radians(40.0) + assert kt._actuated_joints[0].joint.value == np.radians(30.0) + + +def test_ik() -> None: + kc = KinematicsConfig.parse(Path("./test/2dof.yaml")) + kt = KinematicsTree(config=kc) + kt.update_joints_from_list([45.0, 45.0], mm_deg=True) + logger.info(kt.get_transformation(parent="base", child="effector")) + # kt.get_transformation(parent="base", child="effector") + + kt2 = KinematicsTree(config=KinematicsConfig.parse(Path("./test/2dof.yaml"))) + logger.info(kt2.get_transformation(parent="base", child="effector")) + kt2.inverse_kinematics(target_effector=kt.get_transformation(parent="base", child="effector")) + logger.info(kt2.get_transformation(parent="base", child="effector")) + logger.info(kt2.get_joint_values() * 180 / np.pi) + assert np.allclose(kt2.get_joint_values(), [np.radians(45.0), np.radians(45.0)], atol=1e-4) From 15d2c648cea581557e8a1eb8d1cf66a3ab851219 Mon Sep 17 00:00:00 2001 From: Daniel Date: Sun, 12 May 2024 23:18:44 +0200 Subject: [PATCH 02/20] feat: add support for quaternions in Rotation type --- src/ktree/k_types.py | 31 +++++++++++++++++++++++++++++++ src/ktree/ktree.py | 4 ++-- test/test_types.py | 8 +++++++- 3 files changed, 40 insertions(+), 3 deletions(-) diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index 2bb158c..7da23fd 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -115,6 +115,20 @@ class JointType(str, Enum): SPATIAL = "spatial" +class Quaternion(BaseModel): + model_config = ConfigDict(arbitrary_types_allowed=True) + + qx: float = Field(default=0.0) + qy: float = Field(default=0.0) + qz: float = Field(default=0.0) + qw: float = Field(default=1.0) + + @computed_field # type: ignore[misc] + @property + def vector(self) -> NDArray: + return np.array([self.qx, self.qy, self.qz, self.qw]) + + class Rotation(BaseModel): model_config = ConfigDict(arbitrary_types_allowed=True) @@ -203,6 +217,23 @@ def matrix(self, matrix: NDArray) -> None: self.rpy = np.array([rx, ry, rz]) + @computed_field # type: ignore[misc] + @property + def quaternion(self) -> Quaternion: + cr = np.cos(self.rpy[0] * 0.5) + sr = np.sin(self.rpy[0] * 0.5) + cp = np.cos(self.rpy[1] * 0.5) + sp = np.sin(self.rpy[1] * 0.5) + cy = np.cos(self.rpy[2] * 0.5) + sy = np.sin(self.rpy[2] * 0.5) + + return Quaternion( + qx=sr * cp * cy - cr * sp * sy, + qy=cr * sp * cy + sr * cp * sy, + qz=cr * cp * sy - sr * sp * cy, + qw=cr * cp * cy + sr * sp * sy, + ) + def __mul__(self, other: Self | Vector | NDArray[np.float_]) -> "Vector | Rotation": if isinstance(other, Vector): return Vector(vector=self.matrix @ other.vector) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 1d47c55..250621e 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -204,7 +204,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: start_pose = self.get_transformation(parent=self.config.base, child=self.config.end_effector) # delta_pose = start_pose.inv() * target_effector delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) - pose_tol = np.array([1e-6] * 6) + pose_tol = np.array([1e-4] * 3 + [1e-5] * 3) ITERATIONS = 1000 iter = 0 dx = delta_pose / 100 @@ -228,7 +228,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: logger.info(f"Converged in {iter} iterations") index = pd.MultiIndex.from_tuples( [ - (f"{self.config.base}_in_{transformation.child}", coordinate) + (f"{transformation.child}", coordinate) for transformation in self.config.transformations for coordinate in ["x", "y", "z", "rx", "ry", "rz"] ] diff --git a/test/test_types.py b/test/test_types.py index 1fcb145..488df3d 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -246,4 +246,10 @@ def test_ik() -> None: kt2.inverse_kinematics(target_effector=kt.get_transformation(parent="base", child="effector")) logger.info(kt2.get_transformation(parent="base", child="effector")) logger.info(kt2.get_joint_values() * 180 / np.pi) - assert np.allclose(kt2.get_joint_values(), [np.radians(45.0), np.radians(45.0)], atol=1e-4) + assert np.allclose(kt2.get_joint_values(), [np.radians(45.0), np.radians(45.0)], atol=np.radians(0.05)) + + +def test_quaternion() -> None: + r = R.from_euler("xyz", [10, 20, 30], degrees=True) + q = r.as_quat() + assert np.allclose(q, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).quaternion.vector) From 04e24420bc7d94df50bdfb7e20961658a94d6425 Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 15 May 2024 19:05:57 +0200 Subject: [PATCH 03/20] jacobian test wip --- test/test_types.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/test_types.py b/test/test_types.py index 488df3d..98f331c 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -227,7 +227,7 @@ def test_2dof() -> None: kc = KinematicsConfig.parse(Path("./test/2dof.yaml")) kt = KinematicsTree(config=kc) logger.info(kt.get_transformation(parent="base", child="link1")) - assert kt._actuated_joints[0].joint.value == 10.0 + assert kt._actuated_joints[0].joint.value == 0.0 kt.update_joints_from_list([30.0, 40.0], mm_deg=True) assert kt.get_transformation(parent="base", child="link1").joint.value == np.radians(30.0) assert kt.get_transformation(parent="link1", child="link2").joint.value == np.radians(40.0) @@ -246,7 +246,7 @@ def test_ik() -> None: kt2.inverse_kinematics(target_effector=kt.get_transformation(parent="base", child="effector")) logger.info(kt2.get_transformation(parent="base", child="effector")) logger.info(kt2.get_joint_values() * 180 / np.pi) - assert np.allclose(kt2.get_joint_values(), [np.radians(45.0), np.radians(45.0)], atol=np.radians(0.05)) + assert np.allclose(kt2.get_joint_values(), [np.radians(45.0), np.radians(45.0)], atol=np.radians(0.1)) def test_quaternion() -> None: From f782755de4d6d075a0e600415129c0b2fc216b6d Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 15 May 2024 21:08:03 +0200 Subject: [PATCH 04/20] fix validation --- src/ktree/k_types.py | 5 ++--- test/test_types.py | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index 7da23fd..4d02c74 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -1,6 +1,5 @@ import numpy as np from enum import Enum -from loguru import logger from numpy.typing import NDArray from pydantic import ( BaseModel, @@ -356,7 +355,7 @@ class Transformation(BaseModel): @model_validator(mode="after") # type: ignore[misc] def _joint_validator(self) -> "Transformation": match self.joint.type: - case JointType.REVOLUTE: + case JointType.REVOLUTE if self.joint.value is not None: match self.joint.axis: case JointAxis.X: self.pose.rotation.rx = self.joint.value @@ -364,7 +363,7 @@ def _joint_validator(self) -> "Transformation": self.pose.rotation.ry = self.joint.value case JointAxis.Z: self.pose.rotation.rz = self.joint.value - case JointType.PRISMATIC: + case JointType.PRISMATIC if self.joint.value is not None: match self.joint.axis: case JointAxis.X: self.pose.translation.x = self.joint.value diff --git a/test/test_types.py b/test/test_types.py index 98f331c..05ce1c5 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -251,5 +251,5 @@ def test_ik() -> None: def test_quaternion() -> None: r = R.from_euler("xyz", [10, 20, 30], degrees=True) - q = r.as_quat() + q = r.as_quat() assert np.allclose(q, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).quaternion.vector) From e091610d0964187f9894e3db2b9635139c8af8ea Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 15 May 2024 21:51:24 +0200 Subject: [PATCH 05/20] wip --- src/ktree/ktree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 250621e..7e9fe50 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -208,7 +208,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: ITERATIONS = 1000 iter = 0 dx = delta_pose / 100 - iterations = [] + iterations = [self._iteration_row()] while True: if all(abs(dx) < pose_tol): break From 147e0fd2339f1b0d41d4db40cf50480760fdfbb8 Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 15 May 2024 22:07:14 +0200 Subject: [PATCH 06/20] Improve piecewise split of target pose --- src/ktree/ktree.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 7e9fe50..943bb39 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -202,12 +202,12 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: """Inverse kinematics using Jacobian""" target_effector.child = "target_effector" start_pose = self.get_transformation(parent=self.config.base, child=self.config.end_effector) - # delta_pose = start_pose.inv() * target_effector - delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) + delta_pose = start_pose.inv() * target_effector + # delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) pose_tol = np.array([1e-4] * 3 + [1e-5] * 3) ITERATIONS = 1000 iter = 0 - dx = delta_pose / 100 + dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * 0.01 iterations = [self._iteration_row()] while True: if all(abs(dx) < pose_tol): @@ -219,8 +219,8 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: current_effector = self.get_transformation(parent=self.config.base, child=self.config.end_effector) iterations.append(self._iteration_row()) logger.debug(current_effector) - # logger.debug(current_effector.inv() * target_effector) - dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 100 + dx = np.linalg.norm(np.array(current_effector.inv() * target_effector).pose.to_list()) * 0.01 + # dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 100 logger.debug(dx) iter += 1 From 998ef488493d19442c92a6d1e644842cfdf7099d Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 15 May 2024 22:22:49 +0200 Subject: [PATCH 07/20] wip --- src/ktree/ktree.py | 13 +++++++++---- test/test_types.py | 4 ++-- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 943bb39..296a224 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -205,9 +205,9 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: delta_pose = start_pose.inv() * target_effector # delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) pose_tol = np.array([1e-4] * 3 + [1e-5] * 3) - ITERATIONS = 1000 + ITERATIONS = 10000 iter = 0 - dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * 0.01 + dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * np.array(delta_pose.pose.to_list()) * 0.005 iterations = [self._iteration_row()] while True: if all(abs(dx) < pose_tol): @@ -218,10 +218,15 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: self.update_joints_from_list(self.get_joint_values() + dq) current_effector = self.get_transformation(parent=self.config.base, child=self.config.end_effector) iterations.append(self._iteration_row()) + logger.debug(current_effector.inv() * target_effector) logger.debug(current_effector) - dx = np.linalg.norm(np.array(current_effector.inv() * target_effector).pose.to_list()) * 0.01 + if (current_effector.inv() * target_effector).pose.translation.norm() < 0.01: + scale = 0.001 + else: + scale = 0.01 + dx = np.linalg.norm(np.array((current_effector.inv() * target_effector).pose.to_list())) * np.array((current_effector.inv() * target_effector).pose.to_list()) * scale # dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 100 - logger.debug(dx) + # logger.debug(dx) iter += 1 diff --git a/test/test_types.py b/test/test_types.py index 05ce1c5..2d930e5 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -237,7 +237,7 @@ def test_2dof() -> None: def test_ik() -> None: kc = KinematicsConfig.parse(Path("./test/2dof.yaml")) kt = KinematicsTree(config=kc) - kt.update_joints_from_list([45.0, 45.0], mm_deg=True) + kt.update_joints_from_list([90.0, 90.0], mm_deg=True) logger.info(kt.get_transformation(parent="base", child="effector")) # kt.get_transformation(parent="base", child="effector") @@ -246,7 +246,7 @@ def test_ik() -> None: kt2.inverse_kinematics(target_effector=kt.get_transformation(parent="base", child="effector")) logger.info(kt2.get_transformation(parent="base", child="effector")) logger.info(kt2.get_joint_values() * 180 / np.pi) - assert np.allclose(kt2.get_joint_values(), [np.radians(45.0), np.radians(45.0)], atol=np.radians(0.1)) + assert np.allclose(kt2.get_joint_values(), [np.radians(90.0), np.radians(90.0)], atol=np.radians(0.1)) def test_quaternion() -> None: From 91646142caa08412ebedb3ce31f86ab5e973618d Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 24 Jun 2024 23:30:19 +0200 Subject: [PATCH 08/20] add missing test file --- src/ktree/ktree.py | 6 +++++- test/2dof.yaml | 40 ++++++++++++++++++++++++++++++++++++++++ test/test_types.py | 1 - 3 files changed, 45 insertions(+), 2 deletions(-) create mode 100644 test/2dof.yaml diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 296a224..2cce4cf 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -224,7 +224,11 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: scale = 0.001 else: scale = 0.01 - dx = np.linalg.norm(np.array((current_effector.inv() * target_effector).pose.to_list())) * np.array((current_effector.inv() * target_effector).pose.to_list()) * scale + dx = ( + np.linalg.norm(np.array((current_effector.inv() * target_effector).pose.to_list())) + * np.array((current_effector.inv() * target_effector).pose.to_list()) + * scale + ) # dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 100 # logger.debug(dx) diff --git a/test/2dof.yaml b/test/2dof.yaml new file mode 100644 index 0000000..026edbe --- /dev/null +++ b/test/2dof.yaml @@ -0,0 +1,40 @@ +base_frame: base +end_effector_frame: effector +kinematics_chain: +- child: link1 + parent: base + pose: + rx_deg: 0.0 + ry_deg: 0.0 + rz_deg: 0.0 + x_mm: 0.0 + y_mm: 0.0 + z_mm: 0.0 + joint: + type: revolute + axis: z + value: 0.0 +- child: link2 + parent: link1 + pose: + rx_deg: 0.0 + ry_deg: 0.0 + rz_deg: 0.0 + x_mm: 150.0 + y_mm: 0.0 + z_mm: 0.0 + joint: + type: revolute + axis: z + value: 0.0 +- child: effector + parent: link2 + pose: + rx_deg: 0.0 + ry_deg: 0.0 + rz_deg: 0.0 + x_mm: 100.0 + y_mm: 0.0 + z_mm: 0.0 + joint: + type: fixed diff --git a/test/test_types.py b/test/test_types.py index 2d930e5..09940b3 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -121,7 +121,6 @@ def test_rotation_multiplication() -> None: ).as_matrix() result = matrix1 * matrix2 - # result = multiply_homogeneous_transformations(matrix1, matrix2) assert np.allclose(result.hmatrix, expected_result, rtol=1e-3) From 7ca6a2c3441c1ce91137ac9d7d75d9d11a78e69d Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Tue, 25 Jun 2024 18:01:26 +0200 Subject: [PATCH 09/20] Expand rotation type to support angle axis and magnitude --- src/ktree/k_types.py | 39 +++++++++++++++++++++++++++++++++++++-- test/test_types.py | 14 ++++++++++++++ 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index 4d02c74..52e16aa 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -126,15 +126,21 @@ class Quaternion(BaseModel): @property def vector(self) -> NDArray: return np.array([self.qx, self.qy, self.qz, self.qw]) + + +class AngleAxis(BaseModel): + axis: Vector = Field(default=Vector(), description="Axis of rotation") + angle: float = Field(default=0.0, description="Angle of rotation in radians") class Rotation(BaseModel): model_config = ConfigDict(arbitrary_types_allowed=True) rpy: NDArray[np.float_] = Field(default=np.array([0.0, 0.0, 0.0]), min_length=3, max_length=3) + degrees: bool = Field(default=False) - def __init__(self, rpy: NDArray[np.float_] | list[float] = np.array([0.0, 0.0, 0.0])) -> None: - super().__init__(**dict(rpy=rpy)) + def __init__(self, rpy: NDArray[np.float_] | list[float] = np.array([0.0, 0.0, 0.0]), degrees: bool = False) -> None: + super().__init__(**dict(rpy=rpy, degrees=degrees)) self.rpy = _validate_list(self.rpy) # validators @@ -143,6 +149,11 @@ def __init__(self, rpy: NDArray[np.float_] | list[float] = np.array([0.0, 0.0, 0 @classmethod def _rpy_validator(cls, v: NDArray[np.float_] | list[float]) -> NDArray: return _validate_list(v) + + @model_validator(mode="after") + def _to_radians(self) -> "Rotation": + self.rpy = np.deg2rad(self.rpy) if self.degrees else self.rpy + return self @staticmethod def rot_x(angle: float) -> NDArray: @@ -232,6 +243,30 @@ def quaternion(self) -> Quaternion: qz=cr * cp * sy - sr * sp * cy, qw=cr * cp * cy + sr * sp * sy, ) + + @computed_field # type: ignore[misc] + @property + def axis_angle(self) -> AngleAxis: + """ + Returns the axis and angle of rotation from quaternion. + """ + q = self.quaternion.vector + angle = 2 * np.arccos(q[3]) + s = np.sqrt(1 - q[3] ** 2) + if s < 1e-3: + return Vector(), 0 + axis = Vector(vector=q[:3] / s) + return AngleAxis(axis=axis, angle=angle) + + + @computed_field # type: ignore[misc] + @property + def magnitude(self) -> float: + """ + Returns the rotation magnitude based on the quaternion. + """ + return 2 * np.arccos(self.quaternion.qw) + def __mul__(self, other: Self | Vector | NDArray[np.float_]) -> "Vector | Rotation": if isinstance(other, Vector): diff --git a/test/test_types.py b/test/test_types.py index 09940b3..d881e18 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -252,3 +252,17 @@ def test_quaternion() -> None: r = R.from_euler("xyz", [10, 20, 30], degrees=True) q = r.as_quat() assert np.allclose(q, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).quaternion.vector) + + +def test_axis_angle() -> None: + r = R.from_euler("xyz", [10, 20, 30], degrees=True) + q = r.as_rotvec() + axis = q / np.linalg.norm(q) + angle = np.linalg.norm(q) + + assert np.allclose(axis, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).axis_angle[0].vector) + +def test_mangnitude() -> None: + rpy = [10, 22.1234, 30] + r = R.from_euler("xyz", rpy, degrees=True) + assert np.isclose(r.magnitude(), Rotation(rpy=rpy, degrees=True).magnitude) From 41ae971352f618fda03e3081135b44ceb28fd636 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 25 Jun 2024 21:34:08 +0200 Subject: [PATCH 10/20] Update iterations --- src/ktree/ktree.py | 38 +++++++++++++++++++++----------------- test/test_types.py | 2 +- 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 2cce4cf..9c31918 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -202,12 +202,14 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: """Inverse kinematics using Jacobian""" target_effector.child = "target_effector" start_pose = self.get_transformation(parent=self.config.base, child=self.config.end_effector) - delta_pose = start_pose.inv() * target_effector - # delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) + logger.info(f"Starting pose: {start_pose}") + # delta_pose = start_pose.inv() * target_effector + delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) pose_tol = np.array([1e-4] * 3 + [1e-5] * 3) - ITERATIONS = 10000 + ITERATIONS = 1000 iter = 0 - dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * np.array(delta_pose.pose.to_list()) * 0.005 + dx = np.linalg.norm(delta_pose) * delta_pose * 0.005 + # dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * np.array(delta_pose.pose.to_list()) * 0.005 iterations = [self._iteration_row()] while True: if all(abs(dx) < pose_tol): @@ -218,23 +220,25 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: self.update_joints_from_list(self.get_joint_values() + dq) current_effector = self.get_transformation(parent=self.config.base, child=self.config.end_effector) iterations.append(self._iteration_row()) - logger.debug(current_effector.inv() * target_effector) - logger.debug(current_effector) - if (current_effector.inv() * target_effector).pose.translation.norm() < 0.01: - scale = 0.001 - else: - scale = 0.01 - dx = ( - np.linalg.norm(np.array((current_effector.inv() * target_effector).pose.to_list())) - * np.array((current_effector.inv() * target_effector).pose.to_list()) - * scale - ) - # dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 100 + # logger.debug(current_effector.inv() * target_effector) + # logger.debug(current_effector) + # if (current_effector.inv() * target_effector).pose.translation.norm() < 0.01: + # scale = 0.001 + # else: + # scale = 0.01 + # dx = ( + # np.linalg.norm(np.array((current_effector.inv() * target_effector).pose.to_list())) + # * np.array((current_effector.inv() * target_effector).pose.to_list()) + # * scale + # ) + dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 3 # logger.debug(dx) iter += 1 - logger.info(f"Converged in {iter} iterations") + logger.info(f"Finished after {iter} iterations") + logger.info(f"Target pose {target_effector}") + logger.debug(current_effector) index = pd.MultiIndex.from_tuples( [ (f"{transformation.child}", coordinate) diff --git a/test/test_types.py b/test/test_types.py index d881e18..a44beaf 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -245,7 +245,7 @@ def test_ik() -> None: kt2.inverse_kinematics(target_effector=kt.get_transformation(parent="base", child="effector")) logger.info(kt2.get_transformation(parent="base", child="effector")) logger.info(kt2.get_joint_values() * 180 / np.pi) - assert np.allclose(kt2.get_joint_values(), [np.radians(90.0), np.radians(90.0)], atol=np.radians(0.1)) + assert np.allclose(kt2.get_joint_values(), [np.radians(90.0), np.radians(90.0)], atol=np.radians(0.001)) def test_quaternion() -> None: From 222d60d1cecd5fae5194de019851c18fd0d42e56 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 25 Jun 2024 21:45:38 +0200 Subject: [PATCH 11/20] Update iterations --- src/ktree/ktree.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 9c31918..8f0ef11 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -205,7 +205,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: logger.info(f"Starting pose: {start_pose}") # delta_pose = start_pose.inv() * target_effector delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) - pose_tol = np.array([1e-4] * 3 + [1e-5] * 3) + pose_tol = np.array([1e-7] * 3 + [1e-7] * 3) ITERATIONS = 1000 iter = 0 dx = np.linalg.norm(delta_pose) * delta_pose * 0.005 @@ -231,7 +231,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: # * np.array((current_effector.inv() * target_effector).pose.to_list()) # * scale # ) - dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 3 + dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 2 # logger.debug(dx) iter += 1 From 4212b1721b19fa039c9a0d4766e614e74788f0e8 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 25 Jun 2024 21:55:31 +0200 Subject: [PATCH 12/20] Update inv kin --- src/ktree/ktree.py | 25 ++++++++----------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 8f0ef11..62a1497 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -200,13 +200,16 @@ def get_joint_values(self) -> NDArray: def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: """Inverse kinematics using Jacobian""" + ITERATIONS = 1000 + ERROR_SCALE = 0.5 + target_effector.child = "target_effector" start_pose = self.get_transformation(parent=self.config.base, child=self.config.end_effector) - logger.info(f"Starting pose: {start_pose}") + logger.debug(f"Starting pose: {start_pose}") # delta_pose = start_pose.inv() * target_effector delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) pose_tol = np.array([1e-7] * 3 + [1e-7] * 3) - ITERATIONS = 1000 + iter = 0 dx = np.linalg.norm(delta_pose) * delta_pose * 0.005 # dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * np.array(delta_pose.pose.to_list()) * 0.005 @@ -220,24 +223,12 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: self.update_joints_from_list(self.get_joint_values() + dq) current_effector = self.get_transformation(parent=self.config.base, child=self.config.end_effector) iterations.append(self._iteration_row()) - # logger.debug(current_effector.inv() * target_effector) - # logger.debug(current_effector) - # if (current_effector.inv() * target_effector).pose.translation.norm() < 0.01: - # scale = 0.001 - # else: - # scale = 0.01 - # dx = ( - # np.linalg.norm(np.array((current_effector.inv() * target_effector).pose.to_list())) - # * np.array((current_effector.inv() * target_effector).pose.to_list()) - # * scale - # ) - dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) / 2 - # logger.debug(dx) + dx = (np.array(target_effector.pose.to_list()) - current_effector.pose.to_list()) * ERROR_SCALE iter += 1 - logger.info(f"Finished after {iter} iterations") - logger.info(f"Target pose {target_effector}") + logger.debug(f"Finished after {iter} iterations") + logger.debug(f"Target pose {target_effector}") logger.debug(current_effector) index = pd.MultiIndex.from_tuples( [ From fb4bf618c47e173297694697d81a80e59b73d33b Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Fri, 28 Jun 2024 15:46:28 +0200 Subject: [PATCH 13/20] Update types with DHParameters and tests --- poetry.lock | 214 ++++++++++++++++++++----------------------- src/ktree/k_types.py | 72 +++++++++------ src/ktree/ktree.py | 25 ++++- test/test_types.py | 65 +++++++++++-- 4 files changed, 225 insertions(+), 151 deletions(-) diff --git a/poetry.lock b/poetry.lock index 1ebfa96..10e538d 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1,4 +1,4 @@ -# This file is automatically @generated by Poetry 1.5.1 and should not be changed by hand. +# This file is automatically @generated by Poetry 1.8.3 and should not be changed by hand. [[package]] name = "annotated-types" @@ -670,18 +670,18 @@ files = [ [[package]] name = "pydantic" -version = "2.4.2" +version = "2.7.4" description = "Data validation using Python type hints" optional = false -python-versions = ">=3.7" +python-versions = ">=3.8" files = [ - {file = "pydantic-2.4.2-py3-none-any.whl", hash = "sha256:bc3ddf669d234f4220e6e1c4d96b061abe0998185a8d7855c0126782b7abc8c1"}, - {file = "pydantic-2.4.2.tar.gz", hash = "sha256:94f336138093a5d7f426aac732dcfe7ab4eb4da243c88f891d65deb4a2556ee7"}, + {file = "pydantic-2.7.4-py3-none-any.whl", hash = "sha256:ee8538d41ccb9c0a9ad3e0e5f07bf15ed8015b481ced539a1759d8cc89ae90d0"}, + {file = "pydantic-2.7.4.tar.gz", hash = "sha256:0c84efd9548d545f63ac0060c1e4d39bb9b14db8b3c0652338aecc07b5adec52"}, ] [package.dependencies] annotated-types = ">=0.4.0" -pydantic-core = "2.10.1" +pydantic-core = "2.18.4" typing-extensions = ">=4.6.1" [package.extras] @@ -689,117 +689,90 @@ email = ["email-validator (>=2.0.0)"] [[package]] name = "pydantic-core" -version = "2.10.1" -description = "" +version = "2.18.4" +description = "Core functionality for Pydantic validation and serialization" optional = false -python-versions = ">=3.7" +python-versions = ">=3.8" files = [ - {file = "pydantic_core-2.10.1-cp310-cp310-macosx_10_7_x86_64.whl", hash = "sha256:d64728ee14e667ba27c66314b7d880b8eeb050e58ffc5fec3b7a109f8cddbd63"}, - {file = "pydantic_core-2.10.1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:48525933fea744a3e7464c19bfede85df4aba79ce90c60b94d8b6e1eddd67096"}, - {file = "pydantic_core-2.10.1-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ef337945bbd76cce390d1b2496ccf9f90b1c1242a3a7bc242ca4a9fc5993427a"}, - {file = "pydantic_core-2.10.1-cp310-cp310-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:a1392e0638af203cee360495fd2cfdd6054711f2db5175b6e9c3c461b76f5175"}, - {file = "pydantic_core-2.10.1-cp310-cp310-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:0675ba5d22de54d07bccde38997e780044dcfa9a71aac9fd7d4d7a1d2e3e65f7"}, - {file = "pydantic_core-2.10.1-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:128552af70a64660f21cb0eb4876cbdadf1a1f9d5de820fed6421fa8de07c893"}, - {file = "pydantic_core-2.10.1-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8f6e6aed5818c264412ac0598b581a002a9f050cb2637a84979859e70197aa9e"}, - {file = "pydantic_core-2.10.1-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:ecaac27da855b8d73f92123e5f03612b04c5632fd0a476e469dfc47cd37d6b2e"}, - {file = "pydantic_core-2.10.1-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:b3c01c2fb081fced3bbb3da78510693dc7121bb893a1f0f5f4b48013201f362e"}, - {file = "pydantic_core-2.10.1-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:92f675fefa977625105708492850bcbc1182bfc3e997f8eecb866d1927c98ae6"}, - {file = "pydantic_core-2.10.1-cp310-none-win32.whl", hash = "sha256:420a692b547736a8d8703c39ea935ab5d8f0d2573f8f123b0a294e49a73f214b"}, - {file = "pydantic_core-2.10.1-cp310-none-win_amd64.whl", hash = "sha256:0880e239827b4b5b3e2ce05e6b766a7414e5f5aedc4523be6b68cfbc7f61c5d0"}, - {file = "pydantic_core-2.10.1-cp311-cp311-macosx_10_7_x86_64.whl", hash = "sha256:073d4a470b195d2b2245d0343569aac7e979d3a0dcce6c7d2af6d8a920ad0bea"}, - {file = "pydantic_core-2.10.1-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:600d04a7b342363058b9190d4e929a8e2e715c5682a70cc37d5ded1e0dd370b4"}, - {file = "pydantic_core-2.10.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:39215d809470f4c8d1881758575b2abfb80174a9e8daf8f33b1d4379357e417c"}, - {file = "pydantic_core-2.10.1-cp311-cp311-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:eeb3d3d6b399ffe55f9a04e09e635554012f1980696d6b0aca3e6cf42a17a03b"}, - {file = "pydantic_core-2.10.1-cp311-cp311-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:a7a7902bf75779bc12ccfc508bfb7a4c47063f748ea3de87135d433a4cca7a2f"}, - {file = "pydantic_core-2.10.1-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:3625578b6010c65964d177626fde80cf60d7f2e297d56b925cb5cdeda6e9925a"}, - {file = "pydantic_core-2.10.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:caa48fc31fc7243e50188197b5f0c4228956f97b954f76da157aae7f67269ae8"}, - {file = "pydantic_core-2.10.1-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:07ec6d7d929ae9c68f716195ce15e745b3e8fa122fc67698ac6498d802ed0fa4"}, - {file = "pydantic_core-2.10.1-cp311-cp311-musllinux_1_1_aarch64.whl", hash = "sha256:e6f31a17acede6a8cd1ae2d123ce04d8cca74056c9d456075f4f6f85de055607"}, - {file = "pydantic_core-2.10.1-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:d8f1ebca515a03e5654f88411420fea6380fc841d1bea08effb28184e3d4899f"}, - {file = "pydantic_core-2.10.1-cp311-none-win32.whl", hash = "sha256:6db2eb9654a85ada248afa5a6db5ff1cf0f7b16043a6b070adc4a5be68c716d6"}, - {file = "pydantic_core-2.10.1-cp311-none-win_amd64.whl", hash = "sha256:4a5be350f922430997f240d25f8219f93b0c81e15f7b30b868b2fddfc2d05f27"}, - {file = "pydantic_core-2.10.1-cp311-none-win_arm64.whl", hash = "sha256:5fdb39f67c779b183b0c853cd6b45f7db84b84e0571b3ef1c89cdb1dfc367325"}, - {file = "pydantic_core-2.10.1-cp312-cp312-macosx_10_7_x86_64.whl", hash = "sha256:b1f22a9ab44de5f082216270552aa54259db20189e68fc12484873d926426921"}, - {file = "pydantic_core-2.10.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:8572cadbf4cfa95fb4187775b5ade2eaa93511f07947b38f4cd67cf10783b118"}, - {file = "pydantic_core-2.10.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:db9a28c063c7c00844ae42a80203eb6d2d6bbb97070cfa00194dff40e6f545ab"}, - {file = "pydantic_core-2.10.1-cp312-cp312-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:0e2a35baa428181cb2270a15864ec6286822d3576f2ed0f4cd7f0c1708472aff"}, - {file = "pydantic_core-2.10.1-cp312-cp312-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:05560ab976012bf40f25d5225a58bfa649bb897b87192a36c6fef1ab132540d7"}, - {file = "pydantic_core-2.10.1-cp312-cp312-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:d6495008733c7521a89422d7a68efa0a0122c99a5861f06020ef5b1f51f9ba7c"}, - {file = "pydantic_core-2.10.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:14ac492c686defc8e6133e3a2d9eaf5261b3df26b8ae97450c1647286750b901"}, - {file = "pydantic_core-2.10.1-cp312-cp312-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:8282bab177a9a3081fd3d0a0175a07a1e2bfb7fcbbd949519ea0980f8a07144d"}, - {file = "pydantic_core-2.10.1-cp312-cp312-musllinux_1_1_aarch64.whl", hash = "sha256:aafdb89fdeb5fe165043896817eccd6434aee124d5ee9b354f92cd574ba5e78f"}, - {file = "pydantic_core-2.10.1-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:f6defd966ca3b187ec6c366604e9296f585021d922e666b99c47e78738b5666c"}, - {file = "pydantic_core-2.10.1-cp312-none-win32.whl", hash = "sha256:7c4d1894fe112b0864c1fa75dffa045720a194b227bed12f4be7f6045b25209f"}, - {file = "pydantic_core-2.10.1-cp312-none-win_amd64.whl", hash = "sha256:5994985da903d0b8a08e4935c46ed8daf5be1cf217489e673910951dc533d430"}, - {file = "pydantic_core-2.10.1-cp312-none-win_arm64.whl", hash = "sha256:0d8a8adef23d86d8eceed3e32e9cca8879c7481c183f84ed1a8edc7df073af94"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-macosx_10_7_x86_64.whl", hash = "sha256:9badf8d45171d92387410b04639d73811b785b5161ecadabf056ea14d62d4ede"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-macosx_11_0_arm64.whl", hash = "sha256:ebedb45b9feb7258fac0a268a3f6bec0a2ea4d9558f3d6f813f02ff3a6dc6698"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:cfe1090245c078720d250d19cb05d67e21a9cd7c257698ef139bc41cf6c27b4f"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:e357571bb0efd65fd55f18db0a2fb0ed89d0bb1d41d906b138f088933ae618bb"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:b3dcd587b69bbf54fc04ca157c2323b8911033e827fffaecf0cafa5a892a0904"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:9c120c9ce3b163b985a3b966bb701114beb1da4b0468b9b236fc754783d85aa3"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:15d6bca84ffc966cc9976b09a18cf9543ed4d4ecbd97e7086f9ce9327ea48891"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:5cabb9710f09d5d2e9e2748c3e3e20d991a4c5f96ed8f1132518f54ab2967221"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-musllinux_1_1_aarch64.whl", hash = "sha256:82f55187a5bebae7d81d35b1e9aaea5e169d44819789837cdd4720d768c55d15"}, - {file = "pydantic_core-2.10.1-cp37-cp37m-musllinux_1_1_x86_64.whl", hash = "sha256:1d40f55222b233e98e3921df7811c27567f0e1a4411b93d4c5c0f4ce131bc42f"}, - {file = "pydantic_core-2.10.1-cp37-none-win32.whl", hash = "sha256:14e09ff0b8fe6e46b93d36a878f6e4a3a98ba5303c76bb8e716f4878a3bee92c"}, - {file = "pydantic_core-2.10.1-cp37-none-win_amd64.whl", hash = "sha256:1396e81b83516b9d5c9e26a924fa69164156c148c717131f54f586485ac3c15e"}, - {file = "pydantic_core-2.10.1-cp38-cp38-macosx_10_7_x86_64.whl", hash = "sha256:6835451b57c1b467b95ffb03a38bb75b52fb4dc2762bb1d9dbed8de31ea7d0fc"}, - {file = "pydantic_core-2.10.1-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:b00bc4619f60c853556b35f83731bd817f989cba3e97dc792bb8c97941b8053a"}, - {file = "pydantic_core-2.10.1-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:0fa467fd300a6f046bdb248d40cd015b21b7576c168a6bb20aa22e595c8ffcdd"}, - {file = "pydantic_core-2.10.1-cp38-cp38-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:d99277877daf2efe074eae6338453a4ed54a2d93fb4678ddfe1209a0c93a2468"}, - {file = "pydantic_core-2.10.1-cp38-cp38-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:fa7db7558607afeccb33c0e4bf1c9a9a835e26599e76af6fe2fcea45904083a6"}, - {file = "pydantic_core-2.10.1-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:aad7bd686363d1ce4ee930ad39f14e1673248373f4a9d74d2b9554f06199fb58"}, - {file = "pydantic_core-2.10.1-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:443fed67d33aa85357464f297e3d26e570267d1af6fef1c21ca50921d2976302"}, - {file = "pydantic_core-2.10.1-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:042462d8d6ba707fd3ce9649e7bf268633a41018d6a998fb5fbacb7e928a183e"}, - {file = "pydantic_core-2.10.1-cp38-cp38-musllinux_1_1_aarch64.whl", hash = "sha256:ecdbde46235f3d560b18be0cb706c8e8ad1b965e5c13bbba7450c86064e96561"}, - {file = "pydantic_core-2.10.1-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:ed550ed05540c03f0e69e6d74ad58d026de61b9eaebebbaaf8873e585cbb18de"}, - {file = "pydantic_core-2.10.1-cp38-none-win32.whl", hash = "sha256:8cdbbd92154db2fec4ec973d45c565e767ddc20aa6dbaf50142676484cbff8ee"}, - {file = "pydantic_core-2.10.1-cp38-none-win_amd64.whl", hash = "sha256:9f6f3e2598604956480f6c8aa24a3384dbf6509fe995d97f6ca6103bb8c2534e"}, - {file = "pydantic_core-2.10.1-cp39-cp39-macosx_10_7_x86_64.whl", hash = "sha256:655f8f4c8d6a5963c9a0687793da37b9b681d9ad06f29438a3b2326d4e6b7970"}, - {file = "pydantic_core-2.10.1-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:e570ffeb2170e116a5b17e83f19911020ac79d19c96f320cbfa1fa96b470185b"}, - {file = "pydantic_core-2.10.1-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:64322bfa13e44c6c30c518729ef08fda6026b96d5c0be724b3c4ae4da939f875"}, - {file = "pydantic_core-2.10.1-cp39-cp39-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:485a91abe3a07c3a8d1e082ba29254eea3e2bb13cbbd4351ea4e5a21912cc9b0"}, - {file = "pydantic_core-2.10.1-cp39-cp39-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:f7c2b8eb9fc872e68b46eeaf835e86bccc3a58ba57d0eedc109cbb14177be531"}, - {file = "pydantic_core-2.10.1-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:a5cb87bdc2e5f620693148b5f8f842d293cae46c5f15a1b1bf7ceeed324a740c"}, - {file = "pydantic_core-2.10.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:25bd966103890ccfa028841a8f30cebcf5875eeac8c4bde4fe221364c92f0c9a"}, - {file = "pydantic_core-2.10.1-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:f323306d0556351735b54acbf82904fe30a27b6a7147153cbe6e19aaaa2aa429"}, - {file = "pydantic_core-2.10.1-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:0c27f38dc4fbf07b358b2bc90edf35e82d1703e22ff2efa4af4ad5de1b3833e7"}, - {file = "pydantic_core-2.10.1-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:f1365e032a477c1430cfe0cf2856679529a2331426f8081172c4a74186f1d595"}, - {file = "pydantic_core-2.10.1-cp39-none-win32.whl", hash = "sha256:a1c311fd06ab3b10805abb72109f01a134019739bd3286b8ae1bc2fc4e50c07a"}, - {file = "pydantic_core-2.10.1-cp39-none-win_amd64.whl", hash = "sha256:ae8a8843b11dc0b03b57b52793e391f0122e740de3df1474814c700d2622950a"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-macosx_10_7_x86_64.whl", hash = "sha256:d43002441932f9a9ea5d6f9efaa2e21458221a3a4b417a14027a1d530201ef1b"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-macosx_11_0_arm64.whl", hash = "sha256:fcb83175cc4936a5425dde3356f079ae03c0802bbdf8ff82c035f8a54b333521"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:962ed72424bf1f72334e2f1e61b68f16c0e596f024ca7ac5daf229f7c26e4208"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:2cf5bb4dd67f20f3bbc1209ef572a259027c49e5ff694fa56bed62959b41e1f9"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:e544246b859f17373bed915182ab841b80849ed9cf23f1f07b73b7c58baee5fb"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-musllinux_1_1_aarch64.whl", hash = "sha256:c0877239307b7e69d025b73774e88e86ce82f6ba6adf98f41069d5b0b78bd1bf"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-musllinux_1_1_x86_64.whl", hash = "sha256:53df009d1e1ba40f696f8995683e067e3967101d4bb4ea6f667931b7d4a01357"}, - {file = "pydantic_core-2.10.1-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:a1254357f7e4c82e77c348dabf2d55f1d14d19d91ff025004775e70a6ef40ada"}, - {file = "pydantic_core-2.10.1-pp37-pypy37_pp73-macosx_10_7_x86_64.whl", hash = "sha256:524ff0ca3baea164d6d93a32c58ac79eca9f6cf713586fdc0adb66a8cdeab96a"}, - {file = "pydantic_core-2.10.1-pp37-pypy37_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:3f0ac9fb8608dbc6eaf17956bf623c9119b4db7dbb511650910a82e261e6600f"}, - {file = "pydantic_core-2.10.1-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:320f14bd4542a04ab23747ff2c8a778bde727158b606e2661349557f0770711e"}, - {file = "pydantic_core-2.10.1-pp37-pypy37_pp73-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:63974d168b6233b4ed6a0046296803cb13c56637a7b8106564ab575926572a55"}, - {file = "pydantic_core-2.10.1-pp37-pypy37_pp73-musllinux_1_1_aarch64.whl", hash = "sha256:417243bf599ba1f1fef2bb8c543ceb918676954734e2dcb82bf162ae9d7bd514"}, - {file = "pydantic_core-2.10.1-pp37-pypy37_pp73-musllinux_1_1_x86_64.whl", hash = "sha256:dda81e5ec82485155a19d9624cfcca9be88a405e2857354e5b089c2a982144b2"}, - {file = "pydantic_core-2.10.1-pp37-pypy37_pp73-win_amd64.whl", hash = "sha256:14cfbb00959259e15d684505263d5a21732b31248a5dd4941f73a3be233865b9"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-macosx_10_7_x86_64.whl", hash = "sha256:631cb7415225954fdcc2a024119101946793e5923f6c4d73a5914d27eb3d3a05"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-macosx_11_0_arm64.whl", hash = "sha256:bec7dd208a4182e99c5b6c501ce0b1f49de2802448d4056091f8e630b28e9a52"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:149b8a07712f45b332faee1a2258d8ef1fb4a36f88c0c17cb687f205c5dc6e7d"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4d966c47f9dd73c2d32a809d2be529112d509321c5310ebf54076812e6ecd884"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:7eb037106f5c6b3b0b864ad226b0b7ab58157124161d48e4b30c4a43fef8bc4b"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-musllinux_1_1_aarch64.whl", hash = "sha256:154ea7c52e32dce13065dbb20a4a6f0cc012b4f667ac90d648d36b12007fa9f7"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-musllinux_1_1_x86_64.whl", hash = "sha256:e562617a45b5a9da5be4abe72b971d4f00bf8555eb29bb91ec2ef2be348cd132"}, - {file = "pydantic_core-2.10.1-pp38-pypy38_pp73-win_amd64.whl", hash = "sha256:f23b55eb5464468f9e0e9a9935ce3ed2a870608d5f534025cd5536bca25b1402"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-macosx_10_7_x86_64.whl", hash = "sha256:e9121b4009339b0f751955baf4543a0bfd6bc3f8188f8056b1a25a2d45099934"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-macosx_11_0_arm64.whl", hash = "sha256:0523aeb76e03f753b58be33b26540880bac5aa54422e4462404c432230543f33"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:2e0e2959ef5d5b8dc9ef21e1a305a21a36e254e6a34432d00c72a92fdc5ecda5"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:da01bec0a26befab4898ed83b362993c844b9a607a86add78604186297eb047e"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:f2e9072d71c1f6cfc79a36d4484c82823c560e6f5599c43c1ca6b5cdbd54f881"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-musllinux_1_1_aarch64.whl", hash = "sha256:f36a3489d9e28fe4b67be9992a23029c3cec0babc3bd9afb39f49844a8c721c5"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-musllinux_1_1_x86_64.whl", hash = "sha256:f64f82cc3443149292b32387086d02a6c7fb39b8781563e0ca7b8d7d9cf72bd7"}, - {file = "pydantic_core-2.10.1-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:b4a6db486ac8e99ae696e09efc8b2b9fea67b63c8f88ba7a1a16c24a057a0776"}, - {file = "pydantic_core-2.10.1.tar.gz", hash = "sha256:0f8682dbdd2f67f8e1edddcbffcc29f60a6182b4901c367fc8c1c40d30bb0a82"}, + {file = "pydantic_core-2.18.4-cp310-cp310-macosx_10_12_x86_64.whl", hash = "sha256:f76d0ad001edd426b92233d45c746fd08f467d56100fd8f30e9ace4b005266e4"}, + {file = "pydantic_core-2.18.4-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:59ff3e89f4eaf14050c8022011862df275b552caef8082e37b542b066ce1ff26"}, + {file = "pydantic_core-2.18.4-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a55b5b16c839df1070bc113c1f7f94a0af4433fcfa1b41799ce7606e5c79ce0a"}, + {file = "pydantic_core-2.18.4-cp310-cp310-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:4d0dcc59664fcb8974b356fe0a18a672d6d7cf9f54746c05f43275fc48636851"}, + {file = "pydantic_core-2.18.4-cp310-cp310-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:8951eee36c57cd128f779e641e21eb40bc5073eb28b2d23f33eb0ef14ffb3f5d"}, + {file = "pydantic_core-2.18.4-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:4701b19f7e3a06ea655513f7938de6f108123bf7c86bbebb1196eb9bd35cf724"}, + {file = "pydantic_core-2.18.4-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:e00a3f196329e08e43d99b79b286d60ce46bed10f2280d25a1718399457e06be"}, + {file = "pydantic_core-2.18.4-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:97736815b9cc893b2b7f663628e63f436018b75f44854c8027040e05230eeddb"}, + {file = "pydantic_core-2.18.4-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:6891a2ae0e8692679c07728819b6e2b822fb30ca7445f67bbf6509b25a96332c"}, + {file = "pydantic_core-2.18.4-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:bc4ff9805858bd54d1a20efff925ccd89c9d2e7cf4986144b30802bf78091c3e"}, + {file = "pydantic_core-2.18.4-cp310-none-win32.whl", hash = "sha256:1b4de2e51bbcb61fdebd0ab86ef28062704f62c82bbf4addc4e37fa4b00b7cbc"}, + {file = "pydantic_core-2.18.4-cp310-none-win_amd64.whl", hash = "sha256:6a750aec7bf431517a9fd78cb93c97b9b0c496090fee84a47a0d23668976b4b0"}, + {file = "pydantic_core-2.18.4-cp311-cp311-macosx_10_12_x86_64.whl", hash = "sha256:942ba11e7dfb66dc70f9ae66b33452f51ac7bb90676da39a7345e99ffb55402d"}, + {file = "pydantic_core-2.18.4-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:b2ebef0e0b4454320274f5e83a41844c63438fdc874ea40a8b5b4ecb7693f1c4"}, + {file = "pydantic_core-2.18.4-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a642295cd0c8df1b86fc3dced1d067874c353a188dc8e0f744626d49e9aa51c4"}, + {file = "pydantic_core-2.18.4-cp311-cp311-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:5f09baa656c904807e832cf9cce799c6460c450c4ad80803517032da0cd062e2"}, + {file = "pydantic_core-2.18.4-cp311-cp311-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:98906207f29bc2c459ff64fa007afd10a8c8ac080f7e4d5beff4c97086a3dabd"}, + {file = "pydantic_core-2.18.4-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:19894b95aacfa98e7cb093cd7881a0c76f55731efad31073db4521e2b6ff5b7d"}, + {file = "pydantic_core-2.18.4-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0fbbdc827fe5e42e4d196c746b890b3d72876bdbf160b0eafe9f0334525119c8"}, + {file = "pydantic_core-2.18.4-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:f85d05aa0918283cf29a30b547b4df2fbb56b45b135f9e35b6807cb28bc47951"}, + {file = "pydantic_core-2.18.4-cp311-cp311-musllinux_1_1_aarch64.whl", hash = "sha256:e85637bc8fe81ddb73fda9e56bab24560bdddfa98aa64f87aaa4e4b6730c23d2"}, + {file = "pydantic_core-2.18.4-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:2f5966897e5461f818e136b8451d0551a2e77259eb0f73a837027b47dc95dab9"}, + {file = "pydantic_core-2.18.4-cp311-none-win32.whl", hash = "sha256:44c7486a4228413c317952e9d89598bcdfb06399735e49e0f8df643e1ccd0558"}, + {file = "pydantic_core-2.18.4-cp311-none-win_amd64.whl", hash = "sha256:8a7164fe2005d03c64fd3b85649891cd4953a8de53107940bf272500ba8a788b"}, + {file = "pydantic_core-2.18.4-cp311-none-win_arm64.whl", hash = "sha256:4e99bc050fe65c450344421017f98298a97cefc18c53bb2f7b3531eb39bc7805"}, + {file = "pydantic_core-2.18.4-cp312-cp312-macosx_10_12_x86_64.whl", hash = "sha256:6f5c4d41b2771c730ea1c34e458e781b18cc668d194958e0112455fff4e402b2"}, + {file = "pydantic_core-2.18.4-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:2fdf2156aa3d017fddf8aea5adfba9f777db1d6022d392b682d2a8329e087cef"}, + {file = "pydantic_core-2.18.4-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:4748321b5078216070b151d5271ef3e7cc905ab170bbfd27d5c83ee3ec436695"}, + {file = "pydantic_core-2.18.4-cp312-cp312-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:847a35c4d58721c5dc3dba599878ebbdfd96784f3fb8bb2c356e123bdcd73f34"}, + {file = "pydantic_core-2.18.4-cp312-cp312-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:3c40d4eaad41f78e3bbda31b89edc46a3f3dc6e171bf0ecf097ff7a0ffff7cb1"}, + {file = "pydantic_core-2.18.4-cp312-cp312-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:21a5e440dbe315ab9825fcd459b8814bb92b27c974cbc23c3e8baa2b76890077"}, + {file = "pydantic_core-2.18.4-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:01dd777215e2aa86dfd664daed5957704b769e726626393438f9c87690ce78c3"}, + {file = "pydantic_core-2.18.4-cp312-cp312-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:4b06beb3b3f1479d32befd1f3079cc47b34fa2da62457cdf6c963393340b56e9"}, + {file = "pydantic_core-2.18.4-cp312-cp312-musllinux_1_1_aarch64.whl", hash = "sha256:564d7922e4b13a16b98772441879fcdcbe82ff50daa622d681dd682175ea918c"}, + {file = "pydantic_core-2.18.4-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:0eb2a4f660fcd8e2b1c90ad566db2b98d7f3f4717c64fe0a83e0adb39766d5b8"}, + {file = "pydantic_core-2.18.4-cp312-none-win32.whl", hash = "sha256:8b8bab4c97248095ae0c4455b5a1cd1cdd96e4e4769306ab19dda135ea4cdb07"}, + {file = "pydantic_core-2.18.4-cp312-none-win_amd64.whl", hash = "sha256:14601cdb733d741b8958224030e2bfe21a4a881fb3dd6fbb21f071cabd48fa0a"}, + {file = "pydantic_core-2.18.4-cp312-none-win_arm64.whl", hash = "sha256:c1322d7dd74713dcc157a2b7898a564ab091ca6c58302d5c7b4c07296e3fd00f"}, + {file = "pydantic_core-2.18.4-cp38-cp38-macosx_10_12_x86_64.whl", hash = "sha256:823be1deb01793da05ecb0484d6c9e20baebb39bd42b5d72636ae9cf8350dbd2"}, + {file = "pydantic_core-2.18.4-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:ebef0dd9bf9b812bf75bda96743f2a6c5734a02092ae7f721c048d156d5fabae"}, + {file = "pydantic_core-2.18.4-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ae1d6df168efb88d7d522664693607b80b4080be6750c913eefb77e34c12c71a"}, + {file = "pydantic_core-2.18.4-cp38-cp38-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:f9899c94762343f2cc2fc64c13e7cae4c3cc65cdfc87dd810a31654c9b7358cc"}, + {file = "pydantic_core-2.18.4-cp38-cp38-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:99457f184ad90235cfe8461c4d70ab7dd2680e28821c29eca00252ba90308c78"}, + {file = "pydantic_core-2.18.4-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:18f469a3d2a2fdafe99296a87e8a4c37748b5080a26b806a707f25a902c040a8"}, + {file = "pydantic_core-2.18.4-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:b7cdf28938ac6b8b49ae5e92f2735056a7ba99c9b110a474473fd71185c1af5d"}, + {file = "pydantic_core-2.18.4-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:938cb21650855054dc54dfd9120a851c974f95450f00683399006aa6e8abb057"}, + {file = "pydantic_core-2.18.4-cp38-cp38-musllinux_1_1_aarch64.whl", hash = "sha256:44cd83ab6a51da80fb5adbd9560e26018e2ac7826f9626bc06ca3dc074cd198b"}, + {file = "pydantic_core-2.18.4-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:972658f4a72d02b8abfa2581d92d59f59897d2e9f7e708fdabe922f9087773af"}, + {file = "pydantic_core-2.18.4-cp38-none-win32.whl", hash = "sha256:1d886dc848e60cb7666f771e406acae54ab279b9f1e4143babc9c2258213daa2"}, + {file = "pydantic_core-2.18.4-cp38-none-win_amd64.whl", hash = "sha256:bb4462bd43c2460774914b8525f79b00f8f407c945d50881568f294c1d9b4443"}, + {file = "pydantic_core-2.18.4-cp39-cp39-macosx_10_12_x86_64.whl", hash = "sha256:44a688331d4a4e2129140a8118479443bd6f1905231138971372fcde37e43528"}, + {file = "pydantic_core-2.18.4-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:a2fdd81edd64342c85ac7cf2753ccae0b79bf2dfa063785503cb85a7d3593223"}, + {file = "pydantic_core-2.18.4-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:86110d7e1907ab36691f80b33eb2da87d780f4739ae773e5fc83fb272f88825f"}, + {file = "pydantic_core-2.18.4-cp39-cp39-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:46387e38bd641b3ee5ce247563b60c5ca098da9c56c75c157a05eaa0933ed154"}, + {file = "pydantic_core-2.18.4-cp39-cp39-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:123c3cec203e3f5ac7b000bd82235f1a3eced8665b63d18be751f115588fea30"}, + {file = "pydantic_core-2.18.4-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:dc1803ac5c32ec324c5261c7209e8f8ce88e83254c4e1aebdc8b0a39f9ddb443"}, + {file = "pydantic_core-2.18.4-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:53db086f9f6ab2b4061958d9c276d1dbe3690e8dd727d6abf2321d6cce37fa94"}, + {file = "pydantic_core-2.18.4-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:abc267fa9837245cc28ea6929f19fa335f3dc330a35d2e45509b6566dc18be23"}, + {file = "pydantic_core-2.18.4-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:a0d829524aaefdebccb869eed855e2d04c21d2d7479b6cada7ace5448416597b"}, + {file = "pydantic_core-2.18.4-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:509daade3b8649f80d4e5ff21aa5673e4ebe58590b25fe42fac5f0f52c6f034a"}, + {file = "pydantic_core-2.18.4-cp39-none-win32.whl", hash = "sha256:ca26a1e73c48cfc54c4a76ff78df3727b9d9f4ccc8dbee4ae3f73306a591676d"}, + {file = "pydantic_core-2.18.4-cp39-none-win_amd64.whl", hash = "sha256:c67598100338d5d985db1b3d21f3619ef392e185e71b8d52bceacc4a7771ea7e"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-macosx_10_12_x86_64.whl", hash = "sha256:574d92eac874f7f4db0ca653514d823a0d22e2354359d0759e3f6a406db5d55d"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-macosx_11_0_arm64.whl", hash = "sha256:1f4d26ceb5eb9eed4af91bebeae4b06c3fb28966ca3a8fb765208cf6b51102ab"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:77450e6d20016ec41f43ca4a6c63e9fdde03f0ae3fe90e7c27bdbeaece8b1ed4"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d323a01da91851a4f17bf592faf46149c9169d68430b3146dcba2bb5e5719abc"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:43d447dd2ae072a0065389092a231283f62d960030ecd27565672bd40746c507"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-musllinux_1_1_aarch64.whl", hash = "sha256:578e24f761f3b425834f297b9935e1ce2e30f51400964ce4801002435a1b41ef"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-musllinux_1_1_x86_64.whl", hash = "sha256:81b5efb2f126454586d0f40c4d834010979cb80785173d1586df845a632e4e6d"}, + {file = "pydantic_core-2.18.4-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:ab86ce7c8f9bea87b9d12c7f0af71102acbf5ecbc66c17796cff45dae54ef9a5"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-macosx_10_12_x86_64.whl", hash = "sha256:90afc12421df2b1b4dcc975f814e21bc1754640d502a2fbcc6d41e77af5ec312"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-macosx_11_0_arm64.whl", hash = "sha256:51991a89639a912c17bef4b45c87bd83593aee0437d8102556af4885811d59f5"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:293afe532740370aba8c060882f7d26cfd00c94cae32fd2e212a3a6e3b7bc15e"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:b48ece5bde2e768197a2d0f6e925f9d7e3e826f0ad2271120f8144a9db18d5c8"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:eae237477a873ab46e8dd748e515c72c0c804fb380fbe6c85533c7de51f23a8f"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-musllinux_1_1_aarch64.whl", hash = "sha256:834b5230b5dfc0c1ec37b2fda433b271cbbc0e507560b5d1588e2cc1148cf1ce"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-musllinux_1_1_x86_64.whl", hash = "sha256:e858ac0a25074ba4bce653f9b5d0a85b7456eaddadc0ce82d3878c22489fa4ee"}, + {file = "pydantic_core-2.18.4-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:2fd41f6eff4c20778d717af1cc50eca52f5afe7805ee530a4fbd0bae284f16e9"}, + {file = "pydantic_core-2.18.4.tar.gz", hash = "sha256:ec3beeada09ff865c344ff3bc2f427f5e6c26401cc6113d77e372c3fdac73864"}, ] [package.dependencies] @@ -904,6 +877,7 @@ files = [ {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:69b023b2b4daa7548bcfbd4aa3da05b3a74b772db9e23b982788168117739938"}, {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:81e0b275a9ecc9c0c0c07b4b90ba548307583c125f54d5b6946cfee6360c733d"}, {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ba336e390cd8e4d1739f42dfe9bb83a3cc2e80f567d8805e11b46f4a943f5515"}, + {file = "PyYAML-6.0.1-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:326c013efe8048858a6d312ddd31d56e468118ad4cdeda36c719bf5bb6192290"}, {file = "PyYAML-6.0.1-cp310-cp310-win32.whl", hash = "sha256:bd4af7373a854424dabd882decdc5579653d7868b8fb26dc7d0e99f823aa5924"}, {file = "PyYAML-6.0.1-cp310-cp310-win_amd64.whl", hash = "sha256:fd1592b3fdf65fff2ad0004b5e363300ef59ced41c2e6b3a99d4089fa8c5435d"}, {file = "PyYAML-6.0.1-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:6965a7bc3cf88e5a1c3bd2e0b5c22f8d677dc88a455344035f03399034eb3007"}, @@ -911,8 +885,16 @@ files = [ {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:42f8152b8dbc4fe7d96729ec2b99c7097d656dc1213a3229ca5383f973a5ed6d"}, {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:062582fca9fabdd2c8b54a3ef1c978d786e0f6b3a1510e0ac93ef59e0ddae2bc"}, {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d2b04aac4d386b172d5b9692e2d2da8de7bfb6c387fa4f801fbf6fb2e6ba4673"}, + {file = "PyYAML-6.0.1-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:e7d73685e87afe9f3b36c799222440d6cf362062f78be1013661b00c5c6f678b"}, {file = "PyYAML-6.0.1-cp311-cp311-win32.whl", hash = "sha256:1635fd110e8d85d55237ab316b5b011de701ea0f29d07611174a1b42f1444741"}, {file = "PyYAML-6.0.1-cp311-cp311-win_amd64.whl", hash = "sha256:bf07ee2fef7014951eeb99f56f39c9bb4af143d8aa3c21b1677805985307da34"}, + {file = "PyYAML-6.0.1-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:855fb52b0dc35af121542a76b9a84f8d1cd886ea97c84703eaa6d88e37a2ad28"}, + {file = "PyYAML-6.0.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:40df9b996c2b73138957fe23a16a4f0ba614f4c0efce1e9406a184b6d07fa3a9"}, + {file = "PyYAML-6.0.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a08c6f0fe150303c1c6b71ebcd7213c2858041a7e01975da3a99aed1e7a378ef"}, + {file = "PyYAML-6.0.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6c22bec3fbe2524cde73d7ada88f6566758a8f7227bfbf93a408a9d86bcc12a0"}, + {file = "PyYAML-6.0.1-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:8d4e9c88387b0f5c7d5f281e55304de64cf7f9c0021a3525bd3b1c542da3b0e4"}, + {file = "PyYAML-6.0.1-cp312-cp312-win32.whl", hash = "sha256:d483d2cdf104e7c9fa60c544d92981f12ad66a457afae824d146093b8c294c54"}, + {file = "PyYAML-6.0.1-cp312-cp312-win_amd64.whl", hash = "sha256:0d3304d8c0adc42be59c5f8a4d9e3d7379e6955ad754aa9d6ab7a398b59dd1df"}, {file = "PyYAML-6.0.1-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:50550eb667afee136e9a77d6dc71ae76a44df8b3e51e41b77f6de2932bfe0f47"}, {file = "PyYAML-6.0.1-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1fe35611261b29bd1de0070f0b2f47cb6ff71fa6595c077e42bd0c419fa27b98"}, {file = "PyYAML-6.0.1-cp36-cp36m-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:704219a11b772aea0d8ecd7058d0082713c3562b4e271b849ad7dc4a5c90c13c"}, @@ -929,6 +911,7 @@ files = [ {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a0cd17c15d3bb3fa06978b4e8958dcdc6e0174ccea823003a106c7d4d7899ac5"}, {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:28c119d996beec18c05208a8bd78cbe4007878c6dd15091efb73a30e90539696"}, {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:7e07cbde391ba96ab58e532ff4803f79c4129397514e1413a7dc761ccd755735"}, + {file = "PyYAML-6.0.1-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:49a183be227561de579b4a36efbb21b3eab9651dd81b1858589f796549873dd6"}, {file = "PyYAML-6.0.1-cp38-cp38-win32.whl", hash = "sha256:184c5108a2aca3c5b3d3bf9395d50893a7ab82a38004c8f61c258d4428e80206"}, {file = "PyYAML-6.0.1-cp38-cp38-win_amd64.whl", hash = "sha256:1e2722cc9fbb45d9b87631ac70924c11d3a401b2d7f410cc0e3bbf249f2dca62"}, {file = "PyYAML-6.0.1-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:9eb6caa9a297fc2c2fb8862bc5370d0303ddba53ba97e71f08023b6cd73d16a8"}, @@ -936,6 +919,7 @@ files = [ {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:5773183b6446b2c99bb77e77595dd486303b4faab2b086e7b17bc6bef28865f6"}, {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:b786eecbdf8499b9ca1d697215862083bd6d2a99965554781d0d8d1ad31e13a0"}, {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:bc1bf2925a1ecd43da378f4db9e4f799775d6367bdb94671027b73b393a7c42c"}, + {file = "PyYAML-6.0.1-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:04ac92ad1925b2cff1db0cfebffb6ffc43457495c9b3c39d3fcae417d7125dc5"}, {file = "PyYAML-6.0.1-cp39-cp39-win32.whl", hash = "sha256:faca3bdcf85b2fc05d06ff3fbc1f83e1391b3e724afa3feba7d13eeab355484c"}, {file = "PyYAML-6.0.1-cp39-cp39-win_amd64.whl", hash = "sha256:510c9deebc5c0225e8c96813043e62b680ba2f9c50a08d3724c7f28a747d1486"}, {file = "PyYAML-6.0.1.tar.gz", hash = "sha256:bfdf460b1736c775f2ba9f6a92bca30bc2095067b8a9d77876d1fad6cc3b4a43"}, diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index 52e16aa..b9e000a 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -5,6 +5,7 @@ BaseModel, ConfigDict, Field, + ValidationInfo, computed_field, field_validator, model_serializer, @@ -35,10 +36,11 @@ class Vector(BaseModel): model_config = ConfigDict(arbitrary_types_allowed=True, validate_assignment=True) vector: NDArray[np.float_] = Field(default=np.array([0.0, 0.0, 0.0]), min_length=3, max_length=3) + # mm: bool = Field(default=False) - def __init__(self, vector: NDArray[np.float_] | list[float] = np.array([0.0, 0.0, 0.0])) -> None: - super().__init__(**dict(vector=vector)) - self.vector = _validate_list(self.vector) + # def __init__(self, vector: NDArray[np.float_] | list[float] = np.array([0.0, 0.0, 0.0])) -> None: + # super().__init__(**dict(vector=vector)) + # self.vector = _validate_list(self.vector) # validators @field_validator("vector", mode="before") @@ -103,6 +105,11 @@ def __str__(self) -> str: def __eq__(self, other: object) -> bool: if isinstance(other, self.__class__): return np.allclose(self.vector, other.vector) + elif isinstance(other, np.ndarray): + if other.shape == (3,): + return np.allclose(self.vector, other) + else: + raise ValueError(f"Cannot compare Vector with {other}") else: raise NotImplementedError(f"Cannot compare Vector with {other}") @@ -126,34 +133,22 @@ class Quaternion(BaseModel): @property def vector(self) -> NDArray: return np.array([self.qx, self.qy, self.qz, self.qw]) - - + + class AngleAxis(BaseModel): axis: Vector = Field(default=Vector(), description="Axis of rotation") angle: float = Field(default=0.0, description="Angle of rotation in radians") class Rotation(BaseModel): - model_config = ConfigDict(arbitrary_types_allowed=True) + model_config = ConfigDict(arbitrary_types_allowed=True, validate_assignment=True) rpy: NDArray[np.float_] = Field(default=np.array([0.0, 0.0, 0.0]), min_length=3, max_length=3) - degrees: bool = Field(default=False) - - def __init__(self, rpy: NDArray[np.float_] | list[float] = np.array([0.0, 0.0, 0.0]), degrees: bool = False) -> None: - super().__init__(**dict(rpy=rpy, degrees=degrees)) - self.rpy = _validate_list(self.rpy) - # validators - # _rpy_validator = field_validator("rpy", mode="before")(_validate_list) @field_validator("rpy", mode="before") @classmethod - def _rpy_validator(cls, v: NDArray[np.float_] | list[float]) -> NDArray: - return _validate_list(v) - - @model_validator(mode="after") - def _to_radians(self) -> "Rotation": - self.rpy = np.deg2rad(self.rpy) if self.degrees else self.rpy - return self + def _rpy_validator(cls, rpy_value: NDArray[np.float_] | list[float], info: ValidationInfo) -> NDArray: + return _validate_list(rpy_value) @staticmethod def rot_x(angle: float) -> NDArray: @@ -243,7 +238,7 @@ def quaternion(self) -> Quaternion: qz=cr * cp * sy - sr * sp * cy, qw=cr * cp * cy + sr * sp * sy, ) - + @computed_field # type: ignore[misc] @property def axis_angle(self) -> AngleAxis: @@ -254,19 +249,17 @@ def axis_angle(self) -> AngleAxis: angle = 2 * np.arccos(q[3]) s = np.sqrt(1 - q[3] ** 2) if s < 1e-3: - return Vector(), 0 + return AngleAxis(axis=Vector(), angle=0.0) axis = Vector(vector=q[:3] / s) return AngleAxis(axis=axis, angle=angle) - - + @computed_field # type: ignore[misc] @property def magnitude(self) -> float: """ - Returns the rotation magnitude based on the quaternion. + Returns the rotation magnitude based on the quaternion. """ return 2 * np.arccos(self.quaternion.qw) - def __mul__(self, other: Self | Vector | NDArray[np.float_]) -> "Vector | Rotation": if isinstance(other, Vector): @@ -333,6 +326,33 @@ class JointAxis(str, Enum): Z = "z" +class DHParameters(BaseModel): + model_config = ConfigDict(arbitrary_types_allowed=True, validate_assignment=True) + + a: float = Field(default=0.0, description="Distance between z_i-1 and z_i along x_i") + alpha: float = Field(default=0.0, description="Angle between z_i-1 and z_i along x_i") + d: float = Field(default=0.0, description="Distance between x_i-1 and x_i along z_i-1") + theta: float = Field(default=0.0, description="Angle between x_i-1 and x_i along z_i-1") + + @staticmethod + def from_matrix(matrix: NDArray) -> "DHParameters": + return DHParameters( + a=matrix[0, 3], + alpha=np.arccos(matrix[2, 2]), + d=matrix[2, 3] / matrix[2, 2], + theta=np.arccos(matrix[0, 0]), + ) + + def to_list(self) -> list[float]: + return [self.a, self.alpha, self.d, self.theta] + + def __eq__(self, other: object) -> bool: + if isinstance(other, self.__class__): + return np.allclose(self.to_list(), other.to_list()) + else: + raise NotImplementedError(f"Cannot compare DHParameters with {other}") + + class Joint(BaseModel): type: JointType = Field(default=JointType.FIXED, description="Degree of freedom type of the joint") axis: JointAxis | None = Field( diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 62a1497..1280c68 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -1,7 +1,7 @@ import networkx as nx import numpy as np import pandas as pd -from ktree.k_types import JointType, Pose, Transformation, Vector +from ktree.k_types import DHParameters, JointType, Pose, Transformation, Vector from ktree.models import KinematicsConfig from loguru import logger from numpy.typing import NDArray @@ -209,7 +209,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: # delta_pose = start_pose.inv() * target_effector delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) pose_tol = np.array([1e-7] * 3 + [1e-7] * 3) - + iter = 0 dx = np.linalg.norm(delta_pose) * delta_pose * 0.005 # dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * np.array(delta_pose.pose.to_list()) * 0.005 @@ -218,6 +218,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: if all(abs(dx) < pose_tol): break if iter > ITERATIONS: + logger.warning("Max iterations reached. Solution might not have converged.") break dq = np.linalg.pinv(self._get_jacobian()) @ dx self.update_joints_from_list(self.get_joint_values() + dq) @@ -229,7 +230,7 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: logger.debug(f"Finished after {iter} iterations") logger.debug(f"Target pose {target_effector}") - logger.debug(current_effector) + logger.debug(f"Current pose {current_effector}") index = pd.MultiIndex.from_tuples( [ (f"{transformation.child}", coordinate) @@ -239,6 +240,24 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: ) return pd.DataFrame(np.array(iterations).reshape(-1, len(self.config.transformations) * 6), columns=index) + def _parameter_jacobian(self) -> NDArray: + return np.array([]) + + # def _get_dh_parameters(self) -> DHParameters: + # for joint in self._actuated_joints: + # dh_matrix = self.get_transformation(parent=self.config.base, child=joint.child).hmatrix + # return DHParameters.from_matrix(dh_matrix) + # DHParameters.from_matrix() + # match joint.joint.type: + # case JointType.PRISMATIC: + + # case JointType.REVOLUTE: + # pass + # case JointType.FIXED: + # pass + # case _: + # pass + def _iteration_row(self) -> list: return [ self.get_transformation(parent=self.config.base, child=transformation.child).pose.to_list() diff --git a/test/test_types.py b/test/test_types.py index a44beaf..7d09ca5 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -3,6 +3,7 @@ import random import yaml from ktree.k_types import ( + DHParameters, Joint, JointAxis, JointType, @@ -72,6 +73,34 @@ def test_rotation() -> None: assert Rotation(rpy=[1.2, 1.3, 1.4]) == Rotation(rpy=[1.2, 1.3, 1.4]) +def test_transformation() -> None: + x = 100.0 + y = 200.0 + z = 300.0 + rx = 10.0 + ry = 20.0 + rz = 30.0 + + trans = Vector(vector=[x, y, z]) + rot = Rotation(rpy=[rx, ry, rz]) + logger.info(trans) + logger.info(rot) + p = Pose(translation=Vector(vector=[x, y, z]), rotation=Rotation(rpy=[rx, ry, rz])) + logger.info(p) + + t = Transformation(parent="A", child="B", pose=Pose().from_list([x, y, z, rx, ry, rz], mm_deg=True)) + t2 = Transformation( + parent="A", + child="B", + pose=Pose( + translation=Vector(vector=np.array([x, y, z]) / 1000), rotation=Rotation(rpy=np.radians([rx, ry, rz])) + ), + ) + + np.testing.assert_allclose(t.pose.to_list(mm_deg=True), [x, y, z, rx, ry, rz]) + np.testing.assert_allclose(t2.pose.to_list(mm_deg=True), [x, y, z, rx, ry, rz]) + + def test_mult() -> None: # random 3x3 orthonormal matrix rotation1 = Rotation(rpy=[0.12, 0.23, 0.34]) @@ -252,17 +281,39 @@ def test_quaternion() -> None: r = R.from_euler("xyz", [10, 20, 30], degrees=True) q = r.as_quat() assert np.allclose(q, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).quaternion.vector) - + def test_axis_angle() -> None: r = R.from_euler("xyz", [10, 20, 30], degrees=True) q = r.as_rotvec() axis = q / np.linalg.norm(q) angle = np.linalg.norm(q) - - assert np.allclose(axis, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).axis_angle[0].vector) - + + assert np.allclose(axis, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).axis_angle.axis.vector) + assert np.allclose(angle, Rotation(rpy=[np.radians(10), np.radians(20), np.radians(30)]).axis_angle.angle) + assert np.allclose(0.0, Rotation().axis_angle.angle) + + def test_mangnitude() -> None: - rpy = [10, 22.1234, 30] - r = R.from_euler("xyz", rpy, degrees=True) - assert np.isclose(r.magnitude(), Rotation(rpy=rpy, degrees=True).magnitude) + rpy = np.radians([10, 22.1234, 30]) + r = R.from_euler("xyz", rpy) + assert np.isclose(r.magnitude(), Rotation(rpy=rpy).magnitude) + + +def test_dh_params() -> None: + a, alpha, d, theta = 1.0, np.radians(10.0), 2.0, np.radians(20.0) + Tx = Transformation( + pose=Pose(translation=Vector(vector=[a, 0.0, 0.0]), rotation=Rotation()), parent="base", child="a" + ) + Rx = Transformation( + pose=Pose(translation=Vector(), rotation=Rotation(rpy=[alpha, 0.0, 0.0])), parent="a", child="b" + ) + Tz = Transformation(pose=Pose(translation=Vector(vector=[0.0, 0.0, d]), rotation=Rotation()), parent="b", child="c") + Rz = Transformation( + pose=Pose(translation=Vector(), rotation=Rotation(rpy=[0.0, 0.0, theta])), parent="c", child="d" + ) + + DH = Tx * Rx * Tz * Rz + parameters = DHParameters.from_matrix(DH.hmatrix) + + np.testing.assert_allclose(parameters.to_list(), [a, alpha, d, theta], atol=1e-5) From 999a9662c05d276e2595c54a52a0d6faf15899aa Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Fri, 28 Jun 2024 18:48:36 +0200 Subject: [PATCH 14/20] Add wip parameter jacobian --- src/ktree/k_types.py | 12 +++++++++ src/ktree/ktree.py | 58 +++++++++++++++++++++++++++++++++----------- test/test_types.py | 15 ++++++++++++ 3 files changed, 71 insertions(+), 14 deletions(-) diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index b9e000a..ce63905 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -112,6 +112,18 @@ def __eq__(self, other: object) -> bool: raise ValueError(f"Cannot compare Vector with {other}") else: raise NotImplementedError(f"Cannot compare Vector with {other}") + + @staticmethod + def unit_x() -> "Vector": + return Vector(vector=np.array([1.0, 0.0, 0.0])) + + @staticmethod + def unit_y() -> "Vector": + return Vector(vector=np.array([0.0, 1.0, 0.0])) + + @staticmethod + def unit_z() -> "Vector": + return Vector(vector=np.array([0.0, 0.0, 1.0])) class JointType(str, Enum): diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 1280c68..e33432b 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -126,6 +126,9 @@ def get_transformation(self, parent: str, child: str) -> Transformation: ) return total_transformation + + def get_end_effector(self) -> Transformation: + return self.get_transformation(parent=self.config.base, child=self.config.end_effector) def _get_jacobian(self) -> NDArray: jacobian = np.zeros((6, self._n_actuated_joints)) @@ -243,20 +246,47 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: def _parameter_jacobian(self) -> NDArray: return np.array([]) - # def _get_dh_parameters(self) -> DHParameters: - # for joint in self._actuated_joints: - # dh_matrix = self.get_transformation(parent=self.config.base, child=joint.child).hmatrix - # return DHParameters.from_matrix(dh_matrix) - # DHParameters.from_matrix() - # match joint.joint.type: - # case JointType.PRISMATIC: - - # case JointType.REVOLUTE: - # pass - # case JointType.FIXED: - # pass - # case _: - # pass + def _get_dh_parameters(self) -> DHParameters: + parent_frame = self.config.base + for joint in self._actuated_joints: + dh_matrix = self.get_transformation(parent=parent_frame, child=joint.child).hmatrix + parent_frame = joint.child + dh_params = DHParameters.from_matrix(dh_matrix) + logger.info(f"DH Params {dh_params} joint {joint.child.upper()}") + + def _get_parameter_jacobian(self) -> NDArray: + + def di(joint: str) -> Vector: + Ri = self.get_transformation(parent=self.config.base, child=joint).pose.rotation + pi = self.get_transformation(parent=joint, child=self.config.end_effector).pose.translation + + return Ri * pi + + jacobian_a = np.zeros((3, self._n_actuated_joints)) + jacobian_d = np.zeros((3, self._n_actuated_joints)) + jacobian_alpha = np.zeros((3, self._n_actuated_joints)) + jacobian_theta = np.zeros((3, self._n_actuated_joints)) + + joint_world_transforms = [self.get_transformation(parent=self.config.base, child=joint.child) for joint in self._actuated_joints] + + for joint_index, joint in enumerate(joint_world_transforms): + if joint_index == 0: + jacobian_a[:, joint_index] = Vector.unit_x().vector + jacobian_alpha[:, joint_index] = (Vector.unit_x() @ self.get_end_effector().pose.translation).vector + else: + jacobian_a[:, joint_index] = (joint_world_transforms[joint_index-1].pose.rotation * Vector.unit_x()).vector + jacobian_alpha[:, joint_index] = ((joint_world_transforms[joint_index-1].pose.rotation * Vector.unit_x()) @ di(joint.parent)).vector + + jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector + jacobian_theta[:, joint_index] = ((joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child)).vector + + + logger.opt(raw=True).info(f"Jacobian A \n{jacobian_a}\n") + logger.opt(raw=True).info(f"Jacobian D \n{jacobian_d}\n") + logger.opt(raw=True).info(f"Jacobian Alpha \n{jacobian_alpha}\n") + logger.opt(raw=True).info(f"Jacobian Theta \n{jacobian_theta}\n") + + return np.vstack((jacobian_a, jacobian_d, jacobian_alpha, jacobian_theta)) def _iteration_row(self) -> list: return [ diff --git a/test/test_types.py b/test/test_types.py index 7d09ca5..bfce4fd 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -317,3 +317,18 @@ def test_dh_params() -> None: parameters = DHParameters.from_matrix(DH.hmatrix) np.testing.assert_allclose(parameters.to_list(), [a, alpha, d, theta], atol=1e-5) + +def test_dh_params_from_model() -> None: + kc = KinematicsConfig.parse(Path("./test/roc-x.yaml")) + kt = KinematicsTree(config=kc) + kt.update_joints_from_list([0.10, 0.1, 0.2]) + + dhp = kt._get_dh_parameters() + +def test_parameter_jacobian() -> None: + kc = KinematicsConfig.parse(Path("./test/roc-x.yaml")) + kt = KinematicsTree(config=kc) + kt.update_joints_from_list([10.0, 20.0, 30.0], mm_deg=True) + logger.info(kt.get_joint_values()) + kt._get_parameter_jacobian() + pass \ No newline at end of file From 3d2b7181280290113b1796ef5a8052a328498079 Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Mon, 1 Jul 2024 13:30:22 +0200 Subject: [PATCH 15/20] Working modified DHparameters jacobian --- src/ktree/k_types.py | 129 ++++++++++++++++++++++++++++++++++++++----- src/ktree/ktree.py | 52 ++++++++++------- test/test_types.py | 28 +++++++--- 3 files changed, 168 insertions(+), 41 deletions(-) diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index ce63905..8012120 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -112,15 +112,15 @@ def __eq__(self, other: object) -> bool: raise ValueError(f"Cannot compare Vector with {other}") else: raise NotImplementedError(f"Cannot compare Vector with {other}") - + @staticmethod def unit_x() -> "Vector": return Vector(vector=np.array([1.0, 0.0, 0.0])) - + @staticmethod def unit_y() -> "Vector": return Vector(vector=np.array([0.0, 1.0, 0.0])) - + @staticmethod def unit_z() -> "Vector": return Vector(vector=np.array([0.0, 0.0, 1.0])) @@ -338,22 +338,119 @@ class JointAxis(str, Enum): Z = "z" +class DHType(str, Enum): + STANDARD = "standard" + MODIFIED = "modified" + HAYATI = "hayati" + + class DHParameters(BaseModel): model_config = ConfigDict(arbitrary_types_allowed=True, validate_assignment=True) - a: float = Field(default=0.0, description="Distance between z_i-1 and z_i along x_i") - alpha: float = Field(default=0.0, description="Angle between z_i-1 and z_i along x_i") - d: float = Field(default=0.0, description="Distance between x_i-1 and x_i along z_i-1") - theta: float = Field(default=0.0, description="Angle between x_i-1 and x_i along z_i-1") + a: float = Field(default=0.0) + alpha: float = Field(default=0.0) + d: float = Field(default=0.0) + theta: float = Field(default=0.0) + beta: float = Field(default=0.0) + dhtype: DHType = Field(default=DHType.MODIFIED) @staticmethod - def from_matrix(matrix: NDArray) -> "DHParameters": - return DHParameters( - a=matrix[0, 3], - alpha=np.arccos(matrix[2, 2]), - d=matrix[2, 3] / matrix[2, 2], - theta=np.arccos(matrix[0, 0]), + def from_matrix(matrix: NDArray, dhtype: DHType = DHType.MODIFIED) -> "DHParameters": + match dhtype: + case DHType.STANDARD: + return DHParameters( + a=matrix[0, 3] / matrix[0, 0], + alpha=np.arccos(matrix[2, 2]), + d=matrix[2, 3], + theta=np.arccos(matrix[0, 0]), + dhtype=dhtype, + ) + case DHType.MODIFIED: + return DHParameters( + a=matrix[0, 3], + alpha=np.arccos(matrix[2, 2]), + d=matrix[2, 3] / matrix[2, 2], + theta=np.arccos(matrix[0, 0]), + dhtype=dhtype, + ) + case DHType.HAYATI: + beta = np.arctan2(matrix[0, 2], matrix[2, 2]) + return DHParameters( + a=matrix[0, 3] / np.cos(beta), + alpha=-np.arcsin(matrix[1, 2]), + theta=np.arctan2(matrix[1, 0], matrix[1, 1]), + beta=beta, + dhtype=dhtype, + ) + case _: + raise ValueError(f"Invalid DH type {type}") + + def matrix(self) -> NDArray: + match self.dhtype: + case DHType.STANDARD: + return self._standard_matrix() + case DHType.MODIFIED: + return self._modified_matrix() + case DHType.HAYATI: + return self._hayati_matrix() + case _: + raise ValueError(f"Invalid DH type {type}") + + def _standard_matrix(self) -> NDArray: + matrix = np.eye(4) + matrix[0, 0] = np.cos(self.theta) + matrix[0, 1] = -np.sin(self.theta) * np.cos(self.alpha) + matrix[0, 2] = np.sin(self.theta) * np.sin(self.alpha) + matrix[0, 3] = self.a * np.cos(self.theta) + matrix[1, 0] = np.sin(self.theta) + matrix[1, 1] = np.cos(self.theta) * np.cos(self.alpha) + matrix[1, 2] = -np.cos(self.theta) * np.sin(self.alpha) + matrix[1, 3] = self.a * np.sin(self.theta) + matrix[2, 1] = np.sin(self.alpha) + matrix[2, 2] = np.cos(self.alpha) + matrix[2, 3] = self.d + + return matrix + + def _modified_matrix(self) -> NDArray: + matrix = np.eye(4) + matrix[0, 0] = np.cos(self.theta) + matrix[0, 1] = -np.sin(self.theta) + matrix[0, 3] = self.a + matrix[1, 0] = np.sin(self.theta) * np.cos(self.alpha) + matrix[1, 1] = np.cos(self.theta) * np.cos(self.alpha) + matrix[1, 2] = -np.sin(self.alpha) + matrix[1, 3] = -self.d * np.sin(self.alpha) + matrix[2, 0] = np.sin(self.theta) * np.sin(self.alpha) + matrix[2, 1] = np.cos(self.theta) * np.sin(self.alpha) + matrix[2, 2] = np.cos(self.alpha) + matrix[2, 3] = self.d * np.cos(self.alpha) + + return matrix + + def _hayati_matrix(self) -> NDArray: + matrix = np.eye(4) + matrix[0, 0] = np.sin(self.alpha) * np.sin(self.beta) * np.sin(self.theta) + np.cos(self.beta) * np.cos( + self.theta + ) + matrix[0, 1] = np.sin(self.alpha) * np.sin(self.beta) * np.cos(self.theta) - np.sin(self.theta) * np.cos( + self.beta + ) + matrix[0, 2] = np.sin(self.beta) * np.cos(self.alpha) + matrix[0, 3] = self.a * np.cos(self.beta) + matrix[1, 0] = np.sin(self.theta) * np.cos(self.alpha) + matrix[1, 1] = np.cos(self.alpha) * np.cos(self.theta) + matrix[1, 2] = -np.sin(self.alpha) + matrix[2, 0] = np.sin(self.alpha) * np.sin(self.theta) * np.cos(self.beta) - np.sin(self.beta) * np.cos( + self.theta + ) + matrix[2, 1] = np.sin(self.alpha) * np.cos(self.beta) * np.cos(self.theta) + np.sin(self.beta) * np.sin( + self.theta ) + matrix[2, 2] = np.cos(self.alpha) * np.cos(self.beta) + matrix[2, 3] = -self.a * np.sin(self.beta) + + return matrix def to_list(self) -> list[float]: return [self.a, self.alpha, self.d, self.theta] @@ -364,6 +461,12 @@ def __eq__(self, other: object) -> bool: else: raise NotImplementedError(f"Cannot compare DHParameters with {other}") + def __str__(self) -> str: + return ( + f"a: {self.a:.3g} m, alpha: {self.alpha:.3g} rad, d: {self.d:.3g} m, theta: {self.theta:.3g} rad, beta:" + f" {self.beta:.3g} rad" + ) + class Joint(BaseModel): type: JointType = Field(default=JointType.FIXED, description="Degree of freedom type of the joint") diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index e33432b..76310c0 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -1,7 +1,7 @@ import networkx as nx import numpy as np import pandas as pd -from ktree.k_types import DHParameters, JointType, Pose, Transformation, Vector +from ktree.k_types import DHParameters, DHType, JointType, Pose, Transformation, Vector from ktree.models import KinematicsConfig from loguru import logger from numpy.typing import NDArray @@ -126,7 +126,7 @@ def get_transformation(self, parent: str, child: str) -> Transformation: ) return total_transformation - + def get_end_effector(self) -> Transformation: return self.get_transformation(parent=self.config.base, child=self.config.end_effector) @@ -246,46 +246,58 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: def _parameter_jacobian(self) -> NDArray: return np.array([]) - def _get_dh_parameters(self) -> DHParameters: + def _get_dh_parameters(self) -> list[DHParameters]: + # for dhtype in DHType: + dhtype = DHType.MODIFIED parent_frame = self.config.base - for joint in self._actuated_joints: + list_dh_params: list[DHParameters] = [DHParameters()] * self._n_actuated_joints + for index, joint in enumerate(self._actuated_joints): dh_matrix = self.get_transformation(parent=parent_frame, child=joint.child).hmatrix + dh_params = DHParameters.from_matrix(matrix=dh_matrix, dhtype=dhtype) + logger.info(f"DH Params {dhtype} {dh_params} joint {parent_frame.upper()} -> {joint.child.upper()}") parent_frame = joint.child - dh_params = DHParameters.from_matrix(dh_matrix) - logger.info(f"DH Params {dh_params} joint {joint.child.upper()}") - + list_dh_params[index] = dh_params + + return list_dh_params + def _get_parameter_jacobian(self) -> NDArray: - def di(joint: str) -> Vector: Ri = self.get_transformation(parent=self.config.base, child=joint).pose.rotation pi = self.get_transformation(parent=joint, child=self.config.end_effector).pose.translation - return Ri * pi - + return Ri * pi + jacobian_a = np.zeros((3, self._n_actuated_joints)) jacobian_d = np.zeros((3, self._n_actuated_joints)) jacobian_alpha = np.zeros((3, self._n_actuated_joints)) jacobian_theta = np.zeros((3, self._n_actuated_joints)) - - joint_world_transforms = [self.get_transformation(parent=self.config.base, child=joint.child) for joint in self._actuated_joints] - + + joint_world_transforms = [ + self.get_transformation(parent=self.config.base, child=joint.child) for joint in self._actuated_joints + ] + for joint_index, joint in enumerate(joint_world_transforms): if joint_index == 0: jacobian_a[:, joint_index] = Vector.unit_x().vector jacobian_alpha[:, joint_index] = (Vector.unit_x() @ self.get_end_effector().pose.translation).vector else: - jacobian_a[:, joint_index] = (joint_world_transforms[joint_index-1].pose.rotation * Vector.unit_x()).vector - jacobian_alpha[:, joint_index] = ((joint_world_transforms[joint_index-1].pose.rotation * Vector.unit_x()) @ di(joint.parent)).vector - + jacobian_a[:, joint_index] = ( + joint_world_transforms[joint_index - 1].pose.rotation * Vector.unit_x() + ).vector + jacobian_alpha[:, joint_index] = ( + (joint_world_transforms[joint_index - 1].pose.rotation * Vector.unit_x()) @ di(joint.parent) + ).vector + jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector - jacobian_theta[:, joint_index] = ((joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child)).vector - - + jacobian_theta[:, joint_index] = ( + (joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child) + ).vector + logger.opt(raw=True).info(f"Jacobian A \n{jacobian_a}\n") logger.opt(raw=True).info(f"Jacobian D \n{jacobian_d}\n") logger.opt(raw=True).info(f"Jacobian Alpha \n{jacobian_alpha}\n") logger.opt(raw=True).info(f"Jacobian Theta \n{jacobian_theta}\n") - + return np.vstack((jacobian_a, jacobian_d, jacobian_alpha, jacobian_theta)) def _iteration_row(self) -> list: diff --git a/test/test_types.py b/test/test_types.py index bfce4fd..214833a 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -318,17 +318,29 @@ def test_dh_params() -> None: np.testing.assert_allclose(parameters.to_list(), [a, alpha, d, theta], atol=1e-5) + def test_dh_params_from_model() -> None: - kc = KinematicsConfig.parse(Path("./test/roc-x.yaml")) + kc = KinematicsConfig.parse(Path("./test/2dof_rev.yaml")) kt = KinematicsTree(config=kc) - kt.update_joints_from_list([0.10, 0.1, 0.2]) - + logger.info(kt.get_end_effector().hmatrix) + dhp = kt._get_dh_parameters() - + + expected_params = [ + DHParameters(a=0.150, alpha=0.1, d=0.0, theta=0.0), + DHParameters(a=0.250, alpha=0.2, d=0.0, theta=0.0), + ] + + for param, expected_param in zip(dhp, expected_params): + assert param == expected_param + + def test_parameter_jacobian() -> None: - kc = KinematicsConfig.parse(Path("./test/roc-x.yaml")) + kc = KinematicsConfig.parse(Path("./test/2dof_rev.yaml")) kt = KinematicsTree(config=kc) - kt.update_joints_from_list([10.0, 20.0, 30.0], mm_deg=True) + logger.info(kt.get_end_effector().pose.translation) + kt.update_joints_from_list([0.1, 0.2]) logger.info(kt.get_joint_values()) - kt._get_parameter_jacobian() - pass \ No newline at end of file + print(kt._get_parameter_jacobian()) + + pass From b8eed5207c748984775ea5617800e1759288c88c Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Mon, 1 Jul 2024 16:24:11 +0200 Subject: [PATCH 16/20] Fix typo in parameter jacobian --- src/ktree/k_types.py | 9 ++++++++- src/ktree/ktree.py | 21 ++++++++++++++++----- test/test_types.py | 5 ++++- 3 files changed, 28 insertions(+), 7 deletions(-) diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index 8012120..b7da3c7 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -550,7 +550,14 @@ def _pose_validator(cls, v: Pose | NDArray[np.float_] | list[float] | dict[str, case list() | np.ndarray(): return Pose.from_list(v) case dict(): - pose_dict = dict() + pose_dict = dict({ + X + M_SUFFIX: 0.0, + Y + M_SUFFIX: 0.0, + Z + M_SUFFIX: 0.0, + RX + RAD_SUFFIX: 0.0, + RY + RAD_SUFFIX: 0.0, + RZ + RAD_SUFFIX: 0.0, + }) for key, value in v.items(): if key.endswith(MM_SUFFIX): pose_dict[key.replace(MM_SUFFIX, M_SUFFIX)] = value / 1000 diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 76310c0..3cdf26f 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -275,23 +275,34 @@ def di(joint: str) -> Vector: joint_world_transforms = [ self.get_transformation(parent=self.config.base, child=joint.child) for joint in self._actuated_joints ] + joints_type = [joint.joint.type for joint in self._actuated_joints] - for joint_index, joint in enumerate(joint_world_transforms): + for joint_index, (joint, joint_type) in enumerate(zip(joint_world_transforms, joints_type)): if joint_index == 0: jacobian_a[:, joint_index] = Vector.unit_x().vector jacobian_alpha[:, joint_index] = (Vector.unit_x() @ self.get_end_effector().pose.translation).vector + joint_parent = joint.child else: jacobian_a[:, joint_index] = ( joint_world_transforms[joint_index - 1].pose.rotation * Vector.unit_x() ).vector jacobian_alpha[:, joint_index] = ( - (joint_world_transforms[joint_index - 1].pose.rotation * Vector.unit_x()) @ di(joint.parent) + (joint_world_transforms[joint_index - 1].pose.rotation * Vector.unit_x()) @ di(joint_parent) ).vector - - jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector - jacobian_theta[:, joint_index] = ( + joint_parent = joint.child + + # jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector + # jacobian_theta[:, joint_index] = ( + # (joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child) + # ).vector + + match joint_type: + case JointType.PRISMATIC: + jacobian_theta[:, joint_index] = ( (joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child) ).vector + case JointType.REVOLUTE: + jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector logger.opt(raw=True).info(f"Jacobian A \n{jacobian_a}\n") logger.opt(raw=True).info(f"Jacobian D \n{jacobian_d}\n") diff --git a/test/test_types.py b/test/test_types.py index 214833a..9db9f82 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -338,8 +338,11 @@ def test_dh_params_from_model() -> None: def test_parameter_jacobian() -> None: kc = KinematicsConfig.parse(Path("./test/2dof_rev.yaml")) kt = KinematicsTree(config=kc) + kt._get_dh_parameters() + + + kt.update_joints_from_list([0.1, 0.2, 0.3]) logger.info(kt.get_end_effector().pose.translation) - kt.update_joints_from_list([0.1, 0.2]) logger.info(kt.get_joint_values()) print(kt._get_parameter_jacobian()) From a1601a41bfb8d0e44377d4aaf3e311358ccf2c2c Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Mon, 1 Jul 2024 16:56:14 +0200 Subject: [PATCH 17/20] fix horizontal concat paramter jacobian --- src/ktree/ktree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 3cdf26f..98c87da 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -309,7 +309,7 @@ def di(joint: str) -> Vector: logger.opt(raw=True).info(f"Jacobian Alpha \n{jacobian_alpha}\n") logger.opt(raw=True).info(f"Jacobian Theta \n{jacobian_theta}\n") - return np.vstack((jacobian_a, jacobian_d, jacobian_alpha, jacobian_theta)) + return np.hstack((jacobian_a, jacobian_d, jacobian_alpha, jacobian_theta)) def _iteration_row(self) -> list: return [ From aade81f7934e69563f8762a3fff9ed800947ff1b Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Tue, 2 Jul 2024 11:56:29 +0200 Subject: [PATCH 18/20] Add a link to unit test --- src/ktree/ktree.py | 10 +++++----- test/test_types.py | 5 +++-- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 98c87da..906e971 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -254,7 +254,7 @@ def _get_dh_parameters(self) -> list[DHParameters]: for index, joint in enumerate(self._actuated_joints): dh_matrix = self.get_transformation(parent=parent_frame, child=joint.child).hmatrix dh_params = DHParameters.from_matrix(matrix=dh_matrix, dhtype=dhtype) - logger.info(f"DH Params {dhtype} {dh_params} joint {parent_frame.upper()} -> {joint.child.upper()}") + logger.debug(f"DH Params {dhtype} {dh_params} joint {parent_frame.upper()} -> {joint.child.upper()}") parent_frame = joint.child list_dh_params[index] = dh_params @@ -304,10 +304,10 @@ def di(joint: str) -> Vector: case JointType.REVOLUTE: jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector - logger.opt(raw=True).info(f"Jacobian A \n{jacobian_a}\n") - logger.opt(raw=True).info(f"Jacobian D \n{jacobian_d}\n") - logger.opt(raw=True).info(f"Jacobian Alpha \n{jacobian_alpha}\n") - logger.opt(raw=True).info(f"Jacobian Theta \n{jacobian_theta}\n") + logger.opt(raw=True).debug(f"Jacobian A \n{jacobian_a}\n") + logger.opt(raw=True).debug(f"Jacobian D \n{jacobian_d}\n") + logger.opt(raw=True).debug(f"Jacobian Alpha \n{jacobian_alpha}\n") + logger.opt(raw=True).debug(f"Jacobian Theta \n{jacobian_theta}\n") return np.hstack((jacobian_a, jacobian_d, jacobian_alpha, jacobian_theta)) diff --git a/test/test_types.py b/test/test_types.py index 9db9f82..9479451 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -327,8 +327,9 @@ def test_dh_params_from_model() -> None: dhp = kt._get_dh_parameters() expected_params = [ - DHParameters(a=0.150, alpha=0.1, d=0.0, theta=0.0), - DHParameters(a=0.250, alpha=0.2, d=0.0, theta=0.0), + DHParameters(a=0.150, alpha=0.1, d=0.1, theta=0.0), + DHParameters(a=0.250, alpha=0.2, d=0.1, theta=0.0), + DHParameters(a=0.200, alpha=0.3, d=0.1, theta=0.0), ] for param, expected_param in zip(dhp, expected_params): From 7824d94342ed11a539bfe608f82f1438cd7b62f6 Mon Sep 17 00:00:00 2001 From: Daniel Lemus Date: Tue, 2 Jul 2024 16:44:56 +0200 Subject: [PATCH 19/20] add rounding on dh parameter estimation --- src/ktree/k_types.py | 23 +++++++++++++++-------- src/ktree/ktree.py | 6 +++--- test/test_types.py | 8 +++++--- 3 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/ktree/k_types.py b/src/ktree/k_types.py index b7da3c7..5c8757f 100644 --- a/src/ktree/k_types.py +++ b/src/ktree/k_types.py @@ -354,6 +354,11 @@ class DHParameters(BaseModel): beta: float = Field(default=0.0) dhtype: DHType = Field(default=DHType.MODIFIED) + @field_validator("a", "alpha", "d", "theta", "beta", mode="after") + @classmethod + def round_parameters(cls, v: float) -> float: + return round(v, 8) + @staticmethod def from_matrix(matrix: NDArray, dhtype: DHType = DHType.MODIFIED) -> "DHParameters": match dhtype: @@ -550,14 +555,16 @@ def _pose_validator(cls, v: Pose | NDArray[np.float_] | list[float] | dict[str, case list() | np.ndarray(): return Pose.from_list(v) case dict(): - pose_dict = dict({ - X + M_SUFFIX: 0.0, - Y + M_SUFFIX: 0.0, - Z + M_SUFFIX: 0.0, - RX + RAD_SUFFIX: 0.0, - RY + RAD_SUFFIX: 0.0, - RZ + RAD_SUFFIX: 0.0, - }) + pose_dict = dict( + { + X + M_SUFFIX: 0.0, + Y + M_SUFFIX: 0.0, + Z + M_SUFFIX: 0.0, + RX + RAD_SUFFIX: 0.0, + RY + RAD_SUFFIX: 0.0, + RZ + RAD_SUFFIX: 0.0, + } + ) for key, value in v.items(): if key.endswith(MM_SUFFIX): pose_dict[key.replace(MM_SUFFIX, M_SUFFIX)] = value / 1000 diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 906e971..33093e6 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -290,7 +290,7 @@ def di(joint: str) -> Vector: (joint_world_transforms[joint_index - 1].pose.rotation * Vector.unit_x()) @ di(joint_parent) ).vector joint_parent = joint.child - + # jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector # jacobian_theta[:, joint_index] = ( # (joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child) @@ -299,8 +299,8 @@ def di(joint: str) -> Vector: match joint_type: case JointType.PRISMATIC: jacobian_theta[:, joint_index] = ( - (joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child) - ).vector + (joint_world_transforms[joint_index].pose.rotation * Vector.unit_z()) @ di(joint.child) + ).vector case JointType.REVOLUTE: jacobian_d[:, joint_index] = (joint.pose.rotation * Vector.unit_z()).vector diff --git a/test/test_types.py b/test/test_types.py index 9479451..ac4560d 100644 --- a/test/test_types.py +++ b/test/test_types.py @@ -301,7 +301,7 @@ def test_mangnitude() -> None: def test_dh_params() -> None: - a, alpha, d, theta = 1.0, np.radians(10.0), 2.0, np.radians(20.0) + a, alpha, d, theta = 0.1234, np.radians(10.0), 20.5345, np.radians(20.0) Tx = Transformation( pose=Pose(translation=Vector(vector=[a, 0.0, 0.0]), rotation=Rotation()), parent="base", child="a" ) @@ -318,6 +318,9 @@ def test_dh_params() -> None: np.testing.assert_allclose(parameters.to_list(), [a, alpha, d, theta], atol=1e-5) + parameters2 = DHParameters(a=a, alpha=alpha, d=d, theta=theta) + np.testing.assert_allclose(parameters2.matrix(), DH.hmatrix, atol=1e-5) + def test_dh_params_from_model() -> None: kc = KinematicsConfig.parse(Path("./test/2dof_rev.yaml")) @@ -340,8 +343,7 @@ def test_parameter_jacobian() -> None: kc = KinematicsConfig.parse(Path("./test/2dof_rev.yaml")) kt = KinematicsTree(config=kc) kt._get_dh_parameters() - - + kt.update_joints_from_list([0.1, 0.2, 0.3]) logger.info(kt.get_end_effector().pose.translation) logger.info(kt.get_joint_values()) From 04dc21055f83e57725cb14aeec6955bc865f2d60 Mon Sep 17 00:00:00 2001 From: Daniel Lemus Perez Date: Fri, 18 Oct 2024 14:22:40 +0200 Subject: [PATCH 20/20] wip --- src/ktree/ktree.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/ktree/ktree.py b/src/ktree/ktree.py index 33093e6..4962747 100644 --- a/src/ktree/ktree.py +++ b/src/ktree/ktree.py @@ -209,13 +209,11 @@ def inverse_kinematics(self, target_effector: Transformation) -> pd.DataFrame: target_effector.child = "target_effector" start_pose = self.get_transformation(parent=self.config.base, child=self.config.end_effector) logger.debug(f"Starting pose: {start_pose}") - # delta_pose = start_pose.inv() * target_effector - delta_pose = np.array(target_effector.pose.to_list()) - np.array(start_pose.pose.to_list()) + pose_tol = np.array([1e-7] * 3 + [1e-7] * 3) iter = 0 - dx = np.linalg.norm(delta_pose) * delta_pose * 0.005 - # dx = np.linalg.norm(np.array(delta_pose.pose.to_list())) * np.array(delta_pose.pose.to_list()) * 0.005 + dx = (np.array(target_effector.pose.to_list()) - start_pose.pose.to_list()) * ERROR_SCALE iterations = [self._iteration_row()] while True: if all(abs(dx) < pose_tol):