update isaac demo with pytorch (#694)

Co-authored-by: PeterMitrano [email protected]

Author: MarqRazz

.dockerignore

@@ -6,6 +6,5 @@ __pycache__
 **/_templates
 **/build
 **/.vscode
-**/.docker
 *.md
 *.rst

doc/how_to_guides/isaac_panda/.docker/Dockerfile

@@ -2,6 +2,9 @@ FROM osrf/ros:humble-desktop-jammy
 
 SHELL ["/bin/bash", "-c", "-o", "pipefail"]
 
+ARG CUDA_MAJOR_VERSION=11
+ARG CUDA_MINOR_VERSION=7
+
 RUN echo "deb [trusted=yes] https://raw.githubusercontent.com/moveit/moveit2_packages/jammy-humble/ ./" \
     | sudo tee /etc/apt/sources.list.d/moveit_moveit2_packages.list
 RUN echo "yaml https://raw.githubusercontent.com/moveit/moveit2_packages/jammy-humble/local.yaml humble" \
@@ -12,9 +15,23 @@ RUN echo "yaml https://raw.githubusercontent.com/moveit/moveit2_packages/jammy-h
 RUN apt-get update && apt-get upgrade -y && rosdep update
 
 # Install packages required to run the demo
-RUN apt-get install -y --no-install-recommends \
+RUN apt-get update && apt-get install -y --no-install-recommends \
     ros-humble-moveit \
-    ros-humble-moveit-resources
+    ros-humble-moveit-resources \
+    wget \
+    python3-pip
+
+# Install CUDA
+ENV DEBIAN_FRONTEND noninteractive
+RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2204/x86_64/cuda-keyring_1.0-1_all.deb && \
+    dpkg -i cuda-keyring_1.0-1_all.deb && \
+    apt-get update && \
+    apt-get -y install cuda-cudart-${CUDA_MAJOR_VERSION}-${CUDA_MINOR_VERSION}  cuda-compat-${CUDA_MAJOR_VERSION}-${CUDA_MINOR_VERSION} cuda-toolkit-${CUDA_MAJOR_VERSION}-${CUDA_MINOR_VERSION}
+ENV PATH=/usr/local/nvidia/bin:/usr/local/cuda/bin:$PATH
+ENV LD_LIBRARY_PATH=/usr/local/nvidia/lib:/usr/local/nvidia/lib64:$LD_LIBRARY_PATH
+
+# Install pytorch
+RUN pip3 install torch torchvision
 
 # Create Colcon workspace and clone the needed source code to run the demo
 RUN mkdir -p /root/isaac_moveit_tutorial_ws/src
@@ -34,6 +51,10 @@ RUN mkdir -p /opt/.ros
 COPY ./doc/how_to_guides/isaac_panda/.docker/fastdds.xml /opt/.ros/fastdds.xml
 ENV FASTRTPS_DEFAULT_PROFILES_FILE=/opt/.ros/fastdds.xml
 
+ENV NVIDIA_VISIBLE_DEVICES all
+ENV NVIDIA_DRIVER_CAPABILITIES compute,utility
+ENV NVIDIA_REQUIRE_CUDA "cuda>=${CUDA_MAJOR_VERSION}.${CUDA_MINOR_VERSION}"
+
 # Build the Colcon workspace for the user
 RUN source /opt/ros/humble/setup.bash && colcon build
 

doc/how_to_guides/isaac_panda/docker-compose.yaml

@@ -35,9 +35,21 @@ services:
       # Allows graphical programs in the container
       - /tmp/.X11-unix:/tmp/.X11-unix:rw
       - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority
-  demo_isaac:
+
+  # Use this service if you want to run the moveit container on a computer without a GPU
+  # You will still be able to use pytorch, but you will not be able to use the GPU.
+  demo_isaac_no_gpu:
     extends: base
     command: ros2 launch moveit2_tutorials isaac_demo.launch.py
+
   demo_mock_components:
     extends: base
     command: ros2 launch moveit2_tutorials isaac_demo.launch.py ros2_control_hardware_type:=mock_components
+
+  demo_isaac:
+    extends: demo_isaac_no_gpu
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - capabilities: [gpu]

doc/how_to_guides/isaac_panda/isaac_panda_tutorial.rst

@@ -1,13 +1,14 @@
 How To Command Simulated Isaac Robot
 ====================================
 
-This tutorial requires a machine with ``Isaac Sim 2022.2.0`` installed.
+This tutorial requires a machine with ``Isaac Sim 2022.2.0`` or ``Isaac Sim 2022.2.1`` installed.
 For Isaac Sim requirements and installation please see the `Omniverse documentation <https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html>`_.
 
 This tutorial has the following assumptions on system configuration:
 
-1. NVIDIA Isaac Sim 2022.2.0 is installed on a Ubuntu 20.04 host in the ``$HOME/.local/share/ov/pkg/isaac_sim-2022.2.0`` directory. (this is the default location)
+1. NVIDIA Isaac Sim 2022.2.0 or 2022.2.1 is installed on a Ubuntu 20.04 host in the default location.
 2. Docker is installed.
+   If you plan to use your GPU with MoveIt, you will need to install `nvidia-docker <https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html#installing-on-ubuntu-and-debian>`_.
 3. You clone this repo so that you can build a Ubuntu 22.04 Humble based Docker image that can communicate with Isaac and run this tutorial.
 
 Introduction to ros2_control
@@ -57,6 +58,9 @@ updated to load the ``TopicBasedSystem`` plugin when the flag ``ros2_control_har
 In this tutorial we have included a Python script that loads a Panda robot
 and builds an `OmniGraph <https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_omnigraph.html>`_
 to publish and subscribe to the ROS topics used to control the robot.
+The OmniGraph also contains nodes to publish RGB and Depth images from the camera mounted on the hand of the Panda.
+The RGB image is published to the topic ``/rgb``, the camera info to ``/camera_info``, and the depth image to ``/depth``.
+The frame ID of the camera frame is ``/sim_camera``.
 To learn about configuring your Isaac Sim robot to communicate with ROS 2 please see the
 `Joint Control tutorial <https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_ros2_manipulation.html>`_
 on Omniverse.
@@ -78,15 +82,18 @@ Computer Setup
 
   cd moveit2_tutorials/doc/how_to_guides/isaac_panda
 
-4. Build the Docker image.
+4. Build the Docker image. This docker image also contains ``pytorch``.
 
 .. code-block:: bash
 
-  docker compose build
+  docker compose build base
+
 
 Running the MoveIt Interactive Marker Demo with Mock Components
 ---------------------------------------------------------------
 
+This section tests out the ``mock_components/GenericSystem`` hardware interface, as opposed to using Isaac Sim.
+
 1. To test out the ``mock_components/GenericSystem`` hardware interface run:
 
 .. code-block:: bash
@@ -111,7 +118,7 @@ Running the MoveIt Interactive Marker Demo with Isaac Sim
 
 2. Then run the following command to load the Panda Robot pre-configured to work with this tutorial.
 
-.. note:: This step assumes Isaac Sim is installed on the host in the ``$HOME/.local/share/ov/pkg/isaac_sim-2022.2.0" directory``.
+.. note:: This step assumes Isaac Sim version 2022.2.0 or 2022.2.1 is installed on the host in the ``$HOME/.local/share/ov/pkg/" directory``.
   This step also takes a few minutes to download the assets and setup Isaac Sim so please be
   patient and don't click the ``Force Quit`` dialog that pops up while the simulator starts.
 
@@ -130,5 +137,5 @@ This will open up RViz with the Panda robot using the ``TopicBasedSystem`` inter
 .. raw:: html
 
     <div style="position: relative; padding-bottom: 5%; height: 0; overflow: hidden; max-width: 100%; height: auto;">
-        <iframe width="700px" height="400px" src="https://www.youtube.com/embed/af3zkAOWk2Q" frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe>
+        <iframe width="700px" height="400px" src="https://www.youtube.com/embed/EiLaJ7e4M-4" frameborder="0" allow="autoplay; encrypted-media" allowfullscreen></iframe>
     </div>

doc/how_to_guides/isaac_panda/launch/isaac_demo.launch.py

@@ -68,13 +68,29 @@ def generate_launch_description():
     )
 
     # Static TF
