diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..df3861e --- /dev/null +++ b/.gitattributes @@ -0,0 +1,30 @@ +# Ignore Python files in linguist +*.py linguist-detectable=false + +# Images +*.gif filter=lfs diff=lfs merge=lfs -text +*.jpg filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.psd filter=lfs diff=lfs merge=lfs -text + +# Archives +*.gz filter=lfs diff=lfs merge=lfs -text +*.tar filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text + +# Documents +*.pdf filter=lfs diff=lfs merge=lfs -text + +# Shared libraries +*.so filter=lfs diff=lfs merge=lfs -text +*.so.* filter=lfs diff=lfs merge=lfs -text + +# ROS Bags +**/resources/**/*.zstd filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.bag filter=lfs diff=lfs merge=lfs -text + +# FIXME: Only for DNN packages +# DNN Model files +*.onnx filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..cb3a863 --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +# Ignore all pycache files +**/__pycache__/** + +# FIXME: Only for DNN-based packages +# Ignore TensorRT plan files +*.plan +*.engine diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..66edd3e --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "curobo_core/curobo"] + path = curobo_core/curobo + url = https://github.com/NVlabs/curobo + branch = isaac-3.0 diff --git a/CHANGELOG b/CHANGELOG new file mode 100644 index 0000000..bfc2f54 --- /dev/null +++ b/CHANGELOG @@ -0,0 +1,39 @@ +.. + FIXME: Remove this before publishing! + + HOW TO USE THIS TEMPLATE: + + - Set up your editor alongside a live ReStructuredText rendering tool + - Suggestion: use the VSCode integrations provided as part of the `isaac_ros-dev` dev environment + - Create a copy of this file at the ROOT level of a new Isaac ROS package + - Look for `FIXME` tags throughout the file and resolve accordingly + - Use Semantic Versioning (https://semver.org/) to update version numbers as (Major).(Minor).(Patch) + - Carefully read through the rendered output to ensure all text is displayed appropriately + - Remove this comment before publishing README! + +isaac_ros_[FIXME: package name] +================================= + +(#major).(#minor).(#patch) (YYY2-M2-D2) +--------------------------------------- +* FIXME: One-liner description for change 1 + + FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point + +* (...) + +* FIXME: One-liner description for change N + + FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point + +(#major).(#minor).(#patch) (YYY1-M1-D1) +--------------------------------------- +* FIXME: One-liner description for change 1 + + FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point + +* (...) + +* FIXME: One-liner description for change N + + FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..e393b37 --- /dev/null +++ b/LICENSE @@ -0,0 +1,66 @@ +NVIDIA ISAAC ROS SOFTWARE LICENSE + +This license is a legal agreement between you and NVIDIA Corporation ("NVIDIA") and governs the use of the NVIDIA Isaac ROS software and materials provided hereunder (“SOFTWARE”). + +This license can be accepted only by an adult of legal age of majority in the country in which the SOFTWARE is used. + +If you are entering into this license on behalf of a company or other legal entity, you represent that you have the legal authority to bind the entity to this license, in which case “you” will mean the entity you represent. + +If you don’t have the required age or authority to accept this license, or if you don’t accept all the terms and conditions of this license, do not download, install or use the SOFTWARE. + +You agree to use the SOFTWARE only for purposes that are permitted by (a) this license, and (b) any applicable law, regulation or generally accepted practices or guidelines in the relevant jurisdictions. + +1. LICENSE. Subject to the terms of this license, NVIDIA hereby grants you a non-exclusive, non-transferable license, without the right to sublicense (except as expressly provided in this license) to: +a. Install and use the SOFTWARE, +b. Modify and create derivative works of sample or reference source code delivered in the SOFTWARE, and +c. Distribute any part of the SOFTWARE (i) as incorporated into a software application that has material additional functionality beyond the included portions of the SOFTWARE, or (ii) unmodified in binary format, in each case subject to the distribution requirements indicated in this license. + +2. DISTRIBUTION REQUIREMENTS. These are the distribution requirements for you to exercise the distribution grant above: + a. The following notice shall be included in modifications and derivative works of source code distributed: “This software contains source code provided by NVIDIA Corporation.” + b. You agree to distribute the SOFTWARE subject to the terms at least as protective as the terms of this license, including (without limitation) terms relating to the license grant, license restrictions and protection of NVIDIA’s intellectual property rights. Additionally, you agree that you will protect the privacy, security and legal rights of your application users. + c. You agree to notify NVIDIA in writing of any known or suspected distribution or use of the SOFTWARE not in compliance with the requirements of this license, and to enforce the terms of your agreements with respect to the distributed portions of the SOFTWARE. +3. AUTHORIZED USERS. You may allow employees and contractors of your entity or of your subsidiary(ies) to access and use the SOFTWARE from your secure network to perform work on your behalf. If you are an academic institution you may allow users enrolled or employed by the academic institution to access and use the SOFTWARE from your secure network. You are responsible for the compliance with the terms of this license by your authorized users. + +4. LIMITATIONS. Your license to use the SOFTWARE is restricted as follows: + a. The SOFTWARE is licensed for you to develop applications only for their use in systems with NVIDIA GPUs. + b. You may not reverse engineer, decompile or disassemble, or remove copyright or other proprietary notices from any portion of the SOFTWARE or copies of the SOFTWARE. + c. Except as expressly stated above in this license, you may not sell, rent, sublicense, transfer, distribute, modify, or create derivative works of any portion of the SOFTWARE. + d. Unless you have an agreement with NVIDIA for this purpose, you may not indicate that an application created with the SOFTWARE is sponsored or endorsed by NVIDIA. + e. You may not bypass, disable, or circumvent any technical limitation, encryption, security, digital rights management or authentication mechanism in the SOFTWARE. + f. You may not use the SOFTWARE in any manner that would cause it to become subject to an open source software license. As examples, licenses that require as a condition of use, modification, and/or distribution that the SOFTWARE be: (i) disclosed or distributed in source code form; (ii) licensed for the purpose of making derivative works; or (iii) redistributable at no charge. + g. You acknowledge that the SOFTWARE as delivered is not tested or certified by NVIDIA for use in connection with the design, construction, maintenance, and/or operation of any system where the use or failure of such system could result in a situation that threatens the safety of human life or results in catastrophic damages (each, a "Critical Application"). Examples of Critical Applications include use in avionics, navigation, autonomous vehicle applications, ai solutions for automotive products, military, medical, life support or other life critical applications. NVIDIA shall not be liable to you or any third party, in whole or in part, for any claims or damages arising from such uses. You are solely responsible for ensuring that any product or service developed with the SOFTWARE as a whole includes sufficient features to comply with all applicable legal and regulatory standards and requirements. + h. You agree to defend, indemnify and hold harmless NVIDIA and its affiliates, and their respective employees, contractors, agents, officers and directors, from and against any and all claims, damages, obligations, losses, liabilities, costs or debt, fines, restitutions and expenses (including but not limited to attorney’s fees and costs incident to establishing the right of indemnification) arising out of or related to your use of goods and/or services that include or utilize the SOFTWARE, or for use of the SOFTWARE outside of the scope of this license or not in compliance with its terms. + +5. UPDATES. NVIDIA may, at its option, make available patches, workarounds or other updates to this SOFTWARE. Unless the updates are provided with their separate governing terms, they are deemed part of the SOFTWARE licensed to you as provided in this license. + +6. PRE-RELEASE VERSIONS. SOFTWARE versions identified as alpha, beta, preview, early access or otherwise as pre-release may not be fully functional, may contain errors or design flaws, and may have reduced or different security, privacy, availability, and reliability standards relative to commercial versions of NVIDIA software and materials. You may use a pre-release SOFTWARE version at your own risk, understanding that these versions are not intended for use in production or business-critical systems. + +7. COMPONENTS UNDER OTHER LICENSES. The SOFTWARE may include NVIDIA or third-party components with separate legal notices or terms as may be described in proprietary notices accompanying the SOFTWARE, such as components governed by open source software licenses. If and to the extent there is a conflict between the terms in this license and the license terms associated with a component, the license terms associated with the component controls only to the extent necessary to resolve the conflict. + +8. OWNERSHIP. + +8.1 NVIDIA reserves all rights, title and interest in and to the SOFTWARE not expressly granted to you under this license. NVIDIA and its suppliers hold all rights, title and interest in and to the SOFTWARE, including their respective intellectual property rights. The SOFTWARE is copyrighted and protected by the laws of the United States and other countries, and international treaty provisions. + +8.2 Subject to the rights of NVIDIA and its suppliers in the SOFTWARE, you hold all rights, title and interest in and to your applications and your derivative works of the sample or reference source code delivered in the SOFTWARE including their respective intellectual property rights. With respect to source code samples or reference source code licensed to you, NVIDIA and its affiliates are free to continue independently developing source code samples and you covenant not to sue NVIDIA, its affiliates or their licensees with respect to later versions of NVIDIA released source code. + +9. FEEDBACK. You may, but are not obligated to, provide to NVIDIA Feedback. “Feedback” means suggestions, fixes, modifications, feature requests or other feedback regarding the SOFTWARE. Feedback, even if designated as confidential by you, shall not create any confidentiality obligation for NVIDIA. NVIDIA and its designees have a perpetual, non-exclusive, worldwide, irrevocable license to use, reproduce, publicly display, modify, create derivative works of, license, sublicense, and otherwise distribute and exploit Feedback as NVIDIA sees fit without payment and without obligation or restriction of any kind on account of intellectual property rights or otherwise. + +10. NO WARRANTIES. THE SOFTWARE IS PROVIDED AS-IS. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES EXPRESSLY DISCLAIM ALL WARRANTIES OF ANY KIND OR NATURE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR A PARTICULAR PURPOSE. NVIDIA DOES NOT WARRANT THAT THE SOFTWARE WILL MEET YOUR REQUIREMENTS OR THAT THE OPERATION THEREOF WILL BE UNINTERRUPTED OR ERROR-FREE, OR THAT ALL ERRORS WILL BE CORRECTED. + +11. LIMITATIONS OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES SHALL NOT BE LIABLE FOR ANY SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL DAMAGES, OR FOR ANY LOST PROFITS, PROJECT DELAYS, LOSS OF USE, LOSS OF DATA OR LOSS OF GOODWILL, OR THE COSTS OF PROCURING SUBSTITUTE PRODUCTS, ARISING OUT OF OR IN CONNECTION WITH THIS LICENSE OR THE USE OR PERFORMANCE OF THE SOFTWARE, WHETHER SUCH LIABILITY ARISES FROM ANY CLAIM BASED UPON BREACH OF CONTRACT, BREACH OF WARRANTY, TORT (INCLUDING NEGLIGENCE), PRODUCT LIABILITY OR ANY OTHER CAUSE OF ACTION OR THEORY OF LIABILITY, EVEN IF NVIDIA HAS PREVIOUSLY BEEN ADVISED OF, OR COULD REASONABLY HAVE FORESEEN, THE POSSIBILITY OF SUCH DAMAGES. IN NO EVENT WILL NVIDIA’S AND ITS AFFILIATES TOTAL CUMULATIVE LIABILITY UNDER OR ARISING OUT OF THIS LICENSE EXCEED US$10.00. THE NATURE OF THE LIABILITY OR THE NUMBER OF CLAIMS OR SUITS SHALL NOT ENLARGE OR EXTEND THIS LIMIT. + +12. TERMINATION. Your rights under this license will terminate automatically without notice from NVIDIA if you fail to comply with any term and condition of this license or if you commence or participate in any legal proceeding against NVIDIA with respect to the SOFTWARE. NVIDIA may terminate this license with advance written notice to you, if NVIDIA decides to no longer provide the SOFTWARE in a country or, in NVIDIA’s sole discretion, the continued use of it is no longer commercially viable. Upon any termination of this license, you agree to promptly discontinue use of the SOFTWARE and destroy all copies in your possession or control. Your prior distributions in accordance with this license are not affected by the termination of this license. All provisions of this license will survive termination, except for the license granted to you. + +13. APPLICABLE LAW. This license will be governed in all respects by the laws of the United States and of the State of Delaware, without regard to the conflicts of laws principles. The United Nations Convention on Contracts for the International Sale of Goods is specifically disclaimed. You agree to all terms of this license in the English language. The state or federal courts residing in Santa Clara County, California shall have exclusive jurisdiction over any dispute or claim arising out of this license. Notwithstanding this, you agree that NVIDIA shall still be allowed to apply for injunctive remedies or urgent legal relief in any jurisdiction. + +14. NO ASSIGNMENT. This license and your rights and obligations thereunder may not be assigned by you by any means or operation of law without NVIDIA’s permission. Any attempted assignment not approved by NVIDIA in writing shall be void and of no effect. NVIDIA may assign, delegate or transfer this license and its rights and obligations, and if to a non-affiliate you will be notified. + +15. EXPORT. The SOFTWARE is subject to United States export laws and regulations. You agree to comply with all applicable U.S. and international export laws, including the Export Administration Regulations (EAR) administered by the U.S. Department of Commerce and economic sanctions administered by the U.S. Department of Treasury’s Office of Foreign Assets Control (OFAC). These laws include restrictions on destinations, end-users and end-use. By accepting this license, you confirm that you are not currently residing in a country or region currently embargoed by the U.S. and that you are not otherwise prohibited from receiving the SOFTWARE. + +16. GOVERNMENT USE. The SOFTWARE is, and shall be treated as being, “Commercial Items” as that term is defined at 48 CFR § 2.101, consisting of “commercial computer software” and “commercial computer software documentation”, respectively, as such terms are used in, respectively, 48 CFR § 12.212 and 48 CFR §§ 227.7202 & 252.227-7014(a)(1). Use, duplication or disclosure by the U.S. Government or a U.S. Government subcontractor is subject to the restrictions in this license pursuant to 48 CFR § 12.212 or 48 CFR § 227.7202. In no event shall the US Government user acquire rights in the SOFTWARE beyond those specified in 48 C.F.R. 52.227-19(b)(1)-(2). + +17. NOTICES. Please direct your legal notices or other correspondence to NVIDIA Corporation, 2788 San Tomas Expressway, Santa Clara, California 95051, United States of America, Attention: Legal Department. + +18. ENTIRE AGREEMENT. This license is the final, complete and exclusive agreement between the parties relating to the subject matter of this license and supersedes all prior or contemporaneous understandings and agreements relating to this subject matter, whether oral or written. If any court of competent jurisdiction determines that any provision of this license is illegal, invalid or unenforceable, the remaining provisions will remain in full force and effect. Any amendment or waiver under this license shall be in writing and signed by representatives of both parties. + +(v. November 17, 2021) diff --git a/README.md b/README.md index 8b5bb96..3e63fa8 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,67 @@ -# isaac_ros_cumotion -NVIDIA-accelerated packages for arm motion planning and control +# Isaac ROS cuMotion + +NVIDIA accelerated packages for arm motion planning and control + +
image
+ +## Overview + +[Isaac ROS cuMotion](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_cumotion) provides CUDA-accelerated manipulation +capabilities for robots in ROS 2, enabling faster processing speeds and real-time performance +that are crucial to complex robotics tasks such as motion planning. +It provides two main capabilities, motion generation for robot +arms via an integration of cuMotion into MoveIt 2 and segmentation of robot from depth streams +using cuMotion’s kinematics and geometry processing functions to accurately identify and filter robot parts. +This allows one to reconstruct obstacles in the environment without spurious contributions from the robot itself. + +The key advantages of using Isaac ROS cuMotion are: + +* Increased Efficiency: CUDA acceleration significantly speeds up processing times, + allowing for complex computation, such as collision avoidance, occurring at real-time. +* Enhanced Precision: Accurate motion planning and segmentation allow for better + performance in tasks requiring fine manipulation and detailed environmental interaction. +* Improved Flexibility: Modular design allows easy integration with existing ROS 2 setups, + such as configurations using MoveIt 2, enabling customization and scalability using familiar + tooling. + +The Isaac ROS cuMotion repository currently contains the following packages: + +`isaac_ros_cumotion`: +: This package contains the cuMotion planner node and the robot segmentation node. + +`isaac_ros_cumotion_examples`: +: This package contains various examples demonstrating use of cuMotion with MoveIt. + +`isaac_ros_cumotion_moveit`: +: This package provides a plugin for MoveIt 2 that exposes cuMotion as an external planner, leveraging `isaac_ros_cumotion`. + +Isaac ROS cuMotion is also featured as part of [Isaac Manipulator](https://nvidia-isaac-ros.github.io/reference_workflows/isaac_manipulator/index.html). + +--- + +## Documentation + +Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/index.html) to learn how to use this repository. + +--- + +## Packages + +* [`isaac_ros_cumotion`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion/index.html) + * [Motion Generation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion/index.html#motion-generation) + * [Robot Segmentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion/index.html#robot-segmentation) +* [`isaac_ros_cumotion_moveit`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index.html) + * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index.html#quickstart) + * [Try More Examples](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index.html#try-more-examples) + * [Troubleshooting](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index.html#troubleshooting) +* [`isaac_ros_esdf_visualizer`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_esdf_visualizer/index.html) + * [Overview](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_esdf_visualizer/index.html#overview) + * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_esdf_visualizer/index.html#api) +* [`isaac_ros_moveit_goal_setter`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_moveit_goal_setter/index.html) + * [Overview](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_moveit_goal_setter/index.html#overview) + * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_moveit_goal_setter/index.html#api) +* [`isaac_ros_moveit_goal_setter_interfaces`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_moveit_goal_setter_interfaces/index.html) + +## Latest + +Update 2024-05-30: Initial release diff --git a/curobo_core/curobo b/curobo_core/curobo new file mode 160000 index 0000000..3bfed9d --- /dev/null +++ b/curobo_core/curobo @@ -0,0 +1 @@ +Subproject commit 3bfed9d7737fc3865595e4a50b1fe84a32cf627f diff --git a/curobo_core/package.xml b/curobo_core/package.xml new file mode 100644 index 0000000..41582bc --- /dev/null +++ b/curobo_core/package.xml @@ -0,0 +1,18 @@ + + + + curobo_core + 3.0.0 + This package wraps the cuRobo library as a ROS 2 package. cuRobo serves as the current backend for cuMotion. + Isaac ROS Maintainers + NVIDIA Isaac ROS Software License + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/curobo_core/resource/curobo_core b/curobo_core/resource/curobo_core new file mode 100644 index 0000000..e69de29 diff --git a/curobo_core/setup.cfg b/curobo_core/setup.cfg new file mode 100644 index 0000000..5943538 --- /dev/null +++ b/curobo_core/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/curobo_core +[install] +install_scripts=$base/lib/curobo_core diff --git a/curobo_core/setup.py b/curobo_core/setup.py new file mode 100644 index 0000000..956c8a4 --- /dev/null +++ b/curobo_core/setup.py @@ -0,0 +1,107 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +from setuptools import find_namespace_packages, setup +from torch.utils.cpp_extension import BuildExtension, CUDAExtension + +package_name = 'curobo_core' + +extra_cuda_args = { + 'nvcc': [ + '--threads=8', + '-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', + [ + 'curobo/src/curobo/curobolib/cpp/lbfgs_step_cuda.cpp', + 'curobo/src/curobo/curobolib/cpp/lbfgs_step_kernel.cu', + ], + extra_compile_args=extra_cuda_args, + ), + CUDAExtension( + 'curobo.curobolib.kinematics_fused_cu', + [ + 'curobo/src/curobo/curobolib/cpp/kinematics_fused_cuda.cpp', + 'curobo/src/curobo/curobolib/cpp/kinematics_fused_kernel.cu', + ], + extra_compile_args=extra_cuda_args, + ), + CUDAExtension( + 'curobo.curobolib.geom_cu', + [ + 'curobo/src/curobo/curobolib/cpp/geom_cuda.cpp', + 'curobo/src/curobo/curobolib/cpp/sphere_obb_kernel.cu', + 'curobo/src/curobo/curobolib/cpp/pose_distance_kernel.cu', + 'curobo/src/curobo/curobolib/cpp/self_collision_kernel.cu', + ], + extra_compile_args=extra_cuda_args, + ), + CUDAExtension( + 'curobo.curobolib.line_search_cu', + [ + 'curobo/src/curobo/curobolib/cpp/line_search_cuda.cpp', + 'curobo/src/curobo/curobolib/cpp/line_search_kernel.cu', + 'curobo/src/curobo/curobolib/cpp/update_best_kernel.cu', + ], + extra_compile_args=extra_cuda_args, + ), + CUDAExtension( + 'curobo.curobolib.tensor_step_cu', + [ + 'curobo/src/curobo/curobolib/cpp/tensor_step_cuda.cpp', + 'curobo/src/curobo/curobolib/cpp/tensor_step_kernel.cu', + ], + extra_compile_args=extra_cuda_args, + ), +] + +setup( + name=package_name, + version='3.0.0', + packages=find_namespace_packages(where='curobo/src'), + package_dir={'': 'curobo/src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='This package wraps the cuRobo library as a ROS 2 package. cuRobo serves as the current backend for cuMotion.', + license='NVIDIA Isaac ROS Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, + ext_modules=ext_modules, + cmdclass={'build_ext': BuildExtension}, + package_data={ + 'curobo': ['**/*.*'], + }, + include_package_data=True, +) diff --git a/curobo_core/test/test_copyright.py b/curobo_core/test/test_copyright.py new file mode 100644 index 0000000..e445f78 --- /dev/null +++ b/curobo_core/test/test_copyright.py @@ -0,0 +1,24 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.skip(reason='This package has a proprietary license') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/__init__.py b/isaac_ros_cumotion/isaac_ros_cumotion/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py new file mode 100644 index 0000000..c2dbb2d --- /dev/null +++ b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py @@ -0,0 +1,723 @@ +# Copyright (c) 2024 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 os import path +import time + +from ament_index_python.packages import get_package_share_directory +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import Cuboid +from curobo.geom.types import Cylinder +from curobo.geom.types import Mesh +from curobo.geom.types import Sphere +from curobo.geom.types import VoxelGrid as CuVoxelGrid +from curobo.geom.types import WorldConfig +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.state import JointState as CuJointState +from curobo.util.logger import setup_curobo_logger +from curobo.util_file import get_robot_configs_path +from curobo.util_file import join_path +from curobo.util_file import load_yaml +from curobo.wrap.reacher.motion_gen import MotionGen +from curobo.wrap.reacher.motion_gen import MotionGenConfig +from curobo.wrap.reacher.motion_gen import MotionGenPlanConfig +from curobo.wrap.reacher.motion_gen import MotionGenStatus +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from isaac_ros_cumotion.xrdf_utils import convert_xrdf_to_curobo +from moveit_msgs.action import MoveGroup +from moveit_msgs.msg import CollisionObject +from moveit_msgs.msg import MoveItErrorCodes +from moveit_msgs.msg import RobotTrajectory +import numpy as np +from nvblox_msgs.srv import EsdfAndGradients +import rclpy +from rclpy.action import ActionServer +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import JointState +from shape_msgs.msg import SolidPrimitive +from std_msgs.msg import ColorRGBA +import torch +from trajectory_msgs.msg import JointTrajectory +from trajectory_msgs.msg import JointTrajectoryPoint +from visualization_msgs.msg import Marker + + +class CumotionActionServer(Node): + + def __init__(self): + super().__init__('cumotion_action_server') + self.tensor_args = TensorDeviceType() + self.declare_parameter('robot', 'ur5e.yml') + self.declare_parameter('time_dilation_factor', 0.5) + self.declare_parameter('collision_cache_mesh', 20) + self.declare_parameter('collision_cache_cuboid', 20) + self.declare_parameter('interpolation_dt', 0.02) + self.declare_parameter('voxel_dims', [2.0, 2.0, 2.0]) + self.declare_parameter('voxel_size', 0.05) + self.declare_parameter('read_esdf_world', False) + self.declare_parameter('publish_curobo_world_as_voxels', False) + self.declare_parameter('add_ground_plane', False) + self.declare_parameter('publish_voxel_size', 0.05) + self.declare_parameter('max_publish_voxels', 50000) + self.declare_parameter('joint_states_topic', '/joint_states') + self.declare_parameter('tool_frame', rclpy.Parameter.Type.STRING) + + self.declare_parameter('grid_position', [0.0, 0.0, 0.0]) + + self.declare_parameter('esdf_service_name', '/nvblox_node/get_esdf_and_gradient') + self.declare_parameter('urdf_path', rclpy.Parameter.Type.STRING) + self.declare_parameter('enable_curobo_debug_mode', False) + self.declare_parameter('override_moveit_scaling_factors', False) + debug_mode = ( + self.get_parameter('enable_curobo_debug_mode').get_parameter_value().bool_value + ) + if debug_mode: + setup_curobo_logger('info') + else: + setup_curobo_logger('warning') + + self.__voxel_pub = self.create_publisher(Marker, '/curobo/voxels', 10) + self._action_server = ActionServer( + self, MoveGroup, 'cumotion/move_group', self.execute_callback + ) + + try: + self.__urdf_path = self.get_parameter('urdf_path') + self.__urdf_path = self.__urdf_path.get_parameter_value().string_value + if self.__urdf_path == '': + self.__urdf_path = None + except rclpy.exceptions.ParameterUninitializedException: + self.__urdf_path = None + + try: + self.__tool_frame = self.get_parameter('tool_frame') + self.__tool_frame = self.__tool_frame.get_parameter_value().string_value + if self.__tool_frame == '': + self.__tool_frame = None + except rclpy.exceptions.ParameterUninitializedException: + self.__tool_frame = None + + self.__xrdf_path = path.join( + get_package_share_directory('isaac_ros_cumotion_robot_description'), 'xrdf' + ) + self.__joint_states_topic = ( + self.get_parameter('joint_states_topic').get_parameter_value().string_value + ) + self.__add_ground_plane = ( + self.get_parameter('add_ground_plane').get_parameter_value().bool_value + ) + self.__override_moveit_scaling_factors = ( + self.get_parameter('override_moveit_scaling_factors').get_parameter_value().bool_value + ) + # ESDF service + + self.__read_esdf_grid = ( + self.get_parameter('read_esdf_world').get_parameter_value().bool_value + ) + self.__publish_curobo_world_as_voxels = ( + self.get_parameter('publish_curobo_world_as_voxels').get_parameter_value().bool_value + ) + self.__grid_position = ( + self.get_parameter('grid_position').get_parameter_value().double_array_value + ) + self.__max_publish_voxels = ( + self.get_parameter('max_publish_voxels').get_parameter_value().integer_value + ) + self.__voxel_dims = ( + self.get_parameter('voxel_dims').get_parameter_value().double_array_value + ) + self.__publish_voxel_size = ( + self.get_parameter('publish_voxel_size').get_parameter_value().double_value + ) + self.__voxel_size = self.get_parameter('voxel_size').get_parameter_value().double_value + self.__esdf_client = None + self.__esdf_req = None + if self.__read_esdf_grid: + esdf_service_name = ( + self.get_parameter('esdf_service_name').get_parameter_value().string_value + ) + + esdf_service_cb_group = MutuallyExclusiveCallbackGroup() + self.__esdf_client = self.create_client( + EsdfAndGradients, esdf_service_name, callback_group=esdf_service_cb_group + ) + while not self.__esdf_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info( + f'Service({esdf_service_name}) not available, waiting again...' + ) + self.__esdf_req = EsdfAndGradients.Request() + self.warmup() + self.__query_count = 0 + self.__tensor_args = self.motion_gen.tensor_args + self.subscription = self.create_subscription( + JointState, self.__joint_states_topic, self.js_callback, 10 + ) + self.__js_buffer = None + + def js_callback(self, msg): + self.__js_buffer = { + 'joint_names': msg.name, + 'position': msg.position, + 'velocity': msg.velocity, + } + + def warmup(self): + robot_file = self.get_parameter('robot').get_parameter_value().string_value + if robot_file == '': + self.get_logger().fatal('Received empty robot file') + raise SystemExit + + collision_cache_cuboid = ( + self.get_parameter('collision_cache_cuboid').get_parameter_value().integer_value + ) + collision_cache_mesh = ( + self.get_parameter('collision_cache_mesh').get_parameter_value().integer_value + ) + interpolation_dt = ( + self.get_parameter('interpolation_dt').get_parameter_value().double_value + ) + + self.get_logger().info('Loaded robot file name: ' + robot_file) + self.get_logger().info('warming up cuMotion, wait until ready') + + tensor_args = self.tensor_args + world_file = WorldConfig.from_dict( + { + 'cuboid': { + 'table': { + 'pose': [0, 0, -0.05, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz + 'dims': [2.0, 2.0, 0.1], + } + }, + 'voxel': { + 'world_voxel': { + 'dims': self.__voxel_dims, + 'pose': [0, 0, 0, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz + 'voxel_size': self.__voxel_size, + 'feature_dtype': torch.bfloat16, + }, + }, + } + ) + + if robot_file.lower().endswith('.xrdf'): + if self.__urdf_path is None: + self.get_logger().fatal('urdf_path is required to load robot from .xrdf') + raise SystemExit + robot_dict = load_yaml(join_path(self.__xrdf_path, robot_file)) + robot_dict = convert_xrdf_to_curobo(self.__urdf_path, robot_dict, self.get_logger()) + else: + robot_dict = load_yaml(join_path(get_robot_configs_path(), robot_file)) + + if self.__urdf_path is not None: + robot_dict['robot_cfg']['kinematics']['urdf_path'] = self.__urdf_path + + robot_dict = robot_dict['robot_cfg'] + motion_gen_config = MotionGenConfig.load_from_robot_config( + robot_dict, + world_file, + tensor_args, + interpolation_dt=interpolation_dt, + collision_cache={ + 'obb': collision_cache_cuboid, + 'mesh': collision_cache_mesh, + }, + collision_checker_type=CollisionCheckerType.VOXEL, + ee_link_name=self.__tool_frame, + ) + + motion_gen = MotionGen(motion_gen_config) + self.__robot_base_frame = motion_gen.kinematics.base_link + + motion_gen.warmup(enable_graph=True) + + self.__world_collision = motion_gen.world_coll_checker + if not self.__add_ground_plane: + motion_gen.clear_world_cache() + self.motion_gen = motion_gen + self.get_logger().info('cuMotion is ready for planning queries!') + + def update_voxel_grid(self): + self.get_logger().info('Calling ESDF service') + # This is half of x,y and z dims + aabb_min = Point() + aabb_min.x = -1 * self.__voxel_dims[0] / 2 + aabb_min.y = -1 * self.__voxel_dims[1] / 2 + aabb_min.z = -1 * self.__voxel_dims[2] / 2 + # This is a voxel size. + voxel_dims = Vector3() + voxel_dims.x = self.__voxel_dims[0] + voxel_dims.y = self.__voxel_dims[1] + voxel_dims.z = self.__voxel_dims[2] + esdf_future = self.send_request(aabb_min, voxel_dims) + while not esdf_future.done(): + time.sleep(0.001) + response = esdf_future.result() + esdf_grid = self.get_esdf_voxel_grid(response) + if torch.max(esdf_grid.feature_tensor) <= (-1000.0 + 0.5 * self.__voxel_size + 1e-5): + self.get_logger().error('ESDF data is empty, try again after few seconds.') + return False + self.__world_collision.update_voxel_data(esdf_grid) + self.get_logger().info('Updated ESDF grid') + return True + + def send_request(self, aabb_min_m, aabb_size_m): + self.__esdf_req.aabb_min_m = aabb_min_m + self.__esdf_req.aabb_size_m = aabb_size_m + self.get_logger().info( + f'ESDF req = {self.__esdf_req.aabb_min_m}, {self.__esdf_req.aabb_size_m}' + ) + esdf_future = self.__esdf_client.call_async(self.__esdf_req) + + return esdf_future + + def get_esdf_voxel_grid(self, esdf_data): + esdf_array = esdf_data.esdf_and_gradients + array_shape = [ + esdf_array.layout.dim[0].size, + esdf_array.layout.dim[1].size, + esdf_array.layout.dim[2].size, + ] + array_data = np.array(esdf_array.data) + + array_data = self.__tensor_args.to_device(array_data) + + # Array data is reshaped to x y z channels + array_data = array_data.view(array_shape[0], array_shape[1], array_shape[2]).contiguous() + + # Array is squeezed to 1 dimension + array_data = array_data.reshape(-1, 1) + + # nvblox uses negative distance inside obstacles, cuRobo needs the opposite: + array_data = -1 * array_data + + # nvblox assigns a value of -1000.0 for unobserved voxels, making + array_data[array_data >= 1000.0] = -1000.0 + + # nvblox distance are at origin of each voxel, cuRobo's esdf needs it to be at faces + array_data = array_data + 0.5 * self.__voxel_size + + esdf_grid = CuVoxelGrid( + name='world_voxel', + dims=self.__voxel_dims, + pose=[ + self.__grid_position[0], + self.__grid_position[1], + self.__grid_position[2], + 1, + 0.0, + 0.0, + 0, + ], # x, y, z, qw, qx, qy, qz + voxel_size=self.__voxel_size, + feature_dtype=torch.float32, + feature_tensor=array_data, + ) + + return esdf_grid + + def get_cumotion_collision_object(self, mv_object: CollisionObject): + objs = [] + pose = mv_object.pose + + world_pose = [ + pose.position.x, + pose.position.y, + pose.position.z, + pose.orientation.w, + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + ] + world_pose = Pose.from_list(world_pose) + supported_objects = True + if len(mv_object.primitives) > 0: + for k in range(len(mv_object.primitives)): + pose = mv_object.primitive_poses[k] + primitive_pose = [ + pose.position.x, + pose.position.y, + pose.position.z, + pose.orientation.w, + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + ] + object_pose = world_pose.multiply(Pose.from_list(primitive_pose)).tolist() + + if mv_object.primitives[k].type == SolidPrimitive.BOX: + # cuboid: + dims = mv_object.primitives[k].dimensions + obj = Cuboid( + name=str(mv_object.id) + '_' + str(k) + '_cuboid', + pose=object_pose, + dims=dims, + ) + objs.append(obj) + elif mv_object.primitives[k].type == SolidPrimitive.SPHERE: + # sphere: + radius = mv_object.primitives[k].dimensions[ + mv_object.primitives[k].SPHERE_RADIUS + ] + obj = Sphere( + name=str(mv_object.id) + '_' + str(k) + '_sphere', + pose=object_pose, + radius=radius, + ) + objs.append(obj) + elif mv_object.primitives[k].type == SolidPrimitive.CYLINDER: + # cylinder: + cyl_height = mv_object.primitives[k].dimensions[ + mv_object.primitives[k].CYLINDER_HEIGHT + ] + cyl_radius = mv_object.primitives[k].dimensions[ + mv_object.primitives[k].CYLINDER_RADIUS + ] + obj = Cylinder( + name=str(mv_object.id) + '_' + str(k) + '_cylinder', + pose=object_pose, + height=cyl_height, + radius=cyl_radius, + ) + objs.append(obj) + elif mv_object.primitives[k].type == SolidPrimitive.CONE: + self.get_logger().error('Cone primitive is not supported') + supported_objects = False + else: + self.get_logger().error('Unknown primitive type') + supported_objects = False + if len(mv_object.meshes) > 0: + for k in range(len(mv_object.meshes)): + pose = mv_object.mesh_poses[k] + mesh_pose = [ + pose.position.x, + pose.position.y, + pose.position.z, + pose.orientation.w, + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + ] + object_pose = world_pose.multiply(Pose.from_list(mesh_pose)).tolist() + verts = mv_object.meshes[k].vertices + verts = [[v.x, v.y, v.z] for v in verts] + tris = [ + [v.vertex_indices[0], v.vertex_indices[1], v.vertex_indices[2]] + for v in mv_object.meshes[k].triangles + ] + + obj = Mesh( + name=str(mv_object.id) + '_' + str(len(objs)) + '_mesh', + pose=object_pose, + vertices=verts, + faces=tris, + ) + objs.append(obj) + return objs, supported_objects + + def get_joint_trajectory(self, js: CuJointState, dt: float): + traj = RobotTrajectory() + cmd_traj = JointTrajectory() + q_traj = js.position.cpu().view(-1, js.position.shape[-1]).numpy() + vel = js.velocity.cpu().view(-1, js.position.shape[-1]).numpy() + acc = js.acceleration.view(-1, js.position.shape[-1]).cpu().numpy() + for i in range(len(q_traj)): + traj_pt = JointTrajectoryPoint() + traj_pt.positions = q_traj[i].tolist() + if js is not None and i < len(vel): + traj_pt.velocities = vel[i].tolist() + if js is not None and i < len(acc): + traj_pt.accelerations = acc[i].tolist() + time_d = rclpy.time.Duration(seconds=i * dt).to_msg() + traj_pt.time_from_start = time_d + cmd_traj.points.append(traj_pt) + cmd_traj.joint_names = js.joint_names + cmd_traj.header.stamp = self.get_clock().now().to_msg() + traj.joint_trajectory = cmd_traj + return traj + + def update_world_objects(self, moveit_objects): + world_update_status = True + if len(moveit_objects) > 0: + cuboid_list = [] + sphere_list = [] + cylinder_list = [] + mesh_list = [] + for i, obj in enumerate(moveit_objects): + cumotion_objects, world_update_status = self.get_cumotion_collision_object(obj) + for cumotion_object in cumotion_objects: + if isinstance(cumotion_object, Cuboid): + cuboid_list.append(cumotion_object) + elif isinstance(cumotion_object, Cylinder): + cylinder_list.append(cumotion_object) + elif isinstance(cumotion_object, Sphere): + sphere_list.append(cumotion_object) + elif isinstance(cumotion_object, Mesh): + mesh_list.append(cumotion_object) + + world_model = WorldConfig( + cuboid=cuboid_list, + cylinder=cylinder_list, + sphere=sphere_list, + mesh=mesh_list, + ).get_collision_check_world() + self.motion_gen.update_world(world_model) + if self.__read_esdf_grid: + world_update_status = self.update_voxel_grid() + if self.__publish_curobo_world_as_voxels: + voxels = self.__world_collision.get_esdf_in_bounding_box( + Cuboid( + name='test', + pose=[0.0, 0.0, 0.0, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz + dims=self.__voxel_dims, + ), + voxel_size=self.__publish_voxel_size, + ) + xyzr_tensor = voxels.xyzr_tensor.clone() + xyzr_tensor[..., 3] = voxels.feature_tensor + self.publish_voxels(xyzr_tensor) + return world_update_status + + def execute_callback(self, goal_handle): + self.get_logger().info('Executing goal...') + # check moveit scaling factors: + min_scaling_factor = min(goal_handle.request.request.max_velocity_scaling_factor, + goal_handle.request.request.max_acceleration_scaling_factor) + time_dilation_factor = min(1.0, min_scaling_factor) + + if time_dilation_factor <= 0.0 or self.__override_moveit_scaling_factors: + time_dilation_factor = self.get_parameter( + 'time_dilation_factor').get_parameter_value().double_value + self.get_logger().info('Planning with time_dilation_factor: ' + + str(time_dilation_factor)) + plan_req = goal_handle.request.request + goal_handle.succeed() + + scene = goal_handle.request.planning_options.planning_scene_diff + + world_objects = scene.world.collision_objects + world_update_status = self.update_world_objects(world_objects) + result = MoveGroup.Result() + + if not world_update_status: + result.error_code.val = MoveItErrorCodes.COLLISION_CHECKING_UNAVAILABLE + self.get_logger().error('World update failed.') + return result + start_state = None + if len(plan_req.start_state.joint_state.position) > 0: + start_state = self.motion_gen.get_active_js( + CuJointState.from_position( + position=self.tensor_args.to_device( + plan_req.start_state.joint_state.position + ).unsqueeze(0), + joint_names=plan_req.start_state.joint_state.name, + ) + ) + else: + self.get_logger().info( + 'PlanRequest start state was empty, reading current joint state' + ) + if start_state is None or plan_req.start_state.is_diff: + if self.__js_buffer is None: + self.get_logger().error( + 'joint_state was not received from ' + self.__joint_states_topic + ) + return result + + # read joint state: + state = CuJointState.from_position( + position=self.tensor_args.to_device(self.__js_buffer['position']).unsqueeze(0), + joint_names=self.__js_buffer['joint_names'], + ) + state.velocity = self.tensor_args.to_device(self.__js_buffer['velocity']).unsqueeze(0) + current_joint_state = self.motion_gen.get_active_js(state) + if start_state is not None and plan_req.start_state.is_diff: + start_state.position += current_joint_state.position + start_state.velocity += current_joint_state.velocity + else: + start_state = current_joint_state + + if len(plan_req.goal_constraints[0].joint_constraints) > 0: + self.get_logger().info('Calculating goal pose from Joint target') + goal_config = [ + plan_req.goal_constraints[0].joint_constraints[x].position + for x in range(len(plan_req.goal_constraints[0].joint_constraints)) + ] + goal_jnames = [ + plan_req.goal_constraints[0].joint_constraints[x].joint_name + for x in range(len(plan_req.goal_constraints[0].joint_constraints)) + ] + + goal_state = self.motion_gen.get_active_js( + CuJointState.from_position( + position=self.tensor_args.to_device(goal_config).view(1, -1), + joint_names=goal_jnames, + ) + ) + goal_pose = self.motion_gen.compute_kinematics(goal_state).ee_pose.clone() + elif ( + len(plan_req.goal_constraints[0].position_constraints) > 0 + and len(plan_req.goal_constraints[0].orientation_constraints) > 0 + ): + self.get_logger().info('Using goal from Pose') + + position = ( + plan_req.goal_constraints[0] + .position_constraints[0] + .constraint_region.primitive_poses[0] + .position + ) + position = [position.x, position.y, position.z] + orientation = plan_req.goal_constraints[0].orientation_constraints[0].orientation + orientation = [orientation.w, orientation.x, orientation.y, orientation.z] + pose_list = position + orientation + goal_pose = Pose.from_list(pose_list, tensor_args=self.tensor_args) + + # Check if link names match: + position_link_name = plan_req.goal_constraints[0].position_constraints[0].link_name + orientation_link_name = ( + plan_req.goal_constraints[0].orientation_constraints[0].link_name + ) + plan_link_name = self.motion_gen.kinematics.ee_link + if position_link_name != orientation_link_name: + self.get_logger().error( + 'Link name for Target Position "' + + position_link_name + + '" and Target Orientation "' + + orientation_link_name + + '" do not match' + ) + result.error_code.val = MoveItErrorCodes.INVALID_LINK_NAME + return result + if position_link_name != plan_link_name: + self.get_logger().error( + 'Link name for Target Pose "' + + position_link_name + + '" and Planning frame "' + + plan_link_name + + '" do not match, relaunch node with tool_frame = ' + + position_link_name + ) + result.error_code.val = MoveItErrorCodes.INVALID_LINK_NAME + return result + else: + self.get_logger().error('Goal constraints not supported') + + self.motion_gen.reset(reset_seed=False) + motion_gen_result = self.motion_gen.plan_single( + start_state, + goal_pose, + MotionGenPlanConfig(max_attempts=5, enable_graph_attempt=1, + time_dilation_factor=time_dilation_factor), + ) + + result = MoveGroup.Result() + if motion_gen_result.success.item(): + result.error_code.val = MoveItErrorCodes.SUCCESS + result.trajectory_start = plan_req.start_state + traj = self.get_joint_trajectory( + motion_gen_result.optimized_plan, motion_gen_result.optimized_dt.item() + ) + result.planning_time = motion_gen_result.total_time + result.planned_trajectory = traj + elif not motion_gen_result.valid_query: + self.get_logger().error( + f'Invalid planning query: {motion_gen_result.status}' + ) + if motion_gen_result.status == MotionGenStatus.INVALID_START_STATE_JOINT_LIMITS: + result.error_code.val = MoveItErrorCodes.START_STATE_INVALID + if motion_gen_result.status in [ + MotionGenStatus.INVALID_START_STATE_WORLD_COLLISION, + MotionGenStatus.INVALID_START_STATE_SELF_COLLISION, + ]: + + result.error_code.val = MoveItErrorCodes.START_STATE_IN_COLLISION + else: + self.get_logger().error( + f'Motion planning failed wih status: {motion_gen_result.status}' + ) + if motion_gen_result.status == MotionGenStatus.IK_FAIL: + result.error_code.val = MoveItErrorCodes.NO_IK_SOLUTION + + self.get_logger().info( + 'returned planning result (query, success, failure_status): ' + + str(self.__query_count) + + ' ' + + str(motion_gen_result.success.item()) + + ' ' + + str(motion_gen_result.status) + ) + self.__query_count += 1 + return result + + def publish_voxels(self, voxels): + vox_size = self.__publish_voxel_size + + # create marker: + marker = Marker() + marker.header.frame_id = self.__robot_base_frame + marker.id = 0 + marker.type = 6 # cube list + marker.ns = 'curobo_world' + marker.action = 0 + marker.pose.orientation.w = 1.0 + marker.lifetime = rclpy.duration.Duration(seconds=1000.0).to_msg() + marker.frame_locked = False + marker.scale.x = vox_size + marker.scale.y = vox_size + marker.scale.z = vox_size + + # get only voxels that are inside surfaces: + + voxels = voxels[voxels[:, 3] >= 0.0] + vox = voxels.view(-1, 4).cpu().numpy() + marker.points = [] + + for i in range(min(len(vox), self.__max_publish_voxels)): + + pt = Point() + pt.x = float(vox[i, 0]) + pt.y = float(vox[i, 1]) + pt.z = float(vox[i, 2]) + color = ColorRGBA() + d = vox[i, 3] + + rgba = [min(1.0, 1.0 - float(d)), 0.0, 0.0, 1.0] + + color.r = rgba[0] + color.g = rgba[1] + color.b = rgba[2] + color.a = rgba[3] + marker.colors.append(color) + marker.points.append(pt) + # publish voxels: + marker.header.stamp = self.get_clock().now().to_msg() + + self.__voxel_pub.publish(marker) + + +def main(args=None): + rclpy.init(args=args) + cumotion_action_server = CumotionActionServer() + executor = MultiThreadedExecutor() + executor.add_node(cumotion_action_server) + try: + executor.spin() + except KeyboardInterrupt: + cumotion_action_server.get_logger().info('KeyboardInterrupt, shutting down.\n') + cumotion_action_server.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/robot_segmenter.py b/isaac_ros_cumotion/isaac_ros_cumotion/robot_segmenter.py new file mode 100644 index 0000000..a436df0 --- /dev/null +++ b/isaac_ros_cumotion/isaac_ros_cumotion/robot_segmenter.py @@ -0,0 +1,347 @@ +# Copyright (c) 2024 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 copy import deepcopy +import threading +import time + +from curobo.types.base import TensorDeviceType +from curobo.types.camera import CameraObservation +from curobo.types.math import Pose as CuPose +from curobo.types.state import JointState as CuJointState +from curobo.util_file import get_robot_configs_path +from curobo.util_file import join_path +from curobo.util_file import load_yaml +from curobo.wrap.model.robot_segmenter import RobotSegmenter +from cv_bridge import CvBridge +from isaac_ros_cumotion.util import get_spheres_marker +from isaac_ros_cumotion.xrdf_utils import convert_xrdf_to_curobo +from message_filters import ApproximateTimeSynchronizer +from message_filters import Subscriber +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +from sensor_msgs.msg import JointState +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +import torch +from visualization_msgs.msg import MarkerArray + + +torch.backends.cuda.matmul.allow_tf32 = True +torch.backends.cudnn.allow_tf32 = True + + +class CumotionRobotSegmenter(Node): + """This node filters out depth pixels assosiated with a robot body using a mask.""" + + def __init__(self): + super().__init__('cumotion_robot_segmentation') + self.declare_parameter('robot', 'ur5e.yml') + self.declare_parameter('cuda_device', 0) + self.declare_parameter('distance_threshold', 0.1) + self.declare_parameter('time_sync_slop', 0.1) + self.declare_parameter('tf_lookup_duration', 5.0) + + self.declare_parameter('joint_states_topic', '/joint_states') + self.declare_parameter('debug_robot_topic', '/cumotion/robot_segmenter/robot_spheres') + + self.declare_parameter('depth_image_topics', ['/cumotion/depth_1/image_raw']) + self.declare_parameter('depth_camera_infos', ['/cumotion/depth_1/camera_info']) + self.declare_parameter('robot_mask_publish_topics', ['/cumotion/depth_1/robot_mask']) + self.declare_parameter('world_depth_publish_topics', ['/cumotion/depth_1/world_depth']) + + self.declare_parameter('log_debug', False) + self.declare_parameter('urdf_path', rclpy.Parameter.Type.STRING) + + try: + self.__urdf_path = self.get_parameter('urdf_path') + self.__urdf_path = self.__urdf_path.get_parameter_value().string_value + except rclpy.exceptions.ParameterUninitializedException: + self.__urdf_path = None + + distance_threshold = ( + self.get_parameter('distance_threshold').get_parameter_value().double_value) + time_sync_slop = self.get_parameter('time_sync_slop').get_parameter_value().double_value + self._tf_lookup_duration = ( + self.get_parameter('tf_lookup_duration').get_parameter_value().double_value + ) + joint_states_topic = ( + self.get_parameter('joint_states_topic').get_parameter_value().string_value) + debug_robot_topic = ( + self.get_parameter('debug_robot_topic').get_parameter_value().string_value) + depth_image_topics = ( + self.get_parameter('depth_image_topics').get_parameter_value().string_array_value) + depth_camera_infos = ( + self.get_parameter('depth_camera_infos').get_parameter_value().string_array_value) + publish_mask_topics = ( + self.get_parameter( + 'robot_mask_publish_topics').get_parameter_value().string_array_value) + world_depth_topics = ( + self.get_parameter( + 'world_depth_publish_topics').get_parameter_value().string_array_value) + + self._log_debug = self.get_parameter('log_debug').get_parameter_value().bool_value + num_cameras = len(depth_image_topics) + self._num_cameras = num_cameras + + if len(depth_camera_infos) != num_cameras: + self.get_logger().error( + 'Number of topics in depth_camera_infos does not match depth_image_topics') + if len(publish_mask_topics) != num_cameras: + self.get_logger().error( + 'Number of topics in publish_mask_topics does not match depth_image_topics') + if len(world_depth_topics) != num_cameras: + self.get_logger().error( + 'Number of topics in world_depth_topics does not match depth_image_topics') + + cuda_device_id = self.get_parameter('cuda_device').get_parameter_value().integer_value + + self._tensor_args = TensorDeviceType(device=torch.device('cuda', cuda_device_id)) + + # Create subscribers: + subscribers = [Subscriber(self, Image, topic) for topic in depth_image_topics] + subscribers.append(Subscriber(self, JointState, joint_states_topic)) + # Subscribe to topics with sync: + self.approx_time_sync = ApproximateTimeSynchronizer( + tuple(subscribers), queue_size=100, slop=time_sync_slop) + self.approx_time_sync.registerCallback(self.process_depth_and_joint_state) + + self.info_subscribers = [] + + for idx in range(num_cameras): + self.info_subscribers.append( + self.create_subscription( + CameraInfo, depth_camera_infos[idx], + lambda msg, index=idx: self.camera_info_cb(msg, index), 10) + ) + + self.mask_publishers = [ + self.create_publisher(Image, topic, 10) for topic in publish_mask_topics] + self.segmented_publishers = [ + self.create_publisher(Image, topic, 10) for topic in world_depth_topics] + + self.debug_robot_publisher = self.create_publisher(MarkerArray, debug_robot_topic, 10) + + self.tf_buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=60.0)) + self.tf_listener = TransformListener(self.tf_buffer, self) + + # Create a depth mask publisher: + robot_file = self.get_parameter('robot').get_parameter_value().string_value + self.br = CvBridge() + + # Create buffers to store data: + self._depth_buffers = None + self._depth_intrinsics = [None for x in range(num_cameras)] + self._robot_pose_camera = [None for x in range(num_cameras)] + self._depth_encoding = None + + self._js_buffer = None + self._timestamp = None + self._camera_headers = [] + self.lock = threading.Lock() + self.timer = self.create_timer(0.01, self.on_timer) + + robot_dict = load_yaml(join_path(get_robot_configs_path(), robot_file)) + if robot_file.lower().endswith('.xrdf'): + if self.__urdf_path is None: + self.get_logger().fatal('urdf_path is required to load robot from .xrdf') + raise SystemExit + robot_dict = convert_xrdf_to_curobo(self.__urdf_path, robot_dict, self.get_logger()) + + if self.__urdf_path is not None: + robot_dict['robot_cfg']['kinematics']['urdf_path'] = self.__urdf_path + + self._cumotion_segmenter = RobotSegmenter.from_robot_file( + robot_dict, distance_threshold=distance_threshold) + + self._cumotion_base_frame = self._cumotion_segmenter.base_link + self._robot_pose_cameras = None + self.get_logger().info(f'Node initialized with {self._num_cameras} cameras') + + def process_depth_and_joint_state(self, *msgs): + self._depth_buffers = [] + self._depth_encoding = [] + self._camera_headers = [] + for msg in msgs: + if (isinstance(msg, Image)): + img = self.br.imgmsg_to_cv2(msg) + if msg.encoding == '32FC1': + img = 1000.0 * img + self._depth_buffers.append(img) + self._camera_headers.append(msg.header) + self._depth_encoding.append(msg.encoding) + if (isinstance(msg, JointState)): + self._js_buffer = {'joint_names': msg.name, 'position': msg.position} + self._timestamp = msg.header.stamp + + def camera_info_cb(self, msg, idx): + self._depth_intrinsics[idx] = msg.k + + def publish_robot_spheres(self, traj: CuJointState): + kin_state = self._cumotion_segmenter.robot_world.get_kinematics(traj.position) + spheres = kin_state.link_spheres_tensor.cpu().numpy() + current_time = self.get_clock().now().to_msg() + + m_arr = get_spheres_marker( + spheres[0], + self._cumotion_base_frame, + current_time, + rgb=[0.0, 1.0, 0.0, 1.0], + ) + + self.debug_robot_publisher.publish(m_arr) + + def is_subscribed(self) -> bool: + count_mask = max( + [mask_pub.get_subscription_count() for mask_pub in self.mask_publishers] + + [seg_pub.get_subscription_count() for seg_pub in self.segmented_publishers] + ) + if count_mask > 0: + return True + return False + + def publish_images(self, depth_mask, segmented_depth, camera_header, idx: int): + + if self.mask_publishers[idx].get_subscription_count() > 0: + depth_mask = depth_mask[idx] + msg = self.br.cv2_to_imgmsg(depth_mask, 'mono8') + msg.header = camera_header[idx] + self.mask_publishers[idx].publish(msg) + + if self.segmented_publishers[idx].get_subscription_count() > 0: + segmented_depth = segmented_depth[idx] + if self._depth_encoding[idx] == '16UC1': + segmented_depth = segmented_depth.astype(np.uint16) + elif self._depth_encoding[idx] == '32FC1': + segmented_depth = segmented_depth / 1000.0 + msg = self.br.cv2_to_imgmsg(segmented_depth, self._depth_encoding[idx]) + msg.header = camera_header[idx] + self.segmented_publishers[idx].publish(msg) + + def on_timer(self): + computation_time = -1.0 + node_time = -1.0 + + if not self.is_subscribed(): + return + + if ((not all(isinstance(intrinsic, np.ndarray) for intrinsic in self._depth_intrinsics)) + or (len(self._camera_headers) == 0) or (self._timestamp is None)): + return + + timestamp = self._timestamp + + # Read camera transforms + if self._robot_pose_cameras is None: + self.get_logger().info('Reading TF from cameras') + + with self.lock: + camera_headers = deepcopy(self._camera_headers) + + for i in range(self._num_cameras): + if self._robot_pose_camera[i] is None: + try: + t = self.tf_buffer.lookup_transform( + self._cumotion_base_frame, + camera_headers[i].frame_id, + timestamp, + rclpy.duration.Duration(seconds=self._tf_lookup_duration), + ) + self._robot_pose_camera[i] = CuPose.from_list( + [ + t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z, + t.transform.rotation.w, + t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + ] + ) + except TransformException as ex: + self.get_logger().debug(f'Could not transform {camera_headers[i].frame_id} \ + to { self._cumotion_base_frame}: {ex}') + continue + if None not in self._robot_pose_camera: + self._robot_pose_cameras = CuPose.cat(self._robot_pose_camera) + self.get_logger().info('Received TF from cameras to robot') + + # Check if all camera transforms have been received + if self._robot_pose_cameras is None: + return + + with self.lock: + timestamp = self._timestamp + depth_image = np.copy(np.stack((self._depth_buffers))) + intrinsics = np.copy(np.stack(self._depth_intrinsics)) + js = np.copy(self._js_buffer['position']) + j_names = deepcopy(self._js_buffer['joint_names']) + camera_headers = deepcopy(self._camera_headers) + self._timestamp = None + self._camera_headers = [] + start_node_time = time.time() + + depth_image = self._tensor_args.to_device(depth_image.astype(np.float32)) + depth_image = depth_image.view( + self._num_cameras, depth_image.shape[-2], depth_image.shape[-1]) + + if not self._cumotion_segmenter.ready: + intrinsics = self._tensor_args.to_device(intrinsics).view(self._num_cameras, 3, 3) + cam_obs = CameraObservation(depth_image=depth_image, intrinsics=intrinsics) + self._cumotion_segmenter.update_camera_projection(cam_obs) + self.get_logger().info('Updated Projection Matrices') + cam_obs = CameraObservation(depth_image=depth_image, pose=self._robot_pose_cameras) + q = CuJointState.from_numpy( + position=js, joint_names=j_names, tensor_args=self._tensor_args).unsqueeze(0) + q = self._cumotion_segmenter.robot_world.get_active_js(q) + + start_segmentation_time = time.time() + depth_mask, segmented_depth = self._cumotion_segmenter.get_robot_mask_from_active_js( + cam_obs, q) + if self._log_debug: + torch.cuda.synchronize() + computation_time = time.time() - start_segmentation_time + depth_mask = depth_mask.cpu().numpy().astype(np.uint8) * 255 + segmented_depth = segmented_depth.cpu().numpy() + + for x in range(depth_mask.shape[0]): + self.publish_images(depth_mask, segmented_depth, camera_headers, x) + + if self.debug_robot_publisher.get_subscription_count() > 0: + self.publish_robot_spheres(q) + if self._log_debug: + node_time = time.time() - start_node_time + self.get_logger().info(f'Node Time(ms), Computation Time(ms): {node_time * 1000.0},\ + {computation_time * 1000.0}') + + +def main(args=None): + + # Initialize the rclpy library + rclpy.init(args=args) + + # Create the node + cumotion_segmenter = CumotionRobotSegmenter() + + # Spin the node so the callback function is called. + rclpy.spin(cumotion_segmenter) + + # Destroy the node explicitly + cumotion_segmenter.destroy_node() + + # Shutdown the ROS client library for Python + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/util.py b/isaac_ros_cumotion/isaac_ros_cumotion/util.py new file mode 100644 index 0000000..64a165f --- /dev/null +++ b/isaac_ros_cumotion/isaac_ros_cumotion/util.py @@ -0,0 +1,51 @@ +# Copyright (c) 2024 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 curobo.geom.types import Sphere +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + + +def get_spheres_marker( + robot_spheres, base_frame: str, time, rgb=[0.1, 0.1, 0.1, 0.5], start_idx: int = 0): + m_arr = MarkerArray() + + for i in range(len(robot_spheres)): + r_s = Sphere( + name='sphere', + radius=robot_spheres[i, -1], + pose=robot_spheres[i, :3].tolist() + [1, 0, 0, 0], + ) + # print(rgb[i]) + m = get_marker_sphere(r_s, base_frame, time, start_idx + i, rgb) + m_arr.markers.append(m) + return m_arr + + +def get_marker_sphere(sphere: Sphere, base_frame: str, time, idx=0, rgb=[0.4, 0.4, 0.8, 1.0]): + marker = Marker() + marker.header.frame_id = base_frame + marker.header.stamp = time + marker.id = idx + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.scale.x = sphere.radius * 2 + marker.scale.y = sphere.radius * 2 + marker.scale.z = sphere.radius * 2 + marker.color.r = rgb[0] + marker.color.g = rgb[1] + marker.color.b = rgb[2] + marker.color.a = rgb[3] + + # pose: + marker.pose.position.x = sphere.position[0] + marker.pose.position.y = sphere.position[1] + marker.pose.position.z = sphere.position[2] + marker.pose.orientation.w = 1.0 + return marker diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/xrdf_utils.py b/isaac_ros_cumotion/isaac_ros_cumotion/xrdf_utils.py new file mode 100644 index 0000000..746ab46 --- /dev/null +++ b/isaac_ros_cumotion/isaac_ros_cumotion/xrdf_utils.py @@ -0,0 +1,158 @@ +# Copyright (c) 2024 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 typing import Any, Dict, Union + +from curobo.cuda_robot_model.urdf_kinematics_parser import UrdfKinematicsParser +from curobo.util_file import load_yaml + + +def return_value_if_exists(input_dict: Dict, key: str, logger, suffix: str = 'xrdf') -> Any: + if key not in input_dict: + logger.error(key + ' key not found in ' + suffix) + raise ValueError(key + ' key not found in ' + suffix) + return input_dict[key] + + +def convert_xrdf_to_curobo(urdf_string: str, input_dict: Union[str, Dict], logger) -> Dict: + # urdf_string needs to be a path to a urdf file. + + if isinstance(input_dict, str): + input_dict = load_yaml(input_dict) + + if return_value_if_exists(input_dict, 'format', logger) != 'xrdf': + logger.error('format is not xrdf') + raise ValueError('format is not xrdf') + + if return_value_if_exists(input_dict, 'format_version', logger) > 1.0: + logger.warn('format_version is greater than 1.0') + # Also get base link as root of urdf + kinematics_parser = UrdfKinematicsParser(urdf_string, build_scene_graph=True) + joint_names = kinematics_parser.get_controlled_joint_names() + base_link = kinematics_parser.root_link + + output_dict = {} + if 'collision' in input_dict: + + coll_name = return_value_if_exists(input_dict['collision'], 'geometry', logger) + + if 'spheres' not in input_dict['geometry'][coll_name]: + logger.error('spheres key not found in xrdf') + raise ValueError('spheres key not found in xrdf') + coll_spheres = return_value_if_exists(input_dict['geometry'][coll_name], 'spheres', logger) + output_dict['collision_spheres'] = coll_spheres + + buffer_distance = return_value_if_exists( + input_dict['collision'], 'buffer_distance', logger + ) + output_dict['collision_sphere_buffer'] = buffer_distance + output_dict['collision_link_names'] = list(coll_spheres.keys()) + + if 'self_collision' in input_dict: + if input_dict['self_collision']['geometry'] != input_dict['collision']['geometry']: + logger.error('self_collision geometry does not match collision geometry') + raise ValueError('self_collision geometry does not match collision geometry') + + self_collision_ignore = return_value_if_exists( + input_dict['self_collision'], + 'ignore', + logger, + ) + + self_collision_buffer = return_value_if_exists( + input_dict['self_collision'], + 'buffer_distance', + logger, + ) + + output_dict['self_collision_ignore'] = self_collision_ignore + output_dict['self_collision_buffer'] = self_collision_buffer + else: + logger.error('self_collision key not found in xrdf') + raise ValueError('self_collision key not found in xrdf') + else: + logger.warn('collision key not found in xrdf, collision avoidance is disabled') + + tool_frames = return_value_if_exists(input_dict, 'tool_frames', logger) + output_dict['ee_link'] = tool_frames[0] + output_dict['link_names'] = None + if len(tool_frames) > 1: + output_dict['link_names'] = input_dict['tool_frames'] + + # cspace: + cspace_dict = return_value_if_exists(input_dict, 'cspace', logger) + + active_joints = return_value_if_exists(cspace_dict, 'joint_names', logger) + + default_joint_positions = return_value_if_exists(input_dict, 'default_joint_positions', logger) + active_config = [] + locked_joints = {} + + for j in joint_names: + if j in active_joints: + if j in default_joint_positions: + active_config.append(default_joint_positions[j]) + else: + active_config.append(0.0) + else: + locked_joints[j] = 0.0 + if j in default_joint_positions: + locked_joints[j] = default_joint_positions[j] + + acceleration_limits = return_value_if_exists(cspace_dict, 'acceleration_limits', logger) + jerk_limits = return_value_if_exists(cspace_dict, 'jerk_limits', logger) + + max_acc = max(acceleration_limits) + max_jerk = max(jerk_limits) + output_dict['lock_joints'] = locked_joints + all_joint_names = active_joints + list(locked_joints.keys()) + output_cspace = { + 'joint_names': all_joint_names, + 'retract_config': active_config + list(locked_joints.values()), + 'null_space_weight': [1.0 for _ in range(len(all_joint_names))], + 'cspace_distance_weight': [1.0 for _ in range(len(all_joint_names))], + 'max_acceleration': acceleration_limits + + [max_acc for _ in range(len(all_joint_names) - len(active_joints))], + 'max_jerk': jerk_limits + + [max_jerk for _ in range(len(all_joint_names) - len(active_joints))], + } + + output_dict['cspace'] = output_cspace + + extra_links = {} + if 'modifiers' in input_dict: + for k in range(len(input_dict['modifiers'])): + mod_list = list(input_dict['modifiers'][k].keys()) + if len(mod_list) > 1: + logger.error('Each modifier should have only one key') + raise ValueError('Each modifier should have only one key') + mod_type = mod_list[0] + if mod_type == 'set_base_frame': + base_link = input_dict['modifiers'][k]['set_base_frame'] + elif mod_type == 'add_frame': + frame_data = input_dict['modifiers'][k]['add_frame'] + extra_links[frame_data['frame_name']] = { + 'parent_link_name': frame_data['parent_frame_name'], + 'link_name': frame_data['frame_name'], + 'joint_name': frame_data['joint_name'], + 'joint_type': frame_data['joint_type'], + 'fixed_transform': frame_data['fixed_transform']['position'] + + [frame_data['fixed_transform']['orientation']['w']] + + frame_data['fixed_transform']['orientation']['xyz'], + } + else: + logger.warn('XRDF modifier "' + mod_type + '" not recognized') + output_dict['extra_links'] = extra_links + + output_dict['base_link'] = base_link + + output_dict['urdf_path'] = urdf_string + + output_dict = {'robot_cfg': {'kinematics': output_dict}} + return output_dict diff --git a/isaac_ros_cumotion/launch/isaac_ros_cumotion.launch.py b/isaac_ros_cumotion/launch/isaac_ros_cumotion.launch.py new file mode 100644 index 0000000..efb60e1 --- /dev/null +++ b/isaac_ros_cumotion/launch/isaac_ros_cumotion.launch.py @@ -0,0 +1,64 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import yaml + + +def read_params(pkg_name, params_dir, params_file_name): + params_file = os.path.join( + get_package_share_directory(pkg_name), params_dir, params_file_name) + return yaml.safe_load(open(params_file, 'r')) + + +def launch_args_from_params(pkg_name, params_dir, params_file_name, prefix: str = None): + launch_args = [] + launch_configs = {} + params = read_params(pkg_name, params_dir, params_file_name) + + for param, value in params['/**']['ros__parameters'].items(): + if value is not None: + arg_name = param if prefix is None else f'{prefix}.{param}' + launch_args.append(DeclareLaunchArgument(name=arg_name, default_value=str(value))) + launch_configs[param] = LaunchConfiguration(arg_name) + + return launch_args, launch_configs + + +def generate_launch_description(): + """Launch file to bring up cumotion planner node.""" + launch_args, launch_configs = launch_args_from_params( + 'isaac_ros_cumotion', 'params', 'isaac_ros_cumotion_params.yaml', 'cumotion_planner') + + cumotion_planner_node = Node( + name='cumotion_planner', + package='isaac_ros_cumotion', + namespace='', + executable='cumotion_planner_node', + parameters=[ + launch_configs + ], + output='screen', + ) + + return launch.LaunchDescription(launch_args + [cumotion_planner_node]) diff --git a/isaac_ros_cumotion/launch/robot_segmentation.launch.py b/isaac_ros_cumotion/launch/robot_segmentation.launch.py new file mode 100644 index 0000000..1997300 --- /dev/null +++ b/isaac_ros_cumotion/launch/robot_segmentation.launch.py @@ -0,0 +1,62 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import yaml + + +def read_params(pkg_name, params_dir, params_file_name): + params_file = os.path.join( + get_package_share_directory(pkg_name), params_dir, params_file_name) + return yaml.safe_load(open(params_file, 'r')) + + +def launch_args_from_params(pkg_name, params_dir, params_file_name, prefix: str = None): + launch_args = [] + launch_configs = {} + params = read_params(pkg_name, params_dir, params_file_name) + + for param, value in params['/**']['ros__parameters'].items(): + if value is not None: + arg_name = param if prefix is None else f'{prefix}.{param}' + launch_args.append(DeclareLaunchArgument(name=arg_name, default_value=str(value))) + launch_configs[param] = LaunchConfiguration(arg_name) + + return launch_args, launch_configs + + +def generate_launch_description(): + """Launch file to bring up robot segmenter node.""" + launch_args, launch_configs = launch_args_from_params( + 'isaac_ros_cumotion', 'params', 'robot_segmentation_params.yaml', 'robot_segmenter') + + robot_segmenter_node = Node( + package='isaac_ros_cumotion', + namespace='', + executable='robot_segmenter_node', + name='robot_segmenter', + parameters=[launch_configs], + output='screen', + ) + + return LaunchDescription(launch_args + [robot_segmenter_node]) diff --git a/isaac_ros_cumotion/package.xml b/isaac_ros_cumotion/package.xml new file mode 100644 index 0000000..40a3f5c --- /dev/null +++ b/isaac_ros_cumotion/package.xml @@ -0,0 +1,33 @@ + + + + isaac_ros_cumotion + 3.0.0 + Package adds a cuMotion planner node + Isaac ROS Maintainers + NVIDIA Isaac ROS Software License + https://developer.nvidia.com/isaac-ros-gems/ + Balakumar Sundaralingam + + message_filters + moveit_msgs + nvblox_msgs + shape_msgs + sensor_msgs + std_msgs + trajectory_msgs + visualization_msgs + + curobo_core + isaac_ros_cumotion_robot_description + ros2launch + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml b/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml new file mode 100644 index 0000000..ed48c41 --- /dev/null +++ b/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + robot: "" + time_dilation_factor: 0.5 + collision_cache_mesh: 20 + collision_cache_cuboid: 20 + interpolation_dt: 0.02 + voxel_dims: [2.0, 2.0, 2.0] + voxel_size: 0.05 + read_esdf_world: False + publish_curobo_world_as_voxels: False + add_ground_plane: False + publish_voxel_size: 0.05 + max_publish_voxels: 50000 + tool_frame: "" + grid_position: [0.0, 0.0, 0.0] + esdf_service_name: "/nvblox_node/get_esdf_and_gradient" + urdf_path: "" + enable_curobo_debug_mode: False + override_moveit_scaling_factors: False \ No newline at end of file diff --git a/isaac_ros_cumotion/params/robot_segmentation_params.yaml b/isaac_ros_cumotion/params/robot_segmentation_params.yaml new file mode 100644 index 0000000..5e64019 --- /dev/null +++ b/isaac_ros_cumotion/params/robot_segmentation_params.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + robot: ur5e.yml + depth_image_topics: [/depth_image] + depth_camera_infos: [/rgb/camera_info] + robot_mask_publish_topics: [/cumotion/camera_1/robot_mask] + world_depth_publish_topics: [/cumotion/camera_1/world_depth] + joint_states_topic: /joint_states + debug_robot_topic: /cumotion/robot_segmenter/robot_spheres + distance_threshold: 0.2 + time_sync_slop: 0.1 + tf_lookup_duration: 5.0 + cuda_device: 0 + log_debug: False + urdf_path: null diff --git a/isaac_ros_cumotion/resource/isaac_ros_cumotion b/isaac_ros_cumotion/resource/isaac_ros_cumotion new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_cumotion/setup.cfg b/isaac_ros_cumotion/setup.cfg new file mode 100644 index 0000000..65a2d28 --- /dev/null +++ b/isaac_ros_cumotion/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_cumotion +[install] +install_scripts=$base/lib/isaac_ros_cumotion \ No newline at end of file diff --git a/isaac_ros_cumotion/setup.py b/isaac_ros_cumotion/setup.py new file mode 100644 index 0000000..f03c486 --- /dev/null +++ b/isaac_ros_cumotion/setup.py @@ -0,0 +1,55 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_cumotion' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ( + os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*')), + ), + ( + os.path.join('share', package_name, 'params'), + glob(os.path.join('params', '*.[yma]*')), + ), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + author='Balakumar Sundaralingam', + description='Package adds a cuMotion planner node', + license='NVIDIA Isaac ROS Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'cumotion_planner_node = isaac_ros_cumotion.cumotion_planner:main', + 'robot_segmenter_node = isaac_ros_cumotion.robot_segmenter:main', + ], + }, +) diff --git a/isaac_ros_cumotion/test/test_copyright.py b/isaac_ros_cumotion/test/test_copyright.py new file mode 100644 index 0000000..e445f78 --- /dev/null +++ b/isaac_ros_cumotion/test/test_copyright.py @@ -0,0 +1,24 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.skip(reason='This package has a proprietary license') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_cumotion/test/test_flake8.py b/isaac_ros_cumotion/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_cumotion/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_cumotion/test/test_pep257.py b/isaac_ros_cumotion/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_cumotion/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_cumotion_examples/isaac_sim_scripts/start_isaac_sim_franka.py b/isaac_ros_cumotion_examples/isaac_sim_scripts/start_isaac_sim_franka.py new file mode 100644 index 0000000..0758133 --- /dev/null +++ b/isaac_ros_cumotion_examples/isaac_sim_scripts/start_isaac_sim_franka.py @@ -0,0 +1,321 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2020-2024, NVIDIA CORPORATION. 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. +# +# Portions contributed by PickNik, LLC under BSD 3-Clause License +# +# Copyright (c) 2023, PickNik, LLC. +# 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. +# +# This Isaac Sim example is derived from +# https://github.com/ros-planning/moveit2_tutorials/blob/efef1d3/doc/how_to_guides/isaac_panda/launch/isaac_moveit.py +# which in turn was derived from an example provided with Isaac Sim 2022.2.1, found at +# standalone_examples/api/omni.isaac.ros2_bridge/moveit.py +# +# flake8: noqa + +import sys +import re +import os + +import carb +import numpy as np +from omni.isaac.kit import SimulationApp + +FRANKA_STAGE_PATH = "/Franka" +FRANKA_USD_PATH = "/Isaac/Robots/Franka/franka_alt_fingers.usd" +CAMERA_PRIM_PATH = f"{FRANKA_STAGE_PATH}/panda_hand/geometry/realsense/realsense_camera" +BACKGROUND_STAGE_PATH = "/background" +BACKGROUND_USD_PATH = "/Isaac/Environments/Simple_Room/simple_room.usd" +GRAPH_PATH = "/ActionGraph" +REALSENSE_VIEWPORT_NAME = "realsense_viewport" + +CONFIG = {"renderer": "RayTracedLighting", "headless": False} + +# Example ROS2 bridge sample demonstrating the manual loading of stages +# and creation of ROS components +simulation_app = SimulationApp(CONFIG) + + +# More imports that need to compare after we create the app +from omni.isaac.core import SimulationContext # noqa E402 +from omni.isaac.core.utils.prims import set_targets +from omni.isaac.core.utils import ( # noqa E402 + extensions, + nucleus, + prims, + rotations, + stage, + viewports, +) +from pxr import Gf, UsdGeom # noqa E402 +import omni.graph.core as og # noqa E402 +import omni + +# enable ROS2 bridge extension +extensions.enable_extension("omni.isaac.ros2_bridge") + +simulation_context = SimulationContext(stage_units_in_meters=1.0) + +# Locate Isaac Sim assets folder to load environment and robot stages +assets_root_path = nucleus.get_assets_root_path() +if assets_root_path is None: + carb.log_error("Could not find Isaac Sim assets folder") + simulation_app.close() + sys.exit() + +# Preparing stage +viewports.set_camera_view(eye=np.array([1.2, 1.2, 0.8]), target=np.array([0, 0, 0.5])) + +# Loading the simple_room environment +stage.add_reference_to_stage( + assets_root_path + BACKGROUND_USD_PATH, BACKGROUND_STAGE_PATH +) + +# Loading the franka robot USD +prims.create_prim( + FRANKA_STAGE_PATH, + "Xform", + position=np.array([0, -0.64, 0]), + orientation=rotations.gf_rotation_to_np_array(Gf.Rotation(Gf.Vec3d(0, 0, 1), 90)), + usd_path=assets_root_path + FRANKA_USD_PATH, +) + +# add some objects, spread evenly along the X axis +# with a fixed offset from the robot in the Y and Z +prims.create_prim( + "/cracker_box", + "Xform", + position=np.array([-0.2, -0.25, 0.15]), + orientation=rotations.gf_rotation_to_np_array(Gf.Rotation(Gf.Vec3d(1, 0, 0), -90)), + usd_path=assets_root_path + + "/Isaac/Props/YCB/Axis_Aligned_Physics/003_cracker_box.usd", +) +prims.create_prim( + "/sugar_box", + "Xform", + position=np.array([-0.07, -0.25, 0.1]), + orientation=rotations.gf_rotation_to_np_array(Gf.Rotation(Gf.Vec3d(0, 1, 0), -90)), + usd_path=assets_root_path + + "/Isaac/Props/YCB/Axis_Aligned_Physics/004_sugar_box.usd", +) +prims.create_prim( + "/soup_can", + "Xform", + position=np.array([0.1, -0.25, 0.10]), + orientation=rotations.gf_rotation_to_np_array(Gf.Rotation(Gf.Vec3d(1, 0, 0), -90)), + usd_path=assets_root_path + + "/Isaac/Props/YCB/Axis_Aligned_Physics/005_tomato_soup_can.usd", +) +prims.create_prim( + "/mustard_bottle", + "Xform", + position=np.array([0.0, 0.15, 0.12]), + orientation=rotations.gf_rotation_to_np_array(Gf.Rotation(Gf.Vec3d(1, 0, 0), -90)), + usd_path=assets_root_path + + "/Isaac/Props/YCB/Axis_Aligned_Physics/006_mustard_bottle.usd", +) + +simulation_app.update() + +try: + ros_domain_id = int(os.environ["ROS_DOMAIN_ID"]) + print("Using ROS_DOMAIN_ID: ", ros_domain_id) +except ValueError: + print("Invalid ROS_DOMAIN_ID integer value. Setting value to 0") + ros_domain_id = 0 +except KeyError: + print("ROS_DOMAIN_ID environment variable is not set. Setting value to 0") + ros_domain_id = 0 + +# Creating a action graph with ROS component nodes +try: + og.Controller.edit( + {"graph_path": GRAPH_PATH, "evaluator_name": "execution"}, + { + og.Controller.Keys.CREATE_NODES: [ + ("OnImpulseEvent", "omni.graph.action.OnImpulseEvent"), + ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"), + ("Context", "omni.isaac.ros2_bridge.ROS2Context"), + ("PublishJointState", "omni.isaac.ros2_bridge.ROS2PublishJointState"), + ( + "SubscribeJointState", + "omni.isaac.ros2_bridge.ROS2SubscribeJointState", + ), + ( + "ArticulationController", + "omni.isaac.core_nodes.IsaacArticulationController", + ), + ("PublishClock", "omni.isaac.ros2_bridge.ROS2PublishClock"), + ("OnTick", "omni.graph.action.OnTick"), + ("createViewport", "omni.isaac.core_nodes.IsaacCreateViewport"), + ( + "getRenderProduct", + "omni.isaac.core_nodes.IsaacGetViewportRenderProduct", + ), + ("setCamera", "omni.isaac.core_nodes.IsaacSetCameraOnRenderProduct"), + ("cameraHelperRgb", "omni.isaac.ros2_bridge.ROS2CameraHelper"), + ("cameraHelperInfo", "omni.isaac.ros2_bridge.ROS2CameraHelper"), + ("cameraHelperDepth", "omni.isaac.ros2_bridge.ROS2CameraHelper"), + ], + og.Controller.Keys.CONNECT: [ + ("OnImpulseEvent.outputs:execOut", "PublishJointState.inputs:execIn"), + ("OnImpulseEvent.outputs:execOut", "SubscribeJointState.inputs:execIn"), + ("OnImpulseEvent.outputs:execOut", "PublishClock.inputs:execIn"), + ( + "OnImpulseEvent.outputs:execOut", + "ArticulationController.inputs:execIn", + ), + ("Context.outputs:context", "PublishJointState.inputs:context"), + ("Context.outputs:context", "SubscribeJointState.inputs:context"), + ("Context.outputs:context", "PublishClock.inputs:context"), + ( + "ReadSimTime.outputs:simulationTime", + "PublishJointState.inputs:timeStamp", + ), + ("ReadSimTime.outputs:simulationTime", "PublishClock.inputs:timeStamp"), + ( + "SubscribeJointState.outputs:jointNames", + "ArticulationController.inputs:jointNames", + ), + ( + "SubscribeJointState.outputs:positionCommand", + "ArticulationController.inputs:positionCommand", + ), + ( + "SubscribeJointState.outputs:velocityCommand", + "ArticulationController.inputs:velocityCommand", + ), + ( + "SubscribeJointState.outputs:effortCommand", + "ArticulationController.inputs:effortCommand", + ), + ("OnTick.outputs:tick", "createViewport.inputs:execIn"), + ("createViewport.outputs:execOut", "getRenderProduct.inputs:execIn"), + ("createViewport.outputs:viewport", "getRenderProduct.inputs:viewport"), + ("getRenderProduct.outputs:execOut", "setCamera.inputs:execIn"), + ( + "getRenderProduct.outputs:renderProductPath", + "setCamera.inputs:renderProductPath", + ), + ("setCamera.outputs:execOut", "cameraHelperRgb.inputs:execIn"), + ("setCamera.outputs:execOut", "cameraHelperInfo.inputs:execIn"), + ("setCamera.outputs:execOut", "cameraHelperDepth.inputs:execIn"), + ("Context.outputs:context", "cameraHelperRgb.inputs:context"), + ("Context.outputs:context", "cameraHelperInfo.inputs:context"), + ("Context.outputs:context", "cameraHelperDepth.inputs:context"), + ( + "getRenderProduct.outputs:renderProductPath", + "cameraHelperRgb.inputs:renderProductPath", + ), + ( + "getRenderProduct.outputs:renderProductPath", + "cameraHelperInfo.inputs:renderProductPath", + ), + ( + "getRenderProduct.outputs:renderProductPath", + "cameraHelperDepth.inputs:renderProductPath", + ), + ], + og.Controller.Keys.SET_VALUES: [ + ("Context.inputs:domain_id", ros_domain_id), + # Setting the /Franka target prim to Articulation Controller node + ("ArticulationController.inputs:usePath", True), + ("ArticulationController.inputs:robotPath", FRANKA_STAGE_PATH), + ("PublishJointState.inputs:topicName", "isaac_joint_states"), + ("SubscribeJointState.inputs:topicName", "isaac_joint_commands"), + ("createViewport.inputs:name", REALSENSE_VIEWPORT_NAME), + ("createViewport.inputs:viewportId", 1), + ("cameraHelperRgb.inputs:frameId", "sim_camera"), + ("cameraHelperRgb.inputs:topicName", "rgb"), + ("cameraHelperRgb.inputs:type", "rgb"), + ("cameraHelperInfo.inputs:frameId", "sim_camera"), + ("cameraHelperInfo.inputs:topicName", "camera_info"), + ("cameraHelperInfo.inputs:type", "camera_info"), + ("cameraHelperDepth.inputs:frameId", "sim_camera"), + ("cameraHelperDepth.inputs:topicName", "depth"), + ("cameraHelperDepth.inputs:type", "depth"), + ], + }, + ) +except Exception as e: + print(e) + + +# Setting the /Franka target prim to Publish JointState node +set_targets( + prim=stage.get_current_stage().GetPrimAtPath("/ActionGraph/PublishJointState"), + attribute="inputs:targetPrim", + target_prim_paths=[FRANKA_STAGE_PATH], +) + +# Fix camera settings since the defaults in the realsense model are inaccurate +realsense_prim = camera_prim = UsdGeom.Camera( + stage.get_current_stage().GetPrimAtPath(CAMERA_PRIM_PATH) +) +realsense_prim.GetHorizontalApertureAttr().Set(20.955) +realsense_prim.GetVerticalApertureAttr().Set(15.7) +realsense_prim.GetFocalLengthAttr().Set(18.8) +realsense_prim.GetFocusDistanceAttr().Set(400) + +set_targets( + prim=stage.get_current_stage().GetPrimAtPath(GRAPH_PATH + "/setCamera"), + attribute="inputs:cameraPrim", + target_prim_paths=[CAMERA_PRIM_PATH], +) + +simulation_app.update() + +# need to initialize physics getting any articulation..etc +simulation_context.initialize_physics() + +simulation_context.play() + +# Dock the second camera window +viewport = omni.ui.Workspace.get_window("Viewport") +rs_viewport = omni.ui.Workspace.get_window(REALSENSE_VIEWPORT_NAME) +rs_viewport.dock_in(viewport, omni.ui.DockPosition.RIGHT) + + +while simulation_app.is_running(): + + # Run with a fixed step size + simulation_context.step(render=True) + + # Tick the Publish/Subscribe JointState, Publish TF and Publish Clock nodes each frame + og.Controller.set( + og.Controller.attribute("/ActionGraph/OnImpulseEvent.state:enableImpulse"), True + ) + +simulation_context.stop() +simulation_app.close() diff --git a/isaac_ros_cumotion_examples/launch/franka.launch.py b/isaac_ros_cumotion_examples/launch/franka.launch.py new file mode 100644 index 0000000..769335f --- /dev/null +++ b/isaac_ros_cumotion_examples/launch/franka.launch.py @@ -0,0 +1,69 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +# To avoid code duplication, we patch and then execute the Franka demo launch file provided by +# the moveit2_tutorials package. + +from os import path + +from ament_index_python.packages import get_package_share_directory + +import yaml + + +def augment_moveit_config(moveit_config): + """Add cuMotion and its config to the planning_pipelines dict of a MoveItConfigs object.""" + config_file_path = path.join( + get_package_share_directory('isaac_ros_cumotion_moveit'), + 'config', + 'isaac_ros_cumotion_planning.yaml' + ) + with open(config_file_path) as config_file: + config = yaml.safe_load(config_file) + moveit_config.planning_pipelines['planning_pipelines'].append('isaac_ros_cumotion') + moveit_config.planning_pipelines['isaac_ros_cumotion'] = config + + +def generate_launch_description(): + franka_demo_launch_file = path.join( + get_package_share_directory('moveit2_tutorials'), + 'launch', + 'demo.launch.py' + ) + lf = open(franka_demo_launch_file).read() + + # Rename generate_launch_description() in base launch file. + lf = lf.replace('generate_launch_description', 'generate_base_launch_description') + + # The standard way to make isaac_ros_cumotion_planning.yaml available to MoveIt would be to + # copy the file into the config/ directory within a given robot's moveit_config package. + # It would then suffice to add "isaac_ros_cumotion" to the list of planning_pipelines in the + # MoveItConfigsBuilder, e.g., via the following substitution. + # + # lf = lf.replace('"ompl"', '"isaac_ros_cumotion", "ompl"') + # + # Here we avoid adding the file to the moveit_resources_panda package by loading the file + # manually and augmenting the MoveItConfigs object after it's built. + lf = lf.replace( + 'run_move_group_node =', + 'augment_moveit_config(moveit_config)\n run_move_group_node =' + ) + + # Execute modified launch file. + exec(lf, globals()) + + return generate_base_launch_description() # noqa: F821 diff --git a/isaac_ros_cumotion_examples/launch/franka_isaac_sim.launch.py b/isaac_ros_cumotion_examples/launch/franka_isaac_sim.launch.py new file mode 100644 index 0000000..57d364d --- /dev/null +++ b/isaac_ros_cumotion_examples/launch/franka_isaac_sim.launch.py @@ -0,0 +1,225 @@ +# SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# 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. +# +# This launch file was originally derived from +# https://github.com/ros-planning/moveit2_tutorials/blob/efef1d3/doc/how_to_guides/isaac_panda/launch/isaac_demo.launch.py # noqa +# +# BSD 3-Clause License +# +# Copyright (c) 2008-2013, Willow Garage, Inc. +# Copyright (c) 2015-2023, PickNik, LLC. +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + +import yaml + + +def generate_launch_description(): + + # Command-line arguments + ros2_control_hardware_type = DeclareLaunchArgument( + 'ros2_control_hardware_type', + default_value='isaac', + description=( + 'ROS2 control hardware interface type to use for the launch file -- ' + 'possible values: [mock_components, isaac]' + ) + ) + + moveit_config = ( + MoveItConfigsBuilder('moveit_resources_panda') + .robot_description( + file_path='config/panda.urdf.xacro', + mappings={ + 'ros2_control_hardware_type': LaunchConfiguration( + 'ros2_control_hardware_type' + ) + }, + ) + .robot_description_semantic(file_path='config/panda.srdf') + .trajectory_execution(file_path='config/gripper_moveit_controllers.yaml') + .planning_pipelines(pipelines=['ompl', 'pilz_industrial_motion_planner']) + .to_moveit_configs() + ) + + # Add cuMotion to list of planning pipelines. + cumotion_config_file_path = os.path.join( + get_package_share_directory('isaac_ros_cumotion_moveit'), + 'config', + 'isaac_ros_cumotion_planning.yaml' + ) + with open(cumotion_config_file_path) as cumotion_config_file: + cumotion_config = yaml.safe_load(cumotion_config_file) + moveit_config.planning_pipelines['planning_pipelines'].append('isaac_ros_cumotion') + moveit_config.planning_pipelines['isaac_ros_cumotion'] = cumotion_config + + # The current Franka asset in Isaac Sim 2023.1.1 tends to drift slightly from commanded joint + # positions, which prevents trajectory execution if the drift exceeds `allowed_start_tolerance` + # for any joint; the default tolerance is 0.01 radians. This is more likely to occur if the + # robot hasn't fully settled when the trajectory is computed or if significant time has + # elapsed between trajectory computation and execution. For this simulation use case, + # there's little harm in disabling this check by setting `allowed_start_tolerance` to 0. + moveit_config.trajectory_execution['trajectory_execution']['allowed_start_tolerance'] = 0.0 + + # Start the actual move_group node/action server + move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[moveit_config.to_dict()], + arguments=['--ros-args', '--log-level', 'info'], + ) + + # RViz + rviz_config_file = os.path.join( + get_package_share_directory('isaac_ros_cumotion_examples'), + 'rviz', + 'franka_moveit_config.rviz', + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # Static TF + world2robot_tf_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=['--frame-id', 'world', '--child-frame-id', 'panda_link0'], + ) + hand2camera_tf_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=[ + '0.04', + '0.0', + '0.04', + '0.0', + '0.0', + '0.0', + 'panda_hand', + 'sim_camera', + ], + ) + + # Publish TF + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory('moveit_resources_panda_moveit_config'), + 'config', + 'ros2_controllers.yaml', + ) + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[ros2_controllers_path], + remappings=[ + ('/controller_manager/robot_description', '/robot_description'), + ], + output='screen', + ) + + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_state_broadcaster', + '--controller-manager', + '/controller_manager', + ], + ) + + panda_arm_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['panda_arm_controller', '-c', '/controller_manager'], + ) + + panda_hand_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['panda_hand_controller', '-c', '/controller_manager'], + ) + + return LaunchDescription( + [ + ros2_control_hardware_type, + rviz_node, + world2robot_tf_node, + hand2camera_tf_node, + robot_state_publisher, + move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + panda_hand_controller_spawner, + ] + ) diff --git a/isaac_ros_cumotion_examples/launch/ur.launch.py b/isaac_ros_cumotion_examples/launch/ur.launch.py new file mode 100644 index 0000000..f7234d5 --- /dev/null +++ b/isaac_ros_cumotion_examples/launch/ur.launch.py @@ -0,0 +1,69 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +# To avoid code duplication, we patch and then execute the launch file provided by the +# ur_moveit_config package. + +from os import path + +from ament_index_python.packages import get_package_share_directory + +import yaml + + +def cumotion_params(): + # The standard way to make isaac_ros_cumotion_planning.yaml available to MoveIt would be to + # copy the file into the config/ directory within a given robot's moveit_config package. + # It would then suffice to add "isaac_ros_cumotion" to the list of planning_pipelines. + # Here we avoid adding the file to the ur_moveit_config package by loading the file manually + # and adding its contents to the parameter list. + config_file_path = path.join( + get_package_share_directory('isaac_ros_cumotion_moveit'), + 'config', + 'isaac_ros_cumotion_planning.yaml' + ) + with open(config_file_path) as config_file: + config = yaml.safe_load(config_file) + + return ( + {'planning_pipelines': ['ompl', 'isaac_ros_cumotion']}, + {'isaac_ros_cumotion': config} + ) + + +def generate_launch_description(): + ur_moveit_launch_file = path.join( + get_package_share_directory('ur_moveit_config'), + 'launch', + 'ur_moveit.launch.py' + ) + lf = open(ur_moveit_launch_file).read() + + # Rename generate_launch_description() in base launch file. + lf = lf.replace('generate_launch_description', 'generate_base_launch_description') + + # Add required parameters to the move_group node. This substitution relies on the fact that + # the string "moveit_controllers," appears only once in the base launch file. + lf = lf.replace( + 'moveit_controllers,', + 'moveit_controllers, *cumotion_params(),' + ) + + # Execute modified launch file. + exec(lf, globals()) + + return generate_base_launch_description() # noqa: F821 diff --git a/isaac_ros_cumotion_examples/package.xml b/isaac_ros_cumotion_examples/package.xml new file mode 100644 index 0000000..c27a6db --- /dev/null +++ b/isaac_ros_cumotion_examples/package.xml @@ -0,0 +1,24 @@ + + + + isaac_ros_cumotion_examples + 3.0.0 + Examples demonstrating Isaac ROS cuMotion with MoveIt + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros-gems/ + + isaac_ros_cumotion_moveit + moveit2_tutorials + ros2launch + ur_moveit_config + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_cumotion_examples/resource/isaac_ros_cumotion_examples b/isaac_ros_cumotion_examples/resource/isaac_ros_cumotion_examples new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_cumotion_examples/rviz/franka_moveit_config.rviz b/isaac_ros_cumotion_examples/rviz/franka_moveit_config.rviz new file mode 100644 index 0000000..2412f4a --- /dev/null +++ b/isaac_ros_cumotion_examples/rviz/franka_moveit_config.rviz @@ -0,0 +1,542 @@ + +# Derived from https://github.com/ros-planning/moveit2_tutorials/blob/efef1d3/doc/how_to_guides/isaac_panda/config/panda_moveit_config.rviz +# +# BSD 3-Clause License +# +# Copyright (c) 2008-2013, Willow Garage, Inc. +# Copyright (c) 2015-2023, PickNik, LLC. +# 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. + +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 363 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.119211196899414 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.02386285550892353 + Y: 0.15478567779064178 + Z: 0.039489321410655975 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5953981876373291 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.958578109741211 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 1920 + Y: 0 diff --git a/isaac_ros_cumotion_examples/setup.py b/isaac_ros_cumotion_examples/setup.py new file mode 100644 index 0000000..3ec9d2e --- /dev/null +++ b/isaac_ros_cumotion_examples/setup.py @@ -0,0 +1,48 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_cumotion_examples' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ( + os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*')), + ), + ( + os.path.join('share', package_name, 'rviz'), + glob(os.path.join('rviz', '*.rviz')), + ), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Examples demonstrating Isaac ROS cuMotion with MoveIt', + license='Apache-2.0', + tests_require=['pytest'], +) diff --git a/isaac_ros_cumotion_examples/test/test_copyright.py b/isaac_ros_cumotion_examples/test/test_copyright.py new file mode 100644 index 0000000..67272f9 --- /dev/null +++ b/isaac_ros_cumotion_examples/test/test_copyright.py @@ -0,0 +1,28 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[ + '.', 'test', + '--exclude', + './launch/franka_isaac_sim.launch.py', + './isaac_sim_scripts/start_isaac_sim_franka.py' + ]) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_cumotion_examples/test/test_flake8.py b/isaac_ros_cumotion_examples/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_cumotion_examples/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_cumotion_examples/test/test_pep257.py b/isaac_ros_cumotion_examples/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_cumotion_examples/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_cumotion_moveit/CMakeLists.txt b/isaac_ros_cumotion_moveit/CMakeLists.txt new file mode 100644 index 0000000..a7ff938 --- /dev/null +++ b/isaac_ros_cumotion_moveit/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.22.1) +project(isaac_ros_cumotion_moveit) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(moveit_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp_components + moveit_core +) + +include_directories(include) + +set(MOVEIT_LIB_NAME isaac_ros_cumotion_moveit) + +# cuMotion planning plugin +add_library(${MOVEIT_LIB_NAME} + SHARED + src/cumotion_planner_manager.cpp + src/cumotion_interface.cpp + src/cumotion_planning_context.cpp + src/cumotion_move_group_client.cpp) + +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +ament_target_dependencies(${MOVEIT_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${MOVEIT_LIB_NAME} + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install config files. +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +pluginlib_export_plugin_description_file(moveit_core cumotion_planner_plugin_description.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + + +ament_package() diff --git a/isaac_ros_cumotion_moveit/config/isaac_ros_cumotion_planning.yaml b/isaac_ros_cumotion_moveit/config/isaac_ros_cumotion_planning.yaml new file mode 100644 index 0000000..93b8213 --- /dev/null +++ b/isaac_ros_cumotion_moveit/config/isaac_ros_cumotion_planning.yaml @@ -0,0 +1,8 @@ +planning_plugin: isaac_ros_cumotion_moveit/CumotionPlanner +request_adapters: >- + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints +start_state_max_bounds_error: 0.1 +num_steps: 32 diff --git a/isaac_ros_cumotion_moveit/cumotion_planner_plugin_description.xml b/isaac_ros_cumotion_moveit/cumotion_planner_plugin_description.xml new file mode 100644 index 0000000..156237d --- /dev/null +++ b/isaac_ros_cumotion_moveit/cumotion_planner_plugin_description.xml @@ -0,0 +1,7 @@ + + + + The cuMotion planner generates collision-free trajectories leveraging CUDA compute. + + + diff --git a/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_interface.hpp b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_interface.hpp new file mode 100644 index 0000000..58b2979 --- /dev/null +++ b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_interface.hpp @@ -0,0 +1,60 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_CUMOTION_INTERFACE_H +#define ISAAC_ROS_CUMOTION_INTERFACE_H + +#include + +#include "moveit/planning_interface/planning_interface.h" +#include "rclcpp/rclcpp.hpp" + +#include "isaac_ros_cumotion_moveit/cumotion_move_group_client.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +class CumotionInterface +{ +public: + CumotionInterface(const rclcpp::Node::SharedPtr & node) + : node_(node), + action_client_(std::make_shared(node)) + { + } + + bool solve( + const planning_scene::PlanningSceneConstPtr & planning_scene, + const planning_interface::MotionPlanRequest & request, + planning_interface::MotionPlanDetailedResponse & response); + + bool planner_busy = false; + +private: + std::shared_ptr node_; + std::shared_ptr action_client_; +}; + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia + +#endif // ISAAC_ROS_CUMOTION_INTERFACE_H diff --git a/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_move_group_client.hpp b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_move_group_client.hpp new file mode 100644 index 0000000..edc09b2 --- /dev/null +++ b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_move_group_client.hpp @@ -0,0 +1,81 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_CUMOTION_MOVE_GROUP_CLIENT_H +#define ISAAC_ROS_CUMOTION_MOVE_GROUP_CLIENT_H + +#include +#include + +#include "moveit/planning_interface/planning_interface.h" +#include "moveit/planning_scene/planning_scene.h" +#include "moveit_msgs/action/move_group.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +class CumotionMoveGroupClient +{ + using GoalHandle = rclcpp_action::ClientGoalHandle; + +public: + CumotionMoveGroupClient(const rclcpp::Node::SharedPtr & node); + + bool sendGoal(); + + void updateGoal( + const planning_scene::PlanningSceneConstPtr & planning_scene, + const planning_interface::MotionPlanRequest & req); + + void getGoal(); + + bool result_ready; + bool success; + moveit_msgs::msg::MotionPlanDetailedResponse plan_response; + +private: + void goalResponseCallback(const GoalHandle::SharedPtr & future); + + void feedbackCallback( + GoalHandle::SharedPtr, + const std::shared_ptr feedback); + + void resultCallback(const GoalHandle::WrappedResult & result); + + bool get_goal_handle_; + bool get_result_handle_; + std::shared_ptr node_; + rclcpp::CallbackGroup::SharedPtr client_cb_group_; + rclcpp_action::Client::SharedPtr client_; + rclcpp_action::Client::SendGoalOptions send_goal_options_; + std::shared_future goal_h_; + std::shared_future result_future_; + moveit_msgs::msg::PlanningScene planning_scene_; + planning_interface::MotionPlanRequest planning_request_; +}; + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia + +#endif // ISAAC_ROS_CUMOTION_MOVE_GROUP_CLIENT_H diff --git a/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_planner_manager.hpp b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_planner_manager.hpp new file mode 100644 index 0000000..2400456 --- /dev/null +++ b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_planner_manager.hpp @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_CUMOTION_PLANNER_MANAGER_H +#define ISAAC_ROS_CUMOTION_PLANNER_MANAGER_H + +#include +#include +#include + +#include "moveit/planning_interface/planning_interface.h" +#include "moveit/planning_scene/planning_scene.h" + +#include "isaac_ros_cumotion_moveit/cumotion_planning_context.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +class CumotionPlannerManager : public planning_interface::PlannerManager +{ + inline static constexpr char kCumotionPlannerId[] = "cuMotion"; + +public: + CumotionPlannerManager() + { + } + + bool initialize( + const moveit::core::RobotModelConstPtr & model, + const rclcpp::Node::SharedPtr & node, + const std::string & parameter_namespace) override; + + bool canServiceRequest(const planning_interface::MotionPlanRequest & req) const override + { + return req.planner_id == kCumotionPlannerId; + } + + std::string getDescription() const override; + + void getPlanningAlgorithms(std::vector & algs) const override; + + planning_interface::PlanningContextPtr getPlanningContext( + const planning_scene::PlanningSceneConstPtr & planning_scene, + const planning_interface::MotionPlanRequest & req, + moveit_msgs::msg::MoveItErrorCodes & error_code) const override; + + void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap & pcs) override; + +private: + std::shared_ptr node_; + std::map> planning_contexts_; + planning_interface::PlannerConfigurationMap planner_configs_; +}; + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia + +#endif // ISAAC_ROS_CUMOTION_PLANNER_MANAGER_H diff --git a/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_planning_context.hpp b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_planning_context.hpp new file mode 100644 index 0000000..741f19a --- /dev/null +++ b/isaac_ros_cumotion_moveit/include/isaac_ros_cumotion_moveit/cumotion_planning_context.hpp @@ -0,0 +1,72 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_CUMOTION_PLANNING_CONTEXT_H +#define ISAAC_ROS_CUMOTION_PLANNING_CONTEXT_H + +#include +#include + +#include "moveit/planning_interface/planning_interface.h" + +#include "isaac_ros_cumotion_moveit/cumotion_interface.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +class CumotionPlanningContext : public planning_interface::PlanningContext +{ +public: + CumotionPlanningContext( + const std::string & context_name, + const std::string & group_name, + const rclcpp::Node::SharedPtr & node) + : planning_interface::PlanningContext(context_name, group_name), + cumotion_interface_(std::make_shared(node)) + { + } + + ~CumotionPlanningContext() override + { + } + + bool solve(planning_interface::MotionPlanResponse & res) override; + + bool solve(planning_interface::MotionPlanDetailedResponse & res) override; + + bool terminate() override + { + return true; + } + + void clear() override + { + } + +private: + std::shared_ptr cumotion_interface_; +}; + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia + +#endif // ISAAC_ROS_CUMOTION_PLANNING_CONTEXT_H diff --git a/isaac_ros_cumotion_moveit/package.xml b/isaac_ros_cumotion_moveit/package.xml new file mode 100644 index 0000000..12c5453 --- /dev/null +++ b/isaac_ros_cumotion_moveit/package.xml @@ -0,0 +1,30 @@ + + + + isaac_ros_cumotion_moveit + 3.0.0 + Package adds a cuMotion planner plugin to MoveIt + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros-gems/ + Balakumar Sundaralingam + + ament_cmake + + isaac_ros_cumotion + moveit_common + moveit_core + moveit_ros_move_group + pluginlib + rclcpp + rclcpp_components + + ros2launch + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/isaac_ros_cumotion_moveit/src/cumotion_interface.cpp b/isaac_ros_cumotion_moveit/src/cumotion_interface.cpp new file mode 100644 index 0000000..ec420cc --- /dev/null +++ b/isaac_ros_cumotion_moveit/src/cumotion_interface.cpp @@ -0,0 +1,101 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "isaac_ros_cumotion_moveit/cumotion_interface.hpp" + +#include +#include + +#include "moveit/planning_interface/planning_interface.h" +#include "moveit/planning_scene/planning_scene.h" +#include "moveit/robot_state/conversions.h" +#include "rclcpp/rclcpp.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +namespace +{ + +constexpr unsigned kSleepIntervalInMs = 5; +constexpr unsigned kTimeoutIntervalInSeconds = 5; + +} // namespace + +bool CumotionInterface::solve( + const planning_scene::PlanningSceneConstPtr & planning_scene, + const planning_interface::MotionPlanRequest & request, + planning_interface::MotionPlanDetailedResponse & response) +{ + RCLCPP_INFO(node_->get_logger(), "Planning trajectory"); + + if (!planner_busy) { + action_client_->updateGoal(planning_scene, request); + action_client_->sendGoal(); + planner_busy = true; + } + + rclcpp::Time start_time = node_->now(); + while ( + !action_client_->result_ready && + node_->now().seconds() - start_time.seconds() < kTimeoutIntervalInSeconds) + { + action_client_->getGoal(); + std::this_thread::sleep_for(std::chrono::milliseconds(kSleepIntervalInMs)); + } + + if (!action_client_->result_ready) { + RCLCPP_ERROR(node_->get_logger(), "Timed out!"); + planner_busy = false; + return false; + } + RCLCPP_INFO(node_->get_logger(), "Received trajectory result"); + + if (!action_client_->success) { + RCLCPP_ERROR(node_->get_logger(), "No trajectory"); + response.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + planner_busy = false; + return false; + } + RCLCPP_INFO(node_->get_logger(), "Trajectory success!"); + + response.error_code_ = action_client_->plan_response.error_code; + response.description_ = action_client_->plan_response.description; + auto result_traj = std::make_shared( + planning_scene->getRobotModel(), request.group_name); + moveit::core::RobotState robot_state(planning_scene->getRobotModel()); + moveit::core::robotStateMsgToRobotState( + action_client_->plan_response.trajectory_start, + robot_state); + result_traj->setRobotTrajectoryMsg( + robot_state, + action_client_->plan_response.trajectory[0]); + response.trajectory_.clear(); + response.trajectory_.push_back(result_traj); + response.processing_time_ = action_client_->plan_response.processing_time; + + planner_busy = false; + return true; +} + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_cumotion_moveit/src/cumotion_move_group_client.cpp b/isaac_ros_cumotion_moveit/src/cumotion_move_group_client.cpp new file mode 100644 index 0000000..5f581e4 --- /dev/null +++ b/isaac_ros_cumotion_moveit/src/cumotion_move_group_client.cpp @@ -0,0 +1,209 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "isaac_ros_cumotion_moveit/cumotion_move_group_client.hpp" + +#include +#include +#include +#include + +#include "moveit_msgs/action/move_group.hpp" +#include "moveit_msgs/msg/planning_options.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +namespace +{ + +constexpr unsigned kGetGoalWaitIntervalInMs = 10; + +} // namespace + +CumotionMoveGroupClient::CumotionMoveGroupClient(const rclcpp::Node::SharedPtr & node) +: result_ready(false), + success(false), + get_goal_handle_(false), + get_result_handle_(false), + node_(node), + client_cb_group_(node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)) +{ + client_ = rclcpp_action::create_client( + node_, + "cumotion/move_group", + client_cb_group_); + + send_goal_options_ = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options_.goal_response_callback = std::bind( + &CumotionMoveGroupClient::goalResponseCallback, this, std::placeholders::_1); + send_goal_options_.feedback_callback = std::bind( + &CumotionMoveGroupClient::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); + send_goal_options_.result_callback = std::bind( + &CumotionMoveGroupClient::resultCallback, this, std::placeholders::_1); +} + +void CumotionMoveGroupClient::updateGoal( + const planning_scene::PlanningSceneConstPtr & planning_scene, + const planning_interface::MotionPlanRequest & req) +{ + planning_request_ = req; + planning_scene->getPlanningSceneMsg(planning_scene_); +} + +bool CumotionMoveGroupClient::sendGoal() +{ + result_ready = false; + success = false; + + moveit_msgs::msg::PlanningOptions plan_options; + plan_options.planning_scene_diff = planning_scene_; + + if (!client_->wait_for_action_server()) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = moveit_msgs::action::MoveGroup::Goal(); + + goal_msg.planning_options = plan_options; + goal_msg.request = planning_request_; + RCLCPP_INFO(node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_->async_send_goal(goal_msg, send_goal_options_); + goal_h_ = goal_handle_future; + get_result_handle_ = true; + get_goal_handle_ = true; + return true; +} + +void CumotionMoveGroupClient::getGoal() +{ + using namespace std::chrono_literals; + + if (get_goal_handle_) { + if (goal_h_.wait_for(std::chrono::milliseconds(kGetGoalWaitIntervalInMs)) != + std::future_status::ready) + { + return; + } + + GoalHandle::SharedPtr goal_handle = goal_h_.get(); + + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return; + } + auto result_future = client_->async_get_result(goal_handle); + result_future_ = result_future; + get_goal_handle_ = false; + } + + if (get_result_handle_) { + if (result_future_.wait_for(std::chrono::milliseconds(kGetGoalWaitIntervalInMs)) != + std::future_status::ready) + { + return; + } + + auto res = result_future_.get(); + + RCLCPP_INFO(node_->get_logger(), "Checking results"); + + if (res.code == rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_INFO(node_->get_logger(), "Success"); + result_ready = true; + success = false; + plan_response.error_code = res.result->error_code; + if (plan_response.error_code.val == 1) { + success = true; + plan_response.trajectory_start = res.result->trajectory_start; + plan_response.group_name = planning_request_.group_name; + plan_response.trajectory.resize(1); + plan_response.trajectory[0] = res.result->planned_trajectory; + plan_response.processing_time = {res.result->planning_time}; + } + } else { + RCLCPP_INFO(node_->get_logger(), "Failed"); + } + get_result_handle_ = false; + } + +} + +void CumotionMoveGroupClient::goalResponseCallback(const GoalHandle::SharedPtr & future) +{ + auto goal_handle = future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + result_ready = true; + success = false; + } else { + RCLCPP_INFO(node_->get_logger(), "Goal accepted by server, waiting for result"); + } +} + +void CumotionMoveGroupClient::feedbackCallback( + GoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + std::string status = feedback->state; + RCLCPP_INFO(node_->get_logger(), "Checking status"); + RCLCPP_INFO(node_->get_logger(), status.c_str()); +} + +void CumotionMoveGroupClient::resultCallback(const GoalHandle::WrappedResult & result) +{ + RCLCPP_INFO(node_->get_logger(), "Received result"); + + result_ready = true; + success = false; + + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(node_->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(node_->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return; + } + + plan_response.error_code = result.result->error_code; + if (plan_response.error_code.val == 1) { + success = true; + plan_response.trajectory_start = result.result->trajectory_start; + plan_response.group_name = planning_request_.group_name; + plan_response.trajectory = {result.result->planned_trajectory}; + plan_response.processing_time = {result.result->planning_time}; + } +} + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_cumotion_moveit/src/cumotion_planner_manager.cpp b/isaac_ros_cumotion_moveit/src/cumotion_planner_manager.cpp new file mode 100644 index 0000000..a8ffe48 --- /dev/null +++ b/isaac_ros_cumotion_moveit/src/cumotion_planner_manager.cpp @@ -0,0 +1,102 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "isaac_ros_cumotion_moveit/cumotion_planner_manager.hpp" + +#include "moveit/planning_interface/planning_interface.h" +#include "moveit/planning_scene/planning_scene.h" +#include "pluginlib/class_list_macros.hpp" + +#include "isaac_ros_cumotion_moveit/cumotion_planning_context.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +bool CumotionPlannerManager::initialize( + const moveit::core::RobotModelConstPtr & model, + const rclcpp::Node::SharedPtr & node, + const std::string & parameter_namespace) +{ + node_ = node; + for (const std::string & group_name : model->getJointModelGroupNames()) { + planning_contexts_[group_name] = + std::make_shared("cumotion_planning_context", group_name, node); + } + static_cast(model); // Suppress "unused" warning. + static_cast(parameter_namespace); // Suppress "unused" warning. + return true; +} + +std::string CumotionPlannerManager::getDescription() const +{ + return "Generate minimum-jerk trajectories using NVIDIA Isaac ROS cuMotion"; +} + +void CumotionPlannerManager::getPlanningAlgorithms(std::vector & algs) const +{ + algs.clear(); + algs.push_back(kCumotionPlannerId); +} + +planning_interface::PlanningContextPtr CumotionPlannerManager::getPlanningContext( + const planning_scene::PlanningSceneConstPtr & planning_scene, + const planning_interface::MotionPlanRequest & req, + moveit_msgs::msg::MoveItErrorCodes & error_code) const +{ + error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + + if (!planning_scene) { + RCLCPP_ERROR(node_->get_logger(), "No planning scene supplied as input"); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return planning_interface::PlanningContextPtr(); + } + + if (req.group_name.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No group specified to plan for"); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; + return planning_interface::PlanningContextPtr(); + } + + // Retrieve and configure existing context. + const std::shared_ptr & context = planning_contexts_.at(req.group_name); + + context->setPlanningScene(planning_scene); + context->setMotionPlanRequest(req); + + error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + + return context; +} + +void CumotionPlannerManager::setPlannerConfigurations( + const planning_interface::PlannerConfigurationMap & pcs) +{ + planner_configs_ = pcs; +} + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia + +// Register the `CumotionPlannerManager` class as a plugin. +PLUGINLIB_EXPORT_CLASS( + nvidia::isaac::manipulation::CumotionPlannerManager, + planning_interface::PlannerManager) diff --git a/isaac_ros_cumotion_moveit/src/cumotion_planning_context.cpp b/isaac_ros_cumotion_moveit/src/cumotion_planning_context.cpp new file mode 100644 index 0000000..4f291ed --- /dev/null +++ b/isaac_ros_cumotion_moveit/src/cumotion_planning_context.cpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "isaac_ros_cumotion_moveit/cumotion_planning_context.hpp" + +namespace nvidia +{ +namespace isaac +{ +namespace manipulation +{ + +bool CumotionPlanningContext::solve(planning_interface::MotionPlanDetailedResponse & res) +{ + return cumotion_interface_->solve(planning_scene_, request_, res); +} + +bool CumotionPlanningContext::solve(planning_interface::MotionPlanResponse & res) +{ + planning_interface::MotionPlanDetailedResponse res_detailed; + bool planning_success = solve(res_detailed); + + res.error_code_ = res_detailed.error_code_; + + if (planning_success) { + res.trajectory_ = res_detailed.trajectory_[0]; + res.planning_time_ = res_detailed.processing_time_[0]; + } + + return planning_success; +} + +} // namespace manipulation +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_cumotion_robot_description/package.xml b/isaac_ros_cumotion_robot_description/package.xml new file mode 100644 index 0000000..f33a6c3 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/package.xml @@ -0,0 +1,19 @@ + + + + isaac_ros_cumotion_robot_description + 3.0.0 + Package containing XRDF (extended robot description format) files for various robots + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros-gems/ + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_cumotion_robot_description/resource/isaac_ros_cumotion_robot_description b/isaac_ros_cumotion_robot_description/resource/isaac_ros_cumotion_robot_description new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_cumotion_robot_description/setup.py b/isaac_ros_cumotion_robot_description/setup.py new file mode 100644 index 0000000..500ed18 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/setup.py @@ -0,0 +1,45 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_cumotion_robot_description' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ( + os.path.join('share', package_name, 'xrdf'), + glob(os.path.join('xrdf', '*.xrdf')), + ), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Package containing XRDF (extended robot description format) files for ' + 'various robots', + license='Apache-2.0', + tests_require=['pytest'], +) diff --git a/isaac_ros_cumotion_robot_description/test/test_copyright.py b/isaac_ros_cumotion_robot_description/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_cumotion_robot_description/test/test_flake8.py b/isaac_ros_cumotion_robot_description/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_cumotion_robot_description/test/test_pep257.py b/isaac_ros_cumotion_robot_description/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_cumotion_robot_description/xrdf/franka.xrdf b/isaac_ros_cumotion_robot_description/xrdf/franka.xrdf new file mode 100644 index 0000000..af4b39e --- /dev/null +++ b/isaac_ros_cumotion_robot_description/xrdf/franka.xrdf @@ -0,0 +1,252 @@ +format: xrdf +format_version: 1.0 + +modifiers: + - add_frame: + frame_name: "attached_object" + parent_frame_name: "panda_hand" + joint_name: "attach_joint" + joint_type: FIXED + fixed_transform: + position: [0.0, 0.0, 0.0] + orientation: {w: 1.0, xyz: [0.0, 0.0, 0.0]} + +default_joint_positions: + panda_joint1: 0.0 + panda_joint2: -1.3 + panda_joint3: 0.0 + panda_joint4: -2.5 + panda_joint5: 0.0 + panda_joint6: 1.0 + panda_joint7: 0.0 + panda_finger_joint1: 0.04 + panda_finger_joint2: 0.04 + +cspace: + joint_names: + - "panda_joint1" + - "panda_joint2" + - "panda_joint3" + - "panda_joint4" + - "panda_joint5" + - "panda_joint6" + - "panda_joint7" + acceleration_limits: [15.0, 15.0, 15.0, 15.0, 15.0, 15.0, 15.0] + jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0, 500.0] + +tool_frames: ["panda_hand"] + +collision: + geometry: "franka_collision_spheres" + buffer_distance: + panda_link0: 0.004 + panda_link1: 0.004 + panda_link2: 0.004 + panda_link3: 0.004 + panda_link4: 0.004 + panda_link5: 0.004 + panda_link6: 0.004 + panda_link7: 0.004 + panda_hand: 0.004 + panda_leftfinger: 0.004 + panda_rightfinger: 0.004 + attached_object: 0.004 + +self_collision: + geometry: "franka_collision_spheres" + buffer_distance: + panda_link0: 0.1 + panda_link1: 0.05 + panda_hand: 0.02 + panda_leftfinger: 0.01 + panda_rightfinger: 0.01 + 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_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" + +geometry: + franka_collision_spheres: + spheres: + panda_link0: + - center: [0.0, 0.0, 0.085] + radius: 0.03 + - center: [-0.1, 0.0, 0.085] + radius: 0.03 + panda_link1: + - center: [0.0, -0.08, 0.0] + radius: 0.055 + - center: [0.0, -0.03, 0.0] + radius: 0.06 + - center: [0.0, 0.0, -0.12] + radius: 0.06 + - center: [0.0, 0.0, -0.17] + radius: 0.06 + panda_link2: + - center: [0.0, 0.0, 0.03] + radius: 0.055 + - center: [0.0, 0.0, 0.08] + radius: 0.055 + - center: [0.0, -0.12, 0.0] + radius: 0.055 + - center: [0.0, -0.17, 0.0] + radius: 0.055 + panda_link3: + - center: [0.0, 0.0, -0.06] + radius: 0.05 + - center: [0.0, 0.0, -0.1] + radius: 0.06 + - center: [0.08, 0.06, 0.0] + radius: 0.052 + - center: [0.08, 0.02, 0.0] + radius: 0.052 + panda_link4: + - center: [0.0, 0.0, 0.02] + radius: 0.052 + - center: [0.0, 0.0, 0.06] + radius: 0.052 + - center: [-0.08, 0.095, 0.0] + radius: 0.055 + - center: [-0.08, 0.06, 0.0] + radius: 0.052 + panda_link5: + - center: [0.0, 0.03, 0.0] + radius: 0.05 + - center: [0.0, 0.082, 0.0] + radius: 0.05 + - center: [0.0, 0.000, -0.22] + radius: 0.05 + - center: [0.0, 0.052, -0.18] + radius: 0.04 + - center: [0.01, 0.08, -0.14] + radius: 0.022 + - center: [0.01, 0.085, -0.11] + radius: 0.022 + - center: [0.01, 0.09, -0.08] + radius: 0.022 + - center: [0.01, 0.095, -0.05] + radius: 0.022 + - center: [-0.01, 0.08, -0.14] + radius: 0.022 + - center: [-0.01, 0.085, -0.11] + radius: 0.022 + - center: [-0.01, 0.09, -0.08] + radius: 0.022 + - center: [-0.01, 0.095, -0.05] + radius: 0.022 + - center: [0.0, -0.009, 0.0] + radius: 0.05 + panda_link6: + - center: [0.085, 0.035, 0.0] + radius: 0.045 + - center: [0.085, 0.0, 0.0] + radius: 0.045 + - center: [0.085, -0.015, 0.0] + radius: 0.045 + panda_link7: + - center: [0.0, 0.0, 0.07] + radius: 0.045 + - center: [0.02, 0.04, 0.08] + radius: 0.024 + - center: [0.04, 0.02, 0.08] + radius: 0.024 + - center: [0.04, 0.06, 0.085] + radius: 0.02 + - center: [0.06, 0.04, 0.085] + radius: 0.02 + panda_hand: + - center: [0.0, -0.075, 0.01] + radius: 0.023 + - center: [0.0, -0.045, 0.01] + radius: 0.023 + - center: [0.0, -0.015, 0.01] + radius: 0.023 + - center: [0.0, 0.015, 0.01] + radius: 0.023 + - center: [0.0, 0.045, 0.01] + radius: 0.023 + - center: [0.0, 0.075, 0.01] + radius: 0.023 + - center: [0.0, -0.08, 0.03] + radius: 0.022 + - center: [0.0, -0.045, 0.03] + radius: 0.022 + - center: [0.0, -0.015, 0.03] + radius: 0.022 + - center: [0.0, 0.015, 0.03] + radius: 0.022 + - center: [0.0, 0.045, 0.03] + radius: 0.022 + - center: [0.0, 0.08, 0.03] + radius: 0.022 + - center: [0.0, -0.08, 0.045] + radius: 0.022 + - center: [0.0, -0.045, 0.045] + radius: 0.022 + - center: [0.0, -0.015, 0.045] + radius: 0.022 + - center: [0.0, 0.015, 0.045] + radius: 0.022 + - center: [0.0, 0.045, 0.045] + radius: 0.022 + - center: [0.0, 0.08, 0.045] + radius: 0.022 + panda_leftfinger: + - center: [0.0, 0.01, 0.043] + radius: 0.011 + - center: [0.0, 0.02, 0.015] + radius: 0.011 + panda_rightfinger: + - center: [0.0, -0.01, 0.043] + radius: 0.011 + - center: [0.0, -0.01, 0.015] + radius: 0.011 + attached_object: + - center: [0.0, 0.0, 0.0] + radius: -10.0 + - center: [0.0, 0.0, 0.0] + radius: -10.0 + - center: [0.0, 0.0, 0.0] + radius: -10.0 + - center: [0.0, 0.0, 0.0] + radius: -10.0 diff --git a/isaac_ros_cumotion_robot_description/xrdf/ur10e.xrdf b/isaac_ros_cumotion_robot_description/xrdf/ur10e.xrdf new file mode 100644 index 0000000..6180ba5 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/xrdf/ur10e.xrdf @@ -0,0 +1,100 @@ +format: xrdf +format_version: 1.0 + +modifiers: + - set_base_frame: "base_link" + +default_joint_positions: + shoulder_pan_joint: 0.0 + shoulder_lift_joint: -2.2 + elbow_joint: 1.9 + wrist_1_joint: -1.383 + wrist_2_joint: -1.57 + wrist_3_joint: 0.0 + +cspace: + joint_names: + - "shoulder_pan_joint" + - "shoulder_lift_joint" + - "elbow_joint" + - "wrist_1_joint" + - "wrist_2_joint" + - "wrist_3_joint" + acceleration_limits: [12.0, 12.0, 12.0, 12.0, 12.0, 12.0] + jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0] + +tool_frames: ["tool0"] + +collision: + geometry: "ur10e_collision_spheres" + buffer_distance: + shoulder_link: 0.01 + upper_arm_link: 0.01 + forearm_link: 0.01 + wrist_1_link: 0.01 + wrist_2_link: 0.01 + wrist_3_link: 0.01 + tool0: 0.01 + +self_collision: + geometry: "ur10e_collision_spheres" + buffer_distance: + shoulder_link: 0.07 + tool0: 0.05 + ignore: + upper_arm_link: ["forearm_link", "shoulder_link"] + forearm_link: ["wrist_1_link"] + wrist_1_link: ["wrist_2_link","wrist_3_link"] + wrist_2_link: ["wrist_3_link", "tool0"] + wrist_3_link: ["tool0"] + +geometry: + ur10e_collision_spheres: + spheres: + shoulder_link: + - center: [0, 0, 0] + radius: 0.05 + upper_arm_link: + - center: [-0, -0, 0.18] + radius: 0.09 + - center: [-0.102167, 0, 0.18] + radius: 0.05 + - center: [-0.204333, 0, 0.18] + radius: 0.05 + - center: [-0.3065, 0, 0.18] + radius: 0.05 + - center: [-0.408667, 0, 0.18] + radius: 0.05 + - center: [-0.510833, 0, 0.18] + radius: 0.05 + - center: [-0.613, 0,0.18] + radius: 0.07 + forearm_link: + - center: [-0, 0, 0.03] + radius: 0.05 + - center: [-0.0951667, 0, 0.03] + radius: 0.05 + - center: [-0.190333, 0, 0.03] + radius: 0.05 + - center: [-0.2855, 0, 0.03] + radius: 0.05 + - center: [-0.380667, 0,0.03] + radius: 0.05 + - center: [-0.475833, 0,0.03] + radius: 0.05 + - center: [-0.571, -1.19904e-17, 0.03] + radius: 0.05 + wrist_1_link: + - center: [0, 0, 0] + radius: 0.05 + wrist_2_link: + - center: [0, 0, 0] + radius: 0.05 + wrist_3_link: + - center: [0, 0, 0] + radius: 0.05 + - center: [0, 0, 0.06] + radius: 0.07 + tool0: + - center: [0, 0, 0.12] + radius: -0.01 diff --git a/isaac_ros_cumotion_robot_description/xrdf/ur5e.xrdf b/isaac_ros_cumotion_robot_description/xrdf/ur5e.xrdf new file mode 100644 index 0000000..e8d4383 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/xrdf/ur5e.xrdf @@ -0,0 +1,115 @@ +format: xrdf +format_version: 1.0 + +modifiers: + - set_base_frame: "base_link" + +default_joint_positions: + shoulder_pan_joint: 0.0 + shoulder_lift_joint: -2.2 + elbow_joint: 1.9 + wrist_1_joint: -1.383 + wrist_2_joint: -1.57 + wrist_3_joint: 0.0 + +cspace: + joint_names: + - "shoulder_pan_joint" + - "shoulder_lift_joint" + - "elbow_joint" + - "wrist_1_joint" + - "wrist_2_joint" + - "wrist_3_joint" + acceleration_limits: [12.0, 12.0, 12.0, 12.0, 12.0, 12.0] + jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0] + +tool_frames: ["tool0"] + +collision: + geometry: "ur5e_collision_spheres" + buffer_distance: + shoulder_link: 0.01 + upper_arm_link: 0.01 + forearm_link: 0.01 + wrist_1_link: 0.01 + wrist_2_link: 0.01 + wrist_3_link: 0.01 + tool0: 0.025 + +self_collision: + geometry: "ur5e_collision_spheres" + buffer_distance: + tool0: 0.025 + ignore: + upper_arm_link: ["forearm_link", "shoulder_link"] + forearm_link: ["wrist_1_link", "wrist_2_link", "wrist_3_link"] + wrist_1_link: ["wrist_2_link", "wrist_3_link", "tool0"] + wrist_2_link: ["wrist_3_link", "tool0"] + wrist_3_link: ["tool0"] + +geometry: + ur5e_collision_spheres: + spheres: + shoulder_link: + - center: [0.0, 0.0, 0.0] + radius: 0.1 + upper_arm_link: + - center: [-0.416, -0.0, 0.143] + radius: 0.078 + - center: [-0.015, 0.0, 0.134] + radius: 0.077 + - center: [-0.14, 0.0, 0.138] + radius: 0.062 + - center: [-0.285, -0.001, 0.139] + radius: 0.061 + - center: [-0.376, 0.001, 0.138] + radius: 0.077 + - center: [-0.222, 0.001, 0.139] + radius: 0.061 + - center: [-0.055, 0.008, 0.14] + radius: 0.07 + - center: [-0.001, -0.002, 0.143] + radius: 0.08 + forearm_link: + - center: [-0.01, 0.002, 0.031] + radius: 0.072 + - center: [-0.387, 0.0, 0.014] + radius: 0.057 + - center: [-0.121, -0.0, 0.006] + radius: 0.057 + - center: [-0.206, 0.001, 0.007] + radius: 0.057 + - center: [-0.312, -0.001, 0.006] + radius: 0.056 + - center: [-0.057, 0.003, 0.008] + radius: 0.065 + - center: [-0.266, 0.0, 0.006] + radius: 0.057 + - center: [-0.397, -0.001, -0.018] + radius: 0.052 + - center: [-0.164, -0.0, 0.007] + radius: 0.057 + wrist_1_link: + - center: [-0.0, 0.0, -0.009] + radius: 0.047 + - center: [-0.0, 0.0, -0.052] + radius: 0.047 + - center: [-0.002, 0.027, -0.001] + radius: 0.045 + - center: [0.001, -0.01, 0.0] + radius: 0.046 + wrist_2_link: + - center: [0.0, -0.01, -0.001] + radius: 0.047 + - center: [0.0, 0.008, -0.001] + radius: 0.047 + - center: [0.001, -0.001, -0.036] + radius: 0.047 + - center: [0.001, -0.03, -0.0] + radius: 0.047 + wrist_3_link: + - center: [0.001, 0.001, -0.029] + radius: 0.043 + tool0: + - center: [0.001, 0.001, 0.05] + radius: -0.01 diff --git a/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/__init__.py b/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/__init__.py new file mode 100644 index 0000000..34d9bfe --- /dev/null +++ b/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/__init__.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/esdf_visualizer.py b/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/esdf_visualizer.py new file mode 100644 index 0000000..9489852 --- /dev/null +++ b/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/esdf_visualizer.py @@ -0,0 +1,301 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.sdf.world import WorldCollisionConfig +from curobo.geom.sdf.world_voxel import WorldVoxelCollision +from curobo.geom.types import Cuboid as CuCuboid +from curobo.geom.types import VoxelGrid as CuVoxelGrid +from curobo.geom.types import WorldConfig +from curobo.types.base import TensorDeviceType +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +import numpy as np +from nvblox_msgs.srv import EsdfAndGradients +import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.node import Node +from std_msgs.msg import ColorRGBA +import torch +from visualization_msgs.msg import Marker + + +class ESDFVisualizer(Node): + + def __init__(self): + super().__init__('esdf_visualizer') + + self.declare_parameter('voxel_dims', [1.25, 1.8, 1.8]) + self.declare_parameter('grid_position', [0.0, 0.0, 0.0]) + + self.declare_parameter('voxel_size', 0.05) + self.declare_parameter('publish_voxel_size', 0.025) + self.declare_parameter('max_publish_voxels', 50000) + + self.declare_parameter('esdf_service_name', '/nvblox_node/get_esdf_and_gradient') + self.declare_parameter('robot_base_frame', 'base_link') + self.__esdf_future = None + # Voxel publisher + self.__voxel_pub = self.create_publisher(Marker, '/curobo/voxels', 10) + # ESDF service + esdf_service_name = ( + self.get_parameter('esdf_service_name').get_parameter_value().string_value + ) + esdf_service_cb_group = MutuallyExclusiveCallbackGroup() + self.__esdf_client = self.create_client( + EsdfAndGradients, esdf_service_name, callback_group=esdf_service_cb_group + ) + while not self.__esdf_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info(f'Service({esdf_service_name}) not available, waiting again...') + self.__esdf_req = EsdfAndGradients.Request() + + # Timer for calling the ESDF service + timer_period = 0.01 + timer_cb_group = MutuallyExclusiveCallbackGroup() + self.timer = self.create_timer( + timer_period, self.timer_callback, callback_group=timer_cb_group + ) + + self.__voxel_dims = ( + self.get_parameter('voxel_dims').get_parameter_value().double_array_value + ) + + self.__grid_position = ( + self.get_parameter('grid_position').get_parameter_value().double_array_value + ) + self.__voxel_size = self.get_parameter('voxel_size').get_parameter_value().double_value + self.__publish_voxel_size = ( + self.get_parameter('publish_voxel_size').get_parameter_value().double_value + ) + self.__max_publish_voxels = ( + self.get_parameter('max_publish_voxels').get_parameter_value().integer_value + ) + # Init WorldVoxelCollision + world_cfg = WorldConfig.from_dict( + { + 'voxel': { + 'world_voxel': { + 'dims': self.__voxel_dims, + 'pose': [ + self.__grid_position[0], + self.__grid_position[1], + self.__grid_position[2], + 1, + 0, + 0, + 0, + ], # x, y, z, qw, qx, qy, qz + 'voxel_size': self.__voxel_size, + 'feature_dtype': torch.float32, + }, + }, + }, + ) + tensor_args = TensorDeviceType() + world_collision_config = WorldCollisionConfig.load_from_dict( + { + 'checker_type': CollisionCheckerType.VOXEL, + 'max_distance': 10.0, + 'n_envs': 1, + }, + world_cfg, + tensor_args, + ) + self.__world_collision = WorldVoxelCollision(world_collision_config) + + self.__robot_base_frame = ( + self.get_parameter('robot_base_frame').get_parameter_value().string_value + ) + self.__tensor_args = tensor_args + + esdf_grid = CuVoxelGrid( + name='world_voxel', + dims=self.__voxel_dims, + pose=[ + self.__grid_position[0], + self.__grid_position[1], + self.__grid_position[2], + 1, + 0, + 0, + 0, + ], + voxel_size=self.__voxel_size, + feature_dtype=torch.float32, + ) + self.__grid_shape, _, _ = esdf_grid.get_grid_shape() + + def timer_callback(self): + if self.__esdf_future is None: + self.get_logger().info('Calling ESDF service') + # This is half of x,y and z dims + aabb_min = Point() + aabb_min.x = (-0.5 * self.__voxel_dims[0]) + self.__grid_position[0] + aabb_min.y = (-0.5 * self.__voxel_dims[1]) + self.__grid_position[1] + aabb_min.z = (-0.5 * self.__voxel_dims[2]) + self.__grid_position[2] + # This is a voxel size. + voxel_dims = Vector3() + voxel_dims.x = self.__voxel_dims[0] + voxel_dims.y = self.__voxel_dims[1] + voxel_dims.z = self.__voxel_dims[2] + + self.__esdf_future = self.send_request(aabb_min, voxel_dims) + if self.__esdf_future is not None and self.__esdf_future.done(): + response = self.__esdf_future.result() + self.fill_marker(response) + self.__esdf_future = None + + def send_request(self, aabb_min_m, aabb_size_m): + self.__esdf_req.aabb_min_m = aabb_min_m + self.__esdf_req.aabb_size_m = aabb_size_m + self.get_logger().info( + f'ESDF req = {self.__esdf_req.aabb_min_m}, {self.__esdf_req.aabb_size_m}' + ) + esdf_future = self.__esdf_client.call_async(self.__esdf_req) + return esdf_future + + def get_esdf_voxel_grid(self, esdf_data): + esdf_array = esdf_data.esdf_and_gradients + array_shape = [ + esdf_array.layout.dim[0].size, + esdf_array.layout.dim[1].size, + esdf_array.layout.dim[2].size, + ] + array_data = np.array(esdf_array.data) + + array_data = self.__tensor_args.to_device(array_data) + + # Array data is reshaped to x y z channels + array_data = array_data.view(array_shape[0], array_shape[1], array_shape[2]).contiguous() + + # Array is squeezed to 1 dimension + array_data = array_data.reshape(-1, 1) + + # nvblox uses negative distance inside obstacles, cuRobo needs the opposite: + array_data = -1 * array_data + + # nvblox assigns a value of -1000.0 for unobserved voxels, making + array_data[array_data >= 1000.0] = -1000.0 + + # nvblox distance are at origin of each voxel, cuRobo's esdf needs it to be at faces + array_data = array_data + 0.5 * self.__voxel_size + + esdf_grid = CuVoxelGrid( + name='world_voxel', + dims=self.__voxel_dims, + pose=[ + self.__grid_position[0], + self.__grid_position[1], + self.__grid_position[2], + 1, + 0.0, + 0.0, + 0, + ], # x, y, z, qw, qx, qy, qz + voxel_size=self.__voxel_size, + feature_dtype=torch.float32, + feature_tensor=array_data, + ) + + return esdf_grid + + def fill_marker(self, esdf_data): + esdf_grid = self.get_esdf_voxel_grid(esdf_data) + self.__world_collision.update_voxel_data(esdf_grid) + vox_size = self.__publish_voxel_size + voxels = self.__world_collision.get_esdf_in_bounding_box( + CuCuboid( + name='test', + pose=[0.0, 0.0, 0.0, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz + dims=self.__voxel_dims, + ), + voxel_size=vox_size, + ) + xyzr_tensor = voxels.xyzr_tensor.clone() + xyzr_tensor[..., 3] = voxels.feature_tensor + self.publish_voxels(xyzr_tensor) + + def publish_voxels(self, voxels): + vox_size = 0.25 * self.__publish_voxel_size + + # create marker: + marker = Marker() + marker.header.frame_id = self.__robot_base_frame + marker.id = 0 + marker.type = 6 # cube list + marker.ns = 'curobo_world' + marker.action = 0 + marker.pose.orientation.w = 1.0 + marker.lifetime = rclpy.duration.Duration(seconds=1000.0).to_msg() + marker.frame_locked = False + marker.scale.x = vox_size + marker.scale.y = vox_size + marker.scale.z = vox_size + + # get only voxels that are inside surfaces: + + voxels = voxels[voxels[:, 3] >= 0.0] + vox = voxels.view(-1, 4).cpu().numpy() + marker.points = [] + + for i in range(min(len(vox), self.__max_publish_voxels)): + + pt = Point() + pt.x = float(vox[i, 0]) + pt.y = float(vox[i, 1]) + pt.z = float(vox[i, 2]) + color = ColorRGBA() + d = vox[i, 3] + + rgba = [min(1.0, 1.0 - float(d)), 0.0, 0.0, 1.0] + + color.r = rgba[0] + color.g = rgba[1] + color.b = rgba[2] + color.a = rgba[3] + marker.colors.append(color) + marker.points.append(pt) + # publish voxels: + marker.header.stamp = self.get_clock().now().to_msg() + + self.__voxel_pub.publish(marker) + + +def main(args=None): + # Initialize the rclpy library + rclpy.init(args=args) + + # Create the node + esdf_client = ESDFVisualizer() + + # Spin the node so the callback function is called. + try: + esdf_client.get_logger().info('Starting ESDFVisualizer node') + rclpy.spin(esdf_client) + + except KeyboardInterrupt: + esdf_client.get_logger().info('Destroying ESDFVisualizer node') + + # Destroy the node explicitly + esdf_client.destroy_node() + + # Shutdown the ROS client library for Python + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_esdf_visualizer/package.xml b/isaac_ros_esdf_visualizer/package.xml new file mode 100644 index 0000000..52ffc39 --- /dev/null +++ b/isaac_ros_esdf_visualizer/package.xml @@ -0,0 +1,21 @@ + + + + isaac_ros_esdf_visualizer + 3.0.0 + Package for ESDF Voxel visualizer. + Isaac ROS Maintainers + Apache-2.0 + + curobo_core + nvblox_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_esdf_visualizer/resource/isaac_ros_esdf_visualizer b/isaac_ros_esdf_visualizer/resource/isaac_ros_esdf_visualizer new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_esdf_visualizer/setup.cfg b/isaac_ros_esdf_visualizer/setup.cfg new file mode 100644 index 0000000..e142e50 --- /dev/null +++ b/isaac_ros_esdf_visualizer/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_esdf_visualizer +[install] +install_scripts=$base/lib/isaac_ros_esdf_visualizer diff --git a/isaac_ros_esdf_visualizer/setup.py b/isaac_ros_esdf_visualizer/setup.py new file mode 100644 index 0000000..c405ae4 --- /dev/null +++ b/isaac_ros_esdf_visualizer/setup.py @@ -0,0 +1,43 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_esdf_visualizer' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Package for ESDF Voxel visualizer.', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'esdf_visualizer = isaac_ros_esdf_visualizer.esdf_visualizer:main' + ], + }, +) diff --git a/isaac_ros_esdf_visualizer/test/test_copyright.py b/isaac_ros_esdf_visualizer/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/isaac_ros_esdf_visualizer/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_esdf_visualizer/test/test_flake8.py b/isaac_ros_esdf_visualizer/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_esdf_visualizer/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_esdf_visualizer/test/test_pep257.py b/isaac_ros_esdf_visualizer/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_esdf_visualizer/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_goal_setter_interfaces/CMakeLists.txt b/isaac_ros_goal_setter_interfaces/CMakeLists.txt new file mode 100644 index 0000000..b95d2db --- /dev/null +++ b/isaac_ros_goal_setter_interfaces/CMakeLists.txt @@ -0,0 +1,51 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.22.1) +project(isaac_ros_goal_setter_interfaces LANGUAGES C CXX) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# The FindPythonInterp and FindPythonLibs modules are removed +if(POLICY CMP0148) + cmake_policy(SET CMP0148 OLD) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Prepare custom goal setter interfaces +find_package(rosidl_default_generators REQUIRED) + +set(SRV_FILES + "srv/SetTargetPose.srv" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${SRV_FILES} + DEPENDENCIES geometry_msgs +) +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/isaac_ros_goal_setter_interfaces/package.xml b/isaac_ros_goal_setter_interfaces/package.xml new file mode 100644 index 0000000..d7d56eb --- /dev/null +++ b/isaac_ros_goal_setter_interfaces/package.xml @@ -0,0 +1,47 @@ + + + + + + + isaac_ros_goal_setter_interfaces + 3.0.0 + Interfaces for Isaac ROS Goal Setter + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros-gems/ + Swapnesh Wani + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + + geometry_msgs + isaac_ros_common + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/isaac_ros_goal_setter_interfaces/srv/SetTargetPose.srv b/isaac_ros_goal_setter_interfaces/srv/SetTargetPose.srv new file mode 100644 index 0000000..f217e64 --- /dev/null +++ b/isaac_ros_goal_setter_interfaces/srv/SetTargetPose.srv @@ -0,0 +1,7 @@ +# This service sets up the target pose. +# Request +geometry_msgs/PoseStamped pose +--- +# Response +# Result of the service +bool success diff --git a/isaac_ros_moveit_goal_setter/CMakeLists.txt b/isaac_ros_moveit_goal_setter/CMakeLists.txt new file mode 100644 index 0000000..2dece6e --- /dev/null +++ b/isaac_ros_moveit_goal_setter/CMakeLists.txt @@ -0,0 +1,41 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.22.1) +project(isaac_ros_moveit_goal_setter) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(${PROJECT_NAME} + src/goal_setter_node.cpp +) + +install(PROGRAMS scripts/pose_to_pose.py DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/isaac_ros_moveit_goal_setter/include/isaac_ros_moveit_goal_setter/goal_setter_node.hpp b/isaac_ros_moveit_goal_setter/include/isaac_ros_moveit_goal_setter/goal_setter_node.hpp new file mode 100644 index 0000000..c5d9111 --- /dev/null +++ b/isaac_ros_moveit_goal_setter/include/isaac_ros_moveit_goal_setter/goal_setter_node.hpp @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_MOVEIT_GOAL_SETTER__GOAL_SETTER_NODE_HPP_ +#define ISAAC_ROS_MOVEIT_GOAL_SETTER__GOAL_SETTER_NODE_HPP_ + +#include + +#include "isaac_ros_common/qos.hpp" +#include "isaac_ros_goal_setter_interfaces/srv/set_target_pose.hpp" +#include +#include + +namespace nvidia +{ +namespace isaac_ros +{ +namespace manipulation +{ + +class GoalSetterNode +{ +public: + GoalSetterNode(std::string name, const rclcpp::NodeOptions & options); + ~GoalSetterNode() = default; + + std::shared_ptr GetNode() const {return node_;} + + void ConfigureMoveit(); + +private: + void SetTargetPoseCallback( + const std::shared_ptr req, + std::shared_ptr res); + + const std::shared_ptr node_; + std::string planner_group_name_; + std::string planner_id_; + std::string end_effector_link_; + moveit::planning_interface::MoveGroupInterface move_group_interface_; + + rclcpp::Service::SharedPtr + set_target_pose_service_; + +}; + +} // namespace manipulation +} // namespace isaac_ros +} // namespace nvidia + + +#endif // ISAAC_ROS_MOVEIT_GOAL_SETTER__GOAL_SETTER_NODE_HPP_ diff --git a/isaac_ros_moveit_goal_setter/launch/isaac_ros_goal_setter.launch.py b/isaac_ros_moveit_goal_setter/launch/isaac_ros_goal_setter.launch.py new file mode 100644 index 0000000..66ddae3 --- /dev/null +++ b/isaac_ros_moveit_goal_setter/launch/isaac_ros_goal_setter.launch.py @@ -0,0 +1,109 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def get_robot_description(): + ur_type = LaunchConfiguration('ur_type') + robot_ip = LaunchConfiguration('robot_ip') + + joint_limit_params = PathJoinSubstitution( + [FindPackageShare('ur_description'), 'config', ur_type, 'joint_limits.yaml'] + ) + kinematics_params = PathJoinSubstitution( + [FindPackageShare('ur_description'), 'config', ur_type, 'default_kinematics.yaml'] + ) + physical_params = PathJoinSubstitution( + [FindPackageShare('ur_description'), 'config', ur_type, 'physical_parameters.yaml'] + ) + visual_params = PathJoinSubstitution( + [FindPackageShare('ur_description'), 'config', ur_type, 'visual_parameters.yaml'] + ) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', + PathJoinSubstitution([FindPackageShare('ur_description'), 'urdf', 'ur.urdf.xacro']), + ' ', 'robot_ip:=', robot_ip, + ' ', 'joint_limit_params:=', joint_limit_params, + ' ', 'kinematics_params:=', kinematics_params, + ' ', 'physical_params:=', physical_params, + ' ', 'visual_params:=', visual_params, + ' ', 'safety_limits:=true', + ' ', 'safety_pos_margin:=0.15', + ' ', 'safety_k_position:=20', + ' ', 'name:=ur', ' ', 'ur_type:=', ur_type, ' ', 'prefix:=''', + ] + ) + + robot_description = {'robot_description': robot_description_content} + return robot_description + + +def get_robot_description_semantic(): + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', + PathJoinSubstitution([FindPackageShare('ur_moveit_config'), 'srdf', 'ur.srdf.xacro']), + ' ', 'name:=ur', ' ', 'prefix:=""', + ] + ) + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_content + } + return robot_description_semantic + + +def generate_launch_description(): + launch_args = [ + DeclareLaunchArgument( + 'ur_type', + description='Type/series of used UR robot.', + choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e', 'ur20'], + default_value='ur5e', + ), + DeclareLaunchArgument( + 'robot_ip', + description='IP address of the robot', + default_value='192.56.1.2', + ), + + ] + moveit_kinematics_params = PathJoinSubstitution( + [FindPackageShare('ur_moveit_config'), 'config', 'default_kinematics.yaml'] + ) + robot_description = get_robot_description() + robot_description_semantic = get_robot_description_semantic() + isaac_ros_moveit_goal_setter = Node( + package='isaac_ros_moveit_goal_setter', + executable='isaac_ros_moveit_goal_setter', + name='isaac_ros_moveit_goal_setter', + output='screen', + parameters=[ + robot_description, + robot_description_semantic, + moveit_kinematics_params + ], + ) + + return launch.LaunchDescription(launch_args + [isaac_ros_moveit_goal_setter]) diff --git a/isaac_ros_moveit_goal_setter/package.xml b/isaac_ros_moveit_goal_setter/package.xml new file mode 100644 index 0000000..3087849 --- /dev/null +++ b/isaac_ros_moveit_goal_setter/package.xml @@ -0,0 +1,48 @@ + + + + + + + isaac_ros_moveit_goal_setter + 3.0.0 + This package sets the goal for MoveIt + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros-gems/ + Swapnesh Wani + Kajanan Chinniah + + ament_cmake_auto + + isaac_ros_common + isaac_ros_goal_setter_interfaces + geometry_msgs + moveit_ros_planning_interface + rclcpp + rclcpp_components + vision_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/isaac_ros_moveit_goal_setter/scripts/pose_to_pose.py b/isaac_ros_moveit_goal_setter/scripts/pose_to_pose.py new file mode 100755 index 0000000..19c1970 --- /dev/null +++ b/isaac_ros_moveit_goal_setter/scripts/pose_to_pose.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +import time + +from geometry_msgs.msg import Pose, PoseStamped +from isaac_ros_goal_setter_interfaces.srv import SetTargetPose +import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class PoseToPoseNode(Node): + + def __init__(self): + super().__init__('pose_to_pose_node') + + self._world_frame = self.declare_parameter( + 'world_frame', 'base_link').get_parameter_value().string_value + + self._target_frames = self.declare_parameter( + 'target_frames', ['target1_frame']).get_parameter_value().string_array_value + + self._target_frame_idx = 0 + + self._plan_timer_period = self.declare_parameter( + 'plan_timer_period', 0.01).get_parameter_value().double_value + + self._tf_buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=60.0)) + self._tf_listener = TransformListener(self._tf_buffer, self) + + self._goal_service_cb_group = MutuallyExclusiveCallbackGroup() + + self._goal_client = self.create_client( + SetTargetPose, 'set_target_pose', callback_group=self._goal_service_cb_group) + + while not self._goal_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service set_target_pose not available! Waiting...') + self._goal_req = SetTargetPose.Request() + self.timer = self.create_timer(self._plan_timer_period, self.on_timer) + + def _transform_msg_to_pose_msg(self, tf_msg): + pose = Pose() + pose.position.x = tf_msg.translation.x + pose.position.y = tf_msg.translation.y + pose.position.z = tf_msg.translation.z + + pose.orientation.x = tf_msg.rotation.x + pose.orientation.y = tf_msg.rotation.y + pose.orientation.z = tf_msg.rotation.z + pose.orientation.w = tf_msg.rotation.w + return pose + + def send_goal(self, pose): + self.get_logger().debug('Sending pose target to planner.') + self._goal_req.pose = pose + self.future = self._goal_client.call_async(self._goal_req) + while not self.future.done(): + time.sleep(0.001) + return self.future.result() + + def on_timer(self): + + # Check if there is a valid transform between world and target frame + try: + world_frame_pose_target_frame = self._tf_buffer.lookup_transform( + self._world_frame, self._target_frames[self._target_frame_idx], + self.get_clock().now(), rclpy.duration.Duration(seconds=10.0) + ) + except TransformException as ex: + self.get_logger().warning(f'Waiting for target_frame pose transform to be available \ + in TF, between {self._world_frame} and \ + {self._target_frames[self._target_frame_idx]}. if \ + warning persists, check if the transform is \ + published to tf. Message from TF: {ex}') + return + + output_msg = PoseStamped() + output_msg.header.stamp = self.get_clock().now().to_msg() + output_msg.header.frame_id = self._world_frame + output_msg.pose = self._transform_msg_to_pose_msg(world_frame_pose_target_frame.transform) + + response = self.send_goal(output_msg) + self.get_logger().debug(f'Goal set with response: {response}') + if response.success: + self._target_frame_idx = (self._target_frame_idx + 1) % len(self._target_frames) + else: + self.get_logger().warning('target pose was not reachable by planner, trying again \ + on the next iteration') + + +def main(args=None): + + rclpy.init(args=args) + + pose_to_pose_node = PoseToPoseNode() + executor = MultiThreadedExecutor() + executor.add_node(pose_to_pose_node) + try: + executor.spin() + except KeyboardInterrupt: + pose_to_pose_node.get_logger().info( + 'KeyboardInterrupt, shutting down.\n' + ) + pose_to_pose_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_moveit_goal_setter/src/goal_setter_node.cpp b/isaac_ros_moveit_goal_setter/src/goal_setter_node.cpp new file mode 100644 index 0000000..feff23d --- /dev/null +++ b/isaac_ros_moveit_goal_setter/src/goal_setter_node.cpp @@ -0,0 +1,104 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// 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. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "isaac_ros_moveit_goal_setter/goal_setter_node.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace manipulation +{ + +GoalSetterNode::GoalSetterNode(std::string name, const rclcpp::NodeOptions & options) +: node_{std::make_shared(name, options)}, + planner_group_name_(node_->declare_parameter( + "planner_group_name", "ur_manipulator")), + planner_id_(node_->declare_parameter("planner_id", "cuMotion")), + end_effector_link_(node_->declare_parameter("end_effector_link", "wrist_3_link")), + move_group_interface_{moveit::planning_interface::MoveGroupInterface(node_, planner_group_name_)} +{ + set_target_pose_service_ = + node_->create_service( + "set_target_pose", std::bind( + &GoalSetterNode::SetTargetPoseCallback, this, std::placeholders::_1, std::placeholders::_2)); + ConfigureMoveit(); +} + +void GoalSetterNode::ConfigureMoveit() +{ + // Initialize the move group interface + move_group_interface_.setPlannerId(planner_id_); + RCLCPP_INFO(node_->get_logger(), "Planner ID : %s", move_group_interface_.getPlannerId().c_str()); + + move_group_interface_.setEndEffectorLink(end_effector_link_); + RCLCPP_INFO(node_->get_logger(), "End Effector Link : %s", end_effector_link_.c_str()); +} + +void GoalSetterNode::SetTargetPoseCallback( + const std::shared_ptr req, + std::shared_ptr res) +{ + res->success = false; + RCLCPP_DEBUG(node_->get_logger(), "Triggered SetTargetPoseCallback"); + RCLCPP_DEBUG( + node_->get_logger(), "Pose : x=%f, y=%f, z=%f, qx=%f, qy=%f, qz=%f, qw=%f", + req->pose.pose.position.x, req->pose.pose.position.y, req->pose.pose.position.z, + req->pose.pose.orientation.x, req->pose.pose.orientation.y, req->pose.pose.orientation.z, + req->pose.pose.orientation.w); + + auto success = move_group_interface_.setPoseTarget(req->pose, end_effector_link_); + if (!success) { + RCLCPP_ERROR(node_->get_logger(), "Failed to set target pose!"); + return; + } + + auto const [status, plan] = [this] { + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface_.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if (status) { + RCLCPP_ERROR(node_->get_logger(), "Executing!"); + move_group_interface_.execute(plan); + res->success = true; + } else { + RCLCPP_ERROR(node_->get_logger(), "Planning failed!"); + } + +} + +} // namespace manipulation +} // namespace isaac_ros +} // namespace nvidia + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto goal_setter_node = std::make_shared( + "moveit_goal_setter", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(goal_setter_node->GetNode()); + executor.spin(); + rclcpp::shutdown(); + return 0; +}