Update isaac_sim

multiverse/modules/multiverse_connectors/bin/multiverse_view.sh

@@ -35,6 +35,17 @@ MULTIVERSE_PATH=$(dirname $(dirname "$0"))
 # Split the input string into an array of variables
 read -ra vars <<< "$@"
 
+for virtualenvwrapper in $(which virtualenvwrapper.sh) /usr/share/virtualenvwrapper/virtualenvwrapper.sh /usr/local/bin/virtualenvwrapper.sh /home/$USER/.local/bin/virtualenvwrapper.sh; do
+    if [ -f $virtualenvwrapper ]; then
+        . $virtualenvwrapper
+        break
+    fi
+done
+if [ ! -f $virtualenvwrapper ]; then
+    echo "virtualenvwrapper.sh not found"
+    exit 1
+fi
+
 # Print each variable
 for var in "${vars[@]}"; do
     echo "$var"
@@ -51,10 +62,13 @@ for var in "${vars[@]}"; do
 
     # Check if ROS2 or ROS1 exists
     if [ -f "/opt/ros/foxy/setup.bash" ]; then
+        workon multiverse3.8
         source /opt/ros/foxy/setup.bash && source $MULTIVERSE_PATH/../multiverse_ws2/install/setup.bash
     elif [ -f "/opt/ros/humble/setup.bash" ]; then
+        workon multiverse
         source /opt/ros/humble/setup.bash && source $MULTIVERSE_PATH/../multiverse_ws2/install/setup.bash
     elif [ -f "/opt/ros/jazzy/setup.bash" ]; then
+        workon multiverse3.12
         source /opt/ros/jazzy/setup.bash && source $MULTIVERSE_PATH/../multiverse_ws2/install/setup.bash
     else
         echo "Warning: ROS not found."

multiverse/modules/multiverse_connectors/scripts/launch_simulators.py

