release repository

.clangd

@@ -0,0 +1,11 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+ColumnLimit: 100
+IncludeBlocks: Preserve
+DerivePointerAlignment: false
+PointerAlignment: Right
+---
+Language: Proto
+BasedOnStyle: Google
+...

.gitattributes

@@ -0,0 +1,19 @@
+src/curobo/content/assets/debug** filter=lfs diff=lfs merge=lfs -text
+*.obj filter=lfs diff=lfs merge=lfs -text
+*.stl filter=lfs diff=lfs merge=lfs -text
+*.dae filter=lfs diff=lfs merge=lfs -text
+*.pt filter=lfs diff=lfs merge=lfs -text
+*.mtl filter=lfs diff=lfs merge=lfs -text
+*.nvblx filter=lfs diff=lfs merge=lfs -text
+*.ply filter=lfs diff=lfs merge=lfs -text
+*.png filter=lfs diff=lfs merge=lfs -text
+*.gif filter=lfs diff=lfs merge=lfs -text
+*.svg filter=lfs diff=lfs merge=lfs -text
+*.usd filter=lfs diff=lfs merge=lfs -text
+*.usda filter=lfs diff=lfs merge=lfs -text
+*.mp4 filter=lfs diff=lfs merge=lfs -text
+*.webm filter=lfs diff=lfs merge=lfs -text
+docs/_static/nvidia_logo.png filter=lfs diff=lfs merge=lfs -text
+docs/images/motion_opt.png filter=lfs diff=lfs merge=lfs -text
+src/curobo/content/assets/urdf/nvblox/out4_color.png filter=lfs diff=lfs merge=lfs -text
+docs/_html_extra/* filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,214 @@
+.vscode*
+*.hdf5
+*data/
+*.bat
+*.p
+*.mlx
+
+*#
+*logs
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+*.json
+
+# C extensions
+*.so
+requirements.txt
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+docs/_source/
+_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+.#*
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+experiments/
+
+# torch pickles:
+*.t
+*.npy
+*.zip
+*.qdrep
+*.sql*
+*.npz
+/examples/*.stl
+
+*Flymake log*
+#*.pt
+*.pth*
+*.tar
+examples/*.pt
+
+curobo.*
+docs/modules.rst
+docs/setup.rst
+docs/install_instructions.rst
+docs/_api/*
+
+# VIM
+*swp
+*swo
+
+examples/debug/*.onnx
+*.engine
+*.profile
+*.cache
+*.flatbuffers
+#*.obj
+*.glb
+*.onnx
+*stltoobj.bat
+external/*
+public/*
+docs/doxygen/html/*
+docs/doxygen/xml/*
+
+
+docker/pkgs/*
+
+
+nvidia_curobo*/
+
+*.code-workspace
+*.usda
+*.pdf
+#*.png
+*.usd
+*.obj
+*.STL
+*.dae
+*.stl
+*.mtl
+*.SLDPRT
+
+*tfevents*
+
+*.csv

CHANGELOG.md

@@ -0,0 +1,15 @@
+<!--
+Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+
+NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+property and proprietary rights in and to this material, related
+documentation and any modifications thereto. Any use, reproduction,
+disclosure or distribution of this material and related documentation
+without an express license agreement from NVIDIA CORPORATION or
+its affiliates is strictly prohibited.
+-->
+# Changelog
+
+## Version 0.0.0
+
+- First version of CuRobo.

CODEOWNERS

@@ -0,0 +1,16 @@
+# Copyright (c) 2023, NVIDIA CORPORATION & AFFILIATES.  All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto.  Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+
+#######################################################
+# KEEP ITEMS IN ALPHABETICAL ORDER WITHIN THEIR GROUP #
+#######################################################
+# See this page for information about this file
+# https://docs.gitlab.com/ee/user/project/code_owners.html
+
+# Default
+NVIDIA

LICENSE

@@ -0,0 +1,77 @@
+Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES.  All rights reserved.
+
+
+NVIDIA License
+
+1. Definitions
+
+“Licensor” means any person or entity that distributes its Work.
+“Work” means (a) the original work of authorship made available under this license, which may 
+include software, documentation, or other files, and (b) any additions to or derivative works  
+thereof  that are made available under this license.
+The terms “reproduce,” “reproduction,” “derivative works,” and “distribution” have the meaning as 
+provided under U.S. copyright law; provided, however, that for the purposes of this license, 
+derivative works shall not include works that remain separable from, or merely link (or bind by 
+name) to the interfaces of, the Work.
+
+Works are “made available” under this license by including in or with the Work either (a) a 
+copyright notice referencing the applicability of this license to the Work, or (b) a copy of 
+this license.
+
+2. License Grant
+
+2.1 Copyright Grant. Subject to the terms and conditions of this license, each Licensor grants to 
+you a perpetual, worldwide, non-exclusive, royalty-free, copyright license to use, reproduce, 
+prepare derivative works of, publicly display, publicly perform, sublicense and distribute its Work
+and any resulting derivative works in any form.
+
+3. Limitations
+
+3.1 Redistribution. You may reproduce or distribute the Work only if (a) you do so under this 
+license, (b) you include a complete copy of this license with your distribution, and (c) you retain 
+without modification any copyright, patent, trademark, or attribution notices that are present in 
+the Work.
+
+3.2 Derivative Works. You may specify that additional or different terms apply to the use, 
+reproduction, and distribution of your derivative works of the Work (“Your Terms”) only if (a) 
+Your Terms provide that the use limitation in Section 3.3 applies to your derivative works, and (b) 
+you identify the specific derivative works that are subject to Your Terms. Notwithstanding Your 
+Terms, this license (including the redistribution requirements in Section 3.1) will continue to 
+apply to the Work itself.
+
+3.3 Use Limitation. The Work and any derivative works thereof only may be used or intended for 
+use non-commercially. Notwithstanding the foregoing, NVIDIA Corporation and its affiliates may use 
+the Work and any derivative works commercially. As used herein, “non-commercially” means for 
+research or evaluation purposes only.
+
+3.4 Patent Claims. If you bring or threaten to bring a patent claim against any Licensor (including
+any claim, cross-claim or counterclaim in a lawsuit) to enforce any patents that you allege are 
+infringed by any Work, then your rights under this license from such Licensor (including the grant 
+in Section 2.1) will terminate immediately.
+
+3.5 Trademarks. This license does not grant any rights to use any Licensor’s or its affiliates’ 
+names, logos, or trademarks, except as necessary to reproduce the notices described in this license.
+
+3.6 Termination. If you violate any term of this license, then your rights under this license 
+(including the grant in Section 2.1) will terminate immediately.
+
+4. Disclaimer of Warranty.
+
+THE WORK IS PROVIDED “AS IS” WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR 
+IMPLIED, INCLUDING WARRANTIES OR CONDITIONS OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, 
+TITLE OR NON-INFRINGEMENT. YOU BEAR THE RISK OF UNDERTAKING ANY ACTIVITIES UNDER THIS LICENSE. 
+
+5. Limitation of Liability.
+
+EXCEPT AS PROHIBITED BY APPLICABLE LAW, IN NO EVENT AND UNDER NO LEGAL THEORY, WHETHER IN TORT 
+(INCLUDING NEGLIGENCE), CONTRACT, OR OTHERWISE SHALL ANY LICENSOR BE LIABLE TO YOU FOR DAMAGES, 
+INCLUDING ANY DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR 
+RELATED TO THIS LICENSE, THE USE OR INABILITY TO USE THE WORK (INCLUDING BUT NOT LIMITED TO LOSS OF
+GOODWILL, BUSINESS INTERRUPTION, LOST PROFITS OR DATA, COMPUTER FAILURE OR MALFUNCTION, OR ANY OTHER
+DAMAGES OR LOSSES), EVEN IF THE LICENSOR HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+****************************************************************************************************
+
+This package contains examples that use external assets (urdf and mesh files). Licenses for external
+assets are listed in LICENSE_ASSETS.
+

LICENSE_ASSETS

@@ -0,0 +1,402 @@
+
+Licenses for external assets are listed here:
+
+franka_ros 0.7.0
+----------------
+
+The Franka urdf files in src/curobo/content/assets/robot/franka_description/ has minor modifications
+from those in the "franka_description" component of the franka_ros package.
+
+Site:    https://github.com/frankaemika/franka_ros/
+License: https://github.com/frankaemika/franka_ros/blob/0.7.0/NOTICE
+         https://github.com/frankaemika/franka_ros/blob/0.7.0/LICENSE
+
+Copyright 2017 Franka Emika GmbH
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+***********************
+The files in src/curobo/content/assets/robot/kinova were obtained from 
+https://github.com/Kinovarobotics/ros_kortex/tree/noetic-devel/kortex_description with minor 
+modifications to make the gripper fingers as fixed joints.
+
+The license for this is given below:
+
+Copyright (c) 2018, Kinova inc.
+All rights reserved.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+* Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+* Neither the name of the copyright holder nor the names of its contributors
+may be used to endorse or promote products derived from this software
+without specific prior written permission.
+ 
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+***********************
+
+The files in src/curobo/content/assets/robot/ur_description have been obtained from 
+https://github.com/UniversalRobots/Universal_Robots_ROS2_Description with BSD 3-Clause License
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+
+***********************
+The files in src/curobo/content/assets/robot/jaco were obtained from 
+https://github.com/Kinovarobotics/kinova-ros which has BSD 3-Clause License
+
+Copyright (c) 2017, Kinova Robotics inc.
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of the copyright holder nor the names of its contributors
+      may be used to endorse or promote products derived from this software
+      without specific prior written permission.
+      
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+***********************
+
+The urdfs in  src/curobo/content/assets/robot/iiwa_allegro_description were obtained by
+combining files from https://github.com/IFL-CAMP/iiwa_stack for the KUKA IIWA and 
+https://github.com/simlabrobotics/allegro_hand_ros_catkin for SimLab's Allegro Hand.
+
+Both have BSD 2-Cluase License as below.
+
+BSD 2-Clause License
+
+Copyright (c) 2016-2019, Salvatore Virga - salvo.virga@tum.de, Marco Esposito - marco.esposito@tum.de 
+Technische Universität München 
+Chair for Computer Aided Medical Procedures and Augmented Reality 
+Fakultät für Informatik / I16, Boltzmannstraße 3, 85748 Garching bei München, Germany 
+http://campar.in.tum.de
+All rights reserved.
+
+Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, Orebro University, Sweden
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+Copyright (c) 2016, SimLab
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+**********************************
+The files in src/curobo/content/assets/robot/techman were obtained from 
+https://github.com/TechmanRobotInc/tmr_ros1 which have BSD 3-Clause license.
+
+BSD 3-Clause License
+
+Copyright (c) 2019-2023, TECHMAN ROBOT INC.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors
+   may be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+***********************************

MANIFEST.in

@@ -0,0 +1,18 @@
+# Copyright (c) 2023, NVIDIA CORPORATION & AFFILIATES.  All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto.  Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+
+# This file list the additional files that should be include in the package distribution.
+#
+# References:
+# * https://packaging.python.org/guides/using-manifest-in/
+# * https://setuptools.readthedocs.io/en/latest/userguide/datafiles.html
+# * https://stackoverflow.com/questions/6028000/how-to-read-a-static-file-from-inside-a-python-package
+
+# graft <path to files or directories to add to the project>
+graft src/curobo/content
+global-include py.typed

README.md

@@ -0,0 +1,54 @@
+<!--
+Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+
+NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+property and proprietary rights in and to this material, related
+documentation and any modifications thereto. Any use, reproduction,
+disclosure or distribution of this material and related documentation
+without an express license agreement from NVIDIA CORPORATION or
+its affiliates is strictly prohibited.
+-->
+# CuRobo
+
+*CUDA Accelerated Robot Library*
+
+**Check [curobo.org](https://curobo.org) for installing and getting started with examples!**
+
+Use [Discussions](https://github.com/NVlabs/curobo/discussions) for questions on using this package.
+
+Use [Issues](https://github.com/NVlabs/curobo/issues) if you find a bug.
+
+
+For business inquiries, please visit our website and submit the form: [NVIDIA Research Licensing](https://www.nvidia.com/en-us/research/inquiries/)
+
+## Overview
+
+CuRobo is a CUDA accelerated library containing a suite of robotics algorithms that run significantly faster than existing implementations leveraging parallel compute. CuRobo currently provides the following algorithms: (1) forward and inverse kinematics,
+(2) collision checking between robot and world, with the world represented as Cuboids, Meshes, and Depth images, (3) numerical optimization with gradient descent, L-BFGS, and MPPI, (4) geometric planning, (5) trajectory optimization, (6) motion generation that combines inverse kinematics, geometric planning, and trajectory optimization to generate global motions within 30ms.
+
+<p align="center">
+<img width="500" src="images/robot_demo.gif">
+</p>
+
+
+CuRobo performs trajectory optimization across many seeds in parallel to find a solution. CuRobo's trajectory optimization penalizes jerk and accelerations, encouraging smoother and shorter trajectories. Below we compare CuRobo's motion generation on the left to a BiRRT planner on a pick and place task.
+
+<p align="center">
+<img width="500" src="images/rrt_compare.gif">
+</p>
+
+## Citation
+
+If you found this work useful, please cite the below report,
+
+```
+@article{curobo_report23,
+         title={CuRobo: Parallelized Collision-Free Minimum-Jerk Robot Motion Generation},
+         author={Sundaralingam, Balakumar and Hari, Siva Kumar Sastry and 
+         Fishman, Adam and Garrett, Caelan and Van Wyk, Karl and Blukis, Valts and 
+         Millane, Alexander and Oleynikova, Helen and Handa, Ankur and 
+         Ramos, Fabio and Ratliff, Nathan and Fox, Dieter},
+         journal={arXiv preprint},
+         year={2023}
+}
+```

benchmark/README.md

@@ -0,0 +1,13 @@
+<!--
+Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+
+NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+property and proprietary rights in and to this material, related
+documentation and any modifications thereto. Any use, reproduction,
+disclosure or distribution of this material and related documentation
+without an express license agreement from NVIDIA CORPORATION or
+its affiliates is strictly prohibited.
+-->
+This folder contains scripts to run the motion planning benchmarks.
+
+Refer to Benchmarks & Profiling instructions in documentation for more information.

benchmark/curobo_benchmark.py

@@ -0,0 +1,727 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+from copy import deepcopy
+from typing import Optional
+
+# Third Party
+import numpy as np
+import torch
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType, WorldConfig
+from curobo.geom.types import Mesh
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import RobotConfig
+from curobo.types.state import JointState
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import (
+    get_assets_path,
+    get_robot_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+    write_yaml,
+)
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+# torch.set_num_threads(8)
+# torch.use_deterministic_algorithms(True)
+torch.manual_seed(0)
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+# torch.backends.cuda.matmul.allow_tf32 = False
+# torch.backends.cudnn.allow_tf32 = False
+
+# torch.backends.cuda.matmul.allow_fp16_reduced_precision_reduction = True
+np.random.seed(0)
+# Standard Library
+import argparse
+import warnings
+from typing import List, Optional
+
+# Third Party
+from metrics import CuroboGroupMetrics, CuroboMetrics
+from robometrics.datasets import demo_raw, motion_benchmaker_raw, mpinets_raw
+
+
+def plot_cost_iteration(cost: torch.Tensor, save_path="cost", title="", log_scale=False):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    fig = plt.figure(figsize=(5, 4))
+    cost = cost.cpu().numpy()
+    # save to csv:
+    np.savetxt(save_path + ".csv", cost, delimiter=",")
+
+    # if cost.shape[0] > 1:
+    colormap = plt.cm.winter
+    plt.gca().set_prop_cycle(plt.cycler("color", colormap(np.linspace(0, 1, cost.shape[0]))))
+    x = [i for i in range(cost.shape[-1])]
+    for i in range(cost.shape[0]):
+        plt.plot(x, cost[i], label="seed_" + str(i))
+    plt.tight_layout()
+    # plt.title(title)
+    plt.xlabel("iteration")
+    plt.ylabel("cost")
+    if log_scale:
+        plt.yscale("log")
+    plt.grid()
+    # plt.legend()
+    plt.tight_layout()
+    plt.savefig(save_path + ".pdf")
+    plt.close()
+
+
+def plot_traj(act_seq: JointState, dt=0.25, title="", save_path="plot.png", sma_filter=False):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    fig, ax = plt.subplots(4, 1, figsize=(5, 8), sharex=True)
+    t_steps = np.linspace(0, act_seq.position.shape[0] * dt, act_seq.position.shape[0])
+    # compute acceleration from finite difference of velocity:
+    # act_seq.acceleration = (torch.roll(act_seq.velocity, -1, 0) - act_seq.velocity) / dt
+    # act_seq.acceleration = ( act_seq.velocity - torch.roll(act_seq.velocity, 1, 0)) / dt
+    # act_seq.acceleration[0,:] = 0.0
+    # act_seq.jerk = ( act_seq.acceleration - torch.roll(act_seq.acceleration, 1, 0)) / dt
+    # act_seq.jerk[0,:] = 0.0
+    if sma_filter:
+        kernel = 5
+        sma = torch.nn.AvgPool1d(kernel_size=kernel, stride=1, padding=2, ceil_mode=False).cuda()
+    # act_seq.jerk = sma(act_seq.jerk)
+    # act_seq.acceleration[-1,:] = 0.0
+    for i in range(act_seq.position.shape[-1]):
+        ax[0].plot(t_steps, act_seq.position[:, i].cpu(), "-", label=str(i))
+        # act_seq.velocity[1:-1, i] = sma(act_seq.velocity[:,i].view(1,-1)).squeeze()#@[1:-2]
+
+        ax[1].plot(t_steps[: act_seq.velocity.shape[0]], act_seq.velocity[:, i].cpu(), "-")
+        if sma_filter:
+            act_seq.acceleration[:, i] = sma(
+                act_seq.acceleration[:, i].view(1, -1)
+            ).squeeze()  # @[1:-2]
+
+        ax[2].plot(t_steps[: act_seq.acceleration.shape[0]], act_seq.acceleration[:, i].cpu(), "-")
+        if sma_filter:
+            act_seq.jerk[:, i] = sma(act_seq.jerk[:, i].view(1, -1)).squeeze()  # @[1:-2]\
+
+        ax[3].plot(t_steps[: act_seq.jerk.shape[0]], act_seq.jerk[:, i].cpu(), "-")
+    ax[0].set_title(title + " dt=" + "{:.3f}".format(dt))
+    ax[3].set_xlabel("Time(s)")
+    ax[3].set_ylabel("Jerk rad. s$^{-3}$")
+    ax[0].set_ylabel("Position rad.")
+    ax[1].set_ylabel("Velocity rad. s$^{-1}$")
+    ax[2].set_ylabel("Acceleration rad. s$^{-2}$")
+    ax[0].grid()
+    ax[1].grid()
+    ax[2].grid()
+    ax[3].grid()
+    # ax[0].legend(loc="upper right")
+    ax[0].legend(bbox_to_anchor=(0.5, 1.6), loc="upper center", ncol=4)
+    plt.tight_layout()
+    plt.savefig(save_path)
+    plt.close()
+    # plt.legend()
+
+
+def load_curobo(
+    n_cubes: int,
+    enable_debug: bool = False,
+    tsteps: int = 30,
+    trajopt_seeds: int = 4,
+    mpinets: bool = False,
+    graph_mode: bool = True,
+    mesh_mode: bool = False,
+    cuda_graph: bool = True,
+    collision_buffer: float = -0.01,
+):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = collision_buffer
+    robot_cfg["kinematics"]["collision_spheres"] = "spheres/franka_mesh.yml"
+    robot_cfg["kinematics"]["collision_link_names"].remove("attached_object")
+
+    # del robot_cfg["kinematics"]
+
+    ik_seeds = 30  # 500
+    if graph_mode:
+        trajopt_seeds = 4
+    if trajopt_seeds >= 14:
+        ik_seeds = max(100, trajopt_seeds * 4)
+    if mpinets:
+        robot_cfg["kinematics"]["lock_joints"] = {
+            "panda_finger_joint1": 0.025,
+            "panda_finger_joint2": -0.025,
+        }
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_obb_world()
+    interpolation_steps = 2000
+    c_checker = CollisionCheckerType.PRIMITIVE
+    c_cache = {"obb": n_cubes}
+    if mesh_mode:
+        c_checker = CollisionCheckerType.MESH
+        c_cache = {"mesh": n_cubes}
+        world_cfg = world_cfg.get_mesh_world()
+    if graph_mode:
+        interpolation_steps = 100
+
+    robot_cfg_instance = RobotConfig.from_dict(robot_cfg, tensor_args=TensorDeviceType())
+
+    K = robot_cfg_instance.kinematics.kinematics_config.joint_limits
+    K.position[0, :] -= 0.1
+    K.position[1, :] += 0.1
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg_instance,
+        world_cfg,
+        trajopt_tsteps=tsteps,
+        collision_checker_type=c_checker,
+        use_cuda_graph=cuda_graph,
+        collision_cache=c_cache,
+        position_threshold=0.005,  # 5 mm
+        rotation_threshold=0.05,
+        num_ik_seeds=ik_seeds,
+        num_graph_seeds=trajopt_seeds,
+        num_trajopt_seeds=trajopt_seeds,
+        interpolation_dt=0.025,
+        store_ik_debug=enable_debug,
+        store_trajopt_debug=enable_debug,
+        interpolation_steps=interpolation_steps,
+        collision_activation_distance=0.03,
+        trajopt_dt=0.25,
+        finetune_dt_scale=1.05,  # 1.05,
+        maximum_trajectory_dt=0.1,
+    )
+    mg = MotionGen(motion_gen_config)
+    mg.warmup(enable_graph=True, warmup_js_trajopt=False)
+    return mg, robot_cfg
+
+
+def benchmark_mb(
+    write_usd=False,
+    save_log=False,
+    write_plot=False,
+    write_benchmark=False,
+    plot_cost=False,
+    override_tsteps: Optional[int] = None,
+    save_kpi=False,
+    graph_mode=False,
+    args=None,
+):
+    # load dataset:
+
+    interpolation_dt = 0.02
+    # mpinets_data = True
+    # if mpinets_data:
+    file_paths = [motion_benchmaker_raw, mpinets_raw][:]
+    if args.demo:
+        file_paths = [demo_raw]
+
+    # else:22
+    #    file_paths = [get_mb_dataset_path()][:1]
+    enable_debug = save_log or plot_cost
+    all_files = []
+    og_tsteps = 32
+    if override_tsteps is not None:
+        og_tsteps = override_tsteps
+
+    og_trajopt_seeds = 12
+    for file_path in file_paths:
+        all_groups = []
+        mpinets_data = False
+        problems = file_path()
+        if "dresser_task_oriented" in list(problems.keys()):
+            mpinets_data = True
+        for key, v in tqdm(problems.items()):
+            tsteps = og_tsteps
+            trajopt_seeds = og_trajopt_seeds
+
+            if "cage_panda" in key:
+                trajopt_seeds = 16
+            # else:
+            #    continue
+            if "table_under_pick_panda" in key:
+                tsteps = 44
+                trajopt_seeds = 28
+
+            if "cubby_task_oriented" in key and "merged" not in key:
+                trajopt_seeds = 16
+            if "dresser_task_oriented" in key:
+                trajopt_seeds = 16
+            scene_problems = problems[key]  # [:4]  # [:1]  # [:20]  # [0:10]
+            n_cubes = check_problems(scene_problems)
+            # torch.cuda.empty_cache()
+            mg, robot_cfg = load_curobo(
+                n_cubes,
+                enable_debug,
+                tsteps,
+                trajopt_seeds,
+                mpinets_data,
+                graph_mode,
+                args.mesh,
+                not args.disable_cuda_graph,
+                collision_buffer=args.collision_buffer,
+            )
+            m_list = []
+            i = 0
+            ik_fail = 0
+            for problem in tqdm(scene_problems, leave=False):
+                i += 1
+                if problem["collision_buffer_ik"] < 0.0:
+                    #    print("collision_ik:", problem["collision_buffer_ik"])
+                    continue
+                # if i != 269: # 226
+                #    continue
+
+                plan_config = MotionGenPlanConfig(
+                    max_attempts=100,  # 00,  # 00,  # 100,  # 00,  # 000,#,00,#00,  # 5000,
+                    enable_graph_attempt=3,
+                    enable_finetune_trajopt=True,
+                    partial_ik_opt=False,
+                    enable_graph=graph_mode,
+                    timeout=60,
+                    enable_opt=not graph_mode,
+                )
+                # if "table_under_pick_panda" in key:
+                #    plan_config.enable_graph = True
+                #    plan_config.partial_ik_opt = False
+                q_start = problem["start"]
+                pose = (
+                    problem["goal_pose"]["position_xyz"] + problem["goal_pose"]["quaternion_wxyz"]
+                )
+                problem_name = "d_" + key + "_" + str(i)
+
+                # reset planner
+                mg.reset(reset_seed=False)
+                if args.mesh:
+                    world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_mesh_world()
+                else:
+                    world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_obb_world()
+                mg.world_coll_checker.clear_cache()
+                mg.update_world(world)
+                # continue
+                # load obstacles
+
+                # run planner
+                start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
+                goal_pose = Pose.from_list(pose)
+
+                result = mg.plan_single(
+                    start_state,
+                    goal_pose,
+                    plan_config,
+                )
+                if result.status == "IK Fail":
+                    ik_fail += 1
+                # rint(plan_config.enable_graph, plan_config.enable_graph_attempt)
+                problem["solution"] = None
+                if plan_config.enable_finetune_trajopt:
+                    problem_name = key + "_" + str(i)
+                else:
+                    problem_name = "noft_" + key + "_" + str(i)
+                problem_name = "nosw_" + problem_name
+                if write_usd or save_log:
+                    # CuRobo
+                    from curobo.util.usd_helper import UsdHelper
+
+                    world.randomize_color(r=[0.5, 0.9], g=[0.2, 0.5], b=[0.0, 0.2])
+
+                    gripper_mesh = Mesh(
+                        name="target_gripper",
+                        file_path=join_path(
+                            get_assets_path(),
+                            "robot/franka_description/meshes/visual/hand.dae",
+                        ),
+                        color=[0.0, 0.8, 0.1, 1.0],
+                        pose=pose,
+                    )
+                    world.add_obstacle(gripper_mesh)
+                # get costs:
+                if plot_cost:
+                    dt = 0.5
+                    problem_name = "approx_wolfe_p" + problem_name
+                    if result.optimized_dt is not None:
+                        dt = result.optimized_dt.item()
+                    if "trajopt_result" in result.debug_info:
+                        success = result.success.item()
+                        traj_cost = (
+                            # result.debug_info["trajopt_result"].debug_info["solver"]["cost"][0]
+                            result.debug_info["trajopt_result"].debug_info["solver"]["cost"][-1]
+                        )
+                        # print(traj_cost[0])
+                        traj_cost = torch.cat(traj_cost, dim=-1)
+                        plot_cost_iteration(
+                            traj_cost,
+                            title=problem_name + "_" + str(success) + "_" + str(dt),
+                            save_path=join_path("log/plot/", problem_name + "_cost"),
+                            log_scale=False,
+                        )
+                        if "finetune_trajopt_result" in result.debug_info:
+                            traj_cost = result.debug_info["finetune_trajopt_result"].debug_info[
+                                "solver"
+                            ]["cost"][0]
+                            traj_cost = torch.cat(traj_cost, dim=-1)
+                            plot_cost_iteration(
+                                traj_cost,
+                                title=problem_name + "_" + str(success) + "_" + str(dt),
+                                save_path=join_path("log/plot/", problem_name + "_ft_cost"),
+                                log_scale=False,
+                            )
+                if result.success.item():
+                    # print("GT: ", result.graph_time)
+                    q_traj = result.get_interpolated_plan()
+                    problem["goal_ik"] = q_traj.position.cpu().squeeze().numpy()[-1, :].tolist()
+                    problem["solution"] = {
+                        "position": result.get_interpolated_plan()
+                        .position.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "velocity": result.get_interpolated_plan()
+                        .velocity.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "acceleration": result.get_interpolated_plan()
+                        .acceleration.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "jerk": result.get_interpolated_plan()
+                        .jerk.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "dt": interpolation_dt,
+                    }
+                    # print(problem["solution"]["position"])
+                    # exit()
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "opt_traj": {
+                            "position": result.optimized_plan.position.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "velocity": result.optimized_plan.velocity.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "acceleration": result.optimized_plan.acceleration.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "jerk": result.optimized_plan.jerk.cpu().squeeze().numpy().tolist(),
+                            "dt": result.optimized_dt.item(),
+                        },
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+                    # print(
+                    #    "T: ",
+                    #    result.motion_time.item(),
+                    #    result.optimized_dt.item(),
+                    #    (len(problem["solution"]["position"]) - 1 ) * result.interpolation_dt,
+                    #    result.interpolation_dt,
+                    #    )
+                    # exit()
+                    reached_pose = mg.compute_kinematics(result.optimized_plan[-1]).ee_pose
+                    rot_error = goal_pose.angular_distance(reached_pose) * 100.0
+                    if args.graph:
+                        solve_time = result.graph_time
+                    else:
+                        solve_time = result.solve_time
+                    current_metrics = CuroboMetrics(
+                        skip=False,
+                        success=True,
+                        time=result.total_time,
+                        collision=False,
+                        joint_limit_violation=False,
+                        self_collision=False,
+                        position_error=result.position_error.item() * 1000.0,
+                        orientation_error=rot_error.item(),
+                        eef_position_path_length=10,
+                        eef_orientation_path_length=10,
+                        attempts=result.attempts,
+                        motion_time=result.motion_time.item(),
+                        solve_time=solve_time,
+                    )
+
+                    if write_usd:
+                        # CuRobo
+
+                        q_traj = result.get_interpolated_plan()
+                        UsdHelper.write_trajectory_animation_with_robot_usd(
+                            robot_cfg,
+                            world,
+                            start_state,
+                            q_traj,
+                            dt=result.interpolation_dt,
+                            save_path=join_path("log/usd/", problem_name) + ".usd",
+                            interpolation_steps=1,
+                            write_robot_usd_path="log/usd/assets/",
+                            robot_usd_local_reference="assets/",
+                            base_frame="/world_" + problem_name,
+                            visualize_robot_spheres=True,
+                        )
+
+                    if write_plot:
+                        problem_name = problem_name
+                        plot_traj(
+                            result.optimized_plan,
+                            result.optimized_dt.item(),
+                            # result.get_interpolated_plan(),
+                            # result.interpolation_dt,
+                            title=problem_name,
+                            save_path=join_path("log/plot/", problem_name + ".pdf"),
+                        )
+                        plot_traj(
+                            # result.optimized_plan,
+                            # result.optimized_dt.item(),
+                            result.get_interpolated_plan(),
+                            result.interpolation_dt,
+                            title=problem_name,
+                            save_path=join_path("log/plot/", problem_name + "_int.pdf"),
+                        )
+                        # exit()
+
+                    m_list.append(current_metrics)
+                    all_groups.append(current_metrics)
+                elif result.valid_query:
+                    # print("fail")
+                    # print(result.status)
+                    current_metrics = CuroboMetrics()
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "status": result.status,
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+
+                    m_list.append(current_metrics)
+                    all_groups.append(current_metrics)
+                else:
+                    # print("invalid: " + problem_name)
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "status": result.status,
+                        "valid_query": result.valid_query,
+                    }
+
+                    problem["solution_debug"] = debug
+                    if False:
+                        world.save_world_as_mesh(problem_name + ".obj")
+
+                        q_traj = start_state  # .unsqueeze(0)
+                        # CuRobo
+                        from curobo.util.usd_helper import UsdHelper
+
+                        UsdHelper.write_trajectory_animation_with_robot_usd(
+                            robot_cfg,
+                            world,
+                            start_state,
+                            q_traj,
+                            dt=result.interpolation_dt,
+                            save_path=join_path("log/usd/", problem_name) + ".usd",
+                            interpolation_steps=1,
+                            write_robot_usd_path="log/usd/assets/",
+                            robot_usd_local_reference="assets/",
+                            base_frame="/world_" + problem_name,
+                            visualize_robot_spheres=True,
+                        )
+                if save_log:  # and not result.success.item():
+                    # print("save log")
+                    UsdHelper.write_motion_gen_log(
+                        result,
+                        robot_cfg,
+                        world,
+                        start_state,
+                        Pose.from_list(pose),
+                        join_path("log/usd/", problem_name) + "_debug",
+                        write_ik=False,
+                        write_trajopt=True,
+                        visualize_robot_spheres=False,
+                        grid_space=2,
+                    )
+                # exit()
+
+            g_m = CuroboGroupMetrics.from_list(m_list)
+            if not args.kpi:
+                print(
+                    key,
+                    f"{g_m.success:2.2f}",
+                    # g_m.motion_time,
+                    g_m.time.mean,
+                    # g_m.time.percent_75,
+                    g_m.time.percent_98,
+                    g_m.position_error.percent_98,
+                    # g_m.position_error.median,
+                    g_m.orientation_error.percent_98,
+                    # g_m.orientation_error.median,
+                )  # , g_m.attempts)
+                print(g_m.attempts)
+            # print("MT: ", g_m.motion_time)
+            # $print(ik_fail)
+            # exit()
+            # print(g_m.position_error, g_m.orientation_error)
+
+        g_m = CuroboGroupMetrics.from_list(all_groups)
+        if not args.kpi:
+            print(
+                "All: ",
+                f"{g_m.success:2.2f}",
+                g_m.motion_time.percent_98,
+                g_m.time.mean,
+                g_m.time.percent_75,
+                g_m.position_error.percent_75,
+                g_m.orientation_error.percent_75,
+            )  # g_m.time, g_m.attempts)
+        # print("MT: ", g_m.motion_time)
+
+        # print(g_m.position_error, g_m.orientation_error)
+
+        # exit()
+        if write_benchmark:
+            if not mpinets_data:
+                write_yaml(problems, args.file_name + "_mb_solution.yaml")
+            else:
+                write_yaml(problems, args.file_name + "_mpinets_solution.yaml")
+        all_files += all_groups
+    g_m = CuroboGroupMetrics.from_list(all_files)
+    # print(g_m.success, g_m.time, g_m.attempts, g_m.position_error, g_m.orientation_error)
+    print("######## FULL SET ############")
+    print("All: ", f"{g_m.success:2.2f}")
+    print("MT: ", g_m.motion_time)
+    print("PT:", g_m.time)
+    print("ST: ", g_m.solve_time)
+    print("position error (mm): ", g_m.position_error)
+    print("orientation error(%): ", g_m.orientation_error)
+
+    if args.kpi:
+        kpi_data = {
+            "Success": g_m.success,
+            "Planning Time Mean": float(g_m.time.mean),
+            "Planning Time Std": float(g_m.time.std),
+            "Planning Time Median": float(g_m.time.median),
+            "Planning Time 75th": float(g_m.time.percent_75),
+            "Planning Time 98th": float(g_m.time.percent_98),
+        }
+        write_yaml(kpi_data, join_path(args.save_path, args.file_name + ".yml"))
+
+    # run on mb dataset:
+
+
+def check_problems(all_problems):
+    n_cube = 0
+    for problem in all_problems:
+        cache = (
+            WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_obb_world().get_cache_dict()
+        )
+        n_cube = max(n_cube, cache["obb"])
+    return n_cube
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--save_path",
+        type=str,
+        default=".",
+        help="path to save file",
+    )
+    parser.add_argument(
+        "--file_name",
+        type=str,
+        default="mg_curobo_",
+        help="File name prefix to use to save benchmark results",
+    )
+    parser.add_argument(
+        "--collision_buffer",
+        type=float,
+        default=-0.00,  # in meters
+        help="Robot collision buffer",
+    )
+
+    parser.add_argument(
+        "--graph",
+        action="store_true",
+        help="When True, runs only geometric planner",
+        default=False,
+    )
+    parser.add_argument(
+        "--mesh",
+        action="store_true",
+        help="When True, converts obstacles to meshes",
+        default=False,
+    )
+    parser.add_argument(
+        "--kpi",
+        action="store_true",
+        help="When True, saves minimal metrics",
+        default=False,
+    )
+
+    parser.add_argument(
+        "--demo",
+        action="store_true",
+        help="When True, runs only on small dataaset",
+        default=False,
+    )
+    parser.add_argument(
+        "--disable_cuda_graph",
+        action="store_true",
+        help="When True, disable cuda graph during benchmarking",
+        default=False,
+    )
+    parser.add_argument(
+        "--write_benchmark",
+        action="store_true",
+        help="When True, writes paths to file",
+        default=False,
+    )
+
+    args = parser.parse_args()
+
+    setup_curobo_logger("error")
+    benchmark_mb(
+        save_log=False,
+        write_usd=False,
+        write_plot=False,
+        write_benchmark=args.write_benchmark,
+        plot_cost=False,
+        save_kpi=args.kpi,
+        graph_mode=args.graph,
+        args=args,
+    )

benchmark/curobo_nvblox_benchmark.py

@@ -0,0 +1,558 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+import argparse
+from copy import deepcopy
+from typing import Optional
+
+# Third Party
+import matplotlib.pyplot as plt
+import numpy as np
+import torch
+from metrics import CuroboGroupMetrics, CuroboMetrics
+from nvblox_torch.datasets.mesh_dataset import MeshDataset
+from robometrics.datasets import demo_raw, motion_benchmaker_raw, mpinets_raw
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType, WorldConfig
+from curobo.geom.types import Mesh
+from curobo.types.base import TensorDeviceType
+from curobo.types.camera import CameraObservation
+from curobo.types.math import Pose
+from curobo.types.robot import RobotConfig
+from curobo.types.state import JointState
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import (
+    get_assets_path,
+    get_robot_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+    write_yaml,
+)
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+torch.manual_seed(0)
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+np.random.seed(0)
+
+
+def plot_cost_iteration(cost: torch.Tensor, save_path="cost", title="", log_scale=False):
+    fig = plt.figure(figsize=(5, 4))
+    cost = cost.cpu().numpy()
+    # save to csv:
+    np.savetxt(save_path + ".csv", cost, delimiter=",")
+
+    # if cost.shape[0] > 1:
+    colormap = plt.cm.winter
+    plt.gca().set_prop_cycle(plt.cycler("color", colormap(np.linspace(0, 1, cost.shape[0]))))
+    x = [i for i in range(cost.shape[-1])]
+    for i in range(cost.shape[0]):
+        plt.plot(x, cost[i], label="seed_" + str(i))
+    plt.tight_layout()
+    # plt.title(title)
+    plt.xlabel("iteration")
+    plt.ylabel("cost")
+    if log_scale:
+        plt.yscale("log")
+    plt.grid()
+    # plt.legend()
+    plt.tight_layout()
+    plt.savefig(save_path + ".pdf")
+    plt.close()
+
+
+def plot_traj(act_seq: JointState, dt=0.25, title="", save_path="plot.png", sma_filter=False):
+    fig, ax = plt.subplots(4, 1, figsize=(5, 8), sharex=True)
+    t_steps = np.linspace(0, act_seq.position.shape[0] * dt, act_seq.position.shape[0])
+
+    if sma_filter:
+        kernel = 5
+        sma = torch.nn.AvgPool1d(kernel_size=kernel, stride=1, padding=2, ceil_mode=False).cuda()
+
+    for i in range(act_seq.position.shape[-1]):
+        ax[0].plot(t_steps, act_seq.position[:, i].cpu(), "-", label=str(i))
+
+        ax[1].plot(t_steps[: act_seq.velocity.shape[0]], act_seq.velocity[:, i].cpu(), "-")
+        if sma_filter:
+            act_seq.acceleration[:, i] = sma(
+                act_seq.acceleration[:, i].view(1, -1)
+            ).squeeze()  # @[1:-2]
+
+        ax[2].plot(t_steps[: act_seq.acceleration.shape[0]], act_seq.acceleration[:, i].cpu(), "-")
+        if sma_filter:
+            act_seq.jerk[:, i] = sma(act_seq.jerk[:, i].view(1, -1)).squeeze()  # @[1:-2]\
+
+        ax[3].plot(t_steps[: act_seq.jerk.shape[0]], act_seq.jerk[:, i].cpu(), "-")
+    ax[0].set_title(title + " dt=" + "{:.3f}".format(dt))
+    ax[3].set_xlabel("Time(s)")
+    ax[3].set_ylabel("Jerk rad. s$^{-3}$")
+    ax[0].set_ylabel("Position rad.")
+    ax[1].set_ylabel("Velocity rad. s$^{-1}$")
+    ax[2].set_ylabel("Acceleration rad. s$^{-2}$")
+    ax[0].grid()
+    ax[1].grid()
+    ax[2].grid()
+    ax[3].grid()
+    ax[0].legend(bbox_to_anchor=(0.5, 1.6), loc="upper center", ncol=4)
+    plt.tight_layout()
+    plt.savefig(save_path)
+    plt.close()
+
+
+def load_curobo(
+    n_cubes: int,
+    enable_debug: bool = False,
+    tsteps: int = 30,
+    trajopt_seeds: int = 4,
+    mpinets: bool = False,
+    graph_mode: bool = False,
+    cuda_graph: bool = True,
+):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.015
+
+    ik_seeds = 30  # 500
+    if graph_mode:
+        trajopt_seeds = 4
+    if trajopt_seeds >= 14:
+        ik_seeds = max(100, trajopt_seeds * 4)
+    if mpinets:
+        robot_cfg["kinematics"]["lock_joints"] = {
+            "panda_finger_joint1": 0.025,
+            "panda_finger_joint2": -0.025,
+        }
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_nvblox_online.yml"))
+    )
+    interpolation_steps = 2000
+    if graph_mode:
+        interpolation_steps = 100
+    robot_cfg_instance = RobotConfig.from_dict(robot_cfg, tensor_args=TensorDeviceType())
+    K = robot_cfg_instance.kinematics.kinematics_config.joint_limits
+    K.position[0, :] -= 0.1
+    K.position[1, :] += 0.1
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg_instance,
+        world_cfg,
+        trajopt_tsteps=tsteps,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=cuda_graph,
+        position_threshold=0.005,  # 0.5 cm
+        rotation_threshold=0.05,
+        num_ik_seeds=ik_seeds,
+        num_graph_seeds=trajopt_seeds,
+        num_trajopt_seeds=trajopt_seeds,
+        interpolation_dt=0.025,
+        store_ik_debug=enable_debug,
+        store_trajopt_debug=enable_debug,
+        interpolation_steps=interpolation_steps,
+        collision_activation_distance=0.025,
+        state_finite_difference_mode="CENTRAL",
+        trajopt_dt=0.25,
+        minimize_jerk=True,
+        finetune_dt_scale=1.05,
+        maximum_trajectory_dt=0.1,
+    )
+    mg = MotionGen(motion_gen_config)
+    mg.warmup(enable_graph=True, warmup_js_trajopt=False)
+    return mg, robot_cfg
+
+
+def benchmark_mb(
+    write_usd=False,
+    save_log=False,
+    write_plot=False,
+    write_benchmark=False,
+    plot_cost=False,
+    override_tsteps: Optional[int] = None,
+    args=None,
+):
+    # load dataset:
+    graph_mode = args.graph
+    interpolation_dt = 0.02
+    file_paths = [demo_raw, motion_benchmaker_raw, mpinets_raw][2:]
+
+    enable_debug = save_log or plot_cost
+    all_files = []
+    og_tsteps = 32
+    if override_tsteps is not None:
+        og_tsteps = override_tsteps
+
+    og_trajopt_seeds = 12
+
+    if args.graph:
+        og_trajopt_seeds = 4
+    for file_path in file_paths:
+        all_groups = []
+        mpinets_data = False
+        problems = file_path()
+        if "dresser_task_oriented" in list(problems.keys()):
+            mpinets_data = True
+
+        mg, robot_cfg = load_curobo(
+            1,
+            enable_debug,
+            og_tsteps,
+            og_trajopt_seeds,
+            mpinets_data,
+            graph_mode,
+            not args.disable_cuda_graph,
+        )
+
+        for key, v in tqdm(problems.items()):
+            scene_problems = problems[key][:]  # [:1]  # [:20]  # [0:10]
+            m_list = []
+            i = 0
+            ik_fail = 0
+            for problem in tqdm(scene_problems, leave=False):
+                i += 1
+
+                plan_config = MotionGenPlanConfig(
+                    max_attempts=10,  # 00,  # 00,  # 100,  # 00,  # 000,#,00,#00,  # 5000,
+                    enable_graph_attempt=3,
+                    enable_finetune_trajopt=True,
+                    partial_ik_opt=False,
+                    enable_graph=graph_mode,
+                    timeout=60,
+                    enable_opt=not graph_mode,
+                )
+
+                q_start = problem["start"]
+                pose = (
+                    problem["goal_pose"]["position_xyz"] + problem["goal_pose"]["quaternion_wxyz"]
+                )
+
+                problem_name = "d_" + key + "_" + str(i)
+
+                # reset planner
+                mg.reset(reset_seed=False)
+                world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_mesh_world(
+                    merge_meshes=True
+                )
+                mesh = world.mesh[0].get_trimesh_mesh()
+
+                # world.save_world_as_mesh(problem_name + ".stl")
+                mg.world_coll_checker.clear_cache()
+                m_dataset = MeshDataset(
+                    None, n_frames=200, image_size=640, save_data_dir=None, trimesh_mesh=mesh
+                )
+                tensor_args = mg.tensor_args
+                for j in range(len(m_dataset)):
+                    data = m_dataset[j]
+                    cam_obs = CameraObservation(
+                        rgb_image=tensor_args.to_device(data["rgba"]),
+                        depth_image=tensor_args.to_device(data["depth"]),
+                        intrinsics=data["intrinsics"],
+                        pose=Pose.from_matrix(data["pose"].to(device=mg.tensor_args.device)),
+                    )
+                    cam_obs = cam_obs
+
+                    mg.add_camera_frame(cam_obs, "world")
+
+                    mg.process_camera_frames("world", False)
+                torch.cuda.synchronize()
+                mg.world_coll_checker.update_blox_hashes()
+                torch.cuda.synchronize()
+                if save_log or write_usd:
+                    # nvblox_obs = mg.world_coll_checker.get_mesh_from_blox_layer("world", mode="nvblox")
+
+                    # nvblox_obs.save_as_mesh("debug_tsdf.obj")
+                    nvblox_obs = mg.world_coll_checker.get_mesh_from_blox_layer(
+                        "world", mode="voxel"
+                    )
+                    nvblox_obs.color = [0.0, 0.0, 0.8, 0.8]
+                    # nvblox_obs.save_as_mesh("debug_voxel_occ.obj")
+                    # exit()
+                    nvblox_obs.name = "nvblox_world"
+                    world.add_obstacle(nvblox_obs)
+                # run planner
+                start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
+                result = mg.plan_single(
+                    start_state,
+                    Pose.from_list(pose),
+                    plan_config,
+                )
+                if result.status == "IK Fail":
+                    ik_fail += 1
+                problem["solution"] = None
+                if plan_config.enable_finetune_trajopt:
+                    problem_name = key + "_" + str(i)
+                else:
+                    problem_name = "noft_" + key + "_" + str(i)
+                problem_name = "nvblox_" + problem_name
+                if write_usd or save_log:
+                    # CuRobo
+                    from curobo.util.usd_helper import UsdHelper
+
+                    world.randomize_color(r=[0.5, 0.9], g=[0.2, 0.5], b=[0.0, 0.2])
+                    if len(world.mesh) > 1:
+                        world.mesh[1].color = [125 / 255, 255 / 255, 70.0 / 255, 1.0]
+                    gripper_mesh = Mesh(
+                        name="target_gripper",
+                        file_path=join_path(
+                            get_assets_path(),
+                            "robot/franka_description/meshes/visual/hand.dae",
+                        ),
+                        color=[0.0, 0.8, 0.1, 1.0],
+                        pose=pose,
+                    )
+                    world.add_obstacle(gripper_mesh)
+
+                # get costs:
+                if plot_cost:
+                    dt = 0.5
+                    problem_name = "approx_wolfe_p" + problem_name
+                    if result.optimized_dt is not None:
+                        dt = result.optimized_dt.item()
+                    if "trajopt_result" in result.debug_info:
+                        success = result.success.item()
+                        traj_cost = result.debug_info["trajopt_result"].debug_info["solver"][
+                            "cost"
+                        ][-1]
+                        traj_cost = torch.cat(traj_cost, dim=-1)
+                        plot_cost_iteration(
+                            traj_cost,
+                            title=problem_name + "_" + str(success) + "_" + str(dt),
+                            save_path=join_path("log/plot/", problem_name + "_cost"),
+                            log_scale=False,
+                        )
+                        if "finetune_trajopt_result" in result.debug_info:
+                            traj_cost = result.debug_info["finetune_trajopt_result"].debug_info[
+                                "solver"
+                            ]["cost"][0]
+                            traj_cost = torch.cat(traj_cost, dim=-1)
+                            plot_cost_iteration(
+                                traj_cost,
+                                title=problem_name + "_" + str(success) + "_" + str(dt),
+                                save_path=join_path("log/plot/", problem_name + "_ft_cost"),
+                                log_scale=False,
+                            )
+                if result.success.item():
+                    q_traj = result.get_interpolated_plan()
+                    problem["goal_ik"] = q_traj.position.cpu().squeeze().numpy()[-1, :].tolist()
+                    problem["solution"] = {
+                        "position": result.get_interpolated_plan()
+                        .position.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "velocity": result.get_interpolated_plan()
+                        .velocity.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "acceleration": result.get_interpolated_plan()
+                        .acceleration.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "jerk": result.get_interpolated_plan()
+                        .jerk.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "dt": interpolation_dt,
+                    }
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "opt_traj": {
+                            "position": result.optimized_plan.position.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "velocity": result.optimized_plan.velocity.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "acceleration": result.optimized_plan.acceleration.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "jerk": result.optimized_plan.jerk.cpu().squeeze().numpy().tolist(),
+                            "dt": result.optimized_dt.item(),
+                        },
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+                    current_metrics = CuroboMetrics(
+                        skip=False,
+                        success=True,
+                        time=result.total_time,
+                        collision=False,
+                        joint_limit_violation=False,
+                        self_collision=False,
+                        position_error=result.position_error.item() * 100.0,
+                        orientation_error=result.rotation_error.item() * 100.0,
+                        eef_position_path_length=10,
+                        eef_orientation_path_length=10,
+                        attempts=result.attempts,
+                        motion_time=result.motion_time.item(),
+                        solve_time=result.solve_time,
+                    )
+
+                    if write_usd:
+                        # CuRobo
+
+                        q_traj = result.get_interpolated_plan()
+                        UsdHelper.write_trajectory_animation_with_robot_usd(
+                            robot_cfg,
+                            world,
+                            start_state,
+                            q_traj,
+                            dt=result.interpolation_dt,
+                            save_path=join_path("log/usd/", problem_name) + ".usd",
+                            interpolation_steps=1,
+                            write_robot_usd_path="log/usd/assets/",
+                            robot_usd_local_reference="assets/",
+                            base_frame="/world_" + problem_name,
+                            visualize_robot_spheres=True,
+                        )
+
+                    if write_plot:
+                        problem_name = problem_name
+                        plot_traj(
+                            result.optimized_plan,
+                            result.optimized_dt.item(),
+                            # result.get_interpolated_plan(),
+                            # result.interpolation_dt,
+                            title=problem_name,
+                            save_path=join_path("log/plot/", problem_name + ".pdf"),
+                        )
+                        plot_traj(
+                            # result.optimized_plan,
+                            # result.optimized_dt.item(),
+                            result.get_interpolated_plan(),
+                            result.interpolation_dt,
+                            title=problem_name,
+                            save_path=join_path("log/plot/", problem_name + "_int.pdf"),
+                        )
+
+                    m_list.append(current_metrics)
+                    all_groups.append(current_metrics)
+                elif result.valid_query:
+                    current_metrics = CuroboMetrics()
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "status": result.status,
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+
+                    m_list.append(current_metrics)
+                    all_groups.append(current_metrics)
+                else:
+                    print("invalid: " + problem_name)
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "status": result.status,
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+                if save_log:
+                    UsdHelper.write_motion_gen_log(
+                        result,
+                        robot_cfg,
+                        world,
+                        start_state,
+                        Pose.from_list(pose),
+                        join_path("log/usd/", problem_name) + "_debug",
+                        write_ik=not result.success.item(),
+                        write_trajopt=True,
+                        visualize_robot_spheres=True,
+                        grid_space=2,
+                    )
+            g_m = CuroboGroupMetrics.from_list(m_list)
+            print(
+                key,
+                f"{g_m.success:2.2f}",
+                g_m.time.mean,
+                g_m.time.percent_98,
+                g_m.position_error.percent_98,
+                g_m.orientation_error.percent_98,
+            )
+        g_m = CuroboGroupMetrics.from_list(all_groups)
+        print(
+            "All: ",
+            f"{g_m.success:2.2f}",
+            g_m.motion_time.percent_98,
+            g_m.time.mean,
+            g_m.time.percent_75,
+            g_m.position_error.percent_75,
+            g_m.orientation_error.percent_75,
+        )
+        print(g_m.attempts)
+        if write_benchmark:
+            if not mpinets_data:
+                write_yaml(problems, "mb_curobo_solution_nvblox.yaml")
+            else:
+                write_yaml(problems, "mpinets_curobo_solution_nvblox.yaml")
+        all_files += all_groups
+    g_m = CuroboGroupMetrics.from_list(all_files)
+    print("######## FULL SET ############")
+    print("All: ", f"{g_m.success:2.2f}")
+    print("MT: ", g_m.motion_time)
+    print("PT:", g_m.time)
+    print("ST: ", g_m.solve_time)
+    print("accuracy: ", g_m.position_error, g_m.orientation_error)
+
+
+if __name__ == "__main__":
+    setup_curobo_logger("error")
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument(
+        "--graph",
+        action="store_true",
+        help="When True, runs only geometric planner",
+        default=False,
+    )
+    parser.add_argument(
+        "--disable_cuda_graph",
+        action="store_true",
+        help="When True, disable cuda graph during benchmarking",
+        default=False,
+    )
+    args = parser.parse_args()
+    benchmark_mb(
+        save_log=False,
+        write_usd=False,
+        write_plot=False,
+        write_benchmark=False,
+        plot_cost=False,
+        args=args,
+    )

benchmark/curobo_nvblox_profile.py

@@ -0,0 +1,193 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+import time
+from typing import Any, Dict, List
+
+# Third Party
+import numpy as np
+import torch
+import torch.autograd.profiler as profiler
+from robometrics.datasets import demo_raw
+from torch.profiler import ProfilerActivity, profile, record_function
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType, WorldConfig
+from curobo.geom.types import Mesh
+from curobo.types.math import Pose
+from curobo.types.state import JointState
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import (
+    get_assets_path,
+    get_robot_configs_path,
+    get_task_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+)
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+# torch.set_num_threads(8)
+# ttorch.use_deterministic_algorithms(True)
+torch.manual_seed(0)
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+np.random.seed(10)
+# Third Party
+from nvblox_torch.datasets.mesh_dataset import MeshDataset
+
+# CuRobo
+from curobo.types.camera import CameraObservation
+
+
+def load_curobo(n_cubes: int, enable_log: bool = False):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.015
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        "collision_nvblox_online.yml",
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=False,
+        position_threshold=0.005,
+        rotation_threshold=0.05,
+        num_ik_seeds=30,
+        num_trajopt_seeds=12,
+        interpolation_dt=0.02,
+        # grad_trajopt_iters=200,
+        store_ik_debug=enable_log,
+        store_trajopt_debug=enable_log,
+    )
+    mg = MotionGen(motion_gen_config)
+    mg.warmup(enable_graph=False)
+    # print("warmed up")
+    # exit()
+    return mg
+
+
+def benchmark_mb(write_usd=False, save_log=False):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    spheres = robot_cfg["kinematics"]["collision_spheres"]
+    if isinstance(spheres, str):
+        spheres = load_yaml(join_path(get_robot_configs_path(), spheres))["collision_spheres"]
+
+    plan_config = MotionGenPlanConfig(
+        max_attempts=2,
+        enable_graph_attempt=3,
+        enable_finetune_trajopt=True,
+        partial_ik_opt=False,
+        enable_graph=False,
+    )
+    # load dataset:
+
+    file_paths = [demo_raw]
+    all_files = []
+    for file_path in file_paths:
+        all_groups = []
+
+        problems = file_path()
+
+        for key, v in tqdm(problems.items()):
+            # if key not in ["table_under_pick_panda"]:
+            #    continue
+            scene_problems = problems[key]  # [:2]
+            n_cubes = check_problems(scene_problems)
+            mg = load_curobo(n_cubes, save_log)
+            m_list = []
+            i = 0
+            for problem in tqdm(scene_problems, leave=False):
+                q_start = problem["start"]
+                pose = (
+                    problem["goal_pose"]["position_xyz"] + problem["goal_pose"]["quaternion_wxyz"]
+                )
+
+                # reset planner
+                mg.reset(reset_seed=False)
+                world = WorldConfig.from_dict(problem["obstacles"]).get_mesh_world(
+                    merge_meshes=True
+                )
+                # clear cache:
+                mesh = world.mesh[0].get_trimesh_mesh()
+                mg.clear_world_cache()
+                obs = []
+                # get camera_observations:
+                m_dataset = MeshDataset(
+                    None, n_frames=200, image_size=640, save_data_dir=None, trimesh_mesh=mesh
+                )
+                obs = []
+                tensor_args = mg.tensor_args
+                for j in range(len(m_dataset)):
+                    with profiler.record_function("nvblox/create_camera_images"):
+                        data = m_dataset[j]
+                        cam_obs = CameraObservation(
+                            rgb_image=tensor_args.to_device(data["rgba"]),
+                            depth_image=tensor_args.to_device(data["depth"]),
+                            intrinsics=data["intrinsics"],
+                            pose=Pose.from_matrix(data["pose"].to(device=mg.tensor_args.device)),
+                        )
+                        obs.append(cam_obs)
+                with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+                    for j in range(len(obs)):
+                        cam_obs = obs[j]
+                        cam_obs.rgb_image = None
+                        with profiler.record_function("nvblox/add_camera_images"):
+                            mg.add_camera_frame(cam_obs, "world")
+
+                        with profiler.record_function("nvblox/process_camera_images"):
+                            mg.process_camera_frames("world", False)
+                    mg.world_coll_checker.update_blox_hashes()
+
+                    # run planner
+                    start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
+                    result = mg.plan_single(
+                        start_state,
+                        Pose.from_list(pose),
+                        plan_config,
+                    )
+                print("Exporting the trace..")
+                prof.export_chrome_trace("log/trace/trajopt_global_nvblox.json")
+                print(result.success, result.status)
+                exit()
+
+
+def get_metrics_obstacles(obs: Dict[str, List[Any]]):
+    obs_list = []
+    if "cylinder" in obs and len(obs["cylinder"].items()) > 0:
+        for _, vi in enumerate(obs["cylinder"].values()):
+            obs_list.append(
+                Cylinder(
+                    np.ravel(vi["pose"][:3]), vi["radius"], vi["height"], np.ravel(vi["pose"][3:])
+                )
+            )
+    if "cuboid" in obs and len(obs["cuboid"].items()) > 0:
+        for _, vi in enumerate(obs["cuboid"].values()):
+            obs_list.append(
+                Cuboid(np.ravel(vi["pose"][:3]), np.ravel(vi["dims"]), np.ravel(vi["pose"][3:]))
+            )
+    return obs_list
+
+
+def check_problems(all_problems):
+    n_cube = 0
+    for problem in all_problems:
+        cache = WorldConfig.from_dict(problem["obstacles"]).get_obb_world().get_cache_dict()
+        n_cube = max(n_cube, cache["obb"])
+    return n_cube
+
+
+if __name__ == "__main__":
+    setup_curobo_logger("error")
+    benchmark_mb()

benchmark/curobo_profile.py

@@ -0,0 +1,188 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+import argparse
+import time
+from typing import Any, Dict, List
+
+# Third Party
+import numpy as np
+import torch
+
+# from geometrout.primitive import Cuboid, Cylinder
+# from geometrout.transform import SE3
+# from robometrics.robot import CollisionSphereConfig, Robot
+from torch.profiler import ProfilerActivity, profile, record_function
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType, WorldConfig
+from curobo.geom.types import Mesh
+from curobo.types.math import Pose
+from curobo.types.state import JointState
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+# torch.set_num_threads(8)
+# ttorch.use_deterministic_algorithms(True)
+torch.manual_seed(0)
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+np.random.seed(10)
+# Third Party
+from robometrics.datasets import demo_raw
+
+
+def load_curobo(
+    n_cubes: int, enable_log: bool = False, mesh_mode: bool = False, cuda_graph: bool = False
+):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.015
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_obb_world()
+
+    c_checker = CollisionCheckerType.PRIMITIVE
+    c_cache = {"obb": n_cubes}
+    if mesh_mode:
+        c_checker = CollisionCheckerType.MESH
+        c_cache = {"mesh": n_cubes}
+        world_cfg = world_cfg.get_mesh_world()
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        trajopt_tsteps=32,
+        collision_checker_type=c_checker,
+        use_cuda_graph=cuda_graph,
+        collision_cache=c_cache,
+        ee_link_name="panda_hand",
+        position_threshold=0.005,
+        rotation_threshold=0.05,
+        num_ik_seeds=30,
+        num_trajopt_seeds=10,
+        interpolation_dt=0.02,
+        # grad_trajopt_iters=200,
+        store_ik_debug=enable_log,
+        store_trajopt_debug=enable_log,
+    )
+    mg = MotionGen(motion_gen_config)
+    mg.warmup(enable_graph=False)
+    return mg
+
+
+def benchmark_mb(args):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    spheres = robot_cfg["kinematics"]["collision_spheres"]
+    if isinstance(spheres, str):
+        spheres = load_yaml(join_path(get_robot_configs_path(), spheres))["collision_spheres"]
+
+    plan_config = MotionGenPlanConfig(
+        max_attempts=2,
+        enable_graph_attempt=3,
+        enable_finetune_trajopt=True,
+        partial_ik_opt=False,
+        enable_graph=False,
+    )
+    # load dataset:
+
+    file_paths = [demo_raw]
+    all_files = []
+    for file_path in file_paths:
+        all_groups = []
+
+        problems = file_path()
+
+        for key, v in tqdm(problems.items()):
+            # if key not in ["table_under_pick_panda"]:
+            #    continue
+            scene_problems = problems[key]  # [:2]
+            n_cubes = check_problems(scene_problems)
+            mg = load_curobo(n_cubes, False, args.mesh, args.cuda_graph)
+            m_list = []
+            i = 0
+            for problem in tqdm(scene_problems, leave=False):
+                q_start = problem["start"]
+
+                pose = (
+                    problem["goal_pose"]["position_xyz"] + problem["goal_pose"]["quaternion_wxyz"]
+                )
+
+                # reset planner
+                mg.reset(reset_seed=False)
+                if args.mesh:
+                    world = WorldConfig.from_dict(problem["obstacles"]).get_mesh_world()
+
+                else:
+                    world = WorldConfig.from_dict(problem["obstacles"]).get_obb_world()
+
+                mg.update_world(world)
+                # continue
+                # load obstacles
+
+                # run planner
+                start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
+                with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+                    result = mg.plan_single(
+                        start_state,
+                        Pose.from_list(pose),
+                        plan_config,
+                    )
+                print("Exporting the trace..")
+                prof.export_chrome_trace(join_path(args.save_path, args.file_name) + ".json")
+                print(result.success, result.status)
+                exit()
+
+
+def check_problems(all_problems):
+    n_cube = 0
+    for problem in all_problems:
+        cache = WorldConfig.from_dict(problem["obstacles"]).get_obb_world().get_cache_dict()
+        n_cube = max(n_cube, cache["obb"])
+    return n_cube
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--save_path",
+        type=str,
+        default="log/trace",
+        help="path to save file",
+    )
+    parser.add_argument(
+        "--file_name",
+        type=str,
+        default="motion_gen_trace",
+        help="File name prefix to use to save benchmark results",
+    )
+    parser.add_argument(
+        "--mesh",
+        action="store_true",
+        help="When True, converts obstacles to meshes",
+        default=False,
+    )
+    parser.add_argument(
+        "--cuda_graph",
+        action="store_true",
+        help="When True, uses cuda graph during profiing",
+        default=False,
+    )
+
+    args = parser.parse_args()
+
+    setup_curobo_logger("error")
+    benchmark_mb(args)

benchmark/curobo_python_profile.py

@@ -0,0 +1,180 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+import argparse
+import cProfile
+import time
+
+# Third Party
+import torch
+from torch.profiler import ProfilerActivity, profile, record_function
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util_file import get_robot_configs_path, get_robot_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+
+def demo_motion_gen():
+    # Standard Library
+
+    st_time = time.time()
+    tensor_args = TensorDeviceType()
+    world_file = "collision_table.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=False,
+        num_trajopt_seeds=4,
+        num_graph_seeds=1,
+        evaluate_interpolated_trajectory=True,
+        interpolation_dt=0.01,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+
+    # st_time = time.time()
+    motion_gen.warmup(batch=50, enable_graph=False, warmup_js_trajopt=False)
+    print("motion gen time:", time.time() - st_time)
+
+    # print(time.time() - st_time)
+    return
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = motion_gen.get_retract_config()
+    print(retract_cfg)
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
+    result = motion_gen.plan(
+        start_state, retract_pose, enable_graph=True, enable_opt=False, max_attempts=1
+    )
+    print(result.optimized_plan.position.shape)
+    traj = result.get_interpolated_plan()  # $.position.view(-1, 7)  # optimized plan
+    print("Trajectory Generated: ", result.success, result.optimized_dt.item())
+
+
+def demo_basic_robot():
+    st_time = time.time()
+    tensor_args = TensorDeviceType()
+    # load a urdf:
+    config_file = load_yaml(join_path(get_robot_path(), "franka.yml"))
+
+    urdf_file = config_file["robot_cfg"]["kinematics"][
+        "urdf_path"
+    ]  # Send global path starting with "/"
+    base_link = config_file["robot_cfg"]["kinematics"]["base_link"]
+    ee_link = config_file["robot_cfg"]["kinematics"]["ee_link"]
+    robot_cfg = RobotConfig.from_basic(urdf_file, base_link, ee_link, tensor_args)
+
+    kin_model = CudaRobotModel(robot_cfg.kinematics)
+    print("base kin time:", time.time() - st_time)
+    return
+    # compute forward kinematics:
+
+    # q = torch.rand((10, kin_model.get_dof()), **vars(tensor_args))
+    # out = kin_model.get_state(q)
+    # here is the kinematics state:
+    # print(out)
+
+
+def demo_full_config_robot(config_file):
+    st_time = time.time()
+    tensor_args = TensorDeviceType()
+    # load a urdf:
+
+    robot_cfg = RobotConfig.from_dict(config_file, tensor_args)
+
+    # kin_model = CudaRobotModel(robot_cfg.kinematics)
+    print("full kin time: ", time.time() - st_time)
+    # compute forward kinematics:
+    # q = torch.rand((10, kin_model.get_dof()), **vars(tensor_args))
+    # out = kin_model.get_state(q)
+    # here is the kinematics state:
+    # print(out)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--save_path",
+        type=str,
+        default="log/trace",
+        help="path to save file",
+    )
+    parser.add_argument(
+        "--file_name",
+        type=str,
+        default="startup_trace",
+        help="File name prefix to use to save benchmark results",
+    )
+    parser.add_argument(
+        "--motion_gen",
+        action="store_true",
+        help="When True, runs startup for motion generation",
+        default=False,
+    )
+    parser.add_argument(
+        "--kinematics",
+        action="store_true",
+        help="When True, runs startup for kinematics",
+        default=True,
+    )
+
+    args = parser.parse_args()
+
+    # cProfile.run('demo_motion_gen()')
+    config_file = load_yaml(join_path(get_robot_path(), "franka.yml"))["robot_cfg"]
+
+    # Third Party
+
+    if args.kinematics:
+        for _ in range(5):
+            demo_full_config_robot(config_file)
+
+        pr = cProfile.Profile()
+        pr.enable()
+        demo_full_config_robot(config_file)
+        pr.disable()
+        filename = join_path(args.save_path, args.file_name) + "_kinematics_cprofile.prof"
+        pr.dump_stats(filename)
+
+        with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+            demo_full_config_robot(config_file)
+        filename = join_path(args.save_path, args.file_name) + "_kinematics_trace.json"
+        prof.export_chrome_trace(filename)
+
+    if args.motion_gen:
+        for _ in range(5):
+            demo_motion_gen()
+
+        pr = cProfile.Profile()
+        pr.enable()
+        demo_motion_gen()
+        pr.disable()
+        filename = join_path(args.save_path, args.file_name) + "_motion_gen_cprofile.prof"
+        pr.dump_stats(filename)
+
+        with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+            demo_motion_gen()
+        filename = join_path(args.save_path, args.file_name) + "_motion_gen_trace.json"
+        prof.export_chrome_trace(filename)

benchmark/ik_benchmark.py

@@ -0,0 +1,182 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+# Standard Library
+import argparse
+import time
+
+# Third Party
+import numpy as np
+import torch
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+from curobo.geom.types import WorldConfig
+from curobo.rollout.arm_base import ArmBase, ArmBaseConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import RobotConfig
+from curobo.util_file import (
+    get_motion_gen_robot_list,
+    get_robot_configs_path,
+    get_robot_list,
+    get_task_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+    write_yaml,
+)
+from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+
+torch.backends.cudnn.benchmark = True
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+
+def run_full_config_collision_free_ik(
+    robot_file,
+    world_file,
+    batch_size,
+    use_cuda_graph=False,
+    collision_free=True,
+    high_precision=False,
+):
+    tensor_args = TensorDeviceType()
+    robot_data = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    if not collision_free:
+        robot_data["kinematics"]["collision_link_names"] = None
+        robot_data["kinematics"]["lock_joints"] = {}
+    robot_cfg = RobotConfig.from_dict(robot_data)
+    world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+    position_threshold = 0.005
+    if high_precision:
+        position_threshold = 0.001
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        rotation_threshold=0.05,
+        position_threshold=position_threshold,
+        num_seeds=30,
+        self_collision_check=collision_free,
+        self_collision_opt=collision_free,
+        tensor_args=tensor_args,
+        use_cuda_graph=use_cuda_graph,
+        high_precision=high_precision,
+        regularization=False,
+        # grad_iters=500,
+    )
+    ik_solver = IKSolver(ik_config)
+
+    for _ in range(3):
+        q_sample = ik_solver.sample_configs(batch_size)
+        while q_sample.shape[0] == 0:
+            q_sample = ik_solver.sample_configs(batch_size)
+
+        kin_state = ik_solver.fk(q_sample)
+        goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+        st_time = time.time()
+        result = ik_solver.solve_batch(goal)
+        torch.cuda.synchronize()
+        total_time = (time.time() - st_time) / q_sample.shape[0]
+    return (
+        total_time,
+        100.0 * torch.count_nonzero(result.success).item() / len(q_sample),
+        np.percentile(result.position_error[result.success].cpu().numpy(), 98).item(),
+        np.percentile(result.rotation_error[result.success].cpu().numpy(), 98).item(),
+    )
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--save_path",
+        type=str,
+        default=".",
+        help="path to save file",
+    )
+    parser.add_argument(
+        "--high_precision",
+        action="store_true",
+        help="When True, enables IK for 1 mm precision, when False 5mm precision",
+        default=False,
+    )
+    parser.add_argument(
+        "--file_name",
+        type=str,
+        default="ik",
+        help="File name prefix to use to save benchmark results",
+    )
+
+    args = parser.parse_args()
+
+    b_list = [1, 10, 100, 1000][-1:]
+
+    robot_list = get_motion_gen_robot_list()
+
+    world_file = "collision_test.yml"
+
+    print("running...")
+    data = {
+        "robot": [],
+        "IK time(ms)": [],
+        "Collision-Free IK time(ms)": [],
+        "Batch Size": [],
+        "Success IK": [],
+        "Success Collision-Free IK": [],
+        "Position Error(mm)": [],
+        "Orientation Error": [],
+        "Position Error Collision-Free IK(mm)": [],
+        "Orientation Error Collision-Free IK": [],
+    }
+    for robot_file in robot_list:
+        # create a sampler with dof:
+        for b_size in b_list:
+            # sample test configs:
+
+            dt_cu_ik, succ, p_err, q_err = run_full_config_collision_free_ik(
+                robot_file,
+                world_file,
+                batch_size=b_size,
+                use_cuda_graph=True,
+                collision_free=False,
+                high_precision=args.high_precision,
+            )
+            dt_cu_ik_cfree, success, p_err_c, q_err_c = run_full_config_collision_free_ik(
+                robot_file,
+                world_file,
+                batch_size=b_size,
+                use_cuda_graph=True,
+                collision_free=True,
+            )
+
+            # print(dt_cu/b_size, dt_cu_cg/b_size)
+            data["robot"].append(robot_file)
+            data["IK time(ms)"].append(dt_cu_ik * 1000.0)
+            data["Batch Size"].append(b_size)
+            data["Success Collision-Free IK"].append(success)
+            data["Success IK"].append(succ)
+
+            data["Position Error(mm)"].append(p_err * 1000.0)
+            data["Orientation Error"].append(q_err)
+            data["Position Error Collision-Free IK(mm)"].append(p_err_c * 1000.0)
+            data["Orientation Error Collision-Free IK"].append(q_err_c)
+
+            data["Collision-Free IK time(ms)"].append(dt_cu_ik_cfree * 1000.0)
+    write_yaml(data, join_path(args.save_path, args.file_name + ".yml"))
+    try:
+        # Third Party
+        import pandas as pd
+
+        df = pd.DataFrame(data)
+        print(df)
+        df.to_csv(join_path(args.save_path, args.file_name + ".csv"))
+    except ImportError:
+        pass

benchmark/kinematics_benchmark.py

@@ -0,0 +1,231 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+# Standard Library
+import argparse
+import time
+
+# Third Party
+import numpy as np
+import torch
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+from curobo.rollout.arm_base import ArmBase, ArmBaseConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.robot import RobotConfig
+from curobo.util_file import (
+    get_robot_configs_path,
+    get_robot_list,
+    get_task_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+    write_yaml,
+)
+
+torch.backends.cudnn.benchmark = True
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+
+def load_curobo(robot_file, world_file):
+    # load curobo arm base?
+
+    world_cfg = load_yaml(join_path(get_world_configs_path(), world_file))
+
+    base_config_data = load_yaml(join_path(get_task_configs_path(), "base_cfg.yml"))
+    graph_config_data = load_yaml(join_path(get_task_configs_path(), "graph.yml"))
+    # base_config_data["constraint"]["self_collision_cfg"]["weight"] = 0.0
+    # if not compute_distance:
+    #    base_config_data["constraint"]["primitive_collision_cfg"]["classify"] = False
+    robot_config_data = load_yaml(join_path(get_robot_configs_path(), robot_file))
+
+    arm_base = ArmBaseConfig.from_dict(
+        robot_config_data["robot_cfg"],
+        graph_config_data["model"],
+        base_config_data["cost"],
+        base_config_data["constraint"],
+        base_config_data["convergence"],
+        base_config_data["world_collision_checker_cfg"],
+        world_cfg,
+    )
+    arm_base = ArmBase(arm_base)
+    return arm_base
+
+
+def bench_collision_curobo(robot_file, world_file, q_test, use_cuda_graph=True):
+    arm_base = load_curobo(robot_file, world_file)
+    # load graph module:
+    tensor_args = TensorDeviceType()
+    q_test = tensor_args.to_device(q_test).unsqueeze(1)
+
+    tensor_args = TensorDeviceType()
+    q_warm = q_test + 0.5
+
+    if not use_cuda_graph:
+        for _ in range(10):
+            out = arm_base.rollout_constraint(q_warm)
+            torch.cuda.synchronize()
+
+        torch.cuda.synchronize()
+
+        st_time = time.time()
+        out = arm_base.rollout_constraint(q_test)
+
+        torch.cuda.synchronize()
+        dt = time.time() - st_time
+    else:
+        q = q_warm.clone()
+
+        g = torch.cuda.CUDAGraph()
+        s = torch.cuda.Stream()
+        s.wait_stream(torch.cuda.current_stream())
+        with torch.cuda.stream(s):
+            for i in range(3):
+                out = arm_base.rollout_constraint(q_warm)
+        torch.cuda.current_stream().wait_stream(s)
+        with torch.cuda.graph(g):
+            out = arm_base.rollout_constraint(q_warm)
+
+        for _ in range(10):
+            q.copy_(q_warm.detach())
+            g.replay()
+            a = out.feasible
+            # print(a)
+            # a = ee_mat.clone()
+        # q_new = torch.rand((b_size, robot_model.get_dof()), **vars(tensor_args))
+
+        torch.cuda.synchronize()
+        st_time = time.time()
+
+        q.copy_(q_test.detach().requires_grad_(False))
+        g.replay()
+        a = out.feasible
+        # print(a)
+        # a = ee_mat.clone()
+        torch.cuda.synchronize()
+        dt = time.time() - st_time
+    return dt
+
+
+def bench_kin_curobo(robot_file, q_test, use_cuda_graph=True, use_coll_spheres=True):
+    robot_data = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    if not use_coll_spheres:
+        robot_data["kinematics"]["collision_spheres"] = None
+        robot_data["kinematics"]["collision_link_names"] = None
+        robot_data["kinematics"]["use_global_cumul"] = False
+        robot_data["kinematics"]["lock_joints"] = {}
+    tensor_args = TensorDeviceType()
+    robot_data["kinematics"]["use_global_cumul"] = False
+
+    robot_cfg = RobotConfig.from_dict(robot_data)
+    robot_model = CudaRobotModel(robot_cfg.kinematics)
+
+    if not use_cuda_graph:
+        for _ in range(10):
+            q = torch.rand((b_size, robot_model.get_dof()), **vars(tensor_args))
+            out = robot_model.forward(q)
+            torch.cuda.synchronize()
+
+        q = torch.rand((b_size, robot_model.get_dof()), **vars(tensor_args))
+        torch.cuda.synchronize()
+
+        st_time = time.time()
+        out = robot_model.forward(q)
+        torch.cuda.synchronize()
+        dt = time.time() - st_time
+    else:
+        q = torch.rand((b_size, robot_model.get_dof()), **vars(tensor_args))
+
+        g = torch.cuda.CUDAGraph()
+        q = q.detach()
+        s = torch.cuda.Stream()
+        s.wait_stream(torch.cuda.current_stream())
+        with torch.cuda.stream(s):
+            for i in range(3):
+                ee_mat, _, _, _, _, _, _ = robot_model.forward(q=q)
+        torch.cuda.current_stream().wait_stream(s)
+        with torch.cuda.graph(g):
+            ee_mat, _, _, _, _, _, _ = robot_model.forward(q=q)
+        q_new = torch.rand((b_size, robot_model.get_dof()), **vars(tensor_args))
+
+        for _ in range(10):
+            q.copy_(q_new.detach().requires_grad_(False))
+            g.replay()
+            # a = ee_mat.clone()
+        q_new = torch.rand((b_size, robot_model.get_dof()), **vars(tensor_args))
+
+        torch.cuda.synchronize()
+        st_time = time.time()
+
+        q.copy_(q_new.detach().requires_grad_(False))
+        g.replay()
+        # a = ee_mat.clone()
+        torch.cuda.synchronize()
+        dt = time.time() - st_time
+    return dt
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--save_path",
+        type=str,
+        default=".",
+        help="path to save file",
+    )
+    parser.add_argument(
+        "--file_name",
+        type=str,
+        default="kinematics",
+        help="File name prefix to use to save benchmark results",
+    )
+
+    args = parser.parse_args()
+    b_list = [1, 10, 100, 1000, 10000]
+
+    robot_list = get_robot_list()
+
+    world_file = "collision_test.yml"
+
+    print("running...")
+    data = {"robot": [], "Kinematics": [], "Collision Checking": [], "Batch Size": []}
+    for robot_file in robot_list:
+        arm_sampler = load_curobo(robot_file, world_file)
+
+        # create a sampler with dof:
+        for b_size in b_list:
+            # sample test configs:
+            q_test = arm_sampler.sample_random_actions(b_size).cpu().numpy()
+
+            dt_cu_cg = bench_collision_curobo(
+                robot_file,
+                world_file,
+                q_test,
+                use_cuda_graph=True,
+            )
+            dt_kin_cg = bench_kin_curobo(
+                robot_file, q_test, use_cuda_graph=True, use_coll_spheres=True
+            )
+
+            data["robot"].append(robot_file)
+            data["Collision Checking"].append(dt_cu_cg)
+            data["Kinematics"].append(dt_kin_cg)
+            data["Batch Size"].append(b_size)
+    write_yaml(data, join_path(args.save_path, args.file_name + ".yml"))
+    try:
+        # Third Party
+        import pandas as pd
+
+        df = pd.DataFrame(data)
+        df.to_csv(join_path(args.save_path, args.file_name + ".csv"))
+    except ImportError:
+        pass

benchmark/metrics.py

@@ -0,0 +1,36 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+from dataclasses import dataclass
+from typing import List
+
+# Third Party
+import numpy as np
+from robometrics.statistics import Statistic, TrajectoryGroupMetrics, TrajectoryMetrics
+
+
+@dataclass
+class CuroboMetrics(TrajectoryMetrics):
+    time: float = np.inf
+
+
+@dataclass
+class CuroboGroupMetrics(TrajectoryGroupMetrics):
+    time: float = np.inf
+
+    @classmethod
+    def from_list(cls, group: List[CuroboMetrics]):
+        unskipped = [m for m in group if not m.skip]
+        successes = [m for m in unskipped if m.success]
+        data = super().from_list(group)
+        data.time = Statistic.from_list([m.time for m in successes])
+        return data

benchmark/robometrics_benchmark.py

@@ -0,0 +1,612 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+import argparse
+import time
+from copy import deepcopy
+from typing import Any, Dict, List, Optional
+
+# Third Party
+import numpy as np
+import torch
+from geometrout.primitive import Cuboid, Cylinder
+from geometrout.transform import SE3
+from metrics import CuroboGroupMetrics, CuroboMetrics
+from robometrics.datasets import demo_raw, motion_benchmaker_raw, mpinets_raw
+from robometrics.evaluator import Evaluator
+from robometrics.robot import CollisionSphereConfig, Robot
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import RobotConfig
+from curobo.types.state import JointState
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import (
+    get_assets_path,
+    get_robot_configs_path,
+    get_task_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+    write_yaml,
+)
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+torch.manual_seed(0)
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+np.random.seed(0)
+
+
+def load_curobo(
+    n_cubes: int,
+    enable_debug: bool = False,
+    tsteps: int = 30,
+    trajopt_seeds: int = 4,
+    mpinets: bool = False,
+    graph_mode: bool = True,
+):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0
+    robot_cfg["kinematics"]["collision_spheres"] = "spheres/franka_mesh.yml"
+    robot_cfg["kinematics"]["collision_link_names"].remove("attached_object")
+
+    ik_seeds = 30  # 500
+    if graph_mode:
+        trajopt_seeds = 4
+    if trajopt_seeds >= 14:
+        ik_seeds = max(100, trajopt_seeds * 4)
+    if mpinets:
+        robot_cfg["kinematics"]["lock_joints"] = {
+            "panda_finger_joint1": 0.025,
+            "panda_finger_joint2": -0.025,
+        }
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_obb_world()
+    interpolation_steps = 2000
+    if graph_mode:
+        interpolation_steps = 100
+    robot_cfg_instance = RobotConfig.from_dict(robot_cfg, tensor_args=TensorDeviceType())
+
+    K = robot_cfg_instance.kinematics.kinematics_config.joint_limits
+    K.position[0, :] -= 0.1
+    K.position[1, :] += 0.1
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg_instance,
+        world_cfg,
+        trajopt_tsteps=tsteps,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=True,
+        collision_cache={"obb": n_cubes},
+        position_threshold=0.005,  # 0.5 cm
+        rotation_threshold=0.05,
+        num_ik_seeds=ik_seeds,
+        num_graph_seeds=trajopt_seeds,
+        num_trajopt_seeds=trajopt_seeds,
+        interpolation_dt=0.025,
+        interpolation_steps=interpolation_steps,
+        collision_activation_distance=0.03,
+        state_finite_difference_mode="CENTRAL",
+        trajopt_dt=0.25,
+        minimize_jerk=True,
+        finetune_dt_scale=1.05,  # 1.05,
+        maximum_trajectory_dt=0.1,
+    )
+    mg = MotionGen(motion_gen_config)
+    mg.warmup(enable_graph=True, warmup_js_trajopt=False)
+    robot_cfg["kinematics"]["ee_link"] = "panda_hand"
+    return mg, robot_cfg
+
+
+def benchmark_mb(
+    write_usd=False,
+    save_log=False,
+    write_plot=False,
+    write_benchmark=False,
+    plot_cost=False,
+    override_tsteps: Optional[int] = None,
+    save_kpi=False,
+    graph_mode=False,
+    args=None,
+):
+    interpolation_dt = 0.02
+
+    enable_debug = False
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    urdf = join_path(get_assets_path(), robot_cfg["kinematics"]["urdf_path"])
+    spheres = robot_cfg["kinematics"]["collision_spheres"]
+
+    if isinstance(spheres, str):
+        spheres = load_yaml(join_path(get_robot_configs_path(), spheres))["collision_spheres"]
+    for s in spheres:
+        for k in spheres[s]:
+            k["radius"] = max(0.001, k["radius"] - 0.015)
+    data = {
+        "collision_spheres": spheres,
+        "self_collision_ignore": robot_cfg["kinematics"]["self_collision_ignore"],
+        "self_collision_buffer": robot_cfg["kinematics"]["self_collision_buffer"],
+    }
+    config = CollisionSphereConfig.load_from_dictionary(data)
+    metrics_robot = Robot(urdf, config)
+    evaluator = Evaluator(metrics_robot)
+    all_files = []
+
+    # modify robot joint limits as some start states in the dataset are at the joint limits:
+    if True:
+        for k in evaluator.robot.actuated_joints:
+            k.limit.lower -= 0.1
+            k.limit.upper += 0.1
+
+    plan_config = MotionGenPlanConfig(
+        max_attempts=60,
+        enable_graph_attempt=3,
+        enable_finetune_trajopt=False,
+        partial_ik_opt=True,
+    )
+    file_paths = [motion_benchmaker_raw, mpinets_raw]
+    if args.demo:
+        file_paths = [demo_raw]
+    # load dataset:
+    og_tsteps = 32
+    if override_tsteps is not None:
+        og_tsteps = override_tsteps
+
+    og_trajopt_seeds = 12
+    for file_path in file_paths:
+        all_groups = []
+        mpinets_data = False
+        problems = file_path()
+        if "dresser_task_oriented" in list(problems.keys()):
+            mpinets_data = True
+        for key, v in tqdm(problems.items()):
+            tsteps = og_tsteps
+            trajopt_seeds = og_trajopt_seeds
+            if "cage_panda" in key:
+                trajopt_seeds = 16
+            if "table_under_pick_panda" in key:
+                tsteps = 44
+                trajopt_seeds = 28
+
+            if "cubby_task_oriented" in key and "merged" not in key:
+                trajopt_seeds = 16
+
+            if "dresser_task_oriented" in key:
+                trajopt_seeds = 16
+
+            scene_problems = problems[key]  # [:4]  # [:1]  # [:20]  # [0:10]
+            n_cubes = check_problems(scene_problems)
+            mg, robot_cfg = load_curobo(
+                n_cubes, enable_debug, tsteps, trajopt_seeds, mpinets_data, graph_mode
+            )
+            m_list = []
+            i = 0
+            ik_fail = 0
+            for problem in tqdm(scene_problems, leave=False):
+                i += 1
+                if problem["collision_buffer_ik"] < 0.0:
+                    continue
+
+                plan_config = MotionGenPlanConfig(
+                    max_attempts=100,  # 00,  # 00,  # 100,  # 00,  # 000,#,00,#00,  # 5000,
+                    enable_graph_attempt=3,
+                    enable_finetune_trajopt=True,
+                    partial_ik_opt=False,
+                    enable_graph=graph_mode,
+                    timeout=60,
+                    enable_opt=not graph_mode,
+                )
+                q_start = problem["start"]
+                pose = (
+                    problem["goal_pose"]["position_xyz"] + problem["goal_pose"]["quaternion_wxyz"]
+                )
+
+                problem_name = "d_" + key + "_" + str(i)
+
+                # reset planner
+                mg.reset(reset_seed=False)
+                world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_obb_world()
+                # world.save_world_as_mesh(problem_name + ".stl")
+                mg.world_coll_checker.clear_cache()
+                mg.update_world(world)
+                # continue
+                # load obstacles
+
+                # run planner
+                start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
+                result = mg.plan_single(
+                    start_state,
+                    Pose.from_list(pose),
+                    plan_config,
+                )
+                if result.status == "IK Fail":
+                    ik_fail += 1
+                if result.success.item():
+                    eval_obs = get_metrics_obstacles(problem["obstacles"])
+
+                    q_traj = result.get_interpolated_plan()
+                    ee_trajectory = mg.compute_kinematics(q_traj)
+
+                    q_metrics = q_traj.position.cpu().numpy()
+                    ee_m = ee_trajectory.ee_pose
+                    ee_pos = ee_m.position.cpu().numpy()
+                    ee_q = ee_m.quaternion.cpu().numpy()
+                    se3_list = [
+                        SE3(np.ravel(ee_pos[p]), np.ravel(ee_q[p]))
+                        for p in range(ee_m.position.shape[0])
+                    ]
+                    # add gripper position:
+                    q_met = np.zeros((q_metrics.shape[0], 8))
+                    q_met[:, :7] = q_metrics
+                    q_met[:, 7] = 0.04
+                    if mpinets_data:
+                        q_met[:, 7] = 0.025
+                    st_time = time.time()
+
+                    current_metrics = evaluator.evaluate_trajectory(
+                        q_met,
+                        se3_list,
+                        SE3(np.ravel(pose[:3]), np.ravel(pose[3:])),
+                        eval_obs,
+                        result.total_time,
+                    )
+                    # if not current_metrics.success:
+                    #    print(current_metrics)
+                    #    write_usd = True
+                    # else:
+                    #    write_usd = False
+                # rint(plan_config.enable_graph, plan_config.enable_graph_attempt)
+                problem["solution"] = None
+                if plan_config.enable_finetune_trajopt:
+                    problem_name = key + "_" + str(i)
+                else:
+                    problem_name = "noft_" + key + "_" + str(i)
+                problem_name = "nosw_" + problem_name
+                if write_usd or save_log:
+                    # CuRobo
+                    from curobo.util.usd_helper import UsdHelper
+
+                    world.randomize_color(r=[0.5, 0.9], g=[0.2, 0.5], b=[0.0, 0.2])
+
+                    gripper_mesh = Mesh(
+                        name="target_gripper",
+                        file_path=join_path(
+                            get_assets_path(),
+                            "robot/franka_description/meshes/visual/hand_ee_link.dae",
+                        ),
+                        color=[0.0, 0.8, 0.1, 1.0],
+                        pose=pose,
+                    )
+                    world.add_obstacle(gripper_mesh)
+                # get costs:
+                if plot_cost:
+                    dt = 0.5
+                    problem_name = "approx_wolfe_p" + problem_name
+                    if result.optimized_dt is not None:
+                        dt = result.optimized_dt.item()
+                    if "trajopt_result" in result.debug_info:
+                        success = result.success.item()
+                        traj_cost = (
+                            # result.debug_info["trajopt_result"].debug_info["solver"]["cost"][0]
+                            result.debug_info["trajopt_result"].debug_info["solver"]["cost"][-1]
+                        )
+                        # print(traj_cost[0])
+                        traj_cost = torch.cat(traj_cost, dim=-1)
+                        plot_cost_iteration(
+                            traj_cost,
+                            title=problem_name + "_" + str(success) + "_" + str(dt),
+                            save_path=join_path("log/plot/", problem_name + "_cost"),
+                            log_scale=False,
+                        )
+                        if "finetune_trajopt_result" in result.debug_info:
+                            traj_cost = result.debug_info["finetune_trajopt_result"].debug_info[
+                                "solver"
+                            ]["cost"][0]
+                            traj_cost = torch.cat(traj_cost, dim=-1)
+                            plot_cost_iteration(
+                                traj_cost,
+                                title=problem_name + "_" + str(success) + "_" + str(dt),
+                                save_path=join_path("log/plot/", problem_name + "_ft_cost"),
+                                log_scale=False,
+                            )
+                if result.success.item() and current_metrics.success:
+                    q_traj = result.get_interpolated_plan()
+                    problem["goal_ik"] = q_traj.position.cpu().squeeze().numpy()[-1, :].tolist()
+                    problem["solution"] = {
+                        "position": result.get_interpolated_plan()
+                        .position.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "velocity": result.get_interpolated_plan()
+                        .velocity.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "acceleration": result.get_interpolated_plan()
+                        .acceleration.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "jerk": result.get_interpolated_plan()
+                        .jerk.cpu()
+                        .squeeze()
+                        .numpy()
+                        .tolist(),
+                        "dt": interpolation_dt,
+                    }
+                    # print(problem["solution"]["position"])
+                    # exit()
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "opt_traj": {
+                            "position": result.optimized_plan.position.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "velocity": result.optimized_plan.velocity.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "acceleration": result.optimized_plan.acceleration.cpu()
+                            .squeeze()
+                            .numpy()
+                            .tolist(),
+                            "jerk": result.optimized_plan.jerk.cpu().squeeze().numpy().tolist(),
+                            "dt": result.optimized_dt.item(),
+                        },
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+                    # print(
+                    #    "T: ",
+                    #    result.motion_time.item(),
+                    #    result.optimized_dt.item(),
+                    #    (len(problem["solution"]["position"]) - 1 ) * result.interpolation_dt,
+                    #    result.interpolation_dt,
+                    #    )
+                    # exit()
+                    current_metrics = CuroboMetrics(
+                        skip=False,
+                        success=True,
+                        time=result.total_time,
+                        collision=False,
+                        joint_limit_violation=False,
+                        self_collision=False,
+                        position_error=result.position_error.item() * 100.0,
+                        orientation_error=result.rotation_error.item() * 100.0,
+                        eef_position_path_length=10,
+                        eef_orientation_path_length=10,
+                        attempts=result.attempts,
+                        motion_time=result.motion_time.item(),
+                        solve_time=result.solve_time,
+                    )
+
+                    if write_usd:
+                        # CuRobo
+
+                        q_traj = result.get_interpolated_plan()
+                        UsdHelper.write_trajectory_animation_with_robot_usd(
+                            robot_cfg,
+                            world,
+                            start_state,
+                            q_traj,
+                            dt=result.interpolation_dt,
+                            save_path=join_path("log/usd/", problem_name) + ".usd",
+                            interpolation_steps=1,
+                            write_robot_usd_path="log/usd/assets/",
+                            robot_usd_local_reference="assets/",
+                            base_frame="/world_" + problem_name,
+                            visualize_robot_spheres=True,
+                        )
+
+                    if write_plot:
+                        problem_name = problem_name
+                        plot_traj(
+                            result.optimized_plan,
+                            result.optimized_dt.item(),
+                            # result.get_interpolated_plan(),
+                            # result.interpolation_dt,
+                            title=problem_name,
+                            save_path=join_path("log/plot/", problem_name + ".pdf"),
+                        )
+                        plot_traj(
+                            # result.optimized_plan,
+                            # result.optimized_dt.item(),
+                            result.get_interpolated_plan(),
+                            result.interpolation_dt,
+                            title=problem_name,
+                            save_path=join_path("log/plot/", problem_name + "_int.pdf"),
+                        )
+                        # exit()
+
+                    m_list.append(current_metrics)
+                    all_groups.append(current_metrics)
+                elif result.valid_query:
+                    # print("fail")
+                    # print(result.status)
+                    current_metrics = CuroboMetrics()
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "status": result.status,
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+
+                    m_list.append(current_metrics)
+                    all_groups.append(current_metrics)
+                else:
+                    # print("invalid: " + problem_name)
+                    debug = {
+                        "used_graph": result.used_graph,
+                        "attempts": result.attempts,
+                        "ik_time": result.ik_time,
+                        "graph_time": result.graph_time,
+                        "trajopt_time": result.trajopt_time,
+                        "total_time": result.total_time,
+                        "solve_time": result.solve_time,
+                        "status": result.status,
+                        "valid_query": result.valid_query,
+                    }
+                    problem["solution_debug"] = debug
+                if save_log:  # and not result.success.item():
+                    # print("save log")
+                    UsdHelper.write_motion_gen_log(
+                        result,
+                        robot_cfg,
+                        world,
+                        start_state,
+                        Pose.from_list(pose),
+                        join_path("log/usd/", problem_name) + "_debug",
+                        write_ik=False,
+                        write_trajopt=True,
+                        visualize_robot_spheres=False,
+                        grid_space=2,
+                    )
+                # exit()
+
+            g_m = CuroboGroupMetrics.from_list(m_list)
+            print(
+                key,
+                f"{g_m.success:2.2f}",
+                # g_m.motion_time,
+                g_m.time.mean,
+                # g_m.time.percent_75,
+                g_m.time.percent_98,
+                g_m.position_error.percent_98,
+                # g_m.position_error.median,
+                g_m.orientation_error.percent_98,
+                # g_m.orientation_error.median,
+            )  # , g_m.attempts)
+            print(g_m.attempts)
+            # print("MT: ", g_m.motion_time)
+            # $print(ik_fail)
+            # exit()
+            # print(g_m.position_error, g_m.orientation_error)
+
+        g_m = CuroboGroupMetrics.from_list(all_groups)
+        print(
+            "All: ",
+            f"{g_m.success:2.2f}",
+            g_m.motion_time.percent_98,
+            g_m.time.mean,
+            g_m.time.percent_75,
+            g_m.position_error.percent_75,
+            g_m.orientation_error.percent_75,
+        )  # g_m.time, g_m.attempts)
+        # print("MT: ", g_m.motion_time)
+
+        # print(g_m.position_error, g_m.orientation_error)
+
+        # exit()
+        if write_benchmark:
+            if not mpinets_data:
+                write_yaml(problems, "robometrics_mb_curobo_solution.yaml")
+            else:
+                write_yaml(problems, "robometrics_mpinets_curobo_solution.yaml")
+        all_files += all_groups
+    g_m = CuroboGroupMetrics.from_list(all_files)
+    # print(g_m.success, g_m.time, g_m.attempts, g_m.position_error, g_m.orientation_error)
+    print("######## FULL SET ############")
+    print("All: ", f"{g_m.success:2.2f}")
+    print("MT: ", g_m.motion_time)
+    print("PT:", g_m.time)
+    print("ST: ", g_m.solve_time)
+    print("position accuracy: ", g_m.position_error)
+    print("orientation accuracy: ", g_m.orientation_error)
+
+    if args.kpi:
+        kpi_data = {"Success": g_m.success, "Planning Time": float(g_m.time.mean)}
+        write_yaml(kpi_data, join_path(args.save_path, args.file_name + ".yml"))
+
+
+def get_metrics_obstacles(obs: Dict[str, List[Any]]):
+    obs_list = []
+    if "cylinder" in obs and len(obs["cylinder"].items()) > 0:
+        for _, vi in enumerate(obs["cylinder"].values()):
+            obs_list.append(
+                Cylinder(
+                    np.ravel(vi["pose"][:3]), vi["radius"], vi["height"], np.ravel(vi["pose"][3:])
+                )
+            )
+    if "cuboid" in obs and len(obs["cuboid"].items()) > 0:
+        for _, vi in enumerate(obs["cuboid"].values()):
+            obs_list.append(
+                Cuboid(np.ravel(vi["pose"][:3]), np.ravel(vi["dims"]), np.ravel(vi["pose"][3:]))
+            )
+    return obs_list
+
+
+def check_problems(all_problems):
+    n_cube = 0
+    for problem in all_problems:
+        cache = WorldConfig.from_dict(problem["obstacles"]).get_obb_world().get_cache_dict()
+        n_cube = max(n_cube, cache["obb"])
+    return n_cube
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--save_path",
+        type=str,
+        default=".",
+        help="path to save file",
+    )
+    parser.add_argument(
+        "--file_name",
+        type=str,
+        default="mg",
+        help="File name prefix to use to save benchmark results",
+    )
+    parser.add_argument(
+        "--graph",
+        action="store_true",
+        help="When True, runs only geometric planner",
+        default=False,
+    )
+    parser.add_argument(
+        "--kpi",
+        action="store_true",
+        help="When True, saves minimal metrics",
+        default=False,
+    )
+
+    parser.add_argument(
+        "--demo",
+        action="store_true",
+        help="When True, runs only on small dataaset",
+        default=False,
+    )
+
+    args = parser.parse_args()
+    setup_curobo_logger("error")
+    benchmark_mb(args=args, save_kpi=args.kpi)

docker/.dockerignore

@@ -0,0 +1,3 @@
+*.git
+*_build
+*build

docker/README.md

@@ -0,0 +1,36 @@
+<!--
+Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+
+NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+property and proprietary rights in and to this material, related
+documentation and any modifications thereto. Any use, reproduction,
+disclosure or distribution of this material and related documentation
+without an express license agreement from NVIDIA CORPORATION or
+its affiliates is strictly prohibited.
+-->
+# Docker Instructions
+
+## Running docker from NGC (Recommended)
+1. `sh build_user_docker.sh $UID`
+2. `sh start_docker_x86.sh` will start the docker
+
+## Building your own docker image with CuRobo
+1. Add default nvidia runtime to enable cuda compilation during docker build:
+    ```
+    Edit/create the /etc/docker/daemon.json with content:
+    {
+    "runtimes": {
+        "nvidia": {
+            "path": "/usr/bin/nvidia-container-runtime",
+            "runtimeArgs": []
+         } 
+    },
+    "default-runtime": "nvidia" # ADD this line (the above lines will already exist in your json file)
+    }
+    ```
+2. `sh pull_repos.sh`
+3. `bash build_dev_docker.sh`
+4. Change the docker image name in `user.dockerfile`
+5. `sh build_user_docker.sh`
+6. `sh start_docker_x86.sh` will start the docker
+

docker/arm64.dockerfile

@@ -0,0 +1,181 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+FROM nvcr.io/nvidia/l4t-pytorch:r35.1.0-pth1.13-py3 AS l4t_pytorch
+
+# Install ros components:
+RUN apt-get update &&\
+    apt-get install -y sudo git bash unattended-upgrades glmark2 &&\
+    rm -rf /var/lib/apt/lists/*
+
+
+# Deal with getting tons of debconf messages
+# See: https://github.com/phusion/baseimage-docker/issues/58
+RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
+
+# TODO: Don't hardcode timezone setting to Los_Angeles, pull from host computer
+# Set timezone info
+RUN apt-get update && apt-get install -y \
+  tzdata \
+  && rm -rf /var/lib/apt/lists/* \
+  && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \
+  && echo "America/Los_Angeles" > /etc/timezone \
+  && dpkg-reconfigure -f noninteractive tzdata
+
+# Install apt-get packages necessary for building, downloading, etc
+# NOTE: Dockerfile best practices recommends having apt-get update
+# and install commands in one line to avoid apt-get caching issues.
+# https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#run
+RUN apt-get update && apt-get install -y \
+  curl \
+  lsb-core \
+  software-properties-common \
+  wget \
+  && rm -rf /var/lib/apt/lists/*
+
+RUN add-apt-repository -y ppa:git-core/ppa
+
+RUN apt-get update && apt-get install -y \
+  build-essential \
+  cmake \
+  git \
+  git-lfs \
+  iputils-ping \
+  make \
+  openssh-server \
+  openssh-client \
+  libeigen3-dev \
+  libssl-dev \
+  python3-pip \
+  python3-ipdb \
+  python3-tk \
+  python3-wstool \
+  sudo git bash unattended-upgrades \
+  apt-utils \
+  terminator \
+  && rm -rf /var/lib/apt/lists/*
+
+ARG ROS_PKG=ros_base # desktop does not work
+ENV ROS_DISTRO=noetic
+ENV ROS_ROOT=/opt/ros/${ROS_DISTRO}
+ENV ROS_PYTHON_VERSION=3
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+WORKDIR /workspace
+
+
+#
+# add the ROS deb repo to the apt sources list
+#
+RUN apt-get update && \
+    apt-get install -y --no-install-recommends \
+          git \
+		cmake \
+		build-essential \
+		curl \
+		wget \
+		gnupg2 \
+		lsb-release \
+		ca-certificates \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add -
+
+
+#
+# install bootstrap dependencies
+#
+RUN apt-get update && \
+    apt-get install -y --no-install-recommends \
+          libpython3-dev \
+          python3-rosdep \
+          python3-rosinstall-generator \
+          python3-vcstool \
+          build-essential && \
+    rosdep init && \
+    rosdep update && \
+    rm -rf /var/lib/apt/lists/*
+
+
+#
+# download/build the ROS source
+#
+RUN mkdir ros_catkin_ws && \
+    cd ros_catkin_ws && \
+    rosinstall_generator ${ROS_PKG} vision_msgs --rosdistro ${ROS_DISTRO} --deps --tar > ${ROS_DISTRO}-${ROS_PKG}.rosinstall && \
+    mkdir src && \
+    vcs import --input ${ROS_DISTRO}-${ROS_PKG}.rosinstall ./src && \
+    apt-get update && \
+    rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro ${ROS_DISTRO} --skip-keys python3-pykdl -y && \
+    python3 ./src/catkin/bin/catkin_make_isolated --install --install-space ${ROS_ROOT} -DCMAKE_BUILD_TYPE=Release && \
+    rm -rf /var/lib/apt/lists/*
+
+
+RUN pip3 install trimesh \
+  numpy-quaternion \
+  networkx \
+  pyyaml \
+  rospkg \ 
+  rosdep \
+  empy
+# copy pkgs directory:
+COPY pkgs /pkgs
+
+
+# install warp:
+RUN cd /pkgs/warp && python3 build_lib.py && pip3 install .
+
+# install curobolib-extras/warp_torch:
+RUN cd /pkgs/warp_torch && pip3 install .
+
+# install nvblox:
+RUN apt-get update && \
+    apt-get install -y libgoogle-glog-dev libgtest-dev curl libsqlite3-dev && \
+    cd /usr/src/googletest && cmake . && cmake --build . --target install && \
+    rm -rf /var/lib/apt/lists/*
+RUN cd /pkgs/nvblox/nvblox && mkdir build && cd build && cmake .. && \
+    make -j8 && make install
+
+# install curobolib-extras/nvblox_torch:
+RUN cd /pkgs/nvblox_torch && sh install.sh 
+
+# install curobolib and curobo:
+RUN cd /pkgs/curobolib && pip3 install .
+
+RUN cd /pkgs/curobo && pip3 install .
+
+# sudo apt-get install lcov libbullet-extras-dev python3-catkin-tools swig ros-noetic-ifopt libyaml-cpp-dev libjsoncpp-dev
+# pip3 install tqdm
+# sudo apt-get install liborocos-kdl-dev ros-noetic-fcl libpcl-dev libompl-dev  libnlopt-cxx-dev liburdf-dev libkdlparser-dev
+# clone taskflow and compile
+
+
+# libfranka:
+RUN apt-get update && apt-get install -y build-essential cmake git libpoco-dev libeigen3-dev && cd /pkgs && git clone --recursive https://github.com/frankaemika/libfranka && cd libfranka && git checkout 0.7.1 && git submodule update && mkdir build && cd build && cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF .. && \
+  cmake --build . && cpack -G DEB && dpkg -i libfranka*.deb &&  rm -rf /var/lib/apt/lists/*
+# franka_ros and franka_motion_control:
+RUN mkdir -p /curobo_ws/src && mv /pkgs/curobo_ros /curobo_ws/src/ && mv /pkgs/franka_motion_control /curobo_ws/src/
+
+RUN cd /curobo_ws/src && git clone --branch 0.7.1 https://github.com/frankaemika/franka_ros.git && \
+    git clone https://github.com/justagist/franka_panda_description.git 
+
+
+RUN apt-get update && apt-get install -y ros-noetic-robot-state-publisher ros-noetic-rviz \
+  ros-noetic-moveit-visualization ros-noetic-moveit-msgs \
+  ros-noetic-controller-interface ros-noetic-combined-robot-hw ros-noetic-joint-limits-interface \
+  ros-noetic-control-msgs ros-noetic-controller-manager ros-noetic-realtime-tools ros-noetic-eigen-conversions ros-noetic-tf-conversions ros-noetic-moveit-ros-visualization && rm -rf /var/lib/apt/lists/*
+
+RUN cd /curobo_ws && \
+  /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make -DCMAKE_BUILD_TYPE=Release"
+
+#RUN apt-get update && apt-get install -y ros-noetic-robot-state-publisher ros-noetic-rviz ros-noetic-moveit-msgs\
+#  ros-noetic-moveit-ros ros-noetic-controller-interface ros-noetic-combined-robot-hw ros-noetic-joint-limits-interface ros-noetic-control-msgs && rm -rf /var/lib/apt/lists/*

docker/base.dockerfile

@@ -0,0 +1,30 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+
+#@FROM nvcr.io/nvidia/pytorch:22.12-py3
+FROM nvcr.io/nvidia/pytorch:23.08-py3 AS torch_cuda_base
+
+RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
+
+RUN apt-get update && apt-get install -y \
+  tzdata \
+  && rm -rf /var/lib/apt/lists/* \
+  && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \
+  && echo "America/Los_Angeles" > /etc/timezone \
+  && dpkg-reconfigure -f noninteractive tzdata
+
+RUN apt-get update &&\
+    apt-get install -y sudo git bash software-properties-common graphviz &&\
+    rm -rf /var/lib/apt/lists/*
+
+
+
+RUN python -m pip install --upgrade pip && python3 -m pip install graphviz

docker/build_dev_docker.sh

@@ -0,0 +1,46 @@
+#!/bin/bash
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+
+
+# This script will create a dev docker. Run this script by calling `bash build_dev_docker.sh`
+
+# Make sure you have pulled all required repos into pkgs folder (see pull_repos.sh script)
+
+# Check architecture to build:
+arch=`uname -m`
+
+if [ ${arch} == "x86_64" ]; then
+    echo "Building for X86 Architecture"
+    dockerfile="x86.dockerfile"
+elif [ ${arch} = "aarch64" ]; then
+    echo "Building for ARM Architecture"
+    dockerfile="arm64.dockerfile"
+else
+    echo "Unknown Architecture, defaulting to " + ${arch}
+    dockerfile="x86.dockerfile"
+fi
+
+# build docker file:
+# Make sure you enable nvidia runtime by:
+# Edit/create the /etc/docker/daemon.json with content:
+# {
+#    "runtimes": {
+#        "nvidia": {
+#            "path": "/usr/bin/nvidia-container-runtime",
+#            "runtimeArgs": []
+#         } 
+#    },
+#    "default-runtime": "nvidia" # ADD this line (the above lines will already exist in your json file)
+# }
+# 
+echo "${dockerfile}"
+docker build -t curobo_docker:latest -f ${dockerfile} .

docker/build_user_docker.sh

@@ -0,0 +1,11 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+docker build --build-arg USERNAME=$USER --no-cache --build-arg USER_ID=$1 --tag curobo_user_docker:latest -f user.dockerfile . 

docker/ros1_x86.dockerfile

@@ -0,0 +1,69 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+FROM nvcr.io/nvidia/pytorch:23.02-py3
+
+
+RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
+
+RUN apt-get update && apt-get install -y \
+  tzdata \
+  && rm -rf /var/lib/apt/lists/* \
+  && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \
+  && echo "America/Los_Angeles" > /etc/timezone \
+  && dpkg-reconfigure -f noninteractive tzdata
+
+RUN apt-get update &&\
+    apt-get install -y sudo git bash software-properties-common graphviz &&\
+    rm -rf /var/lib/apt/lists/*
+
+
+
+RUN python -m pip install --upgrade pip && python3 -m pip install graphviz
+
+# Install ROS noetic
+RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list' \
+&& apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 \
+&& apt-get update && apt-get install -y \
+  ros-noetic-desktop-full git build-essential python3-rosdep \
+  && rm -rf /var/lib/apt/lists/*
+
+
+# install realsense and azure kinect
+# Install the RealSense library (https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages)
+#RUN sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
+#RUN sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
+#RUN apt-get update && apt-get install -y \
+#  librealsense2-dkms \
+#  software-properties-common \
+#  librealsense2-utils \
+#  && rm -rf /var/lib/apt/lists/*
+
+
+# install moveit from source for all algos:
+ARG ROS_DISTRO=noetic
+RUN apt-get update && apt-get install -y \
+  ros-$ROS_DISTRO-apriltag-ros \
+  ros-$ROS_DISTRO-realsense2-camera \
+  ros-$ROS_DISTRO-ros-numpy \
+  ros-$ROS_DISTRO-vision-msgs \
+  ros-$ROS_DISTRO-franka-ros \
+  ros-$ROS_DISTRO-moveit-resources \
+  ros-$ROS_DISTRO-rosparam-shortcuts \
+  libglfw3-dev \
+  ros-$ROS_DISTRO-collada-urdf \
+  ros-$ROS_DISTRO-ur-msgs \
+  swig \
+  && rm -rf /var/lib/apt/lists/*
+
+
+RUN apt-get update && rosdep init && rosdep update && apt-get install -y ros-noetic-moveit-ros-visualization && rm -rf /var/lib/apt/lists/*
+RUN pip3 install netifaces
+

docker/start_docker_arm64.sh

@@ -0,0 +1,22 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+docker run --rm -it \
+--runtime nvidia \
+--mount type=bind,src=/home/$USER/code,target=/home/$USER/code \
+--hostname ros1-docker \
+--add-host ros1-docker:127.0.0.1 \
+--network host \
+--gpus all \
+--env ROS_HOSTNAME=localhost \
+--env DISPLAY=$DISPLAY \
+--volume /tmp/.X11-unix:/tmp/.X11-unix \
+--volume /dev/input:/dev/input \
+curobo_user_docker:latest

docker/start_docker_x86.sh

@@ -0,0 +1,22 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+docker run --rm -it \
+--privileged --mount type=bind,src=/home/$USER/code,target=/home/$USER/code \
+-e NVIDIA_DISABLE_REQUIRE=1 \
+-e NVIDIA_DRIVER_CAPABILITIES=all  --device /dev/dri \
+--hostname ros1-docker \
+--add-host ros1-docker:127.0.0.1 \
+--gpus all \
+--network host \
+--env DISPLAY=unix$DISPLAY \
+--volume /tmp/.X11-unix:/tmp/.X11-unix \
+--volume /dev:/dev \
+curobo_user_docker:latest

docker/start_docker_x86_robot.sh

@@ -0,0 +1,24 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+docker run --rm -it \
+--privileged --mount type=bind,src=/home/$USER/code,target=/home/$USER/code \
+-e NVIDIA_DISABLE_REQUIRE=1 \
+-e NVIDIA_DRIVER_CAPABILITIES=all  --device /dev/dri \
+--hostname ros1-docker \
+--add-host ros1-docker:127.0.0.1 \
+--gpus all \
+--network host \
+--env ROS_MASTER_URI=http://127.0.0.1:11311 \
+--env ROS_IP=127.0.0.1 \
+--env DISPLAY=unix$DISPLAY \
+--volume /tmp/.X11-unix:/tmp/.X11-unix \
+--volume /dev/input:/dev/input \
+curobo_user_docker:latest

docker/user.dockerfile

@@ -0,0 +1,34 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+
+# Check architecture and load:
+FROM curobo_docker:latest
+# Set variables
+ARG USERNAME
+ARG USER_ID
+
+# Set environment variables
+
+# Set up sudo user
+#RUN /sbin/adduser --disabled-password --gecos '' --uid $USER_ID $USERNAME
+RUN useradd -l -u $USER_ID -g users $USERNAME
+
+RUN /sbin/adduser $USERNAME sudo
+RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers
+
+# Set user
+USER $USERNAME
+WORKDIR /home/$USERNAME
+ENV USER=$USERNAME
+ENV PATH="${PATH}:/home/${USER}/.local/bin"
+
+RUN echo 'completed'
+

docker/x86.dockerfile

@@ -0,0 +1,93 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+FROM nvcr.io/nvidia/pytorch:23.08-py3 AS torch_cuda_base
+
+LABEL maintainer "User Name"
+
+# add GL:
+RUN apt-get update && apt-get install -y --no-install-recommends \
+        pkg-config \
+        libglvnd-dev \
+        libgl1-mesa-dev \
+        libegl1-mesa-dev \
+        libgles2-mesa-dev && \
+    rm -rf /var/lib/apt/lists/*
+
+ENV NVIDIA_VISIBLE_DEVICES all
+ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute
+
+
+RUN apt-get update &&\
+    apt-get install -y sudo git bash unattended-upgrades glmark2 &&\
+    rm -rf /var/lib/apt/lists/*
+
+
+# Deal with getting tons of debconf messages
+# See: https://github.com/phusion/baseimage-docker/issues/58
+RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
+
+# Set timezone info
+RUN apt-get update && apt-get install -y \
+  tzdata \
+  software-properties-common \
+  && rm -rf /var/lib/apt/lists/* \
+  && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \
+  && echo "America/Los_Angeles" > /etc/timezone \
+  && dpkg-reconfigure -f noninteractive tzdata \
+  && add-apt-repository -y ppa:git-core/ppa
+
+RUN apt-get update && apt-get install -y \
+  curl \
+  lsb-core \
+  wget \
+  build-essential \
+  cmake \
+  git \
+  git-lfs \
+  iputils-ping \
+  make \
+  openssh-server \
+  openssh-client \
+  libeigen3-dev \
+  libssl-dev \
+  python3-pip \
+  python3-ipdb \
+  python3-tk \
+  python3-wstool \
+  sudo git bash unattended-upgrades \
+  apt-utils \
+  terminator \
+  && rm -rf /var/lib/apt/lists/*
+
+# push defaults to bashrc:
+RUN apt-get update && apt-get install --reinstall -y \
+  libmpich-dev \
+  hwloc-nox libmpich12 mpich \
+  && rm -rf /var/lib/apt/lists/*
+
+# This is required to enable mpi lib access:
+ENV PATH="${PATH}:/opt/hpcx/ompi/bin"
+ENV LD_LIBRARY_PATH="${LD_LIBRARY_PATH}:/opt/hpcx/ompi/lib"
+
+
+
+ENV TORCH_CUDA_ARCH_LIST "7.0+PTX"
+ENV LD_LIBRARY_PATH="/usr/local/lib:${LD_LIBRARY_PATH}"
+
+
+# copy pkgs directory: clone curobo into docker/pkgs folder.
+COPY pkgs /pkgs
+
+RUN pip install "robometrics[evaluator] @ git+https://github.com/fishbotics/robometrics.git"
+
+
+RUN cd /pkgs/curobo && pip3 install .[dev,usd] --no-build-isolation
+

examples/collision_check_example.py

@@ -0,0 +1,57 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+""" Example computing collisions using curobo
+
+"""
+# Third Party
+import torch
+
+# CuRobo
+from curobo.types.base import TensorDeviceType
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+if __name__ == "__main__":
+    robot_file = "franka.yml"
+    world_file = "collision_test.yml"
+    tensor_args = TensorDeviceType()
+    # config = RobotWorldConfig.load_from_config(robot_file, world_file, pose_weight=[10, 200, 1, 10],
+    #                                           collision_activation_distance=0.0)
+    # curobo_fn = RobotWorld(config)
+    robot_file = "franka.yml"
+    world_config = {
+        "cuboid": {
+            "table": {"dims": [2, 2, 0.2], "pose": [0.4, 0.0, 0.3, 1, 0, 0, 0]},
+            "cube_1": {"dims": [0.1, 0.1, 0.2], "pose": [0.4, 0.0, 0.5, 1, 0, 0, 0]},
+        },
+        "mesh": {
+            "scene": {
+                "pose": [1.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
+                "file_path": "scene/nvblox/srl_ur10_bins.obj",
+            }
+        },
+    }
+    tensor_args = TensorDeviceType()
+    config = RobotWorldConfig.load_from_config(
+        robot_file, world_file, collision_activation_distance=0.0
+    )
+    curobo_fn = RobotWorld(config)
+
+    q_sph = torch.randn((10, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    q_sph[..., 3] = 0.2
+    d = curobo_fn.get_collision_distance(q_sph)
+    print(d)
+
+    q_s = curobo_fn.sample(5, mask_valid=False)
+
+    d_world, d_self = curobo_fn.get_world_self_collision_distance_from_joints(q_s)
+    print("Collision Distance:")
+    print("World:", d_world)
+    print("Self:", d_self)

examples/ik_example.py

@@ -0,0 +1,234 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+# Standard Library
+import time
+
+# Third Party
+import torch
+
+# CuRobo
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import RobotConfig
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+
+def demo_basic_ik():
+    tensor_args = TensorDeviceType()
+
+    config_file = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))
+    urdf_file = config_file["robot_cfg"]["kinematics"][
+        "urdf_path"
+    ]  # Send global path starting with "/"
+    base_link = config_file["robot_cfg"]["kinematics"]["base_link"]
+    ee_link = config_file["robot_cfg"]["kinematics"]["ee_link"]
+    robot_cfg = RobotConfig.from_basic(urdf_file, base_link, ee_link, tensor_args)
+
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        None,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=20,
+        self_collision_check=False,
+        self_collision_opt=False,
+        tensor_args=tensor_args,
+        use_cuda_graph=False,
+    )
+    ik_solver = IKSolver(ik_config)
+
+    # print(kin_state)
+    for _ in range(10):
+        q_sample = ik_solver.sample_configs(5000)
+        kin_state = ik_solver.fk(q_sample)
+        goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+        st_time = time.time()
+        result = ik_solver.solve_batch(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s) ",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            q_sample.shape[0] / (time.time() - st_time),
+            torch.mean(result.position_error) * 100.0,
+            torch.mean(result.rotation_error) * 100.0,
+        )
+
+
+def demo_full_config_collision_free_ik():
+    tensor_args = TensorDeviceType()
+    world_file = "collision_cage.yml"
+
+    robot_file = "franka.yml"
+    robot_cfg = RobotConfig.from_dict(
+        load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    )
+    world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=20,
+        self_collision_check=True,
+        self_collision_opt=True,
+        tensor_args=tensor_args,
+        use_cuda_graph=True,
+        # use_fixed_samples=True,
+    )
+    ik_solver = IKSolver(ik_config)
+
+    # print(kin_state)
+    print("Running Single IK")
+    for _ in range(10):
+        q_sample = ik_solver.sample_configs(5000)
+        kin_state = ik_solver.fk(q_sample)
+        goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+        st_time = time.time()
+        result = ik_solver.solve_batch(goal)
+        torch.cuda.synchronize()
+        total_time = (time.time() - st_time) / q_sample.shape[0]
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            total_time,
+            1.0 / total_time,
+            torch.mean(result.position_error) * 100.0,
+            torch.mean(result.rotation_error) * 100.0,
+        )
+    exit()
+    print("Running Batch IK (10 goals)")
+    q_sample = ik_solver.sample_configs(10)
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+    print("Running Goalset IK (10 goals in 1 set)")
+    q_sample = ik_solver.sample_configs(10)
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(kin_state.ee_position.unsqueeze(0), kin_state.ee_quaternion.unsqueeze(0))
+
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_goalset(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(result.success),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+    print("Running Batch Goalset IK (10 goals in 10 sets)")
+    q_sample = ik_solver.sample_configs(100)
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(
+        kin_state.ee_position.view(10, 10, 3).contiguous(),
+        kin_state.ee_quaternion.view(10, 10, 4).contiguous(),
+    )
+
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch_goalset(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(result.success.view(-1)),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+
+def demo_full_config_batch_env_collision_free_ik():
+    tensor_args = TensorDeviceType()
+    world_file = ["collision_test.yml", "collision_cubby.yml"]
+
+    robot_file = "franka.yml"
+    robot_cfg = RobotConfig.from_dict(
+        load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    )
+    world_cfg = [
+        WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), x))) for x in world_file
+    ]
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=100,
+        self_collision_check=True,
+        self_collision_opt=True,
+        tensor_args=tensor_args,
+        use_cuda_graph=False,
+        # use_fixed_samples=True,
+    )
+    ik_solver = IKSolver(ik_config)
+    q_sample = ik_solver.sample_configs(len(world_file))
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+    print("Running Batch Env IK")
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch_env(goal)
+        print(result.success)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+    q_sample = ik_solver.sample_configs(10 * len(world_file))
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(
+        kin_state.ee_position.view(len(world_file), 10, 3),
+        kin_state.ee_quaternion.view(len(world_file), 10, 4),
+    )
+
+    print("Running Batch Env Goalset IK")
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch_env_goalset(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(result.success.view(-1)),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+
+if __name__ == "__main__":
+    demo_basic_ik()
+    # demo_full_config_collision_free_ik()
+    # demo_full_config_batch_env_collision_free_ik()

examples/isaac_sim/batch_collision_checker.py

@@ -0,0 +1,171 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.util_file import get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Third Party
+import carb
+import numpy as np
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+
+def main():
+    usd_help = UsdHelper()
+    act_distance = 0.2
+
+    n_envs = 2
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+    # Make a target to follow
+    target_list = []
+    target_material_list = []
+    offset_x = 3.5
+    radius = 0.1
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+
+    for i in range(n_envs):
+        if i > 0:
+            pose.position[0, 0] += offset_x
+        usd_help.add_subroot("/World", "/World/world_" + str(i), pose)
+
+        target_material = OmniPBR("/World/looks/t_" + str(i), color=np.array([0, 1, 0]))
+
+        target = sphere.VisualSphere(
+            "/World/world_" + str(i) + "/target",
+            position=np.array([0.5, 0, 0.5]) + pose.position[0].cpu().numpy(),
+            orientation=np.array([1, 0, 0, 0]),
+            radius=radius,
+            visual_material=target_material,
+        )
+        target_list.append(target)
+        target_material_list.append(target_material)
+
+    setup_curobo_logger("warn")
+
+    # warmup curobo instance
+
+    tensor_args = TensorDeviceType()
+    robot_file = "franka.yml"
+    world_file = ["collision_thin_walls.yml", "collision_test.yml"]
+    world_cfg_list = []
+    for i in range(n_envs):
+        world_cfg = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), world_file[i]))
+        )  # .get_mesh_world()
+        world_cfg.objects[0].pose[2] += 0.1
+        world_cfg.randomize_color(r=[0.2, 0.3], b=[0.0, 0.05], g=[0.2, 0.3])
+        usd_help.add_world_to_stage(world_cfg, base_frame="/World/world_" + str(i))
+        world_cfg_list.append(world_cfg)
+    config = RobotWorldConfig.load_from_config(
+        robot_file, world_cfg_list, collision_activation_distance=act_distance
+    )
+    model = RobotWorld(config)
+    i = 0
+    max_distance = 0.5
+    x_sph = torch.zeros((n_envs, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+    env_query_idx = torch.arange(n_envs, device=tensor_args.device, dtype=torch.int32)
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+
+        if step_index < 20:
+            continue
+        sp_buffer = []
+        for k in target_list:
+            sph_position, _ = k.get_local_pose()
+            sp_buffer.append(sph_position)
+
+        x_sph[..., :3] = tensor_args.to_device(sp_buffer).view(n_envs, 1, 1, 3)
+
+        d, d_vec = model.get_collision_vector(x_sph, env_query_idx=env_query_idx)
+
+        d = d.view(-1).cpu()
+
+        for i in range(d.shape[0]):
+            p = d[i].item()
+            p = max(1, p * 5)
+            if d[i].item() == 0.0:
+                target_material_list[i].set_color(np.ravel([0, 1, 0]))
+            elif d[i].item() <= model.contact_distance:
+                target_material_list[i].set_color(np.array([0, 0, p]))
+            elif d[i].item() >= model.contact_distance:
+                target_material_list[i].set_color(np.array([p, 0, 0]))
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/batch_motion_gen_reacher.py

@@ -0,0 +1,311 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+
+def main():
+    usd_help = UsdHelper()
+    act_distance = 0.2
+
+    n_envs = 2
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+    # Make a target to follow
+    target_list = []
+    target_material_list = []
+    offset_y = 2.5
+    radius = 0.1
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    robot_list = []
+
+    for i in range(n_envs):
+        if i > 0:
+            pose.position[0, 1] += offset_y
+        usd_help.add_subroot("/World", "/World/world_" + str(i), pose)
+
+        target = cuboid.VisualCuboid(
+            "/World/world_" + str(i) + "/target",
+            position=np.array([0.5, 0, 0.5]) + pose.position[0].cpu().numpy(),
+            orientation=np.array([0, 1, 0, 0]),
+            color=np.array([1.0, 0, 0]),
+            size=0.05,
+        )
+        target_list.append(target)
+        r = add_robot_to_scene(
+            robot_cfg,
+            my_world,
+            "/World/world_" + str(i) + "/",
+            robot_name="robot_" + str(i),
+            position=pose.position[0].cpu().numpy(),
+        )
+        robot_list.append(r[0])
+    setup_curobo_logger("warn")
+
+    # warmup curobo instance
+
+    tensor_args = TensorDeviceType()
+    robot_file = "franka.yml"
+
+    world_file = ["collision_test.yml", "collision_thin_walls.yml"]
+    world_cfg_list = []
+    for i in range(n_envs):
+        world_cfg = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), world_file[i]))
+        )  # .get_mesh_world()
+        world_cfg.objects[0].pose[2] -= 0.02
+        world_cfg.randomize_color(r=[0.2, 0.3], b=[0.0, 0.05], g=[0.2, 0.3])
+        usd_help.add_world_to_stage(world_cfg, base_frame="/World/world_" + str(i))
+        world_cfg_list.append(world_cfg)
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg_list,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_cache={"obb": 6, "mesh": 6},
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        fixed_iters_trajopt=True,
+        finetune_dt_scale=1.05,
+        grad_trajopt_iters=300,
+        minimize_jerk=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    print("warming up...")
+    motion_gen.warmup(
+        enable_graph=False, warmup_js_trajopt=False, batch=n_envs, batch_env_mode=True
+    )
+
+    config = RobotWorldConfig.load_from_config(
+        robot_file, world_cfg_list, collision_activation_distance=act_distance
+    )
+    model = RobotWorld(config)
+    i = 0
+    max_distance = 0.5
+    x_sph = torch.zeros((n_envs, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+    env_query_idx = torch.arange(n_envs, device=tensor_args.device, dtype=torch.int32)
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+    prev_goal = None
+    cmd_plan = [None, None]
+    art_controllers = [r.get_articulation_controller() for r in robot_list]
+    cmd_idx = 0
+    past_goal = None
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            for robot in robot_list:
+                idx_list = [robot.get_dof_index(x) for x in j_names]
+                robot.set_joint_positions(default_config, idx_list)
+
+                robot._articulation_view.set_max_efforts(
+                    values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+                )
+        if step_index < 20:
+            continue
+        sp_buffer = []
+        sq_buffer = []
+        for k in target_list:
+            sph_position, sph_orientation = k.get_local_pose()
+            sp_buffer.append(sph_position)
+            sq_buffer.append(sph_orientation)
+
+        ik_goal = Pose(
+            position=tensor_args.to_device(sp_buffer),
+            quaternion=tensor_args.to_device(sq_buffer),
+        )
+        if prev_goal is None:
+            prev_goal = ik_goal.clone()
+        if past_goal is None:
+            past_goal = ik_goal.clone()
+        sim_js_names = robot_list[0].dof_names
+        sim_js = robot_list[0].get_joints_state()
+        full_js = JointState(
+            position=tensor_args.to_device(sim_js.positions).view(1, -1),
+            velocity=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+            joint_names=sim_js_names,
+        )
+        for i in range(1, len(robot_list)):
+            sim_js = robot_list[i].get_joints_state()
+            cu_js = JointState(
+                position=tensor_args.to_device(sim_js.positions).view(1, -1),
+                velocity=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+                acceleration=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+                jerk=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+                joint_names=sim_js_names,
+            )
+            full_js = full_js.stack(cu_js)
+
+        prev_distance = ik_goal.distance(prev_goal)
+        past_distance = ik_goal.distance(past_goal)
+
+        if (
+            (torch.sum(prev_distance[0] > 1e-2) or torch.sum(prev_distance[1] > 1e-2))
+            and (torch.sum(past_distance[0]) == 0.0 and torch.sum(past_distance[1] == 0.0))
+            and torch.norm(full_js.velocity) < 0.01
+            and cmd_plan[0] is None
+            and cmd_plan[1] is None
+        ):
+            full_js = full_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+            result = motion_gen.plan_batch_env(full_js, ik_goal, plan_config.clone())
+
+            prev_goal.copy_(ik_goal)
+            trajs = result.get_paths()
+            for s in range(len(result.success)):
+                if result.success[s]:
+                    cmd_plan[s] = motion_gen.get_full_js(trajs[s])
+                    # cmd_plan = result.get_interpolated_plan()
+                    # cmd_plan = motion_gen.get_full_js(cmd_plan)
+                    # get only joint names that are in both:
+                    idx_list = []
+                    common_js_names = []
+                    for x in sim_js_names:
+                        if x in cmd_plan[s].joint_names:
+                            idx_list.append(robot_list[s].get_dof_index(x))
+                            common_js_names.append(x)
+
+                    cmd_plan[s] = cmd_plan[s].get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+        # print(cmd_plan)
+
+        for s in range(len(cmd_plan)):
+            if cmd_plan[s] is not None and cmd_idx < len(cmd_plan[s].position):
+                cmd_state = cmd_plan[s][cmd_idx]
+
+                # get full dof state
+                art_action = ArticulationAction(
+                    cmd_state.position.cpu().numpy(),
+                    cmd_state.velocity.cpu().numpy(),
+                    joint_indices=idx_list,
+                )
+                # print(cmd_state.position)
+                # set desired joint angles obtained from IK:
+                art_controllers[s].apply_action(art_action)
+            else:
+                cmd_plan[s] = None
+        cmd_idx += 1
+        past_goal.copy_(ik_goal)
+
+        for _ in range(2):
+            my_world.step(render=False)
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/collision_checker.py

@@ -0,0 +1,212 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.util_file import get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+import numpy as np
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--nvblox", action="store_true", help="When True, enables headless mode", default=False
+)
+args = parser.parse_args()
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(1, 0, 0, 0.8)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+def main():
+    usd_help = UsdHelper()
+    act_distance = 0.4
+    ignore_list = ["/World/target", "/World/defaultGroundPlane"]
+
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    radius = 0.1
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+
+    target = sphere.VisualSphere(
+        "/World/target",
+        position=np.array([0.5, 0, 1.0]) + pose.position[0].cpu().numpy(),
+        orientation=np.array([1, 0, 0, 0]),
+        radius=radius,
+        visual_material=target_material,
+    )
+
+    setup_curobo_logger("warn")
+
+    # warmup curobo instance
+
+    tensor_args = TensorDeviceType()
+    robot_file = "franka.yml"
+    world_file = ["collision_thin_walls.yml", "collision_test.yml"][-1]
+    collision_checker_type = CollisionCheckerType.MESH
+    world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+    world_cfg.objects[0].pose[2] += 0.2
+    vis_world_cfg = world_cfg
+
+    if args.nvblox:
+        world_file = "collision_nvblox.yml"
+        collision_checker_type = CollisionCheckerType.BLOX
+        world_cfg = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), world_file))
+        )
+        world_cfg.objects[0].pose[2] += 0.4
+        ignore_list.append(world_cfg.objects[0].name)
+        vis_world_cfg = world_cfg.get_mesh_world()
+        # world_cfg = vis_world_cfg
+
+    usd_help.add_world_to_stage(vis_world_cfg, base_frame="/World")
+    config = RobotWorldConfig.load_from_config(
+        robot_file,
+        world_cfg,
+        collision_activation_distance=act_distance,
+        collision_checker_type=collision_checker_type,
+    )
+    model = RobotWorld(config)
+    i = 0
+    x_sph = torch.zeros((1, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+
+        if step_index < 20:
+            continue
+        if step_index % 1000 == 0.0:
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                reference_prim_path="/World",
+                ignore_substring=ignore_list,
+            ).get_collision_check_world()
+
+            model.update_world(obstacles)
+            print("Updated World")
+
+        sp_buffer = []
+        sph_position, _ = target.get_local_pose()
+
+        x_sph[..., :3] = tensor_args.to_device(sph_position).view(1, 1, 1, 3)
+
+        d, d_vec = model.get_collision_vector(x_sph)
+
+        d = d.view(-1).cpu()
+
+        p = d.item()
+        p = max(1, p * 5)
+        if d.item() != 0.0:
+            draw_line(sph_position, d_vec[..., :3].view(3).cpu().numpy())
+            print(d, d_vec)
+
+        else:
+            # Third Party
+            from omni.isaac.debug_draw import _debug_draw
+
+            draw = _debug_draw.acquire_debug_draw_interface()
+            # if draw.get_num_points() > 0:
+            draw.clear_lines()
+        if d.item() == 0.0:
+            target_material.set_color(np.ravel([0, 1, 0]))
+        elif d.item() <= model.contact_distance:
+            target_material.set_color(np.array([0, 0, p]))
+        elif d.item() >= model.contact_distance:
+            target_material.set_color(np.array([p, 0, 0]))
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/helper.py

@@ -0,0 +1,134 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+from typing import Dict, List
+
+# Third Party
+import numpy as np
+from matplotlib import cm
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid
+from omni.isaac.core.robots import Robot
+
+try:
+    # Third Party
+    from omni.isaac.urdf import _urdf  # isaacsim 2022.2
+except ImportError:
+    from omni.importer.urdf import _urdf  # isaac sim 2023.1
+# CuRobo
+from curobo.util_file import get_assets_path, get_filename, get_path_of_dir, join_path
+
+
+############################################################
+def add_robot_to_scene(
+    robot_config: Dict,
+    my_world: World,
+    load_from_usd: bool = False,
+    subroot: str = "",
+    robot_name: str = "robot",
+    position: np.array = np.array([0, 0, 0]),
+):
+    urdf_interface = _urdf.acquire_urdf_interface()
+
+    import_config = _urdf.ImportConfig()
+    import_config.merge_fixed_joints = False
+    import_config.convex_decomp = False
+    import_config.import_inertia_tensor = True
+    import_config.fix_base = True
+    import_config.make_default_prim = False
+    import_config.self_collision = False
+    import_config.create_physics_scene = True
+    import_config.import_inertia_tensor = False
+    import_config.default_drive_strength = 20000
+    import_config.default_position_drive_damping = 500
+    import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
+    import_config.distance_scale = 1
+    import_config.density = 0.0
+    full_path = join_path(get_assets_path(), robot_config["kinematics"]["urdf_path"])
+    robot_path = get_path_of_dir(full_path)
+    filename = get_filename(full_path)
+    imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)
+    dest_path = subroot
+    robot_path = urdf_interface.import_robot(
+        robot_path,
+        filename,
+        imported_robot,
+        import_config,
+        dest_path,
+    )
+
+    # prim_path = omni.usd.get_stage_next_free_path(
+    # my_world.scene.stage, str(my_world.scene.stage.GetDefaultPrim().GetPath()) + robot_path, False)
+    # print(prim_path)
+    # robot_prim = my_world.scene.stage.OverridePrim(prim_path)
+    # robot_prim.GetReferences().AddReference(dest_path)
+
+    robot = my_world.scene.add(
+        Robot(
+            prim_path=robot_path,
+            name=robot_name,
+            position=position,
+        )
+    )
+
+    return robot, robot_path
+
+
+class VoxelManager:
+    def __init__(
+        self,
+        num_voxels: int = 5000,
+        size: float = 0.02,
+        color: List[float] = [1, 1, 1],
+        prefix_path: str = "/World/curobo/voxel_",
+        material_path: str = "/World/looks/v_",
+    ) -> None:
+        self.cuboid_list = []
+        self.cuboid_material_list = []
+        self.disable_idx = num_voxels
+        for i in range(num_voxels):
+            target_material = OmniPBR("/World/looks/v_" + str(i), color=np.ravel(color))
+
+            cube = cuboid.VisualCuboid(
+                prefix_path + str(i),
+                position=np.array([0, 0, -10]),
+                orientation=np.array([1, 0, 0, 0]),
+                size=size,
+                visual_material=target_material,
+            )
+            self.cuboid_list.append(cube)
+            self.cuboid_material_list.append(target_material)
+            cube.set_visibility(True)
+
+    def update_voxels(self, voxel_position: np.ndarray, color_axis: int = 0):
+        max_index = min(voxel_position.shape[0], len(self.cuboid_list))
+
+        jet = cm.get_cmap("hot")  # .reversed()
+        z_val = voxel_position[:, 0]
+
+        jet_colors = jet(z_val)
+
+        for i in range(max_index):
+            self.cuboid_list[i].set_visibility(True)
+
+            self.cuboid_list[i].set_local_pose(translation=voxel_position[i])
+            self.cuboid_material_list[i].set_color(jet_colors[i][:3])
+
+        for i in range(max_index, len(self.cuboid_list)):
+            self.cuboid_list[i].set_local_pose(translation=np.ravel([0, 0, -10.0]))
+
+            # self.cuboid_list[i].set_visibility(False)
+
+    def clear(self):
+        for i in range(len(self.cuboid_list)):
+            self.cuboid_list[i].set_local_pose(translation=np.ravel([0, 0, -10.0]))

examples/isaac_sim/ik_reachability.py

@@ -0,0 +1,381 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+    "omni.isaac.urdf",
+]
+[enable_extension(x) for x in ext_list]
+simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+def get_pose_grid(n_x, n_y, n_z, max_x, max_y, max_z):
+    x = np.linspace(-max_x, max_x, n_x)
+    y = np.linspace(-max_y, max_y, n_y)
+    z = np.linspace(0, max_z, n_z)
+    x, y, z = np.meshgrid(x, y, z, indexing="ij")
+
+    position_arr = np.zeros((n_x * n_y * n_z, 3))
+    position_arr[:, 0] = x.flatten()
+    position_arr[:, 1] = y.flatten()
+    position_arr[:, 2] = z.flatten()
+    return position_arr
+
+
+def draw_points(pose, success):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    cpu_pos = pose.position.cpu().numpy()
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        if success[i].item():
+            colors += [(0, 1, 0, 0.25)]
+        else:
+            colors += [(1, 0, 0, 0.25)]
+    sizes = [40.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.002
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=20,
+        self_collision_check=True,
+        self_collision_opt=True,
+        tensor_args=tensor_args,
+        use_cuda_graph=True,
+        collision_checker_type=CollisionCheckerType.MESH,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        # use_fixed_samples=True,
+    )
+    ik_solver = IKSolver(ik_config)
+
+    # get pose grid:
+    position_grid_offset = tensor_args.to_device(get_pose_grid(10, 10, 5, 0.5, 0.5, 0.5))
+
+    # read current ik pose and warmup?
+    fk_state = ik_solver.fk(ik_solver.get_retract_config().view(1, -1))
+    goal_pose = fk_state.ee_pose
+    goal_pose = goal_pose.repeat(position_grid_offset.shape[0])
+    goal_pose.position += position_grid_offset
+
+    result = ik_solver.solve_batch(goal_pose)
+
+    print("Curobo is Ready")
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg, base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        if step_index == 50 or step_index % 500 == 0.0:  # and cmd_plan is None:
+            print("Updating world, reading w.r.t.", robot_prim_path)
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                reference_prim_path=robot_prim_path,
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ],
+            ).get_collision_check_world()
+            print([x.name for x in obstacles.objects])
+            ik_solver.update_world(obstacles)
+            print("Updated World")
+            carb.log_info("Synced CuRobo world from stage.")
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(ik_solver.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = ik_solver.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        # print(sim_js.velocities)
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_pose.position[:] = ik_goal.position[:] + position_grid_offset
+            goal_pose.quaternion[:] = ik_goal.quaternion[:]
+            result = ik_solver.solve_batch(goal_pose)
+
+            succ = torch.any(result.success)
+            print(
+                "IK completed: Poses: "
+                + str(goal_pose.batch)
+                + " Time(s): "
+                + str(result.solve_time)
+            )
+            # get spheres and flags:
+            draw_points(goal_pose, result.success)
+
+            if succ:
+                # get all solutions:
+
+                cmd_plan = result.js_solution[result.success]
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None and step_index % 20 == 0 and True:
+            cmd_state = cmd_plan[cmd_idx]
+
+            robot.set_joint_positions(cmd_state.position.cpu().numpy(), idx_list)
+
+            # set desired joint angles obtained from IK:
+            # articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+                my_world.step(render=True)
+                robot.set_joint_positions(default_config, idx_list)
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/load_all_robots.py

@@ -0,0 +1,180 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Standard Library
+import argparse
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import (
+    get_motion_gen_robot_list,
+    get_robot_configs_path,
+    get_robot_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+)
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+args = parser.parse_args()
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+
+def main():
+    list_of_robots = get_motion_gen_robot_list()  # [:2]
+    usd_help = UsdHelper()
+
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+    # Make a target to follow
+    offset_y = 2
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+
+    robot_cfg_list = []
+    robot_list = []
+    tensor_args = TensorDeviceType()
+    spheres = []
+
+    for i in range(len(list_of_robots)):
+        if i > 0:
+            pose.position[0, 1] += offset_y
+        if i == int(len(list_of_robots) / 2):
+            pose.position[0, 0] = -offset_y
+            pose.position[0, 1] = 0
+        robot_cfg = load_yaml(join_path(get_robot_configs_path(), list_of_robots[i]))["robot_cfg"]
+        robot_cfg_list.append(robot_cfg)
+        r = add_robot_to_scene(
+            robot_cfg,
+            my_world,
+            "/World/world_" + str(i) + "/",
+            robot_name="/World/world_" + str(i) + "/" "robot_" + str(i),
+            position=pose.position[0].cpu().numpy(),
+        )
+
+        robot_list.append(r[0])
+        robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+
+        kin_model = CudaRobotModel(robot_cfg.kinematics)
+        default_config = kin_model.cspace.retract_config
+
+        sph_list = kin_model.get_robot_as_spheres(default_config)
+        for si, s in enumerate(sph_list[0]):
+            sp = sphere.VisualSphere(
+                prim_path="/curobo/robot_sphere_" + str(i) + "_" + str(si),
+                position=np.ravel(s.position)
+                + pose.position[0].cpu().numpy()
+                + np.ravel([0, 0.5, 0.0]),
+                radius=float(s.radius),
+                color=np.array([0, 0.8, 0.2]),
+            )
+            spheres.append(sp)
+
+    setup_curobo_logger("warn")
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            for ri, robot in enumerate(robot_list):
+                j_names = robot_cfg_list[ri]["kinematics"]["cspace"]["joint_names"]
+                default_config = robot_cfg_list[ri]["kinematics"]["cspace"]["retract_config"]
+
+                idx_list = [robot.get_dof_index(x) for x in j_names]
+                robot.set_joint_positions(default_config, idx_list)
+                robot._articulation_view.set_max_efforts(
+                    values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+                )
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/motion_gen_reacher.py

@@ -0,0 +1,336 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+# simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        fixed_iters_trajopt=True,
+        finetune_dt_scale=1.05,
+        velocity_scale=[0.25, 1, 1, 1, 1.0, 1.0, 1.0, 1.0, 1.0],
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up...")
+    motion_gen.warmup(enable_graph=False, warmup_js_trajopt=False)
+
+    print("Curobo is Ready")
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg, base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index < 2:
+            my_world.reset()
+            robot._articulation_view.initialize()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        if step_index == 50 or step_index % 1000 == 0.0:
+            print("Updating world, reading w.r.t.", robot_prim_path)
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                reference_prim_path=robot_prim_path,
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ],
+            ).get_collision_check_world()
+
+            motion_gen.update_world(obstacles)
+            print("Updated World")
+            carb.log_info("Synced CuRobo world from stage.")
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+
+            result = motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, plan_config)
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            for _ in range(2):
+                my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/motion_gen_reacher_nvblox.py

@@ -0,0 +1,313 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+# [enable_extension(x) for x in ext_list]
+# simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+############################################################
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, _ = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_nvblox.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        finetune_dt_scale=1.05,
+        fixed_iters_trajopt=True,
+        finetune_trajopt_iters=500,
+        minimize_jerk=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up...")
+    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
+
+    print("Curobo is Ready")
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg.get_mesh_world(), base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        # print(sim_js.velocities)
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+
+            result = motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, plan_config)
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            for _ in range(2):
+                my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/mpc_example.py

@@ -0,0 +1,402 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# script running (ubuntu):
+#
+
+############################################################
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+import pickle
+import shutil
+import sys
+
+## import curobo:
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+###########################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {"headless": False, "width": 1920, "height": 1080}
+)  # _make_simulation_app({"headless": False})
+# Third Party
+# Enable the layers and stage windows in the UI
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+simulation_app.update()
+# Standard Library
+import os
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere
+from omni.isaac.core.prims.xform_prim import XFormPrim
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+
+# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper
+from omni.isaac.core.utils.rotations import euler_angles_to_quat
+
+########### frame prim #################
+from omni.isaac.core.utils.stage import get_stage_units
+from omni.isaac.core.utils.types import ArticulationAction
+from omni.isaac.cortex.cortex_object import CortexObject
+from omni.isaac.franka import KinematicsSolver
+from omni.isaac.franka.franka import Franka
+from omni.isaac.franka.tasks import FollowTarget
+from pxr import Gf
+from scipy.spatial.transform import Rotation as R
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+###########
+EXT_DIR = os.path.abspath(os.path.join(os.path.abspath(os.path.dirname(__file__))))
+DATA_DIR = os.path.join(EXT_DIR, "data")
+########### frame prim #################;;;;;
+
+
+# Standard Library
+from typing import Optional
+
+# Third Party
+from helper import add_robot_to_scene
+from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial
+from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path
+from pxr import Sdf, UsdShade
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+from curobo.curobolib import geom_cu
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+############################################################
+
+
+def draw_points(rollouts: torch.Tensor):
+    if rollouts is None:
+        return
+    # Standard Library
+    import random
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    cpu_rollouts = rollouts.cpu().numpy()
+    b, h, _ = cpu_rollouts.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [
+            (cpu_rollouts[i, j, 0], cpu_rollouts[i, j, 1], cpu_rollouts[i, j, 2]) for j in range(h)
+        ]
+        colors += [(1.0 - (i + 1.0 / b), 0.3 * (i + 1.0 / b), 0.0, 0.1) for _ in range(h)]
+    sizes = [10.0 for _ in range(b * h)]
+    draw.draw_points(point_list, colors, sizes)
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] += 0.02
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    init_curobo = False
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].pose[2] = -10.0
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        self_collision_check=True,
+        collision_checker_type=CollisionCheckerType.MESH,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        use_mppi=True,
+        use_lbfgs=False,
+        use_es=False,
+        store_rollouts=True,
+        step_dt=0.02,
+    )
+
+    mpc = MpcSolver(mpc_config)
+
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.clone().unsqueeze(0)
+    joint_names = mpc.rollout_fn.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg, joint_names=joint_names)
+    )
+    current_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    goal = Goal(
+        current_state=current_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+    mpc.update_goal(goal_buffer)
+
+    usd_help.load_stage(my_world.stage)
+    init_world = False
+    cmd_state_full = None
+    step = 0
+    while simulation_app.is_running():
+        if not init_world:
+            for _ in range(10):
+                my_world.step(render=True)
+            init_world = True
+        draw_points(mpc.get_visual_rollouts())
+
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            continue
+
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if not init_curobo:
+            init_curobo = True
+        step += 1
+        step_index = step
+        if step_index % 1000 == 0:
+            print("Updating world")
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ],
+                reference_prim_path=robot_prim_path,
+            )
+            obstacles.add_obstacle(world_cfg_table.cuboid[0])
+            mpc.world_coll_checker.load_collision_model(obstacles)
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position + 1.0
+
+        if np.linalg.norm(cube_position - past_pose) > 1e-3:
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_buffer.goal_pose.copy_(ik_goal)
+            mpc.update_goal(goal_buffer)
+            past_pose = cube_position
+
+        # if not changed don't call curobo:
+
+        # get robot current state:
+        sim_js = robot.get_joints_state()
+        js_names = robot.dof_names
+        sim_js_names = robot.dof_names
+
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        if cmd_state_full is None:
+            current_state.copy_(cu_js)
+        else:
+            current_state_partial = cmd_state_full.get_ordered_joint_state(
+                mpc.rollout_fn.joint_names
+            )
+            current_state.copy_(current_state_partial)
+            current_state.joint_names = current_state_partial.joint_names
+            # current_state = current_state.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        common_js_names = []
+        current_state.copy_(cu_js)
+
+        mpc_result = mpc.step(current_state, max_attempts=2)
+        # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+        succ = True  # ik_result.success.item()
+        cmd_state_full = mpc_result.js_action
+        common_js_names = []
+        idx_list = []
+        for x in sim_js_names:
+            if x in cmd_state_full.joint_names:
+                idx_list.append(robot.get_dof_index(x))
+                common_js_names.append(x)
+
+        cmd_state = cmd_state_full.get_ordered_joint_state(common_js_names)
+        cmd_state_full = cmd_state
+
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=idx_list,
+        )
+        # positions_goal = articulation_action.joint_positions
+        if step_index % 1000 == 0:
+            print(mpc_result.metrics.feasible.item(), mpc_result.metrics.pose_error.item())
+
+        if succ:
+            # set desired joint angles obtained from IK:
+            for _ in range(3):
+                articulation_controller.apply_action(art_action)
+
+        else:
+            carb.log_warn("No action is being taken.")
+
+
+############################################################
+
+if __name__ == "__main__":
+    main()
+    simulation_app.close()

examples/isaac_sim/mpc_nvblox_example.py

@@ -0,0 +1,387 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# script running (ubuntu):
+#
+
+############################################################
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+import pickle
+import shutil
+import sys
+
+## import curobo:
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+###########################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {"headless": False, "width": 1920, "height": 1080}
+)  # _make_simulation_app({"headless": False})
+# Third Party
+# Enable the layers and stage windows in the UI
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+simulation_app.update()
+# Standard Library
+import os
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere
+from omni.isaac.core.prims.xform_prim import XFormPrim
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+
+# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper
+from omni.isaac.core.utils.rotations import euler_angles_to_quat
+
+########### frame prim #################
+from omni.isaac.core.utils.stage import get_stage_units
+from omni.isaac.core.utils.types import ArticulationAction
+from omni.isaac.cortex.cortex_object import CortexObject
+from omni.isaac.franka import KinematicsSolver
+from omni.isaac.franka.franka import Franka
+from omni.isaac.franka.tasks import FollowTarget
+from pxr import Gf
+from scipy.spatial.transform import Rotation as R
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+###########
+EXT_DIR = os.path.abspath(os.path.join(os.path.abspath(os.path.dirname(__file__))))
+DATA_DIR = os.path.join(EXT_DIR, "data")
+########### frame prim #################;;;;;
+
+
+# Standard Library
+from typing import Optional
+
+# Third Party
+from helper import add_robot_to_scene
+from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial
+from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path
+from pxr import Sdf, UsdShade
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+from curobo.curobolib import geom_cu
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+############################################################
+
+
+def draw_points(rollouts: torch.Tensor):
+    if rollouts is None:
+        return
+    # Standard Library
+    import random
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    cpu_rollouts = rollouts.cpu().numpy()
+    b, h, _ = cpu_rollouts.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [
+            (cpu_rollouts[i, j, 0], cpu_rollouts[i, j, 1], cpu_rollouts[i, j, 2]) for j in range(h)
+        ]
+        colors += [(1.0 - (i + 1.0 / b), 0.3 * (i + 1.0 / b), 0.0, 0.1) for _ in range(h)]
+    sizes = [10.0 for _ in range(b * h)]
+    draw.draw_points(point_list, colors, sizes)
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] += 0.02
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+
+    init_curobo = False
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    # world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_nvblox.yml"))
+    )
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        self_collision_check=True,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_mppi=True,
+        use_lbfgs=False,
+        use_es=False,
+        store_rollouts=True,
+        step_dt=0.02,
+    )
+
+    mpc = MpcSolver(mpc_config)
+
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.clone().unsqueeze(0)
+    joint_names = mpc.rollout_fn.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg, joint_names=joint_names)
+    )
+    current_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    goal = Goal(
+        current_state=current_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+    mpc.update_goal(goal_buffer)
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg.get_mesh_world(), base_frame="/World")
+
+    init_world = False
+    cmd_state_full = None
+    step = 0
+    while simulation_app.is_running():
+        if not init_world:
+            for _ in range(10):
+                my_world.step(render=True)
+            init_world = True
+        draw_points(mpc.get_visual_rollouts())
+
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            continue
+
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if not init_curobo:
+            init_curobo = True
+        step += 1
+        step_index = step
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position + 1.0
+
+        if np.linalg.norm(cube_position - past_pose) > 1e-3:
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_buffer.goal_pose.copy_(ik_goal)
+            mpc.update_goal(goal_buffer)
+            past_pose = cube_position
+
+        # if not changed don't call curobo:
+
+        # get robot current state:
+        sim_js = robot.get_joints_state()
+        js_names = robot.dof_names
+        sim_js_names = robot.dof_names
+
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        if cmd_state_full is None:
+            current_state.copy_(cu_js)
+        else:
+            current_state_partial = cmd_state_full.get_ordered_joint_state(
+                mpc.rollout_fn.joint_names
+            )
+            current_state.copy_(current_state_partial)
+            current_state.joint_names = current_state_partial.joint_names
+            # current_state = current_state.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        common_js_names = []
+        current_state.copy_(cu_js)
+
+        mpc_result = mpc.step(current_state, max_attempts=2)
+        # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+        succ = True  # ik_result.success.item()
+        cmd_state_full = mpc_result.js_action
+        common_js_names = []
+        idx_list = []
+        for x in sim_js_names:
+            if x in cmd_state_full.joint_names:
+                idx_list.append(robot.get_dof_index(x))
+                common_js_names.append(x)
+
+        cmd_state = cmd_state_full.get_ordered_joint_state(common_js_names)
+        cmd_state_full = cmd_state
+        # print(ee_translation_goal, ee_orientation_teleop_goal)
+
+        # Compute IK for given EE Teleop goals
+        # articulation_action, succ = my_controller.compute_inverse_kinematics(
+        #    ee_translation_goal,
+        #    ee_orientation_teleop_goal,
+        # )
+
+        # create articulation action:
+        # get full dof state
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=idx_list,
+        )
+        # positions_goal = articulation_action.joint_positions
+        if step_index % 1000 == 0:
+            print(mpc_result.metrics.feasible.item(), mpc_result.metrics.pose_error.item())
+
+        if succ:
+            # set desired joint angles obtained from IK:
+            for _ in range(3):
+                articulation_controller.apply_action(art_action)
+
+        else:
+            carb.log_warn("No action is being taken.")
+
+
+############################################################
+
+if __name__ == "__main__":
+    main()
+    simulation_app.close()

examples/isaac_sim/multi_arm_reacher.py

@@ -0,0 +1,379 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument(
+    "--robot", type=str, default="dual_ur10e.yml", help="robot configuration to load"
+)
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+# [enable_extension(x) for x in ext_list]
+# simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+############################################################
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.02
+
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        maximum_trajectory_dt=0.2,
+        fixed_iters_trajopt=True,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up...")
+    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
+
+    print("Curobo is Ready")
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=10, enable_finetune_trajopt=True
+    )
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg, base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+
+    # read number of targets in link names:
+    link_names = motion_gen.kinematics.link_names
+    ee_link_name = motion_gen.kinematics.ee_link
+    # get link poses at retract configuration:
+
+    kin_state = motion_gen.kinematics.get_state(motion_gen.get_retract_config().view(1, -1))
+
+    link_retract_pose = kin_state.link_pose
+    t_pos = np.ravel(kin_state.ee_pose.to_list())
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=t_pos[:3],
+        orientation=t_pos[3:],
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    # create new targets for new links:
+    ee_idx = link_names.index(ee_link_name)
+    target_links = {}
+    names = []
+    for i in link_names:
+        if i != ee_link_name:
+            k_pose = np.ravel(link_retract_pose[i].to_list())
+            color = np.random.randn(3) * 0.2
+            color[0] += 0.5
+            color[1] = 0.5
+            color[2] = 0.0
+            target_links[i] = cuboid.VisualCuboid(
+                "/World/target_" + i,
+                position=np.array(k_pose[:3]),
+                orientation=np.array(k_pose[3:]),
+                color=color,
+                size=0.05,
+            )
+            names.append("/World/target_" + i)
+    i = 0
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        if step_index == 50 or step_index % 1000 == 0.0:
+            print("Updating world, reading w.r.t.", robot_prim_path)
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                reference_prim_path=robot_prim_path,
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ]
+                + names,
+            ).get_collision_check_world()
+
+            motion_gen.update_world(obstacles)
+            print("Updated World")
+            carb.log_info("Synced CuRobo world from stage.")
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        # print(sim_js.velocities)
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            # add link poses:
+            link_poses = {}
+            for i in target_links.keys():
+                c_p, c_rot = target_links[i].get_world_pose()
+                link_poses[i] = Pose(
+                    position=tensor_args.to_device(c_p),
+                    quaternion=tensor_args.to_device(c_rot),
+                )
+            result = motion_gen.plan_single(
+                cu_js.unsqueeze(0), ik_goal, plan_config.clone(), link_poses=link_poses
+            )
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            for _ in range(2):
+                my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/realsense_collision.py

@@ -0,0 +1,260 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+# Third Party
+import cv2
+import numpy as np
+import torch
+from matplotlib import cm
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.camera import CameraObservation
+from curobo.types.math import Pose
+from curobo.util_file import get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+simulation_app.update()
+# Third Party
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+
+def draw_points(voxels):
+    # Third Party
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    if len(voxels) == 0:
+        return
+
+    jet = cm.get_cmap("plasma").reversed()
+
+    cpu_pos = voxels[..., :3].view(-1, 3).cpu().numpy()
+    z_val = cpu_pos[:, 1]
+    # add smallest and largest values:
+    # z_val = np.append(z_val, 1.0)
+    # z_val = np.append(z_val,0.4)
+    # scale values
+    # z_val += 0.4
+    # z_val[z_val>1.0] = 1.0
+    # z_val = 1.0/z_val
+    # z_val = z_val/1.5
+    # z_val[z_val!=z_val] = 0.0
+    # z_val[z_val==0.0] = 0.4
+
+    jet_colors = jet(z_val)
+
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        colors += [(jet_colors[i][0], jet_colors[i][1], jet_colors[i][2], 1.0)]
+    sizes = [10.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def clip_camera(camera_data):
+    # clip camera image to bounding box:
+    h_ratio = 0.15
+    w_ratio = 0.15
+    depth = camera_data["raw_depth"]
+    depth_tensor = camera_data["depth"]
+    h, w = depth_tensor.shape
+    depth[: int(h_ratio * h), :] = 0.0
+    depth[int((1 - h_ratio) * h) :, :] = 0.0
+    depth[:, : int(w_ratio * w)] = 0.0
+    depth[:, int((1 - w_ratio) * w) :] = 0.0
+
+    depth_tensor[: int(h_ratio * h), :] = 0.0
+    depth_tensor[int(1 - h_ratio * h) :, :] = 0.0
+    depth_tensor[:, : int(w_ratio * w)] = 0.0
+    depth_tensor[:, int(1 - w_ratio * w) :] = 0.0
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(0.0, 0, 0.8, 0.9)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+if __name__ == "__main__":
+    radius = 0.05
+    act_distance = 0.4
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+    # my_world.scene.add_ground_plane(color=np.array([0.2,0.2,0.2]))
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+
+    target = sphere.VisualSphere(
+        "/World/target",
+        position=np.array([0.0, 0, 0.5]),
+        orientation=np.array([1, 0, 0, 0]),
+        radius=radius,
+        visual_material=target_material,
+    )
+
+    # Make a target to follow
+    camera_marker = cuboid.VisualCuboid(
+        "/World/camera_nvblox",
+        position=np.array([0.0, -0.1, 0.25]),
+        orientation=np.array([0.843, -0.537, 0.0, 0.0]),
+        color=np.array([0.1, 0.1, 0.5]),
+        size=0.03,
+    )
+    collision_checker_type = CollisionCheckerType.BLOX
+    world_cfg = WorldConfig.from_dict(
+        {
+            "blox": {
+                "world": {
+                    "pose": [0, 0, 0, 1, 0, 0, 0],
+                    "integrator_type": "occupancy",
+                    "voxel_size": 0.03,
+                }
+            }
+        }
+    )
+
+    config = RobotWorldConfig.load_from_config(
+        "franka.yml",
+        world_cfg,
+        collision_activation_distance=act_distance,
+        collision_checker_type=collision_checker_type,
+    )
+
+    model = RobotWorld(config)
+
+    realsense_data = RealsenseDataloader(clipping_distance_m=1.0)
+    data = realsense_data.get_data()
+
+    camera_pose = Pose.from_list([0, 0, 0, 0.707, 0.707, 0, 0])
+    i = 0
+    tensor_args = TensorDeviceType()
+    x_sph = torch.zeros((1, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        sp_buffer = []
+        sph_position, _ = target.get_local_pose()
+
+        x_sph[..., :3] = tensor_args.to_device(sph_position).view(1, 1, 1, 3)
+
+        model.world_model.decay_layer("world")
+        data = realsense_data.get_data()
+        clip_camera(data)
+        cube_position, cube_orientation = camera_marker.get_local_pose()
+        camera_pose = Pose(
+            position=tensor_args.to_device(cube_position),
+            quaternion=tensor_args.to_device(cube_orientation),
+        )
+        # print(data["rgba"].shape, data["depth"].shape, data["intrinsics"])
+
+        data_camera = CameraObservation(  # rgb_image = data["rgba_nvblox"],
+            depth_image=data["depth"], intrinsics=data["intrinsics"], pose=camera_pose
+        )
+        data_camera = data_camera.to(device=model.tensor_args.device)
+        # print(data_camera.depth_image, data_camera.rgb_image, data_camera.intrinsics)
+        # print("got new message")
+        model.world_model.add_camera_frame(data_camera, "world")
+        # print("added camera frame")
+        model.world_model.process_camera_frames("world", False)
+        torch.cuda.synchronize()
+        model.world_model.update_blox_hashes()
+        bounding = Cuboid("t", dims=[1, 1, 1], pose=[0, 0, 0, 1, 0, 0, 0])
+        voxels = model.world_model.get_voxels_in_bounding_box(bounding, 0.025)
+        # print(data_camera.depth_image)
+        depth_image = data["raw_depth"]
+        color_image = data["raw_rgb"]
+        depth_colormap = cv2.applyColorMap(
+            cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS
+        )
+        images = np.hstack((color_image, depth_colormap))
+
+        cv2.namedWindow("Align Example", cv2.WINDOW_NORMAL)
+        cv2.imshow("Align Example", images)
+        key = cv2.waitKey(1)
+        # Press esc or 'q' to close the image window
+        if key & 0xFF == ord("q") or key == 27:
+            cv2.destroyAllWindows()
+            break
+
+        draw_points(voxels)
+        d, d_vec = model.get_collision_vector(x_sph)
+
+        p = d.item()
+        p = max(1, p * 5)
+        if d.item() == 0.0:
+            target_material.set_color(np.ravel([0, 1, 0]))
+        elif d.item() <= model.contact_distance:
+            target_material.set_color(np.array([0, 0, p]))
+        elif d.item() >= model.contact_distance:
+            target_material.set_color(np.array([p, 0, 0]))
+
+        if d.item() != 0.0:
+            print(d, d_vec)
+
+            draw_line(sph_position, d_vec[..., :3].view(3).cpu().numpy())
+        else:
+            # Third Party
+            from omni.isaac.debug_draw import _debug_draw
+
+            draw = _debug_draw.acquire_debug_draw_interface()
+            # if draw.get_num_points() > 0:
+            draw.clear_lines()
+
+    realsense_data.stop_device()
+    print("finished program")
+    simulation_app.close()

examples/isaac_sim/realsense_mpc.py

@@ -0,0 +1,484 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+# Third Party
+import cv2
+import numpy as np
+import torch
+from matplotlib import cm
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.camera import CameraObservation
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+simulation_app.update()
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+from helper import VoxelManager, add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.rollout.rollout_base import Goal
+from curobo.util.usd_helper import UsdHelper
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+parser = argparse.ArgumentParser()
+
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+parser.add_argument(
+    "--waypoints", action="store_true", help="When True, sets robot in static mode", default=False
+)
+parser.add_argument(
+    "--use-debug-draw",
+    action="store_true",
+    help="When True, sets robot in static mode",
+    default=False,
+)
+args = parser.parse_args()
+
+
+def draw_rollout_points(rollouts: torch.Tensor, clear: bool = False):
+    if rollouts is None:
+        return
+    # Standard Library
+    import random
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    if clear:
+        draw.clear_points()
+    # if draw.get_num_points() > 0:
+    # draw.clear_points()
+    cpu_rollouts = rollouts.cpu().numpy()
+    b, h, _ = cpu_rollouts.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [
+            (cpu_rollouts[i, j, 0], cpu_rollouts[i, j, 1], cpu_rollouts[i, j, 2]) for j in range(h)
+        ]
+        colors += [(1.0 - (i + 1.0 / b), 0.3 * (i + 1.0 / b), 0.0, 0.1) for _ in range(h)]
+    sizes = [10.0 for _ in range(b * h)]
+    draw.draw_points(point_list, colors, sizes)
+
+
+def draw_points(voxels):
+    # Third Party
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    if len(voxels) == 0:
+        return
+
+    jet = cm.get_cmap("plasma").reversed()
+
+    cpu_pos = voxels[..., :3].view(-1, 3).cpu().numpy()
+    z_val = cpu_pos[:, 0]
+
+    jet_colors = jet(z_val)
+
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        colors += [(jet_colors[i][0], jet_colors[i][1], jet_colors[i][2], 0.8)]
+    sizes = [20.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def clip_camera(camera_data):
+    # clip camera image to bounding box:
+    h_ratio = 0.05
+    w_ratio = 0.05
+    depth = camera_data["raw_depth"]
+    depth_tensor = camera_data["depth"]
+    h, w = depth_tensor.shape
+    depth[: int(h_ratio * h), :] = 0.0
+    depth[int((1 - h_ratio) * h) :, :] = 0.0
+    depth[:, : int(w_ratio * w)] = 0.0
+    depth[:, int((1 - w_ratio) * w) :] = 0.0
+
+    depth_tensor[: int(h_ratio * h), :] = 0.0
+    depth_tensor[int(1 - h_ratio * h) :, :] = 0.0
+    depth_tensor[:, : int(w_ratio * w)] = 0.0
+    depth_tensor[:, int(1 - w_ratio * w) :] = 0.0
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(0.0, 0, 0.8, 0.9)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+if __name__ == "__main__":
+    radius = 0.05
+    act_distance = 0.4
+    voxel_size = 0.05
+    render_voxel_size = 0.02
+    clipping_distance = 0.7
+
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+    target_material_2 = OmniPBR("/World/looks/t2", color=np.array([0, 1, 0]))
+    if not args.waypoints:
+        target = cuboid.VisualCuboid(
+            "/World/target_1",
+            position=np.array([0.5, 0.0, 0.4]),
+            orientation=np.array([0, 1.0, 0, 0]),
+            size=0.04,
+            visual_material=target_material,
+        )
+
+    else:
+        target = cuboid.VisualCuboid(
+            "/World/target_1",
+            position=np.array([0.4, -0.5, 0.2]),
+            orientation=np.array([0, 1.0, 0, 0]),
+            size=0.04,
+            visual_material=target_material,
+        )
+
+    # Make a target to follow
+    target_2 = cuboid.VisualCuboid(
+        "/World/target_2",
+        position=np.array([0.4, 0.5, 0.2]),
+        orientation=np.array([0.0, 1, 0.0, 0.0]),
+        size=0.04,
+        visual_material=target_material_2,
+    )
+
+    # Make a target to follow
+    camera_marker = cuboid.VisualCuboid(
+        "/World/camera_nvblox",
+        position=np.array([-0.05, 0.0, 0.45]),
+        # orientation=np.array([0.793, 0, 0.609,0.0]),
+        orientation=np.array([0.5, -0.5, 0.5, -0.5]),
+        # orientation=np.array([0.561, -0.561, 0.431,-0.431]),
+        color=np.array([0, 0, 1]),
+        size=0.01,
+    )
+    camera_marker.set_visibility(False)
+    collision_checker_type = CollisionCheckerType.BLOX
+    world_cfg = WorldConfig.from_dict(
+        {
+            "blox": {
+                "world": {
+                    "pose": [0, 0, 0, 1, 0, 0, 0],
+                    "integrator_type": "occupancy",
+                    "voxel_size": 0.03,
+                }
+            }
+        }
+    )
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.02
+    robot, _ = add_robot_to_scene(robot_cfg, my_world, "/World/world_robot/")
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_wall.yml"))
+    )
+
+    world_cfg_table.cuboid[0].pose[2] -= 0.01
+    usd_help = UsdHelper()
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg_table.get_mesh_world(), base_frame="/World")
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+    world_cfg.add_obstacle(world_cfg_table.cuboid[1])
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        self_collision_check=True,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_mppi=True,
+        use_lbfgs=False,
+        use_es=False,
+        store_rollouts=True,
+        step_dt=0.02,
+    )
+
+    mpc = MpcSolver(mpc_config)
+
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.clone().unsqueeze(0)
+    joint_names = mpc.rollout_fn.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg, joint_names=joint_names)
+    )
+    current_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    goal = Goal(
+        current_state=current_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+    mpc.update_goal(goal_buffer)
+
+    world_model = mpc.world_collision
+    realsense_data = RealsenseDataloader(clipping_distance_m=clipping_distance)
+    data = realsense_data.get_data()
+
+    camera_pose = Pose.from_list([0, 0, 0, 0.707, 0.707, 0, 0])
+    i = 0
+    tensor_args = TensorDeviceType()
+    target_list = [target, target_2]
+    target_material_list = [target_material, target_material_2]
+    for material in target_material_list:
+        material.set_color(np.array([0.1, 0.1, 0.1]))
+    target_idx = 0
+    cmd_idx = 0
+    cmd_plan = None
+    articulation_controller = robot.get_articulation_controller()
+    cmd_state_full = None
+
+    cmd_step_idx = 0
+    current_error = 0.0
+    error_thresh = 0.01
+    first_target = False
+    if not args.use_debug_draw:
+        voxel_viewer = VoxelManager(100, size=render_voxel_size)
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+        step_index = my_world.current_time_step_index
+        if cmd_step_idx == 0:
+            draw_rollout_points(mpc.get_visual_rollouts(), clear=not args.use_debug_draw)
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if step_index % 2 == 0.0:
+            # camera data updation
+            world_model.decay_layer("world")
+            data = realsense_data.get_data()
+            clip_camera(data)
+            cube_position, cube_orientation = camera_marker.get_local_pose()
+            camera_pose = Pose(
+                position=tensor_args.to_device(cube_position),
+                quaternion=tensor_args.to_device(cube_orientation),
+            )
+
+            data_camera = CameraObservation(  # rgb_image = data["rgba_nvblox"],
+                depth_image=data["depth"], intrinsics=data["intrinsics"], pose=camera_pose
+            )
+            data_camera = data_camera.to(device=tensor_args.device)
+            world_model.add_camera_frame(data_camera, "world")
+            world_model.process_camera_frames("world", False)
+            torch.cuda.synchronize()
+            world_model.update_blox_hashes()
+
+            bounding = Cuboid("t", dims=[1, 1, 1.0], pose=[0, 0, 0, 1, 0, 0, 0])
+            voxels = world_model.get_voxels_in_bounding_box(bounding, voxel_size)
+            if voxels.shape[0] > 0:
+                voxels = voxels[voxels[:, 2] > voxel_size]
+                voxels = voxels[voxels[:, 0] > 0.0]
+                if args.use_debug_draw:
+                    draw_points(voxels)
+
+                else:
+                    voxels = voxels.cpu().numpy()
+                    voxel_viewer.update_voxels(voxels[:, :3])
+            else:
+                if not args.use_debug_draw:
+                    voxel_viewer.clear()
+
+        if True:
+            depth_image = data["raw_depth"]
+            color_image = data["raw_rgb"]
+            depth_colormap = cv2.applyColorMap(
+                cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS
+            )
+            color_image = cv2.flip(color_image, 1)
+            depth_colormap = cv2.flip(depth_colormap, 1)
+
+            images = np.hstack((color_image, depth_colormap))
+
+            cv2.namedWindow("NVBLOX Example", cv2.WINDOW_NORMAL)
+            cv2.imshow("NVBLOX Example", images)
+            key = cv2.waitKey(1)
+            # Press esc or 'q' to close the image window
+            if key & 0xFF == ord("q") or key == 27:
+                cv2.destroyAllWindows()
+                break
+
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+
+        if cmd_state_full is None:
+            current_state.copy_(cu_js)
+        else:
+            current_state_partial = cmd_state_full.get_ordered_joint_state(
+                mpc.rollout_fn.joint_names
+            )
+            current_state.copy_(current_state_partial)
+            current_state.joint_names = current_state_partial.joint_names
+
+        if current_error <= error_thresh and (not first_target or args.waypoints):
+            first_target = True
+            # motion generation:
+            for ks in range(len(target_material_list)):
+                if ks == target_idx:
+                    target_material_list[ks].set_color(np.ravel([0, 1.0, 0]))
+                else:
+                    target_material_list[ks].set_color(np.ravel([0.1, 0.1, 0.1]))
+
+            cube_position, cube_orientation = target_list[target_idx].get_world_pose()
+
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_buffer.goal_pose.copy_(ik_goal)
+            mpc.update_goal(goal_buffer)
+            target_idx += 1
+            if target_idx >= len(target_list):
+                target_idx = 0
+
+        if cmd_step_idx == 0:
+            mpc_result = mpc.step(current_state, max_attempts=2)
+            current_error = mpc_result.metrics.pose_error.item()
+        cmd_state_full = mpc_result.js_action
+        common_js_names = []
+        idx_list = []
+        for x in sim_js_names:
+            if x in cmd_state_full.joint_names:
+                idx_list.append(robot.get_dof_index(x))
+                common_js_names.append(x)
+
+        cmd_state = cmd_state_full.get_ordered_joint_state(common_js_names)
+        cmd_state_full = cmd_state
+
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=idx_list,
+        )
+        articulation_controller.apply_action(art_action)
+
+        if cmd_step_idx == 2:
+            cmd_step_idx = 0
+
+        # positions_goal = a
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                # cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_step_idx += 1
+            # for _ in range(2):
+            #    my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    realsense_data.stop_device()
+    print("finished program")
+
+    simulation_app.close()

examples/isaac_sim/realsense_reacher.py

@@ -0,0 +1,414 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+# Third Party
+import cv2
+import numpy as np
+import torch
+from matplotlib import cm
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.camera import CameraObservation
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+simulation_app.update()
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+from helper import VoxelManager, add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.usd_helper import UsdHelper
+
+parser = argparse.ArgumentParser()
+
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+parser.add_argument(
+    "--use-debug-draw",
+    action="store_true",
+    help="When True, sets robot in static mode",
+    default=False,
+)
+args = parser.parse_args()
+
+
+def draw_points(voxels):
+    # Third Party
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    if len(voxels) == 0:
+        return
+
+    jet = cm.get_cmap("plasma").reversed()
+
+    cpu_pos = voxels[..., :3].view(-1, 3).cpu().numpy()
+    z_val = cpu_pos[:, 0]
+
+    jet_colors = jet(z_val)
+
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        colors += [(jet_colors[i][0], jet_colors[i][1], jet_colors[i][2], 0.8)]
+    sizes = [20.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def clip_camera(camera_data):
+    # clip camera image to bounding box:
+    h_ratio = 0.05
+    w_ratio = 0.05
+    depth = camera_data["raw_depth"]
+    depth_tensor = camera_data["depth"]
+    h, w = depth_tensor.shape
+    depth[: int(h_ratio * h), :] = 0.0
+    depth[int((1 - h_ratio) * h) :, :] = 0.0
+    depth[:, : int(w_ratio * w)] = 0.0
+    depth[:, int((1 - w_ratio) * w) :] = 0.0
+
+    depth_tensor[: int(h_ratio * h), :] = 0.0
+    depth_tensor[int(1 - h_ratio * h) :, :] = 0.0
+    depth_tensor[:, : int(w_ratio * w)] = 0.0
+    depth_tensor[:, int(1 - w_ratio * w) :] = 0.0
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(0.0, 0, 0.8, 0.9)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+if __name__ == "__main__":
+    radius = 0.05
+    act_distance = 0.4
+    voxel_size = 0.05
+    render_voxel_size = 0.02
+    clipping_distance = 0.7
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+    target_material_2 = OmniPBR("/World/looks/t2", color=np.array([0, 1, 0]))
+
+    target = cuboid.VisualCuboid(
+        "/World/target_1",
+        position=np.array([0.4, -0.5, 0.2]),
+        orientation=np.array([0, 1.0, 0, 0]),
+        size=0.04,
+        visual_material=target_material,
+    )
+
+    # Make a target to follow
+    target_2 = cuboid.VisualCuboid(
+        "/World/target_2",
+        position=np.array([0.4, 0.5, 0.2]),
+        orientation=np.array([0.0, 1, 0.0, 0.0]),
+        size=0.04,
+        visual_material=target_material_2,
+    )
+
+    # Make a target to follow
+    camera_marker = cuboid.VisualCuboid(
+        "/World/camera_nvblox",
+        position=np.array([-0.05, 0.0, 0.45]),
+        # orientation=np.array([0.793, 0, 0.609,0.0]),
+        orientation=np.array([0.5, -0.5, 0.5, -0.5]),
+        # orientation=np.array([0.561, -0.561, 0.431,-0.431]),
+        color=np.array([0, 0, 1]),
+        size=0.01,
+    )
+    camera_marker.set_visibility(False)
+
+    collision_checker_type = CollisionCheckerType.BLOX
+    world_cfg = WorldConfig.from_dict(
+        {
+            "blox": {
+                "world": {
+                    "pose": [0, 0, 0, 1, 0, 0, 0],
+                    "integrator_type": "occupancy",
+                    "voxel_size": 0.03,
+                }
+            }
+        }
+    )
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, _ = add_robot_to_scene(robot_cfg, my_world, "/World/world_robot/")
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_wall.yml"))
+    )
+
+    world_cfg_table.cuboid[0].pose[2] -= 0.01
+    usd_help = UsdHelper()
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg_table.get_mesh_world(), base_frame="/World")
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+    world_cfg.add_obstacle(world_cfg_table.cuboid[1])
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        finetune_dt_scale=1.05,
+        fixed_iters_trajopt=True,
+        finetune_trajopt_iters=300,
+        minimize_jerk=True,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up..")
+    motion_gen.warmup(warmup_js_trajopt=False)
+
+    world_model = motion_gen.world_collision
+    realsense_data = RealsenseDataloader(clipping_distance_m=clipping_distance)
+    data = realsense_data.get_data()
+
+    camera_pose = Pose.from_list([0, 0, 0, 0.707, 0.707, 0, 0])
+    i = 0
+    tensor_args = TensorDeviceType()
+    target_list = [target, target_2]
+    target_material_list = [target_material, target_material_2]
+    for material in target_material_list:
+        material.set_color(np.array([0.1, 0.1, 0.1]))
+    target_idx = 0
+    cmd_idx = 0
+    cmd_plan = None
+    articulation_controller = robot.get_articulation_controller()
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+    if not args.use_debug_draw:
+        voxel_viewer = VoxelManager(100, size=render_voxel_size)
+    cmd_step_idx = 0
+    while simulation_app.is_running():
+        my_world.step(render=True)
+
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if step_index % 5 == 0.0:
+            # camera data updation
+            world_model.decay_layer("world")
+            data = realsense_data.get_data()
+            clip_camera(data)
+            cube_position, cube_orientation = camera_marker.get_local_pose()
+            camera_pose = Pose(
+                position=tensor_args.to_device(cube_position),
+                quaternion=tensor_args.to_device(cube_orientation),
+            )
+
+            data_camera = CameraObservation(  # rgb_image = data["rgba_nvblox"],
+                depth_image=data["depth"], intrinsics=data["intrinsics"], pose=camera_pose
+            )
+            data_camera = data_camera.to(device=tensor_args.device)
+            world_model.add_camera_frame(data_camera, "world")
+            world_model.process_camera_frames("world", False)
+            torch.cuda.synchronize()
+            world_model.update_blox_hashes()
+
+            bounding = Cuboid("t", dims=[1, 1, 1.0], pose=[0, 0, 0, 1, 0, 0, 0])
+            voxels = world_model.get_voxels_in_bounding_box(bounding, voxel_size)
+            if voxels.shape[0] > 0:
+                voxels = voxels[voxels[:, 2] > voxel_size]
+                voxels = voxels[voxels[:, 0] > 0.0]
+                if args.use_debug_draw:
+                    draw_points(voxels)
+                else:
+                    voxels = voxels.cpu().numpy()
+                    voxel_viewer.update_voxels(voxels[:, :3])
+
+                voxel_viewer.update_voxels(voxels[:, :3])
+            else:
+                if not args.use_debug_draw:
+                    voxel_viewer.clear()
+            # draw_points(voxels)
+
+        if True:
+            depth_image = data["raw_depth"]
+            color_image = data["raw_rgb"]
+            depth_colormap = cv2.applyColorMap(
+                cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS
+            )
+            color_image = cv2.flip(color_image, 1)
+            depth_colormap = cv2.flip(depth_colormap, 1)
+
+            images = np.hstack((color_image, depth_colormap))
+
+            cv2.namedWindow("NVBLOX Example", cv2.WINDOW_NORMAL)
+            cv2.imshow("NVBLOX Example", images)
+            key = cv2.waitKey(1)
+            # Press esc or 'q' to close the image window
+            if key & 0xFF == ord("q") or key == 27:
+                cv2.destroyAllWindows()
+                break
+
+        if cmd_plan is None and step_index % 10 == 0:
+            # motion generation:
+            for ks in range(len(target_material_list)):
+                if ks == target_idx:
+                    target_material_list[ks].set_color(np.ravel([0, 1.0, 0]))
+                else:
+                    target_material_list[ks].set_color(np.ravel([0.1, 0.1, 0.1]))
+
+            sim_js = robot.get_joints_state()
+            sim_js_names = robot.dof_names
+            cu_js = JointState(
+                position=tensor_args.to_device(sim_js.positions),
+                velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+                acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+                jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+                joint_names=sim_js_names,
+            )
+            cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+            cube_position, cube_orientation = target_list[target_idx].get_world_pose()
+
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+
+            result = motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, plan_config)
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+                target_idx += 1
+                if target_idx >= len(target_list):
+                    target_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_step_idx += 1
+            if cmd_step_idx == 2:
+                cmd_idx += 1
+                cmd_step_idx = 0
+            # for _ in range(2):
+            #    my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    realsense_data.stop_device()
+    print("finished program")
+
+    simulation_app.close()

examples/isaac_sim/realsense_viewer.py

@@ -0,0 +1,46 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import cv2
+import numpy as np
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+
+
+def view_realsense():
+    realsense_data = RealsenseDataloader(clipping_distance_m=1.0)
+    # Streaming loop
+    try:
+        while True:
+            data = realsense_data.get_raw_data()
+            depth_image = data[0]
+            color_image = data[1]
+            # Render images:
+            #   depth align to color on left
+            #   depth on right
+            depth_colormap = cv2.applyColorMap(
+                cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_JET
+            )
+            images = np.hstack((color_image, depth_colormap))
+
+            cv2.namedWindow("Align Example", cv2.WINDOW_NORMAL)
+            cv2.imshow("Align Example", images)
+            key = cv2.waitKey(1)
+            # Press esc or 'q' to close the image window
+            if key & 0xFF == ord("q") or key == 27:
+                cv2.destroyAllWindows()
+                break
+    finally:
+        realsense_data.stop_device()
+
+
+if __name__ == "__main__":
+    view_realsense()

examples/isaac_sim/simple_stacking.py

@@ -0,0 +1,487 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(
+    4, device="cuda:0"
+)  # this is necessary to allow isaac sim to use this torch instance
+# Third Party
+import numpy as np
+
+np.set_printoptions(suppress=True)
+# Standard Library
+import os.path as osp
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp({"headless": False})  # This adds paths for the following importing
+# Standard Library
+from typing import Optional
+
+# Third Party
+import carb
+from omni.isaac.core import World
+from omni.isaac.core.controllers import BaseController
+from omni.isaac.core.tasks import Stacking as BaseStacking
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.prims import is_prim_path_valid
+from omni.isaac.core.utils.stage import get_stage_units
+from omni.isaac.core.utils.string import find_unique_string_name
+from omni.isaac.core.utils.types import ArticulationAction
+from omni.isaac.core.utils.viewports import set_camera_view
+from omni.isaac.franka import Franka
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.sphere_fit import SphereFitType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import (
+    MotionGen,
+    MotionGenConfig,
+    MotionGenPlanConfig,
+    MotionGenResult,
+)
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+ext_list = [
+    "omni.kit.asset_converter",
+    "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+    # "omni.kit.window.sequencer.scripts.sequencer_extension",
+    "omni.kit.window.movie_capture",
+]
+[enable_extension(x) for x in ext_list]  # toggle this for remote streaming
+simulation_app.update()
+
+
+class CuroboController(BaseController):
+    def __init__(
+        self,
+        my_world: World,
+        my_task: BaseStacking,
+        name: str = "curobo_controller",
+    ) -> None:
+        BaseController.__init__(self, name=name)
+        self._save_log = False
+        self.my_world = my_world
+        self.my_task = my_task
+        n_obstacle_cuboids = 20
+        n_obstacle_mesh = 2
+        # warmup curobo instance
+        self.usd_help = UsdHelper()
+        self.init_curobo = False
+        self.world_file = "collision_table.yml"
+        self.cmd_js_names = [
+            "panda_joint1",
+            "panda_joint2",
+            "panda_joint3",
+            "panda_joint4",
+            "panda_joint5",
+            "panda_joint6",
+            "panda_joint7",
+        ]
+        self.tensor_args = TensorDeviceType()
+        self.robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+        self.robot_cfg["kinematics"][
+            "base_link"
+        ] = "panda_link0"  # controls which frame the controller is controlling
+
+        self.robot_cfg["kinematics"][
+            "ee_link"
+        ] = "panda_hand"  # controls which frame the controller is controlling
+        # self.robot_cfg["kinematics"]["cspace"]["max_acceleration"] = 10.0 # controls how fast robot moves
+        self.robot_cfg["kinematics"]["extra_collision_spheres"] = {"attached_object": 100}
+        self.robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0
+        self.robot_cfg["kinematics"]["collision_spheres"] = "spheres/franka_collision_mesh.yml"
+
+        world_cfg_table = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+        )
+        self._world_cfg_table = world_cfg_table
+
+        world_cfg1 = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+        ).get_mesh_world()
+        world_cfg1.mesh[0].pose[2] = -10.5
+
+        self._world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+        motion_gen_config = MotionGenConfig.load_from_robot_config(
+            self.robot_cfg,
+            self._world_cfg,
+            self.tensor_args,
+            trajopt_tsteps=32,
+            collision_checker_type=CollisionCheckerType.MESH,
+            use_cuda_graph=True,
+            num_ik_seeds=40,
+            num_trajopt_seeds=10,
+            num_graph_seeds=10,
+            interpolation_dt=0.01,
+            collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+            store_ik_debug=self._save_log,
+            store_trajopt_debug=self._save_log,
+            state_finite_difference_mode="CENTRAL",
+            minimize_jerk=True,
+            acceleration_scale=0.3,
+            fixed_iters_trajopt=True,
+        )
+        self.motion_gen = MotionGen(motion_gen_config)
+        print("warming up...")
+        self.motion_gen.warmup()
+
+        self.plan_config = MotionGenPlanConfig(
+            enable_graph=False,
+            max_attempts=10,
+            enable_graph_attempt=None,
+            enable_finetune_trajopt=True,
+            partial_ik_opt=False,
+        )
+        self.usd_help.load_stage(self.my_world.stage)
+        self.cmd_plan = None
+        self.cmd_idx = 0
+        self.idx_list = None
+
+    def attach_obj(
+        self,
+        sim_js: JointState,
+        js_names: list,
+    ) -> None:
+        cube_name = self.my_task.get_cube_prim(self.my_task.target_cube)
+
+        cu_js = JointState(
+            position=self.tensor_args.to_device(sim_js.positions),
+            velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=js_names,
+        )
+
+        self.motion_gen.attach_objects_to_robot(
+            cu_js,
+            [cube_name],
+            sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE,
+            world_objects_pose_offset=Pose.from_list([0, 0, 0.005, 1, 0, 0, 0], self.tensor_args),
+        )
+
+    def detach_obj(self) -> None:
+        self.motion_gen.detach_object_from_robot()
+
+    def plan(
+        self,
+        ee_translation_goal: np.array,
+        ee_orientation_goal: np.array,
+        sim_js: JointState,
+        js_names: list,
+    ) -> MotionGenResult:
+        ik_goal = Pose(
+            position=self.tensor_args.to_device(ee_translation_goal),
+            quaternion=self.tensor_args.to_device(ee_orientation_goal),
+        )
+        cu_js = JointState(
+            position=self.tensor_args.to_device(sim_js.positions),
+            velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(self.motion_gen.kinematics.joint_names)
+        result = self.motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, self.plan_config.clone())
+        if self._save_log:  # and not result.success.item(): # logging for debugging
+            UsdHelper.write_motion_gen_log(
+                result,
+                {"robot_cfg": self.robot_cfg},
+                self._world_cfg,
+                cu_js,
+                ik_goal,
+                join_path("log/usd/", "cube") + "_debug",
+                write_ik=False,
+                write_trajopt=True,
+                visualize_robot_spheres=True,
+                link_spheres=self.motion_gen.kinematics.kinematics_config.link_spheres,
+                grid_space=2,
+                write_robot_usd_path="log/usd/assets",
+            )
+        return result
+
+    def forward(
+        self,
+        sim_js: JointState,
+        js_names: list,
+    ) -> ArticulationAction:
+        assert self.my_task.target_position is not None
+        assert self.my_task.target_cube is not None
+
+        if self.cmd_plan is None:
+            self.cmd_idx = 0
+            # Set EE goals
+            ee_translation_goal = self.my_task.target_position
+            ee_orientation_goal = np.array([0, 0, -1, 0])
+            # compute curobo solution:
+            result = self.plan(ee_translation_goal, ee_orientation_goal, sim_js, js_names)
+            succ = result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                self.idx_list = [i for i in range(len(self.cmd_js_names))]
+                self.cmd_plan = cmd_plan.get_ordered_joint_state(self.cmd_js_names)
+            else:
+                carb.log_warn("Plan did not converge to a solution.")
+                return None
+
+        cmd_state = self.cmd_plan[self.cmd_idx]
+        self.cmd_idx += 1
+        # get full dof state
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=self.idx_list,
+        )
+        if self.cmd_idx >= len(self.cmd_plan.position):
+            self.cmd_idx = 0
+            self.cmd_plan = None
+
+        return art_action
+
+    def reached_target(self, observations: dict) -> bool:
+        curr_ee_position = observations["my_franka"]["end_effector_position"]
+        if np.linalg.norm(
+            self.my_task.target_position - curr_ee_position
+        ) < 0.04 and (  # This is half gripper width, curobo succ threshold is 0.5 cm
+            self.cmd_plan is None
+        ):
+            if self.my_task.cube_in_hand is None:
+                print("reached picking target: ", self.my_task.target_cube)
+            else:
+                print("reached placing target: ", self.my_task.target_cube)
+            return True
+        else:
+            return False
+
+    def reset(
+        self,
+        ignore_substring: str,
+        robot_prim_path: str,
+    ) -> None:
+        # init
+        self.update(ignore_substring, robot_prim_path)
+        self.init_curobo = True
+        self.cmd_plan = None
+        self.cmd_idx = 0
+
+    def update(
+        self,
+        ignore_substring: str,
+        robot_prim_path: str,
+    ) -> None:
+        # print("updating world...")
+        obstacles = self.usd_help.get_obstacles_from_stage(
+            ignore_substring=ignore_substring, reference_prim_path=robot_prim_path
+        ).get_collision_check_world()
+        # add ground plane as it's not readable:
+        obstacles.add_obstacle(self._world_cfg_table.cuboid[0])
+        self.motion_gen.update_world(obstacles)
+        self._world_cfg = obstacles
+
+
+class MultiModalStacking(BaseStacking):
+    def __init__(
+        self,
+        name: str = "multi_modal_stacking",
+        offset: Optional[np.ndarray] = None,
+    ) -> None:
+        BaseStacking.__init__(
+            self,
+            name=name,
+            cube_initial_positions=np.array(
+                [
+                    [0.50, 0.0, 0.1],
+                    [0.50, -0.20, 0.1],
+                    [0.50, 0.20, 0.1],
+                    [0.30, -0.20, 0.1],
+                    [0.30, 0.0, 0.1],
+                    [0.30, 0.20, 0.1],
+                    [0.70, -0.20, 0.1],
+                    [0.70, 0.0, 0.1],
+                    [0.70, 0.20, 0.1],
+                ]
+            )
+            / get_stage_units(),
+            cube_initial_orientations=None,
+            stack_target_position=None,
+            cube_size=np.array([0.045, 0.045, 0.07]),
+            offset=offset,
+        )
+        self.cube_list = None
+        self.target_position = None
+        self.target_cube = None
+        self.cube_in_hand = None
+
+    def reset(self) -> None:
+        self.cube_list = self.get_cube_names()
+        self.target_position = None
+        self.target_cube = None
+        self.cube_in_hand = None
+
+    def update_task(self) -> bool:
+        # after detaching the cube in hand
+        assert self.target_cube is not None
+        assert self.cube_in_hand is not None
+        self.cube_list.insert(0, self.cube_in_hand)
+        self.target_cube = None
+        self.target_position = None
+        self.cube_in_hand = None
+        if len(self.cube_list) <= 1:
+            task_finished = True
+        else:
+            task_finished = False
+        return task_finished
+
+    def get_cube_prim(self, cube_name: str):
+        for i in range(self._num_of_cubes):
+            if cube_name == self._cubes[i].name:
+                return self._cubes[i].prim_path
+
+    def get_place_position(self, observations: dict) -> None:
+        assert self.target_cube is not None
+        self.cube_in_hand = self.target_cube
+        self.target_cube = self.cube_list[0]
+        ee_to_grasped_cube = (
+            observations["my_franka"]["end_effector_position"][2]
+            - observations[self.cube_in_hand]["position"][2]
+        )
+        self.target_position = observations[self.target_cube]["position"] + [
+            0,
+            0,
+            self._cube_size[2] + ee_to_grasped_cube + 0.02,
+        ]
+        self.cube_list.remove(self.target_cube)
+
+    def get_pick_position(self, observations: dict) -> None:
+        assert self.cube_in_hand is None
+        self.target_cube = self.cube_list[1]
+        self.target_position = observations[self.target_cube]["position"] + [
+            0,
+            0,
+            self._cube_size[2] / 2 + 0.092,
+        ]
+        self.cube_list.remove(self.target_cube)
+
+    def set_robot(self) -> Franka:
+        franka_prim_path = find_unique_string_name(
+            initial_name="/World/Franka", is_unique_fn=lambda x: not is_prim_path_valid(x)
+        )
+        franka_robot_name = find_unique_string_name(
+            initial_name="my_franka", is_unique_fn=lambda x: not self.scene.object_exists(x)
+        )
+        return Franka(
+            prim_path=franka_prim_path, name=franka_robot_name, end_effector_prim_name="panda_hand"
+        )
+
+
+robot_prim_path = "/World/Franka/panda_link0"
+ignore_substring = ["Franka", "TargetCube", "material", "Plane"]
+my_world = World(stage_units_in_meters=1.0)
+stage = my_world.stage
+stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+my_task = MultiModalStacking()
+my_world.add_task(my_task)
+my_world.reset()
+robot_name = my_task.get_params()["robot_name"]["value"]
+my_franka = my_world.scene.get_object(robot_name)
+my_controller = CuroboController(my_world=my_world, my_task=my_task)
+articulation_controller = my_franka.get_articulation_controller()
+set_camera_view(eye=[2, 0, 1], target=[0.00, 0.00, 0.00], camera_prim_path="/OmniverseKit_Persp")
+wait_steps = 8
+
+initial_steps = 10
+################################################################
+print("Start simulation...")
+my_franka.gripper.open()
+for _ in range(wait_steps):
+    my_world.step(render=True)
+my_task.reset()
+task_finished = False
+observations = my_world.get_observations()
+my_task.get_pick_position(observations)
+robot = my_franka
+print("**********************")
+# print(robot.get_solver_position_iteration_count(), robot.get_solver_velocity_iteration_count())
+robot.enable_gravity()
+robot._articulation_view.set_gains(
+    kps=np.array([100000000, 10000000]), joint_indices=np.array([0, 2])
+)
+
+robot._articulation_view.set_max_efforts(
+    values=np.array([10000, 10000]), joint_indices=np.array([0, 2])
+)
+i = 0
+while simulation_app.is_running():
+    my_world.step(render=True)  # necessary to visualize changes
+    i += 1
+
+    if task_finished or i < initial_steps:
+        continue
+
+    if not my_controller.init_curobo:
+        my_controller.reset(ignore_substring, robot_prim_path)
+
+    step_index = my_world.current_time_step_index
+    observations = my_world.get_observations()
+    sim_js = my_franka.get_joints_state()
+
+    if my_controller.reached_target(observations):
+        if my_franka.gripper.get_joint_positions()[0] < 0.035:  # reached placing target
+            my_franka.gripper.open()
+            for _ in range(wait_steps):
+                my_world.step(render=True)
+            my_controller.detach_obj()
+            my_controller.update(
+                ignore_substring, robot_prim_path
+            )  # update world collision configuration
+            task_finished = my_task.update_task()
+            if task_finished:
+                print("\nTASK DONE\n")
+                for _ in range(wait_steps):
+                    my_world.step(render=True)
+                continue
+            else:
+                my_task.get_pick_position(observations)
+
+        else:  # reached picking target
+            my_franka.gripper.close()
+            for _ in range(wait_steps):
+                my_world.step(render=True)
+            sim_js = my_franka.get_joints_state()
+            my_controller.update(ignore_substring, robot_prim_path)
+            my_controller.attach_obj(sim_js, my_franka.dof_names)
+            my_task.get_place_position(observations)
+
+    else:  # target position has been set
+        sim_js = my_franka.get_joints_state()
+        art_action = my_controller.forward(sim_js, my_franka.dof_names)
+        if art_action is not None:
+            articulation_controller.apply_action(art_action)
+            for _ in range(2):
+                my_world.step(render=False)
+
+simulation_app.close()

examples/isaac_sim/util/convert_urdf_to_usd.py

@@ -0,0 +1,172 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--robot",
+    type=str,
+    default="franka.yml",
+    help="Robot configuration to download",
+)
+parser.add_argument("--save_usd", default=False, action="store_true")
+args = parser.parse_args()
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp({"headless": args.save_usd})
+
+# Third Party
+import omni.usd
+from omni.isaac.core import World
+from omni.isaac.core.robots import Robot
+from omni.isaac.core.utils.types import ArticulationAction
+
+try:
+    # Third Party
+    from omni.isaac.urdf import _urdf  # isaacsim 2022.2
+except ImportError:
+    from omni.importer.urdf import _urdf  # isaac sim 2023.1
+
+# CuRobo
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+
+def save_usd():
+    my_world = World(stage_units_in_meters=1.0)
+
+    import_config = _urdf.ImportConfig()
+    import_config.merge_fixed_joints = False
+    import_config.convex_decomp = False
+    import_config.import_inertia_tensor = True
+    import_config.fix_base = True
+    import_config.make_default_prim = True
+    import_config.self_collision = False
+    import_config.create_physics_scene = True
+    import_config.import_inertia_tensor = False
+    import_config.default_drive_strength = 10000
+    import_config.default_position_drive_damping = 100
+    import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
+    import_config.distance_scale = 1
+    import_config.density = 0.0
+    # Get the urdf file path
+    robot_config = load_yaml(join_path(get_robot_configs_path(), args.robot))
+    urdf_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    asset_path = join_path(
+        get_assets_path(), robot_config["robot_cfg"]["kinematics"]["asset_root_path"]
+    )
+    urdf_interface = _urdf.acquire_urdf_interface()
+    full_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    default_config = robot_config["robot_cfg"]["kinematics"]["cspace"]["retract_config"]
+    j_names = robot_config["robot_cfg"]["kinematics"]["cspace"]["joint_names"]
+
+    robot_path = get_path_of_dir(full_path)
+    filename = get_filename(full_path)
+    imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)
+    robot_path = urdf_interface.import_robot(
+        robot_path, filename, imported_robot, import_config, ""
+    )
+    robot = my_world.scene.add(Robot(prim_path=robot_path, name="robot"))
+    # robot.disable_gravity()
+    i = 0
+
+    my_world.reset()
+
+    usd_help = UsdHelper()
+    usd_help.load_stage(my_world.stage)
+    save_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["usd_path"])
+    usd_help.write_stage_to_file(save_path, True)
+    print("Wrote usd file to " + save_path)
+    simulation_app.close()
+
+
+def debug_usd():
+    my_world = World(stage_units_in_meters=1.0)
+
+    import_config = _urdf.ImportConfig()
+    import_config.merge_fixed_joints = False
+    import_config.convex_decomp = False
+    import_config.import_inertia_tensor = True
+    import_config.fix_base = True
+    import_config.make_default_prim = True
+    import_config.self_collision = False
+    import_config.create_physics_scene = True
+    import_config.import_inertia_tensor = False
+    import_config.default_drive_strength = 10000
+    import_config.default_position_drive_damping = 100
+    import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
+    import_config.distance_scale = 1
+    import_config.density = 0.0
+    # Get the urdf file path
+    robot_config = load_yaml(join_path(get_robot_configs_path(), args.robot))
+    urdf_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    asset_path = join_path(
+        get_assets_path(), robot_config["robot_cfg"]["kinematics"]["asset_root_path"]
+    )
+    urdf_interface = _urdf.acquire_urdf_interface()
+    full_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    default_config = robot_config["robot_cfg"]["kinematics"]["cspace"]["retract_config"]
+    j_names = robot_config["robot_cfg"]["kinematics"]["cspace"]["joint_names"]
+
+    robot_path = get_path_of_dir(full_path)
+    filename = get_filename(full_path)
+    imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)
+    robot_path = urdf_interface.import_robot(
+        robot_path, filename, imported_robot, import_config, ""
+    )
+    robot = my_world.scene.add(Robot(prim_path=robot_path, name="robot"))
+    # robot.disable_gravity()
+    i = 0
+
+    articulation_controller = robot.get_articulation_controller()
+    my_world.reset()
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if i == 0:
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+            i += 1
+        # if dof_n is not None:
+        #    dof_i = [robot.get_dof_index(x) for x in j_names]
+        #
+        #    robot.set_joint_positions(default_config, dof_i)
+        if robot.is_valid():
+            art_action = ArticulationAction(default_config, joint_indices=idx_list)
+            articulation_controller.apply_action(art_action)
+    usd_help = UsdHelper()
+    usd_help.load_stage(my_world.stage)
+    save_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["usd_path"])
+    usd_help.write_stage_to_file(save_path, True)
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    if args.save_usd:
+        save_usd()
+    else:
+        debug_usd()

examples/isaac_sim/util/dowload_assets.py

@@ -0,0 +1,69 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# This script downloads robot usd assets from isaac sim for using in CuRobo.
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp({"headless": True})
+# Third Party
+from omni.isaac.core import World
+from omni.isaac.core.robots import Robot
+from omni.isaac.core.utils.nucleus import get_assets_root_path as nucleus_path
+from omni.isaac.core.utils.stage import add_reference_to_stage
+
+# CuRobo
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import get_assets_path, get_robot_configs_path, join_path, load_yaml
+
+# supported robots:
+robots = ["franka.yml", "ur10.yml"]
+# Standard Library
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--robot",
+    type=str,
+    default="franka.yml",
+    help="Robot configuration to download",
+)
+args = parser.parse_args()
+
+if __name__ == "__main__":
+    r = args.robot
+    my_world = World(stage_units_in_meters=1.0)
+    robot_config = load_yaml(join_path(get_robot_configs_path(), r))
+    usd_path = nucleus_path() + robot_config["robot_cfg"]["kinematics"]["isaac_usd_path"]
+
+    usd_help = UsdHelper()
+    robot_name = r
+    prim_path = robot_config["robot_cfg"]["kinematics"]["usd_robot_root"]
+    add_reference_to_stage(usd_path=usd_path, prim_path=prim_path)
+    robot = my_world.scene.add(Robot(prim_path=prim_path, name=robot_name))
+    usd_help.load_stage(my_world.stage)
+
+    my_world.reset()
+    articulation_controller = robot.get_articulation_controller()
+
+    # create a new stage and add robot to usd path:
+    save_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["usd_path"])
+    usd_help.write_stage_to_file(save_path, True)
+    my_world.clear()
+    my_world.clear_instance()
+    simulation_app.close()

examples/kinematics_example.py

@@ -0,0 +1,66 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+""" Example computing fk using curobo
+
+"""
+# Third Party
+import torch
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.robot import RobotConfig
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import get_robot_path, join_path, load_yaml
+
+
+def demo_basic_robot():
+    tensor_args = TensorDeviceType()
+
+    # load a urdf:
+    config_file = load_yaml(join_path(get_robot_path(), "franka.yml"))
+
+    urdf_file = config_file["robot_cfg"]["kinematics"][
+        "urdf_path"
+    ]  # Send global path starting with "/"
+    base_link = config_file["robot_cfg"]["kinematics"]["base_link"]
+    ee_link = config_file["robot_cfg"]["kinematics"]["ee_link"]
+    robot_cfg = RobotConfig.from_basic(urdf_file, base_link, ee_link, tensor_args)
+
+    kin_model = CudaRobotModel(robot_cfg.kinematics)
+
+    # compute forward kinematics:
+
+    q = torch.rand((10, kin_model.get_dof()), **vars(tensor_args))
+    out = kin_model.get_state(q)
+    # here is the kinematics state:
+    # print(out)
+
+
+def demo_full_config_robot():
+    setup_curobo_logger("info")
+    tensor_args = TensorDeviceType()
+    # load a urdf:
+    config_file = load_yaml(join_path(get_robot_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(config_file, tensor_args)
+
+    kin_model = CudaRobotModel(robot_cfg.kinematics)
+
+    # compute forward kinematics:
+    q = torch.rand((10, kin_model.get_dof()), **vars(tensor_args))
+    out = kin_model.get_state(q)
+    # here is the kinematics state:
+    # print(out)
+
+
+if __name__ == "__main__":
+    demo_basic_robot()
+    demo_full_config_robot()

examples/motion_gen_api_example.py

@@ -0,0 +1,101 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+# CuRobo
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState
+from curobo.util.usd_helper import UsdHelper
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig
+
+
+def demo_motion_gen_api():
+    tensor_args = TensorDeviceType(device=torch.device("cuda:0"))
+
+    interpolation_dt = 0.02
+    collision_activation_distance = 0.02  # meters
+    # create motion gen with a cuboid cache to be able to load obstacles later:
+    motion_gen_cfg = MotionGenConfig.load_from_robot_config(
+        "franka.yml",
+        "collision_table.yml",
+        tensor_args,
+        trajopt_tsteps=34,
+        interpolation_steps=5000,
+        num_ik_seeds=50,
+        num_trajopt_seeds=6,
+        grad_trajopt_iters=500,
+        trajopt_dt=0.5,
+        interpolation_dt=interpolation_dt,
+        evaluate_interpolated_trajectory=True,
+        js_trajopt_dt=0.5,
+        js_trajopt_tsteps=34,
+        collision_activation_distance=collision_activation_distance,
+    )
+    motion_gen = MotionGen(motion_gen_cfg)
+
+    motion_gen.warmup()
+
+    # create world representation:
+    motion_gen.world_coll_checker.clear_cache()
+    motion_gen.reset(reset_seed=False)
+    cuboids = [Cuboid(name="obs_1", pose=[0.9, 0.0, 0.5, 1, 0, 0, 0], dims=[0.1, 0.5, 0.5])]
+    world = WorldConfig(cuboid=cuboids)
+
+    motion_gen.update_world(world)
+
+    q_start = JointState.from_position(
+        tensor_args.to_device([[0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0.0]]),
+        joint_names=[
+            "panda_joint1",
+            "panda_joint2",
+            "panda_joint3",
+            "panda_joint4",
+            "panda_joint5",
+            "panda_joint6",
+            "panda_joint7",
+        ],
+    )
+
+    goal_pose = Pose(
+        position=tensor_args.to_device([[0.5, 0.0, 0.3]]),
+        quaternion=tensor_args.to_device([[1, 0, 0, 0]]),
+    )
+
+    result = motion_gen.plan_single(q_start, goal_pose)
+
+    if result.success.item():
+        # get result:
+        # this contains trajectory with 34 tsteps and the final
+        # result.optimized_plan
+        # result.optimized_dt
+
+        # this contains a linearly interpolated trajectory with fixed dt
+        interpolated_solution = result.get_interpolated_plan()
+
+        UsdHelper.write_trajectory_animation_with_robot_usd(
+            "franka.yml",
+            world,
+            q_start,
+            interpolated_solution,
+            dt=result.interpolation_dt,
+            save_path="demo.usd",
+            base_frame="/world_base",
+        )
+    else:
+        print("Failed")
+
+
+if __name__ == "__main__":
+    demo_motion_gen_api()

examples/motion_gen_example.py

@@ -0,0 +1,366 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+
+# Third Party
+import torch
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+
+def plot_traj(trajectory, dt):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(4, 1)
+    q = trajectory.position.cpu().numpy()
+    qd = trajectory.velocity.cpu().numpy()
+    qdd = trajectory.acceleration.cpu().numpy()
+    qddd = trajectory.jerk.cpu().numpy()
+    timesteps = [i * dt for i in range(q.shape[0])]
+    for i in range(q.shape[-1]):
+        axs[0].plot(timesteps, q[:, i], label=str(i))
+        axs[1].plot(timesteps, qd[:, i], label=str(i))
+        axs[2].plot(timesteps, qdd[:, i], label=str(i))
+        axs[3].plot(timesteps, qddd[:, i], label=str(i))
+
+    plt.legend()
+    # plt.savefig("test.png")
+    plt.show()
+
+
+def plot_iters_traj(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(len(trajectory), 1)
+    if len(trajectory) == 1:
+        axs = [axs]
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            axs[k].plot(
+                q[i][seed, :-1, d_id].cpu(),
+                "r+-",
+                label=str(i),
+                alpha=0.1 + min(0.9, float(i) / (len(q))),
+            )
+    plt.legend()
+    plt.show()
+
+
+def plot_iters_traj_3d(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    ax = plt.axes(projection="3d")
+    c = 0
+    h = trajectory[0][0].shape[1] - 1
+    x = [x for x in range(h)]
+
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            # ax.plot3D(x,[c for _ in range(h)],  q[i][seed, :, d_id].cpu())#, 'r')
+            ax.scatter3D(
+                x, [c for _ in range(h)], q[i][seed, :h, d_id].cpu(), c=q[i][seed, :, d_id].cpu()
+            )
+            # @plt.show()
+            c += 1
+    # plt.legend()
+    plt.show()
+
+
+def demo_motion_gen_mesh():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_mesh_scene.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=40,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = robot_cfg.retract_config
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.5)
+    result = motion_gen.plan(
+        start_state,
+        retract_pose,
+        enable_graph=False,
+        enable_opt=True,
+        max_attempts=1,
+        num_trajopt_seeds=10,
+        num_graph_seeds=10,
+    )
+    print(result.status, result.attempts, result.trajopt_time)
+    traj = result.raw_plan  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    if PLOT:
+        plot_traj(traj.cpu().numpy())
+
+
+def demo_motion_gen():
+    # Standard Library
+    PLOT = False
+    tensor_args = TensorDeviceType()
+    world_file = "collision_table.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        interpolation_dt=0.01,
+    )
+
+    motion_gen = MotionGen(motion_gen_config)
+
+    motion_gen.warmup(enable_graph=False)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = motion_gen.get_retract_config()
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
+    result = motion_gen.plan_single(start_state, retract_pose, MotionGenPlanConfig(max_attempts=1))
+    traj = result.get_interpolated_plan()
+    print("Trajectory Generated: ", result.success, result.optimized_dt.item())
+    if PLOT:
+        plot_traj(traj, result.interpolation_dt)
+        # plt.save("test.png")
+        # plt.close()
+        # traj = result.debug_info["opt_solution"].position.view(-1,7)
+        # plot_traj(traj.cpu().numpy())
+
+
+def demo_motion_gen_debug():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_cubby.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=24,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=True,
+        num_trajopt_seeds=1,
+        num_graph_seeds=1,
+        store_ik_debug=True,
+        store_trajopt_debug=True,
+        trajopt_particle_opt=False,
+        grad_trajopt_iters=100,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = robot_cfg.cspace.retract_config
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.4)
+    result = motion_gen.plan(start_state, retract_pose, enable_graph=True, enable_opt=True)
+    if result.status not in [None, "Opt Fail"]:
+        return
+    traj = result.plan.view(-1, 7)  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    trajectory_iter_steps = result.debug_info["trajopt_result"].debug_info["solver"]["steps"]
+
+    if PLOT:
+        plot_iters_traj_3d(trajectory_iter_steps, d_id=6)
+        # plot_traj(traj.cpu().numpy())
+
+
+def demo_motion_gen_batch():
+    PLOT = False
+    tensor_args = TensorDeviceType()
+    world_file = "collision_cubby.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=30,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=True,
+        num_trajopt_seeds=4,
+        num_graph_seeds=1,
+        num_ik_seeds=30,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = motion_gen.get_retract_config()
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.6)
+
+    retract_pose = retract_pose.repeat_seeds(2)
+    retract_pose.position[0, 0] = -0.3
+    for _ in range(2):
+        result = motion_gen.plan_batch(
+            start_state.repeat_seeds(2),
+            retract_pose,
+            MotionGenPlanConfig(enable_graph=False, enable_opt=True),
+        )
+    traj = result.optimized_plan.position.view(2, -1, 7)  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    if PLOT:
+        plot_traj(traj[0, : result.path_buffer_last_tstep[0], :].cpu().numpy())
+        plot_traj(traj[1, : result.path_buffer_last_tstep[1], :].cpu().numpy())
+
+
+def demo_motion_gen_api():
+    tensor_args = TensorDeviceType(device=torch.device("cuda:0"))
+    interpolation_dt = 0.02
+    # create motion gen with a cuboid cache to be able to load obstacles later:
+    motion_gen_cfg = MotionGenConfig.load_from_robot_config(
+        "franka.yml",
+        "collision_table.yml",
+        tensor_args,
+        trajopt_tsteps=34,
+        interpolation_steps=5000,
+        num_ik_seeds=50,
+        num_trajopt_seeds=6,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        grad_trajopt_iters=500,
+        trajopt_dt=0.5,
+        interpolation_dt=interpolation_dt,
+        evaluate_interpolated_trajectory=True,
+        js_trajopt_dt=0.5,
+        js_trajopt_tsteps=34,
+    )
+    motion_gen = MotionGen(motion_gen_cfg)
+
+    motion_gen.warmup(warmup_js_trajopt=False)
+
+    # create world representation:
+
+    cuboids = [Cuboid(name="obs_1", pose=[0, 0, 0, 1, 0, 0, 0], dims=[0.1, 0.1, 0.1])]
+    world = WorldConfig(cuboid=cuboids)
+
+    motion_gen.update_world(world)
+
+    q_start = JointState.from_position(
+        tensor_args.to_device([[0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0.0]]),
+        joint_names=[
+            "panda_joint1",
+            "panda_joint2",
+            "panda_joint3",
+            "panda_joint4",
+            "panda_joint5",
+            "panda_joint6",
+            "panda_joint7",
+        ],
+    )
+
+    goal_pose = Pose(
+        position=tensor_args.to_device([[0.5, 0.0, 0.3]]),
+        quaternion=tensor_args.to_device([[1, 0, 0, 0]]),
+    )
+    print(goal_pose)
+
+    result = motion_gen.plan_single(q_start, goal_pose)
+
+    # get result:
+
+    interpolated_solution = result.get_interpolated_plan()
+
+    print(result.get_interpolated_plan())
+
+
+def demo_motion_gen_batch_env(n_envs: int = 10):
+    tensor_args = TensorDeviceType()
+    world_files = ["collision_table.yml" for _ in range(int(n_envs / 2))] + [
+        "collision_test.yml" for _ in range(int(n_envs / 2))
+    ]
+
+    world_cfg = [
+        WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+        for world_file in world_files
+    ]
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=30,
+        use_cuda_graph=False,
+        num_trajopt_seeds=4,
+        num_ik_seeds=30,
+        num_batch_ik_seeds=30,
+        evaluate_interpolated_trajectory=True,
+        interpolation_dt=0.05,
+        interpolation_steps=500,
+        grad_trajopt_iters=30,
+    )
+    motion_gen_batch_env = MotionGen(motion_gen_config)
+    motion_gen_batch_env.reset()
+    motion_gen_batch_env.warmup(
+        enable_graph=False, batch=n_envs, warmup_js_trajopt=False, batch_env_mode=True
+    )
+    retract_cfg = motion_gen_batch_env.get_retract_config().clone()
+    state = motion_gen_batch_env.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    goal_pose = Pose(
+        state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze()
+    ).repeat_seeds(n_envs)
+
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3).repeat_seeds(n_envs)
+
+    goal_pose.position[1, 0] -= 0.2
+
+    m_config = MotionGenPlanConfig(
+        False, True, max_attempts=1, enable_graph_attempt=None, enable_finetune_trajopt=False
+    )
+    result = motion_gen_batch_env.plan_batch_env(start_state, goal_pose, m_config)
+
+    print(n_envs, result.total_time, result.total_time / n_envs)
+
+
+if __name__ == "__main__":
+    setup_curobo_logger("error")
+    demo_motion_gen()
+    n = [2, 10]
+    for n_envs in n:
+        demo_motion_gen_batch_env(n_envs=n_envs)

examples/motion_gen_profile.py

@@ -0,0 +1,81 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+
+
+# CuRobo
+
+# Third Party
+from torch.profiler import ProfilerActivity, profile
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util_file import get_robot_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig
+
+
+def plot_traj(trajectory):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(1, 1)
+    q = trajectory
+
+    for i in range(q.shape[-1]):
+        axs.plot(q[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def demo_motion_gen():
+    PLOT = False
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=40,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = robot_cfg.retract_config
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.5)
+    result = motion_gen.plan(start_state, retract_pose, enable_graph=False)
+
+    # profile:
+    with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+        result = motion_gen.plan(start_state, retract_pose, enable_graph=False)
+
+    print("Exporting the trace..")
+    prof.export_chrome_trace("trace.json")
+    exit(10)
+    traj = result.raw_plan  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    if PLOT:
+        plot_traj(traj.cpu().numpy())
+
+
+if __name__ == "__main__":
+    demo_motion_gen()

examples/mpc_example.py

@@ -0,0 +1,131 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+import time
+
+# Third Party
+import numpy as np
+import torch
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+
+def plot_traj(trajectory, dof):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(3, 1)
+    q = trajectory[:, :dof]
+    qd = trajectory[:, dof : dof * 2]
+    qdd = trajectory[:, dof * 2 : dof * 3]
+
+    for i in range(q.shape[-1]):
+        axs[0].plot(q[:, i], label=str(i))
+        axs[1].plot(qd[:, i], label=str(i))
+        axs[2].plot(qdd[:, i], label=str(i))
+    plt.legend()
+    plt.savefig("test.png")
+    # plt.show()
+
+
+def demo_full_config_mpc():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_file,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        use_lbfgs=False,
+        use_es=False,
+        use_mppi=True,
+        store_rollouts=True,
+        step_dt=0.03,
+    )
+    mpc = MpcSolver(mpc_config)
+
+    # retract_cfg = robot_cfg.cspace.retract_config.view(1, -1)
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.unsqueeze(0)
+    joint_names = mpc.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg + 0.5, joint_names=joint_names)
+    )
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    start_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+
+    goal = Goal(
+        current_state=start_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+
+    # test_q = tensor_args.to_device( [2.7735, -1.6737,  0.4998, -2.9865,  0.3386,  0.8413,  0.4371])
+    # start_state.position[:] = test_q
+    converged = False
+    tstep = 0
+    traj_list = []
+    mpc_time = []
+    mpc.update_goal(goal_buffer)
+    current_state = start_state  # .clone()
+    while not converged:
+        st_time = time.time()
+        # current_state.position += 0.1
+        print(current_state.position)
+        result = mpc.step(current_state, 1)
+
+        print(mpc.get_visual_rollouts().shape)
+        # exit()
+        torch.cuda.synchronize()
+        if tstep > 5:
+            mpc_time.append(time.time() - st_time)
+        # goal_buffer.current_state.position[:] = result.action.position
+        # result.action.position += 0.1
+        current_state.copy_(result.action)
+        # goal_buffer.current_state.velocity[:] = result.action.vel
+        traj_list.append(result.action.get_state_tensor())
+        tstep += 1
+        # if tstep % 10 == 0:
+        #    print(result.metrics.pose_error.item(), result.solve_time, mpc_time[-1])
+        if result.metrics.pose_error.item() < 0.01:
+            converged = True
+        if tstep > 1000:
+            break
+    print(
+        "MPC (converged, error, steps, opt_time, mpc_time): ",
+        converged,
+        result.metrics.pose_error.item(),
+        tstep,
+        result.solve_time,
+        np.mean(mpc_time),
+    )
+    if PLOT:
+        plot_traj(torch.cat(traj_list, dim=0).cpu().numpy(), dof=retract_cfg.shape[-1])
+
+
+if __name__ == "__main__":
+    demo_full_config_mpc()
+    # demo_full_config_mesh_mpc()

examples/nvblox_example.py

@@ -0,0 +1,120 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+
+
+# CuRobo
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util_file import get_robot_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+
+def plot_traj(trajectory):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(1, 1)
+    q = trajectory
+
+    for i in range(q.shape[-1]):
+        axs.plot(q[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def plot_iters_traj(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(len(trajectory), 1)
+    if len(trajectory) == 1:
+        axs = [axs]
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            axs[k].plot(
+                q[i][seed, :-1, d_id].cpu(),
+                "r+-",
+                label=str(i),
+                alpha=0.1 + min(0.9, float(i) / (len(q))),
+            )
+    plt.legend()
+    plt.show()
+
+
+def plot_iters_traj_3d(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    ax = plt.axes(projection="3d")
+    c = 0
+    h = trajectory[0][0].shape[1] - 1
+    x = [x for x in range(h)]
+
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            # ax.plot3D(x,[c for _ in range(h)],  q[i][seed, :, d_id].cpu())#, 'r')
+            ax.scatter3D(
+                x, [c for _ in range(h)], q[i][seed, :h, d_id].cpu(), c=q[i][seed, :, d_id].cpu()
+            )
+            # @plt.show()
+            c += 1
+    # plt.legend()
+    plt.show()
+
+
+def demo_motion_gen_nvblox():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_nvblox.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=False,
+        num_trajopt_seeds=2,
+        num_graph_seeds=2,
+        evaluate_interpolated_trajectory=True,
+    )
+    goals = tensor_args.to_device(
+        [
+            [0.5881, 0.0589, 0.3055],
+            [0.5881, 0.4155, 0.3055],
+            [0.5881, 0.4155, 0.1238],
+            [0.5881, -0.4093, 0.1238],
+            [0.7451, 0.0287, 0.2539],
+        ]
+    ).view(-1, 3)
+
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    motion_gen.warmup()
+    print("ready")
+    # print("Trajectory Generated: ", result.success)
+    # if PLOT:
+    #    plot_traj(traj.cpu().numpy())
+
+
+if __name__ == "__main__":
+    demo_motion_gen_nvblox()

examples/torch_layers_example.py

@@ -0,0 +1,229 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+"""This example will use CuRobo's functions as pytorch layers as part of a NN."""
+# Standard Library
+import uuid
+from typing import Optional
+
+# Third Party
+import numpy as np
+import torch
+from torch import nn
+from torch.utils.tensorboard import SummaryWriter
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import WorldConfig
+from curobo.geom.types import Mesh
+from curobo.types.math import Pose
+from curobo.types.state import JointState
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import get_assets_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+
+class CuroboTorch(torch.nn.Module):
+    def __init__(self, robot_world: RobotWorld):
+        """Build a simple structured NN:
+
+        q_current -> kinematics -> sdf -> features
+        [features, x_des] -> NN -> kinematics -> sdf -> [sdf, pose_distance] -> NN -> q_out
+        loss = (fk(q_out) - x_des) + (q_current - q_out) + valid(q_out)
+        """
+        super(CuroboTorch, self).__init__()
+
+        feature_dims = robot_world.kinematics.kinematics_config.link_spheres.shape[0] * 5 + 7 + 1
+        q_feature_dims = 7
+        final_feature_dims = feature_dims + 1 + 7
+        output_dims = robot_world.kinematics.get_dof()
+
+        # build neural network:
+        self._robot_world = robot_world
+
+        self._feature_mlp = nn.Sequential(
+            nn.Linear(q_feature_dims, 512),
+            nn.ReLU6(),
+            nn.Linear(512, 512),
+            nn.ReLU6(),
+            nn.Linear(512, 512),
+            nn.ReLU6(),
+            nn.Linear(512, output_dims),
+            nn.Tanh(),
+        )
+        self._final_mlp = nn.Sequential(
+            nn.Linear(final_feature_dims, 256),
+            nn.ReLU6(),
+            nn.Linear(256, 256),
+            nn.ReLU6(),
+            nn.Linear(256, 64),
+            nn.ReLU6(),
+            nn.Linear(64, output_dims),
+            nn.Tanh(),
+        )
+
+    def get_features(self, q: torch.Tensor, x_des: Optional[Pose] = None):
+        kin_state = self._robot_world.get_kinematics(q)
+        spheres = kin_state.link_spheres_tensor.unsqueeze(2)
+        q_sdf = self._robot_world.get_collision_distance(spheres)
+        q_self = self._robot_world.get_self_collision_distance(
+            kin_state.link_spheres_tensor.unsqueeze(1)
+        )
+
+        features = [
+            kin_state.link_spheres_tensor.view(q.shape[0], -1),
+            q_sdf,
+            q_self,
+            kin_state.ee_position,
+            kin_state.ee_quaternion,
+        ]
+        if x_des is not None:
+            pose_distance = self._robot_world.pose_distance(x_des, kin_state.ee_pose)
+            features.append(pose_distance)
+            features.append(x_des.position)
+            features.append(x_des.quaternion)
+        features = torch.cat(features, dim=-1)
+
+        return features
+
+    def forward(self, q: torch.Tensor, x_des: Pose):
+        """Forward for neural network
+
+        Args:
+            q (torch.Tensor): _description_
+            x_des (torch.Tensor): _description_
+        """
+        # get features for input:
+        in_features = torch.cat([x_des.position, x_des.quaternion], dim=-1)
+        # pass through initial mlp:
+        q_mid = self._feature_mlp(in_features)
+
+        q_scale = self._robot_world.bound_scale * q_mid
+        # get new features:
+        mid_features = self.get_features(q_scale, x_des=x_des)
+        q_out = self._final_mlp(mid_features)
+        q_out = self._robot_world.bound_scale * q_out
+        return q_out
+
+    def loss(self, x_des: Pose, q: torch.Tensor, q_in: torch.Tensor):
+        kin_state = self._robot_world.get_kinematics(q)
+        distance = self._robot_world.pose_distance(x_des, kin_state.ee_pose)
+        d_sdf = self._robot_world.collision_constraint(kin_state.link_spheres_tensor.unsqueeze(1))
+        d_self = self._robot_world.self_collision_cost(kin_state.link_spheres_tensor.unsqueeze(1))
+        loss = 0.1 * torch.linalg.norm(q_in - q, dim=-1) + distance + 100.0 * (d_self + d_sdf)
+        return loss
+
+    def val_loss(self, x_des: Pose, q: torch.Tensor, q_in: torch.Tensor):
+        kin_state = self._robot_world.get_kinematics(q)
+        distance = self._robot_world.pose_distance(x_des, kin_state.ee_pose)
+        d_sdf = self._robot_world.collision_constraint(kin_state.link_spheres_tensor.unsqueeze(1))
+        d_self = self._robot_world.self_collision_cost(kin_state.link_spheres_tensor.unsqueeze(1))
+        loss = 10.0 * (d_self + d_sdf) + distance
+        return loss
+
+
+if __name__ == "__main__":
+    update_goal = False
+    write_usd = False
+    writer = SummaryWriter("log/runs/ik/" + str(uuid.uuid4()))
+    robot_file = "franka.yml"
+    world_file = "collision_table.yml"
+    config = RobotWorldConfig.load_from_config(robot_file, world_file, pose_weight=[10, 200, 1, 10])
+    curobo_fn = RobotWorld(config)
+
+    model = CuroboTorch(curobo_fn)
+    model.cuda()
+    with torch.no_grad():
+        # q_train = curobo_fn.sample(10000)
+        q_val = curobo_fn.sample(100)
+        q_train = curobo_fn.sample(5 * 2048)
+    usd_list = []
+    optimizer = torch.optim.Adam(model.parameters(), lr=1e-3, weight_decay=1e-8)
+    batch_size = 512
+    batch_start = torch.arange(0, q_train.shape[0], batch_size)
+    with torch.no_grad():
+        x_des = curobo_fn.get_kinematics(q_train[1:2]).ee_pose
+        x_des_train = curobo_fn.get_kinematics(q_train).ee_pose
+        x_des_val = x_des.repeat(q_val.shape[0])
+    q_debug = []
+    bar = tqdm(range(500))
+    for e in bar:
+        model.train()
+        for j in range(batch_start.shape[0]):
+            x_train = q_train[batch_start[j] : batch_start[j] + batch_size]
+            if x_train.shape[0] != batch_size:
+                continue
+            x_des_batch = x_des_train[batch_start[j] : batch_start[j] + batch_size]
+            q = model.forward(x_train, x_des_batch)
+            loss = model.loss(x_des_batch, q, x_train)
+            loss = torch.mean(loss)
+
+            optimizer.zero_grad()
+            loss.backward()
+            optimizer.step()
+        writer.add_scalar("training loss", loss.item(), e)
+        model.eval()
+        with torch.no_grad():
+            q_pred = model.forward(q_val, x_des_val)
+            val_loss = model.val_loss(x_des_val, q_pred, q_val)
+            val_loss = torch.mean(val_loss)
+            q_debug.append(q_pred[0:1].clone())
+        writer.add_scalar("validation loss", val_loss.item(), e)
+        bar.set_description("t: " + str(val_loss.item()))
+        if e % 100 == 0 and len(q_debug) > 1:
+            if write_usd:
+                q_traj = torch.cat(q_debug, dim=0)
+                world_model = WorldConfig.from_dict(
+                    load_yaml(join_path(get_world_configs_path(), world_file))
+                )
+                gripper_mesh = Mesh(
+                    name="target_gripper",
+                    file_path=join_path(
+                        get_assets_path(),
+                        "robot/franka_description/meshes/visual/hand_ee_link.dae",
+                    ),
+                    color=[0.0, 0.8, 0.1, 1.0],
+                    pose=x_des[0].tolist(),
+                )
+                world_model.add_obstacle(gripper_mesh)
+                save_name = "e_" + str(e)
+                UsdHelper.write_trajectory_animation_with_robot_usd(
+                    "franka.yml",
+                    world_model,
+                    JointState(position=q_traj[0]),
+                    JointState(position=q_traj),
+                    dt=1.0,
+                    visualize_robot_spheres=False,
+                    save_path=save_name + ".usd",
+                    base_frame="/" + save_name,
+                )
+                usd_list.append(save_name + ".usd")
+            if update_goal:
+                with torch.no_grad():
+                    rand_perm = torch.randperm(q_val.shape[0])
+                    q_val = q_val[rand_perm].clone()
+                    x_des = curobo_fn.get_kinematics(q_val[0:1]).ee_pose
+                    x_des_val = x_des.repeat(q_val.shape[0])
+            q_debug = []
+
+    # create loss function:
+    if write_usd:
+        UsdHelper.create_grid_usd(
+            usd_list,
+            "epoch_grid.usd",
+            "/world",
+            max_envs=len(usd_list),
+            max_timecode=len(q_debug),
+            x_space=2.0,
+            y_space=2.0,
+            x_per_row=int(np.sqrt(len(usd_list))) + 1,
+            local_asset_path="",
+            dt=1,
+        )

examples/trajopt_example.py

@@ -0,0 +1,122 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+# Standard Library
+import time
+
+# Third Party
+import torch
+
+# CuRobo
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.wrap.reacher.trajopt import TrajOptSolver, TrajOptSolverConfig
+
+
+def plot_js(trajectory: JointState):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(4, 1)
+    q = trajectory.position.cpu().numpy()
+    qd = trajectory.velocity.cpu().numpy()
+    qdd = trajectory.acceleration.cpu().numpy()
+    qddd = None
+    if trajectory.jerk is not None:
+        qddd = trajectory.jerk.cpu().numpy()
+
+    for i in range(q.shape[-1]):
+        axs[0].plot(q[:, i], label=str(i))
+        axs[1].plot(qd[:, i], label=str(i))
+        axs[2].plot(qdd[:, i], label=str(i))
+        if qddd is not None:
+            axs[3].plot(qddd[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def plot_traj(trajectory):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(1, 1)
+    q = trajectory
+
+    for i in range(q.shape[-1]):
+        axs.plot(q[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def demo_trajopt_collision_free():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_table.yml"
+
+    robot_file = "franka.yml"
+    robot_cfg = RobotConfig.from_dict(
+        load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    )
+    world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+
+    trajopt_config = TrajOptSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        use_cuda_graph=False,
+    )
+    trajopt_solver = TrajOptSolver(trajopt_config)
+    q_start = trajopt_solver.retract_config
+
+    q_goal = q_start.clone() + 0.1
+    # q_goal[...,-1] -=0.2
+    kin_state = trajopt_solver.fk(q_goal)
+    goal_pose = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+    goal_state = JointState.from_position(q_goal)
+    current_state = JointState.from_position(q_start)
+    # do single planning:
+    # print("Running Goal State trajopt")
+    # js_goal = Goal(goal_state=goal_state, current_state=current_state)
+    # result = trajopt_solver.solve_single(js_goal)
+    # traj = result.solution.position
+    # print(result.success, result.cspace_error)
+    # print(goal_state.position)
+    # print(traj[...,-1,:])
+    # print(torch.linalg.norm((goal_state.position - traj[...,-1,:])).item())
+
+    # exit()
+    # if PLOT:
+    #    #plot_traj(traj)
+    #    plot_js(result.solution)
+    # exit()
+    print("Running Goal Pose trajopt")
+    js_goal = Goal(goal_pose=goal_pose, current_state=current_state)
+    result = trajopt_solver.solve_single(js_goal)
+    print(result.success)
+    if PLOT:
+        plot_js(result.solution)
+
+    # run goalset planning:
+    print("Running Goal Pose Set trajopt")
+    # g_set = Pose(kin_state.ee_position, kin_state.ee_quaternion.repeat(2,1).view())
+    # js_goal = Goal(goal_pose=goal_pose, current_state=current_state)
+    # result = trajopt_solver.solve_single(js_goal)
+
+
+if __name__ == "__main__":
+    # demo_basic_ik()
+    # demo_full_config_collision_free_ik()
+    # demo_full_config_batch_env_collision_free_ik()
+    demo_trajopt_collision_free()

examples/usd_example.py

@@ -0,0 +1,171 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import (
+    get_assets_path,
+    get_robot_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+)
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+
+def save_curobo_world_to_usd():
+    world_file = "collision_table.yml"
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), world_file))
+    ).get_mesh_world(process=False)
+    usd_helper = UsdHelper()
+    usd_helper.create_stage()
+
+    usd_helper.add_obstacles_to_stage(world_cfg)
+
+    usd_helper.write_stage_to_file("test.usd")
+
+
+def get_trajectory(robot_file="franka.yml", dt=1.0 / 60.0):
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=24,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=True,
+        num_trajopt_seeds=2,
+        num_graph_seeds=2,
+        evaluate_interpolated_trajectory=True,
+        interpolation_dt=dt,
+        self_collision_check=True,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    motion_gen.warmup()
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = motion_gen.get_retract_config()
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1).clone() + 0.5)
+    # start_state.position[0,2] = 0.5
+    # start_state.position[0,1] = 0.5
+    # start_state.position[0,0] = 0.5
+    # print(start_state.position)
+    result = motion_gen.plan_single(start_state, retract_pose)
+    # print(result.plan_state.position)
+    print(result.success)
+    # print(result.position_error)
+    # result = motion_gen.plan(
+    #    start_state, retract_pose, enable_graph=False, enable_opt=True, max_attempts=10
+    # )
+    traj = result.get_interpolated_plan()  # optimized plan
+    return traj
+
+
+def save_curobo_robot_world_to_usd(robot_file="franka.yml"):
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    world_model = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), world_file))
+    ).get_obb_world()
+    dt = 1 / 60.0
+
+    q_traj = get_trajectory(robot_file, dt)
+    q_start = q_traj[0]
+
+    UsdHelper.write_trajectory_animation_with_robot_usd(
+        robot_file, world_model, q_start, q_traj, save_path="test.usd"
+    )
+
+
+def save_curobo_robot_to_usd(robot_file="franka.yml"):
+    # print(robot_file)
+    tensor_args = TensorDeviceType()
+    robot_cfg_y = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg_y["kinematics"]["use_usd_kinematics"] = True
+    print(
+        len(robot_cfg_y["kinematics"]["cspace"]["null_space_weight"]),
+        len(robot_cfg_y["kinematics"]["cspace"]["retract_config"]),
+        len(robot_cfg_y["kinematics"]["cspace"]["joint_names"]),
+    )
+    # print(robot_cfg_y)
+    robot_cfg = RobotConfig.from_dict(robot_cfg_y, tensor_args)
+    start = JointState.from_position(robot_cfg.cspace.retract_config)
+    retract_cfg = robot_cfg.cspace.retract_config.clone()
+    retract_cfg[0] = 0.5
+
+    # print(retract_cfg)
+    kin_model = CudaRobotModel(robot_cfg.kinematics)
+    position = retract_cfg
+    q_traj = JointState.from_position(position.unsqueeze(0))
+    q_traj.joint_names = kin_model.joint_names
+    # print(q_traj.joint_names)
+    usd_helper = UsdHelper()
+    # usd_helper.create_stage(
+    #    "test.usd", timesteps=q_traj.position.shape[0] + 1, dt=dt, interpolation_steps=10
+    # )
+    world_file = "collision_table.yml"
+    world_model = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), world_file))
+    ).get_obb_world()
+
+    # print(q_traj.position.shape)
+    # usd_helper.load_robot_usd(robot_cfg.kinematics.usd_path, js)
+    usd_helper.write_trajectory_animation_with_robot_usd(
+        {"robot_cfg": robot_cfg_y},
+        world_model,
+        start,
+        q_traj,
+        save_path="test.usd",
+        # robot_asset_prim_path="/robot"
+    )
+
+    # usd_helper.save()
+    # usd_helper.write_stage_to_file("test.usda")
+
+
+def read_world_from_usd(file_path: str):
+    usd_helper = UsdHelper()
+    usd_helper.load_stage_from_file(file_path)
+    # world_model = usd_helper.get_obstacles_from_stage(reference_prim_path="/Root/world_obstacles")
+    world_model = usd_helper.get_obstacles_from_stage(
+        only_paths=["/world/obstacles"], reference_prim_path="/world"
+    )
+    # print(world_model)
+    for x in world_model.cuboid:
+        print(x.name + ":")
+        print("  pose: ", x.pose)
+        print("  dims: ", x.dims)
+
+
+def read_robot_from_usd(robot_file: str = "franka.yml"):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg["kinematics"]["use_usd_kinematics"] = True
+    robot_cfg = RobotConfig.from_dict(robot_cfg, TensorDeviceType())
+
+
+if __name__ == "__main__":
+    save_curobo_world_to_usd()

examples/world_representation_example.py

@@ -0,0 +1,105 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# CuRobo
+from curobo.geom.types import Capsule, Cuboid, Cylinder, Mesh, Sphere, WorldConfig
+from curobo.util_file import get_assets_path, join_path
+
+
+def approximate_geometry():
+    # CuRobo
+    from curobo.geom.sphere_fit import SphereFitType
+    from curobo.geom.types import Capsule, WorldConfig
+
+    obstacle_capsule = Capsule(
+        name="capsule",
+        radius=0.2,
+        base=[0, 0, 0],
+        tip=[0, 0, 0.5],
+        pose=[0.0, 5, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    sph = obstacle_capsule.get_bounding_spheres(
+        500, 0.005, SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE
+    )
+
+    WorldConfig(sphere=sph).save_world_as_mesh("bounding_spheres.obj")
+
+    WorldConfig(capsule=[obstacle_capsule]).save_world_as_mesh("capsule.obj")
+
+
+def doc_example():
+    # describe a cuboid obstacle
+
+    obstacle_1 = Cuboid(
+        name="cube_1",
+        pose=[0.0, 0.0, 0.0, 0.043, -0.471, 0.284, 0.834],
+        dims=[0.2, 1.0, 0.2],
+        color=[0.8, 0.0, 0.0, 1.0],
+    )
+
+    # describe a mesh obstacle
+    # import a mesh file:
+
+    mesh_file = join_path(get_assets_path(), "scene/nvblox/srl_ur10_bins.obj")
+
+    obstacle_2 = Mesh(
+        name="mesh_1",
+        pose=[0.0, 2, 0.5, 0.043, -0.471, 0.284, 0.834],
+        file_path=mesh_file,
+        scale=[0.5, 0.5, 0.5],
+    )
+
+    obstacle_3 = Capsule(
+        name="capsule",
+        radius=0.2,
+        base=[0, 0, 0],
+        tip=[0, 0, 0.5],
+        pose=[0.0, 5, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    obstacle_4 = Cylinder(
+        name="cylinder_1",
+        radius=0.2,
+        height=0.5,
+        pose=[0.0, 6, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    obstacle_5 = Sphere(
+        name="sphere_1",
+        radius=0.2,
+        pose=[0.0, 7, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    world_model = WorldConfig(
+        mesh=[obstacle_2],
+        cuboid=[obstacle_1],
+        capsule=[obstacle_3],
+        cylinder=[obstacle_4],
+        sphere=[obstacle_5],
+    )
+    world_model.randomize_color(r=[0.2, 0.7], g=[0.4, 0.8], b=[0.0, 0.4])
+    file_path = "debug_mesh.obj"
+    world_model.save_world_as_mesh(file_path)
+
+    cuboid_world = WorldConfig.create_obb_world(world_model)
+    cuboid_world.save_world_as_mesh("debug_cuboid_mesh.obj")
+
+    collision_support_world = WorldConfig.create_collision_support_world(world_model)
+    collision_support_world.save_world_as_mesh("debug_collision_mesh.obj")
+
+
+if __name__ == "__main__":
+    approximate_geometry()

images/robot_demo.gif

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8496a75cf043db596aa194894c8f8c8038fdc6fc46f3c15f0cc64acf5eb1c05e
+size 4026516

images/rrt_compare.gif

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:df0dbe0775664436989433c5e785878733520d38c4116a863f601f34dc4788c5
+size 2181094

pyproject.toml

@@ -0,0 +1,66 @@
+# Copyright (c) 2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+
+[build-system]
+requires = ["setuptools>=45", "setuptools_scm>=6.2", "wheel", "torch"]
+
+build-backend = "setuptools.build_meta"
+
+[tool.black]
+line-length = 100
+extend-exclude = "deprecated/"
+
+# See the `setuptools_scm` documentation for the description of the schemes used below.
+# https://pypi.org/project/setuptools-scm/
+# NOTE: If these values are updated, they need to be also updated in `__init__.py`.
+[tool.setuptools_scm]
+version_scheme = "no-guess-dev"
+local_scheme = "dirty-tag"
+
+[tool.isort]
+# see https://github.com/PyCQA/isort#multi-line-output-modes
+multi_line_output = 2
+import_heading_stdlib = "Standard Library"
+import_heading_thirdparty = "Third Party"
+import_heading_firstparty = "CuRobo"
+import_heading_localfolder = "Local Folder"
+import_heading_isaacgym = "Loading isaacgym before pytorch"
+known_first_party = "CuRobo"
+known_isaacgym = "isaacgym"
+line_length = 100
+sections = "FUTURE, STDLIB, ISAACGYM, THIRDPARTY, FIRSTPARTY, LOCALFOLDER"
+
+[tool.pytest.ini_options]
+norecursedirs = [".git", ".venv", "deprecated", "dist"]
+python_files = ["*_test.py"]
+
+
+#[tool.pylint.TYPECHECK]
+# List of members which are set dynamically and missed by Pylint inference
+# system, and so shouldn't trigger E1101 when accessed.
+# generated-members=numpy.*, torch.*
+
+
+
+[tool.coverage.report]
+# NOTE: Single-quoted strings are required in TOML for regular expressions
+exclude_lines = [
+	"pragma: no cover", # Need to re-enable the standard no cover match
+	'^\s*pass\s*$',  # Skip any pass lines
+    "wp.kernel",
+    "torch.jit.script",
+    "abstractmethod",
+    "def backward",
+]
+omit = [
+    "src/curobo/util/error_metrics.py",
+    "src/curobo/rollout/cost/straight_line_cost.py",
+    "src/curobo/rollout/cost/projected_dist_cost.py",
+    "src/curobo/rollout/cost/manipulability_cost.py",
+]
+

setup.cfg

@@ -0,0 +1,124 @@
+# Copyright (c) 2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+
+# Additional files that need to be included in the package distribution must be
+# listed in the MANIFEST.in file.
+#
+# References:
+# * https://newbedev.com/how-include-static-files-to-setuptools-python-package
+
+[metadata]
+
+# Configure specific project settings
+name = nvidia_curobo
+author = NVIDIA Seattle Robotics Lab
+description = GPU robot motion toolkit containing cuda accelerated kinematics, IK, MPC, Global motion planning, and optimization solvers.
+url = https://curobo.org
+license = NVIDIA
+
+# Configure general project settings
+long_description = file: README.md
+long-description-content-type = text/markdown
+license_file = LICENSE
+
+# List of classifiers can be found here:
+# https://pypi.org/classifiers/
+classifiers =
+  License :: Other/Proprietary License
+  Operating System :: OS Independent
+  Intended Audience :: Developers
+  Natural Language :: English
+  Programming Language :: Python :: 3
+  Topic :: Scientific/Engineering :: Robotics
+
+# Change if running only on Windows, Mac or Linux (comma-separated)
+platforms = any
+
+[options]
+install_requires =
+  networkx
+  numpy
+  numpy-quaternion
+  pyyaml
+  setuptools_scm>=6.2
+  torchtyping
+  torch>=1.10
+  trimesh
+  yourdfpy>=0.0.53
+  warp-lang>=0.9.0
+  scipy>=1.7.0
+  tqdm
+  wheel
+  importlib_resources
+
+packages = find_namespace:
+package_dir =
+  = src
+include_package_data = True
+
+[options.packages.find]
+where = src
+exclude =
+    tests
+
+[options.extras_require]
+ci =
+  anybadge
+  black
+  build
+  flake8
+  flake8-docstrings
+  flake8-isort
+  pytest>6.2.5
+  pytest-cov
+  twine
+  sphinx
+  sphinx_rtd_theme
+  graphviz>=0.20.1
+  sphinxcontrib-youtube
+  sphinxcontrib-video>=0.2.0
+
+# this is only available in 3.8+
+smooth = 
+  trajectory_smoothing @ https://github.com/balakumar-s/trajectory_smoothing/raw/main/dist/trajectory_smoothing-0.3-cp38-cp38-linux_x86_64.whl
+
+usd = 
+  usd-core
+
+dev =
+  ipdb
+  ipython
+  black
+  flake8
+  flake8-docstrings
+  flake8-isort
+  pytest>6.2.5
+  pytest-cov
+
+
+doc = 
+  sphinx
+  sphinx_rtd_theme
+  graphviz>=0.20.1
+  sphinxcontrib-youtube
+  sphinxcontrib-video>=0.2.0
+
+[options.entry_points]
+# Add here console scripts like:
+# console_scripts =
+#     script_name = package.module:function
+
+# NOTE (roflaherty): Flake8 doesn't support pyproject.toml configuration yet.
+[flake8]
+max-line-length = 100
+docstring-convention = google
+exclude = .git,build,deprecated,dist,venv
+ignore =
+  W503
+  E203
+  E731

setup.py

@@ -0,0 +1,90 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+"""curobo package setuptools."""
+
+# NOTE (roflaherty): This file is still needed to allow the package to be
+# installed in editable mode.
+#
+# References:
+# * https://setuptools.pypa.io/en/latest/setuptools.html#setup-cfg-only-projects
+
+# Third Party
+import setuptools
+from torch.utils.cpp_extension import BuildExtension, CUDAExtension
+
+print("*********************************")
+print("CuRobo: Compiling CUDA kernels...")
+print(
+    "This will take 20+ minutes, to speedup compilation set TORCH_CUDA_ARCH_LIST={X}+PTX to "
+    + " compile only for a specific architecture. e.g., export TORCH_CUDA_ARCH_LIST=7.0+PTX"
+)
+extra_cuda_args = {
+    "nvcc": [
+        "--threads=6",
+        "-O3",
+        "--ftz=true",
+        "--fmad=true",
+        "--prec-div=false",
+        "--prec-sqrt=false",
+    ]
+}
+# create a list of modules to be compiled:
+ext_modules = [
+    CUDAExtension(
+        "curobo.curobolib.lbfgs_step_cu",
+        [
+            "src/curobo/curobolib/cpp/lbfgs_step_cuda.cpp",
+            "src/curobo/curobolib/cpp/lbfgs_step_kernel.cu",
+        ],
+        extra_compile_args=extra_cuda_args,
+    ),
+    CUDAExtension(
+        "curobo.curobolib.kinematics_fused_cu",
+        [
+            "src/curobo/curobolib/cpp/kinematics_fused_cuda.cpp",
+            "src/curobo/curobolib/cpp/kinematics_fused_kernel.cu",
+        ],
+        extra_compile_args=extra_cuda_args,
+    ),
+    CUDAExtension(
+        "curobo.curobolib.geom_cu",
+        [
+            "src/curobo/curobolib/cpp/geom_cuda.cpp",
+            "src/curobo/curobolib/cpp/sphere_obb_kernel.cu",
+            "src/curobo/curobolib/cpp/pose_distance_kernel.cu",
+            "src/curobo/curobolib/cpp/self_collision_kernel.cu",
+        ],
+        extra_compile_args=extra_cuda_args,
+    ),
+    CUDAExtension(
+        "curobo.curobolib.line_search_cu",
+        [
+            "src/curobo/curobolib/cpp/line_search_cuda.cpp",
+            "src/curobo/curobolib/cpp/line_search_kernel.cu",
+            "src/curobo/curobolib/cpp/update_best_kernel.cu",
+        ],
+        extra_compile_args=extra_cuda_args,
+    ),
+    CUDAExtension(
+        "curobo.curobolib.tensor_step_cu",
+        [
+            "src/curobo/curobolib/cpp/tensor_step_cuda.cpp",
+            "src/curobo/curobolib/cpp/tensor_step_kernel.cu",
+        ],
+        extra_compile_args=extra_cuda_args,
+    ),
+]
+
+setuptools.setup(
+    ext_modules=ext_modules,
+    cmdclass={"build_ext": BuildExtension},
+)

src/curobo/__init__.py

@@ -0,0 +1,52 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+"""CuRobo package."""
+
+
+# NOTE (roflaherty): This is inspired by how matplotlib does creates its version value.
+# https://github.com/matplotlib/matplotlib/blob/master/lib/matplotlib/__init__.py#L161
+def _get_version():
+    """Return the version string used for __version__."""
+    # Standard Library
+    import pathlib
+
+    root = pathlib.Path(__file__).resolve().parent.parent.parent
+    if (root / ".git").exists() and not (root / ".git/shallow").exists():
+        # Third Party
+        import setuptools_scm
+
+        # See the `setuptools_scm` documentation for the description of the schemes used below.
+        # https://pypi.org/project/setuptools-scm/
+        # NOTE: If these values are updated, they need to be also updated in `pyproject.toml`.
+        return setuptools_scm.get_version(
+            root=root,
+            version_scheme="no-guess-dev",
+            local_scheme="dirty-tag",
+        )
+    else:  # Get the version from the _version.py setuptools_scm file.
+        try:
+            # Standard Library
+            from importlib.metadata import version
+        except ModuleNotFoundError:
+            # NOTE: `importlib.resources` is part of the standard library in Python 3.9.
+            # `importlib_metadata` is the back ported library for older versions of python.
+            # Third Party
+            from importlib_metadata import version
+
+        return version("nvidia_curobo")
+
+
+# Set `__version__` attribute
+__version__ = _get_version()
+
+# Remove `_get_version` so it is not added as an attribute
+del _get_version

src/curobo/content/assets/robot/franka_description/franka_panda.urdf

@@ -0,0 +1,279 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <joint name="panda_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_link1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>    
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    <!-- something is weird with this joint limit config
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/>  -->
+  </joint>
+  <link name="panda_link5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+    <!-- <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
+  </joint>
+  <link name="panda_link7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <!--
+  <link name="panda_link8"/>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  Removing this joint seems to help with some stability things
+  -->
+  <joint name="panda_hand_joint" type="fixed">
+    <!--
+    <parent link="panda_link8"/>
+    -->
+    <parent link="panda_link7"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0.107"/>
+    <!--
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+    -->
+  </joint>
+  <link name="panda_hand">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/hand.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/hand.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_leftfinger">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="prismatic">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="prismatic">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="20" lower="-0.04" upper="0.0" velocity="0.2"/>
+    <mimic joint="panda_finger_joint1"/>
+  </joint>
+  
+  <link name="ee_link"/>
+
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_link"/>
+    <origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
+  </joint>
+  <link name="right_gripper">
+    <inertial>
+      <!-- Dummy inertial parameters to avoid link lumping-->
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="right_gripper" type="fixed">
+    <!--<origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>-->
+    <origin rpy="0 0 2.35619449019" xyz="0 0 0.207"/>
+    <axis xyz="0 0 1"/>
+    <parent link="panda_link7"/>
+    <!--<parent link="panda_link8"/>-->
+    <child link="right_gripper"/>
+  </joint>
+
+
+</robot>
+
+

src/curobo/content/assets/robot/franka_description/franka_panda_mobile.urdf

@@ -0,0 +1,325 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <link name="base_link_x"/>
+  <link name="base_link_y"/>
+  <link name="base_link_z"/>
+
+
+<joint name="base_x" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="base_link_x"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.0" upper="1.0" velocity="2.1750"/>
+  </joint>
+
+<joint name="base_y" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_x"/>
+    <child link="base_link_y"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.0" upper="1.0" velocity="2.1750"/>
+  </joint>
+
+<joint name="base_z" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_y"/>
+    <child link="base_link_z"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="0.1" upper="1.0" velocity="2.1750"/>
+  </joint>
+
+  
+
+<joint name="base_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_z"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  
+
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+    
+  </link>
+  <link name="panda_link1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>    
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    <!-- something is weird with this joint limit config
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/>  -->
+  </joint>
+  <link name="panda_link5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
+    <!-- <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
+  </joint>
+  <link name="panda_link7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link8"/>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <!--
+    <parent link="panda_link8"/>
+    -->
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
+    <!--
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+    -->
+  </joint>
+  <link name="panda_hand">
+    <visual>
+      <origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
+      <!--origin rpy="-1.57 0.0 0.0" xyz="0 0 0.0"/-->
+      <geometry>
+        <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--box size="0.01 0.01 0.01"/-->
+        <!--mesh filename="meshes/visual/hand.obj"/-->
+        <mesh filename="meshes/collision/hand_gripper.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/> 
+  </joint>
+  <link name="grasp_frame">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+
+      <origin rpy="0 0 0" xyz="0.0 0 0"/>
+    </collision>
+  </link>
+    <joint name="ee_fixed_t" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_link"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+    </joint>
+  <link name="ee_link"/>
+  <link name="ee_grasp_frame"/>
+  <joint name="ee_grasp_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/> 
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+  </joint>
+
+ 
+
+
+  
+    <link name="panda_leftfinger">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
+  </joint>
+
+</robot>
+
+

src/curobo/content/assets/robot/franka_description/franka_panda_mobile_base.urdf

@@ -0,0 +1,52 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <link name="base_link_x"/>
+  <link name="base_link_y"/>
+
+<joint name="base_x" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="base_link_x"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
+  </joint>
+
+<joint name="base_y" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_x"/>
+    <child link="base_link_y"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
+  </joint>
+
+<joint name="base_z" type="revolute">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_y"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-6.0" upper="6.0" velocity="1.50"/>
+  </joint>
+
+  
+
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+    
+  </link>
+</robot>
+
+

src/curobo/content/assets/robot/franka_description/franka_panda_mobile_x.urdf

@@ -0,0 +1,303 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <link name="base_link_x"/>
+
+
+<joint name="base_x" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="base_link_x"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
+  </joint>
+
+<joint name="base_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.1"/>
+    <parent link="base_link_x"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  
+
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+    
+  </link>
+  <link name="panda_link1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>    
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    <!-- something is weird with this joint limit config
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/>  -->
+  </joint>
+  <link name="panda_link5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
+    <!-- <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
+  </joint>
+  <link name="panda_link7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link8"/>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <!--
+    <parent link="panda_link8"/>
+    -->
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
+    <!--
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+    -->
+  </joint>
+  <link name="panda_hand">
+    <visual>
+      <origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
+      <!--origin rpy="-1.57 0.0 0.0" xyz="0 0 0.0"/-->
+      <geometry>
+        <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--box size="0.01 0.01 0.01"/-->
+        <!--mesh filename="meshes/visual/hand.obj"/-->
+        <mesh filename="meshes/collision/hand_gripper.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/> 
+  </joint>
+  <link name="grasp_frame">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+
+      <origin rpy="0 0 0" xyz="0.0 0 0"/>
+    </collision>
+  </link>
+    <joint name="ee_fixed_t" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_link"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+    </joint>
+  <link name="ee_link"/>
+  <link name="ee_grasp_frame"/>
+  <joint name="ee_grasp_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/> 
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+  </joint>
+
+ 
+
+
+  
+    <link name="panda_leftfinger">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
+  </joint>
+
+</robot>
+
+

src/curobo/content/assets/robot/franka_description/franka_panda_mobile_xy.urdf

@@ -0,0 +1,316 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <link name="base_link_x"/>
+  <link name="base_link_y"/>
+
+
+<joint name="base_x" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="base_link_x"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.0" upper="1.0" velocity="3.0"/>
+  </joint>
+
+<joint name="base_y" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_x"/>
+    <child link="base_link_y"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.0" upper="1.0" velocity="3.0"/>
+  </joint>
+
+
+  
+
+<joint name="base_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.1"/>
+    <parent link="base_link_y"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  
+
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+    
+  </link>
+  <link name="panda_link1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>    
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    <!-- something is weird with this joint limit config
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/>  -->
+  </joint>
+  <link name="panda_link5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
+    <!-- <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
+  </joint>
+  <link name="panda_link7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link8"/>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <!--
+    <parent link="panda_link8"/>
+    -->
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
+    <!--
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+    -->
+  </joint>
+  <link name="panda_hand">
+    <visual>
+      <!--origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/-->
+      <origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--box size="0.01 0.01 0.01"/-->
+        <!--mesh filename="meshes/visual/hand.obj"/-->
+        <mesh filename="meshes/collision/hand_gripper.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/> 
+  </joint>
+  <link name="grasp_frame">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+
+      <origin rpy="0 0 0" xyz="0.0 0 0"/>
+    </collision>
+  </link>
+    <joint name="ee_fixed_t" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_link"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+    </joint>
+  <link name="ee_link"/>
+  <link name="ee_grasp_frame"/>
+  <joint name="ee_grasp_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/> 
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+  </joint>
+
+ 
+
+
+  
+    <link name="panda_leftfinger">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
+  </joint>
+
+</robot>
+
+

src/curobo/content/assets/robot/franka_description/franka_panda_mobile_xy_tz.urdf

@@ -0,0 +1,332 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <link name="base_link_x"/>
+  <link name="base_link_y"/>
+  <link name="base_link_z"/>
+
+
+<joint name="base_x" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="base_link_x"/>
+    <axis xyz="1 0 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
+  </joint>
+
+<joint name="base_y" type="prismatic">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_x"/>
+    <child link="base_link_y"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
+  </joint>
+
+<joint name="base_z" type="revolute">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link_y"/>
+    <child link="base_link_z"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-6.0" upper="6.0" velocity="1.50"/>
+  </joint>
+
+  
+
+<joint name="base_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.1"/>
+    <parent link="base_link_z"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  
+
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+    
+  </link>
+  <link name="panda_link1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>    
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    <!-- something is weird with this joint limit config
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/>  -->
+  </joint>
+  <link name="panda_link5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
+    <!-- <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
+  </joint>
+  <link name="panda_link7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link8"/>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <!--
+    <parent link="panda_link8"/>
+    -->
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
+    <!--
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+    -->
+  </joint>
+  <link name="panda_hand">
+    <visual>
+      <origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
+      <!--origin rpy="-1.57 0.0 0.0" xyz="0 0 0.0"/-->
+      <geometry>
+        <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--box size="0.01 0.01 0.01"/-->
+        <!--mesh filename="meshes/visual/hand.obj"/-->
+        <mesh filename="meshes/collision/hand_gripper.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/> 
+  </joint>
+  <link name="grasp_frame">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+
+      <origin rpy="0 0 0" xyz="0.0 0 0"/>
+    </collision>
+  </link>
+    <joint name="ee_fixed_t" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_link"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+    </joint>
+  <link name="ee_link"/>
+  <link name="ee_grasp_frame"/>
+  <joint name="ee_grasp_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/> 
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+  </joint>
+
+ 
+
+
+  
+    <link name="panda_leftfinger">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="prismatic">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="prismatic">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="20" lower="-0.04" upper="0.0" velocity="0.2"/>
+    <mimic joint="panda_finger_joint1"/>
+  </joint>
+
+</robot>
+
+

src/curobo/content/assets/robot/franka_description/franka_panda_no_gripper.urdf

@@ -0,0 +1,302 @@
+<?xml version="1.0" ?>
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link"/>
+  <joint name="panda_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.0"/>
+    <parent link="base_link"/>
+    <child link="panda_link0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+    
+  </link>
+  <link name="panda_link1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>    
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    <!-- something is weird with this joint limit config
+    <dynamics damping="10.0"/>
+    <limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/>  -->
+  </joint>
+  <link name="panda_link5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
+    <!-- <dynamics damping="10.0"/>
+    <limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
+  </joint>
+  <link name="panda_link7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="10.0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link8"/>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <!--
+    <parent link="panda_link8"/>
+    -->
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
+    <!--
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+    -->
+  </joint>
+  <link name="panda_hand">
+    <visual>
+      <!--origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/-->
+      <origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--box size="0.01 0.01 0.01"/-->
+        <!--mesh filename="meshes/visual/hand.obj"/-->
+        <mesh filename="meshes/collision/hand_gripper.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/> 
+  </joint>
+  <link name="grasp_frame">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+
+      <origin rpy="0 0 0" xyz="0.0 0 0"/>
+    </collision>
+  </link>
+    <joint name="ee_fixed_t" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_link"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+    </joint>
+  <link name="ee_link"/>
+  <link name="ee_grasp_frame"/>
+  <joint name="ee_grasp_joint" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="ee_grasp_frame"/>
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
+    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/> 
+    <!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/--> 
+  </joint>
+
+
+  
+    <link name="panda_leftfinger">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
+  </joint>
+  <link name="right_gripper">
+    <inertial>
+      <!-- Dummy inertial parameters to avoid link lumping-->
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="right_gripper" type="fixed">
+    <!--<origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>-->
+    <origin rpy="0 0 2.35619449019" xyz="0 0 0.207"/>
+    <axis xyz="0 0 1"/>
+    <parent link="panda_link7"/>
+    <!--<parent link="panda_link8"/>-->
+    <child link="right_gripper"/>
+  </joint>
+
+</robot>
+
+

src/curobo/content/assets/robot/franka_description/franka_test.urdf

@@ -0,0 +1,312 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from panda_arm_hand.urdf.xacro      | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="panda_link0">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.05"/>
+       <mass value="2.9"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/collision/link0.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link0.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_link1">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 -0.04 -0.05"/>
+       <mass value="2.7"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/link1.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link1.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 -0.04 0.06"/>
+       <mass value="2.73"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/link2.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-1.8326" upper="1.8326" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+	  <inertial>
+      <origin rpy="0 0 0" xyz="0.01 0.01 -0.05"/>
+       <mass value="2.04"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/link3.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link3.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="-0.03 0.03 0.02"/>
+       <mass value="2.08"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/link4.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link4.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-3.1416" upper="0.0" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link5">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0.04 -0.12"/>
+       <mass value="3"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/link5.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link5.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0.04 0 0"/>
+       <mass value="1.3"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/link6.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link6.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-0.0873" upper="3.8223" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link7">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.08"/>
+       <mass value=".2"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/collision/link7.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/link7.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link8">
+  	 <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="0.0"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+  </joint>
+  <link name="panda_hand">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.04"/>
+       <mass value=".81"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/hand.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_leftfinger">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0.01 0.02"/>
+       <mass value="0.1"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="package://meshes/visual/finger.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 -0.01 0.02"/>
+       <mass value="0.1"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://meshes/visual/finger.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://meshes/collision/finger.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+    <mimic joint="panda_finger_joint1"/>
+  </joint>
+  <link name="right_gripper">
+    <inertial>
+      <!-- Dummy inertial parameters to avoid link lumping-->
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="right_gripper" type="fixed">
+    <origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>
+      <axis xyz="0 0 1"/>
+    <parent link="panda_link8"/>
+    <child link="right_gripper"/>
+  </joint>
+   <link name="panda_grasptarget">
+ <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="0.0"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+   </link>
+   <joint name="panda_grasptarget_hand" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_grasptarget"/>
+    <origin rpy="0 0 0" xyz="0 0 0.105"/>
+  </joint>
+
+</robot>

src/curobo/content/assets/robot/franka_description/moving_gripper.urdf

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<robot name="mug">
+  <link name="base"/>
+  <link name="base1"/>
+  <link name="base2"/>
+  <link name="base3"/>
+  <link name="base4"/>
+  <link name="base5"/>
+  <joint name="x_joint" type="prismatic">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="base"/>
+    <child link="base1"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+
+  <joint name="y_joint" type="prismatic">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="base1"/>
+    <child link="base2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+
+  <joint name="z_joint" type="prismatic">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="base2"/>
+    <child link="base3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <joint name="rx_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="base3"/>
+    <child link="base4"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <joint name="ry_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="base4"/>
+    <child link="base5"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <joint name="rz_joint" type="revolute">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="base5"/>
+    <child link="gripper"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="gripper">
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0 0"/>
+      <geometry>
+         <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0"/>
+      <geometry>
+          <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.0"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+  </link>
+</robot>

src/curobo/content/assets/robot/franka_description/panda.urdf

@@ -0,0 +1,342 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from panda_arm_hand.urdf.xacro      | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="panda_link0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link0.dae"/>
+      </geometry>
+      <material name="panda_white">
+    		<color rgba="1. 1. 1. 1."/>
+  		</material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link0.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <link name="panda_link1">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 -0.04 -0.05"/>
+       <mass value="2.7"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link1.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link1.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <joint name="panda_joint1" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="0 0 0" xyz="0 0 0.333"/>
+    <parent link="panda_link0"/>
+    <child link="panda_link1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link2">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 -0.04 0.06"/>
+       <mass value="2.73"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link2.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link2.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <joint name="panda_joint2" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link1"/>
+    <child link="panda_link2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link3">
+	  <inertial>
+      <origin rpy="0 0 0" xyz="0.01 0.01 -0.05"/>
+       <mass value="2.04"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link3.dae"/>
+      </geometry>
+      <material name="panda_red">
+    		<color rgba="1. 1. 1. 1."/>
+  		</material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_joint3" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
+    <parent link="panda_link2"/>
+    <child link="panda_link3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link4">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="-0.03 0.03 0.02"/>
+       <mass value="2.08"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link4.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link4.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <joint name="panda_joint4" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
+    <parent link="panda_link3"/>
+    <child link="panda_link4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+  </joint>
+  <link name="panda_link5">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0.04 -0.12"/>
+       <mass value="3"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link5.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link5.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <joint name="panda_joint5" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8975" soft_upper_limit="2.8975"/>
+    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
+    <parent link="panda_link4"/>
+    <child link="panda_link5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-2.8975" upper="2.8975" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link6">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0.04 0 0"/>
+       <mass value="1.3"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link6.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link6.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <joint name="panda_joint6" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <parent link="panda_link5"/>
+    <child link="panda_link6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link7">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.08"/>
+       <mass value=".2"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/link7.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <joint name="panda_joint7" type="revolute">
+    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
+    <parent link="panda_link6"/>
+    <child link="panda_link7"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+  </joint>
+  <link name="panda_link8">
+  	 <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="0.0"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+  </joint>
+  <link name="panda_hand">
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.04"/>
+       <mass value=".81"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/hand.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/hand.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <link name="panda_leftfinger">
+       <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.1"/>
+      <lateral_friction value="1.0"/>
+    </contact>
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 0.01 0.02"/>
+       <mass value="0.1"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+        <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.1"/>
+      <lateral_friction value="1.0"/>
+    </contact>
+
+  	<inertial>
+      <origin rpy="0 0 0" xyz="0 -0.01 0.02"/>
+       <mass value="0.1"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.dae"/>
+      </geometry>
+      <material name="panda_white"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.stl"/>
+      </geometry>
+      <material name="panda_white"/>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
+  </joint>
+  <link name="right_gripper">
+    <inertial>
+      <!-- Dummy inertial parameters to avoid link lumping-->
+      <mass value="0.01"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+  <joint name="right_gripper" type="fixed">
+    <origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>
+      <axis xyz="0 0 1"/>
+    <parent link="panda_link8"/>
+    <child link="right_gripper"/>
+  </joint>
+   <link name="panda_grasptarget">
+ <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="0.0"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+   </link>
+   <joint name="panda_grasptarget_hand" type="fixed">
+    <parent link="panda_hand"/>
+    <child link="panda_grasptarget"/>
+    <origin rpy="0 0 0" xyz="0 0 0.105"/>
+  </joint>
+
+</robot>

src/curobo/content/assets/robot/franka_description/panda_grasp_gripper.urdf

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<robot name="panda_gripper">
+  <link name="gripper">
+    <visual>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="meshes/visual/graspnet_panda_mesh.obj"/>
+
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--box size="0.01 0.01 0.01"/-->
+        
+        <mesh filename="meshes/visual/graspnet_panda_mesh.obj"/>
+        <!--mesh filename="meshes/collision/hand_gripper.obj"/-->
+      </geometry>
+    </collision>
+
+    <inertial>
+      <mass value="0.102"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+  </link>
+</robot>

src/curobo/content/assets/robot/franka_description/panda_gripper.urdf

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<robot name="panda_gripper">
+  <link name="gripper">
+    <visual>
+      <origin rpy="0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--mesh filename="meshes/visual/graspnet_panda_mesh.obj"/-->
+        <mesh filename="meshes/visual/hand.obj"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
+      <geometry>
+        <!--box size="0.01 0.01 0.01"/-->
+        <mesh filename="meshes/visual/hand.obj"/>
+        <!--mesh filename="meshes/visual/graspnet_panda_mesh.obj"/-->
+        <!--mesh filename="meshes/collision/hand_gripper.obj"/-->
+      </geometry>
+    </collision>
+
+    <inertial>
+      <mass value="0.102"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+  </link>
+</robot>

src/curobo/content/assets/robot/iiwa_allegro_description/allegro.urdf

@@ -0,0 +1,546 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from lula_kuka_allegro.urdf.xacro   | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="kuka_allegro" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- ======================== BASE PARAMS ========================= -->
+  <!-- ======================== FINGER PARAMS ======================== -->
+  <!-- full height from joint to tip. when used,
+       the radius of the finger tip sphere will be subtracted
+       and one fixed link will be added for the tip. -->
+  <!-- ========================= THUMB PARAMS ========================= -->
+  <!-- ========================= LIMITS ========================= -->
+  <!-- ============================================================================= -->
+  <!-- BASE -->
+  <link name="allegro_mount">
+    <inertial>
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/mounts/allegro_mount.obj"/>
+      </geometry>
+      <material name="color_j7"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/mounts/allegro_mount.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="allegro_mount_joint" type="fixed">
+    <origin rpy="0 -1.5708 0.785398" xyz="-0.008219 -0.02063 0.08086"/>
+    <parent link="allegro_mount"/>
+    <child link="palm_link"/>
+  </joint>
+  <link name="palm_link">
+    <inertial>
+      <mass value="0.4154"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/base_link.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0 "/>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/base_link.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <gazebo reference="palm_link">
+    <material value="Gazebo/Grey"/>
+  </gazebo>
+  <link name="index_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="index_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="index_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="index_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.11"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0. 0.5 0. 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="index_biotac_tip">
+    <inertial>
+      <mass value="0.04"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+  </link>
+  <link name="middle_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0 0.5 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_biotac_tip">
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+  </link>
+-->
+  <link name="ring_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="ring_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="ring_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="ring_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0 0.5 0 1"/>
+      </material>
+    </visual>
+  </link>
+
+  <link name="ring_biotac_tip">
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+  </link>
+  <link name="thumb_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/thumb_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/thumb_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/thumb_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/thumb_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/thumb_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/allegro/thumb_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor_thumb.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor_thumb.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0 0.5 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_biotac_tip">
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+  </link>
+
+  <joint name="index_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 -0.0872638888889" xyz="0.0514302 -0.03632 -0.0113"/>
+    <parent link="palm_link"/>
+    <child link="index_link_0"/>
+    <dynamics damping="0.025" friction="0.035"/>
+  </joint>
+  <joint name="index_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
+    <parent link="index_link_0"/>
+    <child link="index_link_1"/>
+    <dynamics damping="0.025" friction="0.035"/>
+  </joint>
+  <joint name="index_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
+    <parent link="index_link_1"/>
+    <child link="index_link_2"/>
+    <dynamics damping="0.025" friction="0.035"/>
+  </joint>
+  <joint name="index_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
+    <parent link="index_link_2"/>
+    <child link="index_link_3"/>
+    <dynamics damping="0.025" friction="0.035"/>
+  </joint>
+  <joint name="middle_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 0" xyz="0.0537375 0.0087771 -0.0113"/>
+    <parent link="palm_link"/>
+    <child link="middle_link_0"/>
+    <dynamics friction="0.035"/>
+  </joint>
+
+  <joint name="middle_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
+    <parent link="middle_link_0"/>
+    <child link="middle_link_1"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="middle_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
+    <parent link="middle_link_1"/>
+    <child link="middle_link_2"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="middle_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
+    <parent link="middle_link_2"/>
+    <child link="middle_link_3"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="middle_biotac_tip_joint" type="fixed">
+    <origin rpy="0 0 0.436319444444 " xyz="0.055 0.015 0"/>
+    <parent link="middle_link_3"/>
+    <child link="middle_biotac_tip"/>
+  </joint>
+
+  <joint name="ring_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 0.0872638888889" xyz="0.0514302 0.0538749 -0.0113"/>
+    <parent link="palm_link"/>
+    <child link="ring_link_0"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="ring_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
+    <parent link="ring_link_0"/>
+    <child link="ring_link_1"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="ring_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
+    <parent link="ring_link_1"/>
+    <child link="ring_link_2"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="ring_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
+    <parent link="ring_link_2"/>
+    <child link="ring_link_3"/>
+    <dynamics friction="0.035"/>
+  </joint>
+
+  <joint name="ring_biotac_tip_joint" type="fixed">
+    <origin rpy="0 0 0.436319444444 " xyz="0.055 0.015 0"/>
+    <parent link="ring_link_3"/>
+    <child link="ring_biotac_tip"/>
+  </joint>
+
+  <joint name="thumb_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="0.279244444444" upper="1.57075" velocity="6.283"/>
+    <origin rpy="-1.57075 -1.57075 1.48348611111" xyz="-0.0367482 -0.0081281 -0.0295"/>
+    <parent link="palm_link"/>
+    <child link="thumb_link_0"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="thumb_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.331602777778" upper="1.15188333333" velocity="6.283"/>
+    <origin rpy="1.57075 0 0" xyz="0.005 0.0 0.0"/>
+    <parent link="thumb_link_0"/>
+    <child link="thumb_link_1"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="thumb_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 0.0" xyz="0 0 0.0554"/>
+    <parent link="thumb_link_1"/>
+    <child link="thumb_link_2"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="thumb_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.76273055556" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0514 0.0 0.0"/>
+    <parent link="thumb_link_2"/>
+    <child link="thumb_link_3"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="thumb_biotac_tip_joint" type="fixed">
+    <origin rpy="0 0 0.436319444444 " xyz="0.07 0.01 0"/>
+    <parent link="thumb_link_3"/>
+    <child link="thumb_biotac_tip"/>
+  </joint>
+
+</robot>

src/curobo/content/assets/robot/iiwa_allegro_description/iiwa.urdf

@@ -0,0 +1,414 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from lula_kuka_allegro.urdf.xacro   | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="kuka_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="Grey">
+    <color rgba="0.4 0.4 0.4 1.0"/>
+  </material>
+  <material name="Orange">
+    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+  </material>
+  <material name="Brown">
+    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <link name="base_link"/>
+  <!--joint between {parent} and link_0-->
+  <joint name="iiwa7_base_link_iiwa7_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="iiwa7_link_0"/>
+  </joint>
+  <link name="iiwa7_link_0">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
+      <mass value="5"/>
+      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_0.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_0.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </collision>
+    <self_collision_checking>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <capsule length="0.25" radius="0.15"/>
+      </geometry>
+    </self_collision_checking>
+  </link>
+  <!-- joint between link_0 and link_1 -->
+  <joint name="iiwa7_joint_1" type="revolute">
+    <parent link="iiwa7_link_0"/>
+    <child link="iiwa7_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0.15"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="iiwa7_link_1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+      <mass value="3.4525"/>
+      <inertia ixx="0.02183" ixy="0" ixz="0" iyy="0.007703" iyz="-0.003887" izz="0.02083"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0075"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_1.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0075"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_1.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_2" type="revolute">
+    <parent link="iiwa7_link_1"/>
+    <child link="iiwa7_link_2"/>
+    <origin rpy="1.57079632679   0 3.14159265359" xyz="0 0 0.19"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="iiwa7_link_2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
+      <mass value="3.4821"/>
+      <inertia ixx="0.02076" ixy="0" ixz="-0.003626" iyy="0.02179" iyz="0" izz="0.00779"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_2.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_2.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_3" type="revolute">
+    <parent link="iiwa7_link_2"/>
+    <child link="iiwa7_link_3"/>
+    <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.21 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="iiwa7_link_3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.03 0.13"/>
+      <mass value="4.05623"/>
+      <inertia ixx="0.03204" ixy="0" ixz="0" iyy="0.00972" iyz="0.006227" izz="0.03042"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_3.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_3.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_4" type="revolute">
+    <parent link="iiwa7_link_3"/>
+    <child link="iiwa7_link_4"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0.19"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="iiwa7_link_4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.067 0.034"/>
+      <mass value="3.4822"/>
+      <inertia ixx="0.02178" ixy="0" ixz="0" iyy="0.02075" iyz="-0.003625" izz="0.007785"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_4.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_4.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_5" type="revolute">
+    <parent link="iiwa7_link_4"/>
+    <child link="iiwa7_link_5"/>
+    <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.21 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="iiwa7_link_5">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
+      <mass value="2.1633"/>
+      <inertia ixx="0.01287" ixy="0" ixz="0" iyy="0.005708" iyz="-0.003946" izz="0.01112"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_5.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_5.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_6" type="revolute">
+    <parent link="iiwa7_link_5"/>
+    <child link="iiwa7_link_6"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0.06070 0.19"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="iiwa7_link_6">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
+      <mass value="2.3466"/>
+      <inertia ixx="0.006509" ixy="0" ixz="0" iyy="0.006259" iyz="0.00031891" izz="0.004527"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_6.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_6.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_7" type="revolute">
+    <parent link="iiwa7_link_6"/>
+    <child link="iiwa7_link_7"/>
+    <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0.06070"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.01941960595" soft_upper_limit="3.01941960595"/>
+    <dynamics damping="0.5"/>
+  </joint>
+  <link name="iiwa7_link_7">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.02"/>
+      <mass value="3.129"/>
+      <inertia ixx="0.01464" ixy="0.0005912" ixz="0" iyy="0.01465" iyz="0" izz="0.002872"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.0005"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_7.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.0005"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_7.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_ee" type="fixed">
+    <parent link="iiwa7_link_7"/>
+    <child link="iiwa7_link_ee"/>
+    <origin rpy="0 0 0" xyz="0 0 0.071"/>
+  </joint>
+  <link name="iiwa7_link_ee">
+    </link>
+  <!-- Load Gazebo lib and set the robot namespace -->
+  <!-- <gazebo>
+      <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+        <robotNamespace>/${robot_name}</robotNamespace>
+      </plugin>
+    </gazebo> -->
+  <!-- Link0 -->
+  <gazebo reference="iiwa7_link_0">
+    <material>Gazebo/Grey</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link1 -->
+  <gazebo reference="iiwa7_link_1">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link2 -->
+  <gazebo reference="iiwa7_link_2">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link3 -->
+  <gazebo reference="iiwa7_link_3">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link4 -->
+  <gazebo reference="iiwa7_link_4">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link5 -->
+  <gazebo reference="iiwa7_link_5">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link6 -->
+  <gazebo reference="iiwa7_link_6">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link7 -->
+  <gazebo reference="iiwa7_link_7">
+    <material>Gazebo/Grey</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <transmission name="iiwa7_tran_1">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_1">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_1">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_2">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_2">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_2">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_3">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_3">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_3">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_4">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_4">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_4">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_5">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_5">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_5">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_6">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_6">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_6">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_7">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_7">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_7">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+</robot>

src/curobo/content/assets/robot/iiwa_allegro_description/iiwa_allegro.urdf

@@ -0,0 +1,973 @@
+<?xml version="1.0" ?>
+<!-- This file was autogenerated but we have modified quite a bit
+Authors: Karl, Ankur
+
+Karl did the dynamics compensation and obtained the following values for the index finger
+
+Noise: 0.111576
+Masses: 0.005, 0.125164, 0.131691, 0.0211922,
+Rotor Inertia: 0.00386572, 0.00346965, 0.00433775, 0.00366413,
+Viscous Friction: 0.0414019, 0.00587541, 0.010638, 0.0226948,
+Coulomb Friction: 0.0523963, 0.0150275, 0.00616359, 0.0227036,
+Mass Inertia:
+5.1458e-05, 5.1458e-05, 6.125e-05, 0, 0, 0,
+6.39979e-06, 8.88687e-05, 9.13751e-05, -3.26531e-06, 1.23963e-05, 2.07384e-05,
+7.04217e-05, 3.95744e-05, 6.61125e-05, -9.64342e-05, 5.8796e-05, -3.62996e-05,
+2.93743e-05, 7.21391e-05, 7.59731e-05, -3.51896e-05, -6.31225e-05, -9.25392e-07,
+
+Center of Mass:
+0, 0, 0,
+0.027, 0, 0,
+0.038, 0, 0,
+0.029, 0, 0,
+
+-->
+
+<robot name="iiwa_allegro" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="Grey">
+    <color rgba="0.4 0.4 0.4 1.0"/>
+  </material>
+  <material name="Orange">
+    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+  </material>
+  <material name="Brown">
+    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <link name="base_link"/>
+  <!--joint between {parent} and link_0-->
+  <joint name="iiwa7_base_link_iiwa7_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="iiwa7_link_0"/>
+  </joint>
+  <link name="iiwa7_link_0">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
+      <mass value="5"/>
+      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_0.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_0.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </collision>
+    <self_collision_checking>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <capsule length="0.25" radius="0.15"/>
+      </geometry>
+    </self_collision_checking>
+  </link>
+  <!-- joint between link_0 and link_1 -->
+  <joint name="iiwa7_joint_1" type="revolute">
+    <parent link="iiwa7_link_0"/>
+    <child link="iiwa7_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0.15"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
+    <dynamics damping="5.5" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+      <mass value="3.4525"/>
+      <inertia ixx="0.02183" ixy="0" ixz="0" iyy="0.007703" iyz="-0.003887" izz="0.02083"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0075"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_1.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0075"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_1.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_2" type="revolute">
+    <parent link="iiwa7_link_1"/>
+    <child link="iiwa7_link_2"/>
+    <origin rpy="1.57079632679   0 3.14159265359" xyz="0 0 0.19"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
+    <dynamics damping="5.55" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
+      <mass value="3.4821"/>
+      <inertia ixx="0.02076" ixy="0" ixz="-0.003626" iyy="0.02179" iyz="0" izz="0.00779"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_2.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_2.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_3" type="revolute">
+    <parent link="iiwa7_link_2"/>
+    <child link="iiwa7_link_3"/>
+    <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.21 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
+    <dynamics damping="5.5" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.03 0.13"/>
+      <mass value="4.05623"/>
+      <inertia ixx="0.03204" ixy="0" ixz="0" iyy="0.00972" iyz="0.006227" izz="0.03042"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_3.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_3.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_4" type="revolute">
+    <parent link="iiwa7_link_3"/>
+    <child link="iiwa7_link_4"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0.19"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
+    <dynamics damping="5.5" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.067 0.034"/>
+      <mass value="3.4822"/>
+      <inertia ixx="0.02178" ixy="0" ixz="0" iyy="0.02075" iyz="-0.003625" izz="0.007785"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_4.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_4.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_5" type="revolute">
+    <parent link="iiwa7_link_4"/>
+    <child link="iiwa7_link_5"/>
+    <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.21 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
+    <dynamics damping="5.5" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_5">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
+      <mass value="2.1633"/>
+      <inertia ixx="0.01287" ixy="0" ixz="0" iyy="0.005708" iyz="-0.003946" izz="0.01112"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_5.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.026"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_5.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_6" type="revolute">
+    <parent link="iiwa7_link_5"/>
+    <child link="iiwa7_link_6"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0.06070 0.19"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
+    <dynamics damping="0.5" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_6">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
+      <mass value="2.3466"/>
+      <inertia ixx="0.006509" ixy="0" ixz="0" iyy="0.006259" iyz="0.00031891" izz="0.004527"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_6.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_6.obj"/>
+      </geometry>
+      <material name="Orange"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_7" type="revolute">
+    <parent link="iiwa7_link_6"/>
+    <child link="iiwa7_link_7"/>
+    <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0.06070"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.01941960595" soft_upper_limit="3.01941960595"/>
+    <dynamics damping="5.5" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_7">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.02"/>
+      <mass value="3.129"/>
+      <inertia ixx="0.01464" ixy="0.0005912" ixz="0" iyy="0.01465" iyz="0" izz="0.002872"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.0005"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/visual/link_7.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.0005"/>
+      <geometry>
+        <mesh filename="meshes/iiwa7/collision/link_7.obj"/>
+      </geometry>
+      <material name="Grey"/>
+    </collision>
+  </link>
+  <joint name="iiwa7_joint_ee" type="fixed">
+    <parent link="iiwa7_link_7"/>
+    <child link="iiwa7_link_ee"/>
+    <origin rpy="0 0 0" xyz="0 0 0.071"/>
+    <dynamics damping="5.5" friction="0.025"/>
+  </joint>
+  <link name="iiwa7_link_ee">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1e-3" ixy="0.0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3"/>
+    </inertial>
+  </link>
+  
+  <!-- Load Gazebo lib and set the robot namespace -->
+  <!-- <gazebo>
+      <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+        <robotNamespace>/${robot_name}</robotNamespace>
+      </plugin>
+    </gazebo> -->
+  <!-- Link0 -->
+  <gazebo reference="iiwa7_link_0">
+    <material>Gazebo/Grey</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link1 -->
+  <gazebo reference="iiwa7_link_1">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link2 -->
+  <gazebo reference="iiwa7_link_2">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link3 -->
+  <gazebo reference="iiwa7_link_3">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link4 -->
+  <gazebo reference="iiwa7_link_4">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link5 -->
+  <gazebo reference="iiwa7_link_5">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link6 -->
+  <gazebo reference="iiwa7_link_6">
+    <material>Gazebo/Orange</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Link7 -->
+  <gazebo reference="iiwa7_link_7">
+    <material>Gazebo/Grey</material>
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <transmission name="iiwa7_tran_1">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_1">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_1">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_2">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_2">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_2">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_3">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_3">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_3">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_4">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_4">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_4">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_5">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_5">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_5">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_6">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_6">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_6">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="iiwa7_tran_7">
+    <robotNamespace>/iiwa7</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="iiwa7_joint_7">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="iiwa7_motor_7">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!-- ======================== BASE PARAMS ========================= -->
+  <!-- ======================== FINGER PARAMS ======================== -->
+  <!-- full height from joint to tip. when used,
+       the radius of the finger tip sphere will be subtracted
+       and one fixed link will be added for the tip. -->
+  <!-- ========================= THUMB PARAMS ========================= -->
+  <!-- ========================= LIMITS ========================= -->
+  <!-- ============================================================================= -->
+  <!-- BASE -->
+
+  <link name="allegro_mount">
+    <inertial>
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
+    </inertial>
+    <visual>
+      <!-- <origin xyz="-0.0425 -0.0425 0" rpy="0 0 0" /> -->
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/mounts/allegro_mount.obj"/>
+      </geometry>
+      <material name="color_j7"/>
+    </visual>
+    <collision>
+      <!-- <origin xyz="-0.0425 -0.0425 0" rpy="0 0 0" /> -->
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/mounts/allegro_mount.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- <joint name="allegro_mount_joint1" type="fixed">
+    
+    <origin rpy="0 0 0" xyz="-0.00 -0.0 0.0"/>
+    <parent link="iiwa7_link_ee"/>
+    <child link="allegro_mount"/>
+
+  </joint> -->
+  <joint name="allegro_mount_joint" type="fixed">
+    <!-- <origin xyz="0.065 0 0.0275" rpy="0 1.57 0" /> -->
+    <origin rpy="0 -1.5708 0.785398" xyz="0.008219 -0.02063 0.08086"/>
+    <parent link="allegro_mount"/>
+    <child link="palm_link"/>
+
+  </joint>
+  
+  <!-- BASE -->
+  <link name="palm_link">
+    <inertial>
+      <mass value="0.4154"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/base_link.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0 "/>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/allegro/base_link.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="palm_link">
+    <material value="Gazebo/Grey"/>
+  </gazebo>
+  <!-- ============================================================================= -->
+  <!-- FINGERS -->
+  <!-- RIGHT HAND due to which finger is number 0 -->
+  <!-- for LEFT HAND switch the sign of the **offset_origin_y** and **finger_angle_r** parameters-->
+  <!-- [LINK 0, 4, 8] -->
+  <link name="index_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="5.1458e-05" iyy="5.1458e-05" izz="6.125e-05" ixy="0" ixz="0" iyz="0"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="index_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.125164"/>
+      <origin xyz="0.027 0 0"/>
+      <inertia ixx="6.39979e-06" iyy="8.88687e-05" izz="9.13751e-05" ixy="-3.26531e-06" ixz="1.23963e-05" iyz="2.07384e-05"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="index_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.131691"/>
+      <origin xyz="0.039 0 0"/>
+      <inertia ixx="7.04217e-05" iyy="3.95744e-05" izz="6.61125e-05" ixy="-9.64342e-05" ixz="5.8796e-05" iyz="-3.62996e-05"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="index_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.0211922"/>
+      <origin xyz="0.029 0 0"/>
+      <inertia ixx="2.93743e-05" iyy="7.21391e-05" izz="7.59731e-05" ixy="-3.51896e-05" ixz="-6.31225e-05" iyz="-9.25392e-07"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0. 0.5 0. 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="5.1458e-05" iyy="5.1458e-05" izz="6.125e-05" ixy="0" ixz="0" iyz="0"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.125164"/>
+      <origin xyz="0.027 0 0"/>
+      <inertia ixx="6.39979e-06" iyy="8.88687e-05" izz="9.13751e-05" ixy="-3.26531e-06" ixz="1.23963e-05" iyz="2.07384e-05"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.131691"/>
+      <origin xyz="0.039 0 0"/>
+      <inertia ixx="7.04217e-05" iyy="3.95744e-05" izz="6.61125e-05" ixy="-9.64342e-05" ixz="5.8796e-05" iyz="-3.62996e-05"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="middle_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.0211922"/>
+      <origin xyz="0.029 0 0"/>
+      <inertia ixx="2.93743e-05" iyy="7.21391e-05" izz="7.59731e-05" ixy="-3.51896e-05" ixz="-6.31225e-05" iyz="-9.25392e-07"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0 0.5 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="ring_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.005"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="5.1458e-05" iyy="5.1458e-05" izz="6.125e-05" ixy="0" ixz="0" iyz="0"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="ring_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.125164"/>
+      <origin xyz="0.027 0 0"/>
+      <inertia ixx="6.39979e-06" iyy="8.88687e-05" izz="9.13751e-05" ixy="-3.26531e-06" ixz="1.23963e-05" iyz="2.07384e-05"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="ring_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.131691"/>
+      <origin xyz="0.039 0 0"/>
+      <inertia ixx="7.04217e-05" iyy="3.95744e-05" izz="6.61125e-05" ixy="-9.64342e-05" ixz="5.8796e-05" iyz="-3.62996e-05"/>	
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/primary_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="ring_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.0211922"/>
+      <origin xyz="0.029 0 0"/>
+      <inertia ixx="2.93743e-05" iyy="7.21391e-05" izz="7.59731e-05" ixy="-3.51896e-05" ixz="-6.31225e-05" iyz="-9.25392e-07"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0 0.5 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_link_0">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/thumb_base.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/thumb_base.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_link_1">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/thumb_proximal.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/thumb_proximal.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_link_2">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/allegro/thumb_medial.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/allegro/thumb_medial.obj"/>
+      </geometry>
+      <material name="Grey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="thumb_link_3">
+    <collision>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor_thumb.obj"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/biotac/biotac_sensor_thumb.obj"/>
+      </geometry>
+      <material name="Green">
+        <color rgba="0 0.5 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="index_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 -0.0872638888889" xyz="0.0514302 -0.03632 -0.0113"/>
+    <parent link="palm_link"/>
+    <child link="index_link_0"/>
+    <dynamics damping="0.0414019" friction="0.0523963"/>
+  </joint>
+  <joint name="index_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
+    <parent link="index_link_0"/>
+    <child link="index_link_1"/>
+    <dynamics damping="0.00587541" friction="0.0150275"/>
+  </joint>
+  <joint name="index_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
+    <parent link="index_link_1"/>
+    <child link="index_link_2"/>
+    <dynamics damping="0.010638" friction="0.00616359"/>
+  </joint>
+  <joint name="index_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
+    <parent link="index_link_2"/>
+    <child link="index_link_3"/>
+    <dynamics damping="0.0226948" friction="0.0227036"/>
+  </joint>
+  <joint name="middle_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 0" xyz="0.0537375 0.0087771 -0.0113"/>
+    <parent link="palm_link"/>
+    <child link="middle_link_0"/>
+    <dynamics damping="0.0414019" friction="0.0523963"/>
+  </joint>
+  <joint name="middle_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
+    <parent link="middle_link_0"/>
+    <child link="middle_link_1"/>
+    <dynamics damping="0.00587541" friction="0.0150275"/>
+  </joint>
+  <joint name="middle_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
+    <parent link="middle_link_1"/>
+    <child link="middle_link_2"/>
+    <dynamics damping="0.010638" friction="0.00616359"/>
+  </joint>
+  <joint name="middle_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
+    <parent link="middle_link_2"/>
+    <child link="middle_link_3"/>
+    <dynamics damping="0.0226948" friction="0.0227036"/>
+  </joint>
+  <joint name="ring_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 0.0872638888889" xyz="0.0514302 0.0538749 -0.0113"/>
+    <parent link="palm_link"/>
+    <child link="ring_link_0"/>
+    <dynamics damping="0.0414019" friction="0.0523963"/>
+  </joint>
+  <joint name="ring_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
+    <parent link="ring_link_0"/>
+    <child link="ring_link_1"/>
+    <dynamics damping="0.00587541" friction="0.0150275"/>
+  </joint>
+  <joint name="ring_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
+    <parent link="ring_link_1"/>
+    <child link="ring_link_2"/>
+    <dynamics damping="0.010638" friction="0.00616359"/>
+  </joint>
+  <joint name="ring_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
+    <parent link="ring_link_2"/>
+    <child link="ring_link_3"/>
+    <dynamics damping="0.0226948" friction="0.0227036"/>
+  </joint>
+  <joint name="thumb_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="0.279244444444" upper="1.57075" velocity="6.283"/>
+    <origin rpy="-1.57075 -1.57075 1.48348611111" xyz="-0.0367482 -0.0081281 -0.0295"/>
+    <parent link="palm_link"/>
+    <child link="thumb_link_0"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="thumb_joint_1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.331602777778" upper="1.15188333333" velocity="6.283"/>
+    <origin rpy="1.57075 0 0" xyz="0.005 0.0 0.0"/>
+    <parent link="thumb_link_0"/>
+    <child link="thumb_link_1"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="thumb_joint_2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
+    <origin rpy="3.1415 -1.57075 0.0" xyz="0 0 0.0554"/>
+    <parent link="thumb_link_1"/>
+    <child link="thumb_link_2"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <joint name="thumb_joint_3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="0.35" lower="-0.279244444444" upper="1.76273055556" velocity="6.283"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0514 0.0 0.0"/>
+    <parent link="thumb_link_2"/>
+    <child link="thumb_link_3"/>
+    <dynamics friction="0.035"/>
+  </joint>
+  <!-- ============================================================================= -->
+
+  <!-- Create a different *root* for the allegro hand -->
+  <!-- Note: this offset is just eyeballed... -->
+  <joint name="iiwa7_allegro" type="fixed">
+    <parent link="iiwa7_link_ee"/>
+    <child link="allegro_mount"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  
+</robot>

src/curobo/content/assets/robot/jaco/jaco_7n.urdf

@@ -0,0 +1,693 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from j2n7s300_standalone.xacro      | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!-- j2n7s300 refers to jaco v2 7DOF non-spherical service 3fingers -->
+<robot name="j2n7s300" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xi="http://www.w3.org/2001/XInclude">
+  <!-- links      		mesh_no
+   base           		0
+   shoulder       		1
+   arm            		2
+   forearm        		3
+   wrist          		4
+   arm_mico       		5
+   arm_half1 (7dof)		6
+   arm_half2 (7dof)		7
+   wrist_spherical_1  8
+   wrist_spherical_2  9
+
+   hand 3 finger  		55
+   hand_2finger   		56
+   finger_proximal		57
+   finger_distal      58
+-->
+  <!-- links      		mesh_no
+   base           		0
+   shoulder       		1
+   arm            		2
+   forearm        		3
+   wrist          		4
+   arm_mico       		5
+   arm_half1 (7dof)		6
+   arm_half2 (7dof)		7
+   wrist_spherical_1  8
+   wrist_spherical_2  9
+
+   hand 3 finger  		55
+   hand_2finger   		56
+   finger_proximal		57
+   finger_distal      58
+-->
+  <link name="root"/>
+  <!-- for gazebo -->
+  <link name="world"/>
+  <joint name="connect_root_and_world" type="fixed">
+    <child link="root"/>
+    <parent link="world"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>j2n7s300</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_link_base">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/base.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/base.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.46784"/>
+      <origin rpy="0 0 0" xyz="0 0 0.1255"/>
+      <inertia ixx="0.000951270861568" ixy="0" ixz="0" iyy="0.000951270861568" iyz="0" izz="0.00037427200000000004"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_base" type="fixed">
+    <parent link="root"/>
+    <child link="j2n7s300_link_base"/>
+    <axis xyz="0 0 0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="j2n7s300_link_1">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/shoulder.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/shoulder.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.7477"/>
+      <origin xyz="0 -0.002 -0.0605"/>
+      <inertia ixx="0.0015203172520400004" ixy="0" ixz="0" iyy="0.0015203172520400004" iyz="0" izz="0.00059816"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_1" type="continuous">
+    <parent link="j2n7s300_link_base"/>
+    <child link="j2n7s300_link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" velocity="0.6283185307179586"/>
+    <origin rpy="0 3.141592653589793 0" xyz="0 0 0.15675"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2n7s300_joint_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_1_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2n7s300_joint_1">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2n7s300_joint_1_ft_sensor_topic</topicName>
+      <jointName>j2n7s300_joint_1</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_link_2">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/arm_half_1.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/arm_half_1.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.8447"/>
+      <origin xyz="0 -0.103563213 0"/>
+      <inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.000380115" iyz="0" izz="0.00247073761701"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_2" type="revolute">
+    <parent link="j2n7s300_link_1"/>
+    <child link="j2n7s300_link_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="80" lower="0.5235987755982988" upper="5.759586531581287" velocity="0.6283185307179586"/>
+    <origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.0016 -0.11875"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2n7s300_joint_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_2_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2n7s300_joint_2">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2n7s300_joint_2_ft_sensor_topic</topicName>
+      <jointName>j2n7s300_joint_2</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_link_3">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/arm_half_2.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/arm_half_2.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.8447"/>
+      <origin xyz="0 0 -0.1022447445"/>
+      <inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.00247073761701" iyz="0" izz="0.000380115"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_3" type="continuous">
+    <parent link="j2n7s300_link_2"/>
+    <child link="j2n7s300_link_3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" velocity="0.6283185307179586"/>
+    <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 -0.205 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2n7s300_joint_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_3">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_3_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2n7s300_joint_3">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2n7s300_joint_3_ft_sensor_topic</topicName>
+      <jointName>j2n7s300_joint_3</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_link_4">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/forearm.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/forearm.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.6763"/>
+      <origin xyz="0 0.081 -0.0086"/>
+      <inertia ixx="0.0014202243190800001" ixy="0" ixz="0" iyy="0.000304335" iyz="0" izz="0.0014202243190800001"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_4" type="revolute">
+    <parent link="j2n7s300_link_3"/>
+    <child link="j2n7s300_link_4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" lower="0.5235987755982988" upper="5.759586531581287" velocity="0.6283185307179586"/>
+    <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.205"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2n7s300_joint_4_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_4">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_4_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2n7s300_joint_4">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2n7s300_joint_4_ft_sensor_topic</topicName>
+      <jointName>j2n7s300_joint_4</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_link_5">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/wrist.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/ring_small.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/wrist.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.426367"/>
+      <origin xyz="0 -0.037 -0.0642"/>
+      <inertia ixx="7.734969059999999e-05" ixy="0" ixz="0" iyy="7.734969059999999e-05" iyz="0" izz="0.0001428"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_5" type="continuous">
+    <parent link="j2n7s300_link_4"/>
+    <child link="j2n7s300_link_5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20" velocity="0.8377580409572781"/>
+    <origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.2073 -0.0114"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2n7s300_joint_5_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_5">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_5_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2n7s300_joint_5">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2n7s300_joint_5_ft_sensor_topic</topicName>
+      <jointName>j2n7s300_joint_5</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_link_6">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/wrist.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/ring_small.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/wrist.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.426367"/>
+      <origin xyz="0 -0.037 -0.0642"/>
+      <inertia ixx="7.734969059999999e-05" ixy="0" ixz="0" iyy="7.734969059999999e-05" iyz="0" izz="0.0001428"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_6" type="continuous">
+    <parent link="j2n7s300_link_5"/>
+    <child link="j2n7s300_link_6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20" velocity="0.8377580409572781"/>
+    <origin rpy="1.0471975511965976 0 3.141592653589793" xyz="0 -0.03703 -0.06414"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2n7s300_joint_6_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_6">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_6_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2n7s300_joint_6">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2n7s300_joint_6_ft_sensor_topic</topicName>
+      <jointName>j2n7s300_joint_6</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_link_7">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/hand_3finger.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/ring_small.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/hand_3finger.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.99"/>
+      <origin xyz="0 0 -0.06"/>
+      <inertia ixx="0.00034532361869999995" ixy="0" ixz="0" iyy="0.00034532361869999995" iyz="0" izz="0.0005815999999999999"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_7" type="continuous">
+    <parent link="j2n7s300_link_6"/>
+    <child link="j2n7s300_link_7"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20" velocity="0.8377580409572781"/>
+    <origin rpy="1.0471975511965976 0 3.141592653589793" xyz="0 -0.03703 -0.06414"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2n7s300_joint_7_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_7">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_7_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2n7s300_joint_7">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2n7s300_joint_7_ft_sensor_topic</topicName>
+      <jointName>j2n7s300_joint_7</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2n7s300_end_effector"/>
+  <joint name="j2n7s300_joint_end_effector" type="fixed">
+    <parent link="j2n7s300_link_7"/>
+    <child link="j2n7s300_end_effector"/>
+    <axis xyz="0 0 0"/>
+    <origin rpy="3.141592653589793 0 1.5707963267948966" xyz="0 0 -0.1600"/>
+  </joint>
+  <link name="j2n7s300_link_finger_1">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_finger_1" type="revolute">
+    <parent link="j2n7s300_link_7"/>
+    <child link="j2n7s300_link_finger_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="-1.7047873384941834 0.6476144647144773 1.67317415161155" xyz="0.00279 0.03126 -0.11467"/>
+    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+  </joint>
+  <transmission name="j2n7s300_joint_finger_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_finger_1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_finger_1_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2n7s300_link_finger_tip_1">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_finger_tip_1" type="revolute">
+    <parent link="j2n7s300_link_finger_1"/>
+    <child link="j2n7s300_link_finger_tip_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+    <limit effort="2" lower="0" upper="2" velocity="1"/>
+  </joint>
+  <transmission name="j2n7s300_joint_finger_tip_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_finger_tip_1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_finger_tip_1_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2n7s300_link_finger_2">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_finger_2" type="revolute">
+    <parent link="j2n7s300_link_7"/>
+    <child link="j2n7s300_link_finger_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="-1.570796327 .649262481663582 -1.38614049188413" xyz="0.02226 -0.02707 -0.11482"/>
+    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+  </joint>
+  <transmission name="j2n7s300_joint_finger_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_finger_2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_finger_2_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2n7s300_link_finger_tip_2">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_finger_tip_2" type="revolute">
+    <parent link="j2n7s300_link_finger_2"/>
+    <child link="j2n7s300_link_finger_tip_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+    <limit effort="2" lower="0" upper="2" velocity="1"/>
+  </joint>
+  <transmission name="j2n7s300_joint_finger_tip_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_finger_tip_2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_finger_tip_2_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2n7s300_link_finger_3">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_finger_3" type="revolute">
+    <parent link="j2n7s300_link_7"/>
+    <child link="j2n7s300_link_finger_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="-1.570796327 .649262481663582 -1.75545216211587" xyz="-0.02226 -0.02707 -0.11482"/>
+    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+  </joint>
+  <transmission name="j2n7s300_joint_finger_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_finger_3">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_finger_3_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2n7s300_link_finger_tip_3">
+    <visual>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2n7s300_joint_finger_tip_3" type="revolute">
+    <parent link="j2n7s300_link_finger_3"/>
+    <child link="j2n7s300_link_finger_tip_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+    <limit effort="2" lower="0" upper="2" velocity="1"/>
+  </joint>
+  <transmission name="j2n7s300_joint_finger_tip_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2n7s300_joint_finger_tip_3">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2n7s300_joint_finger_tip_3_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+</robot>
+

src/curobo/content/assets/robot/jaco/jaco_7s.urdf

@@ -0,0 +1,693 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from j2s7s300_standalone.xacro      | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!-- j2s7s300 refers to jaco v2 7DOF spherical 3fingers -->
+<robot name="jaco" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xi="http://www.w3.org/2001/XInclude">
+  <!-- links      		mesh_no
+   base           		0
+   shoulder       		1
+   arm            		2
+   forearm        		3
+   wrist          		4
+   arm_mico       		5
+   arm_half1 (7dof)		6
+   arm_half2 (7dof)		7
+   wrist_spherical_1  8
+   wrist_spherical_2  9
+
+   hand 3 finger  		55
+   hand_2finger   		56
+   finger_proximal		57
+   finger_distal      58
+-->
+  <!-- links      		mesh_no
+   base           		0
+   shoulder       		1
+   arm            		2
+   forearm        		3
+   wrist          		4
+   arm_mico       		5
+   arm_half1 (7dof)		6
+   arm_half2 (7dof)		7
+   wrist_spherical_1  8
+   wrist_spherical_2  9
+
+   hand 3 finger  		55
+   hand_2finger   		56
+   finger_proximal		57
+   finger_distal      58
+-->
+  <link name="root"/>
+  <!-- for gazebo -->
+  <link name="world"/>
+  <joint name="connect_root_and_world" type="fixed">
+    <child link="root"/>
+    <parent link="world"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>j2s7s300</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+      <legacyModeNS>true</legacyModeNS>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_link_base">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/base.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/base.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.46784"/>
+      <origin rpy="0 0 0" xyz="0 0 0.1255"/>
+      <inertia ixx="0.000951270861568" ixy="0" ixz="0" iyy="0.000951270861568" iyz="0" izz="0.00037427200000000004"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_base" type="fixed">
+    <parent link="root"/>
+    <child link="j2s7s300_link_base"/>
+    <axis xyz="0 0 0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="j2s7s300_link_1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/shoulder.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/shoulder.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.7477"/>
+      <origin xyz="0 -0.002 -0.0605"/>
+      <inertia ixx="0.0015203172520400004" ixy="0" ixz="0" iyy="0.0015203172520400004" iyz="0" izz="0.00059816"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_1" type="continuous">
+    <parent link="j2s7s300_link_base"/>
+    <child link="j2s7s300_link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" velocity="0.6283185307179586"/>
+    <origin rpy="0 3.141592653589793 0" xyz="0 0 0.15675"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2s7s300_joint_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_1_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2s7s300_joint_1">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2s7s300_joint_1_ft_sensor_topic</topicName>
+      <jointName>j2s7s300_joint_1</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_link_2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/arm_half_1.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/arm_half_1.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.8447"/>
+      <origin xyz="0 -0.103563213 0"/>
+      <inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.000380115" iyz="0" izz="0.00247073761701"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_2" type="revolute">
+    <parent link="j2s7s300_link_1"/>
+    <child link="j2s7s300_link_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="80" lower="0.8203047484373349" upper="5.462880558742252" velocity="0.6283185307179586"/>
+    <origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.0016 -0.11875"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2s7s300_joint_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_2_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2s7s300_joint_2">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2s7s300_joint_2_ft_sensor_topic</topicName>
+      <jointName>j2s7s300_joint_2</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_link_3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/arm_half_2.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/arm_half_2.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.8447"/>
+      <origin xyz="0 0 -0.1022447445"/>
+      <inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.00247073761701" iyz="0" izz="0.000380115"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_3" type="continuous">
+    <parent link="j2s7s300_link_2"/>
+    <child link="j2s7s300_link_3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" velocity="0.6283185307179586"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0 -0.205 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2s7s300_joint_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_3">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_3_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2s7s300_joint_3">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2s7s300_joint_3_ft_sensor_topic</topicName>
+      <jointName>j2s7s300_joint_3</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_link_4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/forearm.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ring_big.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/forearm.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.6763"/>
+      <origin xyz="0 0.081 -0.0086"/>
+      <inertia ixx="0.0014202243190800001" ixy="0" ixz="0" iyy="0.000304335" iyz="0" izz="0.0014202243190800001"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_4" type="revolute">
+    <parent link="j2s7s300_link_3"/>
+    <child link="j2s7s300_link_4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" lower="0.5235987755982988" upper="5.759586531581287" velocity="0.6283185307179586"/>
+    <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.205"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2s7s300_joint_4_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_4">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_4_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2s7s300_joint_4">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2s7s300_joint_4_ft_sensor_topic</topicName>
+      <jointName>j2s7s300_joint_4</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_link_5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/wrist_spherical_1.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ring_small.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/wrist_spherical_1.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.463"/>
+      <origin xyz="0 0.0028848942 -0.0541932613"/>
+      <inertia ixx="0.0004321316048000001" ixy="0" ixz="0" iyy="0.0004321316048000001" iyz="0" izz="9.260000000000001e-05"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_5" type="continuous">
+    <parent link="j2s7s300_link_4"/>
+    <child link="j2s7s300_link_5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20" velocity="0.8377580409572781"/>
+    <origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.2073 -0.0114"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2s7s300_joint_5_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_5">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_5_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2s7s300_joint_5">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2s7s300_joint_5_ft_sensor_topic</topicName>
+      <jointName>j2s7s300_joint_5</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_link_6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/wrist_spherical_2.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ring_small.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/wrist_spherical_2.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.463"/>
+      <origin xyz="0 0.0497208855 -0.0028562765"/>
+      <inertia ixx="0.0004321316048000001" ixy="0" ixz="0" iyy="9.260000000000001e-05" iyz="0" izz="0.0004321316048000001"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_6" type="revolute">
+    <parent link="j2s7s300_link_5"/>
+    <child link="j2s7s300_link_6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20" lower="1.1344640137963142" upper="5.148721293383272" velocity="0.8377580409572781"/>
+    <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.10375"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2s7s300_joint_6_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_6">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_6_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2s7s300_joint_6">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2s7s300_joint_6_ft_sensor_topic</topicName>
+      <jointName>j2s7s300_joint_6</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_link_7">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/hand_3finger.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ring_small.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/hand_3finger.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.99"/>
+      <origin xyz="0 0 -0.06"/>
+      <inertia ixx="0.00034532361869999995" ixy="0" ixz="0" iyy="0.00034532361869999995" iyz="0" izz="0.0005815999999999999"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_7" type="continuous">
+    <parent link="j2s7s300_link_6"/>
+    <child link="j2s7s300_link_7"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20" velocity="0.8377580409572781"/>
+    <origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.10375 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="j2s7s300_joint_7_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_7">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_7_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>160</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--For torque sensing in simulation-->
+  <gazebo reference="j2s7s300_joint_7">
+    <provideFeedback>true</provideFeedback>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
+      <updateRate>30.0</updateRate>
+      <topicName>j2s7s300_joint_7_ft_sensor_topic</topicName>
+      <jointName>j2s7s300_joint_7</jointName>
+    </plugin>
+  </gazebo>
+  <link name="j2s7s300_end_effector"/>
+  <joint name="j2s7s300_joint_end_effector" type="fixed">
+    <parent link="j2s7s300_link_7"/>
+    <child link="j2s7s300_end_effector"/>
+    <axis xyz="0 0 0"/>
+    <origin rpy="3.141592653589793 0 1.5707963267948966" xyz="0 0 -0.1600"/>
+  </joint>
+  <link name="j2s7s300_link_finger_1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/finger_proximal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/finger_proximal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_finger_1" type="revolute">
+    <parent link="j2s7s300_link_7"/>
+    <child link="j2s7s300_link_finger_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="-1.7047873384941834 0.6476144647144773 1.67317415161155" xyz="0.00279 0.03126 -0.11467"/>
+    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+  </joint>
+  <transmission name="j2s7s300_joint_finger_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_finger_1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_finger_1_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2s7s300_link_finger_tip_1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/finger_distal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/finger_distal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_finger_tip_1" type="revolute">
+    <parent link="j2s7s300_link_finger_1"/>
+    <child link="j2s7s300_link_finger_tip_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+    <limit effort="2" lower="0" upper="2" velocity="1"/>
+  </joint>
+  <transmission name="j2s7s300_joint_finger_tip_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_finger_tip_1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_finger_tip_1_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2s7s300_link_finger_2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/finger_proximal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/finger_proximal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_finger_2" type="revolute">
+    <parent link="j2s7s300_link_7"/>
+    <child link="j2s7s300_link_finger_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="-1.570796327 .649262481663582 -1.38614049188413" xyz="0.02226 -0.02707 -0.11482"/>
+    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+  </joint>
+  <transmission name="j2s7s300_joint_finger_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_finger_2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_finger_2_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2s7s300_link_finger_tip_2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/finger_distal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/finger_distal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_finger_tip_2" type="revolute">
+    <parent link="j2s7s300_link_finger_2"/>
+    <child link="j2s7s300_link_finger_tip_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+    <limit effort="2" lower="0" upper="2" velocity="1"/>
+  </joint>
+  <transmission name="j2s7s300_joint_finger_tip_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_finger_tip_2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_finger_tip_2_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2s7s300_link_finger_3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/finger_proximal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/finger_proximal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_finger_3" type="revolute">
+    <parent link="j2s7s300_link_7"/>
+    <child link="j2s7s300_link_finger_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="-1.570796327 .649262481663582 -1.75545216211587" xyz="-0.02226 -0.02707 -0.11482"/>
+    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+  </joint>
+  <transmission name="j2s7s300_joint_finger_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_finger_3">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_finger_3_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="j2s7s300_link_finger_tip_3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/finger_distal.dae"/>
+      </geometry>
+      <material name="carbon_fiber">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/finger_distal.dae"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0.022 0 0"/>
+      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+    </inertial>
+  </link>
+  <joint name="j2s7s300_joint_finger_tip_3" type="revolute">
+    <parent link="j2s7s300_link_finger_3"/>
+    <child link="j2s7s300_link_finger_tip_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+    <limit effort="2" lower="0" upper="2" velocity="1"/>
+  </joint>
+  <transmission name="j2s7s300_joint_finger_tip_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="j2s7s300_joint_finger_tip_3">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="j2s7s300_joint_finger_tip_3_actuator">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+</robot>
+

src/curobo/content/assets/robot/kinova/README.md

@@ -0,0 +1,2 @@
+The files in this directory come from:
+https://github.com/Kinovarobotics/ros_kortex/tree/noetic-devel/kortex_description

src/curobo/content/assets/robot/kinova/kinova_gen3_7dof.urdf

@@ -0,0 +1,577 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from robots/gen3_robotiq_2f_85.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="gen3_robotiq_2f_85">
+  <!-- Run the macros -->
+  <!-- For gazebo and DART -->
+
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.000648 -0.000166 0.084487"/>
+      <mass value="1.697"/>
+      <inertia ixx="0.004622" ixy="9E-06" ixz="6E-05" iyy="0.004495" iyz="9E-06" izz="0.002079"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/base_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="shoulder_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-2.3E-05 -0.010364 -0.07336"/>
+      <mass value="1.3773"/>
+      <inertia ixx="0.00457" ixy="1E-06" ixz="2E-06" iyy="0.004831" iyz="0.000448" izz="0.001409"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_1" type="revolute">
+    <origin rpy="3.1416 2.7629E-18 -4.9305E-36" xyz="0 0 0.15643"/>
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="39" lower="-6.0" upper="6.0" velocity="1.3963"/>
+  </joint>
+  <link name="half_arm_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-4.4E-05 -0.09958 -0.013278"/>
+      <mass value="1.1636"/>
+      <inertia ixx="0.011088" ixy="5E-06" ixz="0" iyy="0.001072" iyz="-0.000691" izz="0.011255"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_2" type="revolute">
+    <origin rpy="1.5708 2.1343E-17 -1.1102E-16" xyz="0 0.005375 -0.12838"/>
+    <parent link="shoulder_link"/>
+    <child link="half_arm_1_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="39" lower="-2.41" upper="2.41" velocity="1.3963"/>
+  </joint>
+  <link name="half_arm_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-4.4E-05 -0.006641 -0.117892"/>
+      <mass value="1.1636"/>
+      <inertia ixx="0.010932" ixy="0" ixz="-7E-06" iyy="0.011127" iyz="0.000606" izz="0.001043"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_3" type="revolute">
+    <origin rpy="-1.5708 1.2326E-32 -2.9122E-16" xyz="0 -0.21038 -0.006375"/>
+    <parent link="half_arm_1_link"/>
+    <child link="half_arm_2_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="39"  lower="-6.0" upper="6.0" velocity="1.3963"/>
+  </joint>
+  <link name="forearm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-1.8E-05 -0.075478 -0.015006"/>
+      <mass value="0.9302"/>
+      <inertia ixx="0.008147" ixy="-1E-06" ixz="0" iyy="0.000631" iyz="-0.0005" izz="0.008316"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/forearm_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/forearm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_4" type="revolute">
+    <origin rpy="1.5708 -6.6954E-17 -1.6653E-16" xyz="0 0.006375 -0.21038"/>
+    <parent link="half_arm_2_link"/>
+    <child link="forearm_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="39" lower="-2.66" upper="2.66" velocity="1.3963"/>
+  </joint>
+  <link name="spherical_wrist_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="1E-06 -0.009432 -0.063883"/>
+      <mass value="0.6781"/>
+      <inertia ixx="0.001596" ixy="0" ixz="0" iyy="0.001607" iyz="0.000256" izz="0.000399"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_5" type="revolute">
+    <origin rpy="-1.5708 2.2204E-16 -6.373E-17" xyz="0 -0.20843 -0.006375"/>
+    <parent link="forearm_link"/>
+    <child link="spherical_wrist_1_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="9" lower="-6.0" upper="6.0"  velocity="1.2218"/>
+  </joint>
+  <link name="spherical_wrist_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="1E-06 -0.045483 -0.00965"/>
+      <mass value="0.6781"/>
+      <inertia ixx="0.001641" ixy="0" ixz="0" iyy="0.00041" iyz="-0.000278" izz="0.001641"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_6" type="revolute">
+    <origin rpy="1.5708 9.2076E-28 -8.2157E-15" xyz="0 0.00017505 -0.10593"/>
+    <parent link="spherical_wrist_1_link"/>
+    <child link="spherical_wrist_2_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="9" lower="-2.23" upper="2.23" velocity="1.2218"/>
+  </joint>
+  <link name="bracelet_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.000281 0.011402 -0.029798"/>
+      <mass value="0.5"/>
+      <inertia ixx="0.000587" ixy="3E-06" ixz="3E-06" iyy="0.000369" iyz="-0.000118" izz="0.000609"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_7" type="revolute">
+    <origin rpy="-1.5708 -5.5511E-17 9.6396E-17" xyz="0 -0.10593 -0.00017505"/>
+    <parent link="spherical_wrist_2_link"/>
+    <child link="bracelet_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="9"  lower="-6.0" upper="6.0" velocity="1.2218"/>
+  </joint>
+  <link name="end_effector_link"/>
+  <joint name="end_effector" type="fixed">
+    <origin rpy="3.14159265358979 1.09937075168372E-32 0" xyz="0 0 -0.0615250000000001"/>
+    <parent link="bracelet_link"/>
+    <child link="end_effector_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="camera_link"/>
+  <joint name="camera_module" type="fixed">
+    <origin rpy="3.14159265358979 3.14159265358979 0" xyz="0 0.05639 -0.00305"/>
+    <parent link="end_effector_link"/>
+    <child link="camera_link"/>
+  </joint>
+  <link name="camera_depth_frame"/>
+  <joint name="depth_module" type="fixed">
+    <origin rpy="3.14159265358979 3.14159265358979 0" xyz="0.0275 0.066 -0.00305"/>
+    <parent link="end_effector_link"/>
+    <child link="camera_depth_frame"/>
+  </joint>
+  <link name="camera_color_frame"/>
+  <joint name="color_module" type="fixed">
+    <origin rpy="3.14159265358979 3.14159265358979 0" xyz="0 0.05639 -0.00305"/>
+    <parent link="end_effector_link"/>
+    <child link="camera_color_frame"/>
+  </joint>
+  <!-- This line was removed by Kinova because we replaced the transmission file with our own
+    <xacro:include filename="$(find robotiq_2f_85_gripper_visualization)/urdf/robotiq_arg2f_transmission.xacro" /> -->
+  <!-- Tool frame used by the arm -->
+  <link name="tool_frame"/>
+  <joint name="tool_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.120"/>
+    <parent link="end_effector_link"/>
+    <child link="tool_frame"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- This joint was added by Kinova -->
+  <joint name="gripper_base_joint" type="fixed">
+    <parent link="end_effector_link"/>
+    <child link="robotiq_arg2f_base_link"/>
+    <origin rpy="0.0 0.0 1.57" xyz="0.0 0.0 0.0"/>
+  </joint>
+  <link name="robotiq_arg2f_base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="8.625E-08 -4.6583E-06 0.03145"/>
+      <mass value="0.22652"/>
+      <inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!-- The DAE file doesn't show well in Gazebo so we're using the STL instead -->
+        <!-- <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_base_link.dae" scale="0.001 0.001 0.001"/> -->
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_base_link.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_base_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="robotiq_arg2f_base_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <link name="left_outer_knuckle">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
+      <mass value="0.00853198276973456"/>
+      <inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="left_outer_finger">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
+      <mass value="0.022614240507152"/>
+      <inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="left_inner_finger">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
+      <mass value="0.0104003125914103"/>
+      <inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="left_inner_finger_pad">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.022 0.00635 0.0375"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.9 0.9 0.9 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.022 0.00635 0.0375"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.9 0.0 0.0 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="left_inner_knuckle">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
+      <mass value="0.0271177346495152"/>
+      <inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="right_outer_knuckle">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
+      <mass value="0.00853198276973456"/>
+      <inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="right_outer_finger">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
+      <mass value="0.022614240507152"/>
+      <inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="right_inner_finger">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
+      <mass value="0.0104003125914103"/>
+      <inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="right_inner_finger_pad">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.022 0.00635 0.0375"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.9 0.9 0.9 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.022 0.00635 0.0375"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.9 0.0 0.0 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="right_inner_knuckle">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
+      <mass value="0.0271177346495152"/>
+      <inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_outer_knuckle_joint" type="fixed">
+    <origin rpy="0 0 3.1415" xyz="0 -0.0306011 0.054904"/>
+    <parent link="robotiq_arg2f_base_link"/>
+    <child link="left_outer_knuckle"/>
+    <!--axis xyz="1 0 0"/>
+    <limit effort="1000" lower="-0.0" upper="0.8" velocity="2.0"/-->
+  </joint>
+  <joint name="left_outer_finger_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
+    <parent link="left_outer_knuckle"/>
+    <child link="left_outer_finger"/>
+  </joint>
+  <joint name="left_inner_knuckle_joint" type="fixed">
+    <origin rpy="0 0 3.141592653589793" xyz="0 -0.0127 0.06142"/>
+    <parent link="robotiq_arg2f_base_link"/>
+    <child link="left_inner_knuckle"/>
+    
+  </joint>
+  <joint name="left_inner_finger_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
+    <parent link="left_outer_finger"/>
+    <child link="left_inner_finger"/>
+
+  </joint>
+  <joint name="left_inner_finger_pad_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.0220203446692936 0.03242"/>
+    <parent link="left_inner_finger"/>
+    <child link="left_inner_finger_pad"/>
+  </joint>
+  <joint name="right_outer_knuckle_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0306011 0.054904"/>
+    <parent link="robotiq_arg2f_base_link"/>
+    <child link="right_outer_knuckle"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="1000" lower="0" upper="0.8" velocity="2.0"/>
+  </joint>
+  <joint name="right_outer_finger_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
+    <parent link="right_outer_knuckle"/>
+    <child link="right_outer_finger"/>
+  </joint>
+  <joint name="right_inner_knuckle_joint" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0.0127 0.06142"/>
+    <parent link="robotiq_arg2f_base_link"/>
+    <child link="right_inner_knuckle"/>
+    
+  </joint>
+  <joint name="right_inner_finger_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
+    <parent link="right_outer_finger"/>
+    <child link="right_inner_finger"/>
+    
+  </joint>
+  <joint name="right_inner_finger_pad_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.0220203446692936 0.03242"/>
+    <parent link="right_inner_finger"/>
+    <child link="right_inner_finger_pad"/>
+  </joint>
+</robot>

src/curobo/content/assets/robot/kinova/kortex_description/grippers/robotiq_2f_85/LICENSE

@@ -0,0 +1,23 @@
+Copyright (c) 2013, ROS-Industrial
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+  Redistributions in binary form must reproduce the above copyright notice, this
+  list of conditions and the following disclaimer in the documentation and/or
+  other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/curobo/content/assets/robot/kinova/kortex_description/grippers/robotiq_2f_85/README.md

@@ -0,0 +1,13 @@
+This is the README file from Robotiq's package for the 2F-85 gripper. The package's license file can also be found in this folder.
+We used commit `4e3196c1c6a83f54acfa3dd8cf02b575ebba3e53` from [Robotiq's ROS Industrial Github repository](https://github.com/ros-industrial/robotiq)
+
+# Robotiq 85mm 2-Finger-Adaptive-Gripper 
+
+This package contains the URDF files describing the 85mm stroke gripper from robotiq, also known as series **C3**.
+
+## Robot Visual
+![85](https://user-images.githubusercontent.com/8356912/49428405-45a6ef00-f7a6-11e8-822b-c6870c39d445.png)
+
+## Robot Collision 
+![852](https://user-images.githubusercontent.com/8356912/49428404-450e5880-f7a6-11e8-82a8-564247ebe7fc.png)
+

src/curobo/content/assets/robot/techman/tm_description/urdf/tm12-nominal.urdf

@@ -0,0 +1,331 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from tm12.urdf.xacro                | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="tm12">
+  <gazebo reference="link_1">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_2">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_3">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_4">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_5">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_6">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+  <transmission name="trans_1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_1">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_2">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_3">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_3">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_4">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_4">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_5">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_5">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_6">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_3_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_6">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <material name="none">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="lightgrey">
+    <color rgba="0.5 0.5 0.5 1.0"/>
+  </material>
+  <material name="grey">
+    <color rgba="0.75 0.75 0.75 1.0"/>
+  </material>
+  <material name="white">
+    <color rgba="0.95 0.95 0.95 1.0"/>
+  </material>
+  <material name="red">
+    <color rgba="0.95 0.0 0.0 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="1.0 0.5 0.0 1.0"/>
+  </material>
+  <material name="yellow">
+    <color rgba="1.0 1.0 0.0 1.0"/>
+  </material>
+  <material name="green">
+    <color rgba="0.0 1.0 0.0 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.0 1.0 1.0"/>
+  </material>
+  <material name="indigo">
+    <color rgba="0.3 0.3 0.6 1.0"/>
+  </material>
+  <material name="violet">
+    <color rgba="0.6 0.0 1.0 1.0"/>
+  </material>
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="darkolive">
+    <color rgba="0.3 0.3 0.25 1.0"/>
+  </material>
+  <!--LinkDescription-->
+  <link name="link_0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tm12-base.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tm12-base_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.0"/>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <inertia ixx="0.00110833289" ixy="0.0" ixz="0.0" iyy="0.00110833289" iyz="0.0" izz="0.0018"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_1_joint" type="revolute">
+    <parent link="link_0"/>
+    <child link="link_1"/>
+    <origin rpy="0.000000 -0.000000 0.000000" xyz="0.000000 0.000000 0.165200"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="353" lower="-4.71238898038469" upper="4.71238898038469" velocity="2.0943951023931953"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tmr_750w_01.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tmr_750w_01_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="7.6"/>
+      <inertia ixx="0.020289334" ixy="0.000000" ixz="0.000000" iyy="0.020289334" iyz="0.000000" izz="0.021396270999999998"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_2_joint" type="revolute">
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <origin rpy="-1.570796 -1.570796 0.000000" xyz="0.000000 0.000000 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="353" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.0943951023931953"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tm12-arm1.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tm12-arm1_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="14.0239"/>
+      <inertia ixx="0.071505715" ixy="0.000000" ixz="0.000000" iyy="1.1758788999999998" iyz="0.000000" izz="1.2033932999999999"/>
+    </inertial>
+  </link>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <origin rpy="0.000000 -0.000000 0.000000" xyz="0.636100 0.000000 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="157" lower="-2.897246558310587" upper="2.897246558310587" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tm12-arm2.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tm12-arm2_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="3.3577"/>
+      <inertia ixx="0.009755469" ixy="0.000000" ixz="0.000000" iyy="0.16334719" iyz="0.000000" izz="0.16656678"/>
+    </inertial>
+  </link>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <origin rpy="0.000000 -0.000000 1.570796" xyz="0.557900 0.000000 -0.156300"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="54" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm5-900/visual/tmr_100w_01.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm5-900/collision/tmr_100w_01_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="1.576"/>
+      <inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
+    </inertial>
+  </link>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.106000 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="54" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm5-900/visual/tmr_100w_02.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm5-900/collision/tmr_100w_02_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="1.576"/>
+      <inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
+    </inertial>
+  </link>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.113150 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="54" lower="-4.71238898038469" upper="4.71238898038469" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm5-900/visual/tmr_ee.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm5-900/collision/tmr_ee_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="0.65"/>
+      <inertia ixx="0.000774544" ixy="0.000000" ixz="0.000000" iyy="0.001383811" iyz="0.000000" izz="0.001559496"/>
+    </inertial>
+  </link>
+  <joint name="flange_fixed_joint" type="fixed">
+    <parent link="link_6"/>
+    <child link="flange_link"/>
+    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+  </joint>
+  <link name="flange_link"/>
+  <link name="base"/>
+  <joint name="base_fixed_joint" type="fixed">
+    <parent link="base"/>
+    <child link="link_0"/>
+    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+  </joint>
+  <link name="tool0"/>
+  <joint name="flange_link-tool0" type="fixed">
+    <parent link="flange_link"/>
+    <child link="tool0"/>
+    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+  </joint>
+  <!--LinkDescription-->
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+</robot>
+

src/curobo/content/assets/robot/techman/tm_description/urdf/tm12x-nominal.urdf

@@ -0,0 +1,331 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from tm12x.urdf.xacro               | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="tm12x">
+  <gazebo reference="link_1">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_2">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_3">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_4">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_5">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link_6">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+  <transmission name="trans_1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_1">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_2">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_3">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_3">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_4">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_4">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_5">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_5">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="trans_6">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_3_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_6">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <material name="none">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="lightgrey">
+    <color rgba="0.5 0.5 0.5 1.0"/>
+  </material>
+  <material name="grey">
+    <color rgba="0.75 0.75 0.75 1.0"/>
+  </material>
+  <material name="white">
+    <color rgba="0.95 0.95 0.95 1.0"/>
+  </material>
+  <material name="red">
+    <color rgba="0.95 0.0 0.0 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="1.0 0.5 0.0 1.0"/>
+  </material>
+  <material name="yellow">
+    <color rgba="1.0 1.0 0.0 1.0"/>
+  </material>
+  <material name="green">
+    <color rgba="0.0 1.0 0.0 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.0 1.0 1.0"/>
+  </material>
+  <material name="indigo">
+    <color rgba="0.3 0.3 0.6 1.0"/>
+  </material>
+  <material name="violet">
+    <color rgba="0.6 0.0 1.0 1.0"/>
+  </material>
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="darkolive">
+    <color rgba="0.3 0.3 0.25 1.0"/>
+  </material>
+  <!--LinkDescription-->
+  <link name="link_0">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tm12-base.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tm12-base_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.0"/>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <inertia ixx="0.00110833289" ixy="0.0" ixz="0.0" iyy="0.00110833289" iyz="0.0" izz="0.0018"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_1_joint" type="revolute">
+    <parent link="link_0"/>
+    <child link="link_1"/>
+    <origin rpy="0.000000 -0.000000 0.000000" xyz="0.000000 0.000000 0.165200"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="353" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_1">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tmr_750w_01.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tmr_750w_01_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="7.6"/>
+      <inertia ixx="0.020289334" ixy="0.000000" ixz="0.000000" iyy="0.020289334" iyz="0.000000" izz="0.021396270999999998"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_2_joint" type="revolute">
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <origin rpy="-1.570796 -1.570796 0.000000" xyz="0.000000 0.000000 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="353" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_2">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tm12-arm1.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tm12-arm1_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="14.0239"/>
+      <inertia ixx="0.071505715" ixy="0.000000" ixz="0.000000" iyy="1.1758788999999998" iyz="0.000000" izz="1.2033932999999999"/>
+    </inertial>
+  </link>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <origin rpy="0.000000 -0.000000 0.000000" xyz="0.636100 0.000000 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="157" lower="-2.897246558310587" upper="2.897246558310587" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_3">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm12/visual/tm12-arm2.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm12/collision/tm12-arm2_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="3.3577"/>
+      <inertia ixx="0.009755469" ixy="0.000000" ixz="0.000000" iyy="0.16334719" iyz="0.000000" izz="0.16656678"/>
+    </inertial>
+  </link>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <origin rpy="0.000000 -0.000000 1.570796" xyz="0.557900 0.000000 -0.156300"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="54" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_4">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm5-900/visual/tmr_100w_01.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm5-900/collision/tmr_100w_01_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="1.576"/>
+      <inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
+    </inertial>
+  </link>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.106000 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="54" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_5">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm5-900/visual/tmr_100w_02.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm5-900/collision/tmr_100w_02_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="1.576"/>
+      <inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
+    </inertial>
+  </link>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.113150 0.000000"/>
+    <axis xyz="0 0 1"/>
+    <!--limit-->
+    <limit effort="54" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="link_6">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/tm5-900/visual/tmr_iox.obj"/>
+      </geometry>
+      <material name="none"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/tm5-900/collision/tmr_iox_c.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="0.31"/>
+      <inertia ixx="0.000226642" ixy="0.000000" ixz="0.000000" iyy="0.000226642" iyz="0.000000" izz="0.000286595"/>
+    </inertial>
+  </link>
+  <joint name="flange_fixed_joint" type="fixed">
+    <parent link="link_6"/>
+    <child link="flange_link"/>
+    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+  </joint>
+  <link name="flange_link"/>
+  <link name="base"/>
+  <joint name="base_fixed_joint" type="fixed">
+    <parent link="base"/>
+    <child link="link_0"/>
+    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+  </joint>
+  <link name="tool0"/>
+  <joint name="flange_link-tool0" type="fixed">
+    <parent link="flange_link"/>
+    <child link="tool0"/>
+    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+  </joint>
+  <!--LinkDescription-->
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+</robot>
+

src/curobo/content/assets/robot/ur_description/dual_ur10e.urdf

@@ -0,0 +1,556 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ur_description/urdf/ur10e.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur10e_robot">
+  <!--
+    Base UR robot series xacro macro.
+
+    NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
+    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
+    (but note: that .xacro must still be processed by the xacro command).
+
+    For use in '.launch' files: use one of the 'load_urX.launch' convenience
+    launch files.
+
+    This file models the base kinematic chain of a UR robot, which then gets
+    parameterised by various configuration files to convert it into a UR3(e),
+    UR5(e), UR10(e) or UR16e.
+
+    NOTE: the default kinematic parameters (ie: link lengths, frame locations,
+    offets, etc) do not correspond to any particular robot. They are defaults
+    only. There WILL be non-zero offsets between the Forward Kinematics results
+    in TF (ie: robot_state_publisher) and the values reported by the Teach
+    Pendant.
+
+    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
+    parameter MUST point to a .yaml file containing the appropriate values for
+    the targetted robot.
+
+    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
+    described in the readme of that repository to extract the kinematic
+    calibration from the controller and generate the required .yaml file.
+
+    Main author of the migration to yaml configs: Ludovic Delval.
+
+    Contributors to previous versions (in no particular order):
+
+     - Felix Messmer
+     - Kelsey Hawkins
+     - Wim Meeussen
+     - Shaun Edwards
+     - Nadia Hammoudeh Garcia
+     - Dave Hershberger
+     - G. vd. Hoorn
+     - Philip Long
+     - Dave Coleman
+     - Miguel Prada
+     - Mathias Luedtke
+     - Marcel Schnirring
+     - Felix von Drigalski
+     - Felix Exner
+     - Jimmy Da Silva
+     - Ajit Krisshna N L
+     - Muhammad Asif Rana
+  -->
+  <!--
+    NOTE: the macro defined in this file is NOT part of the public API of this
+          package. Users CANNOT rely on this file being available, or stored in
+          this location. Nor can they rely on the existence of the macro.
+  -->
+  
+  <!-- links: main serial chain -->
+  <link name="base_fixture_link"/>
+  <link name="base_link"/>
+
+ <link name="base_link_1"/>
+ 
+
+
+    <!-- joints: main serial chain -->
+  <joint name="base_fixture_j_base_link" type="fixed">
+    <parent link="base_fixture_link"/>
+    <child link="base_link"/>
+    <origin rpy="0 0 0" xyz="0.75 0.75 0"/>
+  </joint>
+
+
+    <!-- joints: main serial chain -->
+  <joint name="base_fixture_j_base_link_1" type="fixed">
+    <parent link="base_fixture_link"/>
+    <child link="base_link_1"/>
+    <origin rpy="0 0 0.0" xyz="-0.75 0.75 0"/>
+  </joint>
+
+
+  
+  <link name="base_link_inertia">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <joint name="base_link_base_link_inertia" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_link_inertia"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+
+  <link name="shoulder_link">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
+      <inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
+    </inertial>
+  </link>
+  <link name="forearm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
+      <inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
+      <inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
+    </inertial>
+  </link>
+  
+  
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link_inertia"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1807"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0 0 0" xyz="-0.6127 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+
+  <link name="flange"/>
+  <joint name="wrist_3-flange" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="flange"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="flange-tool0" type="fixed">
+    <!-- default toolframe: X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange"/>
+    <child link="tool0"/>
+  </joint>
+  <!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
+
+
+  
+  <link name="base_link_inertia_link_1">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <link name="shoulder_link_1">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link_1">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
+      <inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
+    </inertial>
+  </link>
+  <link name="forearm_link_1">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
+      <inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link_1">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link_1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link_1">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
+      <inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
+    </inertial>
+  </link>
+  
+  <joint name="base_link_base_link_inertia_joint_1" type="fixed">
+    <parent link="base_link_1"/>
+    <child link="base_link_inertia_link_1"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+
+  <joint name="shoulder_pan_joint_1" type="revolute">
+    <parent link="base_link_inertia_link_1"/>
+    <child link="shoulder_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1807"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint_1" type="revolute">
+    <parent link="shoulder_link_1"/>
+    <child link="upper_arm_link_1"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint_1" type="revolute">
+    <parent link="upper_arm_link_1"/>
+    <child link="forearm_link_1"/>
+    <origin rpy="0 0 0" xyz="-0.6127 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint_1" type="revolute">
+    <parent link="forearm_link_1"/>
+    <child link="wrist_1_link_1"/>
+    <origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint_1" type="revolute">
+    <parent link="wrist_1_link_1"/>
+    <child link="wrist_2_link_1"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint_1" type="revolute">
+    <parent link="wrist_2_link_1"/>
+    <child link="wrist_3_link_1"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange_1"/>
+  <joint name="wrist_3_flange_1" type="fixed">
+    <parent link="wrist_3_link_1"/>
+    <child link="flange_1"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool1"/>
+  <joint name="flange_tool_1" type="fixed">
+    <!-- default toolframe: X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange_1"/>
+    <child link="tool1"/>
+  </joint>
+
+
+</robot>

src/curobo/content/assets/robot/ur_description/tri_ur10e.urdf

@@ -0,0 +1,953 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ur_description/urdf/ur10e.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur10e_robot">
+  <!--
+    Base UR robot series xacro macro.
+
+    NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
+    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
+    (but note: that .xacro must still be processed by the xacro command).
+
+    For use in '.launch' files: use one of the 'load_urX.launch' convenience
+    launch files.
+
+    This file models the base kinematic chain of a UR robot, which then gets
+    parameterised by various configuration files to convert it into a UR3(e),
+    UR5(e), UR10(e) or UR16e.
+
+    NOTE: the default kinematic parameters (ie: link lengths, frame locations,
+    offets, etc) do not correspond to any particular robot. They are defaults
+    only. There WILL be non-zero offsets between the Forward Kinematics results
+    in TF (ie: robot_state_publisher) and the values reported by the Teach
+    Pendant.
+
+    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
+    parameter MUST point to a .yaml file containing the appropriate values for
+    the targetted robot.
+
+    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
+    described in the readme of that repository to extract the kinematic
+    calibration from the controller and generate the required .yaml file.
+
+    Main author of the migration to yaml configs: Ludovic Delval.
+
+    Contributors to previous versions (in no particular order):
+
+     - Felix Messmer
+     - Kelsey Hawkins
+     - Wim Meeussen
+     - Shaun Edwards
+     - Nadia Hammoudeh Garcia
+     - Dave Hershberger
+     - G. vd. Hoorn
+     - Philip Long
+     - Dave Coleman
+     - Miguel Prada
+     - Mathias Luedtke
+     - Marcel Schnirring
+     - Felix von Drigalski
+     - Felix Exner
+     - Jimmy Da Silva
+     - Ajit Krisshna N L
+     - Muhammad Asif Rana
+  -->
+  <!--
+    NOTE: the macro defined in this file is NOT part of the public API of this
+          package. Users CANNOT rely on this file being available, or stored in
+          this location. Nor can they rely on the existence of the macro.
+  -->
+  
+  <!-- links: main serial chain -->
+  <link name="base_fixture_link"/>
+  <link name="base_link"/>
+
+ <link name="base_link_1"/>
+ <link name="base_link_2"/>
+
+
+    <!-- joints: main serial chain -->
+  <joint name="base_fixture_j_base_link" type="fixed">
+    <parent link="base_fixture_link"/>
+    <child link="base_link"/>
+    <origin rpy="0 0 0" xyz="0.75 0.75 0"/>
+  </joint>
+
+
+    <!-- joints: main serial chain -->
+  <joint name="base_fixture_j_base_link_1" type="fixed">
+    <parent link="base_fixture_link"/>
+    <child link="base_link_1"/>
+    <origin rpy="0 0 0.0" xyz="-0.75 0.75 0"/>
+  </joint>
+
+    <!-- joints: main serial chain -->
+  <joint name="base_fixture_j_base_link_2" type="fixed">
+    <parent link="base_fixture_link"/>
+    <child link="base_link_2"/>
+    <origin rpy="0 0 3.14" xyz="0.0 -0.75 0"/>
+  </joint>
+
+
+
+  
+  <link name="base_link_inertia">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <joint name="base_link_base_link_inertia" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_link_inertia"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+
+  <link name="shoulder_link">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
+      <inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
+    </inertial>
+  </link>
+  <link name="forearm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
+      <inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
+      <inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
+    </inertial>
+  </link>
+  
+  
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link_inertia"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1807"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0 0 0" xyz="-0.6127 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+
+  <link name="flange"/>
+  <joint name="wrist_3-flange" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="flange"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="flange-tool0" type="fixed">
+    <!-- default toolframe: X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange"/>
+    <child link="tool0"/>
+  </joint>
+  <!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
+
+
+  
+  <link name="base_link_inertia_link_1">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <link name="shoulder_link_1">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link_1">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
+      <inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
+    </inertial>
+  </link>
+  <link name="forearm_link_1">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
+      <inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link_1">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link_1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link_1">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
+      <inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
+    </inertial>
+  </link>
+  
+  <joint name="base_link_base_link_inertia_joint_1" type="fixed">
+    <parent link="base_link_1"/>
+    <child link="base_link_inertia_link_1"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+
+  <joint name="shoulder_pan_joint_1" type="revolute">
+    <parent link="base_link_inertia_link_1"/>
+    <child link="shoulder_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1807"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint_1" type="revolute">
+    <parent link="shoulder_link_1"/>
+    <child link="upper_arm_link_1"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint_1" type="revolute">
+    <parent link="upper_arm_link_1"/>
+    <child link="forearm_link_1"/>
+    <origin rpy="0 0 0" xyz="-0.6127 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint_1" type="revolute">
+    <parent link="forearm_link_1"/>
+    <child link="wrist_1_link_1"/>
+    <origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint_1" type="revolute">
+    <parent link="wrist_1_link_1"/>
+    <child link="wrist_2_link_1"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint_1" type="revolute">
+    <parent link="wrist_2_link_1"/>
+    <child link="wrist_3_link_1"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange_1"/>
+  <joint name="wrist_3_flange_1" type="fixed">
+    <parent link="wrist_3_link_1"/>
+    <child link="flange_1"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool1"/>
+  <joint name="flange_tool_1" type="fixed">
+    <!-- default toolframe: X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange_1"/>
+    <child link="tool1"/>
+  </joint>
+
+
+  <link name="base_link_inertia_link_2">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <link name="shoulder_link_2">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link_2">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
+      <inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
+    </inertial>
+  </link>
+  <link name="forearm_link_2">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
+      <inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link_2">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link_2">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link_2">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
+      <inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
+    </inertial>
+  </link>
+  
+  <joint name="base_link_base_link_inertia_joint_2" type="fixed">
+    <parent link="base_link_2"/>
+    <child link="base_link_inertia_link_2"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+
+  <joint name="shoulder_pan_joint_2" type="revolute">
+    <parent link="base_link_inertia_link_2"/>
+    <child link="shoulder_link_2"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1807"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint_2" type="revolute">
+    <parent link="shoulder_link_2"/>
+    <child link="upper_arm_link_2"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint_2" type="revolute">
+    <parent link="upper_arm_link_2"/>
+    <child link="forearm_link_2"/>
+    <origin rpy="0 0 0" xyz="-0.6127 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint_2" type="revolute">
+    <parent link="forearm_link_2"/>
+    <child link="wrist_1_link_2"/>
+    <origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint_2" type="revolute">
+    <parent link="wrist_1_link_2"/>
+    <child link="wrist_2_link_2"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint_2" type="revolute">
+    <parent link="wrist_2_link_2"/>
+    <child link="wrist_3_link_2"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange_2"/>
+  <joint name="wrist_3_flange_joint_2" type="fixed">
+    <parent link="wrist_3_link_2"/>
+    <child link="flange_2"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool2"/>
+  <joint name="flange_tool_joint_2" type="fixed">
+    <!-- default toolframe: X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange_2"/>
+    <child link="tool2"/>
+  </joint>
+
+
+  <link name="base_link_inertia_link_3">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <link name="shoulder_link_3">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link_3">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
+      <inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
+    </inertial>
+  </link>
+  <link name="forearm_link_3">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
+      <inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link_3">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link_3">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link_3">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
+      <inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
+    </inertial>
+  </link>
+  
+</robot>

src/curobo/content/assets/robot/ur_description/ur10e.urdf

@@ -0,0 +1,359 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ur_description/urdf/ur10e.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur10e_robot">
+  <!--
+    Base UR robot series xacro macro.
+
+    NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
+    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
+    (but note: that .xacro must still be processed by the xacro command).
+
+    For use in '.launch' files: use one of the 'load_urX.launch' convenience
+    launch files.
+
+    This file models the base kinematic chain of a UR robot, which then gets
+    parameterised by various configuration files to convert it into a UR3(e),
+    UR5(e), UR10(e) or UR16e.
+
+    NOTE: the default kinematic parameters (ie: link lengths, frame locations,
+    offets, etc) do not correspond to any particular robot. They are defaults
+    only. There WILL be non-zero offsets between the Forward Kinematics results
+    in TF (ie: robot_state_publisher) and the values reported by the Teach
+    Pendant.
+
+    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
+    parameter MUST point to a .yaml file containing the appropriate values for
+    the targetted robot.
+
+    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
+    described in the readme of that repository to extract the kinematic
+    calibration from the controller and generate the required .yaml file.
+
+    Main author of the migration to yaml configs: Ludovic Delval.
+
+    Contributors to previous versions (in no particular order):
+
+     - Felix Messmer
+     - Kelsey Hawkins
+     - Wim Meeussen
+     - Shaun Edwards
+     - Nadia Hammoudeh Garcia
+     - Dave Hershberger
+     - G. vd. Hoorn
+     - Philip Long
+     - Dave Coleman
+     - Miguel Prada
+     - Mathias Luedtke
+     - Marcel Schnirring
+     - Felix von Drigalski
+     - Felix Exner
+     - Jimmy Da Silva
+     - Ajit Krisshna N L
+     - Muhammad Asif Rana
+  -->
+  <!--
+    NOTE: the macro defined in this file is NOT part of the public API of this
+          package. Users CANNOT rely on this file being available, or stored in
+          this location. Nor can they rely on the existence of the macro.
+  -->
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_pan_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_lift_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="elbow_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="elbow_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_1_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_2_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_3_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_3_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!-- links: main serial chain -->
+  <link name="base_link"/>
+  <link name="base_link_inertia">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <link name="shoulder_link">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
+      <inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
+    </inertial>
+  </link>
+  <link name="forearm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
+      <inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
+      <geometry>
+        <mesh filename="meshes/ur10e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
+      <inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
+    </inertial>
+  </link>
+  <!-- joints: main serial chain -->
+  <joint name="base_link-base_link_inertia" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_link_inertia"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link_inertia"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1807"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0 0 0" xyz="-0.6127 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
+  <link name="base"/>
+  <joint name="base_link-base_fixed_joint" type="fixed">
+    <!-- Note the rotation over Z of pi radians: as base_link is REP-103
+           aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed
+           to correctly align 'base' with the 'Base' coordinate system of
+           the UR controller.
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange"/>
+  <joint name="wrist_3-flange" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="flange"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="flange-tool0" type="fixed">
+    <!-- default toolframe: X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange"/>
+    <child link="tool0"/>
+  </joint>
+</robot>

src/curobo/content/assets/robot/ur_description/ur5e.urdf

@@ -0,0 +1,359 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ur_description/urdf/ur5e.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur5e_robot">
+  <!--
+    Base UR robot series xacro macro.
+
+    NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
+    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
+    (but note: that .xacro must still be processed by the xacro command).
+
+    For use in '.launch' files: use one of the 'load_urX.launch' convenience
+    launch files.
+
+    This file models the base kinematic chain of a UR robot, which then gets
+    parameterised by various configuration files to convert it into a UR3(e),
+    UR5(e), UR10(e) or UR16e.
+
+    NOTE: the default kinematic parameters (ie: link lengths, frame locations,
+    offets, etc) do not correspond to any particular robot. They are defaults
+    only. There WILL be non-zero offsets between the Forward Kinematics results
+    in TF (ie: robot_state_publisher) and the values reported by the Teach
+    Pendant.
+
+    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
+    parameter MUST point to a .yaml file containing the appropriate values for
+    the targetted robot.
+
+    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
+    described in the readme of that repository to extract the kinematic
+    calibration from the controller and generate the required .yaml file.
+
+    Main author of the migration to yaml configs: Ludovic Delval.
+
+    Contributors to previous versions (in no particular order):
+
+     - Felix Messmer
+     - Kelsey Hawkins
+     - Wim Meeussen
+     - Shaun Edwards
+     - Nadia Hammoudeh Garcia
+     - Dave Hershberger
+     - G. vd. Hoorn
+     - Philip Long
+     - Dave Coleman
+     - Miguel Prada
+     - Mathias Luedtke
+     - Marcel Schnirring
+     - Felix von Drigalski
+     - Felix Exner
+     - Jimmy Da Silva
+     - Ajit Krisshna N L
+     - Muhammad Asif Rana
+  -->
+  <!--
+    NOTE: the macro defined in this file is NOT part of the public API of this
+          package. Users CANNOT rely on this file being available, or stored in
+          this location. Nor can they rely on the existence of the macro.
+  -->
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_pan_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_lift_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="elbow_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="elbow_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_1_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_2_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_3_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_3_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!-- links: main serial chain -->
+  <link name="base_link"/>
+  <link name="base_link_inertia">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+    </inertial>
+  </link>
+  <link name="shoulder_link">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.7"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="8.393"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.2125 0.0 0.138"/>
+      <inertia ixx="0.1338857818623325" ixy="0.0" ixz="0.0" iyy="0.1338857818623325" iyz="0.0" izz="0.0151074"/>
+    </inertial>
+  </link>
+  <link name="forearm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.275"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="-0.1961 0.0 0.007"/>
+      <inertia ixx="0.031209355099586295" ixy="0.0" ixz="0.0" iyy="0.031209355099586295" iyz="0.0" izz="0.004095"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link">
+    <visual>
+      <!-- TODO: Move this to a parameter -->
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.219"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0025598989760400002" ixy="0.0" ixz="0.0" iyy="0.0025598989760400002" iyz="0.0" izz="0.0021942"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.0997"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.0997"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.219"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0025598989760400002" ixy="0.0" ixz="0.0" iyy="0.0025598989760400002" iyz="0.0" izz="0.0021942"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0989"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0989"/>
+      <geometry>
+        <mesh filename="meshes/ur5e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1879"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0229"/>
+      <inertia ixx="9.890410052167731e-05" ixy="0.0" ixz="0.0" iyy="9.890410052167731e-05" iyz="0.0" izz="0.0001321171875"/>
+    </inertial>
+  </link>
+  <!-- joints: main serial chain -->
+  <joint name="base_link-base_link_inertia" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_link_inertia"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link_inertia"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1625"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0 0 0" xyz="-0.425 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.3922 0 0.1333"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.0997 -2.044881182297852e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0996 -2.042830148012698e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
+  <link name="base"/>
+  <joint name="base_link-base_fixed_joint" type="fixed">
+    <!-- Note the rotation over Z of pi radians: as base_link is REP-103
+           aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed
+           to correctly align 'base' with the 'Base' coordinate system of
+           the UR controller.
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange"/>
+  <joint name="wrist_3-flange" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="flange"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="flange-tool0" type="fixed">
+    <!-- default toolframe: X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange"/>
+    <child link="tool0"/>
+  </joint>
+</robot>

src/curobo/content/assets/scene/nvblox/srl_ur10_bins.nvblx

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:93892dddfc6a65dedc4ef311e773eba7273b29f0a2cdcb76637013fdadf22897
+size 87195648

src/curobo/content/assets/scene/nvblox/srl_ur10_bins_color.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4b82b0a4873d7bf1796893a47f9b41ab7cd32751d8caa102cfea3f401f75dd70
+size 1773918

src/curobo/content/configs/robot/dual_ur10e.yml

@@ -0,0 +1,63 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+
+robot_cfg:
+  kinematics:
+    use_usd_kinematics: False
+    urdf_path: "robot/ur_description/dual_ur10e.urdf"
+    asset_root_path: "robot/ur_description"
+    isaac_usd_path: "/Isaac/Robots/UR10/ur10_long_suction.usd"
+    usd_robot_root: "/ur10"
+    usd_path: "robot/ur_description/dual_ur10e.usd"
+    base_link: "base_fixture_link"
+    ee_link: "tool0"
+    link_names: [ "tool1", "tool0"] # , "tool2"] #, "tool3"]
+    collision_link_names: [
+      'shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link', 'tool0',
+      'shoulder_link_1','upper_arm_link_1', 'forearm_link_1', 'wrist_1_link_1', 'wrist_2_link_1' ,'wrist_3_link_1', 'tool1']
+    collision_spheres: 'spheres/dual_ur10e.yml'
+    collision_sphere_buffer: 0.005
+
+
+    self_collision_ignore: {
+      "upper_arm_link": ["shoulder_link","forearm_link"],
+      "forarm_link": ["wrist_1_link"],
+      "wrist_1_link": ["wrist_2_link","wrist_3_link"],
+      "wrist_2_link": ["wrist_3_link", "tool0"],
+      "wrist_3_link": ["tool0"],
+
+      "upper_arm_link_1": ["shoulder_link_1","forearm_link_1"],
+      "forarm_link_1": ["wrist_1_link_1"],
+      "wrist_1_link_1": ["wrist_2_link_1","wrist_3_link_1"],
+      "wrist_2_link_1": ["wrist_3_link_1", "tool1"],
+      "wrist_3_link_1": ["tool1"],
+    
+      }
+    self_collision_buffer: {
+      'shoulder_link': 0.05,
+      'shoulder_link_1': 0.05,
+      
+       }
+    lock_joints: null
+
+    cspace:
+      joint_names: [
+        'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint',
+        'shoulder_pan_joint_1', 'shoulder_lift_joint_1', 'elbow_joint_1', 'wrist_1_joint_1', 'wrist_2_joint_1', 'wrist_3_joint_1']
+      retract_config: [-1.57, -2.2, 1.9, -1.383, -1.57, 0.00, 
+                      -1.57, -2.2, 1.9, -1.383, -1.57, 0.00]
+      null_space_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                          1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+      cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                              1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+                              
+      max_jerk: 500.0
+      max_acceleration: 15.0

src/curobo/content/configs/robot/franka.yml

@@ -0,0 +1,113 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+
+robot_cfg:
+  kinematics:
+    use_usd_kinematics: False
+    isaac_usd_path: "/Isaac/Robots/Franka/franka.usd"
+    usd_path: "robot/non_shipping/franka/franka_panda_meters1.usda"
+    usd_robot_root: "/panda"
+    usd_flip_joints: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
+      "panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
+    usd_flip_joints: {
+    "panda_joint1": "Z",
+    "panda_joint2": "Z",
+    "panda_joint3": "Z",
+    "panda_joint4": "Z",
+     "panda_joint5": "Z",
+    "panda_joint6": "Z",
+    "panda_joint7": "Z",
+    "panda_finger_joint1": "Y",
+     "panda_finger_joint2":  "Y",
+    }
+    usd_flip_joint_limits: ["panda_finger_joint2"]
+    urdf_path: "robot/franka_description/franka_panda.urdf"
+    asset_root_path: "robot/franka_description"
+    base_link: "base_link"
+    ee_link: "panda_hand"
+    collision_link_names:
+      [
+        "panda_link0",
+        "panda_link1",
+        "panda_link2",
+        "panda_link3",
+        "panda_link4",
+        "panda_link5",
+        "panda_link6",
+        "panda_link7",
+        "panda_hand",
+        "panda_leftfinger",
+        "panda_rightfinger",
+        "attached_object",
+      ]
+    collision_spheres: "spheres/franka_mesh.yml"
+    collision_sphere_buffer: 0.0025 
+    extra_collision_spheres: {"attached_object": 4}
+    use_global_cumul: True
+    self_collision_ignore:
+      {
+        "panda_link0": ["panda_link1", "panda_link2"],
+        "panda_link1": ["panda_link2", "panda_link3", "panda_link4"],
+        "panda_link2": ["panda_link3", "panda_link4"],
+        "panda_link3": ["panda_link4", "panda_link6"],
+        "panda_link4":
+          ["panda_link5", "panda_link6", "panda_link7", "panda_link8"],
+        "panda_link5": ["panda_link6", "panda_link7", "panda_hand","panda_leftfinger", "panda_rightfinger"],
+        "panda_link6": ["panda_link7", "panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
+        "panda_link7": ["panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
+        "panda_hand": ["panda_leftfinger", "panda_rightfinger","attached_object"],
+        "panda_leftfinger": ["panda_rightfinger", "attached_object"],
+        "panda_rightfinger": ["attached_object"],
+        
+      }
+    
+    self_collision_buffer:
+      {
+        "panda_link0": 0.1, 
+        "panda_link1": 0.05, 
+        "panda_link2": 0.0,
+        "panda_link3": 0.0,
+        "panda_link4": 0.0,
+        "panda_link5": 0.0,
+        "panda_link6": 0.0,
+        "panda_link7": 0.0,
+        "panda_hand": 0.02,
+        "panda_leftfinger": 0.01,
+        "panda_rightfinger": 0.01,
+        "attached_object": 0.0,
+      }
+
+    mesh_link_names:
+      [
+        "panda_link0",
+        "panda_link1",
+        "panda_link2",
+        "panda_link3",
+        "panda_link4",
+        "panda_link5",
+        "panda_link6",
+        "panda_link7",
+        "panda_hand",
+        "panda_leftfinger",
+        "panda_rightfinger",
+      ]
+    lock_joints: {"panda_finger_joint1": 0.04, "panda_finger_joint2": -0.04}
+    extra_links: {"attached_object":{"parent_link_name": "panda_hand" , 
+    "link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
+    "joint_name": "attach_joint" }}
+    cspace:
+      joint_names: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
+      "panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
+      retract_config: [0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0., 0.04, -0.04]
+      null_space_weight: [1,1,1,1,1,1,1,1,1]
+      cspace_distance_weight: [1,1,1,1,1,1,1,1,1]
+      max_acceleration: 15.0
+      max_jerk: 500.0

src/curobo/content/configs/robot/franka_mobile.yml

@@ -0,0 +1,116 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+
+robot_cfg:
+  kinematics:
+    use_usd_kinematics: False
+    isaac_usd_path: "/Isaac/Robots/Franka/franka.usd"
+    usd_path: "robot/franka_description/franka_panda_meters.usda"
+    usd_robot_root: "/panda"
+    usd_flip_joints: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
+      "panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
+    usd_flip_joints: {
+    "panda_joint1": "Z",
+    "panda_joint2": "Z",
+    "panda_joint3": "Z",
+    "panda_joint4": "Z",
+     "panda_joint5": "Z",
+    "panda_joint6": "Z",
+    "panda_joint7": "Z",
+    "panda_finger_joint1": "Y",
+     "panda_finger_joint2":  "Y",
+    }
+    usd_flip_joint_limits: ["panda_finger_joint2"]
+    urdf_path: "robot/franka_description/franka_panda_mobile_xy_tz.urdf"
+    asset_root_path: "robot/franka_description"
+    base_link: "base_link"
+    ee_link: "panda_hand"
+    # link_names: null
+    collision_link_names:
+      [
+        "panda_link0",
+        "panda_link1",
+        "panda_link2",
+        "panda_link3",
+        "panda_link4",
+        "panda_link5",
+        "panda_link6",
+        "panda_link7",
+        "panda_hand",
+        "panda_leftfinger",
+        "panda_rightfinger",
+        "attached_object",
+      ]
+    collision_spheres: "spheres/franka_mesh.yml"
+    collision_sphere_buffer: 0.002 #0.02
+    extra_collision_spheres: {"attached_object": 4}
+    use_global_cumul: True
+    self_collision_ignore:
+      {
+        "panda_link0": ["panda_link1", "panda_link2"],
+        "panda_link1": ["panda_link2", "panda_link3", "panda_link4"],
+        "panda_link2": ["panda_link3", "panda_link4"],
+        "panda_link3": ["panda_link4", "panda_link6"],
+        "panda_link4":
+          ["panda_link5", "panda_link6", "panda_link7", "panda_link8"],
+        "panda_link5": ["panda_link6", "panda_link7", "panda_hand","panda_leftfinger", "panda_rightfinger"],
+        "panda_link6": ["panda_link7", "panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
+        "panda_link7": ["panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
+        "panda_hand": ["panda_leftfinger", "panda_rightfinger","attached_object"],
+        "panda_leftfinger": ["panda_rightfinger", "attached_object"],
+        "panda_rightfinger": ["attached_object"],
+        
+      }
+    
+    self_collision_buffer:
+      {
+        "panda_link0": 0.1, 
+        "panda_link1": 0.05, 
+        "panda_link2": 0.0,
+        "panda_link3": 0.0,
+        "panda_link4": 0.0,
+        "panda_link5": 0.0,
+        "panda_link6": 0.0,
+        "panda_link7": 0.0,
+        "panda_hand": 0.02,
+        "panda_leftfinger": 0.01,
+        "panda_rightfinger": 0.01,
+        "attached_object": 0.0,
+      }
+
+    mesh_link_names:
+      [
+        "panda_link0",
+        "panda_link1",
+        "panda_link2",
+        "panda_link3",
+        "panda_link4",
+        "panda_link5",
+        "panda_link6",
+        "panda_link7",
+        "panda_hand",
+        "panda_leftfinger",
+        "panda_rightfinger",
+      ]
+    lock_joints: {"panda_finger_joint1": 0.04, "panda_finger_joint2": -0.04}
+    extra_links: {"attached_object":{"parent_link_name": "panda_hand" , 
+    "link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
+    "joint_name": "attach_joint" }}
+    cspace:
+      joint_names: [
+        "base_x", "base_y", "base_z",
+        "panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
+      "panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
+      retract_config: [0,0,0,0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0., 0.04, -0.04]
+      null_space_weight: [1,1,1,1,1,1,1,1,1,1,1,1]
+      cspace_distance_weight: [1,1,1,1,1,1,1,1,1,1,1,1]
+      max_acceleration: 15.0 #15.0
+      max_jerk: 500.0