feat(aloha_ws): Add scripts for generating vx300s usd assets and omni…

…graph support for joint state publishing in isaac sim

aloha_ws/.gitignore

@@ -5,3 +5,6 @@
 /build
 /install
 /log
+
+# Custom ignores
+/isaacsim/assets

aloha_ws/README.md

@@ -9,12 +9,16 @@ docker compose up
 docker exec -it ros2-aloha-ws bash
 ```
 
+The commands in the following sections assume that you are inside the Docker container.
+
 ## View Robot Model in RViz
 
 ```sh
 ros2 launch interbotix_xsarm_descriptions xsarm_description.launch.py robot_model:=vx300s use_joint_pub_gui:=true
 ```
 
+It is worth noting that `aloha_vx300s.urdf.xacro` and `vx300s.urdf.xacro` files are identical. We opt to use `vx300s` since `aloha_vx300s` seems to lack corresponding configs, such as those for MoveIt 2.
+
 ## View Robot Model in Gazebo
 
 ```sh
@@ -30,6 +34,42 @@ ros2 launch interbotix_xsarm_moveit xsarm_moveit.launch.py robot_model:=vx300s h
 ros2 launch interbotix_xsarm_moveit xsarm_moveit.launch.py robot_model:=vx300s hardware_type:=fake
 ```
 
+## View Robot Model in Isaac Sim
+
+Prepare USD files:
+
+```sh
+cd /home/ros2-essentials/aloha_ws/isaacsim/scripts
+./create_urdf_from_xacro.sh
+python3 create_vx300s_from_urdf.py
+python3 create_vx300s_with_omnigraph.py
+```
+
+Launch Isaac Sim app:
+
+```sh
+isaacsim omni.isaac.sim
+```
+
+Open pre-configured USD file with OmniGraph:
+
+- `File > Open` and in `File name:` type `/home/ros2-essentials/aloha_ws/isaacsim/assets/vx300s_og.usd`
+- Click `Window > Visual Scripting > Action Graph`
+- In the `Action Graph` tab, click `Edit Action Graph` and select `/ActionGraph`
+- Click `Play (SPACE)`
+
+View the current joint states:
+
+```sh
+# in a new terminal
+docker exec -it ros2-aloha-ws bash
+ros2 topic echo /joint_states
+```
+
+Set the current joint states:
+
+(TODO)
+
 ## References
 
 - [Interbotix X-Series Arms \| Trossen Robotics Documentation](https://docs.trossenrobotics.com/interbotix_xsarms_docs/index.html)

aloha_ws/docker/Dockerfile

@@ -110,6 +110,8 @@ USER $USERNAME
 # Create Gazebo cache directory with correct ownership to avoid permission issues after volume mount
 RUN mkdir /home/$USERNAME/.gazebo
 # Install Isaac Sim (requires Python 3.10)
+# Note that installing Isaac Sim with pip is experimental, keep this in mind when unexpected error occurs
+# TODO: Remove the note above when it is no longer experimental
 # Ref: https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_python.html#installation-using-pip
 RUN --mount=type=cache,target=/home/user/.cache/pip,sharing=private \
     if [ "$TARGETARCH" = "amd64" ]; then \

aloha_ws/isaacsim/scripts/create_urdf_from_xacro.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+cd ~/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf
+# Ref: https://docs.ros.org/en/humble/Tutorials/Intermediate/URDF/Using-Xacro-to-Clean-Up-a-URDF-File.html
+xacro vx300s.urdf.xacro > ~/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/vx300s.urdf

aloha_ws/isaacsim/scripts/create_vx300s_from_urdf.py

@@ -0,0 +1,68 @@
+# Ref: https://docs.omniverse.nvidia.com/isaacsim/latest/core_api_tutorials/tutorial_core_hello_world.html
+# launch Isaac Sim before any other imports
+# default first two lines in any standalone application
+from isaacsim import SimulationApp
+simulation_app = SimulationApp({"headless": False})
+
+# Ref: https://docs.omniverse.nvidia.com/isaacsim/latest/advanced_tutorials/tutorial_advanced_import_urdf.html#importing-urdf-using-python
+
+import asyncio
+import logging
+import os
+
+import omni.kit.commands
+import omni.usd
+from omni.importer.urdf import _urdf
+from omni.kit import ui_test
+from pxr import UsdPhysics
+
+logger = logging.getLogger(__name__)
+
+
+def log(msg):
+    # Print to console
+    # Ref: https://docs.omniverse.nvidia.com/kit/docs/kit-manual/latest/guide/logging.html
+    logger.info(msg)
+    # Print to Script Editor
+    print(msg)
+
+async def create_new_stage_with_defaults():
+    menu_widget = ui_test.get_menubar()
+    await menu_widget.find_menu("File").click()
+    await menu_widget.find_menu("New").click()
+    log("Done!")
+    simulation_app.close()
+
+def create_vx300s_from_urdf(urdf_path, usd_path):
+    # Set the settings in the import config
+    import_config = _urdf.ImportConfig()
+    import_config.make_default_prim = True
+    # Finally import the robot & save it as USD
+    result, prim_path = omni.kit.commands.execute(
+        "URDFParseAndImportFile", urdf_path=urdf_path,
+        import_config=import_config, dest_path=usd_path,
+    )
+    omni.usd.get_context().open_stage(usd_path)
+    stage = omni.usd.get_context().get_stage()
+    # Articulation Root for a fixed-base articulation can be either:
+    # (1) the fixed joint that connects the articulation base to the world, or
+    # (2) an ancestor of the fixed joint in the USD hierarchy.
+    # Ref: https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/articulations.html#articulationroot
+    # The URDF Importer uses the first option to define articulation root for the imported robot.
+    # Unfortunately, the first option somehow reports a false-positive error when using OmniGraph.
+    # The false-positive error message doesn't affect the actual functionalities, but may bring potential confusions.
+    # Therefore, we opt to manually switch to using the second option.
+    # TODO: Switch back to the first option after this issue is fixed.
+    prim = stage.GetPrimAtPath("/vx300s")
+    # Note that this somehow cannot be applied through GUI, and must be done through Python
+    UsdPhysics.ArticulationRootAPI.Apply(prim)
+    prim = stage.GetPrimAtPath("/vx300s/root_joint")
+    prim.RemoveAPI(UsdPhysics.ArticulationRootAPI)
+    omni.usd.get_context().save_stage()
+
+if __name__ == '__main__':
+    print("Creating `vx300s.usd` from urdf...")
+    vx300s_urdf_path = f'{os.path.expanduser("~")}/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_descriptions/urdf/vx300s.urdf'
+    vx300s_usd_path = '/home/ros2-essentials/aloha_ws/isaacsim/assets/vx300s_urdf.usd'
+    create_vx300s_from_urdf(vx300s_urdf_path, vx300s_usd_path)
+    asyncio.ensure_future(create_new_stage_with_defaults())

aloha_ws/isaacsim/scripts/create_vx300s_with_omnigraph.py

@@ -0,0 +1,93 @@
+# Ref: https://docs.omniverse.nvidia.com/isaacsim/latest/core_api_tutorials/tutorial_core_hello_world.html
+# launch Isaac Sim before any other imports
+# default first two lines in any standalone application
+from isaacsim import SimulationApp
+simulation_app = SimulationApp({"headless": False})
+
+import logging
+
+import omni.graph.core as og
+import omni.kit.app
+import omni.usd
+from omni.isaac.core import World
+
+logger = logging.getLogger(__name__)
+
+
+def log(msg):
+    # Print to console
+    # Ref: https://docs.omniverse.nvidia.com/kit/docs/kit-manual/latest/guide/logging.html
+    logger.info(msg)
+    # Print to Script Editor
+    print(msg)
+
+def create_vx300s_with_omnigraph():
+    # Create vx300s
+    vx300s_prim_path = "/World/vx300s_urdf"
+    stage = omni.usd.get_context().get_stage()
+    stage.DefinePrim(vx300s_prim_path)
+    prim = stage.GetPrimAtPath(vx300s_prim_path)
+    prim.GetReferences().AddReference("/home/ros2-essentials/aloha_ws/isaacsim/assets/vx300s_urdf.usd")
+
+    # Create OmniGraph
+    # Ref: https://docs.omniverse.nvidia.com/isaacsim/latest/ros2_tutorials/tutorial_ros2_manipulation.html#add-joint-states-in-extension
+    # Ref: https://docs.omniverse.nvidia.com/isaacsim/latest/advanced_tutorials/tutorial_advanced_omnigraph_scripting.html
+    og.Controller.edit(
+        {"graph_path": "/ActionGraph", "evaluator_name": "execution"},
+        {
+            og.Controller.Keys.CREATE_NODES: [
+                ("OnPlaybackTick", "omni.graph.action.OnPlaybackTick"),
+                ("PublishJointState", "omni.isaac.ros2_bridge.ROS2PublishJointState"),
+                ("SubscribeJointState", "omni.isaac.ros2_bridge.ROS2SubscribeJointState"),
+                ("ArticulationController", "omni.isaac.core_nodes.IsaacArticulationController"),
+                ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"),
+                ("ConstantToken", "omni.graph.nodes.ConstantToken"),
+                ("ToTarget", "omni.graph.nodes.ToTarget"),
+            ],
+            og.Controller.Keys.CONNECT: [
+                ("OnPlaybackTick.outputs:tick", "PublishJointState.inputs:execIn"),
+                ("OnPlaybackTick.outputs:tick", "SubscribeJointState.inputs:execIn"),
+                ("OnPlaybackTick.outputs:tick", "ArticulationController.inputs:execIn"),
+
+                ("ReadSimTime.outputs:simulationTime", "PublishJointState.inputs:timeStamp"),
+
+                # Ref: https://docs.omniverse.nvidia.com/kit/docs/omni.graph.nodes/latest/GeneratedNodeDocumentation/OgnConstantToken.html
+                # Ref: https://docs.omniverse.nvidia.com/kit/docs/omni.graph.nodes/latest/GeneratedNodeDocumentation/OgnToTarget.html
+                ("ConstantToken.inputs:value", "ToTarget.inputs:value"),
+                ("ToTarget.outputs:converted", "PublishJointState.inputs:targetPrim"),
+                ("ToTarget.outputs:converted", "ArticulationController.inputs:targetPrim"),
+
+                ("SubscribeJointState.outputs:jointNames", "ArticulationController.inputs:jointNames"),
+                ("SubscribeJointState.outputs:positionCommand", "ArticulationController.inputs:positionCommand"),
+                ("SubscribeJointState.outputs:velocityCommand", "ArticulationController.inputs:velocityCommand"),
+                ("SubscribeJointState.outputs:effortCommand", "ArticulationController.inputs:effortCommand"),
+            ],
+            og.Controller.Keys.SET_VALUES: [
+                # Please refer to the `create_vx300s_from_urdf.py` file for reasons on using the root prim instead of root joint.
+                ("ConstantToken.inputs:value", vx300s_prim_path),
+            ],
+        },
+    )
+
+if __name__ == '__main__':
+    # Ref: https://docs.omniverse.nvidia.com/dev-guide/latest/programmer_ref/extensions/enable-kit-extension.html
+    manager = omni.kit.app.get_app().get_extension_manager()
+    # enable immediately
+    manager.set_extension_enabled_immediate("omni.isaac.ros2_bridge", True)
+    log("ros2_bridge enabled: " + str(manager.is_extension_enabled("omni.isaac.ros2_bridge")))
+    create_vx300s_with_omnigraph()
+    vx300s_og_usd_path = '/home/ros2-essentials/aloha_ws/isaacsim/assets/vx300s_og.usd'
+    omni.usd.get_context().save_as_stage(vx300s_og_usd_path)
+    log("Done!")
+    # # Ref: `~/.local/share/ov/pkg/isaac-sim-4.1.0/standalone_examples/api/omni.isaac.franka/follow_target_with_rmpflow.py`
+    # my_world = World(stage_units_in_meters=1.0)
+    # reset_needed = False
+    # while simulation_app.is_running():
+    #     my_world.step(render=True)
+    #     if my_world.is_stopped() and not reset_needed:
+    #         reset_needed = True
+    #     if my_world.is_playing():
+    #         if reset_needed:
+    #             my_world.reset()
+    #             reset_needed = False
+    simulation_app.close()