isaac sim support with deformable

README.md

@@ -2,6 +2,9 @@
 
 This repo is a versatile tool designed to seamlessly rendering objects from simulation or sensors to Mixed Reality (MR) or Augmented Reality (AR) headsets.
 
+## Isaac Sim
+Please check [this](./demos/IsaacSim/README.md) for information on how to setup simpub with isaac sim.
+
 ## Table of Contents
 - [Introduction](#Introduction)
 - [Features](#Features)
@@ -64,3 +67,4 @@ We recommend using a single WiFi router for PC-headset communication to ensure o
 Additionally, using a wired cable to connect your PC can significantly reduce latency.
 
 3. Run the usage examples under the folder `/demos/`, then wear the headset, start the unity application and enjoy!
+

demos/IsaacSim/README.md

@@ -0,0 +1,37 @@
+# Isaac Lab Setup Guide
+
+## Quick Start
+
+Please follow the [readme](../../README.md) in the root folder for installing simpub.
+
+You can install Isaac Sim and Isaac Lab following [this page](https://isaac-sim.github.io/IsaacLab/source/setup/installation/binaries_installation.html). You could also install isaac sim with pip, but I didn't test on it.
+
+Now you can activate your Isaac Lab conda environment, and run some demos. For example, run this in the root folder:
+```
+python demos/IsaacSim/spawn_demo.py
+```
+
+- [spawn_demo.py](./spawn_demo.py) is an example of spawing primitives manually.
+- [env_demo.py](./env_demo.py) is an example of using environments.
+- [meta_quest_3_ik_demo.py](./meta_quest_3_ik_demo.py) is an interactive scene where you can control an robot arm with joysticks with meta quest 3. (It might not work properly...)
+
+To use simpub with Isaac Sim in your code, simply add this after the scene has finished initialization (please check the demos):
+```
+from simpub.sim.isaacsim_publisher import IsaacSimPublisher
+
+# use only one of these
+publisher = IsaacSimPublisher(host="192.168.0.134", stage=sim.stage) # for InteractiveScene
+publisher = IsaacSimPublisher(host="192.168.0.134", stage=env.sim.stage) # for environments
+```
+
+## Tips
+
+The file [demos/IsaacSim/script.py](./script.py) provides some information on how to retrieve materials from usd primitives. But it's not complete yet. You still have to figure out how to retireve albedo color and texture from the Material object. You might find [this](https://openusd.org/dev/api/class_usd_shade_material_binding_a_p_i.html) helpful (or not).
+
+Currently simpub doesn't support normal indexing. So one vertex will only have one normal, which is the cause of the rendering artifacts. Per-vertex normals are smoothed, so you can't render flat surfaces and sharp edges. To solve this, triangle faces in meshes should not only index vertices, but also normals. This means a vertex will have different normals on different triangles.
+
+Check [this](https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/usd_fabric_usdrt.html) for the different between usd, usdrt and fabric api. Basically, in python, you should use usd for cpu simulation and usdrt for gpu simulation.
+
+In omniverse, there are primitive prototypes that can be instantiated. Currently, IsaacSimPublisher doesn't handle this properly. It'll create a different mesh object and store the same geometry repeatedly for all instances of a prototype. For large scenes with complex geometries, this might be a problem.
+
+Currently usdrt stage are created for both cpu and gpu simulation. This works but might result in unexpected things. Maybe add a new parameter to the publisher indicating where the simulation is running and use only usd stage for cpu simulation.

demos/IsaacSim/env_demo.py

@@ -0,0 +1,227 @@
+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import argparse
+import numpy as np
+import torch
+import carb
+from scipy.spatial.transform import Rotation
+
+from omni.isaac.lab.app import AppLauncher
+import gymnasium as gym
+
+# add argparse arguments
+parser = argparse.ArgumentParser(
+    description="This script demonstrates different single-arm manipulators."
+)
+# append AppLauncher cli args
+AppLauncher.add_app_launcher_args(parser)
+# parse the arguments
+args_cli = parser.parse_args()
+
+# launch omniverse app
+app_launcher = AppLauncher(args_cli)
+simulation_app = app_launcher.app
+
+# isaac sim modules are only available after launching it (?)
+
+from pxr import Usd
+
+import omni.isaac.core.utils.prims as prim_utils
+
+import omni.isaac.lab.sim as sim_utils
+from omni.isaac.lab.assets import Articulation, RigidObjectCfg
+from omni.isaac.lab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
+from omni.isaac.lab.sim.spawners.shapes.shapes_cfg import CapsuleCfg
+from omni.isaac.lab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
+from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR
+# from omni.isaac.lab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
+# from omni.isaac.lab.envs.mdp.actions.actions_cfg import (
+#     DifferentialInverseKinematicsActionCfg,
+# )
+
+from omni.isaac.lab.devices import Se3Gamepad, Se3Keyboard, Se3SpaceMouse
+
+import omni.isaac.lab_tasks  # noqa: F401
+from omni.isaac.lab_tasks.utils import parse_env_cfg
+
+from simpub.sim.isaacsim_publisher import IsaacSimPublisher
+# from simpub.xr_device.meta_quest3 import MetaQuest3
+
+print(ISAACLAB_NUCLEUS_DIR)
+
+
+def pre_process_actions(
+    task: str, delta_pose: torch.Tensor, gripper_command: bool
+) -> torch.Tensor:
+    """Pre-process actions for the environment."""
+    # compute actions based on environment
+    if "Reach" in task:
+        # note: reach is the only one that uses a different action space
+        # compute actions
+        return delta_pose
+    else:
+        # resolve gripper command
+        gripper_vel = torch.zeros(delta_pose.shape[0], 1, device=delta_pose.device)
+        gripper_vel[:] = -1.0 if gripper_command else 1.0
+        # compute actions
+        return torch.concat([delta_pose, gripper_vel], dim=1)
+
+
+def main():
+    """Running keyboard teleoperation with Isaac Lab manipulation environment."""
+    task = "Isaac-Lift-Cube-Franka-IK-Rel-v0"
+
+    # parse configuration
+    # why using undefined parameter (use_gpu) doesn't raise error?
+    env_cfg = parse_env_cfg(
+        task_name=task,
+        num_envs=1,
+        use_fabric=True,
+    )
+
+    # add some primitives to the env
+    env_cfg.scene.cube2 = RigidObjectCfg(
+        prim_path="{ENV_REGEX_NS}/Cube2",
+        init_state=RigidObjectCfg.InitialStateCfg(
+            pos=[0.7, 0, 0.155], rot=[1, 0, 0, 0]
+        ),
+        spawn=UsdFileCfg(
+            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd",
+            scale=(0.8, 0.8, 0.8),
+            rigid_props=RigidBodyPropertiesCfg(
+                solver_position_iteration_count=16,
+                solver_velocity_iteration_count=1,
+                max_angular_velocity=1000.0,
+                max_linear_velocity=1000.0,
+                max_depenetration_velocity=5.0,
+                disable_gravity=False,
+            ),
+        ),
+    )
+
+    env_cfg.scene.cube3 = RigidObjectCfg(
+        prim_path="{ENV_REGEX_NS}/Capsule1",
+        init_state=RigidObjectCfg.InitialStateCfg(
+            pos=[0.6, 0, 0.055], rot=[1, 0, 0, 0]
+        ),
+        spawn=CapsuleCfg(
+            radius=0.03,
+            height=0.1,
+            axis="Y",
+            rigid_props=sim_utils.RigidBodyPropertiesCfg(),
+            mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
+            collision_props=sim_utils.CollisionPropertiesCfg(),
+            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.1, 0.1, 1.0)),
+        ),
+    )
+
+    # # you should probably set use_relative_mode to False when using meta quest 3 input
+    # # but currently it does not work (seems to be a problem of coordiante system alignment)
+    # env_cfg.actions.arm_action = DifferentialInverseKinematicsActionCfg(
+    #     asset_name="robot",
+    #     joint_names=["panda_joint.*"],
+    #     body_name="panda_hand",
+    #     controller=DifferentialIKControllerCfg(
+    #         command_type="pose", use_relative_mode=False, ik_method="dls"
+    #     ),
+    #     scale=1.0,
+    #     body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(
+    #         pos=[0.0, 0.0, 0.107]
+    #     ),
+    # )
+
+    # modify configuration
+    env_cfg.terminations.time_out = None
+
+    # create environment
+    env = gym.make(task, cfg=env_cfg)
+    # print(env.sim)
+
+    # check environment name (for reach , we don't allow the gripper)
+    if "Reach" in task:
+        carb.log_warn(
+            f"The environment '{task}' does not support gripper control. The device command will be ignored."
+        )
+
+    # create controller
+    sensitivity = 10
+    teleop_interface = Se3Keyboard(
+        pos_sensitivity=0.005 * sensitivity,
+        rot_sensitivity=0.005 * sensitivity,
+    )
+
+    # add teleoperation key for env reset
+    teleop_interface.add_callback("L", env.reset)
+    # print helper for keyboard
+    print(teleop_interface)
+
+    # reset environment
+    env.reset()
+    teleop_interface.reset()
+
+    print("simulation context:", env.sim)
+    print("stage:", env.sim.stage)
+
+    if env.sim is not None and env.sim.stage is not None:
+        print("parsing usd stage...")
+        publisher = IsaacSimPublisher(host="192.168.0.134", stage=env.sim.stage)
+        # publisher = IsaacSimPublisher(host="127.0.0.1", stage=env.sim.stage)
+
+    # meta_quest3 = MetaQuest3("ALRMetaQuest3")
+
+    # simulate environment
+    while simulation_app.is_running():
+        # run everything in inference mode
+        with torch.inference_mode():
+            # # get meta quest 3 input (does not work...)
+            # input_data = meta_quest3.get_input_data()
+            # if input_data is None:
+            #     continue
+
+            # input_pos = input_data["right"]["pos"]
+            # input_pos[2] -= 0.1
+            # input_rot = input_data["right"]["rot"]
+            # input_rot = Rotation.from_quat(input_rot).as_quat()
+            # input_gripper = input_data["right"]["index_trigger"]
+
+            # input_pos = [0.4, 0.0, 0.2]
+            # input_rot = Rotation.from_euler("XYZ", [0, 180, 0], degrees=True).as_quat()
+
+            # get keyboard command
+            delta_pose, gripper_command = teleop_interface.advance()
+
+            # # get command from meta quest 3
+            # delta_pose = np.array(
+            #     [
+            #         input_pos[0],
+            #         input_pos[1],
+            #         input_pos[2],
+            #         input_rot[3],
+            #         input_rot[0],
+            #         input_rot[1],
+            #         input_rot[2],
+            #     ]
+            # )
+            # print(delta_pose)
+            # gripper_command = input_gripper
+
+            delta_pose = delta_pose.astype("float32")
+            # convert to torch
+            delta_pose = torch.tensor(delta_pose, device=env.unwrapped.device).repeat(
+                env.unwrapped.num_envs, 1
+            )
+            # pre-process actions
+            actions = pre_process_actions(task, delta_pose, gripper_command)
+            # apply actions
+            env.step(actions)
+
+    # close the simulator
+    env.close()
+
+
+if __name__ == "__main__":
+    main()
+    simulation_app.close()