hofee/gen_data_curobo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

release repository

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2023-10-26 04:17:19 -07:00
commit 07e6ccfc91
287 changed files with 70659 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
.clangd Normal file
View File

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

19
.gitattributes vendored Normal file
View File

@@ -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

214
.gitignore vendored Normal file
View File

@@ -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

15
CHANGELOG.md Normal file
View File

@@ -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.

16
CODEOWNERS Normal file
View File

@@ -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

77
LICENSE Normal file
View File

@@ -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.

402
LICENSE_ASSETS Normal file
View File

@@ -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.
***********************************

18
MANIFEST.in Normal file
View File

@@ -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

54
README.md Normal file
View File

@@ -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}
}
```

13
benchmark/README.md Normal file
View File

@@ -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.

727
benchmark/curobo_benchmark.py Normal file
View File

@@ -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,
)

558
benchmark/curobo_nvblox_benchmark.py Normal file
View File

@@ -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,
)

193
benchmark/curobo_nvblox_profile.py Normal file
View File

@@ -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()

188
benchmark/curobo_profile.py Normal file
View File

@@ -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)

180
benchmark/curobo_python_profile.py Normal file
View File

@@ -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)

182
benchmark/ik_benchmark.py Normal file
View File

@@ -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

231
benchmark/kinematics_benchmark.py Normal file
View File

@@ -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

36
benchmark/metrics.py Normal file
View File

@@ -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

612
benchmark/robometrics_benchmark.py Normal file
View File

@@ -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)

3
docker/.dockerignore Normal file
View File

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

36
docker/README.md Normal file
View File

@@ -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

181
docker/arm64.dockerfile Normal file
View File

@@ -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/*

30
docker/base.dockerfile Normal file
View File

@@ -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

46
docker/build_dev_docker.sh Normal file
View File

@@ -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} .

11
docker/build_user_docker.sh Normal file
View File

@@ -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 .

69
docker/ros1_x86.dockerfile Normal file
View File

@@ -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

22
docker/start_docker_arm64.sh Normal file
View File

@@ -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

22
docker/start_docker_x86.sh Normal file
View File

@@ -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

24
docker/start_docker_x86_robot.sh Normal file
View File

@@ -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

34
docker/user.dockerfile Normal file
View File

@@ -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'

93
docker/x86.dockerfile Normal file
View File

@@ -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

57
examples/collision_check_example.py Normal file
View File

@@ -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)

234
examples/ik_example.py Normal file
View File

@@ -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()

171
examples/isaac_sim/batch_collision_checker.py Normal file
View File

@@ -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()

311
examples/isaac_sim/batch_motion_gen_reacher.py Normal file
View File

@@ -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()

212
examples/isaac_sim/collision_checker.py Normal file
View File

@@ -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()

134
examples/isaac_sim/helper.py Normal file
View File

@@ -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]))

381
examples/isaac_sim/ik_reachability.py Normal file
View File

@@ -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()

180
examples/isaac_sim/load_all_robots.py Normal file
View File

@@ -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()

336
examples/isaac_sim/motion_gen_reacher.py Normal file
View File

@@ -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()

313
examples/isaac_sim/motion_gen_reacher_nvblox.py Normal file
View File

@@ -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()

402
examples/isaac_sim/mpc_example.py Normal file
View File

@@ -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()

387
examples/isaac_sim/mpc_nvblox_example.py Normal file
View File

@@ -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()

379
examples/isaac_sim/multi_arm_reacher.py Normal file
View File

@@ -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()

260
examples/isaac_sim/realsense_collision.py Normal file
View File

@@ -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()

484
examples/isaac_sim/realsense_mpc.py Normal file
View File

@@ -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()

414
examples/isaac_sim/realsense_reacher.py Normal file
View File

@@ -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()

46
examples/isaac_sim/realsense_viewer.py Normal file
View File

@@ -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()

487
examples/isaac_sim/simple_stacking.py Normal file
View File

@@ -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()

172
examples/isaac_sim/util/convert_urdf_to_usd.py Normal file
View File

@@ -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()

69
examples/isaac_sim/util/dowload_assets.py Normal file
View File

@@ -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()

66
examples/kinematics_example.py Normal file
View File

@@ -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()

101
examples/motion_gen_api_example.py Normal file
View File

@@ -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()

366
examples/motion_gen_example.py Normal file
View File

@@ -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)

81
examples/motion_gen_profile.py Normal file
View File

@@ -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()

131
examples/mpc_example.py Normal file
View File

@@ -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()

120
examples/nvblox_example.py Normal file
View File

@@ -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()

229
examples/torch_layers_example.py Normal file
View File

@@ -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,
)

122
examples/trajopt_example.py Normal file
View File

@@ -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()

171
examples/usd_example.py Normal file
View File

@@ -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()

105
examples/world_representation_example.py Normal file
View File

@@ -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()

3
images/robot_demo.gif Normal file
View File

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

3
images/rrt_compare.gif Normal file
View File

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

66
pyproject.toml Normal file
View File

@@ -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",
]

124
setup.cfg Normal file
View File

@@ -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

90
setup.py Normal file
View File

@@ -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},
)

52
src/curobo/__init__.py Normal file
View File

@@ -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

279
src/curobo/content/assets/robot/franka_description/franka_panda.urdf Normal file
View File

@@ -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>

325
src/curobo/content/assets/robot/franka_description/franka_panda_mobile.urdf Normal file
View File

@@ -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>

52
src/curobo/content/assets/robot/franka_description/franka_panda_mobile_base.urdf Normal file
View File

@@ -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>

303
src/curobo/content/assets/robot/franka_description/franka_panda_mobile_x.urdf Normal file
View File

@@ -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>

316
src/curobo/content/assets/robot/franka_description/franka_panda_mobile_xy.urdf Normal file
View File

@@ -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>

332
src/curobo/content/assets/robot/franka_description/franka_panda_mobile_xy_tz.urdf Normal file
View File

@@ -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>

302
src/curobo/content/assets/robot/franka_description/franka_panda_no_gripper.urdf Normal file
View File

@@ -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>

312
src/curobo/content/assets/robot/franka_description/franka_test.urdf Normal file
View File

@@ -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>

71
src/curobo/content/assets/robot/franka_description/moving_gripper.urdf Normal file
View File

@@ -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>

342
src/curobo/content/assets/robot/franka_description/panda.urdf Normal file
View File

@@ -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>

26
src/curobo/content/assets/robot/franka_description/panda_grasp_gripper.urdf Normal file
View File

@@ -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>

26
src/curobo/content/assets/robot/franka_description/panda_gripper.urdf Normal file
View File

@@ -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>

546
src/curobo/content/assets/robot/iiwa_allegro_description/allegro.urdf Normal file
View File

@@ -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>

414
src/curobo/content/assets/robot/iiwa_allegro_description/iiwa.urdf Normal file
View File

@@ -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>

973
src/curobo/content/assets/robot/iiwa_allegro_description/iiwa_allegro.urdf Normal file
View File

@@ -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>

693
src/curobo/content/assets/robot/jaco/jaco_7n.urdf Normal file
View File

@@ -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>

693
src/curobo/content/assets/robot/jaco/jaco_7s.urdf Normal file
View File

@@ -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>

2
src/curobo/content/assets/robot/kinova/README.md Normal file
View File

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

577
src/curobo/content/assets/robot/kinova/kinova_gen3_7dof.urdf Normal file
View File

@@ -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>

23
src/curobo/content/assets/robot/kinova/kortex_description/grippers/robotiq_2f_85/LICENSE Normal file
View File

@@ -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.

13
src/curobo/content/assets/robot/kinova/kortex_description/grippers/robotiq_2f_85/README.md Normal file
View File

@@ -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)

331
src/curobo/content/assets/robot/techman/tm_description/urdf/tm12-nominal.urdf Normal file
View File

@@ -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>

331
src/curobo/content/assets/robot/techman/tm_description/urdf/tm12x-nominal.urdf Executable file
View File

@@ -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>

556
src/curobo/content/assets/robot/ur_description/dual_ur10e.urdf Normal file
View File

@@ -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>

1035
src/curobo/content/assets/robot/ur_description/quad_ur10e.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

953
src/curobo/content/assets/robot/ur_description/tri_ur10e.urdf Normal file
View File

@@ -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>

359
src/curobo/content/assets/robot/ur_description/ur10e.urdf Normal file
View File

@@ -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>

359
src/curobo/content/assets/robot/ur_description/ur5e.urdf Normal file
View File

@@ -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>

3
src/curobo/content/assets/scene/nvblox/srl_ur10_bins.nvblx Normal file
View File

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

3
src/curobo/content/assets/scene/nvblox/srl_ur10_bins_color.png Normal file
View File

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

63
src/curobo/content/configs/robot/dual_ur10e.yml Normal file
View File

@@ -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

113
src/curobo/content/configs/robot/franka.yml Normal file
View File

@@ -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

116
src/curobo/content/configs/robot/franka_mobile.yml Normal file
View File

@@ -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

Some files were not shown because too many files have changed in this diff Show More