@@ -127,6 +127,10 @@ def run_simulator(self, compiler_result: subprocess.CompletedProcess, simulation
         if robots_group is not None:
             robots_path = robots_group.group(1)
             cmd += [f"--robots_path={robots_path}"]
+        objects_group = re.search(r"Objects:\s*([^\n]+)", compiler_result.stdout)
+        if objects_group is not None:
+            objects_path = objects_group.group(1)
+            cmd += [f"--objects_path={objects_path}"]
         for config_name, config_data in simulation_data.get("config", {}).items():
             cmd.append(f"--{config_name}={config_data}")
         world_name = simulation_data.get("world", {}).get("name", "world")

multiverse/modules/multiverse_connectors/src/multiverse_simulators/src/isaac_sim_connector/extensions/multiverse_connector/multiverse_connector_python/ui_builder.py

@@ -62,9 +62,9 @@ class MultiverseObject:
 import torch
 import omni.timeline
 import omni.ui as ui
+from isaaclab.sim import SimulationContext
 from isaacsim.gui.components.element_wrappers import CollapsableFrame, TextBlock, StringField, IntField, CheckBox
 from isaacsim.gui.components.ui_utils import get_style
-
 from isaacsim.core.api.world.world import World
 from isaacsim.core.api.scenes.scene import Scene
 from isaacsim.core.api.scenes.scene_registry import SceneRegistry
@@ -133,7 +133,7 @@ def on_stage_event(self, event):
             event (omni.usd.StageEventType): Event Type
         """
         if event.type == int(omni.usd.StageEventType.ASSETS_LOADED):
-            if hasattr(omni.isaac, 'lab') and isinstance(self.world, omni.isaac.lab.sim.simulation_context.SimulationContext) and self._timeline.is_playing():
+            if isinstance(self.world, SimulationContext) and self._timeline.is_playing():
                 self._clean_up()
                 self.build_ui()
                 self._init()
@@ -142,12 +142,12 @@ def on_stage_event(self, event):
                 # self.build_ui()
                 self._init()
         elif event.type == int(omni.usd.StageEventType.ANIMATION_START_PLAY):
-            if hasattr(omni.isaac, 'lab') and isinstance(self.world, omni.isaac.lab.sim.simulation_context.SimulationContext):
+            if isinstance(self.world, SimulationContext):
                 # self._init()
                 pass # TODO: Make pause and unpause in IsaacLab work
         elif event.type == int(omni.usd.StageEventType.SIMULATION_STOP_PLAY):
             # Ignore pause events
-            if hasattr(omni.isaac, 'lab') and isinstance(self.world, omni.isaac.lab.sim.simulation_context.SimulationContext) or self._timeline.is_stopped():
+            if isinstance(self.world, SimulationContext) or self._timeline.is_stopped():
                 self._clean_up(clean_ui=False)
 
     def cleanup(self):
@@ -576,7 +576,7 @@ def bind_response_meta_data() -> None:
                             data = response_meta_data[send_receive][object_name][attribute]
                             if any(x is None for x in data):
                                 continue
-                            if hasattr(omni.isaac, 'lab') and isinstance(self.world, omni.isaac.lab.sim.simulation_context.SimulationContext):
+                            if isinstance(self.world, SimulationContext):
                                 data = torch.tensor(data).to("cuda").float()
                             if attribute == "position":
                                 bodies_positions[view_name][object_idx] = data[:3]
@@ -815,7 +815,7 @@ def bind_receive_data() -> None:
                     cmd_joint_positions = [cmd_joint_value[0] for cmd_joint_value in cmd_joint_values]
                     cmd_joint_velocities = [cmd_joint_value[1] for cmd_joint_value in cmd_joint_values]
                     cmd_joint_efforts = [cmd_joint_value[2] for cmd_joint_value in cmd_joint_values]
-                    if hasattr(omni.isaac, 'lab') and isinstance(self.world, omni.isaac.lab.sim.simulation_context.SimulationContext):
+                    if isinstance(self.world, SimulationContext):
                         cmd_joint_positions = torch.tensor(cmd_joint_positions).to("cuda").float()
                         cmd_joint_velocities = torch.tensor(cmd_joint_velocities).to("cuda").float()
                         cmd_joint_efforts = torch.tensor(cmd_joint_efforts).to("cuda").float()
@@ -889,5 +889,4 @@ def scene_registry(self) -> "SceneRegistry":
     def world(self) -> "World":
         if self._world is None:
             self._world = World()
-            self._world.initialize_physics()
         return self._world

multiverse/modules/multiverse_connectors/src/multiverse_simulators/src/isaac_sim_connector/scripts/isaac_sim.py

@@ -11,6 +11,7 @@
         parser = argparse.ArgumentParser(description=f"Run the Isaac Sim Connector")
         parser.add_argument("--file_path", type=str, required=True, help=f"Path to the USD file")
         parser.add_argument("--robots_path", type=str, required=False, help="Paths to the robots' USD files")
+        parser.add_argument("--objects_path", type=str, required=False, help="Paths to the objects' USD files")
         parser.add_argument("--headless", required=False, action='store_true', help="Run in headless mode")
         parser.add_argument("--number_of_envs", type=int, required=False, default=1, help="Number of environments")
         parser.add_argument("--env_spacing", type=float, required=False, default=2.0, help="Environment spacing")
@@ -26,10 +27,10 @@
         args = parser.parse_args()
 
         multiverse_params = str_to_dict(args.multiverse_params)
-        robots_path = args.robots_path if args.robots_path is not None else ""
 
         simulator = MultiverseIsaacSimConnector(world_path=args.file_path,
-                                                robots_path=robots_path,
+                                                robots_path=args.robots_path,
+                                                objects_path=args.objects_path,
                                                 headless=args.headless,
                                                 number_of_envs=args.number_of_envs,
                                                 env_spacing=args.env_spacing,

multiverse/modules/multiverse_connectors/src/multiverse_simulators/src/isaac_sim_connector/src/isaac_sim_connector/isaac_sim_compiler.py

@@ -22,14 +22,21 @@ def build_world(self,
                     objects: Dict[str, Object],
                     references: Dict[str, Dict[str, Any]] = None,
                     multiverse_params: Dict[str, Dict] = None):
-        from pxr import Usd, UsdGeom, UsdPhysics, Gf  # Ask NVIDIA for this shitty importing style
+        from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf  # Ask NVIDIA for this shitty importing style
 
         for entity in list(robots.values()) + list(objects.values()):
             file_ext = os.path.splitext(entity.path)[1]
             entity_usd_dir = os.path.dirname(entity.path)
             entity_usd_path = os.path.join(self.save_dir_path, entity.name + f"{file_ext}")
             shutil.copy(entity.path, entity_usd_path)
             entity.path = entity_usd_path
+
+            if file_ext == ".usda":
+                with open(entity_usd_path, "r") as f:
+                    data = f.read()
+                data = data.replace("@./", f"@{entity_usd_dir}/")
+                with open(entity_usd_path, "w") as f:
+                    f.write(data)
             entity_stage = Usd.Stage.Open(entity_usd_path)
 
             if "body" in entity.apply:
@@ -39,12 +46,17 @@ def build_world(self,
                     if xform_prim.GetName() == entity.name and not xform_prim.GetParent().IsPseudoRoot():
                         continue
                     xform = UsdGeom.Xform(xform_prim)
+                    pose = xform.GetLocalTransformation()
+                    pos = pose.ExtractTranslation()
+                    pos = [*pos]
+                    quat = pose.ExtractRotationQuat()
+                    quat = [*quat.GetImaginary(), quat.GetReal()]
                     xform.ClearXformOpOrder()
-                    pos = body_apply[xform_prim.GetName()].get("pos", [0, 0, 0])
-                    quat = body_apply[xform_prim.GetName()].get("quat", [1, 0, 0, 0])
+                    pos = body_apply[xform_prim.GetName()].get("pos", pos)
+                    quat = body_apply[xform_prim.GetName()].get("quat", quat)
 
-                    pos = numpy.asarray(pos)
-                    quat = numpy.asarray(quat)
+                    pos = numpy.asfarray(pos)
+                    quat = numpy.asfarray(quat)
                     mat = Gf.Matrix4d()
                     mat.SetTranslateOnly(Gf.Vec3d(*pos))
                     mat.SetRotateOnly(Gf.Quatd(quat[0], Gf.Vec3d(*quat[1:])))
@@ -62,13 +74,6 @@ def build_world(self,
 
             entity_stage.GetRootLayer().Save()
 
-            if file_ext == ".usda":
-                with open(entity_usd_path, "r") as f:
-                    data = f.read()
-                data = data.replace("@./", f"@{entity_usd_dir}/")
-                with open(entity_usd_path, "w") as f:
-                    f.write(data)
-
         robots_path = os.path.join(self.save_dir_path, os.path.basename(self.save_file_path).split(".")[0] + "_robots.usda")
         robots_stage = Usd.Stage.CreateNew(robots_path)
         if len(robots) == 1:
@@ -80,6 +85,25 @@ def build_world(self,
 
         print(f"Robots: {robots_path}")
 
+        objects_path = os.path.join(self.save_dir_path, os.path.basename(self.save_file_path).split(".")[0] + "_objects.usda")
+        objects_stage = Usd.Stage.CreateNew(objects_path)
+        objects_prim_path = Sdf.Path("/Objects")
+        objects_xform = UsdGeom.Xform.Define(objects_stage, objects_prim_path)
+        objects_prim = objects_xform.GetPrim()
+        objects_stage.SetDefaultPrim(objects_prim)
+
+        for object in objects.values():
+            object_prim_path = objects_prim_path.AppendChild(object.name)
+            object_xform = UsdGeom.Xform.Define(objects_stage, object_prim_path)
+            object_prim = object_xform.GetPrim()
+            object_stage = Usd.Stage.Open(object.path)
+            object_prim.GetReferences().AddReference(object_stage.GetRootLayer().identifier, object_stage.GetDefaultPrim().GetPath())
+
+        objects_stage.Flatten()
+        objects_stage.Export(objects_path)
+
+        print(f"Objects: {objects_path}")
+
         file_ext = os.path.splitext(self.world_path)[1]
         if file_ext == ".usda":
             with open(self.save_file_path, "r") as f:

multiverse/modules/multiverse_connectors/src/multiverse_simulators/src/isaac_sim_connector/src/isaac_sim_connector/isaac_sim_connector.py

@@ -6,7 +6,6 @@
 
 import numpy
 import torch
-from isaacsim import SimulationApp
 from isaaclab.app import AppLauncher
 
 from multiverse_simulator import MultiverseSimulator, MultiverseRenderer, MultiverseViewer
@@ -26,7 +25,7 @@ def close(self):
         self.simulation_app.close()
 
     @property
-    def simulation_app(self) -> SimulationApp:
+    def simulation_app(self) -> "SimulationApp":
         return self._simulation_app
 
 
@@ -37,6 +36,7 @@ def __init__(
             self,
             world_path: str,
             robots_path: Optional[str] = None,
+            objects_path: Optional[str] = None,
             number_of_envs: int = 1,
             env_spacing: float = 2.0,
             viewer: Optional[MultiverseViewer] = None,
@@ -105,7 +105,7 @@ class WorldCfg(WorldCfg):
                 ).replace(prim_path="{ENV_REGEX_NS}/World")
 
         @configclass
-        class SceneCfg(WorldCfg):
+        class SceneWithRobotsCfg(WorldCfg):
             pass
 
         if robots_path is not None:
@@ -135,7 +135,7 @@ class SceneCfg(WorldCfg):
                     robots_joint_pos[prim.GetName()] = joint_pos
 
                 @configclass
-                class SceneCfg(WorldCfg):
+                class SceneWithRobotsCfg(WorldCfg):
                     # robots articulation
                     robots = ArticulationCfg(
                         spawn=sim_utils.UsdFileCfg(
@@ -163,6 +163,49 @@ class SceneCfg(WorldCfg):
                         actuators={},
                     ).replace(prim_path="{ENV_REGEX_NS}/Robot")
 
+        @configclass
+        class SceneCfg(SceneWithRobotsCfg):
+            pass
+
+        if objects_path is not None:
+            objects_stage = Usd.Stage.Open(objects_path)
+            objects_prim = objects_stage.GetDefaultPrim()
+            if objects_prim.IsValid():
+                objects_xform = UsdGeom.Xform(objects_prim)
+                objects_pos = objects_xform.GetLocalTransformation().ExtractTranslation()
+                objects_quat = objects_xform.GetLocalTransformation().ExtractRotation().GetQuat()
+                objects_joint_pos = {}
+                for prim in [prim for prim in objects_stage.TraverseAll() if prim.IsA(UsdPhysics.RevoluteJoint) or prim.IsA(UsdPhysics.PrismaticJoint)]:
+                    joint = UsdPhysics.RevoluteJoint(prim) if prim.IsA(UsdPhysics.RevoluteJoint) else UsdPhysics.PrismaticJoint(prim)
+                    lower_limit_joint = joint.GetLowerLimitAttr().Get()
+                    upper_limit_joint = joint.GetUpperLimitAttr().Get()
+                    joint_pos = 0.0
+                    if prim.HasAPI(UsdPhysics.DriveAPI):
+                        drive_api = UsdPhysics.DriveAPI(prim, "angular" if prim.IsA(UsdPhysics.RevoluteJoint) else "linear")
+                        joint_pos = drive_api.GetTargetPositionAttr().Get()
+                    if joint_pos < lower_limit_joint or joint_pos > upper_limit_joint:
+                        joint_pos = (lower_limit_joint + upper_limit_joint) / 2.0
+                    if prim.HasAPI(PhysxSchema.JointStateAPI):
+                        joint_state_api = PhysxSchema.JointStateAPI(prim, "angular" if prim.IsA(UsdPhysics.RevoluteJoint) else "linear")
+                        joint_state_api.GetPositionAttr().Set(joint_pos)
+                    if UsdPhysics.RevoluteJoint(prim):
+                        joint_pos = float(numpy.deg2rad(joint_pos))
+
+                    objects_joint_pos[prim.GetName()] = joint_pos
+
+                @configclass
+                class SceneCfg(SceneWithRobotsCfg):
+                    # objects asset
+                    objects = AssetBaseCfg(
+                        spawn=sim_utils.UsdFileCfg(
+                            usd_path=objects_path,
+                        ),
+                        init_state=AssetBaseCfg.InitialStateCfg(
+                            pos=objects_pos,
+                            rot=(objects_quat.GetReal(), *objects_quat.GetImaginary())
+                        ),
+                    ).replace(prim_path="{ENV_REGEX_NS}/Objects")
+
         simulation_config = sim_utils.SimulationCfg(dt=self.step_size)
         self._simulation_context = SimulationContext(cfg=simulation_config)
         self.simulation_context.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0])