Bug Fixes, Add Isaac Sim 4.5 support for examples

Author: balakumar-s

CHANGELOG.md

@@ -10,6 +10,20 @@ its affiliates is strictly prohibited.
 -->
 # Changelog
 
+## Version 0.7.7
+
+### New Features
+- Add cpu support for types.math.Pose
+- Add cdist fallback for robot segmentation when triton is not available. Requires half the memory.
+
+### BugFixes & Misc.
+- Fix bug in LBFGS where buffers were not reinitialized upon change in history.
+- Isaac Sim 4.5 support for examples. All but one example works now. ``load_all_robots.py`` does
+not work correctly.
+- Fix bug in jerk gradient calculation. Improves convergence in trajectory optimization. Multi-arm
+reacher is slightly improved (see isaac sim example).
+
+
 ## Version 0.7.6
 
 ### Changes in Default Behavior

docker/start_dev_docker.sh

@@ -41,6 +41,7 @@ if [ $input_arg == "x86" ]; then
     --env DISPLAY=unix$DISPLAY \
     --volume /tmp/.X11-unix:/tmp/.X11-unix \
     --volume /dev:/dev \
+    --ipc=host \
     curobo_docker:user_$input_arg
 
 elif [ $input_arg == "aarch64" ]; then
@@ -60,7 +61,7 @@ elif [ $input_arg == "aarch64" ]; then
     curobo_docker:user_$input_arg
 
 elif [[ $input_arg == *isaac_sim* ]] ; then
-   echo "Isaac Sim Dev Docker is not supported" 
+   echo "Isaac Sim Dev Docker is not supported"
 
    mkdir -p ~/docker/isaac-sim ~/docker/isaac-sim/cache/kit \
     ~/docker/isaac-sim/cache/ov \
@@ -70,7 +71,7 @@ elif [[ $input_arg == *isaac_sim* ]] ; then
     ~/docker/isaac-sim/logs \
     ~/docker/isaac-sim/data \
     ~/docker/isaac-sim/documents
- 
+
 
 
    docker run --name container_$input_arg -it --gpus all -e "ACCEPT_EULA=Y" --rm --network=host \
@@ -89,7 +90,7 @@ elif [[ $input_arg == *isaac_sim* ]] ; then
         --volume /dev:/dev \
         --mount type=bind,src=/home/$USER/code,target=/home/$USER/code \
         curobo_docker:user_$input_arg
-    
+
 
 else
     echo "Unknown docker"

examples/collision_check_example.py

@@ -8,9 +8,7 @@
 # without an express license agreement from NVIDIA CORPORATION or
 # its affiliates is strictly prohibited.
 #
-""" Example computing collisions using curobo
-
-"""
+"""Example computing collisions using curobo"""
 # Third Party
 import torch
 

examples/isaac_sim/batch_collision_checker.py

@@ -51,7 +51,13 @@
 import numpy as np
 from helper import add_extensions
 from omni.isaac.core import World
-from omni.isaac.core.materials import OmniPBR
+
+try:
+    from omni.isaac.core.materials import OmniPBR
+except ImportError:
+    from isaacsim.core.api.materials import OmniPBR
+
+
 from omni.isaac.core.objects import sphere
 
 # CuRobo

examples/isaac_sim/batch_motion_gen_reacher.py

@@ -126,9 +126,11 @@ def main():
             "/World/world_" + str(i) + "/",
             robot_name="robot_" + str(i),
             position=pose.position[0].cpu().numpy(),
+            initialize_world=False,
         )
         robot_list.append(r[0])
     setup_curobo_logger("warn")
+    my_world.initialize_physics()
 
     # warmup curobo instance
 
@@ -195,9 +197,10 @@ def main():
             continue
         step_index = my_world.current_time_step_index
 
-        if step_index <= 2:
-            my_world.reset()
+        if step_index <= 10:
+            # my_world.reset()
             for robot in robot_list:
+                robot._articulation_view.initialize()
                 idx_list = [robot.get_dof_index(x) for x in j_names]
                 robot.set_joint_positions(default_config, idx_list)
 
@@ -223,6 +226,8 @@ def main():
             past_goal = ik_goal.clone()
         sim_js_names = robot_list[0].dof_names
         sim_js = robot_list[0].get_joints_state()
