From a26cf1aa4de9f6963464479c91d12125c2bda824 Mon Sep 17 00:00:00 2001 From: Johnson Sun Date: Tue, 10 Sep 2024 02:19:45 +0800 Subject: [PATCH] feat(aloha_ws): Add scripts for generating vx300s usd assets and omnigraph support for joint state publishing in isaac sim --- aloha_ws/.gitignore | 3 + aloha_ws/README.md | 40 ++++++++ aloha_ws/docker/Dockerfile | 2 + aloha_ws/isaacsim/assets/.gitkeep | 0 .../scripts/create_urdf_from_xacro.sh | 5 + .../scripts/create_vx300s_from_urdf.py | 68 ++++++++++++++ .../scripts/create_vx300s_with_omnigraph.py | 93 +++++++++++++++++++ 7 files changed, 211 insertions(+) create mode 100644 aloha_ws/isaacsim/assets/.gitkeep create mode 100755 aloha_ws/isaacsim/scripts/create_urdf_from_xacro.sh create mode 100644 aloha_ws/isaacsim/scripts/create_vx300s_from_urdf.py create mode 100644 aloha_ws/isaacsim/scripts/create_vx300s_with_omnigraph.py diff --git a/aloha_ws/.gitignore b/aloha_ws/.gitignore index 2bc3ebb3..bd34b547 100644 --- a/aloha_ws/.gitignore +++ b/aloha_ws/.gitignore @@ -5,3 +5,6 @@ /build /install /log + +# Custom ignores +/isaacsim/assets diff --git a/aloha_ws/README.md b/aloha_ws/README.md index 1235df40..6e34f010 100644 --- a/aloha_ws/README.md +++ b/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) diff --git a/aloha_ws/docker/Dockerfile b/aloha_ws/docker/Dockerfile index 0c12e5e1..97a92a3d 100644 --- a/aloha_ws/docker/Dockerfile +++ b/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 \ diff --git a/aloha_ws/isaacsim/assets/.gitkeep b/aloha_ws/isaacsim/assets/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/aloha_ws/isaacsim/scripts/create_urdf_from_xacro.sh b/aloha_ws/isaacsim/scripts/create_urdf_from_xacro.sh new file mode 100755 index 00000000..7e314a43 --- /dev/null +++ b/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 diff --git a/aloha_ws/isaacsim/scripts/create_vx300s_from_urdf.py b/aloha_ws/isaacsim/scripts/create_vx300s_from_urdf.py new file mode 100644 index 00000000..4116cfc5 --- /dev/null +++ b/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()) diff --git a/aloha_ws/isaacsim/scripts/create_vx300s_with_omnigraph.py b/aloha_ws/isaacsim/scripts/create_vx300s_with_omnigraph.py new file mode 100644 index 00000000..66ac4cbe --- /dev/null +++ b/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()