-    static_tf_node = Node(
+    world2robot_tf_node = Node(
         package="tf2_ros",
         executable="static_transform_publisher",
         name="static_transform_publisher",
         output="log",
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "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(
@@ -124,7 +140,8 @@ def generate_launch_description():
         [
             ros2_control_hardware_type,
             rviz_node,
-            static_tf_node,
+            world2robot_tf_node,
+            hand2camera_tf_node,
             robot_state_publisher,
             move_group_node,
             ros2_control_node,

doc/how_to_guides/isaac_panda/launch/isaac_moveit.py

@@ -8,23 +8,32 @@
 # license agreement from NVIDIA CORPORATION is strictly prohibited.
 
 import sys
+import re
+import os
 
 import carb
 import numpy as np
+from pathlib import Path
 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)
-import omni.graph.core as og  # noqa E402
+
+
+# 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,
@@ -34,7 +43,9 @@
     viewports,
 )
 from omni.isaac.core_nodes.scripts.utils import set_target_prims  # noqa E402
-from pxr import Gf  # noqa E402
+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")
@@ -65,12 +76,47 @@
     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()
 
 # Creating a action graph with ROS component nodes
 try:
     og.Controller.edit(
-        {"graph_path": "/ActionGraph", "evaluator_name": "execution"},
+        {"graph_path": GRAPH_PATH, "evaluator_name": "execution"},
         {
             og.Controller.Keys.CREATE_NODES: [
                 ("OnImpulseEvent", "omni.graph.action.OnImpulseEvent"),
@@ -86,6 +132,16 @@
                     "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"),
@@ -119,14 +175,51 @@
                     "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:useDomainIDEnvVar", 1),
+                ("Context.inputs:domain_id", int(os.environ["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"),
             ],
         },
     )
@@ -139,13 +232,34 @@
     primPath="/ActionGraph/PublishJointState", targetPrimPaths=[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

package.xml

@@ -61,6 +61,7 @@
   <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ros_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>