+        if sim_js is None:
+            continue
         full_js = JointState(
             position=tensor_args.to_device(sim_js.positions).view(1, -1),
             velocity=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,

examples/isaac_sim/collision_checker.py

@@ -54,7 +54,11 @@
 import numpy as np
 from helper import add_extensions
 from omni.isaac.core import World
-from omni.isaac.core.materials import OmniPBR
+
+try:
+    from omni.isaac.core.materials import OmniPBR
+except ImportError:
+    from isaacsim.core.api.materials import OmniPBR
 from omni.isaac.core.objects import sphere
 
 # CuRobo
@@ -73,7 +77,10 @@
 
 def draw_line(start, gradient):
     # Third Party
-    from omni.isaac.debug_draw import _debug_draw
+    try:
+        from omni.isaac.debug_draw import _debug_draw
+    except ImportError:
+        from isaacsim.util.debug_draw import _debug_draw
 
     draw = _debug_draw.acquire_debug_draw_interface()
     # if draw.get_num_points() > 0:
@@ -194,7 +201,10 @@ def main():
 
         else:
             # Third Party
-            from omni.isaac.debug_draw import _debug_draw
+            try:
+                from omni.isaac.debug_draw import _debug_draw
+            except ImportError:
+                from isaacsim.util.debug_draw import _debug_draw
 
             draw = _debug_draw.acquire_debug_draw_interface()
             # if draw.get_num_points() > 0:

examples/isaac_sim/constrained_reacher.py

@@ -58,7 +58,11 @@
 # Third Party
 import carb
 from omni.isaac.core import World
-from omni.isaac.core.materials import OmniGlass, OmniPBR
+
+try:
+    from omni.isaac.core.materials import OmniGlass, OmniPBR
+except ImportError:
+    from isaacsim.core.api.materials import OmniGlass, OmniPBR
 from omni.isaac.core.objects import cuboid, sphere
 from omni.isaac.core.utils.types import ArticulationAction
 
@@ -207,8 +211,9 @@
             continue
         step_index = my_world.current_time_step_index
 
-        if step_index <= 2:
-            my_world.reset()
+        if step_index <= 10:
+            # my_world.reset()
+            robot._articulation_view.initialize()
             idx_list = [robot.get_dof_index(x) for x in j_names]
             robot.set_joint_positions(default_config, idx_list)
 
@@ -218,7 +223,7 @@
 
         if False and step_index % 50 == 0.0:  # No obstacle update
             obstacles = usd_help.get_obstacles_from_stage(
-                # only_paths=[obstacles_path],
+                only_paths=["/World"],
                 reference_prim_path=robot_prim_path,
                 ignore_substring=[
                     robot_prim_path,

examples/isaac_sim/helper.py

@@ -16,7 +16,6 @@
 import numpy as np
 from matplotlib import cm
 from omni.isaac.core import World
-from omni.isaac.core.materials import OmniPBR
 from omni.isaac.core.objects import cuboid
 from omni.isaac.core.robots import Robot
 from pxr import UsdPhysics
@@ -26,14 +25,28 @@
 from curobo.util.usd_helper import set_prim_transform
 
 ISAAC_SIM_23 = False
+ISAAC_SIM_45 = False
 try:
     # Third Party
     from omni.isaac.urdf import _urdf  # isaacsim 2022.2
 except ImportError:
     # Third Party
-    from omni.importer.urdf import _urdf  # isaac sim 2023.1 or above
+    try:
+        from omni.importer.urdf import _urdf  # isaac sim 2023.1 or above
+    except ImportError:
+        from isaacsim.asset.importer.urdf import _urdf  # isaac sim 4.5+
 
+        ISAAC_SIM_45 = True
     ISAAC_SIM_23 = True
+
+try:
+    # import for older isaacsim installations
+    from omni.isaac.core.materials import OmniPBR
+except ImportError:
+    # import for isaac sim 4.5+
+    from isaacsim.core.api.materials import OmniPBR
+
+
 # Standard Library
 from typing import Optional
 
@@ -67,6 +80,7 @@ def add_robot_to_scene(
     subroot: str = "",
     robot_name: str = "robot",
     position: np.array = np.array([0, 0, 0]),
+    initialize_world: bool = True,
 ):
 
     urdf_interface = _urdf.acquire_urdf_interface()
@@ -91,18 +105,57 @@ def add_robot_to_scene(
         and robot_config["kinematics"]["external_asset_path"] is not None
     ):
         asset_path = robot_config["kinematics"]["external_asset_path"]
+
+    # urdf_path:
+    # meshes_path:
+    # meshes path should be a subset of urdf_path
     full_path = join_path(asset_path, robot_config["kinematics"]["urdf_path"])
+    # full path contains the path to urdf
+    # Get meshes path
     robot_path = get_path_of_dir(full_path)
     filename = get_filename(full_path)
-    imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)
-    dest_path = subroot
-    robot_path = urdf_interface.import_robot(
-        robot_path,
-        filename,
-        imported_robot,
-        import_config,
-        dest_path,
-    )
+    if ISAAC_SIM_45:
+        from isaacsim.core.utils.extensions import get_extension_path_from_name
+        import omni.kit.commands
+        import omni.usd
+
+        # Retrieve the path of the URDF file from the extension
+        extension_path = get_extension_path_from_name("isaacsim.asset.importer.urdf")
+        root_path = robot_path
+        file_name = filename
+
+        # Parse the robot's URDF file to generate a robot model
+
+        dest_path = join_path(
+            root_path, get_filename(file_name, remove_extension=True) + "_temp.usd"
+        )
+
+        result, robot_path = omni.kit.commands.execute(
+            "URDFParseAndImportFile",
+            urdf_path="{}/{}".format(root_path, file_name),
+            import_config=import_config,
+            dest_path=dest_path,
+        )
+        prim_path = omni.usd.get_stage_next_free_path(
+            my_world.scene.stage,
+            str(my_world.scene.stage.GetDefaultPrim().GetPath()) + robot_path,
+            False,
+        )
+        robot_prim = my_world.scene.stage.OverridePrim(prim_path)
+        robot_prim.GetReferences().AddReference(dest_path)
+        robot_path = prim_path
+    else:
+
+        imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)
+        dest_path = subroot
+
+        robot_path = urdf_interface.import_robot(
+            robot_path,
+            filename,
+            imported_robot,
+            import_config,
+            dest_path,
+        )
 
     base_link_name = robot_config["kinematics"]["base_link"]
 
@@ -117,6 +170,11 @@ def add_robot_to_scene(
     set_prim_transform(linkp, [position[0], position[1], position[2], 1, 0, 0, 0])
 
     robot = my_world.scene.add(robot_p)
+    if initialize_world:
+        if ISAAC_SIM_45:
+            my_world.initialize_physics()
+            robot.initialize()
+
     return robot, robot_path
 
 

examples/isaac_sim/ik_reachability.py

@@ -112,7 +112,10 @@ def get_pose_grid(n_x, n_y, n_z, max_x, max_y, max_z):
 
 def draw_points(pose, success):
     # Third Party
-    from omni.isaac.debug_draw import _debug_draw
+    try:
+        from omni.isaac.debug_draw import _debug_draw
+    except ImportError:
+        from isaacsim.util.debug_draw import _debug_draw
 
     draw = _debug_draw.acquire_debug_draw_interface()
     N = 100
@@ -236,9 +239,8 @@ def main():
             continue
 
         step_index = my_world.current_time_step_index
-        # print(step_index)
-        if step_index <= 2:
-            my_world.reset()
+        if step_index <= 10:
+            # my_world.reset()
             idx_list = [robot.get_dof_index(x) for x in j_names]
             robot.set_joint_positions(default_config, idx_list)
 
@@ -251,7 +253,7 @@ def main():
         if step_index == 50 or step_index % 500 == 0.0:  # and cmd_plan is None:
             print("Updating world, reading w.r.t.", robot_prim_path)
             obstacles = usd_help.get_obstacles_from_stage(
-                # only_paths=[obstacles_path],
+                only_paths=["/World"],
                 reference_prim_path=robot_prim_path,
                 ignore_substring=[
                     robot_prim_path,