├── .cspell.json ├── .deny.toml ├── .github ├── .cspell │ ├── project-dictionary.txt │ └── rust-dependencies.txt ├── dependabot.yml └── workflows │ ├── ci.yml │ └── release.yml ├── .gitignore ├── Cargo.toml ├── LICENSE ├── README.md ├── examples └── wasm │ ├── .gitignore │ ├── Cargo.toml │ ├── README.md │ ├── index.html │ ├── index.js │ ├── package.json │ ├── sample.urdf │ ├── src │ └── lib.rs │ └── webpack.config.js ├── img ├── hsr_1.png ├── hsr_2.png ├── nao_1.png ├── nao_2.png ├── nextage_1.png ├── nextage_2.png ├── pepper_1.png ├── pepper_2.png ├── pr2_1.png ├── pr2_2.png ├── sawyer_1.png ├── sawyer_2.png ├── sawyer_gear.mov ├── sawyer_gear_trim.mov ├── thormang3_1.png ├── thormang3_2.png ├── ubr1_1.png └── ubr1_2.png ├── sample.urdf ├── src ├── app.rs ├── assimp_utils.rs ├── bin │ └── urdf-viz.rs ├── errors.rs ├── handle.rs ├── lib.rs ├── mesh.rs ├── point_cloud.rs ├── urdf.rs ├── utils.rs ├── viewer.rs └── web_server.rs └── tools └── spell-check.sh /.cspell.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2", 3 | "gitignoreRoot": ".", 4 | "useGitignore": true, 5 | "dictionaryDefinitions": [ 6 | { 7 | "name": "project-dictionary", 8 | "path": "./.github/.cspell/project-dictionary.txt", 9 | "addWords": true 10 | }, 11 | { 12 | "name": "rust-dependencies", 13 | "path": "./.github/.cspell/rust-dependencies.txt", 14 | "addWords": true 15 | } 16 | ], 17 | "dictionaries": ["project-dictionary", "rust-dependencies"], 18 | "ignoreRegExpList": [ 19 | // Copyright notice 20 | "Copyright .*", 21 | // GHA actions/workflows 22 | "uses: .+@", 23 | // GHA context (repo name, owner name, etc.) 24 | "github.\\w+ (=|!)= '.+'", 25 | // GH username 26 | "( |\\[)@[\\w_-]+", 27 | // Git config username 28 | "git config user.name .*", 29 | // Username in todo comment 30 | "(TODO|FIXME)\\([\\w_., -]+\\)", 31 | // Cargo.toml authors 32 | "authors *= *\\[.*\\]", 33 | "\".* <[\\w_.+-]+@[\\w.-]+>\"" 34 | ], 35 | "languageSettings": [ 36 | { 37 | "languageId": ["*"], 38 | "dictionaries": ["bash", "rust"] 39 | } 40 | ], 41 | "ignorePaths": [ 42 | // Licenses 43 | "**/LICENSE*", 44 | // Binaries 45 | "**/*.png", 46 | "**/*.mov" 47 | ] 48 | } 49 | -------------------------------------------------------------------------------- /.deny.toml: -------------------------------------------------------------------------------- 1 | # https://embarkstudios.github.io/cargo-deny/checks/advisories/cfg.html 2 | [advisories] 3 | yanked = "deny" 4 | unmaintained = "none" 5 | ignore = [ 6 | "RUSTSEC-2021-0145", # atty 0.2, transitively dep of structopt (via old clap) 7 | "RUSTSEC-2021-0119", # nix 0.18/0.20, transitively dep of kiss3d (via old glutin) 8 | "RUSTSEC-2022-0041", # crossbeam-utils 0.7, transitively dep of kiss3d (via old rusttype) 9 | "RUSTSEC-2023-0045", # memoffset 0.5, transitively dep of kiss3d (via old rusttype) 10 | ] 11 | 12 | # https://embarkstudios.github.io/cargo-deny/checks/bans/cfg.html 13 | [bans] 14 | multiple-versions = "allow" # TODO 15 | wildcards = "allow" # https://github.com/EmbarkStudios/cargo-deny/issues/448 16 | skip = [] 17 | 18 | # https://embarkstudios.github.io/cargo-deny/checks/licenses/cfg.html 19 | [licenses] 20 | unused-allowed-license = "deny" 21 | private.ignore = true 22 | allow = [ 23 | "Apache-2.0", 24 | "MIT", 25 | "Unicode-3.0", # unicode-ident 26 | "BSD-3-Clause", 27 | "CC0-1.0", 28 | "ISC", 29 | "Zlib", 30 | "CDLA-Permissive-2.0", # webpki-roots 31 | ] 32 | 33 | [[licenses.clarify]] 34 | name = "ring" 35 | expression = "MIT AND ISC AND OpenSSL" 36 | license-files = [ 37 | { path = "LICENSE", hash = 0xbd0eed23 } 38 | ] 39 | 40 | # https://embarkstudios.github.io/cargo-deny/checks/sources/cfg.html 41 | [sources] 42 | unknown-registry = "deny" 43 | unknown-git = "deny" 44 | allow-git = [] 45 | -------------------------------------------------------------------------------- /.github/.cspell/project-dictionary.txt: -------------------------------------------------------------------------------- 1 | cdylib 2 | collada 3 | ctxt 4 | devel 5 | glfw 6 | glutin 7 | highp 8 | libglu 9 | mediump 10 | memoffset 11 | msvc 12 | nalgebra 13 | ncollide 14 | nextage 15 | nonblocking 16 | openrr 17 | powerset 18 | rgba 19 | rospack 20 | rosrun 21 | rustdocflags 22 | rustflags 23 | RUSTSEC 24 | rusttype 25 | thormang 26 | webpki 27 | xacro 28 | xorg 29 | -------------------------------------------------------------------------------- /.github/.cspell/rust-dependencies.txt: -------------------------------------------------------------------------------- 1 | // This file is @generated by spell-check.sh. 2 | // It is not intended for manual editing. 3 | 4 | assimp 5 | getrandom 6 | structopt 7 | thiserror 8 | urdf 9 | ureq 10 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: cargo 4 | directory: / 5 | schedule: 6 | interval: daily 7 | commit-message: 8 | prefix: '' 9 | ignore: 10 | # These dependencies need to be updated at the same time with k. 11 | - dependency-name: urdf-rs 12 | labels: [] 13 | - package-ecosystem: github-actions 14 | directory: / 15 | schedule: 16 | interval: daily 17 | commit-message: 18 | prefix: '' 19 | labels: [] 20 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | permissions: 4 | contents: read 5 | 6 | on: 7 | push: 8 | branches: [main] 9 | pull_request: 10 | branches: [main] 11 | schedule: 12 | - cron: '0 15 * * 0,4' # Every Monday and Friday at 00:00 JST 13 | 14 | env: 15 | CARGO_INCREMENTAL: 0 16 | CARGO_NET_RETRY: 10 17 | CARGO_TERM_COLOR: always 18 | RUST_BACKTRACE: 1 19 | RUSTDOCFLAGS: -D warnings 20 | RUSTFLAGS: -D warnings 21 | RUSTUP_MAX_RETRIES: 10 22 | 23 | defaults: 24 | run: 25 | shell: bash 26 | 27 | concurrency: 28 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.sha }} 29 | cancel-in-progress: true 30 | 31 | jobs: 32 | test: 33 | strategy: 34 | fail-fast: false 35 | matrix: 36 | include: 37 | - target: x86_64-unknown-linux-gnu 38 | os: ubuntu-22.04 39 | - target: x86_64-apple-darwin 40 | os: macos-13 41 | - target: aarch64-apple-darwin 42 | os: macos-14 43 | - target: x86_64-pc-windows-msvc 44 | os: windows-latest 45 | runs-on: ${{ matrix.os }} 46 | timeout-minutes: 60 47 | steps: 48 | - uses: actions/checkout@v4 49 | - uses: dtolnay/rust-toolchain@stable 50 | with: 51 | components: clippy,rustfmt 52 | - name: Install cargo-hack 53 | uses: taiki-e/install-action@cargo-hack 54 | 55 | - name: Install dependencies (linux) 56 | run: | 57 | sudo apt-get update 58 | sudo apt-get install xorg-dev libglu1-mesa-dev 59 | if: startsWith(matrix.os, 'ubuntu') 60 | - run: echo "RUSTFLAGS=${RUSTFLAGS} -C target-feature=+crt-static" >>"${GITHUB_ENV}" 61 | if: contains(matrix.target, '-windows-msvc') 62 | # https://doc.rust-lang.org/rustc/platform-support.html 63 | - run: echo "MACOSX_DEPLOYMENT_TARGET=10.12" >>"${GITHUB_ENV}" 64 | if: matrix.target == 'x86_64-apple-darwin' 65 | - run: echo "MACOSX_DEPLOYMENT_TARGET=11.0" >>"${GITHUB_ENV}" 66 | if: matrix.target == 'aarch64-apple-darwin' 67 | 68 | - run: cargo fmt --all --check 69 | - run: cargo clippy --all-features --all-targets 70 | - run: cargo hack build --feature-powerset 71 | - run: cargo test --all-features 72 | - run: cargo build --release 73 | # For debugging 74 | - uses: actions/upload-artifact@v4 75 | with: 76 | name: ${{ matrix.os }} 77 | path: target/release/urdf-viz* 78 | 79 | wasm: 80 | runs-on: ubuntu-22.04 81 | timeout-minutes: 60 82 | permissions: 83 | contents: write 84 | steps: 85 | - uses: actions/checkout@v4 86 | - uses: dtolnay/rust-toolchain@stable 87 | with: 88 | targets: wasm32-unknown-unknown 89 | - name: Install wasm-pack 90 | uses: taiki-e/install-action@wasm-pack 91 | - run: cargo build --target wasm32-unknown-unknown --lib 92 | - run: cargo build --target wasm32-unknown-unknown -p urdf-viz-wasm 93 | - name: Build wasm example 94 | run: npm install && npm run build 95 | env: 96 | CARGO_PROFILE_DEV_OPT_LEVEL: 3 97 | NODE_OPTIONS: --openssl-legacy-provider 98 | working-directory: examples/wasm 99 | - name: Deploy to gh-pages 100 | run: | 101 | cd examples/wasm/dist 102 | git init 103 | git add . 104 | git -c user.name='ci' -c user.email='ci' commit -m 'Deploy urdf-viz to gh-pages' 105 | git push -f -q https://git:${{ secrets.github_token }}@github.com/${{ github.repository }} HEAD:gh-pages 106 | if: github.event_name == 'push' && github.event.ref == 'refs/heads/main' && github.repository_owner == 'openrr' 107 | 108 | deny: 109 | runs-on: ubuntu-latest 110 | timeout-minutes: 60 111 | steps: 112 | - uses: actions/checkout@v4 113 | - uses: dtolnay/rust-toolchain@stable 114 | - uses: taiki-e/install-action@cargo-deny 115 | # Workaround for https://github.com/EmbarkStudios/cargo-deny/issues/413 116 | - uses: taiki-e/install-action@cargo-no-dev-deps 117 | - run: cargo no-dev-deps deny --workspace --all-features check 118 | 119 | spell-check: 120 | runs-on: ubuntu-latest 121 | timeout-minutes: 60 122 | permissions: 123 | contents: write 124 | pull-requests: write 125 | steps: 126 | - uses: actions/checkout@v4 127 | - run: echo "REMOVE_UNUSED_WORDS=1" >>"${GITHUB_ENV}" 128 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') 129 | - run: tools/spell-check.sh 130 | - id: diff 131 | run: | 132 | set -euo pipefail 133 | git config user.name "Taiki Endo" 134 | git config user.email "taiki@smilerobotics.com" 135 | git add -N .github/.cspell 136 | if ! git diff --exit-code -- .github/.cspell; then 137 | git add .github/.cspell 138 | git commit -m "Update cspell dictionary" 139 | echo 'success=false' >>"${GITHUB_OUTPUT}" 140 | fi 141 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') 142 | - uses: peter-evans/create-pull-request@v7 143 | with: 144 | title: Update cspell dictionary 145 | body: | 146 | Auto-generated by [create-pull-request][1] 147 | [Please close and immediately reopen this pull request to run CI.][2] 148 | 149 | [1]: https://github.com/peter-evans/create-pull-request 150 | [2]: https://github.com/peter-evans/create-pull-request/blob/main/docs/concepts-guidelines.md#workarounds-to-trigger-further-workflow-runs 151 | branch: update-cspell-dictionary 152 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') && steps.diff.outputs.success == 'false' 153 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: Release 2 | 3 | on: 4 | push: 5 | tags: ['v[0-9]+.*'] 6 | 7 | env: 8 | CARGO_INCREMENTAL: 0 9 | CARGO_NET_RETRY: 10 10 | CARGO_TERM_COLOR: always 11 | RUST_BACKTRACE: 1 12 | RUSTFLAGS: -D warnings 13 | RUSTUP_MAX_RETRIES: 10 14 | 15 | defaults: 16 | run: 17 | shell: bash 18 | 19 | jobs: 20 | create-release: 21 | if: github.repository_owner == 'openrr' 22 | runs-on: ubuntu-22.04 23 | timeout-minutes: 60 24 | steps: 25 | - uses: actions/checkout@v4 26 | - uses: dtolnay/rust-toolchain@stable 27 | 28 | - name: Install dependencies (linux) 29 | run: | 30 | sudo apt-get update 31 | sudo apt-get install xorg-dev libglu1-mesa-dev 32 | 33 | - run: cargo package 34 | - uses: taiki-e/create-gh-release-action@v1 35 | with: 36 | branch: main 37 | env: 38 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 39 | - run: cargo publish 40 | env: 41 | CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} 42 | 43 | upload-assets: 44 | name: upload-assets (${{ matrix.os }}) 45 | if: github.repository_owner == 'openrr' 46 | needs: [create-release] 47 | strategy: 48 | matrix: 49 | include: 50 | - target: x86_64-unknown-linux-gnu 51 | os: ubuntu-22.04 52 | - target: x86_64-apple-darwin 53 | os: macos-13 54 | - target: aarch64-apple-darwin 55 | os: macos-14 56 | - target: x86_64-pc-windows-msvc 57 | os: windows-latest 58 | runs-on: ${{ matrix.os }} 59 | timeout-minutes: 60 60 | steps: 61 | - uses: actions/checkout@v4 62 | - uses: dtolnay/rust-toolchain@stable 63 | 64 | - name: Install dependencies (linux) 65 | run: | 66 | sudo apt-get update 67 | sudo apt-get install xorg-dev libglu1-mesa-dev 68 | if: startsWith(matrix.os, 'ubuntu') 69 | - run: echo "RUSTFLAGS=${RUSTFLAGS} -C target-feature=+crt-static" >>"${GITHUB_ENV}" 70 | if: contains(matrix.target, '-windows-msvc') 71 | # https://doc.rust-lang.org/rustc/platform-support.html 72 | - run: echo "MACOSX_DEPLOYMENT_TARGET=10.12" >>"${GITHUB_ENV}" 73 | if: matrix.target == 'x86_64-apple-darwin' 74 | - run: echo "MACOSX_DEPLOYMENT_TARGET=11.0" >>"${GITHUB_ENV}" 75 | if: matrix.target == 'aarch64-apple-darwin' 76 | 77 | - uses: taiki-e/upload-rust-binary-action@v1 78 | with: 79 | bin: urdf-viz 80 | target: ${{ matrix.target }} 81 | # TODO: Should we enable assimp feature for pre-built binary? 82 | # all-features: true 83 | env: 84 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 85 | CARGO_PROFILE_RELEASE_LTO: true 86 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Generated by Cargo 2 | # will have compiled files and executables 3 | target 4 | 5 | # Remove Cargo.lock from gitignore if creating an executable, leave it for libraries 6 | # More information here http://doc.crates.io/guide.html#cargotoml-vs-cargolock 7 | Cargo.lock 8 | 9 | *~ 10 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "urdf-viz" 3 | # When publishing a new version: 4 | # - Create "v0.x.y" git tag 5 | # - Push the above tag (run `git push origin --tags`) 6 | # Then, CI will publish to crates.io and create a GitHub release. 7 | version = "0.46.1" 8 | authors = ["Takashi Ogura "] 9 | edition = "2021" 10 | description = "URDF visualization" 11 | license = "Apache-2.0" 12 | keywords = ["robotics", "urdf", "visualization"] 13 | categories = ["visualization"] 14 | repository = "https://github.com/openrr/urdf-viz" 15 | exclude = [".github/*", "img/*"] 16 | 17 | [workspace] 18 | members = ["examples/wasm"] 19 | 20 | [features] 21 | default = [] 22 | assimp = ["dep:assimp", "assimp-sys", "tempfile"] 23 | 24 | # Note: k, kiss3d, serde, structopt, tokio, urdf-rs, and wasm-bindgen are public dependencies. 25 | [dependencies] 26 | crossbeam-queue = "0.3.5" 27 | k = "0.32" 28 | kiss3d = "0.35" 29 | mesh-loader = "0.1.6" 30 | rand = "0.8" 31 | serde = { version = "1.0", features = ["derive"] } 32 | structopt = "0.3" 33 | thiserror = "2.0" 34 | tracing = "0.1" 35 | urdf-rs = "0.9" 36 | 37 | assimp = { version = "0.3.1", optional = true } 38 | assimp-sys = { version = "0.3.1", optional = true } 39 | tempfile = { version = "3.8", optional = true } 40 | 41 | [target.'cfg(not(target_family = "wasm"))'.dependencies] 42 | axum = "0.8" 43 | ctrlc = { version = "3", features = ["termination"] } 44 | tokio = { version = "1", features = ["full"] } 45 | tower-http = { version = "0.6", features = ["trace"] } 46 | tracing-subscriber = "0.3" 47 | ureq = "2" 48 | 49 | [target.'cfg(target_family = "wasm")'.dependencies] 50 | base64 = "0.22" 51 | getrandom = { version = "0.2", features = ["js"] } 52 | js-sys = "0.3.31" 53 | serde_json = "1" 54 | serde_qs = "0.15" 55 | url = "2" 56 | wasm-bindgen = "0.2" 57 | wasm-bindgen-futures = "0.4" 58 | web-sys = { version = "0.3", features = [ 59 | "Window", 60 | "Location", 61 | "Url", 62 | "FileReader", 63 | "Blob", 64 | "File", 65 | "Response", 66 | "WebGlRenderingContext", 67 | ] } 68 | 69 | [dev-dependencies] 70 | serde_qs = "0.15" 71 | url = "2" 72 | 73 | [lints] 74 | workspace = true 75 | 76 | [workspace.lints.rust] 77 | missing_debug_implementations = "warn" 78 | # missing_docs = "warn" # TODO 79 | rust_2018_idioms = "warn" 80 | single_use_lifetimes = "warn" 81 | unreachable_pub = "warn" 82 | [workspace.lints.clippy] 83 | lint_groups_priority = { level = "allow", priority = 1 } # https://github.com/rust-lang/rust-clippy/issues/12920 84 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # urdf-viz 2 | 3 | [![Build Status](https://img.shields.io/github/actions/workflow/status/openrr/urdf-viz/ci.yml?branch=main&logo=github)](https://github.com/openrr/urdf-viz/actions) [![crates.io](https://img.shields.io/crates/v/urdf-viz.svg?logo=rust)](https://crates.io/crates/urdf-viz) [![docs](https://docs.rs/urdf-viz/badge.svg)](https://docs.rs/urdf-viz) [![discord](https://dcbadge.vercel.app/api/server/8DAFFKc88B?style=flat)](https://discord.gg/8DAFFKc88B) 4 | 5 | Visualize [URDF(Unified Robot Description Format)](http://wiki.ros.org/urdf) file. 6 | `urdf-viz` is written in Rust-lang. 7 | 8 | ## Install 9 | 10 | ### Install with `cargo` 11 | 12 | If you are using rust-lang already and `cargo` is installed, you can install by `cargo install`. 13 | 14 | ```bash 15 | cargo install urdf-viz 16 | ``` 17 | 18 | If you want to use mesh other than `.obj`, `.stl`, and `.dae` files, you need to install 19 | with assimp like below. 20 | 21 | ```bash 22 | cargo install urdf-viz --features assimp 23 | ``` 24 | 25 | ### Pre-requirements for build 26 | 27 | #### Common 28 | 29 | If you want to use `--features assimp` to use mesh other than `.obj`, `.stl`, and `.dae` files, you need [cmake](https://cmake.org/download/). 30 | 31 | #### On Linux 32 | 33 | If you have not installed ROS, you may need `cmake`, `xorg-dev`, `glu` to 34 | compile `assimp-sys` and `glfw-sys`. 35 | 36 | ```bash 37 | sudo apt-get install cmake xorg-dev libglu1-mesa-dev 38 | ``` 39 | 40 | ### Download binary 41 | 42 | You can download prebuilt binaries from the [release page](https://github.com/openrr/urdf-viz/releases). 43 | Prebuilt binaries are available for macOS, Linux, and Windows (static executable). 44 | 45 | ### Install via Homebrew 46 | 47 | You can install urdf-viz using [Homebrew tap on macOS and Linux](https://github.com/openrr/homebrew-tap/blob/main/Formula/urdf-viz.rb): 48 | 49 | ```sh 50 | brew install openrr/tap/urdf-viz 51 | ``` 52 | 53 | ## How to use 54 | 55 | `urdf-viz` command will be installed. 56 | It needs `rosrun` and `rospack` to resolve `package://` in `` tag, and 57 | it uses `xacro` to convert `.xacro` file into urdf file. 58 | It means you need `$ source ~/catkin_ws/devel/setup.bash` or something before using `urdf-viz`. 59 | 60 | ```bash 61 | urdf-viz URDF_FILE.urdf 62 | ``` 63 | 64 | It is possible to use xacro file directly. 65 | It will be converted by `rosrun xacro xacro` inside of `urdf-viz`. 66 | 67 | ```bash 68 | urdf-viz XACRO_FILE.urdf.xacro 69 | ``` 70 | 71 | If your xacro file has some arguments, you can pass them by `--xacro-args` option. 72 | 73 | ```bash 74 | urdf-viz XACRO_FILE.urdf.xacro --xacro-args arg1=value arg2=value 75 | ``` 76 | 77 | For other options, please read the output of `-h` option. 78 | 79 | ```bash 80 | urdf-viz -h 81 | ``` 82 | 83 | If there are no "package://" in mesh tag, and don't use xacro you can skip install of ROS. 84 | 85 | If there are "package://" in mesh tag, but path or URL to package is known and 86 | don't use xacro you can also skip install of ROS [by replacing package with path 87 | or URL](https://github.com/openrr/urdf-viz/pull/176). 88 | 89 | ## GUI Usage 90 | 91 | In the GUI, you can do some operations with keyboard and mouse. 92 | 93 | * `l` key to reload the urdf from file 94 | * `c` key to toggle collision model or visual mode 95 | * Move a joint 96 | * set the angle of a joint by `Up`/`Down` key 97 | * `Ctrl` + Drag to move the angle of a joint 98 | * change the joint to be moved by `o` (`[`) and `p` (`]`) 99 | * Inverse kinematics (only positions) 100 | * `Shift` + Drag to use inverse kinematics(Y and Z axis) 101 | * `Shift` + `Ctrl` + Drag to use inverse kinematics(X and Z axis) 102 | * change the move target for inverse kinematics by `,` or `.` 103 | * `r` key to set random joints 104 | * `z` key to reset joint positions and origin 105 | * Move view point 106 | * Mouse Right Drag to translate view camera position 107 | * Mouse Left Drag to look around 108 | * Scroll to zoom in/out 109 | 110 | ## Web I/O interface 111 | 112 | You can set/get the joint angles using http/JSON. 113 | Default port number is 7777. You can change it by `-p` option. 114 | (`jq` is used for JSON formatter in the following examples) 115 | 116 | ### Set joint angles 117 | 118 | POST the JSON data, which format is like below. You have to specify the names of joints and positions (angles). 119 | The length of `names` and `positions` have to be the same. You don't need write 120 | all joint names, it means you can specify a part of the joints. 121 | 122 | ```json 123 | { 124 | "names": ["joint_name1", "joint_name2"], 125 | "positions": [0.5, -0.1] 126 | } 127 | ``` 128 | 129 | You can try it using `curl`. 130 | 131 | ```bash 132 | $ curl -H "Accept: application/json" -H "Content-type: application/json" -X POST -d '{"names": ["r_shoulder_yaw", "r_shoulder_pitch"], "positions": [0.8, -0.8]}' http://127.0.0.1:7777/set_joint_positions | jq 133 | { 134 | "is_ok": true, 135 | "reason": "" 136 | } 137 | ``` 138 | 139 | ### Get joint angles as JSON 140 | 141 | The result JSON format of getting the joint angles is the same as the *Set* method. 142 | 143 | ```bash 144 | $ curl http://127.0.0.1:7777/get_joint_positions | jq 145 | { 146 | "names": [ 147 | "r_shoulder_yaw", 148 | "r_shoulder_pitch", 149 | "r_shoulder_roll", 150 | "r_elbow_pitch", 151 | "r_wrist_yaw", 152 | "r_wrist_pitch", 153 | "l_shoulder_yaw", 154 | "l_shoulder_pitch", 155 | "l_shoulder_roll", 156 | "l_elbow_pitch", 157 | "l_wrist_yaw", 158 | "l_wrist_pitch" 159 | ], 160 | "positions": [ 161 | 0.8, 162 | -0.8, 163 | -1.3447506, 164 | -1.6683152, 165 | -1.786362, 166 | -1.0689334, 167 | 0.11638665, 168 | -0.5987091, 169 | 0.7868867, 170 | -0.027412653, 171 | 0.019940138, 172 | -0.6975361 173 | ] 174 | } 175 | ``` 176 | 177 | ### Set Robot Origin 178 | 179 | ```bash 180 | $ curl -H "Accept: application/json" -H "Content-type: application/json" -X POST -d '{"position":[0.2,0.0,0.0],"quaternion":[0.0,0.0,0.0,1.0]}' http://127.0.0.1:7777/set_robot_origin 181 | {"is_ok":true,"reason":""} 182 | ``` 183 | 184 | The order of the quaternion elements is `w, i, j, k`. 185 | 186 | ### Get Robot Origin 187 | 188 | ```bash 189 | $ curl http://127.0.0.1:7777/get_robot_origin 190 | {"position":[0.2,0.0,0.0],"quaternion":[1.0,0.0,0.0,0.0]} 191 | ``` 192 | 193 | ### Get URDF Text 194 | 195 | ```bash 196 | curl http://127.0.0.1:7777/get_urdf_text 197 | ``` 198 | 199 | ## Gallery 200 | 201 | ![sawyer_1.png](img/sawyer_1.png) 202 | ![sawyer_2.png](img/sawyer_2.png) 203 | 204 | ![nextage_1.png](img/nextage_1.png) 205 | ![nextage_2.png](img/nextage_2.png) 206 | 207 | ![hsr_1.png](img/hsr_1.png) 208 | ![hsr_2.png](img/hsr_2.png) 209 | 210 | ![ubr1_1.png](img/ubr1_1.png) 211 | ![ubr1_2.png](img/ubr1_2.png) 212 | 213 | ![pepper_1.png](img/pepper_1.png) 214 | ![pepper_2.png](img/pepper_2.png) 215 | 216 | ![pr2_1.png](img/pr2_1.png) 217 | ![pr2_2.png](img/pr2_2.png) 218 | 219 | ![thormang3_1.png](img/thormang3_1.png) 220 | ![thormang3_2.png](img/thormang3_2.png) 221 | 222 | ## Dependencies 223 | 224 | * [kiss3d](https://github.com/sebcrozet/kiss3d): `urdf-viz` is strongly depend on `kiss3d`, which is super easy to use, great 3D graphic engine. 225 | * [nalgebra](https://github.com/sebcrozet/nalgebra): linear algebra library. 226 | * [k](https://github.com/openrr/k): kinematics library which is based on [nalgebra](https://github.com/sebcrozet/nalgebra). It can load URDF files using `urdf-rs`. 227 | * [mesh-loader](https://github.com/openrr/mesh-loader): Mesh files (`.obj`, `.stl`, and `.dae`) loader. 228 | * [urdf-rs](https://github.com/openrr/urdf-rs): URDF file loader. 229 | * [structopt](https://github.com/TeXitoi/structopt): super easy command line arguments parser. 230 | 231 | ## `OpenRR` Community 232 | 233 | [Here](https://discord.gg/8DAFFKc88B) is a discord server for `OpenRR` users and developers. 234 | -------------------------------------------------------------------------------- /examples/wasm/.gitignore: -------------------------------------------------------------------------------- 1 | dist 2 | node_modules 3 | pkg 4 | package-lock.json 5 | -------------------------------------------------------------------------------- /examples/wasm/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "urdf-viz-wasm" 3 | authors = ["Taiki Endo "] 4 | edition = "2021" 5 | 6 | [lib] 7 | crate-type = ["cdylib"] 8 | 9 | [target.'cfg(target_family = "wasm")'.dependencies] 10 | urdf-viz = { path = "../.." } 11 | console_error_panic_hook = "0.1" 12 | tracing-wasm = "0.2" 13 | wasm-bindgen = "0.2" 14 | wasm-bindgen-futures = "0.4" 15 | 16 | [lints] 17 | workspace = true 18 | -------------------------------------------------------------------------------- /examples/wasm/README.md: -------------------------------------------------------------------------------- 1 | # urdf-viz wasm example 2 | 3 | [You can try this example online.](https://openrr.github.io/urdf-viz/?urdf=https://raw.githubusercontent.com/openrr/urdf-viz/main/sample.urdf) 4 | 5 | ## How to run 6 | 7 | First, install [wasm-pack](https://rustwasm.github.io/wasm-pack). 8 | 9 | Then, build and serve the example. 10 | 11 | ```sh 12 | cd examples/wasm 13 | npm install 14 | npm run serve 15 | ``` 16 | 17 | Then, open in a browser. 18 | 19 | ### Params 20 | 21 | You can specify the same options as the urdf-viz argument as URL parameters (use `urdf=` parameter to specify the urdf file to be loaded). For example: 22 | 23 | 24 | 25 | sample-urdf-viz 26 | -------------------------------------------------------------------------------- /examples/wasm/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | urdf-viz 6 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /examples/wasm/index.js: -------------------------------------------------------------------------------- 1 | import('./pkg').catch(console.error); 2 | -------------------------------------------------------------------------------- /examples/wasm/package.json: -------------------------------------------------------------------------------- 1 | { 2 | "scripts": { 3 | "build": "webpack", 4 | "serve": "webpack-dev-server" 5 | }, 6 | "devDependencies": { 7 | "@wasm-tool/wasm-pack-plugin": "^1.7.0", 8 | "html-webpack-plugin": "^5.6.3", 9 | "webpack": "^5.99.9", 10 | "webpack-cli": "^6.0.1", 11 | "webpack-dev-server": "^5.2.1" 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /examples/wasm/sample.urdf: -------------------------------------------------------------------------------- 1 | ../../sample.urdf -------------------------------------------------------------------------------- /examples/wasm/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![cfg(target_family = "wasm")] 2 | 3 | use urdf_viz::app::*; 4 | use wasm_bindgen::prelude::*; 5 | 6 | #[wasm_bindgen(start)] 7 | pub async fn main() -> Result<(), JsValue> { 8 | console_error_panic_hook::set_once(); 9 | tracing_wasm::set_as_global_default(); 10 | 11 | run().await 12 | } 13 | 14 | const SAMPLE_URDF_PATH: &str = "sample.urdf"; 15 | 16 | async fn run() -> Result<(), JsValue> { 17 | let mut opt = Opt::from_params()?; 18 | if opt.input_urdf_or_xacro.is_empty() { 19 | opt.input_urdf_or_xacro = SAMPLE_URDF_PATH.to_string(); 20 | } 21 | let package_path = opt.create_package_path_map()?; 22 | let urdf_robot = 23 | urdf_viz::utils::RobotModel::new(&opt.input_urdf_or_xacro, package_path).await?; 24 | let ik_constraints = opt.create_ik_constraints(); 25 | let mut app = UrdfViewerApp::new( 26 | urdf_robot, 27 | opt.end_link_names, 28 | opt.is_collision, 29 | opt.disable_texture, 30 | true, 31 | ( 32 | opt.back_ground_color_r, 33 | opt.back_ground_color_g, 34 | opt.back_ground_color_b, 35 | ), 36 | (opt.tile_color1_r, opt.tile_color1_g, opt.tile_color1_b), 37 | (opt.tile_color2_r, opt.tile_color2_g, opt.tile_color2_b), 38 | opt.ground_height, 39 | opt.hide_menu, 40 | opt.axis_scale, 41 | opt.move_base_diff_unit, 42 | opt.move_joint_diff_unit, 43 | )?; 44 | app.set_ik_constraints(ik_constraints); 45 | app.init(); 46 | app.run(); 47 | 48 | Ok(()) 49 | } 50 | -------------------------------------------------------------------------------- /examples/wasm/webpack.config.js: -------------------------------------------------------------------------------- 1 | const path = require('path'); 2 | const HtmlWebpackPlugin = require('html-webpack-plugin'); 3 | const WasmPackPlugin = require('@wasm-tool/wasm-pack-plugin'); 4 | 5 | module.exports = { 6 | entry: './index.js', 7 | output: { 8 | path: path.resolve(__dirname, 'dist'), 9 | filename: 'index.js', 10 | }, 11 | plugins: [ 12 | new HtmlWebpackPlugin({ 13 | template: 'index.html', 14 | }), 15 | new WasmPackPlugin({ 16 | crateDirectory: path.resolve(__dirname, '.'), 17 | }), 18 | ], 19 | mode: 'development', 20 | experiments: { 21 | asyncWebAssembly: true, 22 | }, 23 | }; 24 | -------------------------------------------------------------------------------- /img/hsr_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/hsr_1.png -------------------------------------------------------------------------------- /img/hsr_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/hsr_2.png -------------------------------------------------------------------------------- /img/nao_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/nao_1.png -------------------------------------------------------------------------------- /img/nao_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/nao_2.png -------------------------------------------------------------------------------- /img/nextage_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/nextage_1.png -------------------------------------------------------------------------------- /img/nextage_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/nextage_2.png -------------------------------------------------------------------------------- /img/pepper_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/pepper_1.png -------------------------------------------------------------------------------- /img/pepper_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/pepper_2.png -------------------------------------------------------------------------------- /img/pr2_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/pr2_1.png -------------------------------------------------------------------------------- /img/pr2_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/pr2_2.png -------------------------------------------------------------------------------- /img/sawyer_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/sawyer_1.png -------------------------------------------------------------------------------- /img/sawyer_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/sawyer_2.png -------------------------------------------------------------------------------- /img/sawyer_gear.mov: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/sawyer_gear.mov -------------------------------------------------------------------------------- /img/sawyer_gear_trim.mov: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/sawyer_gear_trim.mov -------------------------------------------------------------------------------- /img/thormang3_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/thormang3_1.png -------------------------------------------------------------------------------- /img/thormang3_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/thormang3_2.png -------------------------------------------------------------------------------- /img/ubr1_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/ubr1_1.png -------------------------------------------------------------------------------- /img/ubr1_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/openrr/urdf-viz/576fe10371d747c7889e3d6d864db887e1f1ef5f/img/ubr1_2.png -------------------------------------------------------------------------------- /sample.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | -------------------------------------------------------------------------------- /src/app.rs: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 Takashi Ogura 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | use k::nalgebra as na; 18 | use k::prelude::*; 19 | use kiss3d::event::{Action, Key, Modifiers, WindowEvent}; 20 | use kiss3d::window::{self, Window}; 21 | use serde::Deserialize; 22 | use std::collections::HashMap; 23 | use std::fmt; 24 | use std::path::PathBuf; 25 | use std::sync::atomic::{AtomicUsize, Ordering::Relaxed}; 26 | use std::sync::Arc; 27 | use structopt::StructOpt; 28 | use tracing::*; 29 | 30 | #[cfg(not(target_family = "wasm"))] 31 | use std::sync::atomic::AtomicBool; 32 | 33 | use crate::{ 34 | handle::{JointNamesAndPositions, RobotOrigin, RobotStateHandle}, 35 | point_cloud::PointCloudRenderer, 36 | utils::RobotModel, 37 | Error, Viewer, ROBOT_OBJECT_ID, 38 | }; 39 | 40 | #[cfg(target_os = "macos")] 41 | static NATIVE_MOD: Modifiers = Modifiers::Super; 42 | 43 | #[cfg(not(target_os = "macos"))] 44 | static NATIVE_MOD: Modifiers = Modifiers::Control; 45 | 46 | fn move_joint_by_random(robot: &mut k::Chain) -> Result<(), k::Error> { 47 | let angles_vec = robot 48 | .iter_joints() 49 | .map(|j| match j.limits { 50 | Some(ref range) => (range.max - range.min) * rand::random::() + range.min, 51 | None => (rand::random::() - 0.5) * 2.0, 52 | }) 53 | .collect::>(); 54 | robot.set_joint_positions(&angles_vec) 55 | } 56 | 57 | fn move_joint_to_zero(robot: &mut k::Chain) { 58 | let angles_vec = vec![0.0; robot.dof()]; 59 | // use clamped variant because 0.0 is maybe out-of-limits. 60 | robot.set_joint_positions_clamped(&angles_vec) 61 | } 62 | 63 | fn move_joint_by_index( 64 | index: usize, 65 | diff_angle: f32, 66 | robot: &mut k::Chain, 67 | ) -> Result<(), k::Error> { 68 | let mut angles_vec = robot.joint_positions(); 69 | assert!(index < robot.dof()); 70 | angles_vec[index] += diff_angle; 71 | robot.set_joint_positions(&angles_vec) 72 | } 73 | 74 | #[derive(Debug)] 75 | struct LoopIndex { 76 | index: usize, 77 | size: usize, 78 | } 79 | 80 | impl LoopIndex { 81 | fn new(size: usize) -> Self { 82 | Self { index: 0, size } 83 | } 84 | fn get(&self) -> usize { 85 | self.index 86 | } 87 | fn inc(&mut self) { 88 | if self.size != 0 { 89 | self.index += 1; 90 | self.index %= self.size; 91 | } 92 | } 93 | fn dec(&mut self) { 94 | if self.size != 0 { 95 | if self.index == 0 { 96 | self.index = self.size - 1; 97 | } else { 98 | self.index -= 1; 99 | } 100 | } 101 | } 102 | } 103 | 104 | const HOW_TO_USE_STR: &str = r#"o: joint ID +1 105 | p: joint ID -1 106 | ,: IK target ID +1 107 | .: IK target ID -1 108 | Up: joint angle +0.1 109 | Down: joint angle -0.1 110 | Ctrl+Drag: move joint 111 | Shift+Drag: IK (y, z) 112 | Shift+Ctrl+Drag: IK (x, z) 113 | l: Reload the file 114 | r: set random angles 115 | z: reset joint positions and origin 116 | c: toggle visual/collision 117 | f: toggle show link frames 118 | n: toggle show link names 119 | m: toggle show menu 120 | "#; 121 | 122 | const FRAME_ARROW_SIZE: f32 = 0.2; 123 | 124 | fn node_to_frame_name(node: &k::Node) -> String { 125 | format!("{}_frame", node.joint().name) 126 | } 127 | 128 | pub struct UrdfViewerApp { 129 | input_path: PathBuf, 130 | urdf_robot: RobotModel, 131 | robot: k::Chain, 132 | viewer: Viewer, 133 | window: Option, 134 | arms: Vec>, 135 | names: Vec, 136 | input_end_link_names: Vec, 137 | index_of_arm: LoopIndex, 138 | index_of_move_joint: LoopIndex, 139 | handle: Arc, 140 | is_collision: bool, 141 | show_frames: bool, 142 | show_link_names: bool, 143 | ik_constraints: k::Constraints, 144 | point_size: f32, 145 | package_path: HashMap, 146 | hide_menu: bool, 147 | axis_scale: f32, 148 | move_base_diff_unit: f32, 149 | move_joint_diff_unit: f32, 150 | } 151 | 152 | impl UrdfViewerApp { 153 | #[allow(clippy::too_many_arguments)] 154 | pub fn new( 155 | mut urdf_robot: RobotModel, 156 | mut end_link_names: Vec, 157 | is_collision: bool, 158 | disable_texture: bool, 159 | disable_assimp: bool, 160 | background_color: (f32, f32, f32), 161 | tile_color1: (f32, f32, f32), 162 | tile_color2: (f32, f32, f32), 163 | ground_height: Option, 164 | hide_menu: bool, 165 | axis_scale: f32, 166 | move_base_diff_unit: f32, 167 | move_joint_diff_unit: f32, 168 | ) -> Result { 169 | let input_path = PathBuf::from(&urdf_robot.path); 170 | let package_path = urdf_robot.take_package_path_map(); 171 | let robot: k::Chain = urdf_robot.get().into(); 172 | println!("{robot}"); 173 | let (mut viewer, mut window) = Viewer::with_background_color("urdf-viz", background_color); 174 | if disable_texture { 175 | viewer.disable_texture(); 176 | } 177 | if disable_assimp { 178 | viewer.disable_assimp(); 179 | } 180 | viewer.add_robot_with_base_dir_and_collision_flag( 181 | &mut window, 182 | urdf_robot.get(), 183 | input_path.parent(), 184 | is_collision, 185 | &package_path, 186 | ); 187 | viewer.add_axis_cylinders_with_scale(&mut window, "origin", 1.0, axis_scale); 188 | if let Some(h) = ground_height { 189 | viewer.add_ground(&mut window, h, 0.5, 3, tile_color1, tile_color2); 190 | } 191 | let input_end_link_names = end_link_names.clone(); 192 | if end_link_names.is_empty() { 193 | end_link_names = robot 194 | .iter() 195 | .filter(|node| node.is_end()) 196 | .map(|node| node.joint().name.clone()) 197 | .collect::>(); 198 | } 199 | let arms = end_link_names 200 | .iter() 201 | .filter_map(|name| robot.find(name).map(k::SerialChain::from_end)) 202 | .collect::>(); 203 | println!("end_link_names = {end_link_names:?}"); 204 | let names = robot 205 | .iter_joints() 206 | .map(|j| j.name.clone()) 207 | .collect::>(); 208 | let num_arms = end_link_names.len(); 209 | let num_joints = names.len(); 210 | println!("DoF={num_joints}"); 211 | println!("joint names={names:?}"); 212 | let mut handle = RobotStateHandle::default(); 213 | handle 214 | .current_joint_positions 215 | .lock() 216 | .unwrap() 217 | .names 218 | .clone_from(&names); 219 | handle.urdf_text = Some(urdf_robot.urdf_text.clone()); 220 | Ok(UrdfViewerApp { 221 | input_path, 222 | viewer, 223 | window: Some(window), 224 | arms, 225 | input_end_link_names, 226 | urdf_robot, 227 | robot, 228 | names, 229 | index_of_arm: LoopIndex::new(num_arms), 230 | index_of_move_joint: LoopIndex::new(num_joints), 231 | handle: Arc::new(handle), 232 | is_collision, 233 | show_frames: false, 234 | show_link_names: false, 235 | ik_constraints: k::Constraints::default(), 236 | point_size: 10.0, 237 | package_path, 238 | hide_menu, 239 | axis_scale, 240 | move_base_diff_unit, 241 | move_joint_diff_unit, 242 | }) 243 | } 244 | pub fn handle(&self) -> Arc { 245 | self.handle.clone() 246 | } 247 | pub fn set_ik_constraints(&mut self, ik_constraints: k::Constraints) { 248 | self.ik_constraints = ik_constraints; 249 | } 250 | pub fn set_point_size(&mut self, point_size: f32) { 251 | self.point_size = point_size; 252 | } 253 | fn has_arms(&self) -> bool { 254 | !self.arms.is_empty() 255 | } 256 | fn has_joints(&self) -> bool { 257 | !self.names.is_empty() 258 | } 259 | pub fn init(&mut self) { 260 | self.update_robot(); 261 | if self.has_arms() { 262 | let window = self.window.as_mut().unwrap(); 263 | self.viewer 264 | .add_axis_cylinders_with_scale(window, "ik_target", 0.2, self.axis_scale); 265 | self.update_ik_target_marker(); 266 | } 267 | let window = self.window.as_mut().unwrap(); 268 | self.robot.iter().for_each(|n| { 269 | self.viewer.add_axis_cylinders_with_scale( 270 | window, 271 | &node_to_frame_name(n), 272 | FRAME_ARROW_SIZE, 273 | self.axis_scale, 274 | ); 275 | }); 276 | self.update_frame_markers(); 277 | } 278 | fn get_arm(&self) -> &k::SerialChain { 279 | &self.arms[self.index_of_arm.get()] 280 | } 281 | fn get_end_transform(&self) -> na::Isometry3 { 282 | self.get_arm().end_transform() 283 | } 284 | fn update_ik_target_marker(&mut self) { 285 | if self.has_arms() { 286 | let pose = self.get_end_transform(); 287 | 288 | if let Some(obj) = self.viewer.scene_node_mut("ik_target") { 289 | obj.set_local_transformation(pose); 290 | }; 291 | } 292 | } 293 | fn update_frame_markers(&mut self) { 294 | for n in self.robot.iter() { 295 | let name = node_to_frame_name(n); 296 | if let Some(obj) = self.viewer.scene_node_mut(&name) { 297 | if self.show_frames { 298 | obj.set_local_transformation(n.world_transform().unwrap()); 299 | } 300 | obj.set_visible(self.show_frames); 301 | } 302 | } 303 | } 304 | fn remove_frame_markers(&mut self, window: &mut Window) { 305 | for n in self.robot.iter() { 306 | let name = node_to_frame_name(n); 307 | if let Some(obj) = self.viewer.scene_node_mut(&name) { 308 | window.remove_node(obj); 309 | } 310 | } 311 | } 312 | 313 | fn draw_link_names(&mut self, window: &mut Window) { 314 | for n in self.robot.iter() { 315 | let name = node_to_frame_name(n); 316 | let link_name = if let Some(n) = name.strip_suffix("_joint_frame") { 317 | n 318 | } else { 319 | name.as_str() 320 | }; 321 | if let Some(tr) = n.world_transform() { 322 | self.viewer.draw_text_from_3d( 323 | window, 324 | link_name, 325 | 40.0, 326 | &na::Point3::new(tr.translation.x, tr.translation.y, tr.translation.z), 327 | &na::Point3::new(1f32, 1.0, 1.0), 328 | ); 329 | } 330 | } 331 | } 332 | 333 | fn update_robot(&mut self) { 334 | // this is hack to handle invalid mimic joints, like hsr 335 | let joint_positions = self.robot.joint_positions(); 336 | // Use clamped variant because 0.0 which is the default position value is maybe out-of-limits. 337 | self.robot.set_joint_positions_clamped(&joint_positions); 338 | self.viewer.update(&self.robot); 339 | self.update_ik_target_marker(); 340 | self.update_frame_markers(); 341 | } 342 | fn reload(&mut self, window: &mut Window, reload_fn: impl FnOnce(&mut RobotModel)) { 343 | // remove previous robot 344 | self.viewer.remove_robot(window, self.urdf_robot.get()); 345 | self.remove_frame_markers(window); 346 | // update urdf_robot 347 | reload_fn(&mut self.urdf_robot); 348 | 349 | // update robot based on new urdf_robot 350 | let urdf_robot = self.urdf_robot.get(); 351 | self.robot = urdf_robot.into(); 352 | self.input_path = PathBuf::from(&self.urdf_robot.path); 353 | let end_link_names = if self.input_end_link_names.is_empty() { 354 | self.robot 355 | .iter() 356 | .filter(|node| node.is_end()) 357 | .map(|node| node.joint().name.clone()) 358 | .collect::>() 359 | } else { 360 | self.input_end_link_names.clone() 361 | }; 362 | self.arms = end_link_names 363 | .iter() 364 | .filter_map(|name| self.robot.find(name).map(k::SerialChain::from_end)) 365 | .collect::>(); 366 | let names: Vec<_> = self.robot.iter_joints().map(|j| j.name.clone()).collect(); 367 | if self.names != names { 368 | let dof = names.len(); 369 | self.names.clone_from(&names); 370 | let mut current_joint_positions = self.handle.current_joint_positions.lock().unwrap(); 371 | current_joint_positions.names = names; 372 | current_joint_positions.positions = vec![0.0; dof]; 373 | } 374 | 375 | self.viewer.add_robot_with_base_dir_and_collision_flag( 376 | window, 377 | self.urdf_robot.get(), 378 | self.input_path.parent(), 379 | self.is_collision, 380 | &self.package_path, 381 | ); 382 | const FRAME_ARROW_SIZE: f32 = 0.2; 383 | self.robot.iter().for_each(|n| { 384 | self.viewer.add_axis_cylinders_with_scale( 385 | window, 386 | &node_to_frame_name(n), 387 | FRAME_ARROW_SIZE, 388 | self.axis_scale, 389 | ); 390 | }); 391 | self.update_robot(); 392 | } 393 | 394 | /// Handle set_joint_positions request from server 395 | fn set_joint_positions_from_request( 396 | &mut self, 397 | joint_positions: &JointNamesAndPositions, 398 | ) -> Result<(), k::Error> { 399 | let mut angles = self.robot.joint_positions(); 400 | for (name, angle) in joint_positions 401 | .names 402 | .iter() 403 | .zip(joint_positions.positions.iter()) 404 | { 405 | if let Some(index) = self.names.iter().position(|n| n == name) { 406 | angles[index] = *angle; 407 | } else { 408 | warn!("{name} not found, but continues"); 409 | } 410 | } 411 | self.robot.set_joint_positions(&angles) 412 | } 413 | 414 | /// Handle set_origin request from server 415 | fn set_robot_origin_from_request(&mut self, origin: &RobotOrigin) { 416 | let pos = origin.position; 417 | let q = origin.quaternion; 418 | let pose = na::Isometry3::from_parts( 419 | na::Translation3::new(pos[0], pos[1], pos[2]), 420 | na::UnitQuaternion::new_normalize(na::Quaternion::new(q[0], q[1], q[2], q[3])), 421 | ); 422 | self.robot.set_origin(pose); 423 | } 424 | 425 | fn increment_move_joint_index(&mut self, is_inc: bool) { 426 | if self.has_joints() { 427 | self.viewer 428 | .reset_temporal_color(&self.names[self.index_of_move_joint.get()]); 429 | if is_inc { 430 | self.index_of_move_joint.inc(); 431 | } else { 432 | self.index_of_move_joint.dec(); 433 | } 434 | self.viewer.set_temporal_color( 435 | &self.names[self.index_of_move_joint.get()], 436 | 1.0, 437 | 0.0, 438 | 0.0, 439 | ); 440 | } 441 | } 442 | fn handle_key_press(&mut self, window: &mut Window, code: Key, modifiers: Modifiers) { 443 | let is_ctrl = modifiers.contains(NATIVE_MOD); 444 | if is_ctrl { 445 | // Heuristic for avoiding conflicts with browser keyboard shortcuts. 446 | return; 447 | } 448 | match code { 449 | Key::O | Key::LBracket => self.increment_move_joint_index(true), 450 | Key::P | Key::RBracket => self.increment_move_joint_index(false), 451 | Key::Period => { 452 | self.index_of_arm.inc(); 453 | self.update_ik_target_marker(); 454 | } 455 | Key::Comma => { 456 | self.index_of_arm.dec(); 457 | self.update_ik_target_marker(); 458 | } 459 | Key::A => { 460 | let mut origin = self.robot.origin(); 461 | origin.translation.vector[1] += self.move_base_diff_unit; 462 | self.robot.set_origin(origin); 463 | self.update_robot(); 464 | } 465 | Key::S => { 466 | let mut origin = self.robot.origin(); 467 | origin.translation.vector[0] -= self.move_base_diff_unit; 468 | self.robot.set_origin(origin); 469 | self.update_robot(); 470 | } 471 | Key::D => { 472 | let mut origin = self.robot.origin(); 473 | origin.translation.vector[1] -= self.move_base_diff_unit; 474 | self.robot.set_origin(origin); 475 | self.update_robot(); 476 | } 477 | Key::W => { 478 | let mut origin = self.robot.origin(); 479 | origin.translation.vector[0] += self.move_base_diff_unit; 480 | self.robot.set_origin(origin); 481 | self.update_robot(); 482 | } 483 | Key::C => { 484 | self.viewer.remove_robot(window, self.urdf_robot.get()); 485 | self.is_collision = !self.is_collision; 486 | self.viewer.add_robot_with_base_dir_and_collision_flag( 487 | window, 488 | self.urdf_robot.get(), 489 | self.input_path.parent(), 490 | self.is_collision, 491 | &self.package_path, 492 | ); 493 | self.update_robot(); 494 | } 495 | Key::F => { 496 | self.show_frames = !self.show_frames; 497 | self.update_frame_markers(); 498 | } 499 | Key::N => { 500 | self.show_link_names = !self.show_link_names; 501 | } 502 | #[cfg(not(target_family = "wasm"))] 503 | Key::L => { 504 | // reload 505 | self.reload(window, |urdf_robot| urdf_robot.reload()); 506 | } 507 | Key::R => { 508 | if self.has_joints() { 509 | move_joint_by_random(&mut self.robot).unwrap_or(()); 510 | self.update_robot(); 511 | } 512 | } 513 | Key::Z => { 514 | self.robot.set_origin(na::Isometry::identity()); 515 | if self.has_joints() { 516 | move_joint_to_zero(&mut self.robot); 517 | } 518 | self.update_robot(); 519 | } 520 | Key::Up => { 521 | if self.has_joints() { 522 | move_joint_by_index( 523 | self.index_of_move_joint.get(), 524 | self.move_joint_diff_unit, 525 | &mut self.robot, 526 | ) 527 | .unwrap_or(()); 528 | self.update_robot(); 529 | } 530 | } 531 | Key::Down => { 532 | if self.has_joints() { 533 | move_joint_by_index( 534 | self.index_of_move_joint.get(), 535 | -self.move_joint_diff_unit, 536 | &mut self.robot, 537 | ) 538 | .unwrap_or(()); 539 | self.update_robot(); 540 | } 541 | } 542 | Key::M => { 543 | self.hide_menu = !self.hide_menu; 544 | } 545 | _ => {} 546 | }; 547 | } 548 | pub fn run(mut self) { 549 | #[cfg(not(target_family = "wasm"))] 550 | ctrlc::set_handler(|| { 551 | ABORTED.store(true, Relaxed); 552 | }) 553 | .unwrap(); 554 | 555 | let window = self.window.take().unwrap(); 556 | let state = AppState { 557 | point_cloud_renderer: PointCloudRenderer::new(self.point_size), 558 | app: self, 559 | solver: k::JacobianIkSolver::default(), 560 | is_ctrl: false, 561 | is_shift: false, 562 | last_cur_pos_x: 0.0, 563 | last_cur_pos_y: 0.0, 564 | }; 565 | window.render_loop(state); 566 | } 567 | } 568 | 569 | impl fmt::Debug for UrdfViewerApp { 570 | fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { 571 | // kiss3d::window::Window doesn't implement Debug. 572 | f.debug_struct("UrdfViewerApp") 573 | .field("input_path", &self.input_path) 574 | .field("urdf_robot", &self.urdf_robot) 575 | .field("robot", &self.robot) 576 | .field("viewer", &self.viewer) 577 | .field("arms", &self.arms) 578 | .field("names", &self.names) 579 | .field("input_end_link_names", &self.input_end_link_names) 580 | .field("index_of_arm", &self.index_of_arm) 581 | .field("index_of_move_joint", &self.index_of_move_joint) 582 | .field("handle", &self.handle) 583 | .field("is_collision", &self.is_collision) 584 | .field("ik_constraints", &self.ik_constraints) 585 | .finish() 586 | } 587 | } 588 | 589 | #[cfg(not(target_family = "wasm"))] 590 | static ABORTED: AtomicBool = AtomicBool::new(false); 591 | 592 | struct AppState { 593 | app: UrdfViewerApp, 594 | solver: k::JacobianIkSolver, 595 | is_ctrl: bool, 596 | is_shift: bool, 597 | last_cur_pos_y: f64, 598 | last_cur_pos_x: f64, 599 | point_cloud_renderer: PointCloudRenderer, 600 | } 601 | 602 | impl AppState { 603 | fn handle_request(&mut self, window: &mut Window) { 604 | let handle = self.app.handle(); 605 | 606 | if let Some(robot) = handle.take_robot() { 607 | self.app.reload(window, |urdf_robot| *urdf_robot = robot); 608 | } 609 | 610 | #[cfg(not(target_family = "wasm"))] 611 | if handle.take_reload_request() { 612 | self.app.reload(window, |urdf_robot| urdf_robot.reload()); 613 | } 614 | 615 | while let Some(points) = handle.point_cloud.pop() { 616 | if points.points.len() == points.colors.len() { 617 | self.point_cloud_renderer 618 | .insert(points.id, &points.points, &points.colors); 619 | } else { 620 | error!( 621 | "point cloud length mismatch points={}, colors={}", 622 | points.points.len(), 623 | points.colors.len() 624 | ); 625 | } 626 | } 627 | 628 | while let Some(cube) = handle.cube.pop() { 629 | static COUNTER: AtomicUsize = AtomicUsize::new(0); 630 | let id = &if let Some(extent) = cube.extent { 631 | let id = cube 632 | .id 633 | .unwrap_or_else(|| format!("__cube{}", COUNTER.fetch_add(1, Relaxed))); 634 | if let Some(scene) = self.app.viewer.scene_node_mut(&id) { 635 | // remove pre-existent node 636 | scene.unlink(); 637 | } 638 | self.app 639 | .viewer 640 | .add_cube(window, &id, extent[0], extent[1], extent[2]); 641 | id 642 | } else if let Some(id) = cube.id { 643 | id 644 | } else { 645 | error!("must be at least one of cube.extent or cube.id specified"); 646 | break; // jump 647 | }; 648 | let scene = self.app.viewer.scene_node_mut(id).unwrap(); 649 | if let Some(color) = cube.color { 650 | scene.set_color(color[0], color[1], color[2]); 651 | } 652 | if let Some(p) = cube.position { 653 | scene.set_local_translation(na::Translation3::new(p[0], p[1], p[2])); 654 | } 655 | if let Some(q) = cube.quaternion { 656 | scene.set_local_rotation(na::UnitQuaternion::new_normalize(na::Quaternion::new( 657 | q[0], q[1], q[2], q[3], 658 | ))); 659 | } 660 | } 661 | 662 | while let Some(capsule) = handle.capsule.pop() { 663 | static COUNTER: AtomicUsize = AtomicUsize::new(0); 664 | let id = &capsule 665 | .id 666 | .unwrap_or_else(|| format!("__capsule{}", COUNTER.fetch_add(1, Relaxed))); 667 | if let Some(scene) = self.app.viewer.scene_node_mut(id) { 668 | // remove pre-existent node 669 | scene.unlink(); 670 | } 671 | self.app 672 | .viewer 673 | .add_capsule(window, id, capsule.radius, capsule.height); 674 | let scene = self.app.viewer.scene_node_mut(id).unwrap(); 675 | if let Some(color) = capsule.color { 676 | scene.set_color(color[0], color[1], color[2]); 677 | } 678 | if let Some(p) = capsule.position { 679 | scene.set_local_translation(na::Translation3::new(p[0], p[1], p[2])); 680 | } 681 | if let Some(q) = capsule.quaternion { 682 | scene.set_local_rotation(na::UnitQuaternion::new_normalize(na::Quaternion::new( 683 | q[0], q[1], q[2], q[3], 684 | ))); 685 | } 686 | } 687 | 688 | while let Some(axis_marker) = handle.axis_marker.pop() { 689 | static COUNTER: AtomicUsize = AtomicUsize::new(0); 690 | let id = &axis_marker 691 | .id 692 | .unwrap_or_else(|| format!("__axis_marker{}", COUNTER.fetch_add(1, Relaxed))); 693 | if let Some(scene) = self.app.viewer.scene_node_mut(id) { 694 | // remove pre-existent node 695 | scene.unlink(); 696 | } 697 | self.app.viewer.add_axis_cylinders_with_scale( 698 | window, 699 | id, 700 | axis_marker.size, 701 | self.app.axis_scale, 702 | ); 703 | let scene = self.app.viewer.scene_node_mut(id).unwrap(); 704 | if let Some(p) = axis_marker.position { 705 | scene.set_local_translation(na::Translation3::new(p[0], p[1], p[2])); 706 | } 707 | if let Some(q) = axis_marker.quaternion { 708 | scene.set_local_rotation(na::UnitQuaternion::new_normalize(na::Quaternion::new( 709 | q[0], q[1], q[2], q[3], 710 | ))); 711 | } 712 | } 713 | 714 | while let Some(relationship) = handle.relationship.pop() { 715 | if relationship.parent != relationship.child { 716 | if let (Some(mut parent), Some(mut child)) = ( 717 | self.app.viewer.scene_node(&relationship.parent).cloned(), 718 | self.app.viewer.scene_node(&relationship.child).cloned(), 719 | ) { 720 | let p = relationship.position; 721 | let t = parent.data().local_translation(); 722 | child.set_local_translation(na::Translation3::new( 723 | t.vector[0] + p[0], 724 | t.vector[1] + p[1], 725 | t.vector[2] + p[2], 726 | )); 727 | let q = relationship.quaternion; 728 | let r = parent.data().local_rotation(); 729 | child.set_local_rotation(na::UnitQuaternion::new_normalize( 730 | na::Quaternion::new(r.w + q[0], r.i + q[1], r.j + q[2], r.k + q[3]), 731 | )); 732 | child.unlink(); 733 | parent.add_child(child); 734 | } 735 | } 736 | } 737 | 738 | if self.app.has_joints() { 739 | // Joint positions for server 740 | if let Some(ja) = handle.take_target_joint_positions() { 741 | match self.app.set_joint_positions_from_request(&ja) { 742 | Ok(_) => { 743 | self.app.update_robot(); 744 | } 745 | Err(err) => { 746 | error!("{err}"); 747 | } 748 | } 749 | } 750 | handle.current_joint_positions.lock().unwrap().positions = 751 | self.app.robot.joint_positions(); 752 | } 753 | 754 | // Robot orientation for server 755 | while let Some(origin) = handle.pop_target_object_origin() { 756 | if origin.id == ROBOT_OBJECT_ID { 757 | self.app.set_robot_origin_from_request(&RobotOrigin { 758 | position: origin.position, 759 | quaternion: origin.quaternion, 760 | }); 761 | self.app.update_robot(); 762 | } else if let Some(scene) = self.app.viewer.scene_node_mut(&origin.id) { 763 | let p = origin.position; 764 | scene.set_local_translation(na::Translation3::new(p[0], p[1], p[2])); 765 | let q = origin.quaternion; 766 | scene.set_local_rotation(na::UnitQuaternion::new_normalize(na::Quaternion::new( 767 | q[0], q[1], q[2], q[3], 768 | ))); 769 | } else { 770 | error!("object '{}' not found", origin.id); 771 | } 772 | } 773 | let mut cur_origin = handle.current_robot_origin.lock().unwrap(); 774 | let origin = self.app.robot.origin(); 775 | for i in 0..3 { 776 | cur_origin.position[i] = origin.translation.vector[i]; 777 | } 778 | cur_origin.quaternion[0] = origin.rotation.quaternion().w; 779 | cur_origin.quaternion[1] = origin.rotation.quaternion().i; 780 | cur_origin.quaternion[2] = origin.rotation.quaternion().j; 781 | cur_origin.quaternion[3] = origin.rotation.quaternion().k; 782 | } 783 | } 784 | 785 | impl window::State for AppState { 786 | fn step(&mut self, window: &mut Window) { 787 | const FONT_SIZE_USAGE: f32 = 60.0; 788 | const FONT_SIZE_INFO: f32 = 80.0; 789 | 790 | #[cfg(not(target_family = "wasm"))] 791 | if ABORTED.load(Relaxed) { 792 | window.close(); 793 | return; 794 | } 795 | 796 | if !self.app.hide_menu { 797 | self.app.viewer.draw_text( 798 | window, 799 | HOW_TO_USE_STR, 800 | FONT_SIZE_USAGE, 801 | // The x2 factor should be removed for kiss3d >= 0.36 802 | // See: https://github.com/sebcrozet/kiss3d/issues/98 803 | &na::Point2::new((window.width() as f32 * 2.0) - 900.0, 10.0), 804 | &na::Point3::new(1f32, 1.0, 1.0), 805 | ); 806 | } 807 | self.handle_request(window); 808 | if self.app.has_joints() { 809 | self.app.viewer.draw_text( 810 | window, 811 | &format!( 812 | "moving joint name [{}]", 813 | self.app.names[self.app.index_of_move_joint.get()] 814 | ), 815 | FONT_SIZE_INFO, 816 | &na::Point2::new(10f32, 20.0), 817 | &na::Point3::new(0.5f32, 0.5, 1.0), 818 | ); 819 | } 820 | if self.app.has_arms() { 821 | let name = &self 822 | .app 823 | .get_arm() 824 | .iter() 825 | .last() 826 | .unwrap() 827 | .joint() 828 | .name 829 | .to_owned(); 830 | self.app.viewer.draw_text( 831 | window, 832 | &format!("IK target name [{name}]"), 833 | FONT_SIZE_INFO, 834 | &na::Point2::new(10f32, 100.0), 835 | &na::Point3::new(0.5f32, 0.8, 0.2), 836 | ); 837 | } 838 | if self.is_ctrl && !self.is_shift { 839 | self.app.viewer.draw_text( 840 | window, 841 | "moving joint by drag", 842 | FONT_SIZE_INFO, 843 | &na::Point2::new(10f32, 150.0), 844 | &na::Point3::new(0.9f32, 0.5, 1.0), 845 | ); 846 | } 847 | if self.is_shift { 848 | self.app.viewer.draw_text( 849 | window, 850 | "solving ik", 851 | FONT_SIZE_INFO, 852 | &na::Point2::new(10f32, 150.0), 853 | &na::Point3::new(0.9f32, 0.5, 1.0), 854 | ); 855 | } 856 | if self.app.show_link_names { 857 | self.app.draw_link_names(window); 858 | } 859 | 860 | for mut event in window.events().iter() { 861 | match event.value { 862 | WindowEvent::MouseButton(_, Action::Press, modifiers) => { 863 | if modifiers.contains(NATIVE_MOD) { 864 | self.is_ctrl = true; 865 | event.inhibited = true; 866 | } 867 | if modifiers.contains(Modifiers::Shift) { 868 | self.is_shift = true; 869 | event.inhibited = true; 870 | } 871 | } 872 | WindowEvent::CursorPos(x, y, _modifiers) => { 873 | if self.is_ctrl && !self.is_shift { 874 | event.inhibited = true; 875 | let move_gain = 0.005; 876 | if self.app.has_joints() { 877 | move_joint_by_index( 878 | self.app.index_of_move_joint.get(), 879 | (((x - self.last_cur_pos_x) + (y - self.last_cur_pos_y)) 880 | * move_gain) as f32, 881 | &mut self.app.robot, 882 | ) 883 | .unwrap_or(()); 884 | self.app.update_robot(); 885 | } 886 | } 887 | if self.is_shift { 888 | event.inhibited = true; 889 | if self.app.has_arms() { 890 | self.app.robot.update_transforms(); 891 | let mut target = self.app.get_end_transform(); 892 | let ik_move_gain = 0.002; 893 | target.translation.vector[2] -= 894 | ((y - self.last_cur_pos_y) * ik_move_gain) as f32; 895 | if self.is_ctrl { 896 | target.translation.vector[0] += 897 | ((x - self.last_cur_pos_x) * ik_move_gain) as f32; 898 | } else { 899 | target.translation.vector[1] += 900 | ((x - self.last_cur_pos_x) * ik_move_gain) as f32; 901 | } 902 | 903 | self.app.update_ik_target_marker(); 904 | let orig_angles = self.app.robot.joint_positions(); 905 | self.solver 906 | .solve_with_constraints( 907 | self.app.get_arm(), 908 | &target, 909 | &self.app.ik_constraints, 910 | ) 911 | .unwrap_or_else(|err| { 912 | self.app.robot.set_joint_positions_unchecked(&orig_angles); 913 | error!("{err}"); 914 | }); 915 | self.app.update_robot(); 916 | } 917 | } 918 | self.last_cur_pos_x = x; 919 | self.last_cur_pos_y = y; 920 | } 921 | WindowEvent::MouseButton(_, Action::Release, _) => { 922 | if self.is_ctrl { 923 | self.is_ctrl = false; 924 | event.inhibited = true; 925 | } else if self.is_shift { 926 | self.is_shift = false; 927 | event.inhibited = true; 928 | } 929 | } 930 | WindowEvent::Key(code, Action::Press, modifiers) => { 931 | self.app.handle_key_press(window, code, modifiers); 932 | event.inhibited = true; 933 | } 934 | _ => {} 935 | } 936 | } 937 | } 938 | 939 | #[allow(clippy::type_complexity)] 940 | fn cameras_and_effect_and_renderer( 941 | &mut self, 942 | ) -> ( 943 | Option<&mut dyn kiss3d::camera::Camera>, 944 | Option<&mut dyn kiss3d::planar_camera::PlanarCamera>, 945 | Option<&mut dyn kiss3d::renderer::Renderer>, 946 | Option<&mut dyn kiss3d::post_processing::PostProcessingEffect>, 947 | ) { 948 | ( 949 | Some(&mut self.app.viewer.arc_ball), 950 | None, 951 | Some(&mut self.point_cloud_renderer), 952 | None, 953 | ) 954 | } 955 | } 956 | 957 | /// Option for visualizing urdf 958 | #[derive(StructOpt, Debug, Deserialize)] 959 | #[serde(rename_all = "kebab-case")] 960 | #[non_exhaustive] 961 | pub struct Opt { 962 | /// Input urdf or xacro 963 | #[serde(default, rename = "urdf")] 964 | pub input_urdf_or_xacro: String, 965 | /// Xacro arguments 966 | #[structopt(long = "xacro-args", value_name = "ARGUMENT=VALUE", parse(try_from_str = parse_xacro_argument))] 967 | #[serde(default)] 968 | pub xacro_arguments: Vec<(String, String)>, 969 | /// end link names 970 | #[structopt(short = "e", long = "end-link-name")] 971 | #[serde(default)] 972 | pub end_link_names: Vec, 973 | /// Show collision element instead of visual 974 | #[structopt(short = "c", long = "collision")] 975 | #[serde(default)] 976 | pub is_collision: bool, 977 | /// Disable texture rendering 978 | #[structopt(short = "d", long = "disable-texture")] 979 | #[serde(default)] 980 | pub disable_texture: bool, 981 | #[cfg(feature = "assimp")] 982 | /// Disable using assimp for loading mesh 983 | #[structopt(long = "disable-assimp")] 984 | #[serde(default)] 985 | pub disable_assimp: bool, 986 | /// Port number for web server interface (default to 7777) 987 | #[structopt(short = "p", long = "web-server-port")] 988 | pub web_server_port: Option, 989 | 990 | #[structopt(long = "ignore-ik-position-x")] 991 | #[serde(default)] 992 | pub ignore_ik_position_x: bool, 993 | #[structopt(long = "ignore-ik-position-y")] 994 | #[serde(default)] 995 | pub ignore_ik_position_y: bool, 996 | #[structopt(long = "ignore-ik-position-z")] 997 | #[serde(default)] 998 | pub ignore_ik_position_z: bool, 999 | 1000 | #[structopt(long = "ignore-ik-rotation-x")] 1001 | #[serde(default)] 1002 | pub ignore_ik_rotation_x: bool, 1003 | #[structopt(long = "ignore-ik-rotation-y")] 1004 | #[serde(default)] 1005 | pub ignore_ik_rotation_y: bool, 1006 | #[structopt(long = "ignore-ik-rotation-z")] 1007 | #[serde(default)] 1008 | pub ignore_ik_rotation_z: bool, 1009 | 1010 | #[structopt(long = "bg-color-r", default_value = "0.0")] 1011 | #[serde(default)] 1012 | pub back_ground_color_r: f32, 1013 | #[structopt(long = "bg-color-g", default_value = "0.0")] 1014 | #[serde(default)] 1015 | pub back_ground_color_g: f32, 1016 | #[structopt(long = "bg-color-b", default_value = "0.3")] 1017 | #[serde(default = "default_back_ground_color_b")] 1018 | pub back_ground_color_b: f32, 1019 | 1020 | #[structopt(long = "tile-color1-r", default_value = "0.1")] 1021 | #[serde(default = "default_tile_color1")] 1022 | pub tile_color1_r: f32, 1023 | #[structopt(long = "tile-color1-g", default_value = "0.1")] 1024 | #[serde(default = "default_tile_color1")] 1025 | pub tile_color1_g: f32, 1026 | #[structopt(long = "tile-color1-b", default_value = "0.1")] 1027 | #[serde(default = "default_tile_color1")] 1028 | pub tile_color1_b: f32, 1029 | 1030 | #[structopt(long = "tile-color2-r", default_value = "0.8")] 1031 | #[serde(default = "default_tile_color2")] 1032 | pub tile_color2_r: f32, 1033 | #[structopt(long = "tile-color2-g", default_value = "0.8")] 1034 | #[serde(default = "default_tile_color2")] 1035 | pub tile_color2_g: f32, 1036 | #[structopt(long = "tile-color2-b", default_value = "0.8")] 1037 | #[serde(default = "default_tile_color2")] 1038 | pub tile_color2_b: f32, 1039 | 1040 | #[structopt(long = "ground-height")] 1041 | pub ground_height: Option, 1042 | 1043 | /// Replace `package://PACKAGE` in mesh tag with PATH. 1044 | #[structopt(long = "package-path", value_name = "PACKAGE=PATH")] 1045 | #[serde(default)] 1046 | pub package_path: Vec, 1047 | 1048 | /// Hide the menu by default. 1049 | #[structopt(short = "m", long = "hide-menu")] 1050 | #[serde(default)] 1051 | pub hide_menu: bool, 1052 | 1053 | #[structopt(short = "s", long = "axis-scale", default_value = "1.0")] 1054 | #[serde(default = "default_axis_scale")] 1055 | pub axis_scale: f32, 1056 | 1057 | #[structopt(short = "b", long = "move-base-diff-unit", default_value = "0.1")] 1058 | #[serde(default = "default_diff_unit")] 1059 | pub move_base_diff_unit: f32, 1060 | 1061 | #[structopt(short = "j", long = "move-joint-diff-unit", default_value = "0.1")] 1062 | #[serde(default = "default_diff_unit")] 1063 | pub move_joint_diff_unit: f32, 1064 | } 1065 | 1066 | fn default_back_ground_color_b() -> f32 { 1067 | 0.3 1068 | } 1069 | fn default_tile_color1() -> f32 { 1070 | 0.1 1071 | } 1072 | fn default_tile_color2() -> f32 { 1073 | 0.8 1074 | } 1075 | fn default_axis_scale() -> f32 { 1076 | 1.0 1077 | } 1078 | fn default_diff_unit() -> f32 { 1079 | 0.1 1080 | } 1081 | 1082 | fn parse_xacro_argument(xacro_argument: &str) -> Result<(String, String), Error> { 1083 | xacro_argument 1084 | .split_once('=') 1085 | .ok_or_else(|| { 1086 | Error::Other(format!( 1087 | "Invalid xacro argument key=value: no `=` found in {xacro_argument}" 1088 | )) 1089 | }) 1090 | .map(|s| (s.0.to_owned(), s.1.to_owned())) 1091 | } 1092 | 1093 | impl Opt { 1094 | pub fn create_ik_constraints(&self) -> k::Constraints { 1095 | k::Constraints { 1096 | position_x: !self.ignore_ik_position_x, 1097 | position_y: !self.ignore_ik_position_y, 1098 | position_z: !self.ignore_ik_position_z, 1099 | rotation_x: !self.ignore_ik_rotation_x, 1100 | rotation_y: !self.ignore_ik_rotation_y, 1101 | rotation_z: !self.ignore_ik_rotation_z, 1102 | ..Default::default() 1103 | } 1104 | } 1105 | 1106 | pub fn create_package_path_map(&self) -> Result, Error> { 1107 | let mut map = HashMap::with_capacity(self.package_path.len()); 1108 | for replace in &self.package_path { 1109 | let (package_name, path) = replace 1110 | .split_once('=') 1111 | .filter(|(k, _)| !k.is_empty()) 1112 | .ok_or_else(|| { 1113 | format!( 1114 | "--package-path may only accept PACKAGE=PATH format, but found '{replace}'" 1115 | ) 1116 | })?; 1117 | map.insert(package_name.to_owned(), path.to_owned()); 1118 | } 1119 | Ok(map) 1120 | } 1121 | 1122 | #[cfg(target_family = "wasm")] 1123 | pub fn from_params() -> Result { 1124 | let href = crate::utils::window()?.location().href()?; 1125 | debug!("href={href}"); 1126 | let url = url::Url::parse(&href).map_err(|e| e.to_string())?; 1127 | Ok(serde_qs::from_str(url.query().unwrap_or_default()).map_err(|e| e.to_string())?) 1128 | } 1129 | } 1130 | 1131 | #[cfg(test)] 1132 | mod tests { 1133 | use std::collections::BTreeSet; 1134 | 1135 | use super::*; 1136 | 1137 | #[test] 1138 | fn test_create_package_path_map() { 1139 | let opt: Opt = StructOpt::from_iter(["urdf-viz", "a.urdf", "--package-path", "a=b"]); 1140 | assert_eq!( 1141 | opt.create_package_path_map() 1142 | .unwrap() 1143 | .into_iter() 1144 | .collect::>(), 1145 | vec![("a".to_owned(), "b".to_owned())] 1146 | ); 1147 | let opt: Opt = StructOpt::from_iter([ 1148 | "urdf-viz", 1149 | "a.urdf", 1150 | "--package-path", 1151 | "a=b", 1152 | "--package-path", 1153 | "c=d=e", 1154 | ]); 1155 | assert_eq!( 1156 | opt.create_package_path_map() 1157 | .unwrap() 1158 | .into_iter() 1159 | .collect::>() 1160 | .into_iter() 1161 | .collect::>(), 1162 | vec![ 1163 | ("a".to_owned(), "b".to_owned()), 1164 | ("c".to_owned(), "d=e".to_owned()) 1165 | ] 1166 | ); 1167 | let opt: Opt = StructOpt::from_iter(["urdf-viz", "a.urdf", "--package-path", "a="]); 1168 | assert_eq!( 1169 | opt.create_package_path_map() 1170 | .unwrap() 1171 | .into_iter() 1172 | .collect::>(), 1173 | vec![("a".to_owned(), "".to_owned())] 1174 | ); 1175 | let opt: Opt = StructOpt::from_iter(["urdf-viz", "a.urdf", "--package-path", "=a"]); 1176 | assert!(opt.create_package_path_map().is_err()); 1177 | let opt: Opt = StructOpt::from_iter(["urdf-viz", "a.urdf", "--package-path", "a"]); 1178 | assert!(opt.create_package_path_map().is_err()); 1179 | } 1180 | 1181 | #[test] 1182 | fn test_params() { 1183 | let url = url::Url::parse("http://localhost:8080/?urdf=https://raw.githubusercontent.com/openrr/urdf-viz/main/sample.urdf").unwrap(); 1184 | let _: Opt = serde_qs::from_str(url.query().unwrap_or_default()).unwrap(); 1185 | } 1186 | } 1187 | -------------------------------------------------------------------------------- /src/assimp_utils.rs: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 Takashi Ogura 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | use k::nalgebra as na; 18 | use kiss3d::resource::Mesh; 19 | use std::cell::RefCell; 20 | use std::ffi::CStr; 21 | use std::os::raw::{c_float, c_uint}; 22 | use std::rc::Rc; 23 | use std::str; 24 | use tracing::*; 25 | 26 | const ASSIMP_DIFFUSE: &CStr = c"$clr.diffuse"; 27 | 28 | type RefCellMesh = Rc>; 29 | 30 | fn assimp_material_texture(material: &assimp::Material<'_>) -> Option { 31 | let mut path = assimp_sys::AiString::default(); 32 | let texture_type = assimp_sys::AiTextureType::Diffuse; 33 | let mat = &**material; 34 | let mapping = assimp_sys::AiTextureMapping::UV; 35 | let mut uv_index: c_uint = 0; 36 | let mut blend: c_float = 0.0; 37 | let mut op = assimp_sys::AiTextureOp::Multiply; 38 | let mut map_mode = assimp_sys::AiTextureMapMode::Wrap; 39 | let mut flags: c_uint = 0; 40 | unsafe { 41 | match assimp_sys::aiGetMaterialTexture( 42 | mat, 43 | texture_type, 44 | 0, 45 | &mut path, 46 | &mapping, 47 | &mut uv_index, 48 | &mut blend, 49 | &mut op, 50 | &mut map_mode, 51 | &mut flags, 52 | ) { 53 | assimp_sys::AiReturn::Success => Some( 54 | // assimp-rs's impl AsRef for AiString does a similar, but it 55 | // might panic because assimp-rs doesn't handle the case where assimp 56 | // returns an out-of-range length (which probably means an error). 57 | str::from_utf8(path.data.get(0..path.length)?) 58 | .ok()? 59 | .to_owned(), 60 | ), 61 | _ => None, 62 | } 63 | } 64 | } 65 | 66 | fn assimp_material_color( 67 | material: &assimp::Material<'_>, 68 | color_type: &'static CStr, 69 | ) -> Option> { 70 | let mut assimp_color = assimp_sys::AiColor4D { 71 | r: 0.0, 72 | g: 0.0, 73 | b: 0.0, 74 | a: 0.0, 75 | }; 76 | let mat = &**material; 77 | unsafe { 78 | match assimp_sys::aiGetMaterialColor(mat, color_type.as_ptr(), 0, 0, &mut assimp_color) { 79 | assimp_sys::AiReturn::Success => Some(na::Vector3::::new( 80 | assimp_color.r, 81 | assimp_color.g, 82 | assimp_color.b, 83 | )), 84 | _ => None, 85 | } 86 | } 87 | } 88 | 89 | pub(crate) fn convert_assimp_scene_to_kiss3d_mesh( 90 | scene: &assimp::Scene<'_>, 91 | ) -> (Vec, Vec, Vec>) { 92 | let meshes = scene 93 | .mesh_iter() 94 | .map(|mesh| { 95 | let mut vertices = Vec::new(); 96 | let mut indices = Vec::new(); 97 | vertices.extend(mesh.vertex_iter().map(|v| na::Point3::new(v.x, v.y, v.z))); 98 | indices.extend(mesh.face_iter().filter_map(|f| { 99 | if f.num_indices == 3 { 100 | // TODO: https://github.com/openrr/urdf-viz/issues/22 101 | Some(na::Point3::new(f[0] as u16, f[1] as u16, f[2] as u16)) 102 | } else { 103 | debug!("invalid mesh!"); 104 | None 105 | } 106 | })); 107 | Rc::new(RefCell::new(Mesh::new( 108 | vertices, indices, None, None, false, 109 | ))) 110 | }) 111 | .collect(); 112 | let colors = scene 113 | .material_iter() 114 | .filter_map(|material| assimp_material_color(&material, ASSIMP_DIFFUSE)) 115 | .collect(); 116 | let texture_files = scene 117 | .material_iter() 118 | .filter_map(|material| assimp_material_texture(&material)) 119 | .collect(); 120 | (meshes, texture_files, colors) 121 | } 122 | -------------------------------------------------------------------------------- /src/bin/urdf-viz.rs: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 Takashi Ogura 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | use structopt::StructOpt; 18 | use tracing::{debug, warn}; 19 | use urdf_viz::{app::*, WebServer}; 20 | 21 | const DEFAULT_WEB_SERVER_PORT: u16 = 7777; 22 | 23 | #[tokio::main] 24 | async fn main() -> urdf_viz::Result<()> { 25 | tracing_subscriber::fmt::init(); 26 | let opt = Opt::from_args(); 27 | debug!(?opt); 28 | let package_path = opt.create_package_path_map()?; 29 | let urdf_robot = urdf_viz::utils::RobotModel::new( 30 | &opt.input_urdf_or_xacro, 31 | package_path, 32 | &opt.xacro_arguments, 33 | )?; 34 | let ik_constraints = opt.create_ik_constraints(); 35 | let mut app = UrdfViewerApp::new( 36 | urdf_robot, 37 | opt.end_link_names, 38 | opt.is_collision, 39 | opt.disable_texture, 40 | #[cfg(feature = "assimp")] 41 | opt.disable_assimp, 42 | #[cfg(not(feature = "assimp"))] 43 | true, 44 | ( 45 | opt.back_ground_color_r, 46 | opt.back_ground_color_g, 47 | opt.back_ground_color_b, 48 | ), 49 | (opt.tile_color1_r, opt.tile_color1_g, opt.tile_color1_b), 50 | (opt.tile_color2_r, opt.tile_color2_g, opt.tile_color2_b), 51 | opt.ground_height, 52 | opt.hide_menu, 53 | opt.axis_scale, 54 | opt.move_base_diff_unit, 55 | opt.move_joint_diff_unit, 56 | )?; 57 | app.set_ik_constraints(ik_constraints); 58 | app.init(); 59 | 60 | match WebServer::new( 61 | opt.web_server_port.unwrap_or(DEFAULT_WEB_SERVER_PORT), 62 | app.handle(), 63 | ) 64 | .bind() 65 | { 66 | Ok(server) => { 67 | tokio::spawn(async move { server.await.unwrap() }); 68 | } 69 | Err(e) => { 70 | if opt.web_server_port.is_none() { 71 | // If the user didn't specify a port, the user may not need web server, so treat as a warning. 72 | warn!("failed to start web server with default port: {e}"); 73 | } else { 74 | // If the user specified a port, the user intends to use web server, so treat as an error. 75 | return Err(e); 76 | } 77 | } 78 | } 79 | 80 | app.run(); 81 | Ok(()) 82 | } 83 | -------------------------------------------------------------------------------- /src/errors.rs: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 Takashi Ogura 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | use std::io; 18 | use thiserror::Error; 19 | 20 | #[derive(Debug, Error)] 21 | #[non_exhaustive] 22 | pub enum Error { 23 | #[error("Error: {:?}", .0)] 24 | Other(String), 25 | #[error("IOError: {:?}", .0)] 26 | IoError(#[from] io::Error), 27 | #[error("FromUtf8Error: {:?}", .0)] 28 | FromUtf8Error(#[from] std::string::FromUtf8Error), 29 | #[error("UrdfError: {:?}", .0)] 30 | Urdf(#[from] urdf_rs::UrdfError), 31 | } 32 | 33 | pub type Result = ::std::result::Result; 34 | 35 | impl<'a> From<&'a str> for Error { 36 | fn from(error: &'a str) -> Self { 37 | Error::Other(error.to_owned()) 38 | } 39 | } 40 | 41 | impl From for Error { 42 | fn from(error: String) -> Self { 43 | Error::Other(error) 44 | } 45 | } 46 | 47 | #[cfg(target_family = "wasm")] 48 | impl From for Error { 49 | fn from(error: wasm_bindgen::JsValue) -> Self { 50 | Error::Other(format!("{error:?}")) 51 | } 52 | } 53 | 54 | #[cfg(target_family = "wasm")] 55 | impl From for wasm_bindgen::JsValue { 56 | fn from(error: Error) -> Self { 57 | Self::from_str(&error.to_string()) 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /src/handle.rs: -------------------------------------------------------------------------------- 1 | use crossbeam_queue::ArrayQueue; 2 | use serde::{Deserialize, Serialize}; 3 | #[cfg(not(target_family = "wasm"))] 4 | use std::sync::atomic::{AtomicBool, Ordering}; 5 | use std::{ 6 | ops, 7 | sync::{Arc, Mutex, MutexGuard}, 8 | }; 9 | 10 | use crate::utils::RobotModel; 11 | 12 | #[derive(Deserialize, Serialize, Debug, Clone, Default)] 13 | pub struct JointNamesAndPositions { 14 | pub names: Vec, 15 | pub positions: Vec, 16 | } 17 | 18 | #[derive(Deserialize, Serialize, Debug, Clone)] 19 | pub struct RobotOrigin { 20 | pub position: [f32; 3], 21 | pub quaternion: [f32; 4], 22 | } 23 | 24 | impl Default for RobotOrigin { 25 | fn default() -> Self { 26 | Self { 27 | position: [0.0; 3], 28 | quaternion: [0.0, 0.0, 0.0, 1.0], 29 | } 30 | } 31 | } 32 | 33 | #[derive(Deserialize, Serialize, Debug, Clone)] 34 | pub struct ObjectOrigin { 35 | pub id: String, 36 | pub position: [f32; 3], 37 | pub quaternion: [f32; 4], 38 | } 39 | 40 | #[derive(Deserialize, Serialize, Debug, Clone)] 41 | pub struct PointsAndColors { 42 | pub id: Option, 43 | pub points: Vec<[f32; 3]>, 44 | pub colors: Vec<[f32; 3]>, 45 | } 46 | 47 | #[derive(Deserialize, Serialize, Debug, Clone)] 48 | pub struct Cube { 49 | pub id: Option, 50 | pub extent: Option<[f32; 3]>, 51 | pub color: Option<[f32; 3]>, 52 | pub position: Option<[f32; 3]>, 53 | pub quaternion: Option<[f32; 4]>, 54 | } 55 | 56 | #[derive(Deserialize, Serialize, Debug, Clone)] 57 | pub struct Capsule { 58 | pub id: Option, 59 | pub height: f32, 60 | pub radius: f32, 61 | pub color: Option<[f32; 3]>, 62 | pub position: Option<[f32; 3]>, 63 | pub quaternion: Option<[f32; 4]>, 64 | } 65 | 66 | #[derive(Deserialize, Serialize, Debug, Clone)] 67 | pub struct AxisMarker { 68 | pub id: Option, 69 | pub size: f32, 70 | pub position: Option<[f32; 3]>, 71 | pub quaternion: Option<[f32; 4]>, 72 | } 73 | 74 | #[derive(Deserialize, Serialize, Debug, Clone)] 75 | pub struct Relationship { 76 | pub parent: String, 77 | pub child: String, 78 | /// Relative position 79 | pub position: [f32; 3], 80 | /// Relative rotation 81 | pub quaternion: [f32; 4], 82 | } 83 | 84 | /// Handle to get and modify the state of the robot. 85 | #[derive(Debug)] 86 | pub struct RobotStateHandle { 87 | pub(crate) target_joint_positions: Mutex>, 88 | pub(crate) current_joint_positions: Mutex, 89 | pub(crate) target_object_origin: ArrayQueue, 90 | pub(crate) current_robot_origin: Mutex, 91 | pub(crate) point_cloud: ArrayQueue, 92 | pub(crate) cube: ArrayQueue, 93 | pub(crate) capsule: ArrayQueue, 94 | pub(crate) axis_marker: ArrayQueue, 95 | pub(crate) relationship: ArrayQueue, 96 | pub(crate) urdf_text: Option>>, 97 | pub(crate) robot: Mutex>, 98 | #[cfg(not(target_family = "wasm"))] 99 | pub(crate) reload_request: AtomicBool, 100 | } 101 | 102 | impl Default for RobotStateHandle { 103 | fn default() -> Self { 104 | const QUEUE_CAP: usize = 8; 105 | Self { 106 | target_joint_positions: Mutex::default(), 107 | current_joint_positions: Mutex::default(), 108 | target_object_origin: ArrayQueue::new(QUEUE_CAP), 109 | current_robot_origin: Mutex::default(), 110 | point_cloud: ArrayQueue::new(QUEUE_CAP), 111 | cube: ArrayQueue::new(QUEUE_CAP), 112 | capsule: ArrayQueue::new(QUEUE_CAP), 113 | axis_marker: ArrayQueue::new(QUEUE_CAP), 114 | relationship: ArrayQueue::new(QUEUE_CAP), 115 | urdf_text: None, 116 | robot: Mutex::default(), 117 | #[cfg(not(target_family = "wasm"))] 118 | reload_request: AtomicBool::new(false), 119 | } 120 | } 121 | } 122 | 123 | // Wrapper type to prevent mutex type changes from becoming a breaking change. 124 | macro_rules! impl_guard { 125 | ($guard_name:ident($target:ident)) => { 126 | #[derive(Debug)] 127 | pub struct $guard_name<'a>(MutexGuard<'a, $target>); 128 | impl ops::Deref for $guard_name<'_> { 129 | type Target = $target; 130 | fn deref(&self) -> &Self::Target { 131 | &self.0 132 | } 133 | } 134 | impl ops::DerefMut for $guard_name<'_> { 135 | fn deref_mut(&mut self) -> &mut Self::Target { 136 | &mut self.0 137 | } 138 | } 139 | }; 140 | } 141 | impl_guard!(JointNamesAndPositionsLockGuard(JointNamesAndPositions)); 142 | impl_guard!(RobotOriginLockGuard(RobotOrigin)); 143 | impl_guard!(UrdfTextLockGuard(String)); 144 | 145 | pub const ROBOT_OBJECT_ID: &str = "robot"; 146 | 147 | impl RobotStateHandle { 148 | pub fn current_joint_positions(&self) -> JointNamesAndPositionsLockGuard<'_> { 149 | JointNamesAndPositionsLockGuard(self.current_joint_positions.lock().unwrap()) 150 | } 151 | 152 | pub fn current_robot_origin(&self) -> RobotOriginLockGuard<'_> { 153 | RobotOriginLockGuard(self.current_robot_origin.lock().unwrap()) 154 | } 155 | 156 | pub fn urdf_text(&self) -> Option> { 157 | Some(UrdfTextLockGuard(self.urdf_text.as_ref()?.lock().unwrap())) 158 | } 159 | 160 | pub fn set_target_joint_positions(&self, joint_positions: JointNamesAndPositions) { 161 | *self.target_joint_positions.lock().unwrap() = Some(joint_positions); 162 | } 163 | 164 | pub fn set_target_robot_origin(&self, robot_origin: RobotOrigin) { 165 | self.target_object_origin.force_push(ObjectOrigin { 166 | id: ROBOT_OBJECT_ID.to_owned(), 167 | position: robot_origin.position, 168 | quaternion: robot_origin.quaternion, 169 | }); 170 | } 171 | 172 | pub fn set_target_object_origin(&self, object_origin: ObjectOrigin) { 173 | self.target_object_origin.force_push(object_origin); 174 | } 175 | 176 | pub fn set_point_cloud(&self, points_and_colors: PointsAndColors) { 177 | self.point_cloud.force_push(points_and_colors); 178 | } 179 | 180 | pub fn set_cube(&self, cube: Cube) { 181 | self.cube.force_push(cube); 182 | } 183 | 184 | pub fn set_capsule(&self, capsule: Capsule) { 185 | self.capsule.force_push(capsule); 186 | } 187 | 188 | pub fn set_axis_marker(&self, axis_marker: AxisMarker) { 189 | self.axis_marker.force_push(axis_marker); 190 | } 191 | 192 | pub fn set_robot(&self, robot: RobotModel) { 193 | // set_robot may change name or number of joints, so reset target_joint_positions. 194 | *self.target_joint_positions.lock().unwrap() = None; 195 | *self.robot.lock().unwrap() = Some(robot); 196 | } 197 | 198 | pub fn set_relationship(&self, relationship: Relationship) { 199 | self.relationship.force_push(relationship); 200 | } 201 | 202 | pub fn take_target_joint_positions(&self) -> Option { 203 | self.target_joint_positions.lock().unwrap().take() 204 | } 205 | 206 | pub fn pop_target_object_origin(&self) -> Option { 207 | self.target_object_origin.pop() 208 | } 209 | 210 | pub(crate) fn take_robot(&self) -> Option { 211 | self.robot.lock().unwrap().take() 212 | } 213 | 214 | #[cfg(not(target_family = "wasm"))] 215 | pub(crate) fn set_reload_request(&self) { 216 | self.reload_request.swap(true, Ordering::SeqCst); 217 | } 218 | 219 | #[cfg(not(target_family = "wasm"))] 220 | pub(crate) fn take_reload_request(&self) -> bool { 221 | self.reload_request.swap(false, Ordering::SeqCst) 222 | } 223 | } 224 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 Takashi Ogura 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | //! Visualize [URDF(Unified Robot Description Format)](http://wiki.ros.org/urdf) file. 18 | //! `urdf-viz` is written by rust-lang. 19 | 20 | #[cfg(feature = "assimp")] 21 | mod assimp_utils; 22 | 23 | pub mod app; 24 | mod errors; 25 | pub use errors::*; 26 | mod handle; 27 | pub use handle::*; 28 | #[cfg(not(target_family = "wasm"))] 29 | mod web_server; 30 | #[cfg(not(target_family = "wasm"))] 31 | pub use web_server::*; 32 | mod viewer; 33 | pub use viewer::*; 34 | mod mesh; 35 | pub use mesh::*; 36 | mod urdf; 37 | pub use urdf::*; 38 | mod point_cloud; 39 | pub mod utils; 40 | 41 | // re-export 42 | #[doc(no_inline)] 43 | pub use kiss3d::{self, event::*}; 44 | -------------------------------------------------------------------------------- /src/mesh.rs: -------------------------------------------------------------------------------- 1 | use crate::{errors::Result, utils::is_url}; 2 | use k::nalgebra as na; 3 | use kiss3d::scene::SceneNode; 4 | use std::{cell::RefCell, ffi::OsStr, io, path::Path, rc::Rc}; 5 | use tracing::*; 6 | 7 | #[cfg(feature = "assimp")] 8 | #[cfg(not(target_family = "wasm"))] 9 | fn load_mesh_assimp( 10 | file_string: &str, 11 | scale: na::Vector3, 12 | opt_urdf_color: &Option>, 13 | group: &mut SceneNode, 14 | use_texture: bool, 15 | ) -> Result { 16 | use crate::assimp_utils::*; 17 | 18 | let filename = Path::new(file_string); 19 | 20 | let mut base = group.add_group(); 21 | let mut importer = assimp::Importer::new(); 22 | importer.pre_transform_vertices(|x| x.enable = true); 23 | importer.collada_ignore_up_direction(true); 24 | importer.triangulate(true); 25 | let (meshes, textures, colors) = 26 | convert_assimp_scene_to_kiss3d_mesh(&importer.read_file(file_string)?); 27 | info!( 28 | "num mesh, texture, colors = {} {} {}", 29 | meshes.len(), 30 | textures.len(), 31 | colors.len() 32 | ); 33 | let mesh_scenes = meshes 34 | .into_iter() 35 | .map(|mesh| { 36 | let mut scene = base.add_mesh(mesh, scale); 37 | // use urdf color as default 38 | if let Some(urdf_color) = *opt_urdf_color { 39 | scene.set_color(urdf_color[0], urdf_color[1], urdf_color[2]); 40 | } 41 | scene 42 | }) 43 | .collect::>(); 44 | // use texture only for dae (collada) 45 | let is_collada = matches!( 46 | filename.extension().and_then(OsStr::to_str), 47 | Some("dae" | "DAE") 48 | ); 49 | // do not use texture, use only color in urdf file. 50 | if !use_texture || !is_collada { 51 | return Ok(base); 52 | } 53 | 54 | // Size of color and mesh are same, use each color for mesh 55 | if mesh_scenes.len() == colors.len() { 56 | for (count, (mut mesh_scene, color)) in 57 | mesh_scenes.into_iter().zip(colors.into_iter()).enumerate() 58 | { 59 | mesh_scene.set_color(color[0], color[1], color[2]); 60 | // Is this OK? 61 | if count < textures.len() { 62 | let mut texture_path = filename.to_path_buf(); 63 | texture_path.set_file_name(textures[count].clone()); 64 | debug!("using texture={}", texture_path.display()); 65 | if texture_path.exists() { 66 | mesh_scene.set_texture_from_file(&texture_path, texture_path.to_str().unwrap()); 67 | } 68 | } 69 | } 70 | } else { 71 | // When size of mesh and color mismatch, use only first color/texture for all meshes. 72 | // If no color found, use urdf color instead. 73 | for mut mesh_scene in mesh_scenes { 74 | if !textures.is_empty() { 75 | let mut texture_path = filename.to_path_buf(); 76 | texture_path.set_file_name(textures[0].clone()); 77 | debug!("texture={}", texture_path.display()); 78 | if texture_path.exists() { 79 | mesh_scene.set_texture_from_file(&texture_path, texture_path.to_str().unwrap()); 80 | } 81 | } 82 | if !colors.is_empty() { 83 | let color = colors[0]; 84 | mesh_scene.set_color(color[0], color[1], color[2]); 85 | } 86 | } 87 | } 88 | Ok(base) 89 | } 90 | 91 | #[cfg(not(target_family = "wasm"))] 92 | pub fn load_mesh( 93 | filename: impl AsRef, 94 | scale: na::Vector3, 95 | opt_color: &Option>, 96 | group: &mut SceneNode, 97 | use_texture: bool, 98 | #[allow(unused_variables)] use_assimp: bool, 99 | ) -> Result { 100 | let file_string = filename.as_ref(); 101 | 102 | #[cfg(feature = "assimp")] 103 | if use_assimp { 104 | if is_url(file_string) { 105 | let file = crate::utils::fetch_tempfile(file_string)?; 106 | let path = file.path().to_str().unwrap(); 107 | return load_mesh_assimp(path, scale, opt_color, group, use_texture); 108 | } 109 | return load_mesh_assimp(file_string, scale, opt_color, group, use_texture); 110 | } 111 | 112 | let ext = Path::new(file_string).extension().and_then(OsStr::to_str); 113 | debug!("load {ext:?}: path = {file_string}"); 114 | load_with_mesh_loader( 115 | &fetch_or_read(file_string)?, 116 | file_string, 117 | scale, 118 | opt_color, 119 | group, 120 | use_texture, 121 | ) 122 | } 123 | 124 | #[cfg(not(target_family = "wasm"))] 125 | fn fetch_or_read(filename: &str) -> Result> { 126 | use std::io::Read; 127 | 128 | const RESPONSE_SIZE_LIMIT: usize = 10 * 1_024 * 1_024; 129 | 130 | if is_url(filename) { 131 | let mut buf = Vec::with_capacity(128); 132 | ureq::get(filename) 133 | .call() 134 | .map_err(|e| crate::Error::Other(e.to_string()))? 135 | .into_reader() 136 | .take((RESPONSE_SIZE_LIMIT + 1) as u64) 137 | .read_to_end(&mut buf)?; 138 | if buf.len() > RESPONSE_SIZE_LIMIT { 139 | return Err(crate::errors::Error::Other(format!( 140 | "{filename} is too big" 141 | ))); 142 | } 143 | Ok(buf) 144 | } else { 145 | Ok(std::fs::read(filename)?) 146 | } 147 | } 148 | 149 | /// NOTE: Unlike other platforms, the first argument should be the data loaded 150 | /// by [`utils::load_mesh`](crate::utils::load_mesh), not the path. 151 | #[cfg(target_family = "wasm")] 152 | pub fn load_mesh( 153 | data: impl AsRef, 154 | scale: na::Vector3, 155 | opt_color: &Option>, 156 | group: &mut SceneNode, 157 | _use_texture: bool, 158 | _use_assimp: bool, 159 | ) -> Result { 160 | let data = crate::utils::Mesh::decode(data.as_ref())?; 161 | let ext = Path::new(&data.path).extension().and_then(OsStr::to_str); 162 | debug!("load {ext:?}: path = {}", data.path); 163 | let use_texture = false; 164 | load_with_mesh_loader( 165 | data.bytes().unwrap(), 166 | &data.path, 167 | scale, 168 | opt_color, 169 | group, 170 | use_texture, 171 | ) 172 | } 173 | 174 | fn load_with_mesh_loader( 175 | bytes: &[u8], 176 | file_string: &str, 177 | scale: na::Vector3, 178 | opt_color: &Option>, 179 | group: &mut SceneNode, 180 | mut use_texture: bool, 181 | ) -> Result { 182 | let mut base = group.add_group(); 183 | let mut loader = mesh_loader::Loader::default(); 184 | use_texture &= !is_url(file_string); 185 | if use_texture { 186 | // TODO: Using fetch_or_read can support remote materials, but loading becomes slow. 187 | // #[cfg(not(target_family = "wasm"))] 188 | // { 189 | // loader = loader 190 | // .custom_reader(|p| fetch_or_read(p.to_str().unwrap()).map_err(io::Error::other)); 191 | // } 192 | } else { 193 | loader = loader.custom_reader(|_| Err(io::Error::other("texture rendering disabled"))); 194 | } 195 | let scene = loader.load_from_slice(bytes, file_string).map_err(|e| { 196 | if e.kind() == io::ErrorKind::Unsupported { 197 | crate::errors::Error::from(format!( 198 | "{file_string} is not supported, because assimp feature is disabled" 199 | )) 200 | } else { 201 | e.into() 202 | } 203 | })?; 204 | 205 | for (mesh, material) in scene.meshes.into_iter().zip(scene.materials) { 206 | let coords = mesh.vertices.into_iter().map(Into::into).collect(); 207 | let faces = mesh 208 | .faces 209 | .into_iter() 210 | // TODO: https://github.com/openrr/urdf-viz/issues/22 211 | .map(|f| na::Point3::new(f[0] as u16, f[1] as u16, f[2] as u16)) 212 | .collect(); 213 | 214 | let kiss3d_mesh = Rc::new(RefCell::new(kiss3d::resource::Mesh::new( 215 | coords, faces, None, None, false, 216 | ))); 217 | let mut kiss3d_scene = base.add_mesh(kiss3d_mesh, scale); 218 | if use_texture { 219 | if let Some(color) = material.color.diffuse { 220 | kiss3d_scene.set_color(color[0], color[1], color[2]); 221 | } 222 | if let Some(path) = &material.texture.diffuse { 223 | let path_string = path.to_str().unwrap(); 224 | // TODO: Using fetch_or_read can support remote materials, but loading becomes slow. 225 | // let buf = fetch_or_read(path_string)?; 226 | // kiss3d_scene.set_texture_from_memory(&buf, path_string); 227 | kiss3d_scene.set_texture_from_file(path, path_string); 228 | } 229 | if let Some(path) = &material.texture.ambient { 230 | let path_string = path.to_str().unwrap(); 231 | // TODO: Using fetch_or_read can support remote materials, but loading becomes slow. 232 | // let buf = fetch_or_read(path_string)?; 233 | // kiss3d_scene.set_texture_from_memory(&buf, path_string); 234 | kiss3d_scene.set_texture_from_file(path, path_string); 235 | } 236 | } 237 | } 238 | if let Some(color) = *opt_color { 239 | base.set_color(color[0], color[1], color[2]); 240 | } 241 | Ok(base) 242 | } 243 | -------------------------------------------------------------------------------- /src/point_cloud.rs: -------------------------------------------------------------------------------- 1 | // Adapted from https://github.com/sebcrozet/kiss3d/blob/v0.30.0/examples/persistent_point_cloud.rs. 2 | 3 | use std::{collections::HashMap, ops::Range}; 4 | 5 | use kiss3d::camera::Camera; 6 | use kiss3d::context::Context; 7 | use kiss3d::nalgebra as na; 8 | use kiss3d::renderer::Renderer; 9 | use kiss3d::resource::{ 10 | AllocationType, BufferType, Effect, GPUVec, ShaderAttribute, ShaderUniform, 11 | }; 12 | use na::{Matrix4, Point3}; 13 | 14 | pub(crate) struct PointCloudRenderer { 15 | shader: Effect, 16 | pos: ShaderAttribute>, 17 | color: ShaderAttribute>, 18 | proj: ShaderUniform>, 19 | view: ShaderUniform>, 20 | colored_points: GPUVec>, 21 | id_map: HashMap>, 22 | point_size: f32, 23 | } 24 | 25 | impl PointCloudRenderer { 26 | pub(crate) fn new(point_size: f32) -> PointCloudRenderer { 27 | let mut shader = Effect::new_from_str(&vertex_shader_src(point_size), FRAGMENT_SHADER_SRC); 28 | 29 | shader.use_program(); 30 | 31 | PointCloudRenderer { 32 | colored_points: GPUVec::new(Vec::new(), BufferType::Array, AllocationType::StreamDraw), 33 | id_map: HashMap::new(), 34 | pos: shader.get_attrib::>("position").unwrap(), 35 | color: shader.get_attrib::>("color").unwrap(), 36 | proj: shader.get_uniform::>("proj").unwrap(), 37 | view: shader.get_uniform::>("view").unwrap(), 38 | shader, 39 | point_size, 40 | } 41 | } 42 | 43 | pub(crate) fn insert(&mut self, id: Option, points: &[[f32; 3]], colors: &[[f32; 3]]) { 44 | assert_eq!(points.len(), colors.len()); 45 | if let Some(colored_points) = self.colored_points.data_mut() { 46 | if let Some(id) = id { 47 | if let Some(range) = self.id_map.remove(&id) { 48 | let len = range.end - range.start; 49 | colored_points.drain(range.clone()); 50 | for r in self.id_map.values_mut() { 51 | if r.start > range.start { 52 | r.start -= len; 53 | r.end -= len; 54 | } 55 | } 56 | } 57 | 58 | let start = colored_points.len(); 59 | colored_points.reserve(points.len() * 2); 60 | for (point, color) in points.iter().zip(colors) { 61 | colored_points.push((*point).into()); 62 | colored_points.push((*color).into()); 63 | } 64 | self.id_map.insert(id, start..colored_points.len()); 65 | } else { 66 | colored_points.reserve(points.len() * 2); 67 | for (point, color) in points.iter().zip(colors) { 68 | colored_points.push((*point).into()); 69 | colored_points.push((*color).into()); 70 | } 71 | } 72 | } 73 | } 74 | } 75 | 76 | impl Renderer for PointCloudRenderer { 77 | fn render(&mut self, pass: usize, camera: &mut dyn Camera) { 78 | if self.colored_points.len() == 0 { 79 | return; 80 | } 81 | 82 | self.shader.use_program(); 83 | self.pos.enable(); 84 | self.color.enable(); 85 | 86 | camera.upload(pass, &mut self.proj, &mut self.view); 87 | 88 | self.color.bind_sub_buffer(&mut self.colored_points, 1, 1); 89 | self.pos.bind_sub_buffer(&mut self.colored_points, 1, 0); 90 | 91 | let ctxt = Context::get(); 92 | ctxt.point_size(self.point_size); 93 | ctxt.draw_arrays(Context::POINTS, 0, (self.colored_points.len() / 2) as i32); 94 | 95 | self.pos.disable(); 96 | self.color.disable(); 97 | } 98 | } 99 | 100 | fn vertex_shader_src(point_size: f32) -> String { 101 | format!( 102 | "#version 100 103 | attribute vec3 position; 104 | attribute vec3 color; 105 | varying vec3 Color; 106 | uniform mat4 proj; 107 | uniform mat4 view; 108 | void main() {{ 109 | gl_Position = proj * view * vec4(position, 1.0); 110 | gl_PointSize = {point_size:.1}; 111 | Color = color; 112 | }}", 113 | ) 114 | } 115 | 116 | const FRAGMENT_SHADER_SRC: &str = "#version 100 117 | #ifdef GL_FRAGMENT_PRECISION_HIGH 118 | precision highp float; 119 | #else 120 | precision mediump float; 121 | #endif 122 | varying vec3 Color; 123 | void main() { 124 | gl_FragColor = vec4(Color, 1.0); 125 | }"; 126 | -------------------------------------------------------------------------------- /src/urdf.rs: -------------------------------------------------------------------------------- 1 | use crate::errors::{Error, Result}; 2 | use crate::mesh::load_mesh; 3 | use crate::utils::is_url; 4 | use k::nalgebra as na; 5 | use kiss3d::scene::SceneNode; 6 | use std::borrow::Cow; 7 | use std::collections::HashMap; 8 | use std::path::Path; 9 | use tracing::*; 10 | 11 | pub fn add_geometry( 12 | geometry: &urdf_rs::Geometry, 13 | opt_color: &Option>, 14 | base_dir: Option<&Path>, 15 | group: &mut SceneNode, 16 | use_texture: bool, 17 | use_assimp: bool, 18 | package_path: &HashMap, 19 | ) -> Result { 20 | match *geometry { 21 | urdf_rs::Geometry::Box { ref size } => { 22 | let mut cube = group.add_cube(size[0] as f32, size[1] as f32, size[2] as f32); 23 | if let Some(color) = *opt_color { 24 | cube.set_color(color[0], color[1], color[2]); 25 | } 26 | Ok(cube) 27 | } 28 | urdf_rs::Geometry::Cylinder { radius, length } => { 29 | let mut base = group.add_group(); 30 | let mut cylinder = base.add_cylinder(radius as f32, length as f32); 31 | cylinder.append_rotation(&na::UnitQuaternion::from_axis_angle( 32 | &na::Vector3::x_axis(), 33 | 1.57, 34 | )); 35 | if let Some(color) = *opt_color { 36 | base.set_color(color[0], color[1], color[2]); 37 | } 38 | Ok(base) 39 | } 40 | urdf_rs::Geometry::Capsule { radius, length } => { 41 | let mut base = group.add_group(); 42 | let mut cylinder = base.add_cylinder(radius as f32, length as f32); 43 | cylinder.append_rotation(&na::UnitQuaternion::from_axis_angle( 44 | &na::Vector3::x_axis(), 45 | 1.57, 46 | )); 47 | let mut sphere1 = base.add_sphere(radius as f32); 48 | sphere1.append_translation(&na::Translation3::new(0.0, 0.0, length as f32 * 0.5)); 49 | let mut sphere2 = base.add_sphere(radius as f32); 50 | sphere2.append_translation(&na::Translation3::new(0.0, 0.0, length as f32 * -0.5)); 51 | 52 | if let Some(color) = *opt_color { 53 | cylinder.set_color(color[0], color[1], color[2]); 54 | sphere1.set_color(color[0], color[1], color[2]); 55 | sphere2.set_color(color[0], color[1], color[2]); 56 | } 57 | Ok(base) 58 | } 59 | urdf_rs::Geometry::Sphere { radius } => { 60 | let mut sphere = group.add_sphere(radius as f32); 61 | if let Some(color) = *opt_color { 62 | sphere.set_color(color[0], color[1], color[2]); 63 | } 64 | Ok(sphere) 65 | } 66 | urdf_rs::Geometry::Mesh { 67 | ref filename, 68 | scale, 69 | } => { 70 | let scale = scale.unwrap_or(DEFAULT_MESH_SCALE); 71 | let mut filename = Cow::Borrowed(&**filename); 72 | if !cfg!(target_family = "wasm") { 73 | // On WASM, this is handled in utils::load_mesh 74 | if filename.starts_with("package://") { 75 | if let Some(replaced_filename) = 76 | crate::utils::replace_package_with_path(&filename, package_path) 77 | { 78 | filename = replaced_filename.into(); 79 | } 80 | }; 81 | let replaced_filename = urdf_rs::utils::expand_package_path(&filename, base_dir)?; 82 | if !is_url(&replaced_filename) && !Path::new(&*replaced_filename).exists() { 83 | return Err(Error::from(format!("{replaced_filename} not found"))); 84 | } 85 | filename = replaced_filename.into_owned().into(); 86 | } 87 | let na_scale = na::Vector3::new(scale[0] as f32, scale[1] as f32, scale[2] as f32); 88 | debug!("filename = {filename}"); 89 | if cfg!(feature = "assimp") { 90 | load_mesh( 91 | &filename, 92 | na_scale, 93 | opt_color, 94 | group, 95 | use_texture, 96 | use_assimp, 97 | ) 98 | } else { 99 | match load_mesh( 100 | &filename, 101 | na_scale, 102 | opt_color, 103 | group, 104 | use_texture, 105 | use_assimp, 106 | ) { 107 | Ok(scene) => Ok(scene), 108 | Err(e) => { 109 | error!("{e}"); 110 | let mut base = group.add_cube(0.05f32, 0.05, 0.05); 111 | if let Some(color) = *opt_color { 112 | base.set_color(color[0], color[1], color[2]); 113 | } 114 | Ok(base) 115 | } 116 | } 117 | } 118 | } 119 | } 120 | } 121 | 122 | // Use material which is defined as root materials if found. 123 | // Root material is used for PR2, but not documented. 124 | pub fn rgba_from_visual(urdf_robot: &urdf_rs::Robot, visual: &urdf_rs::Visual) -> urdf_rs::Vec4 { 125 | match urdf_robot 126 | .materials 127 | .iter() 128 | .find(|mat| visual.material.as_ref().is_some_and(|m| mat.name == m.name)) 129 | .cloned() 130 | { 131 | Some(ref material) => material 132 | .color 133 | .as_ref() 134 | .map(|color| color.rgba) 135 | .unwrap_or_default(), 136 | None => visual 137 | .material 138 | .as_ref() 139 | .and_then(|material| material.color.as_ref().map(|color| color.rgba)) 140 | .unwrap_or_default(), 141 | } 142 | } 143 | 144 | // https://github.com/openrr/urdf-rs/pull/3/files#diff-0fb2eeea3273a4c9b3de69ee949567f546dc8c06b1e190336870d00b54ea0979L36-L38 145 | const DEFAULT_MESH_SCALE: urdf_rs::Vec3 = urdf_rs::Vec3([1.0f64; 3]); 146 | -------------------------------------------------------------------------------- /src/utils.rs: -------------------------------------------------------------------------------- 1 | use std::collections::HashMap; 2 | 3 | #[cfg(not(target_family = "wasm"))] 4 | pub use native::*; 5 | #[cfg(target_family = "wasm")] 6 | pub use wasm::*; 7 | 8 | pub(crate) fn replace_package_with_path( 9 | filename: &str, 10 | package_path: &HashMap, 11 | ) -> Option { 12 | let path = filename.strip_prefix("package://")?; 13 | let (package_name, path) = path.split_once('/')?; 14 | let package_path = package_path.get(package_name)?; 15 | Some(format!( 16 | "{}/{path}", 17 | package_path.strip_suffix('/').unwrap_or(package_path), 18 | )) 19 | } 20 | 21 | pub(crate) fn is_url(path: &str) -> bool { 22 | path.starts_with("https://") || path.starts_with("http://") 23 | } 24 | 25 | #[cfg(not(target_family = "wasm"))] 26 | mod native { 27 | use std::{ 28 | collections::HashMap, 29 | ffi::OsStr, 30 | fs, mem, 31 | path::Path, 32 | sync::{Arc, Mutex}, 33 | }; 34 | 35 | use tracing::error; 36 | 37 | use crate::{utils::is_url, Result}; 38 | 39 | fn read_urdf(path: &str, xacro_args: &[(String, String)]) -> Result<(urdf_rs::Robot, String)> { 40 | let urdf_text = if Path::new(path).extension().and_then(OsStr::to_str) == Some("xacro") { 41 | urdf_rs::utils::convert_xacro_to_urdf_with_args(path, xacro_args)? 42 | } else if is_url(path) { 43 | ureq::get(path) 44 | .call() 45 | .map_err(|e| crate::Error::Other(e.to_string()))? 46 | .into_string()? 47 | } else { 48 | fs::read_to_string(path)? 49 | }; 50 | let robot = urdf_rs::read_from_string(&urdf_text)?; 51 | Ok((robot, urdf_text)) 52 | } 53 | 54 | #[derive(Debug)] 55 | pub struct RobotModel { 56 | pub(crate) path: String, 57 | pub(crate) urdf_text: Arc>, 58 | robot: urdf_rs::Robot, 59 | package_path: HashMap, 60 | xacro_args: Vec<(String, String)>, 61 | } 62 | 63 | impl RobotModel { 64 | pub fn new( 65 | path: impl Into, 66 | package_path: HashMap, 67 | xacro_args: &[(String, String)], 68 | ) -> Result { 69 | let path = path.into(); 70 | let (robot, urdf_text) = read_urdf(&path, xacro_args)?; 71 | Ok(Self { 72 | path, 73 | urdf_text: Arc::new(Mutex::new(urdf_text)), 74 | robot, 75 | package_path, 76 | xacro_args: xacro_args.to_owned(), 77 | }) 78 | } 79 | 80 | pub async fn from_text( 81 | path: impl Into, 82 | urdf_text: impl Into, 83 | package_path: HashMap, 84 | ) -> Result { 85 | let path = path.into(); 86 | let urdf_text = urdf_text.into(); 87 | let robot = urdf_rs::read_from_string(&urdf_text)?; 88 | Ok(Self { 89 | path, 90 | urdf_text: Arc::new(Mutex::new(urdf_text)), 91 | robot, 92 | package_path, 93 | xacro_args: Vec::new(), 94 | }) 95 | } 96 | 97 | pub(crate) fn get(&mut self) -> &urdf_rs::Robot { 98 | &self.robot 99 | } 100 | 101 | pub(crate) fn reload(&mut self) { 102 | match read_urdf(&self.path, &self.xacro_args) { 103 | Ok((robot, text)) => { 104 | self.robot = robot; 105 | *self.urdf_text.lock().unwrap() = text; 106 | } 107 | Err(e) => { 108 | error!("{e}"); 109 | } 110 | } 111 | } 112 | 113 | pub(crate) fn take_package_path_map(&mut self) -> HashMap { 114 | mem::take(&mut self.package_path) 115 | } 116 | } 117 | 118 | #[cfg(feature = "assimp")] 119 | /// http request -> write to tempfile -> return that file 120 | pub(crate) fn fetch_tempfile(url: &str) -> Result { 121 | use std::io::{Read, Write}; 122 | 123 | const RESPONSE_SIZE_LIMIT: usize = 10 * 1_024 * 1_024; 124 | 125 | let mut buf: Vec = vec![]; 126 | ureq::get(url) 127 | .call() 128 | .map_err(|e| crate::Error::Other(e.to_string()))? 129 | .into_reader() 130 | .take((RESPONSE_SIZE_LIMIT + 1) as u64) 131 | .read_to_end(&mut buf)?; 132 | if buf.len() > RESPONSE_SIZE_LIMIT { 133 | return Err(crate::Error::Other(format!("{url} is too big"))); 134 | } 135 | let mut file = tempfile::NamedTempFile::new()?; 136 | file.write_all(&buf)?; 137 | Ok(file) 138 | } 139 | } 140 | 141 | #[cfg(target_family = "wasm")] 142 | mod wasm { 143 | use std::{ 144 | collections::HashMap, 145 | mem, 146 | path::Path, 147 | str, 148 | sync::{Arc, Mutex}, 149 | }; 150 | 151 | use base64::{engine::general_purpose::STANDARD as BASE64, Engine}; 152 | use js_sys::Uint8Array; 153 | use serde::{Deserialize, Serialize}; 154 | use tracing::debug; 155 | use wasm_bindgen::JsCast; 156 | use wasm_bindgen_futures::JsFuture; 157 | use web_sys::Response; 158 | 159 | use crate::{utils::is_url, Error, Result}; 160 | 161 | #[derive(Serialize, Deserialize)] 162 | pub(crate) struct Mesh { 163 | pub(crate) path: String, 164 | data: MeshData, 165 | } 166 | 167 | impl Mesh { 168 | pub(crate) fn decode(data: &str) -> Result { 169 | let mut mesh: Self = serde_json::from_str(data).map_err(|e| e.to_string())?; 170 | match &mesh.data { 171 | MeshData::None => {} 172 | MeshData::Base64(s) => { 173 | mesh.data = MeshData::Bytes(BASE64.decode(s).map_err(|e| e.to_string())?); 174 | } 175 | MeshData::Bytes(_) => unreachable!(), 176 | } 177 | Ok(mesh) 178 | } 179 | 180 | pub(crate) fn bytes(&self) -> Option<&[u8]> { 181 | match &self.data { 182 | MeshData::None => None, 183 | MeshData::Bytes(bytes) => Some(bytes), 184 | MeshData::Base64(_) => unreachable!(), 185 | } 186 | } 187 | } 188 | 189 | #[derive(Serialize, Deserialize)] 190 | enum MeshData { 191 | Base64(String), 192 | Bytes(Vec), 193 | None, 194 | } 195 | 196 | pub fn window() -> Result { 197 | Ok(web_sys::window().ok_or("failed to get window")?) 198 | } 199 | 200 | async fn fetch(input_file: &str) -> Result { 201 | let promise = window()?.fetch_with_str(input_file); 202 | 203 | let response = JsFuture::from(promise) 204 | .await? 205 | .dyn_into::() 206 | .unwrap(); 207 | 208 | Ok(response) 209 | } 210 | 211 | pub async fn read_to_string(input_file: impl AsRef) -> Result { 212 | let promise = fetch(input_file.as_ref()).await?.text()?; 213 | 214 | let s = JsFuture::from(promise).await?; 215 | 216 | Ok(s.as_string() 217 | .ok_or_else(|| format!("{} is not string", input_file.as_ref()))?) 218 | } 219 | 220 | pub async fn read(input_file: impl AsRef) -> Result> { 221 | let promise = fetch(input_file.as_ref()).await?.array_buffer()?; 222 | 223 | let bytes = JsFuture::from(promise).await?; 224 | 225 | Ok(Uint8Array::new(&bytes).to_vec()) 226 | } 227 | 228 | pub async fn load_mesh( 229 | robot: &mut urdf_rs::Robot, 230 | urdf_path: impl AsRef, 231 | package_path: &HashMap, 232 | ) -> Result<()> { 233 | let urdf_path = urdf_path.as_ref(); 234 | for geometry in robot.links.iter_mut().flat_map(|link| { 235 | link.visual 236 | .iter_mut() 237 | .map(|v| &mut v.geometry) 238 | .chain(link.collision.iter_mut().map(|c| &mut c.geometry)) 239 | }) { 240 | if let urdf_rs::Geometry::Mesh { filename, .. } = geometry { 241 | let input_file = if is_url(filename) { 242 | filename.clone() 243 | } else if filename.starts_with("package://") { 244 | crate::utils::replace_package_with_path(filename, package_path).ok_or_else(|| 245 | format!( 246 | "ros package ({filename}) is not supported in wasm; consider using `package-path[]` URL parameter", 247 | ))? 248 | } else if filename.starts_with("file://") { 249 | return Err(Error::from(format!( 250 | "local file ({filename}) is not supported in wasm", 251 | ))); 252 | } else { 253 | // We don't use url::Url::path/set_path here, because 254 | // urdf_path may be a relative path to a file bundled 255 | // with the server. Path::with_file_name works for wasm 256 | // where the separator is /, so we use it. 257 | urdf_path 258 | .with_file_name(&filename) 259 | .to_str() 260 | .unwrap() 261 | .to_string() 262 | }; 263 | 264 | debug!("loading {input_file}"); 265 | let data = MeshData::Base64(BASE64.encode(read(&input_file).await?)); 266 | 267 | let new = serde_json::to_string(&Mesh { 268 | path: filename.clone(), 269 | data, 270 | }) 271 | .unwrap(); 272 | *filename = new; 273 | } 274 | } 275 | Ok(()) 276 | } 277 | 278 | #[derive(Debug)] 279 | pub struct RobotModel { 280 | pub(crate) path: String, 281 | pub(crate) urdf_text: Arc>, 282 | robot: urdf_rs::Robot, 283 | package_path: HashMap, 284 | } 285 | 286 | impl RobotModel { 287 | pub async fn new( 288 | path: impl Into, 289 | package_path: HashMap, 290 | ) -> Result { 291 | let path = path.into(); 292 | let urdf_text = read_to_string(&path).await?; 293 | Self::from_text(path, urdf_text, package_path).await 294 | } 295 | 296 | pub async fn from_text( 297 | path: impl Into, 298 | urdf_text: impl Into, 299 | package_path: HashMap, 300 | ) -> Result { 301 | let path = path.into(); 302 | let urdf_text = urdf_text.into(); 303 | let mut robot = urdf_rs::read_from_string(&urdf_text)?; 304 | load_mesh(&mut robot, &path, &package_path).await?; 305 | Ok(Self { 306 | path, 307 | urdf_text: Arc::new(Mutex::new(urdf_text)), 308 | robot, 309 | package_path, 310 | }) 311 | } 312 | 313 | pub(crate) fn get(&mut self) -> &urdf_rs::Robot { 314 | &self.robot 315 | } 316 | 317 | pub(crate) fn take_package_path_map(&mut self) -> HashMap { 318 | mem::take(&mut self.package_path) 319 | } 320 | } 321 | } 322 | 323 | #[cfg(test)] 324 | mod tests { 325 | use super::*; 326 | 327 | #[test] 328 | fn test_replace_package_with_path() { 329 | let mut package_path = HashMap::new(); 330 | package_path.insert("a".to_owned(), "path".to_owned()); 331 | assert_eq!( 332 | replace_package_with_path("package://a/b/c", &package_path), 333 | Some("path/b/c".to_owned()) 334 | ); 335 | assert_eq!( 336 | replace_package_with_path("package://a", &package_path), 337 | None 338 | ); 339 | assert_eq!( 340 | replace_package_with_path("package://b/b/c", &package_path), 341 | None 342 | ); 343 | assert_eq!(replace_package_with_path("a", &package_path), None); 344 | } 345 | } 346 | -------------------------------------------------------------------------------- /src/viewer.rs: -------------------------------------------------------------------------------- 1 | use crate::urdf::*; 2 | use k::nalgebra as na; 3 | use kiss3d::camera::ArcBall; 4 | use kiss3d::camera::Camera; 5 | use kiss3d::ncollide3d::simba::scalar::SubsetOf; 6 | use kiss3d::scene::SceneNode; 7 | use kiss3d::window::Window; 8 | use std::collections::HashMap; 9 | use std::fmt; 10 | use std::path::Path; 11 | use std::rc::Rc; 12 | use tracing::*; 13 | 14 | const DEFAULT_CYLINDER_RADIUS: f32 = 0.01; 15 | 16 | pub struct Viewer { 17 | scenes: HashMap, 18 | pub arc_ball: ArcBall, 19 | font: Rc, 20 | original_colors: HashMap>>, 21 | is_texture_enabled: bool, 22 | is_assimp_enabled: bool, 23 | link_joint_map: HashMap, 24 | } 25 | 26 | impl Viewer { 27 | pub fn new(title: &str) -> (Viewer, Window) { 28 | Self::with_background_color(title, (0.0, 0.0, 0.3)) 29 | } 30 | 31 | pub fn with_background_color(title: &str, color: (f32, f32, f32)) -> (Viewer, Window) { 32 | let eye = na::Point3::new(3.0f32, 1.0, 1.0); 33 | let at = na::Point3::new(0.0f32, 0.0, 0.25); 34 | let mut window = kiss3d::window::Window::new_with_size(title, 1400, 1000); 35 | window.set_light(kiss3d::light::Light::StickToCamera); 36 | window.set_background_color(color.0, color.1, color.2); 37 | let mut arc_ball = ArcBall::new(eye, at); 38 | arc_ball.set_up_axis(na::Vector3::z()); 39 | arc_ball.set_dist_step(0.99); 40 | let font = kiss3d::text::Font::default(); 41 | let viewer = Viewer { 42 | scenes: HashMap::new(), 43 | arc_ball, 44 | font, 45 | original_colors: HashMap::new(), 46 | is_texture_enabled: true, 47 | is_assimp_enabled: cfg!(feature = "assimp"), 48 | link_joint_map: HashMap::new(), 49 | }; 50 | (viewer, window) 51 | } 52 | pub fn disable_texture(&mut self) { 53 | self.is_texture_enabled = false; 54 | } 55 | pub fn enable_texture(&mut self) { 56 | self.is_texture_enabled = true; 57 | } 58 | 59 | pub fn disable_assimp(&mut self) { 60 | self.is_assimp_enabled = false; 61 | } 62 | /// if `assimp` feature is disabled, this method does nothing. 63 | pub fn enable_assimp(&mut self) { 64 | self.is_assimp_enabled = cfg!(feature = "assimp"); 65 | } 66 | 67 | pub fn add_robot( 68 | &mut self, 69 | window: &mut Window, 70 | urdf_robot: &urdf_rs::Robot, 71 | package_path: &HashMap, 72 | ) { 73 | self.add_robot_with_base_dir(window, urdf_robot, None, package_path); 74 | } 75 | pub fn add_robot_with_base_dir( 76 | &mut self, 77 | window: &mut Window, 78 | urdf_robot: &urdf_rs::Robot, 79 | base_dir: Option<&Path>, 80 | package_path: &HashMap, 81 | ) { 82 | self.add_robot_with_base_dir_and_collision_flag( 83 | window, 84 | urdf_robot, 85 | base_dir, 86 | false, 87 | package_path, 88 | ); 89 | } 90 | pub fn add_robot_with_base_dir_and_collision_flag( 91 | &mut self, 92 | window: &mut Window, 93 | urdf_robot: &urdf_rs::Robot, 94 | base_dir: Option<&Path>, 95 | is_collision: bool, 96 | package_path: &HashMap, 97 | ) { 98 | self.link_joint_map = k::urdf::link_to_joint_map(urdf_robot); 99 | 100 | for l in &urdf_robot.links { 101 | let num = if is_collision { 102 | l.collision.len() 103 | } else { 104 | l.visual.len() 105 | }; 106 | if num == 0 { 107 | continue; 108 | } 109 | let mut scene_group = window.add_group(); 110 | let mut colors = Vec::new(); 111 | for i in 0..num { 112 | let (geom_element, origin_element) = if is_collision { 113 | (&l.collision[i].geometry, &l.collision[i].origin) 114 | } else { 115 | (&l.visual[i].geometry, &l.visual[i].origin) 116 | }; 117 | let mut opt_color = None; 118 | if l.visual.len() > i { 119 | let rgba = rgba_from_visual(urdf_robot, &l.visual[i]); 120 | let color = na::Point3::new(rgba[0] as f32, rgba[1] as f32, rgba[2] as f32); 121 | if color[0] > 0.001 || color[1] > 0.001 || color[2] > 0.001 { 122 | opt_color = Some(color); 123 | } 124 | colors.push(color); 125 | } 126 | match add_geometry( 127 | geom_element, 128 | &opt_color, 129 | base_dir, 130 | &mut scene_group, 131 | self.is_texture_enabled, 132 | self.is_assimp_enabled, 133 | package_path, 134 | ) { 135 | Ok(mut base_group) => { 136 | // set initial origin offset 137 | base_group.set_local_transformation(k::urdf::isometry_from(origin_element)); 138 | } 139 | Err(e) => { 140 | error!("failed to create for link '{}': {e}", l.name); 141 | } 142 | } 143 | } 144 | let joint_name = self 145 | .link_joint_map 146 | .get(&l.name) 147 | .unwrap_or_else(|| panic!("joint for link '{}' not found", l.name)); 148 | self.scenes.insert(joint_name.to_owned(), scene_group); 149 | self.original_colors.insert(joint_name.to_owned(), colors); 150 | } 151 | } 152 | pub fn remove_robot(&mut self, window: &mut Window, urdf_robot: &urdf_rs::Robot) { 153 | for l in &urdf_robot.links { 154 | let joint_name = self 155 | .link_joint_map 156 | .get(&l.name) 157 | .unwrap_or_else(|| panic!("{} not found", l.name)); 158 | if let Some(scene) = self.scenes.get_mut(joint_name) { 159 | window.remove_node(scene); 160 | } 161 | } 162 | } 163 | pub fn add_axis_cylinders(&mut self, window: &mut Window, name: &str, size: f32) { 164 | self.add_axis_cylinders_with_scale(window, name, size, 1.0); 165 | } 166 | pub fn add_axis_cylinders_with_scale( 167 | &mut self, 168 | window: &mut Window, 169 | name: &str, 170 | size: f32, 171 | scale: f32, 172 | ) { 173 | let mut axis_group = window.add_group(); 174 | let radius = DEFAULT_CYLINDER_RADIUS * scale; 175 | let size = size * scale; 176 | let mut x = axis_group.add_cylinder(radius, size); 177 | x.set_color(0.0, 0.0, 1.0); 178 | let mut y = axis_group.add_cylinder(radius, size); 179 | y.set_color(0.0, 1.0, 0.0); 180 | let mut z = axis_group.add_cylinder(radius, size); 181 | z.set_color(1.0, 0.0, 0.0); 182 | let rot_x = na::UnitQuaternion::from_axis_angle(&na::Vector3::x_axis(), 1.57); 183 | let rot_y = na::UnitQuaternion::from_axis_angle(&na::Vector3::y_axis(), 1.57); 184 | let rot_z = na::UnitQuaternion::from_axis_angle(&na::Vector3::z_axis(), 1.57); 185 | x.append_translation(&na::Translation3::new(0.0, 0.0, size * 0.5)); 186 | y.append_translation(&na::Translation3::new(0.0, size * 0.5, 0.0)); 187 | z.append_translation(&na::Translation3::new(size * 0.5, 0.0, 0.0)); 188 | x.set_local_rotation(rot_x); 189 | y.set_local_rotation(rot_y); 190 | z.set_local_rotation(rot_z); 191 | self.scenes.insert(name.to_owned(), axis_group); 192 | } 193 | pub fn scene_node(&self, name: &str) -> Option<&SceneNode> { 194 | self.scenes.get(name) 195 | } 196 | pub fn scene_node_mut(&mut self, name: &str) -> Option<&mut SceneNode> { 197 | self.scenes.get_mut(name) 198 | } 199 | pub fn update(&mut self, robot: &k::Chain) 200 | where 201 | T: k::RealField + k::SubsetOf + kiss3d::ncollide3d::simba::scalar::SubsetOf, 202 | { 203 | robot.update_transforms(); 204 | for link in robot.iter() { 205 | let trans = link.world_transform().unwrap(); 206 | let link_name = &link.joint().name; 207 | let trans_f32: na::Isometry3 = na::Isometry3::to_superset(&trans); 208 | match self.scenes.get_mut(link_name) { 209 | Some(obj) => { 210 | obj.set_local_transformation(trans_f32); 211 | } 212 | None => { 213 | debug!("{link_name} not found"); 214 | } 215 | } 216 | } 217 | } 218 | pub fn draw_text( 219 | &mut self, 220 | window: &mut Window, 221 | text: &str, 222 | size: f32, 223 | pos: &na::Point2, 224 | color: &na::Point3, 225 | ) { 226 | window.draw_text(text, pos, size, &self.font, color); 227 | } 228 | 229 | pub fn draw_text_from_3d( 230 | &mut self, 231 | window: &mut Window, 232 | text: &str, 233 | size: f32, 234 | pos: &na::Point3, 235 | color: &na::Point3, 236 | ) { 237 | let height = window.height() as f32; 238 | let width = window.width() as f32; 239 | let text_position_in_2d: na::Matrix< 240 | f32, 241 | na::Const<2>, 242 | na::Const<1>, 243 | na::ArrayStorage, 244 | > = self.arc_ball.project(pos, &na::Vector2::new(width, height)); 245 | self.draw_text( 246 | window, 247 | text, 248 | size, 249 | // The x2 factor should be removed for kiss3d >= 0.36 250 | // See: https://github.com/sebcrozet/kiss3d/issues/98 251 | &na::Point2::new( 252 | text_position_in_2d.x * 2.0, 253 | (height - text_position_in_2d.y) * 2.0, 254 | ), 255 | color, 256 | ); 257 | } 258 | 259 | pub fn set_temporal_color(&mut self, link_name: &str, r: f32, g: f32, b: f32) { 260 | if let Some(obj) = self.scenes.get_mut(link_name) { 261 | obj.set_color(r, g, b); 262 | } 263 | } 264 | pub fn reset_temporal_color(&mut self, link_name: &str) { 265 | if let Some(colors) = self.original_colors.get(link_name) { 266 | if let Some(obj) = self.scenes.get_mut(link_name) { 267 | for color in colors { 268 | obj.apply_to_scene_nodes_mut(&mut |o| { 269 | o.set_color(color[0], color[1], color[2]); 270 | }); 271 | } 272 | } 273 | } 274 | } 275 | pub fn add_ground( 276 | &mut self, 277 | window: &mut Window, 278 | ground_height: f32, 279 | panel_size: f32, 280 | half_panel_num: i32, 281 | ground_color1: (f32, f32, f32), 282 | ground_color2: (f32, f32, f32), 283 | ) -> Vec { 284 | let mut panels = Vec::new(); 285 | const PANEL_HEIGHT: f32 = 0.0001; 286 | 287 | for i in -half_panel_num..half_panel_num { 288 | for j in -half_panel_num..half_panel_num { 289 | let mut c0 = window.add_cube(panel_size, panel_size, PANEL_HEIGHT); 290 | if (i + j) % 2 == 0 { 291 | c0.set_color(ground_color1.0, ground_color1.1, ground_color1.2); 292 | } else { 293 | c0.set_color(ground_color2.0, ground_color2.1, ground_color2.2); 294 | } 295 | let x_ind = j as f32 + 0.5; 296 | let y_ind = i as f32 + 0.5; 297 | let trans = na::Isometry3::from_parts( 298 | na::Translation3::new( 299 | panel_size * x_ind, 300 | panel_size * y_ind, 301 | ground_height - PANEL_HEIGHT * 0.5, 302 | ), 303 | na::UnitQuaternion::identity(), 304 | ); 305 | c0.set_local_transformation(trans); 306 | panels.push(c0); 307 | } 308 | } 309 | panels 310 | } 311 | 312 | //https://docs.rs/kiss3d/latest/kiss3d/scene/struct.SceneNode.html#method.add_cube 313 | pub(crate) fn add_cube(&mut self, window: &mut Window, name: &str, wx: f32, wy: f32, wz: f32) { 314 | let cube = window.add_cube(wx, wy, wz); 315 | self.scenes.insert(name.to_owned(), cube); 316 | } 317 | 318 | pub(crate) fn add_capsule( 319 | &mut self, 320 | window: &mut Window, 321 | name: &str, 322 | radius: f32, 323 | height: f32, 324 | ) { 325 | let cube = window.add_capsule(radius, height); 326 | self.scenes.insert(name.to_owned(), cube); 327 | } 328 | } 329 | 330 | impl fmt::Debug for Viewer { 331 | fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { 332 | // kiss3d::scene::SceneNode and kiss3d::text::Font don't implement Debug. 333 | f.debug_struct("Viewer") 334 | .field("scenes", &self.scenes.keys()) 335 | .field("original_colors", &self.original_colors) 336 | .field("is_texture_enabled", &self.is_texture_enabled) 337 | .field("link_joint_map", &self.link_joint_map) 338 | .finish() 339 | } 340 | } 341 | -------------------------------------------------------------------------------- /src/web_server.rs: -------------------------------------------------------------------------------- 1 | use std::{future::Future, sync::Arc}; 2 | 3 | use axum::{ 4 | extract::State, 5 | http::StatusCode, 6 | routing::{get, post}, 7 | Json, Router, 8 | }; 9 | use serde::{Deserialize, Serialize}; 10 | 11 | use crate::handle::{JointNamesAndPositions, RobotOrigin, RobotStateHandle}; 12 | 13 | type Handle = Arc; 14 | 15 | #[derive(Deserialize, Serialize, Debug, Clone)] 16 | struct ResultResponse { 17 | is_ok: bool, 18 | reason: String, 19 | } 20 | 21 | impl ResultResponse { 22 | const SUCCESS: Self = ResultResponse { 23 | is_ok: true, 24 | reason: String::new(), 25 | }; 26 | } 27 | 28 | #[derive(Debug)] 29 | pub struct WebServer { 30 | port: u16, 31 | handle: Arc, 32 | } 33 | 34 | impl WebServer { 35 | pub fn new(port: u16, handle: Arc) -> Self { 36 | Self { port, handle } 37 | } 38 | 39 | pub fn handle(&self) -> Arc { 40 | self.handle.clone() 41 | } 42 | 43 | pub fn bind(self) -> crate::Result> + Send> { 44 | let app = app(self.handle()); 45 | 46 | // Use std::net::TcpListener::bind to early catch web server startup failure 47 | let addr: std::net::SocketAddr = ([0, 0, 0, 0], self.port).into(); 48 | let listener = (|| { 49 | let l = std::net::TcpListener::bind(addr)?; 50 | l.set_nonblocking(true)?; 51 | tokio::net::TcpListener::from_std(l) 52 | })() 53 | .map_err(|e| format!("error binding to {addr}: {e}"))?; 54 | 55 | let server = axum::serve(listener, app.into_make_service()); 56 | Ok(async move { server.await.map_err(|e| crate::Error::Other(e.to_string())) }) 57 | } 58 | } 59 | 60 | fn app(handle: Handle) -> Router { 61 | Router::new() 62 | .route("/set_joint_positions", post(set_joint_positions)) 63 | .route("/set_robot_origin", post(set_robot_origin)) 64 | .route("/set_reload_request", post(set_reload_request)) 65 | .route("/get_joint_positions", get(get_joint_positions)) 66 | .route("/get_robot_origin", get(get_robot_origin)) 67 | .route("/get_urdf_text", get(get_urdf_text)) 68 | .with_state(handle) 69 | .layer(tower_http::trace::TraceLayer::new_for_http()) 70 | } 71 | 72 | async fn set_reload_request(State(handle): State) -> Json { 73 | handle.set_reload_request(); 74 | Json(ResultResponse::SUCCESS) 75 | } 76 | 77 | async fn set_joint_positions( 78 | State(handle): State, 79 | Json(jp): Json, 80 | ) -> Json { 81 | if jp.names.len() != jp.positions.len() { 82 | Json(ResultResponse { 83 | is_ok: false, 84 | reason: format!( 85 | "names and positions size mismatch ({} != {})", 86 | jp.names.len(), 87 | jp.positions.len() 88 | ), 89 | }) 90 | } else { 91 | handle.set_target_joint_positions(jp); 92 | Json(ResultResponse::SUCCESS) 93 | } 94 | } 95 | 96 | async fn set_robot_origin( 97 | State(handle): State, 98 | Json(robot_origin): Json, 99 | ) -> Json { 100 | handle.set_target_robot_origin(robot_origin); 101 | Json(ResultResponse::SUCCESS) 102 | } 103 | 104 | async fn get_joint_positions(State(handle): State) -> Json { 105 | Json(handle.current_joint_positions().clone()) 106 | } 107 | 108 | async fn get_robot_origin(State(handle): State) -> Json { 109 | Json(handle.current_robot_origin().clone()) 110 | } 111 | 112 | async fn get_urdf_text(State(handle): State) -> Result { 113 | match handle.urdf_text() { 114 | Some(text) => Ok(text.clone()), 115 | None => Err(StatusCode::NOT_FOUND), 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /tools/spell-check.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # shellcheck disable=SC2046 3 | set -euo pipefail 4 | IFS=$'\n\t' 5 | cd "$(dirname "$0")"/.. 6 | 7 | # Usage: 8 | # ./tools/spell-check.sh 9 | 10 | check_diff() { 11 | if [[ -n "${CI:-}" ]]; then 12 | if ! git --no-pager diff --exit-code "$@"; then 13 | should_fail=1 14 | fi 15 | else 16 | if ! git --no-pager diff --exit-code "$@" &>/dev/null; then 17 | should_fail=1 18 | fi 19 | fi 20 | } 21 | error() { 22 | if [[ -n "${GITHUB_ACTIONS:-}" ]]; then 23 | echo "::error::$*" 24 | else 25 | echo >&2 "error: $*" 26 | fi 27 | should_fail=1 28 | } 29 | warn() { 30 | if [[ -n "${GITHUB_ACTIONS:-}" ]]; then 31 | echo "::warning::$*" 32 | else 33 | echo >&2 "warning: $*" 34 | fi 35 | } 36 | 37 | project_dictionary=.github/.cspell/project-dictionary.txt 38 | has_rust='' 39 | if [[ -n "$(git ls-files '*Cargo.toml')" ]]; then 40 | has_rust='1' 41 | dependencies='' 42 | for manifest_path in $(git ls-files '*Cargo.toml'); do 43 | if [[ "${manifest_path}" != "Cargo.toml" ]] && ! grep -Eq '\[workspace\]' "${manifest_path}"; then 44 | continue 45 | fi 46 | metadata=$(cargo metadata --format-version=1 --all-features --no-deps --manifest-path "${manifest_path}") 47 | for id in $(jq <<<"${metadata}" '.workspace_members[]'); do 48 | dependencies+="$(jq <<<"${metadata}" ".packages[] | select(.id == ${id})" | jq -r '.dependencies[].name')"$'\n' 49 | done 50 | done 51 | # shellcheck disable=SC2001 52 | dependencies=$(sed <<<"${dependencies}" 's/[0-9_-]/\n/g' | LC_ALL=C sort -f -u) 53 | fi 54 | config_old=$(<.cspell.json) 55 | config_new=$(grep <<<"${config_old}" -v '^ *//' | jq 'del(.dictionaries[])' | jq 'del(.dictionaryDefinitions[])') 56 | trap -- 'echo "${config_old}" >.cspell.json; echo >&2 "$0: trapped SIGINT"; exit 1' SIGINT 57 | echo "${config_new}" >.cspell.json 58 | if [[ -n "${has_rust}" ]]; then 59 | dependencies_words=$(npx <<<"${dependencies}" -y cspell stdin --no-progress --no-summary --words-only --unique || true) 60 | fi 61 | all_words=$(npx -y cspell --no-progress --no-summary --words-only --unique $(git ls-files | (grep -v "${project_dictionary//\./\\.}" || true)) || true) 62 | echo "${config_old}" >.cspell.json 63 | trap - SIGINT 64 | cat >.github/.cspell/rust-dependencies.txt <>.github/.cspell/rust-dependencies.txt 70 | fi 71 | if [[ -z "${REMOVE_UNUSED_WORDS:-}" ]]; then 72 | check_diff .github/.cspell/rust-dependencies.txt 73 | fi 74 | 75 | echo "+ npx -y cspell --no-progress --no-summary \$(git ls-files)" 76 | if ! npx -y cspell --no-progress --no-summary $(git ls-files); then 77 | error "spellcheck failed: please fix uses of below words or add to ${project_dictionary} if correct" 78 | echo >&2 "=======================================" 79 | (npx -y cspell --no-progress --no-summary --words-only $(git ls-files) || true) | LC_ALL=C sort -f -u >&2 80 | echo >&2 "=======================================" 81 | echo >&2 82 | fi 83 | 84 | # Make sure the project-specific dictionary does not contain duplicated words. 85 | for dictionary in .github/.cspell/*.txt; do 86 | if [[ "${dictionary}" == "${project_dictionary}" ]]; then 87 | continue 88 | fi 89 | dup=$(sed '/^$/d' "${project_dictionary}" "${dictionary}" | LC_ALL=C sort -f | uniq -d -i | (grep -v '//.*' || true)) 90 | if [[ -n "${dup}" ]]; then 91 | error "duplicated words in dictionaries; please remove the following words from ${project_dictionary}" 92 | echo >&2 "=======================================" 93 | echo >&2 "${dup}" 94 | echo >&2 "=======================================" 95 | echo >&2 96 | fi 97 | done 98 | 99 | # Make sure the project-specific dictionary does not contain unused words. 100 | if [[ -n "${REMOVE_UNUSED_WORDS:-}" ]]; then 101 | grep_args=() 102 | for word in $(grep -v '//.*' "${project_dictionary}" || true); do 103 | if ! grep <<<"${all_words}" -Eq -i "^${word}$"; then 104 | # TODO: single pattern with ERE: ^(word1|word2..)$ 105 | grep_args+=(-e "^${word}$") 106 | fi 107 | done 108 | if [[ ${#grep_args[@]} -gt 0 ]]; then 109 | warn "removing unused words from ${project_dictionary}" 110 | res=$(grep -v "${grep_args[@]}" "${project_dictionary}") 111 | echo "${res}" >"${project_dictionary}" 112 | fi 113 | else 114 | unused='' 115 | for word in $(grep -v '//.*' "${project_dictionary}" || true); do 116 | if ! grep <<<"${all_words}" -Eq -i "^${word}$"; then 117 | unused+="${word}"$'\n' 118 | fi 119 | done 120 | if [[ -n "${unused}" ]]; then 121 | warn "unused words in dictionaries; please remove the following words from ${project_dictionary}" 122 | echo >&2 "=======================================" 123 | echo >&2 -n "${unused}" 124 | echo >&2 "=======================================" 125 | echo >&2 126 | fi 127 | fi 128 | 129 | if [[ -n "${should_fail:-}" ]]; then 130 | exit 1 131 | fi 132 | --------------------------------------------------------------------------------