Skip to content

Commit

Permalink
Update isaac_panda example for compatibility with Isaac Sim 4.0 (#908)
Browse files Browse the repository at this point in the history
* Update isaac_panda example for compatibility with Isaac Sim 4.0

In addition, the header of the Isaac Sim launch script has been updated
to indicate that it has been licensed (by NVIDIA) under BSD-3.

* Fix formatting
  • Loading branch information
rbabich authored Jul 9, 2024
1 parent 2ac49ce commit 45acb1f
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 29 deletions.
94 changes: 65 additions & 29 deletions doc/how_to_guides/isaac_panda/launch/isaac_moveit.py
Original file line number Diff line number Diff line change
@@ -1,11 +1,31 @@
# -*- coding: utf-8 -*-
# Copyright (c) 2020-2022, NVIDIA CORPORATION. All rights reserved.
# Copyright (c) 2020-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
# Copyright (c) 2023 PickNik, LLC. 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.
# 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 sys
import re
Expand All @@ -14,7 +34,13 @@
import carb
import numpy as np
from pathlib import Path
from omni.isaac.kit import SimulationApp

# In older versions of Isaac Sim (prior to 4.0), SimulationApp is imported from
# omni.isaac.kit rather than isaacsim.
try:
from isaacsim import SimulationApp
except:
from omni.isaac.kit import SimulationApp

FRANKA_STAGE_PATH = "/Franka"
FRANKA_USD_PATH = "/Isaac/Robots/Franka/franka_alt_fingers.usd"
Expand All @@ -26,10 +52,14 @@

CONFIG = {"renderer": "RayTracedLighting", "headless": False}

# Example ROS2 bridge sample demonstrating the manual loading of stages
# and creation of ROS components
simulation_app = SimulationApp(CONFIG)

from omni.isaac.version import get_version

# Check the major version number of Isaac Sim to see if it's four digits, corresponding
# to Isaac Sim 2023.1.1 or older. The version numbering scheme changed with the
# Isaac Sim 4.0 release in 2024.
is_legacy_isaacsim = len(get_version()[2]) == 4

# More imports that need to compare after we create the app
from omni.isaac.core import SimulationContext # noqa E402
Expand Down Expand Up @@ -123,8 +153,32 @@
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
# Create an action graph with ROS component nodes
try:
og_keys_set_values = [
("Context.inputs:domain_id", ros_domain_id),
# Set the /Franka target prim to Articulation Controller node
("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"),
]

# In older versions of Isaac Sim, the articulation controller node contained a
# "usePath" checkbox input that should be enabled.
if is_legacy_isaacsim:
og_keys_set_values.insert(1, ("ArticulationController.inputs:usePath", True))

og.Controller.edit(
{"graph_path": GRAPH_PATH, "evaluator_name": "execution"},
{
Expand Down Expand Up @@ -212,25 +266,7 @@
"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"),
],
og.Controller.Keys.SET_VALUES: og_keys_set_values,
},
)
except Exception as e:
Expand Down
7 changes: 7 additions & 0 deletions doc/how_to_guides/isaac_panda/launch/python.sh
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,13 @@ do
fi
done

# When installed from the Omniverse Launcher, recent versions of Isaac Sim have a dash
# rather than underscore in their installation directory name (e.g., isaac-sim-4.0.0).
for ISAAC_SCRIPT_DIR in $(ls -d -- $OV_PKG_DIR/isaac-sim-*);
do
ISAAC_SCRIPT_DIRS+=($ISAAC_SCRIPT_DIR)
done

# Prepend the path to all arguments passed in
CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
NEW_ARGS=""
Expand Down

0 comments on commit 45acb1f

Please sign in to comment.