diff --git a/.idea/GrepConsole.xml b/.idea/GrepConsole.xml index 0e1af88..dd20438 100644 --- a/.idea/GrepConsole.xml +++ b/.idea/GrepConsole.xml @@ -66,6 +66,37 @@ + + + + + + + + + + + diff --git a/Cargo.lock b/Cargo.lock index 90da046..d442817 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -23,17 +23,11 @@ dependencies = [ "memchr", ] -[[package]] -name = "allocator-api2" -version = "0.2.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c6cb57a04249c6480766f7f7cef5467412af1490f8d1e243141daddada3264f" - [[package]] name = "anstream" -version = "0.6.14" +version = "0.6.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "418c75fa768af9c03be99d17643f93f79bbba589895012a80e3452a19ddda15b" +checksum = "64e15c1ab1f89faffbf04a634d5e1962e9074f2741eef6d97f3c4e322426d526" dependencies = [ "anstyle", "anstyle-parse", @@ -46,33 +40,33 @@ dependencies = [ [[package]] name = "anstyle" -version = "1.0.7" +version = "1.0.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "038dfcf04a5feb68e9c60b21c9625a54c2c0616e79b72b0fd87075a056ae1d1b" +checksum = "1bec1de6f59aedf83baf9ff929c98f2ad654b97c9510f4e70cf6f661d49fd5b1" [[package]] name = "anstyle-parse" -version = "0.2.4" +version = "0.2.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c03a11a9034d92058ceb6ee011ce58af4a9bf61491aa7e1e59ecd24bd40d22d4" +checksum = "eb47de1e80c2b463c735db5b217a0ddc39d612e7ac9e2e96a5aed1f57616c1cb" dependencies = [ "utf8parse", ] [[package]] name = "anstyle-query" -version = "1.0.3" +version = "1.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a64c907d4e79225ac72e2a354c9ce84d50ebb4586dee56c82b3ee73004f537f5" +checksum = "6d36fc52c7f6c869915e99412912f22093507da8d9e942ceaf66fe4b7c14422a" dependencies = [ "windows-sys", ] [[package]] name = "anstyle-wincon" -version = "3.0.3" +version = "3.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61a38449feb7068f52bb06c12759005cf459ee52bb4adc1d5a7c4322d716fb19" +checksum = "5bf74e1b6e971609db8ca7a9ce79fd5768ab6ae46441c572e46cf596f59e57f8" dependencies = [ "anstyle", "windows-sys", @@ -95,15 +89,21 @@ checksum = "7d902e3d592a523def97af8f317b08ce16b7ab854c1985a0c671e6f15cebc236" [[package]] name = "autocfg" -version = "1.2.0" +version = "1.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" +checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" [[package]] name = "bytemuck" -version = "1.15.0" +version = "1.18.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" +checksum = "94bbb0ad554ad961ddc5da507a12a29b14e4ae5bda06b19f575a3e6079d2e2ae" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "cfg-if" @@ -113,9 +113,9 @@ checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] name = "clap" -version = "4.5.4" +version = "4.5.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90bc066a67923782aa8515dbaea16946c5bcc5addbd668bb80af688e53e548a0" +checksum = "7be5744db7978a28d9df86a214130d106a89ce49644cbc4e3f0c22c3fba30615" dependencies = [ "clap_builder", "clap_derive", @@ -123,9 +123,9 @@ dependencies = [ [[package]] name = "clap_builder" -version = "4.5.2" +version = "4.5.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae129e2e766ae0ec03484e609954119f123cc1fe650337e155d03b022f24f7b4" +checksum = "a5fbc17d3ef8278f55b282b2a2e75ae6f6c7d4bb70ed3d0382375104bfafdb4b" dependencies = [ "anstream", "anstyle", @@ -135,27 +135,27 @@ dependencies = [ [[package]] name = "clap_derive" -version = "4.5.4" +version = "4.5.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "528131438037fd55894f62d6e9f068b8f45ac57ffa77517819645d10aed04f64" +checksum = "4ac6a0c7b1a9e9a5186361f67dfa1b88213572f427fb9ab038efb2bd8c582dab" dependencies = [ "heck", "proc-macro2", "quote", - "syn 2.0.57", + "syn", ] [[package]] name = "clap_lex" -version = "0.7.0" +version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "98cc8fbded0c607b7ba9dd60cd98df59af97e84d24e49c8557331cfc26d301ce" +checksum = "1462739cb27611015575c0c11df5df7601141071f07518d56fcc1be504cbec97" [[package]] name = "colorchoice" -version = "1.0.1" +version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b6a852b24ab71dffc585bcb46eaf7959d175cb865a7152e35b348d1b2960422" +checksum = "d3fd119d74b830634cea2a0f58bbd0d54540518a14397557951e79340abc28c0" [[package]] name = "encoding_rs" @@ -179,19 +179,18 @@ dependencies = [ [[package]] name = "hashbrown" -version = "0.14.3" +version = "0.14.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" dependencies = [ "ahash", - "allocator-api2", ] [[package]] name = "hashlink" -version = "0.8.4" +version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e8094feaf31ff591f651a2664fb9cfd92bba7a60ce3197265e9482ebe753c8f7" +checksum = "6ba4ff7128dee98c7dc9794b6a411377e1404dba1c97deb8d1a55297bd25d8af" dependencies = [ "hashbrown", ] @@ -204,21 +203,21 @@ checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" [[package]] name = "is_terminal_polyfill" -version = "1.70.0" +version = "1.70.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f8478577c03552c21db0e2724ffb8986a5ce7af88107e6be5d2ee6e158c12800" +checksum = "7943c866cc5cd64cbc25b2e01621d07fa8eb2a1a23160ee81ce38704e97b8ecf" [[package]] name = "libc" -version = "0.2.154" +version = "0.2.159" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" +checksum = "561d97a539a36e26a9a5fad1ea11a3039a67714694aaa379433e580854bc3dc5" [[package]] name = "matrixmultiply" -version = "0.3.8" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7574c1cf36da4798ab73da5b215bbf444f50718207754cb522201d78d1cd0ff2" +checksum = "9380b911e3e96d10c1f415da0876389aaf1b56759054eeb0de7df940c456ba1a" dependencies = [ "autocfg", "rawpointer", @@ -226,15 +225,15 @@ dependencies = [ [[package]] name = "memchr" -version = "2.7.2" +version = "2.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" +checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" [[package]] name = "nalgebra" -version = "0.32.6" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b5c17de023a86f59ed79891b2e5d5a94c705dbe904a5b5c9c952ea6221b03e4" +checksum = "3c4b5f057b303842cf3262c27e465f4c303572e7f6b0648f60e16248ac3397f4" dependencies = [ "approx", "matrixmultiply", @@ -248,20 +247,30 @@ dependencies = [ [[package]] name = "nalgebra-macros" -version = "0.2.1" +version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "91761aed67d03ad966ef783ae962ef9bbaca728d2dd7ceb7939ec110fffad998" +checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" dependencies = [ "proc-macro2", "quote", - "syn 1.0.109", + "syn", +] + +[[package]] +name = "num-bigint" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9" +dependencies = [ + "num-integer", + "num-traits", ] [[package]] name = "num-complex" -version = "0.4.5" +version = "0.4.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23c6602fda94a57c990fe0df199a035d83576b496aa29f4e634a8ac6004e68a6" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" dependencies = [ "num-traits", ] @@ -277,35 +286,35 @@ dependencies = [ [[package]] name = "num-rational" -version = "0.4.1" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0638a1c9d0a3c0914158145bc76cff373a75a627e6ecbfb71cbe6f453a5a19b0" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" dependencies = [ - "autocfg", + "num-bigint", "num-integer", "num-traits", ] [[package]] name = "num-traits" -version = "0.2.18" +version = "0.2.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", ] [[package]] name = "once_cell" -version = "1.19.0" +version = "1.20.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" +checksum = "1261fe7e33c73b354eab43b1273a57c8f967d0391e80353e51f764ac02cf6775" [[package]] name = "paste" -version = "1.0.14" +version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" [[package]] name = "peresil" @@ -315,24 +324,27 @@ checksum = "f658886ed52e196e850cfbbfddab9eaa7f6d90dd0929e264c31e5cec07e09e57" [[package]] name = "ppv-lite86" -version = "0.2.17" +version = "0.2.20" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" +checksum = "77957b295656769bb8ad2b6a6b09d897d94f05c41b069aede1fcdaa675eaea04" +dependencies = [ + "zerocopy", +] [[package]] name = "proc-macro2" -version = "1.0.79" +version = "1.0.86" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e835ff2298f5721608eb1a980ecaee1aef2c132bf95ecc026a11b7bf3c01c02e" +checksum = "5e719e8df665df0d1c8fbfd238015744736151d4445ec0836b8e628aae103b77" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.35" +version = "1.0.37" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef" +checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" dependencies = [ "proc-macro2", ] @@ -375,9 +387,9 @@ checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" [[package]] name = "regex" -version = "1.10.5" +version = "1.11.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b91213439dad192326a0d7c6ee3955910425f441d7038e0d6933b0aec5c4517f" +checksum = "38200e5ee88914975b69f657f0801b6f6dccafd44fd9326302a4aaeecfacb1d8" dependencies = [ "aho-corasick", "memchr", @@ -387,9 +399,9 @@ dependencies = [ [[package]] name = "regex-automata" -version = "0.4.6" +version = "0.4.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" +checksum = "368758f23274712b504848e9d5a6f010445cc8b87a7cdb4d7cbee666c1288da3" dependencies = [ "aho-corasick", "memchr", @@ -398,13 +410,13 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.8.3" +version = "0.8.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" +checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c" [[package]] name = "rs-opw-kinematics" -version = "1.4.0" +version = "1.4.2" dependencies = [ "clap", "nalgebra", @@ -416,18 +428,18 @@ dependencies = [ [[package]] name = "safe_arch" -version = "0.7.1" +version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f398075ce1e6a179b46f51bd88d0598b92b00d3551f1a2d4ac49e771b56ac354" +checksum = "c3460605018fdc9612bce72735cba0d27efbcd9904780d44c7e3a9948f96148a" dependencies = [ "bytemuck", ] [[package]] name = "simba" -version = "0.8.1" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "061507c94fc6ab4ba1c9a0305018408e312e17c041eb63bef8aa726fa33aceae" +checksum = "b3a386a501cd104797982c15ae17aafe8b9261315b5d07e3ec803f2ea26be0fa" dependencies = [ "approx", "num-complex", @@ -454,20 +466,9 @@ dependencies = [ [[package]] name = "syn" -version = "1.0.109" +version = "2.0.79" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" -dependencies = [ - "proc-macro2", - "quote", - "unicode-ident", -] - -[[package]] -name = "syn" -version = "2.0.57" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "11a6ae1e52eb25aab8f3fb9fca13be982a373b8f1157ca14b897a825ba4a2d35" +checksum = "89132cd0bf050864e1d38dc3bbc07a0eb8e7530af26344d3d2bbbef83499f590" dependencies = [ "proc-macro2", "quote", @@ -488,21 +489,21 @@ checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" [[package]] name = "unicode-ident" -version = "1.0.12" +version = "1.0.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" +checksum = "e91b56cd4cadaeb79bbf1a5645f6b4f8dc5bde8834ad5894a8db35fda9efa1fe" [[package]] name = "utf8parse" -version = "0.2.1" +version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "711b9620af191e0cdc7468a8d14e709c3dcdb115b36f838e601583af800a370a" +checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" [[package]] name = "version_check" -version = "0.9.4" +version = "0.9.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" [[package]] name = "wasi" @@ -512,9 +513,9 @@ checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" [[package]] name = "wide" -version = "0.7.15" +version = "0.7.28" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "89beec544f246e679fc25490e3f8e08003bc4bf612068f325120dad4cea02c1c" +checksum = "b828f995bf1e9622031f8009f8481a85406ce1f4d4588ff746d872043e855690" dependencies = [ "bytemuck", "safe_arch", @@ -531,9 +532,9 @@ dependencies = [ [[package]] name = "windows-targets" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" dependencies = [ "windows_aarch64_gnullvm", "windows_aarch64_msvc", @@ -547,57 +548,57 @@ dependencies = [ [[package]] name = "windows_aarch64_gnullvm" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" [[package]] name = "windows_aarch64_msvc" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" [[package]] name = "windows_i686_gnu" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" [[package]] name = "windows_i686_gnullvm" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" [[package]] name = "windows_i686_msvc" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" [[package]] name = "windows_x86_64_gnu" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" [[package]] name = "windows_x86_64_gnullvm" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" [[package]] name = "windows_x86_64_msvc" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" [[package]] name = "yaml-rust2" -version = "0.8.1" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8902160c4e6f2fb145dbe9d6760a75e3c9522d8bf796ed7047c85919ac7115f8" +checksum = "2a1a1c0bc9823338a3bdf8c61f994f23ac004c6fa32c08cd152984499b445e8d" dependencies = [ "arraydeque", "encoding_rs", @@ -606,20 +607,21 @@ dependencies = [ [[package]] name = "zerocopy" -version = "0.7.34" +version = "0.7.35" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae87e3fcd617500e5d106f0380cf7b77f3c6092aae37191433159dda23cfb087" +checksum = "1b9b4fd18abc82b8136838da5d50bae7bdea537c574d8dc1a34ed098d6c166f0" dependencies = [ + "byteorder", "zerocopy-derive", ] [[package]] name = "zerocopy-derive" -version = "0.7.34" +version = "0.7.35" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "15e934569e47891f7d9411f1a451d947a60e000ab3bd24fbb970f000387d1b3b" +checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" dependencies = [ "proc-macro2", "quote", - "syn 2.0.57", + "syn", ] diff --git a/Cargo.toml b/Cargo.toml index bbaf37a..795a631 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.4.0" +version = "1.4.2" edition = "2021" authors = ["Bourumir Wyngs "] description = "Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist." @@ -16,20 +16,20 @@ readme = "README.md" maintenance = { status = "actively-developed" } [dependencies] -nalgebra = "0.32.6" +nalgebra = "0.33" # Others are only needed to read YAML or convert from URDF -yaml-rust2 = { version = "0.8.1", optional = true } +yaml-rust2 = { version = "0.9", optional = true } sxd-document = { version = "0.3", optional = true } regex = { version = "1.10.5", optional = true } -clap = { version = "4.5.4", features = ["derive"], optional = true } +clap = { version = "4.5", features = ["derive"], optional = true } [features] default = ["allow_filesystem"] allow_filesystem = ["yaml-rust2", "sxd-document", "regex", "clap"] # To disable filesystem: -#rs-opw-kinematics = { version = "1.4.0", default-features = false } +#rs-opw-kinematics = { version = "1.4.2", default-features = false } [dev-dependencies] rand = "0.8.5" diff --git a/examples/basic.rs b/examples/basic.rs index 6b8c3bf..5db574c 100644 --- a/examples/basic.rs +++ b/examples/basic.rs @@ -6,27 +6,37 @@ use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; fn main() { let robot = OPWKinematics::new(Parameters::irb2400_10()); let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] - println!("Initial joints with singularity J5 = 0: "); + println!("\nInitial joints with singularity J5 = 0: "); dump_joints(&joints); - println!("Solutions (original angle set is lacking due singularity there: "); + println!("\nSolutions (original angle set is lacking due singularity there: "); let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 let solutions = robot.inverse(&pose); // Solutions is alias of Vec dump_solutions(&solutions); - println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); + println!("\nSolutions assuming we continue from somewhere close. The 'lost solution' returns"); let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; let solutions = robot.inverse_continuing(&pose, &when_continuing_from); dump_solutions(&solutions); - println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); + println!("\nSame pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); dump_solutions(&solutions); - println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0 \ + println!("\nIf we do not have the previous position, we can assume we want J4, J6 close to 0.0 \ The solution appears and the needed rotation is now equally distributed between J4 and J6."); let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); dump_solutions(&solutions); + + println!("\n5 DOF, J6 at fixed angle 77 degrees"); + let solutions5dof = robot.inverse_5dof(&pose, 77.0_f64.to_radians()); + dump_solutions(&solutions5dof); + println!("The XYZ location of TCP is still as in the original pose x = {:.3}, y = {:.3}, z = {:.3}:", + pose.translation.x, pose.translation.y, pose.translation.z); + for solution in &solutions { + let translation = robot.forward(solution).translation; + println!("Translation: x = {:.3}, y = {:.3}, z = {:.3}", translation.x, translation.y, translation.z); + } } \ No newline at end of file diff --git a/src/frame.rs b/src/frame.rs index d5de86c..ff1b35c 100644 --- a/src/frame.rs +++ b/src/frame.rs @@ -149,6 +149,14 @@ impl Kinematics for Frame { self.robot.inverse(&(tcp * self.frame.inverse())) } + fn inverse_5dof(&self, tcp: &Pose, j6: f64) -> Solutions { + self.robot.inverse_5dof(&(tcp * self.frame.inverse()), j6) + } + + fn inverse_continuing_5dof(&self, tcp: &Pose, previous: &Joints) -> Solutions { + self.robot.inverse_continuing_5dof(&(tcp * self.frame.inverse()), previous) + } + fn inverse_continuing(&self, tcp: &Pose, previous: &Joints) -> Solutions { self.robot.inverse_continuing(&(tcp * self.frame.inverse()), previous) } @@ -160,6 +168,16 @@ impl Kinematics for Frame { tcp } + fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] { + // Compute the forward kinematics for the robot itself + let mut poses = self.robot.forward_with_joint_poses(joints); + + // Apply the tool transformation only to the last pose (TCP pose) + poses[5] = poses[5] * self.frame; + + poses + } + fn kinematic_singularity(&self, qs: &Joints) -> Option { self.robot.kinematic_singularity(qs) } diff --git a/src/jacobian.rs b/src/jacobian.rs index dd52d23..e4a6d4f 100644 --- a/src/jacobian.rs +++ b/src/jacobian.rs @@ -296,9 +296,21 @@ mod tests { impl Kinematics for SingleRotaryJointRobot { fn inverse(&self, _pose: &Pose) -> Solutions { - panic!() // Should not be used + panic!() // Not used in this test } + fn inverse_5dof(&self, _pose: &Pose, _j6: f64) -> Solutions { + panic!() // Not used in this test + } + + fn inverse_continuing_5dof(&self, _pose: &Pose, _prev: &Joints) -> Solutions { + panic!() // Not used in this test + } + + fn forward_with_joint_poses(&self, _joints: &Joints) -> [Pose; 6] { + panic!() // Not used in this test + } + /// Simple inverse kinematics for a single rotary joint of the length 1. fn inverse_continuing(&self, pose: &Pose, _previous: &Joints) -> Solutions { let angle = pose.translation.vector[1].atan2(pose.translation.vector[0]); diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index 26c03d5..ee6ee61 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -5,6 +5,7 @@ extern crate nalgebra as na; use std::f64::NAN; use na::{Isometry3}; + /// Pose is used a pose of the robot tcp. It contains both Cartesian position and rotation quaternion /// ``` /// extern crate nalgebra as na; @@ -57,7 +58,6 @@ pub trait Kinematics: Send + Sync { /// valid. fn inverse(&self, pose: &Pose) -> Solutions; - /// Find inverse kinematics (joint position) for this pose /// This function handles the singularity J5 = 0 by keeping the previous values /// the values J4 and J6 from the previous solution @@ -67,10 +67,79 @@ pub trait Kinematics: Send + Sync { fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions; /// Find forward kinematics (pose from joint positions). + /// For 5 DOF robot, the rotation of the joint 6 should normally be 0.0 + /// but some other value can be given, meaning the tool is mounted with + /// fixed rotation offset. fn forward(&self, qs: &Joints) -> Pose; + /// Calculates the inverse kinematics for a robot while ignoring the rotation + /// around joint 6. The position of the tool center point remains precise, + /// but the rotation is approximate (rotation around the tool axis is ignored). + /// The return value for joint 6 is set according to the provided parameter. + /// This method is significantly faster + fn inverse_5dof(&self, pose: &Pose, j6: f64) -> Solutions; + + + /// Calculates the inverse kinematics for a robot while ignoring the rotation + /// around joint 6. The position of the tool center point remains precise, + /// but the rotation is approximate (rotation around the tool axis is ignored). + /// The return value for joint 6 is set based on the previous joint values. + /// This method is significantly faster + fn inverse_continuing_5dof(&self, pose: &Pose, prev: &Joints) -> Solutions; + /// Detect the singularity. Returns either A type singlularity or None if /// no singularity detected. fn kinematic_singularity(&self, qs: &Joints) -> Option; + + /// Computes the forward kinematics for a 6-DOF robotic arm and returns an array of poses + /// representing the position and orientation of each joint, including the final end-effector. + /// + /// The function calculates the transformation for each joint in the robotic arm using the joint + /// angles (in radians) and the kinematic parameters of the robot (link lengths and offsets). + /// It returns an array of 6 poses: one for each joint and the end-effector. + /// + /// # Parameters + /// - `self`: A reference to the kinematics model containing the geometric and joint-specific parameters. + /// - `joints`: A reference to an array of joint angles (in radians) for the 6 joints of the robot. + /// + /// # Returns + /// - An array of 6 `Pose` structures: + /// - Pose 1: The position and orientation of the base link. + /// - Pose 2: The position and orientation of link 1 (first joint). + /// - Pose 3: The position and orientation of link 2 (second joint). + /// - Pose 4: The position and orientation of link 3 (third joint). + /// - Pose 5: The position and orientation of link 4 (fourth joint), before applying any wrist offset. + /// - Pose 6: The position and orientation of the end-effector, including the wrist offset (`c4`). + /// + /// # Example + /// ``` + /// use rs_opw_kinematics::kinematic_traits::Kinematics; + /// use rs_opw_kinematics::kinematics_impl::OPWKinematics; + /// use rs_opw_kinematics::parameters::opw_kinematics::Parameters; + /// let parameters = Parameters { + /// a1: 0.150, + /// a2: 0.000, + /// b: 0.000, + /// c1: 0.550, + /// c2: 0.550, + /// c3: 0.600, + /// c4: 0.110, + /// offsets: [0.0; 6], // No joint offsets + /// ..Parameters::new() + /// }; + /// + /// let joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; // All joints straight up + /// let kinematics = OPWKinematics::new(parameters); + /// + /// let poses = kinematics.forward_with_joint_poses(&joints); + /// + /// assert_eq!(poses.len(), 6); // Should return 6 poses + /// ``` + /// + /// # Notes + /// - The function applies the geometric parameters (like link lengths and joint offsets) and computes + /// each joint's position and orientation relative to the base frame. + /// - The final pose (Pose 6) includes the `c4` offset, which accounts for the wrist length. + fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6]; } diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index 1a43c47..17968bb 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -1,5 +1,5 @@ //! Provides implementation of inverse and direct kinematics. - + use std::f64::{consts::PI}; use crate::kinematic_traits::{Kinematics, Solutions, Pose, Singularity, Joints, JOINTS_AT_ZERO}; use crate::parameters::opw_kinematics::{Parameters}; @@ -41,6 +41,7 @@ impl OPWKinematics { } } + const MM: f64 = 0.001; const DISTANCE_TOLERANCE: f64 = 0.001 * MM; const ANGULAR_TOLERANCE: f64 = 1E-6; @@ -64,15 +65,27 @@ const J5: usize = 4; const J6: usize = 5; impl Kinematics for OPWKinematics { - /// Return the solution that is constraint compliant anv values are valid /// (no NaNs, etc) but otherwise not sorted. + /// If this is 5 degree of freedom robot only, the 6 joint is set to 0.0 + /// The rotation of pose in this case is only approximate. fn inverse(&self, pose: &Pose) -> Solutions { - self.filter_constraints_compliant(self.inverse_intern(&pose)) + if self.parameters.dof == 5 { + // For 5 DOF robot, we can only do 5 DOF approximate inverse. + self.inverse_intern_5_dof(pose, f64::NAN) + } else { + self.filter_constraints_compliant(self.inverse_intern(&pose)) + } } // Replaces singularity with correct solution + // If this is 5 degree of freedom robot only, the 6 joint is set to as it was previous. + // The rotation of pose in this case is only approximate. fn inverse_continuing(&self, pose: &Pose, prev: &Joints) -> Solutions { + if self.parameters.dof == 5 { + return self.inverse_intern_5_dof(pose, prev[5]); + } + let previous; if prev[0].is_nan() { // Special value CONSTRAINT_CENTERED has been used @@ -164,6 +177,7 @@ impl Kinematics for OPWKinematics { fn forward(&self, joints: &Joints) -> Pose { let p = &self.parameters; + // Apply sign corrections and offsets let q1 = joints[0] * p.sign_corrections[0] as f64 - p.offsets[0]; let q2 = joints[1] * p.sign_corrections[1] as f64 - p.offsets[1]; let q3 = joints[2] * p.sign_corrections[2] as f64 - p.offsets[2]; @@ -174,15 +188,20 @@ impl Kinematics for OPWKinematics { let psi3 = f64::atan2(p.a2, p.c3); let k = f64::sqrt(p.a2 * p.a2 + p.c3 * p.c3); - let cx1 = p.c2 * f64::sin(q2) + k * f64::sin(q2 + q3 + psi3) + p.a1; + // Precompute q23_psi3 for better readability and reuse + let q23_psi3 = q2 + q3 + psi3; + let sin_q23_psi3 = q23_psi3.sin(); + let cos_q23_psi3 = q23_psi3.cos(); + + let cx1 = p.c2 * f64::sin(q2) + k * sin_q23_psi3 + p.a1; let cy1 = p.b; - let cz1 = p.c2 * f64::cos(q2) + k * f64::cos(q2 + q3 + psi3); + let cz1 = p.c2 * f64::cos(q2) + k * cos_q23_psi3; let cx0 = cx1 * f64::cos(q1) - cy1 * f64::sin(q1); let cy0 = cx1 * f64::sin(q1) + cy1 * f64::cos(q1); let cz0 = cz1 + p.c1; - // Precompute sines and cosines + // Precompute sines and cosines for efficiency let (s1, c1) = (q1.sin(), q1.cos()); let (s2, c2) = (q2.sin(), q2.cos()); let (s3, c3) = (q3.sin(), q3.cos()); @@ -190,25 +209,147 @@ impl Kinematics for OPWKinematics { let (s5, c5) = (q5.sin(), q5.cos()); let (s6, c6) = (q6.sin(), q6.cos()); + // Compute rotation matrix r_0c let r_0c = Matrix3::new( c1 * c2 * c3 - c1 * s2 * s3, -s1, c1 * c2 * s3 + c1 * s2 * c3, s1 * c2 * c3 - s1 * s2 * s3, c1, s1 * c2 * s3 + s1 * s2 * c3, -s2 * c3 - c2 * s3, 0.0, -s2 * s3 + c2 * c3, ); + // Compute rotation matrix r_ce let r_ce = Matrix3::new( c4 * c5 * c6 - s4 * s6, -c4 * c5 * s6 - s4 * c6, c4 * s5, s4 * c5 * c6 + c4 * s6, -s4 * c5 * s6 + c4 * c6, s4 * s5, -s5 * c6, s5 * s6, c5, ); + // Compute the final rotation matrix r_oe let r_oe = r_0c * r_ce; + // Calculate the final translation let translation = Vector3::new(cx0, cy0, cz0) + p.c4 * r_oe * *self.unit_z; + + // Convert the rotation matrix to a quaternion let rotation = Rotation3::from_matrix_unchecked(r_oe); - Pose::from_parts(Translation3::from(translation), - UnitQuaternion::from_rotation_matrix(&rotation)) + // Return the pose combining translation and rotation + Pose::from_parts( + Translation3::from(translation), + UnitQuaternion::from_rotation_matrix(&rotation), + ) + } + + fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] { + let p = &self.parameters; + + // Use joint angles directly as radians (no conversion needed) + let q1 = joints[0] * p.sign_corrections[0] as f64 - p.offsets[0]; + let q2 = joints[1] * p.sign_corrections[1] as f64 - p.offsets[1]; + let q3 = joints[2] * p.sign_corrections[2] as f64 - p.offsets[2]; + let q4 = joints[3] * p.sign_corrections[3] as f64 - p.offsets[3]; + let q5 = joints[4] * p.sign_corrections[4] as f64 - p.offsets[4]; + let q6 = joints[5] * p.sign_corrections[5] as f64 - p.offsets[5]; + + let psi3 = f64::atan2(p.a2, p.c3); + let k = f64::sqrt(p.a2 * p.a2 + p.c3 * p.c3); + + // Precompute q23_psi3 for better readability and reuse + let q23_psi3 = q2 + q3 + psi3; + let sin_q23_psi3 = q23_psi3.sin(); + let cos_q23_psi3 = q23_psi3.cos(); + + // Precompute sines and cosines for efficiency + let (s1, c1) = (q1.sin(), q1.cos()); + let (s2, c2) = (q2.sin(), q2.cos()); + let (s3, c3) = (q3.sin(), q3.cos()); + let (s4, c4) = (q4.sin(), q4.cos()); + let (s5, c5) = (q5.sin(), q5.cos()); + let (s6, c6) = (q6.sin(), q6.cos()); + + // Pose 1: Base link position + let joint1_pos = Vector3::new(0.0, 0.0, p.c1); // Z-offset from the base height + let joint1_rot = Rotation3::identity(); // No rotation for the base frame + let pose1 = Pose::from_parts(Translation3::from(joint1_pos), UnitQuaternion::from_rotation_matrix(&joint1_rot)); + + // Pose 2: Link 1 position (translation along x = a1, rotation around q1) + let joint2_pos = Vector3::new(p.a1 * c1, p.a1 * s1, p.c1); // a1 affects X, base height remains Z + let joint2_rot = Rotation3::from_euler_angles(q1, 0.0, 0.0); // Rotation around Z-axis (q1) + let pose2 = Pose::from_parts(Translation3::from(joint2_pos), UnitQuaternion::from_rotation_matrix(&joint2_rot)); + + // Pose 3: Link 2 position + let cx1 = p.c2 * f64::sin(q2) + p.a1; + let cy1 = p.b; // Typically 0 for most robots + let cz1 = p.c2 * f64::cos(q2); // Move in Z by link length + let joint3_pos = Vector3::new( + cx1 * c1 - cy1 * s1, + cx1 * s1 + cy1 * c1, + cz1 + p.c1 // Add the base height + ); + let joint3_rot = Rotation3::from_euler_angles(q1, q2, 0.0); // Rotation around Z and Y + let pose3 = Pose::from_parts(Translation3::from(joint3_pos), UnitQuaternion::from_rotation_matrix(&joint3_rot)); + + // Pose 4: Link 3 position (additional translation along Z) + let cx2 = p.c2 * f64::sin(q2) + k * sin_q23_psi3 + p.a1; + let cz2 = p.c2 * f64::cos(q2) + k * cos_q23_psi3; + let joint4_pos = Vector3::new( + cx2 * c1 - cy1 * s1, + cx2 * s1 + cy1 * c1, + cz2 + p.c1 // Continue translating along Z + ); + let joint4_rot = Rotation3::from_euler_angles(q1, q2, q3); // Rotation around Z, Y, and additional Y (q3) + let pose4 = Pose::from_parts(Translation3::from(joint4_pos), UnitQuaternion::from_rotation_matrix(&joint4_rot)); + + // Pose 5: Link 4 position (No c4 applied here, just joint4_pos) + let joint5_pos = joint4_pos; // Do not apply c4 here + let joint5_rot = Rotation3::from_euler_angles(q1, q2, q3 + q4); // Adding q4 for rotation around X-axis + let pose5 = Pose::from_parts(Translation3::from(joint5_pos), UnitQuaternion::from_rotation_matrix(&joint5_rot)); + + // Pose 6: End-effector position (including c4 offset) + let r_0c = Matrix3::new( + c1 * c2 * c3 - c1 * s2 * s3, -s1, c1 * c2 * s3 + c1 * s2 * c3, + s1 * c2 * c3 - s1 * s2 * s3, c1, s1 * c2 * s3 + s1 * s2 * c3, + -s2 * c3 - c2 * s3, 0.0, -s2 * s3 + c2 * c3, + ); + let r_ce = Matrix3::new( + c4 * c5 * c6 - s4 * s6, -c4 * c5 * s6 - s4 * c6, c4 * s5, + s4 * c5 * c6 + c4 * s6, -s4 * c5 * s6 + c4 * c6, s4 * s5, + -s5 * c6, s5 * s6, c5, + ); + let r_oe = r_0c * r_ce; + + // Apply the c4 offset for the final end-effector position + let end_effector_pos = joint5_pos + r_oe * (p.c4 * *self.unit_z); // Apply c4 offset here for the wrist + let end_effector_rot = Rotation3::from_matrix_unchecked(r_oe); // Final rotation + let pose6 = Pose::from_parts(Translation3::from(end_effector_pos), UnitQuaternion::from_rotation_matrix(&end_effector_rot)); + + // Return all 6 Poses, with Pose 6 including c4 offset + [pose1, pose2, pose3, pose4, pose5, pose6] + } + + fn inverse_5dof(&self, pose: &Pose, j6: f64) -> Solutions { + self.filter_constraints_compliant(self.inverse_intern_5_dof(&pose, j6)) + } + + fn inverse_continuing_5dof(&self, pose: &Pose, prev: &Joints) -> Solutions { + let previous; + if prev[0].is_nan() { + // Special value CONSTRAINT_CENTERED has been used + previous = self.constraint_centers(); + } else { + previous = prev; + } + + let mut solutions = self.inverse_intern_5_dof(pose, prev[5]); + + // Before any sorting, normalize all angles to be close to + // 'previous' + for s_idx in 0..solutions.len() { + for joint_idx in 0..6 { + normalize_near(&mut solutions[s_idx][joint_idx], previous[joint_idx]); + } + } + self.sort_by_closeness(&mut solutions, &previous); + self.filter_constraints_compliant(solutions) } fn kinematic_singularity(&self, joints: &Joints) -> Option { @@ -226,7 +367,7 @@ impl OPWKinematics { // Adjust to wrist center let matrix = pose.rotation.to_rotation_matrix(); - let translation_vector = pose.translation.vector; // Get the translation vector component + let translation_vector = &pose.translation.vector; // Get the translation vector component let scaled_z_axis = params.c4 * matrix.transform_vector(&Vector3::z_axis()); // Scale and rotate the z-axis vector let c = translation_vector - scaled_z_axis; @@ -324,104 +465,49 @@ impl OPWKinematics { let theta5_vii = -theta5_iii; let theta5_viii = -theta5_iv; - let zero_threshold: f64 = 1e-6; let theta4_i; let theta6_i; - if theta5_i.abs() < zero_threshold { - theta4_i = 0.0; - let xe = Vector3::new(matrix[(0, 0)], matrix[(1, 0)], matrix[(2, 0)]); - let mut rc = Matrix3::zeros(); // Assuming Matrix3::zeros() creates a 3x3 matrix filled with 0.0 - - // Set columns of Rc - rc.set_column(1, &Vector3::new(-theta1_i.sin(), theta1_i.cos(), 0.0)); // yc - rc.set_column(2, &Vector3::new(matrix[(0, 2)], matrix[(1, 2)], matrix[(2, 2)])); // zc = ze - rc.set_column(0, &rc.column(1).cross(&rc.column(2))); // xc - - let xec = rc.transpose() * xe; - theta6_i = xec[1].atan2(xec[0]); - } else { - let theta4_iy = matrix[(1, 2)] * cos1[0] - matrix[(0, 2)] * sin1[0]; - let theta4_ix = matrix[(0, 2)] * c23[0] * cos1[0] + matrix[(1, 2)] * c23[0] * sin1[0] - matrix[(2, 2)] * s23[0]; - theta4_i = theta4_iy.atan2(theta4_ix); + let theta4_iy = matrix[(1, 2)] * cos1[0] - matrix[(0, 2)] * sin1[0]; + let theta4_ix = matrix[(0, 2)] * c23[0] * cos1[0] + matrix[(1, 2)] * c23[0] * sin1[0] - matrix[(2, 2)] * s23[0]; + theta4_i = theta4_iy.atan2(theta4_ix); - let theta6_iy = matrix[(0, 1)] * s23[0] * cos1[0] + matrix[(1, 1)] * s23[0] * sin1[0] + matrix[(2, 1)] * c23[0]; - let theta6_ix = -matrix[(0, 0)] * s23[0] * cos1[0] - matrix[(1, 0)] * s23[0] * sin1[0] - matrix[(2, 0)] * c23[0]; - theta6_i = theta6_iy.atan2(theta6_ix); - } + let theta6_iy = matrix[(0, 1)] * s23[0] * cos1[0] + matrix[(1, 1)] * s23[0] * sin1[0] + matrix[(2, 1)] * c23[0]; + let theta6_ix = -matrix[(0, 0)] * s23[0] * cos1[0] - matrix[(1, 0)] * s23[0] * sin1[0] - matrix[(2, 0)] * c23[0]; + theta6_i = theta6_iy.atan2(theta6_ix); let theta4_ii; let theta6_ii; - if theta5_ii.abs() < zero_threshold { - theta4_ii = 0.0; - let xe = Vector3::new(matrix[(0, 0)], matrix[(1, 0)], matrix[(2, 0)]); - let mut rc = Matrix3::zeros(); - - // Set columns of Rc - rc.set_column(1, &Vector3::new(-theta1_i.sin(), theta1_i.cos(), 0.0)); // yc - rc.set_column(2, &Vector3::new(matrix[(0, 2)], matrix[(1, 2)], matrix[(2, 2)])); // zc = ze - rc.set_column(0, &rc.column(1).cross(&rc.column(2))); // xc - - let xec = rc.transpose() * xe; - theta6_ii = xec[1].atan2(xec[0]); - } else { - let theta4_iiy = matrix[(1, 2)] * cos1[1] - matrix[(0, 2)] * sin1[1]; - let theta4_iix = matrix[(0, 2)] * c23[1] * cos1[1] + matrix[(1, 2)] * c23[1] * sin1[1] - matrix[(2, 2)] * s23[1]; - theta4_ii = theta4_iiy.atan2(theta4_iix); + let theta4_iiy = matrix[(1, 2)] * cos1[1] - matrix[(0, 2)] * sin1[1]; + let theta4_iix = matrix[(0, 2)] * c23[1] * cos1[1] + matrix[(1, 2)] * c23[1] * sin1[1] - matrix[(2, 2)] * s23[1]; + theta4_ii = theta4_iiy.atan2(theta4_iix); - let theta6_iiy = matrix[(0, 1)] * s23[1] * cos1[1] + matrix[(1, 1)] * s23[1] * sin1[1] + matrix[(2, 1)] * c23[1]; - let theta6_iix = -matrix[(0, 0)] * s23[1] * cos1[1] - matrix[(1, 0)] * s23[1] * sin1[1] - matrix[(2, 0)] * c23[1]; - theta6_ii = theta6_iiy.atan2(theta6_iix); - } + let theta6_iiy = matrix[(0, 1)] * s23[1] * cos1[1] + matrix[(1, 1)] * s23[1] * sin1[1] + matrix[(2, 1)] * c23[1]; + let theta6_iix = -matrix[(0, 0)] * s23[1] * cos1[1] - matrix[(1, 0)] * s23[1] * sin1[1] - matrix[(2, 0)] * c23[1]; + theta6_ii = theta6_iiy.atan2(theta6_iix); let theta4_iii; let theta6_iii; - if theta5_iii.abs() < zero_threshold { - theta4_iii = 0.0; - let xe = Vector3::new(matrix[(0, 0)], matrix[(1, 0)], matrix[(2, 0)]); - let mut rc = Matrix3::zeros(); + let theta4_iiiy = matrix[(1, 2)] * cos1[2] - matrix[(0, 2)] * sin1[2]; + let theta4_iiix = matrix[(0, 2)] * c23[2] * cos1[2] + matrix[(1, 2)] * c23[2] * sin1[2] - matrix[(2, 2)] * s23[2]; + theta4_iii = theta4_iiiy.atan2(theta4_iiix); - // Set columns of Rc - rc.set_column(1, &Vector3::new(-theta1_ii.sin(), theta1_ii.cos(), 0.0)); // yc - rc.set_column(2, &Vector3::new(matrix[(0, 2)], matrix[(1, 2)], matrix[(2, 2)])); // zc = ze - rc.set_column(0, &rc.column(1).cross(&rc.column(2))); // xc - - let xec = rc.transpose() * xe; - theta6_iii = xec[1].atan2(xec[0]); - } else { - let theta4_iiiy = matrix[(1, 2)] * cos1[2] - matrix[(0, 2)] * sin1[2]; - let theta4_iiix = matrix[(0, 2)] * c23[2] * cos1[2] + matrix[(1, 2)] * c23[2] * sin1[2] - matrix[(2, 2)] * s23[2]; - theta4_iii = theta4_iiiy.atan2(theta4_iiix); - - let theta6_iiiy = matrix[(0, 1)] * s23[2] * cos1[2] + matrix[(1, 1)] * s23[2] * sin1[2] + matrix[(2, 1)] * c23[2]; - let theta6_iiix = -matrix[(0, 0)] * s23[2] * cos1[2] - matrix[(1, 0)] * s23[2] * sin1[2] - matrix[(2, 0)] * c23[2]; - theta6_iii = theta6_iiiy.atan2(theta6_iiix); - } + let theta6_iiiy = matrix[(0, 1)] * s23[2] * cos1[2] + matrix[(1, 1)] * s23[2] * sin1[2] + matrix[(2, 1)] * c23[2]; + let theta6_iiix = -matrix[(0, 0)] * s23[2] * cos1[2] - matrix[(1, 0)] * s23[2] * sin1[2] - matrix[(2, 0)] * c23[2]; + theta6_iii = theta6_iiiy.atan2(theta6_iiix); let theta4_iv; let theta6_iv; - if theta5_iv.abs() < zero_threshold { - theta4_iv = 0.0; - let xe = Vector3::new(matrix[(0, 0)], matrix[(1, 0)], matrix[(2, 0)]); - let mut rc = Matrix3::zeros(); - rc.set_column(1, &Vector3::new(-theta1_ii.sin(), theta1_ii.cos(), 0.0)); - rc.set_column(2, &Vector3::new(matrix[(0, 2)], matrix[(1, 2)], matrix[(2, 2)])); - rc.set_column(0, &rc.column(1).cross(&rc.column(2))); - - let xec = rc.transpose() * xe; - theta6_iv = xec[1].atan2(xec[0]); - } else { - let theta4_ivy = matrix[(1, 2)] * cos1[3] - matrix[(0, 2)] * sin1[3]; - let theta4_ivx = matrix[(0, 2)] * c23[3] * cos1[3] + matrix[(1, 2)] * c23[3] * sin1[3] - matrix[(2, 2)] * s23[3]; - theta4_iv = theta4_ivy.atan2(theta4_ivx); + let theta4_ivy = matrix[(1, 2)] * cos1[3] - matrix[(0, 2)] * sin1[3]; + let theta4_ivx = matrix[(0, 2)] * c23[3] * cos1[3] + matrix[(1, 2)] * c23[3] * sin1[3] - matrix[(2, 2)] * s23[3]; + theta4_iv = theta4_ivy.atan2(theta4_ivx); - let theta6_ivy = matrix[(0, 1)] * s23[3] * cos1[3] + matrix[(1, 1)] * s23[3] * sin1[3] + matrix[(2, 1)] * c23[3]; - let theta6_ivx = -matrix[(0, 0)] * s23[3] * cos1[3] - matrix[(1, 0)] * s23[3] * sin1[3] - matrix[(2, 0)] * c23[3]; - theta6_iv = theta6_ivy.atan2(theta6_ivx); - } + let theta6_ivy = matrix[(0, 1)] * s23[3] * cos1[3] + matrix[(1, 1)] * s23[3] * sin1[3] + matrix[(2, 1)] * c23[3]; + let theta6_ivx = -matrix[(0, 0)] * s23[3] * cos1[3] - matrix[(1, 0)] * s23[3] * sin1[3] - matrix[(2, 0)] * c23[3]; + theta6_iv = theta6_ivy.atan2(theta6_ivx); let theta4_v = theta4_i + PI; let theta4_vi = theta4_ii + PI; @@ -487,6 +573,199 @@ impl OPWKinematics { result } + + fn inverse_intern_5_dof(&self, pose: &Pose, j6: f64) -> Solutions { + let params = &self.parameters; + + // Adjust to wrist center + let matrix = pose.rotation.to_rotation_matrix(); + let translation_vector = &pose.translation.vector; // Get the translation vector component + let scaled_z_axis = params.c4 * matrix.transform_vector(&Vector3::z_axis()); // Scale and rotate the z-axis vector + + let c = translation_vector - scaled_z_axis; + + let nx1 = ((c.x * c.x + c.y * c.y) - params.b * params.b).sqrt() - params.a1; + + let tmp1 = c.y.atan2(c.x); // Rust's method call syntax for atan2(y, x) + let tmp2 = params.b.atan2(nx1 + params.a1); + + let theta1_i = tmp1 - tmp2; + let theta1_ii = tmp1 + tmp2 - PI; + + let tmp3 = c.z - params.c1; // Access z directly for nalgebra's Vector3 + let s1_2 = nx1 * nx1 + tmp3 * tmp3; + + let tmp4 = nx1 + 2.0 * params.a1; + let s2_2 = tmp4 * tmp4 + tmp3 * tmp3; + let kappa_2 = params.a2 * params.a2 + params.c3 * params.c3; + + let c2_2 = params.c2 * params.c2; + + let tmp5 = s1_2 + c2_2 - kappa_2; + + let s1 = f64::sqrt(s1_2); + let s2 = f64::sqrt(s2_2); + + let tmp13 = f64::acos(tmp5 / (2.0 * s1 * params.c2)); + let tmp14 = f64::atan2(nx1, c.z - params.c1); + let theta2_i = -tmp13 + tmp14; + let theta2_ii = tmp13 + tmp14; + + let tmp6 = s2_2 + c2_2 - kappa_2; + + let tmp15 = f64::acos(tmp6 / (2.0 * s2 * params.c2)); + let tmp16 = f64::atan2(nx1 + 2.0 * params.a1, c.z - params.c1); + let theta2_iii = -tmp15 - tmp16; + let theta2_iv = tmp15 - tmp16; + + // theta3 + let tmp7 = s1_2 - c2_2 - kappa_2; + let tmp8 = s2_2 - c2_2 - kappa_2; + let tmp9 = 2.0 * params.c2 * f64::sqrt(kappa_2); + let tmp10 = f64::atan2(params.a2, params.c3); + + let tmp11 = f64::acos(tmp7 / tmp9); + let theta3_i = tmp11 - tmp10; + let theta3_ii = -tmp11 - tmp10; + + let tmp12 = f64::acos(tmp8 / tmp9); + let theta3_iii = tmp12 - tmp10; + let theta3_iv = -tmp12 - tmp10; + + let theta1_i_sin = theta1_i.sin(); + let theta1_i_cos = theta1_i.cos(); + let theta1_ii_sin = theta1_ii.sin(); + let theta1_ii_cos = theta1_ii.cos(); + + // orientation part + let sin1: [f64; 4] = [ + theta1_i_sin, theta1_i_sin, theta1_ii_sin, theta1_ii_sin, + ]; + + let cos1: [f64; 4] = [ + theta1_i_cos, theta1_i_cos, theta1_ii_cos, theta1_ii_cos + ]; + + let s23: [f64; 4] = [ + (theta2_i + theta3_i).sin(), + (theta2_ii + theta3_ii).sin(), + (theta2_iii + theta3_iii).sin(), + (theta2_iv + theta3_iv).sin(), + ]; + + let c23: [f64; 4] = [ + (theta2_i + theta3_i).cos(), + (theta2_ii + theta3_ii).cos(), + (theta2_iii + theta3_iii).cos(), + (theta2_iv + theta3_iv).cos(), + ]; + + let m: [f64; 4] = [ + matrix[(0, 2)] * s23[0] * cos1[0] + matrix[(1, 2)] * s23[0] * sin1[0] + matrix[(2, 2)] * c23[0], + matrix[(0, 2)] * s23[1] * cos1[1] + matrix[(1, 2)] * s23[1] * sin1[1] + matrix[(2, 2)] * c23[1], + matrix[(0, 2)] * s23[2] * cos1[2] + matrix[(1, 2)] * s23[2] * sin1[2] + matrix[(2, 2)] * c23[2], + matrix[(0, 2)] * s23[3] * cos1[3] + matrix[(1, 2)] * s23[3] * sin1[3] + matrix[(2, 2)] * c23[3], + ]; + + let theta5_i = f64::atan2((1.0 - m[0] * m[0]).sqrt(), m[0]); + let theta5_ii = f64::atan2((1.0 - m[1] * m[1]).sqrt(), m[1]); + let theta5_iii = f64::atan2((1.0 - m[2] * m[2]).sqrt(), m[2]); + let theta5_iv = f64::atan2((1.0 - m[3] * m[3]).sqrt(), m[3]); + + let theta5_v = -theta5_i; + let theta5_vi = -theta5_ii; + let theta5_vii = -theta5_iii; + let theta5_viii = -theta5_iv; + + let theta4_i; + + let theta4_iy = matrix[(1, 2)] * cos1[0] - matrix[(0, 2)] * sin1[0]; + let theta4_ix = matrix[(0, 2)] * c23[0] * cos1[0] + matrix[(1, 2)] * c23[0] * sin1[0] - matrix[(2, 2)] * s23[0]; + theta4_i = theta4_iy.atan2(theta4_ix); + + let theta4_ii; + + let theta4_iiy = matrix[(1, 2)] * cos1[1] - matrix[(0, 2)] * sin1[1]; + let theta4_iix = matrix[(0, 2)] * c23[1] * cos1[1] + matrix[(1, 2)] * c23[1] * sin1[1] - matrix[(2, 2)] * s23[1]; + theta4_ii = theta4_iiy.atan2(theta4_iix); + + let theta4_iii; + + let theta4_iiiy = matrix[(1, 2)] * cos1[2] - matrix[(0, 2)] * sin1[2]; + let theta4_iiix = matrix[(0, 2)] * c23[2] * cos1[2] + matrix[(1, 2)] * c23[2] * sin1[2] - matrix[(2, 2)] * s23[2]; + theta4_iii = theta4_iiiy.atan2(theta4_iiix); + + let theta4_iv; + + let theta4_ivy = matrix[(1, 2)] * cos1[3] - matrix[(0, 2)] * sin1[3]; + let theta4_ivx = matrix[(0, 2)] * c23[3] * cos1[3] + matrix[(1, 2)] * c23[3] * sin1[3] - matrix[(2, 2)] * s23[3]; + theta4_iv = theta4_ivy.atan2(theta4_ivx); + + let theta4_v = theta4_i + PI; + let theta4_vi = theta4_ii + PI; + let theta4_vii = theta4_iii + PI; + let theta4_viii = theta4_iv + PI; + + let theta: [[f64; 5]; 8] = [ + [theta1_i, theta2_i, theta3_i, theta4_i, theta5_i], + [theta1_i, theta2_ii, theta3_ii, theta4_ii, theta5_ii], + [theta1_ii, theta2_iii, theta3_iii, theta4_iii, theta5_iii], + [theta1_ii, theta2_iv, theta3_iv, theta4_iv, theta5_iv], + [theta1_i, theta2_i, theta3_i, theta4_v, theta5_v], + [theta1_i, theta2_ii, theta3_ii, theta4_vi, theta5_vi], + [theta1_ii, theta2_iii, theta3_iii, theta4_vii, theta5_vii], + [theta1_ii, theta2_iv, theta3_iv, theta4_viii, theta5_viii], + ]; + + let mut sols: [[f64; 6]; 8] = [[f64::NAN; 6]; 8]; + for si in 0..sols.len() { + for ji in 0..5 { + sols[si][ji] = (theta[si][ji] + params.offsets[ji]) * + params.sign_corrections[ji] as f64; + } + sols[si][5] = j6 // J6 goes directly to response and is not more adjusted + } + + let mut result: Solutions = Vec::with_capacity(8); + + // Debug check. Solution failing cross-verification is flagged + // as invalid. This loop also normalizes valid solutions to 0 + for si in 0..sols.len() { + let mut valid = true; + for ji in 0..5 { // J6 is not processed in this loop + let mut angle = sols[si][ji]; + if angle.is_finite() { + while angle > PI { + angle -= 2.0 * PI; + } + while angle < -PI { + angle += 2.0 * PI; + } + sols[si][ji] = angle; + } else { + valid = false; + break; + } + }; + if valid { + let check_xyz = self.forward(&sols[si]).translation; + if Self::compare_xyz_only(&pose.translation, &check_xyz, DISTANCE_TOLERANCE) { + result.push(sols[si]); + } else { + if DEBUG { + println!("********** Pose Failure 5DOF sol {} *********", si); + } + } + } + } + + result + } + + fn compare_xyz_only(pose_translation: &Translation3, check_xyz: &Translation3, tolerance: f64) -> bool { + (pose_translation.vector - check_xyz.vector).norm() <= tolerance + } + fn filter_constraints_compliant(&self, solutions: Solutions) -> Solutions { match &self.constraints { Some(constraints) => constraints.filter(&solutions), diff --git a/src/parameters.rs b/src/parameters.rs index 04846d1..dc30ec9 100644 --- a/src/parameters.rs +++ b/src/parameters.rs @@ -3,20 +3,47 @@ pub mod opw_kinematics { use crate::utils::deg; - /// Parameters for the robot. See parameters_robots.rs for examples for concrete robot models. + /// Parameters for the robot. See [parameters_robots.rs](parameters_robots.rs) for examples of concrete robot models. #[derive(Debug, Clone, Copy)] + /// Parameters for the kinematic model of the robot. pub struct Parameters { + /// The length of the first link of the robot (distance between joint 1 and joint 2). pub a1: f64, + + /// The length of the second link of the robot (distance between joint 2 and joint 3). pub a2: f64, + + /// The offset in the y-direction between joint 1 and joint 2. + /// This can be 0 for robots without a lateral offset that is very common. pub b: f64, + + /// The vertical distance from the base (joint 1) to joint 2 along the z-axis. pub c1: f64, + + /// The vertical distance between joints 2 and 3 along the z-axis. pub c2: f64, + + /// The offset between joint 3 and joint 4, typically along the x-axis. pub c3: f64, + + /// The distance from the wrist center (last joint) to the end-effector mount + /// In 5-DOF robots, this defines the offset between joint 5 and the end-effector mount pub c4: f64, + + /// Offsets applied to each joint angle to adjust the reference zero position. + /// There are 6 values corresponding to each joint in a 6-DOF robot. pub offsets: [f64; 6], + + /// Specifies the direction of positive rotation from the zero angle for each joint. + /// A value of `-1` reverses the default rotation direction for that joint. pub sign_corrections: [i8; 6], + + /// Degrees of freedom for the robot. + /// This can either be 5 for 5-DOF robots or 6 for 6-DOF robots. + pub dof: i8 } + impl Parameters { /// Convert to string yaml representation (quick viewing, etc). pub fn to_yaml(&self) -> String { @@ -30,7 +57,8 @@ pub mod opw_kinematics { c3: {}\n \ c4: {}\n\ opw_kinematics_joint_offsets: [{}]\n\ - opw_kinematics_joint_sign_corrections: [{}]\n", + opw_kinematics_joint_sign_corrections: [{}]\n + dof: [{}]\n", self.a1, self.a2, self.b, @@ -41,7 +69,8 @@ pub mod opw_kinematics { self.offsets.iter().map(|x| deg(x)) .collect::>().join(","), self.sign_corrections.iter().map(|x| x.to_string()) - .collect::>().join(",") + .collect::>().join(","), + self.dof ) } } diff --git a/src/parameters_from_file.rs b/src/parameters_from_file.rs index 0bc4ddc..3c38b66 100644 --- a/src/parameters_from_file.rs +++ b/src/parameters_from_file.rs @@ -24,7 +24,10 @@ impl Parameters { /// c4: 0.10 /// opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, deg(180.0)] /// opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] + /// dof: 6 /// ``` + /// offsets, sign corrections and DOF are optional + /// /// ROS-Industrial provides many such files for FANUC robots on GitHub /// (ros-industrial/fanuc, see fanuc_m10ia_support/config/opw_parameters_m10ia.yaml) /// YAML extension to parse the deg(angle) function is supported. @@ -37,57 +40,92 @@ impl Parameters { let docs = YamlLoader::load_from_str(&contents).map_err( |e| ParameterError::ParseError(e.to_string()))?; - let doc = &docs[0]; + let doc = &docs[0]; let params = &doc["opw_kinematics_geometric_parameters"]; - let offsets_yaml = &doc["opw_kinematics_joint_offsets"]; - let sign_corrections_yaml = &doc["opw_kinematics_joint_sign_corrections"]; + let dof = params["dof"].as_i64().unwrap_or(6) as i8; + let mut sign_corrections = Self::read_sign_corrections(&doc["opw_kinematics_joint_sign_corrections"])?; + if dof == 5 { + // Block J6 at 0 by default for 5DOF robot. + sign_corrections[5] = 0; + } + + Ok(Parameters { + a1: params["a1"].as_f64().ok_or_else(|| ParameterError::MissingField("a1".into()))?, + a2: params["a2"].as_f64().ok_or_else(|| ParameterError::MissingField("a2".into()))?, + b: params["b"].as_f64().ok_or_else(|| ParameterError::MissingField("b".into()))?, + c1: params["c1"].as_f64().ok_or_else(|| ParameterError::MissingField("c1".into()))?, + c2: params["c2"].as_f64().ok_or_else(|| ParameterError::MissingField("c2".into()))?, + c3: params["c3"].as_f64().ok_or_else(|| ParameterError::MissingField("c3".into()))?, + c4: params["c4"].as_f64().ok_or_else(|| ParameterError::MissingField("c4".into()))?, + dof: dof, + offsets: Self::read_offsets(&doc["opw_kinematics_joint_offsets"])?, + sign_corrections: sign_corrections, + }) + } + + + fn read_sign_corrections(doc: &Yaml) -> Result<[i8; 6], ParameterError> { + // Store the temporary vector in a variable for longer lifetime + let default_sign_corrections = vec![Yaml::Integer(1); 6]; - let offsets: [f64; 6] = offsets_yaml.as_vec() - .ok_or_else(|| ParameterError::MissingField("offsets array".into()))? + // Check if the sign corrections field exists, if not default to all 0 + let sign_corrections_yaml = doc.as_vec().unwrap_or(&default_sign_corrections); + + let mut sign_corrections: Vec = sign_corrections_yaml + .iter() + .map(|item| item.as_i64().unwrap_or(0) as i8) // Default missing or invalid entries to 0 + .collect(); + + // Ensure length is either 5 or 6, and pad with 0 if necessary + if sign_corrections.len() == 5 { + sign_corrections.push(0); // Add 0 as the 6th element + } + + if sign_corrections.len() != 6 { + return Err(ParameterError::InvalidLength { + expected: 6, + found: sign_corrections.len(), + }); + } + + let sign_corrections: [i8; 6] = sign_corrections.try_into().unwrap(); // Safe now, we ensured it's of length 6 + Ok(sign_corrections) + } + + + fn read_offsets(offsets_yaml: &Yaml) -> Result<[f64; 6], ParameterError> { + // Check if the offsets field exists, if not default to all 0 + let default_offsets = &vec![Yaml::Integer(0); 6]; + let offsets_yaml = offsets_yaml.as_vec().unwrap_or(&default_offsets); + let mut offsets: Vec = offsets_yaml .iter() .map(|item| match item { Yaml::String(s) => Self::parse_degrees(s), Yaml::Real(s) => s.parse::() .map_err(|_| ParameterError::ParseError("Failed to parse angle".into())), Yaml::Integer(s) => Ok(*s as f64), - _ => Err(ParameterError::ParseError( - "Offset entry is not a number or deg() function".into())), + _ => Ok(0.0), // Default any invalid entry to 0 }) - .collect::, _>>()? - .try_into() - .map_err(|_| ParameterError::InvalidLength { - expected: 6, - found: offsets_yaml.as_vec() - .unwrap().len(), - })?; + .collect::, _>>()?; - let sign_corrections: [i8; 6] = sign_corrections_yaml.as_vec() - .ok_or_else(|| ParameterError::MissingField("sign corrections array".into()))? - .iter() - .map(|item| item.as_i64().ok_or( - ParameterError::ParseError("Sign correction not an integer".into())) - .map(|x| x as i8)) - .collect::, _>>()? - .try_into() - .map_err(|_| ParameterError::InvalidLength { + // Ensure length is either 5 or 6, and pad with 0 if necessary + if offsets.len() == 5 { + offsets.push(0.0); // Add 0 as the 6th element + } + + if offsets.len() != 6 { + return Err(ParameterError::InvalidLength { expected: 6, - found: sign_corrections_yaml.as_vec().unwrap().len(), - })?; + found: offsets.len(), + }); + } - Ok(Parameters { - a1: params["a1"].as_f64().ok_or_else(|| ParameterError::MissingField("a1".into()))?, - a2: params["a2"].as_f64().ok_or_else(|| ParameterError::MissingField("a2".into()))?, - b: params["b"].as_f64().ok_or_else(|| ParameterError::MissingField("b".into()))?, - c1: params["c1"].as_f64().ok_or_else(|| ParameterError::MissingField("c1".into()))?, - c2: params["c2"].as_f64().ok_or_else(|| ParameterError::MissingField("c2".into()))?, - c3: params["c3"].as_f64().ok_or_else(|| ParameterError::MissingField("c3".into()))?, - c4: params["c4"].as_f64().ok_or_else(|| ParameterError::MissingField("c4".into()))?, - offsets, - sign_corrections, - }) + let offsets: [f64; 6] = offsets.try_into().unwrap(); // Safe now, we ensured it's of length 6 + Ok(offsets) } + /// Parses angles from strings in degrees format or plain floats. fn parse_degrees(s: &str) -> Result { if let Some(angle) = s.strip_prefix("deg(") diff --git a/src/parameters_robots.rs b/src/parameters_robots.rs index 14ce60b..a86073e 100644 --- a/src/parameters_robots.rs +++ b/src/parameters_robots.rs @@ -18,6 +18,7 @@ pub mod opw_kinematics { c4: 0.0, offsets: [0.0; 6], sign_corrections: [1; 6], + dof: 6 } } diff --git a/src/tests/data/fanuc/fanuc_m16ib20.yaml b/src/tests/data/fanuc/fanuc_m16ib20.yaml index a03ca4f..2f53da3 100644 --- a/src/tests/data/fanuc/fanuc_m16ib20.yaml +++ b/src/tests/data/fanuc/fanuc_m16ib20.yaml @@ -9,5 +9,5 @@ opw_kinematics_geometric_parameters: c2: 0.77 c3: 0.74 c4: 0.10 -opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, deg(180.0)] +opw_kinematics_joint_offsets: [0, 0, deg(-90), 0, 0, deg(180)] opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] \ No newline at end of file diff --git a/src/tests/data/fanuc/fanuc_m16ib20_5dof.yaml b/src/tests/data/fanuc/fanuc_m16ib20_5dof.yaml new file mode 100644 index 0000000..327e830 --- /dev/null +++ b/src/tests/data/fanuc/fanuc_m16ib20_5dof.yaml @@ -0,0 +1,15 @@ +# +# Artificicial test data set, derived from Fanuc m16ib20 +# +opw_kinematics_geometric_parameters: + a1: 0.15 + a2: -0.10 + b: 0.0 + c1: 0.525 + c2: 0.77 + c3: 0.74 + c4: 0.10 + dof: 5 # 5 DOF robot +opw_kinematics_joint_offsets: [0, 0, deg(-90), 0, 0, deg(180)] +opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1] # 5 members ok + diff --git a/src/tests/mod.rs b/src/tests/mod.rs index bb5ba11..8e922e4 100644 --- a/src/tests/mod.rs +++ b/src/tests/mod.rs @@ -2,3 +2,7 @@ mod testcases; mod constraint_test; mod constraint_test_various; mod urdf_extractor; +mod tool_base_test; +mod test_utils; +mod test_from_yaml; +mod test_individual_link_positions; diff --git a/src/tests/test_from_yaml.rs b/src/tests/test_from_yaml.rs new file mode 100644 index 0000000..bca800f --- /dev/null +++ b/src/tests/test_from_yaml.rs @@ -0,0 +1,70 @@ + +#[cfg(test)] +mod tests { + use crate::parameters::opw_kinematics::Parameters; + + const READ_ERROR: &'static str = "Failed to load parameters from file"; + + #[test] + fn test_parameters_from_yaml() { + let filename = "src/tests/data/fanuc/fanuc_m16ib20.yaml"; + let loaded = + Parameters::from_yaml_file(filename).expect(READ_ERROR); + + let expected = Parameters { + a1: 0.15, + a2: -0.10, + b: 0.0, + c1: 0.525, + c2: 0.77, + c3: 0.74, + c4: 0.10, + offsets: [0.0, 0.0, -90.0_f64.to_radians(), 0.0, 0.0, 180.0_f64.to_radians()], + sign_corrections: [1, 1, -1, -1, -1, -1], + dof: 6, + }; + + + assert_eq!(expected.a1, loaded.a1); + assert_eq!(expected.a2, loaded.a2); + assert_eq!(expected.b, loaded.b); + assert_eq!(expected.c1, loaded.c1); + assert_eq!(expected.c2, loaded.c2); + assert_eq!(expected.c3, loaded.c3); + assert_eq!(expected.c4, loaded.c4); + assert_eq!(expected.offsets, loaded.offsets); + assert_eq!(expected.sign_corrections, loaded.sign_corrections); + } + + #[test] + fn test_parameters_from_yaml_5dof() { + let filename = "src/tests/data/fanuc/fanuc_m16ib20_5dof.yaml"; + let loaded = + Parameters::from_yaml_file(filename).expect(READ_ERROR); + + let expected = Parameters { + a1: 0.15, + a2: -0.10, + b: 0.0, + c1: 0.525, + c2: 0.77, + c3: 0.74, + c4: 0.10, + offsets: [0.0, 0.0, -90.0_f64.to_radians(), 0.0, 0.0, 180.0_f64.to_radians()], + sign_corrections: [1, 1, -1, -1, -1, 0], // Sign corrections last member 0 + dof: 5, // Degrees of freedom 5 + }; + + + assert_eq!(expected.a1, loaded.a1); + assert_eq!(expected.a2, loaded.a2); + assert_eq!(expected.b, loaded.b); + assert_eq!(expected.c1, loaded.c1); + assert_eq!(expected.c2, loaded.c2); + assert_eq!(expected.c3, loaded.c3); + assert_eq!(expected.c4, loaded.c4); + assert_eq!(expected.offsets, loaded.offsets); + assert_eq!(expected.sign_corrections, loaded.sign_corrections); + assert_eq!(expected.dof, loaded.dof); + } +} \ No newline at end of file diff --git a/src/tests/test_individual_link_positions.rs b/src/tests/test_individual_link_positions.rs new file mode 100644 index 0000000..48e9e9a --- /dev/null +++ b/src/tests/test_individual_link_positions.rs @@ -0,0 +1,279 @@ +use std::sync::Arc; +use nalgebra::{Isometry3, Translation3, UnitQuaternion}; +use crate::frame::Frame; +use crate::kinematic_traits::{Kinematics, Pose}; +use crate::kinematics_impl::OPWKinematics; +use crate::parameters::opw_kinematics::Parameters; +use crate::tool::{Base, Tool}; + +const SMALL: f64 = 1e-6; + +#[test] +fn test_forward_kinematics_straight_up() { + // Define the joint angles for pointing the robot straight up (all 0) + let joints: [f64; 6] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + let parameters: Parameters = create_parameters(); + let opw_kinematics: OPWKinematics = OPWKinematics::new(parameters); + let poses = opw_kinematics.forward_with_joint_poses(&joints); + + // Expected positions + let expected_positions = [ + (0.0, 0.0, parameters.c1), // Pose 1: Base link Z height + (parameters.a1, 0.0, parameters.c1), // Pose 2: Link 1 along X by a1, same Z + (parameters.a1, 0.0, parameters.c1 + parameters.c2), // Pose 3: Add link 2 length along Z + (parameters.a1, 0.0, parameters.c1 + parameters.c2 + parameters.c3), // Pose 4: Add link 3 length along Z + (parameters.a1, 0.0, parameters.c1 + parameters.c2 + parameters.c3), // Pose 5: Same Z as Pose 4, no c4 yet + (parameters.a1, 0.0, parameters.c1 + parameters.c2 + parameters.c3 + parameters.c4), // Pose 6: Add wrist length c4 + ]; + + // Check all poses for correct X, Y, and Z translation + check_xyz(poses, expected_positions); + + let standing: UnitQuaternion = UnitQuaternion::identity(); + + // Check that the quaternion (rotation) is identity quaternion for all poses + for (i, pose) in poses.iter().enumerate() { + check_rotation(standing, i, &pose.rotation); + } +} + +#[test] +fn test_forward_kinematics_j2_rotated_90_degrees() { + // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0 + let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0]; + let parameters: Parameters = create_parameters(); + let opw_kinematics: OPWKinematics = OPWKinematics::new(parameters); + let poses = opw_kinematics.forward_with_joint_poses(&joints); + + // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2 + let expected_positions = [ + (0.0, 0.0, parameters.c1), + (parameters.a1, 0.0, parameters.c1), + (parameters.a1 + parameters.c2, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4, 0.0, parameters.c1), + ]; + + // Check all poses for correct X, Y, and Z translation + check_xyz(poses, expected_positions); + + let standing: UnitQuaternion = UnitQuaternion::identity(); + let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0); + + // Check quaternions for each pose (after J2 rotation) + for (i, pose) in poses.iter().enumerate() { + // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied) + if i <= 1 { + check_rotation(standing, i, &pose.rotation); + } else { + // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis) + check_rotation(lying, i, &pose.rotation); + } + } +} + +#[test] +fn test_tool_forward_kinematics() { + let tool_offset = 1.0; + + // Create an instance of OPWKinematics + let parameters: Parameters = create_parameters(); + let robot_without_tool = OPWKinematics::new(parameters); + + // Tool extends 1 meter in the Z direction + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, tool_offset).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let robot_with_tool = Tool { + robot: Arc::new(robot_without_tool), + tool: tool_translation, + }; + + // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0 + let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0]; + + // Compute forward kinematics for the given joint angles + let poses = robot_with_tool.forward_with_joint_poses(&joints); + + // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2, plus the tool offset in Z direction + let expected_positions = [ + (0.0, 0.0, parameters.c1), + (parameters.a1, 0.0, parameters.c1), + (parameters.a1 + parameters.c2, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4 + tool_offset, 0.0, parameters.c1), // Tool adds 1 meter in X + ]; + + // Check all poses for correct X, Y, and Z translation + check_xyz(poses, expected_positions); + + let standing: UnitQuaternion = UnitQuaternion::identity(); + let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0); + + // Check quaternions for each pose (after J2 rotation) + for (i, pose) in poses.iter().enumerate() { + // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied) + if i <= 1 { + check_rotation(standing, i, &pose.rotation); + } else { + // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis) + check_rotation(lying, i, &pose.rotation); + } + } +} + +#[test] +fn test_frame_forward_kinematics() { + let frame_offset = 1.0; + + // Create an instance of OPWKinematics + let parameters: Parameters = create_parameters(); + let robot_without_tool = OPWKinematics::new(parameters); + + // Tool extends 1 meter in the Z direction + let frame_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, frame_offset).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let robot_with_tool = Frame { + robot: Arc::new(robot_without_tool), + frame: frame_translation, + }; + + // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0 + let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0]; + + // Compute forward kinematics for the given joint angles + let poses = robot_with_tool.forward_with_joint_poses(&joints); + + // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2, plus the tool offset in Z direction + let expected_positions = [ + (0.0, 0.0, parameters.c1), + (parameters.a1, 0.0, parameters.c1), + (parameters.a1 + parameters.c2, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1), + (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4 + frame_offset, 0.0, parameters.c1), // Frame adds 1 meter in X + ]; + + // Check all poses for correct X, Y, and Z translation + check_xyz(poses, expected_positions); + + let standing: UnitQuaternion = UnitQuaternion::identity(); + let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0); + + // Check quaternions for each pose (after J2 rotation) + for (i, pose) in poses.iter().enumerate() { + // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied) + if i <= 1 { + check_rotation(standing, i, &pose.rotation); + } else { + // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis) + check_rotation(lying, i, &pose.rotation); + } + } +} + +#[test] +fn test_base_forward_kinematics() { + let base_height = 1.0; + + let parameters: Parameters = create_parameters(); + let robot_without_base = OPWKinematics::new(parameters); + + // 1 meter high pedestal + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, base_height).into(), + UnitQuaternion::identity(), + ); + + // Create the Base instance with the transformation + let robot_with_base = Base { + robot: Arc::new(robot_without_base), + base: base_translation, + }; + + // Define the joint angles: J2 rotated 90 degrees (π/2), other joints are 0 + let joints = [0.0, std::f64::consts::FRAC_PI_2, 0.0, 0.0, 0.0, 0.0]; + + // Compute forward kinematics for the given joint angles + let poses = robot_with_base.forward_with_joint_poses(&joints); + + // Expected positions: Robot should extend horizontally due to 90-degree rotation of J2, plus the base offset in Z direction + let expected_positions = [ + (0.0, 0.0, parameters.c1 + base_height), // Base adds 1 meter in Z + (parameters.a1, 0.0, parameters.c1 + base_height), + (parameters.a1 + parameters.c2, 0.0, parameters.c1 + base_height), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1 + base_height), + (parameters.a1 + parameters.c2 + parameters.c3, 0.0, parameters.c1 + base_height), + (parameters.a1 + parameters.c2 + parameters.c3 + parameters.c4, 0.0, parameters.c1 + base_height), + ]; + + // Check all poses for correct X, Y, and Z translation + check_xyz(poses, expected_positions); + + let standing: UnitQuaternion = UnitQuaternion::identity(); + let lying = UnitQuaternion::from_euler_angles(0.0, std::f64::consts::FRAC_PI_2, 0.0); + + // Check quaternions for each pose (after J2 rotation) + for (i, pose) in poses.iter().enumerate() { + // Pose 1 and Pose 2 should still have identity quaternions (no rotation applied) + if i <= 1 { + check_rotation(standing, i, &pose.rotation); + } else { + // From Pose 3 onwards, J2 rotation is applied (90 degrees around Y-axis) + check_rotation(lying, i, &pose.rotation); + } + } +} + +fn create_parameters() -> Parameters { + let parameters = Parameters { + a1: 0.150, + a2: 0.000, + b: 0.000, + c1: 0.550, + c2: 0.550, // Example model-specific value + c3: 0.600, // Example model-specific value + c4: 0.110, + ..Parameters::new() // Any other required fields + }; + parameters +} + +fn check_xyz(poses: [Pose; 6], expected_positions: [(f64, f64, f64); 6]) { + for (i, &(expected_x, expected_y, expected_z)) in expected_positions.iter().enumerate() { + let translation = poses[i].translation.vector; + + // X, Y, and Z coordinates should match the expected positions + assert!((translation[0] as f64 - expected_x).abs() < SMALL, "Pose {} X- expected {}, got {}", i + 1, expected_x, translation[0]); + assert!((translation[1] as f64 - expected_y).abs() < SMALL, "Pose {} Y- expected {}, got {}", i + 1, expected_y, translation[1]); + assert!((translation[2] as f64 - expected_z).abs() < SMALL, "Pose {} Z- expected {}, got {}", i + 1, expected_z, translation[2]); + } +} + +fn check_rotation(standing: UnitQuaternion, i: usize, quaternion: &UnitQuaternion) { + assert!( + (quaternion.w as f64 - standing.w).abs() < SMALL, + "Pose {} quaternion w mismatch", i + 1 + ); + assert!( + (quaternion.i as f64 - standing.i).abs() < SMALL, + "Pose {} quaternion i mismatch", i + 1 + ); + assert!( + (quaternion.j as f64 - standing.j).abs() < SMALL, + "Pose {} quaternion j mismatch", i + 1 + ); + assert!( + (quaternion.k as f64 - standing.k).abs() < SMALL, + "Pose {} quaternion k mismatch", i + 1 + ); +} diff --git a/src/tests/test_utils.rs b/src/tests/test_utils.rs new file mode 100644 index 0000000..b0db67c --- /dev/null +++ b/src/tests/test_utils.rs @@ -0,0 +1,217 @@ +use std::collections::HashMap; +use std::f64::consts::PI; +use std::fs::File; +use std::io::Read; + +#[derive(Debug)] +pub(crate) struct Pose { + translation: [f64; 3], + quaternion: [f64; 4], // Assuming [x, y, z, w] ordering here +} + +#[derive(Debug)] +pub struct Case { + pub id: i32, + pub(crate) parameters: String, + pub(crate) joints: [f64; 6], + pub _solutions: Vec<[f64; 6]>, + pub(crate) pose: Pose, +} + +impl Case { + // Method to return joints in radians + pub fn joints_in_radians(&self) -> [f64; 6] { + let mut joints_in_radians = [0.0; 6]; + for (i, &joint) in self.joints.iter().enumerate() { + joints_in_radians[i] = joint.to_radians(); + } + joints_in_radians + } +} + +use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion}; +use yaml_rust2::{Yaml, YamlLoader}; +use crate::kinematic_traits::{Joints, Solutions}; +use crate::parameters::opw_kinematics::Parameters; + +impl Pose { + pub fn to_isometry(&self) -> Isometry3 { + let translation = Translation3::new(self.translation[0], self.translation[1], self.translation[2]); + + // Adjusting quaternion creation to match [x, y, z, w] ordering + let quaternion = UnitQuaternion::from_quaternion(Quaternion::new( + self.quaternion[3], // w + self.quaternion[0], // x + self.quaternion[1], // y + self.quaternion[2], // z + )); + + Isometry3::from_parts(translation, quaternion) + } + + pub fn from_isometry(isometry: &Isometry3) -> Self { + let translation = isometry.translation.vector; + + // Extract the quaternion from isometry (rotation part) + let quaternion = isometry.rotation.quaternion(); + + Pose { + translation: [translation.x, translation.y, translation.z], + quaternion: [quaternion.i, quaternion.j, quaternion.k, quaternion.w], // [x, y, z, w] ordering + } + } +} + +pub(crate) fn load_yaml(file_path: &str) -> Result, String> { + let mut file = File::open(file_path).map_err(|e| e.to_string())?; + let mut contents = String::new(); + file.read_to_string(&mut contents).map_err(|e| e.to_string())?; + + let docs = YamlLoader::load_from_str(&contents).map_err(|e| e.to_string())?; + let cases_yaml = &docs[0]["cases"]; + + let mut cases: Vec = Vec::new(); + if let Some(cases_vec) = cases_yaml.as_vec() { + for case_yaml in cases_vec { + let id = case_yaml["id"].as_i64().ok_or("Missing id")? as i32; + let parameters = case_yaml["parameters"].as_str().ok_or("Missing parameters")?.to_string(); + let joints = parse_array::(&case_yaml["joints"])?; + let solutions = parse_solutions(&case_yaml["solutions"])?; + let pose = parse_pose(&case_yaml["pose"])?; + + cases.push(Case { + id, + parameters, + joints, + _solutions: solutions, + pose, + }); + } + } else { + return Err("Expected 'cases' to be a sequence".to_string()); + } + + Ok(cases) +} + +fn parse_array(yaml: &Yaml) -> Result<[T; N], String> +where + ::Err: std::fmt::Display, // Ensure the error type of T's FromStr can be displayed +{ + let vec = yaml.as_vec().ok_or("Expected an array in YAML")?; + + if vec.is_empty() { + return Err("Array in YAML is empty, cannot initialize".to_string()); + } + + // Initialize the array using the first element if it can be parsed, otherwise return an error + let first_value = vec.get(0) + .ok_or_else(|| "Array is non-empty but no first item found".to_string()) + .and_then(|item| { + match item { + Yaml::Real(s) => s.parse::() + .map_err(|e| format!("Failed to parse first item as real: {}, found: '{}'", e, s)), + Yaml::Integer(i) => i.to_string().parse::() + .map_err(|e| format!("Failed to parse first item as integer: {}, found: '{}'", e, i)), + _ => Err(format!("First item is not a real or integer value, found: {:?}", item)) + } + })?; + + let mut array: [T; N] = [first_value; N]; // Use the first parsed value to initialize the array + + // Parse each element in the vector and fill the array + for (i, item) in vec.iter().enumerate() { + array[i] = match item { + Yaml::Real(s) => s.parse::() + .map_err(|e| format!("Error parsing real at index {}: {}, found: '{}'", i, e, s))?, + Yaml::Integer(i) => i.to_string() + .parse::().map_err(|e| format!("Error parsing integer at index {}: {}, found: '{}'", i, e, i))?, + _ => return Err(format!("Expected a real or integer value at index {}, found: {:?}", i, item)) + }; + } + + Ok(array) +} + +fn parse_solutions(yaml: &Yaml) -> Result, String> { + let mut solutions = Vec::new(); + for solution_yaml in yaml.as_vec().ok_or("Expected solutions array")? { + solutions.push(parse_array::(solution_yaml)?); + } + Ok(solutions) +} + +fn parse_pose(yaml: &Yaml) -> Result { + let translation = parse_array::(&yaml["translation"])?; + let quaternion = parse_array::(&yaml["quaternion"])?; + + Ok(Pose { + translation, + quaternion, + }) +} + +pub(crate) fn are_isometries_approx_equal(a: &Isometry3, b: &Isometry3, tolerance: f64) -> bool { + let translation_diff = a.translation.vector - b.translation.vector; + if translation_diff.norm() > tolerance { + return false; + } + + // Check if the rotation components are approximately equal + // This part is a bit more complex due to quaternion properties. + // One way is to calculate the angle between the two quaternions and see if it's within the tolerance. + // This involves converting the unit quaternion difference into an angle. + let rotation_diff = a.rotation.inverse() * b.rotation; + let angle = rotation_diff.angle(); + + angle.abs() <= tolerance +} + +pub(crate) fn create_parameter_map() -> HashMap { + // Create map to get actual parameters that are not in the yaml file (maybe should be?) + let all_parameters: HashMap = vec![ + (String::from("Irb2400_10"), Parameters::irb2400_10()), + (String::from("KukaKR6_R700_sixx"), Parameters::kuka_kr6_r700_sixx()), + (String::from("Fanuc_r2000ib_200r"), Parameters::fanuc_r2000ib_200r()), + (String::from("Staubli_tx40"), Parameters::staubli_tx40()), + (String::from("Irb2600_12_165"), Parameters::irb2600_12_165()), + (String::from("Irb4600_60_205"), Parameters::irb4600_60_205()), + (String::from("Staubli_tx2_140"), Parameters::staubli_tx2_140()), + (String::from("Staubli_tx2_160"), Parameters::staubli_tx2_160()), + (String::from("Staubli_tx2_160l"), Parameters::staubli_tx2_160l()), + ] + .into_iter() + .collect(); + all_parameters +} + +/// Check if 'expected' exists in the given vector of solutions. This function is also +/// used by other tests. +pub fn found_joints_approx_equal(solutions: &Solutions, expected: &Joints, tolerance: f64) -> Option { + for sol_idx in 0..solutions.len() { + // println!("Checking solution at index {}", sol_idx); + + let mut solution_matches = true; + for joint_idx in 0..6 { + let computed = solutions[sol_idx][joint_idx]; + let asserted = expected[joint_idx]; + + let diff = (computed - asserted).abs(); + //println!("Column value: {}, Expected value: {}, Difference: {}", + // computed, asserted, diff); + + if diff >= tolerance && (diff - 2. * PI).abs() > tolerance { + // For angles, 360 degree difference means the same angle. + solution_matches = false; + break; + } + } + + if solution_matches { + return Some(sol_idx as i32); // Return the index of the matching solution + } + } + + println!("No matching solution found"); + return None; // Explicitly indicate that no matching column was found +} \ No newline at end of file diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs index ee7fd3c..6f8c5ec 100644 --- a/src/tests/testcases.rs +++ b/src/tests/testcases.rs @@ -1,215 +1,16 @@ -use std::fs::File; -use std::io::Read; - -#[derive(Debug)] -struct Pose { - translation: [f64; 3], - quaternion: [f64; 4], // Assuming [x, y, z, w] ordering here -} - -#[derive(Debug)] -struct Case { - id: i32, - parameters: String, - joints: [f64; 6], - _solutions: Vec<[f64; 6]>, - pose: Pose, -} - -impl Case { - // Method to return joints in radians - pub fn joints_in_radians(&self) -> [f64; 6] { - let mut joints_in_radians = [0.0; 6]; - for (i, &joint) in self.joints.iter().enumerate() { - joints_in_radians[i] = joint.to_radians(); - } - joints_in_radians - } -} - -use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion}; -use yaml_rust2::{Yaml, YamlLoader}; - -impl Pose { - pub fn to_isometry(&self) -> Isometry3 { - let translation = Translation3::new(self.translation[0], self.translation[1], self.translation[2]); - - // Adjusting quaternion creation to match [x, y, z, w] ordering - let quaternion = UnitQuaternion::from_quaternion(Quaternion::new( - self.quaternion[3], // w - self.quaternion[0], // x - self.quaternion[1], // y - self.quaternion[2], // z - )); - - Isometry3::from_parts(translation, quaternion) - } - - pub fn from_isometry(isometry: &Isometry3) -> Self { - let translation = isometry.translation.vector; - - // Extract the quaternion from isometry (rotation part) - let quaternion = isometry.rotation.quaternion(); - - Pose { - translation: [translation.x, translation.y, translation.z], - quaternion: [quaternion.i, quaternion.j, quaternion.k, quaternion.w], // [x, y, z, w] ordering - } - } -} - -fn load_yaml(file_path: &str) -> Result, String> { - let mut file = File::open(file_path).map_err(|e| e.to_string())?; - let mut contents = String::new(); - file.read_to_string(&mut contents).map_err(|e| e.to_string())?; - - let docs = YamlLoader::load_from_str(&contents).map_err(|e| e.to_string())?; - let cases_yaml = &docs[0]["cases"]; - - let mut cases: Vec = Vec::new(); - if let Some(cases_vec) = cases_yaml.as_vec() { - for case_yaml in cases_vec { - let id = case_yaml["id"].as_i64().ok_or("Missing id")? as i32; - let parameters = case_yaml["parameters"].as_str().ok_or("Missing parameters")?.to_string(); - let joints = parse_array::(&case_yaml["joints"])?; - let solutions = parse_solutions(&case_yaml["solutions"])?; - let pose = parse_pose(&case_yaml["pose"])?; - - cases.push(Case { - id, - parameters, - joints, - _solutions: solutions, - pose, - }); - } - } else { - return Err("Expected 'cases' to be a sequence".to_string()); - } - - Ok(cases) -} - -fn parse_array(yaml: &Yaml) -> Result<[T; N], String> - where - ::Err: std::fmt::Display, // Ensure the error type of T's FromStr can be displayed -{ - let vec = yaml.as_vec().ok_or("Expected an array in YAML")?; - - if vec.is_empty() { - return Err("Array in YAML is empty, cannot initialize".to_string()); - } - - // Initialize the array using the first element if it can be parsed, otherwise return an error - let first_value = vec.get(0) - .ok_or_else(|| "Array is non-empty but no first item found".to_string()) - .and_then(|item| { - match item { - Yaml::Real(s) => s.parse::() - .map_err(|e| format!("Failed to parse first item as real: {}, found: '{}'", e, s)), - Yaml::Integer(i) => i.to_string().parse::() - .map_err(|e| format!("Failed to parse first item as integer: {}, found: '{}'", e, i)), - _ => Err(format!("First item is not a real or integer value, found: {:?}", item)) - } - })?; - - let mut array: [T; N] = [first_value; N]; // Use the first parsed value to initialize the array - - // Parse each element in the vector and fill the array - for (i, item) in vec.iter().enumerate() { - array[i] = match item { - Yaml::Real(s) => s.parse::() - .map_err(|e| format!("Error parsing real at index {}: {}, found: '{}'", i, e, s))?, - Yaml::Integer(i) => i.to_string() - .parse::().map_err(|e| format!("Error parsing integer at index {}: {}, found: '{}'", i, e, i))?, - _ => return Err(format!("Expected a real or integer value at index {}, found: {:?}", i, item)) - }; - } - - Ok(array) -} - -fn parse_solutions(yaml: &Yaml) -> Result, String> { - let mut solutions = Vec::new(); - for solution_yaml in yaml.as_vec().ok_or("Expected solutions array")? { - solutions.push(parse_array::(solution_yaml)?); - } - Ok(solutions) -} - -fn parse_pose(yaml: &Yaml) -> Result { - let translation = parse_array::(&yaml["translation"])?; - let quaternion = parse_array::(&yaml["quaternion"])?; - - Ok(Pose { - translation, - quaternion, - }) -} - -fn are_isometries_approx_equal(a: &Isometry3, b: &Isometry3, tolerance: f64) -> bool { - let translation_diff = a.translation.vector - b.translation.vector; - if translation_diff.norm() > tolerance { - return false; - } - - // Check if the rotation components are approximately equal - // This part is a bit more complex due to quaternion properties. - // One way is to calculate the angle between the two quaternions and see if it's within the tolerance. - // This involves converting the unit quaternion difference into an angle. - let rotation_diff = a.rotation.inverse() * b.rotation; - let angle = rotation_diff.angle(); - - angle.abs() <= tolerance -} - #[cfg(test)] mod tests { - use std::collections::HashMap; use std::f64::consts::PI; - use std::sync::Arc; - use crate::kinematic_traits::{Joints, Kinematics, Singularity, Solutions}; + use nalgebra::{UnitQuaternion, Vector3}; + use crate::kinematic_traits::{Kinematics, Singularity}; use crate::parameters::opw_kinematics::Parameters; use crate::kinematics_impl::OPWKinematics; - use crate::tool::{Base, Tool}; - use crate::utils::{dump_joints, dump_solutions_degrees}; - use super::*; - - /// Check if 'expected' exists in the given vector of solutions. This function is also - /// used by other tests. - pub fn found_joints_approx_equal(solutions: &Solutions, expected: &Joints, tolerance: f64) -> Option { - for sol_idx in 0..solutions.len() { - // println!("Checking solution at index {}", sol_idx); - - let mut solution_matches = true; - for joint_idx in 0..6 { - let computed = solutions[sol_idx][joint_idx]; - let asserted = expected[joint_idx]; - - let diff = (computed - asserted).abs(); - //println!("Column value: {}, Expected value: {}, Difference: {}", - // computed, asserted, diff); - - if diff >= tolerance && (diff - 2. * PI).abs() > tolerance { - // For angles, 360 degree difference means the same angle. - solution_matches = false; - break; - } - } - - if solution_matches { - return Some(sol_idx as i32); // Return the index of the matching solution - } - } - - println!("No matching solution found"); - return None; // Explicitly indicate that no matching column was found - } + use crate::tests::test_utils; #[test] fn test_load_yaml() { let filename = "src/tests/data/cases.yaml"; - let result = load_yaml(filename); + let result = test_utils::load_yaml(filename); if let Err(e) = &result { println!("Error loading or parsing YAML file: {}", e); @@ -226,10 +27,10 @@ mod tests { #[test] fn test_forward_ik() { let filename = "src/tests/data/cases.yaml"; - let result = load_yaml(filename); + let result = test_utils::load_yaml(filename); assert!(result.is_ok(), "Failed to load or parse the YAML file: {}", result.unwrap_err()); let cases = result.expect("Expected a valid Cases struct after parsing"); - let all_parameters = create_parameter_map(); + let all_parameters = test_utils::create_parameter_map(); println!("Forward IK: {} test cases", cases.len()); for case in cases.iter() { @@ -240,9 +41,9 @@ mod tests { // Try forward on the initial data set first. let ik = kinematics.forward(&case.joints_in_radians()); - let pose = Pose::from_isometry(&ik); + let pose = test_utils::Pose::from_isometry(&ik); - if !are_isometries_approx_equal(&ik, &case.pose.to_isometry(), 0.00001) { + if !test_utils::are_isometries_approx_equal(&ik, &case.pose.to_isometry(), 0.00001) { println!("Seems not equal"); println!("joints: {:?} ", &case.joints); println!("case: {:?} ", &pose); @@ -254,13 +55,45 @@ mod tests { } } + #[test] + fn test_forward_ik_with_joint_poses() { + let filename = "src/tests/data/cases.yaml"; + let result = test_utils::load_yaml(filename); + assert!(result.is_ok(), "Failed to load or parse the YAML file: {}", result.unwrap_err()); + let cases = result.expect("Expected a valid Cases struct after parsing"); + let all_parameters = test_utils::create_parameter_map(); + println!("Forward IK: {} test cases", cases.len()); + + for case in cases.iter() { + let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| { + panic!("Parameters for the robot [{}] are unknown", &case.parameters) + }); + let kinematics = OPWKinematics::new(parameters.clone()); + + // This test only checks the final pose so far. + let ik = kinematics.forward_with_joint_poses(&case.joints_in_radians())[5]; + let pose = test_utils::Pose::from_isometry(&ik); + + if !test_utils::are_isometries_approx_equal(&ik, &case.pose.to_isometry(), 0.00001) { + println!("Seems not equal"); + println!("joints: {:?} ", &case.joints); + println!("case: {:?} ", &pose); + println!("IK : {:?} ", &case.pose); + println!("{}", parameters.to_yaml()); + + + panic!("Forward kinematics of the primary pose seems not equal"); + } + } + } + #[test] fn test_inverse_ik() { let filename = "src/tests/data/cases.yaml"; - let result = load_yaml(filename); + let result = test_utils::load_yaml(filename); assert!(result.is_ok(), "Failed to load or parse the YAML file"); let cases = result.expect("Expected a valid Cases struct after parsing"); - let all_parameters = create_parameter_map(); + let all_parameters = test_utils::create_parameter_map(); println!("Inverse IK: {} test cases", cases.len()); for case in cases.iter() { @@ -273,8 +106,8 @@ mod tests { if kinematics.kinematic_singularity(&case.joints_in_radians()).is_none() { // Try forward on the initial data set first. let solutions = kinematics.inverse(&case.pose.to_isometry()); - if found_joints_approx_equal(&solutions, &case.joints_in_radians(), - 0.001_f64.to_radians()).is_none() { + if test_utils::found_joints_approx_equal(&solutions, &case.joints_in_radians(), + 0.001_f64.to_radians()).is_none() { println!("**** No valid solution for case {} on {} ****", case.id, case.parameters); let joints_str = &case.joints.iter() .map(|&val| format!("{:5.2}", val)) @@ -302,10 +135,10 @@ mod tests { #[test] fn test_inverse_ik_continuing() { let filename = "src/tests/data/cases.yaml"; - let result = load_yaml(filename); + let result = test_utils::load_yaml(filename); assert!(result.is_ok(), "Failed to load or parse the YAML file"); let cases = result.expect("Expected a valid Cases struct after parsing"); - let all_parameters = create_parameter_map(); + let all_parameters = test_utils::create_parameter_map(); println!("Inverse IK: {} test cases", cases.len()); for case in cases.iter() { @@ -319,8 +152,8 @@ mod tests { let solutions = kinematics.inverse_continuing( &case.pose.to_isometry(), &case.joints_in_radians()); let found_matching = - found_joints_approx_equal(&solutions, &case.joints_in_radians(), - 0.001_f64.to_radians()); + test_utils::found_joints_approx_equal(&solutions, &case.joints_in_radians(), + 0.001_f64.to_radians()); if !matches!(found_matching, Some(0)) { println!("**** No valid solution: {:?} for case {} on {} ****", found_matching, case.id, case.parameters); @@ -348,6 +181,41 @@ mod tests { } } + #[test] + fn test_inverse_positioning_continuing() { + let filename = "src/tests/data/cases.yaml"; + let result = test_utils::load_yaml(filename); + assert!(result.is_ok(), "Failed to load or parse the YAML file"); + let cases = result.expect("Expected a valid Cases struct after parsing"); + let all_parameters = test_utils::create_parameter_map(); + println!("Inverse Positioning IK: {} test cases", cases.len()); + + for case in cases.iter() { + let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| { + panic!("Parameters for the robot [{}] are unknown", &case.parameters) + }); + let kinematics = OPWKinematics::new(parameters.clone()); + + // Use translation instead of full pose + let isometry = case.pose.to_isometry(); + let solutions = kinematics.inverse_continuing_5dof( + &isometry, &case.joints_in_radians()); + assert!(solutions.len() > 0); + + for solution in solutions { + // Check if TCP stays in the same location + let reconstructed = kinematics.forward(&solution); + let reconstructed_translation: Vector3 = reconstructed.translation.vector; + let expected_translation: Vector3 = isometry.translation.vector; + let translation_diff = (reconstructed_translation - expected_translation).norm(); + assert!( + translation_diff < 1E-6, + "Reconstructed translation does not match. Diff: {}", translation_diff + ); + } + } + } + #[test] fn test_singularity_a_continuing() { // This robot has both A and B type singularity @@ -392,31 +260,11 @@ mod tests { // Make sure singularity is found and included let found_matching = - found_joints_approx_equal(&solutions, &joints_in_radians, 0.001_f64.to_radians()); + test_utils::found_joints_approx_equal(&solutions, &joints_in_radians, 0.001_f64.to_radians()); assert!(matches!(found_matching, Some(0)), "Fully matching joints must come first. Expected Some(0), got {:?}", found_matching); } - - fn create_parameter_map() -> HashMap { -// Create map to get actual parameters that are not in the yaml file (maybe should be?) - let all_parameters: HashMap = vec![ - (String::from("Irb2400_10"), Parameters::irb2400_10()), - (String::from("KukaKR6_R700_sixx"), Parameters::kuka_kr6_r700_sixx()), - (String::from("Fanuc_r2000ib_200r"), Parameters::fanuc_r2000ib_200r()), - (String::from("Staubli_tx40"), Parameters::staubli_tx40()), - (String::from("Irb2600_12_165"), Parameters::irb2600_12_165()), - (String::from("Irb4600_60_205"), Parameters::irb4600_60_205()), - (String::from("Staubli_tx2_140"), Parameters::staubli_tx2_140()), - (String::from("Staubli_tx2_160"), Parameters::staubli_tx2_160()), - (String::from("Staubli_tx2_160l"), Parameters::staubli_tx2_160l()), - ] - .into_iter() - .collect(); - all_parameters - } - - #[test] fn test_singularity_a() { // Assuming joint[4] close to π triggers A type singularity @@ -439,91 +287,33 @@ mod tests { } #[test] - fn test_parameters_from_yaml() { - let filename = "src/tests/data/fanuc/fanuc_m16ib20.yaml"; - let loaded = - Parameters::from_yaml_file(filename).expect("Failed to load parameters from file"); - - let expected = Parameters { - a1: 0.15, - a2: -0.10, - b: 0.0, - c1: 0.525, - c2: 0.77, - c3: 0.74, - c4: 0.10, - offsets: [0.0, 0.0, -90.0_f64.to_radians(), 0.0, 0.0, 180.0_f64.to_radians()], - sign_corrections: [1, 1, -1, -1, -1, -1], - }; - - - assert_eq!(expected.a1, loaded.a1); - assert_eq!(expected.a2, loaded.a2); - assert_eq!(expected.b, loaded.b); - assert_eq!(expected.c1, loaded.c1); - assert_eq!(expected.c2, loaded.c2); - assert_eq!(expected.c3, loaded.c3); - assert_eq!(expected.c4, loaded.c4); - assert_eq!(expected.offsets, loaded.offsets); - assert_eq!(expected.sign_corrections, loaded.sign_corrections); - } + fn test_5dof() { + let mut parameters = Parameters::irb2400_10(); + parameters.dof = 5; // Make it 5 DOF robot + let kinematics = OPWKinematics::new(parameters); - - #[test] - fn test_complex_robot_reversible() { - let filename = "src/tests/data/cases.yaml"; - let result = load_yaml(filename); - assert!(result.is_ok(), "Failed to load or parse the YAML file: {}", result.unwrap_err()); - let cases = result.expect("Expected a valid Cases struct after parsing"); - let all_parameters = create_parameter_map(); - println!("Forward IK: {} test cases", cases.len()); - - for case in cases.iter() { - let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| { - panic!("Parameters for the robot [{}] are unknown", &case.parameters) - }); - let robot_alone = OPWKinematics::new(parameters.clone()); - - // 1 meter high pedestal - let pedestal = 0.5; - let base_translation = Isometry3::from_parts( - Translation3::new(0.0, 0.0, pedestal).into(), - UnitQuaternion::identity(), - ); - - let robot_with_base = Base { - robot: Arc::new(robot_alone), - base: base_translation, - }; - - // Tool extends 1 meter in the Z direction, envisioning something like sword - let sword = 1.0; - let tool_translation = Isometry3::from_parts( - Translation3::new(0.0, 0.0, sword).into(), - UnitQuaternion::identity(), + let joints = [0.0, 0.1, 0.2, 0.3, 0.4, PI]; + let previous = [0.0, 0.1, 0.2, 0.3, 0.4, 0.55]; + + let mut pose = kinematics.forward(&joints); + + // Wipe the rotation + pose.rotation = UnitQuaternion::identity(); + + // As this is 5 DOF robot now, J6 comes from "previous" + let solutions = kinematics.inverse_continuing(&pose, &previous); + assert!(solutions.len() > 0); + for solution in &solutions { + // J6 must be as we passed. + assert!(f64::abs(0.55 - &solution[5]) < 1E-6); + // Translation must match + let reconstructed_translation: Vector3 = kinematics.forward(solution).translation.vector; + let expected_translation: Vector3 = pose.translation.vector; + let translation_diff = (reconstructed_translation - expected_translation).norm(); + assert!( + translation_diff < 1E-6, + "Reconstructed translation does not match. Diff: {}", translation_diff ); - - // Create the Tool instance with the transformation - let kinematics = Tool { - robot: Arc::new(robot_with_base), - tool: tool_translation, - }; - - - // Try forward on the initial data set first. - let joints = case.joints_in_radians(); - let pose = kinematics.forward(&joints); - let solutions = kinematics.inverse_continuing(&pose, &joints); - - // It must be the matching solution, it must be the first in solutions. - if !matches!(found_joints_approx_equal(&solutions, &joints, - 0.001_f64.to_radians()), Some(0)) { - println!("Not found or not first:"); - dump_joints(&joints); - dump_solutions_degrees(&solutions); - panic!(); - } } - } - + } } diff --git a/src/tests/tool_base_test.rs b/src/tests/tool_base_test.rs new file mode 100644 index 0000000..50e1ecf --- /dev/null +++ b/src/tests/tool_base_test.rs @@ -0,0 +1,79 @@ +#[cfg(test)] +mod tests { + use std::sync::Arc; + use nalgebra::{Isometry3, Translation3, UnitQuaternion}; + use crate::kinematic_traits::{Kinematics}; + use crate::kinematics_impl::OPWKinematics; + use crate::tool::{Base, Tool}; + use crate::utils::{dump_joints, dump_solutions_degrees}; + use crate::tests::test_utils; + + #[test] + fn test_complex_robot_reversible() { + let filename = "src/tests/data/cases.yaml"; + let result = test_utils::load_yaml(filename); + assert!(result.is_ok(), "Failed to load or parse the YAML file: {}", result.unwrap_err()); + let cases = result.expect("Expected a valid Cases struct after parsing"); + let all_parameters = test_utils::create_parameter_map(); + println!("Forward IK: {} test cases", cases.len()); + + for case in cases.iter() { + let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| { + panic!("Parameters for the robot [{}] are unknown", &case.parameters) + }); + let robot_alone = OPWKinematics::new(parameters.clone()); + + // 1 meter high pedestal + let pedestal = 0.5; + let base_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, pedestal).into(), + UnitQuaternion::identity(), + ); + + let robot_with_base = Base { + robot: Arc::new(robot_alone), + base: base_translation, + }; + + // Tool extends 1 meter in the Z direction, envisioning something like sword + let sword = 1.0; + let tool_translation = Isometry3::from_parts( + Translation3::new(0.0, 0.0, sword).into(), + UnitQuaternion::identity(), + ); + + // Create the Tool instance with the transformation + let kinematics = Tool { + robot: Arc::new(robot_with_base), + tool: tool_translation, + }; + + + // Try forward on the initial data set first. + let joints = case.joints_in_radians(); + let pose = kinematics.forward(&joints); + let solutions = kinematics.inverse_continuing(&pose, &joints); + + // It must be the matching solution, it must be the first in solutions. + if !matches!(test_utils::found_joints_approx_equal(&solutions, &joints, + 0.001_f64.to_radians()), Some(0)) { + println!("Not found or not first:"); + dump_joints(&joints); + dump_solutions_degrees(&solutions); + panic!(); + } + + // Try also 5DOF with fixed J6: + let solutions_5dof = kinematics.inverse_5dof(&pose, 77.0_f64.to_radians()); + for solution in &solutions_5dof { + let xyz = kinematics.forward(solution).translation; + if (pose.translation.vector - xyz.vector).norm() > 1E-6 { + println!("The XYZ location {}, {}, {} of TCP is not at pose {}, {}, {}", + xyz.x, xyz.y, xyz.z, + pose.translation.x, pose.translation.y, pose.translation.z); + panic!(); + } + } + } + } +} \ No newline at end of file diff --git a/src/tool.rs b/src/tool.rs index 2d319ea..089c85a 100644 --- a/src/tool.rs +++ b/src/tool.rs @@ -81,6 +81,14 @@ impl Kinematics for Tool { self.robot.inverse(&(tcp * self.tool.inverse())) } + fn inverse_5dof(&self, tcp: &Pose, j6: f64) -> Solutions { + self.robot.inverse_5dof(&(tcp * self.tool.inverse()), j6) + } + + fn inverse_continuing_5dof(&self, tcp: &Pose, previous: &Joints) -> Solutions { + self.robot.inverse_continuing(&(tcp * self.tool.inverse()), previous) + } + fn inverse_continuing(&self, tcp: &Pose, previous: &Joints) -> Solutions { self.robot.inverse_continuing(&(tcp * self.tool.inverse()), previous) } @@ -92,6 +100,16 @@ impl Kinematics for Tool { tcp } + fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] { + // Compute the forward kinematics for the robot itself + let mut poses = self.robot.forward_with_joint_poses(joints); + + // Apply the tool transformation only to the last pose (TCP pose) + poses[5] = poses[5] * self.tool; + + poses + } + fn kinematic_singularity(&self, qs: &Joints) -> Option { self.robot.kinematic_singularity(qs) } @@ -106,10 +124,30 @@ impl Kinematics for Base { self.robot.inverse_continuing(&(self.base.inverse() * tcp), &previous) } + fn inverse_5dof(&self, tcp: &Pose, j6: f64) -> Solutions { + self.robot.inverse_5dof(&(self.base.inverse() * tcp), j6) + } + + fn inverse_continuing_5dof(&self, tcp: &Pose, previous: &Joints) -> Solutions { + self.robot.inverse_continuing_5dof(&(self.base.inverse() * tcp), &previous) + } + fn forward(&self, joints: &Joints) -> Pose { self.base * self.robot.forward(joints) } + fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6] { + // Compute the forward kinematics for the robot itself + let mut poses = self.robot.forward_with_joint_poses(joints); + + // Apply the base transformation to each pose + for pose in poses.iter_mut() { + *pose = self.base * *pose; + } + + poses + } + fn kinematic_singularity(&self, qs: &Joints) -> Option { self.robot.kinematic_singularity(qs) } diff --git a/src/urdf.rs b/src/urdf.rs index 55adddc..285361a 100644 --- a/src/urdf.rs +++ b/src/urdf.rs @@ -60,7 +60,8 @@ pub fn from_urdf_file>(path: P) -> OPWKinematics { /// - `xml_content`: A `String` containing the XML data of the URDF file. /// - `joint_names`: An optional array containing joint names. This may be required if /// names do not follow typical naming convention, or there are multiple -/// robots defined in URDF. +/// robots defined in URDF. +/// For 5 DOF robots, use the name of the tool center point instead of "joint6" /// /// # Returns /// - Returns a `Result`. On success, it contains the OPW kinematics @@ -329,6 +330,7 @@ pub struct URDFParameters { pub sign_corrections: [i8; 6], pub from: Joints, // Array to store the lower limits pub to: Joints, // Array to store the upper limits + pub dof: i8 } impl URDFParameters { @@ -344,6 +346,7 @@ impl URDFParameters { c4: self.c4, sign_corrections: self.sign_corrections, offsets: *offsets, + dof: self.dof }, Constraints::new( self.from, @@ -374,6 +377,7 @@ impl URDFParameters { c4: self.c4, sign_corrections: self.sign_corrections, offsets: *offsets, + dof: self.dof } } } @@ -384,6 +388,9 @@ fn populate_opw_parameters(joint_map: HashMap, joint_names: & let names = joint_names.unwrap_or_else( || ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]); + + let is_six_dof = joint_map.contains_key(names[5]); + opw_parameters.c4 = 0.0; // joint6 would be something like "tcp" for the 5 DOF robot. Otherwise, 0 is assumed. for j in 0..6 { let joint = joint_map @@ -468,6 +475,20 @@ fn populate_opw_parameters(joint_map: HashMap, joint_names: & } } + if is_six_dof { + opw_parameters.dof = 6 + } else { + opw_parameters.dof = 5; + + // Set reasonable values for non-existing joint 6. + // Constraint checker will still be checking this range. + opw_parameters.sign_corrections[5] = 0; // Always suppress + // With from=to, constraint is suppressed. + opw_parameters.from[5] = 0.0; + opw_parameters.to[5] = 0.0; + } + + Ok(opw_parameters) }