release repository

examples/isaac_sim/batch_collision_checker.py

@@ -0,0 +1,171 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.util_file import get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Third Party
+import carb
+import numpy as np
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+
+def main():
+    usd_help = UsdHelper()
+    act_distance = 0.2
+
+    n_envs = 2
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+    # Make a target to follow
+    target_list = []
+    target_material_list = []
+    offset_x = 3.5
+    radius = 0.1
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+
+    for i in range(n_envs):
+        if i > 0:
+            pose.position[0, 0] += offset_x
+        usd_help.add_subroot("/World", "/World/world_" + str(i), pose)
+
+        target_material = OmniPBR("/World/looks/t_" + str(i), color=np.array([0, 1, 0]))
+
+        target = sphere.VisualSphere(
+            "/World/world_" + str(i) + "/target",
+            position=np.array([0.5, 0, 0.5]) + pose.position[0].cpu().numpy(),
+            orientation=np.array([1, 0, 0, 0]),
+            radius=radius,
+            visual_material=target_material,
+        )
+        target_list.append(target)
+        target_material_list.append(target_material)
+
+    setup_curobo_logger("warn")
+
+    # warmup curobo instance
+
+    tensor_args = TensorDeviceType()
+    robot_file = "franka.yml"
+    world_file = ["collision_thin_walls.yml", "collision_test.yml"]
+    world_cfg_list = []
+    for i in range(n_envs):
+        world_cfg = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), world_file[i]))
+        )  # .get_mesh_world()
+        world_cfg.objects[0].pose[2] += 0.1
+        world_cfg.randomize_color(r=[0.2, 0.3], b=[0.0, 0.05], g=[0.2, 0.3])
+        usd_help.add_world_to_stage(world_cfg, base_frame="/World/world_" + str(i))
+        world_cfg_list.append(world_cfg)
+    config = RobotWorldConfig.load_from_config(
+        robot_file, world_cfg_list, collision_activation_distance=act_distance
+    )
+    model = RobotWorld(config)
+    i = 0
+    max_distance = 0.5
+    x_sph = torch.zeros((n_envs, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+    env_query_idx = torch.arange(n_envs, device=tensor_args.device, dtype=torch.int32)
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+
+        if step_index < 20:
+            continue
+        sp_buffer = []
+        for k in target_list:
+            sph_position, _ = k.get_local_pose()
+            sp_buffer.append(sph_position)
+
+        x_sph[..., :3] = tensor_args.to_device(sp_buffer).view(n_envs, 1, 1, 3)
+
+        d, d_vec = model.get_collision_vector(x_sph, env_query_idx=env_query_idx)
+
+        d = d.view(-1).cpu()
+
+        for i in range(d.shape[0]):
+            p = d[i].item()
+            p = max(1, p * 5)
+            if d[i].item() == 0.0:
+                target_material_list[i].set_color(np.ravel([0, 1, 0]))
+            elif d[i].item() <= model.contact_distance:
+                target_material_list[i].set_color(np.array([0, 0, p]))
+            elif d[i].item() >= model.contact_distance:
+                target_material_list[i].set_color(np.array([p, 0, 0]))
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/batch_motion_gen_reacher.py

@@ -0,0 +1,311 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+
+def main():
+    usd_help = UsdHelper()
+    act_distance = 0.2
+
+    n_envs = 2
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+    # Make a target to follow
+    target_list = []
+    target_material_list = []
+    offset_y = 2.5
+    radius = 0.1
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    robot_list = []
+
+    for i in range(n_envs):
+        if i > 0:
+            pose.position[0, 1] += offset_y
+        usd_help.add_subroot("/World", "/World/world_" + str(i), pose)
+
+        target = cuboid.VisualCuboid(
+            "/World/world_" + str(i) + "/target",
+            position=np.array([0.5, 0, 0.5]) + pose.position[0].cpu().numpy(),
+            orientation=np.array([0, 1, 0, 0]),
+            color=np.array([1.0, 0, 0]),
+            size=0.05,
+        )
+        target_list.append(target)
+        r = add_robot_to_scene(
+            robot_cfg,
+            my_world,
+            "/World/world_" + str(i) + "/",
+            robot_name="robot_" + str(i),
+            position=pose.position[0].cpu().numpy(),
+        )
+        robot_list.append(r[0])
+    setup_curobo_logger("warn")
+
+    # warmup curobo instance
+
+    tensor_args = TensorDeviceType()
+    robot_file = "franka.yml"
+
+    world_file = ["collision_test.yml", "collision_thin_walls.yml"]
+    world_cfg_list = []
+    for i in range(n_envs):
+        world_cfg = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), world_file[i]))
+        )  # .get_mesh_world()
+        world_cfg.objects[0].pose[2] -= 0.02
+        world_cfg.randomize_color(r=[0.2, 0.3], b=[0.0, 0.05], g=[0.2, 0.3])
+        usd_help.add_world_to_stage(world_cfg, base_frame="/World/world_" + str(i))
+        world_cfg_list.append(world_cfg)
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg_list,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_cache={"obb": 6, "mesh": 6},
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        fixed_iters_trajopt=True,
+        finetune_dt_scale=1.05,
+        grad_trajopt_iters=300,
+        minimize_jerk=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    print("warming up...")
+    motion_gen.warmup(
+        enable_graph=False, warmup_js_trajopt=False, batch=n_envs, batch_env_mode=True
+    )
+
+    config = RobotWorldConfig.load_from_config(
+        robot_file, world_cfg_list, collision_activation_distance=act_distance
+    )
+    model = RobotWorld(config)
+    i = 0
+    max_distance = 0.5
+    x_sph = torch.zeros((n_envs, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+    env_query_idx = torch.arange(n_envs, device=tensor_args.device, dtype=torch.int32)
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+    prev_goal = None
+    cmd_plan = [None, None]
+    art_controllers = [r.get_articulation_controller() for r in robot_list]
+    cmd_idx = 0
+    past_goal = None
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            for robot in robot_list:
+                idx_list = [robot.get_dof_index(x) for x in j_names]
+                robot.set_joint_positions(default_config, idx_list)
+
+                robot._articulation_view.set_max_efforts(
+                    values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+                )
+        if step_index < 20:
+            continue
+        sp_buffer = []
+        sq_buffer = []
+        for k in target_list:
+            sph_position, sph_orientation = k.get_local_pose()
+            sp_buffer.append(sph_position)
+            sq_buffer.append(sph_orientation)
+
+        ik_goal = Pose(
+            position=tensor_args.to_device(sp_buffer),
+            quaternion=tensor_args.to_device(sq_buffer),
+        )
+        if prev_goal is None:
+            prev_goal = ik_goal.clone()
+        if past_goal is None:
+            past_goal = ik_goal.clone()
+        sim_js_names = robot_list[0].dof_names
+        sim_js = robot_list[0].get_joints_state()
+        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,
+            acceleration=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+            joint_names=sim_js_names,
+        )
+        for i in range(1, len(robot_list)):
+            sim_js = robot_list[i].get_joints_state()
+            cu_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,
+                acceleration=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+                jerk=tensor_args.to_device(sim_js.velocities).view(1, -1) * 0.0,
+                joint_names=sim_js_names,
+            )
+            full_js = full_js.stack(cu_js)
+
+        prev_distance = ik_goal.distance(prev_goal)
+        past_distance = ik_goal.distance(past_goal)
+
+        if (
+            (torch.sum(prev_distance[0] > 1e-2) or torch.sum(prev_distance[1] > 1e-2))
+            and (torch.sum(past_distance[0]) == 0.0 and torch.sum(past_distance[1] == 0.0))
+            and torch.norm(full_js.velocity) < 0.01
+            and cmd_plan[0] is None
+            and cmd_plan[1] is None
+        ):
+            full_js = full_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+            result = motion_gen.plan_batch_env(full_js, ik_goal, plan_config.clone())
+
+            prev_goal.copy_(ik_goal)
+            trajs = result.get_paths()
+            for s in range(len(result.success)):
+                if result.success[s]:
+                    cmd_plan[s] = motion_gen.get_full_js(trajs[s])
+                    # cmd_plan = result.get_interpolated_plan()
+                    # cmd_plan = motion_gen.get_full_js(cmd_plan)
+                    # get only joint names that are in both:
+                    idx_list = []
+                    common_js_names = []
+                    for x in sim_js_names:
+                        if x in cmd_plan[s].joint_names:
+                            idx_list.append(robot_list[s].get_dof_index(x))
+                            common_js_names.append(x)
+
+                    cmd_plan[s] = cmd_plan[s].get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+        # print(cmd_plan)
+
+        for s in range(len(cmd_plan)):
+            if cmd_plan[s] is not None and cmd_idx < len(cmd_plan[s].position):
+                cmd_state = cmd_plan[s][cmd_idx]
+
+                # get full dof state
+                art_action = ArticulationAction(
+                    cmd_state.position.cpu().numpy(),
+                    cmd_state.velocity.cpu().numpy(),
+                    joint_indices=idx_list,
+                )
+                # print(cmd_state.position)
+                # set desired joint angles obtained from IK:
+                art_controllers[s].apply_action(art_action)
+            else:
+                cmd_plan[s] = None
+        cmd_idx += 1
+        past_goal.copy_(ik_goal)
+
+        for _ in range(2):
+            my_world.step(render=False)
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/collision_checker.py

@@ -0,0 +1,212 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.util_file import get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+import numpy as np
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--nvblox", action="store_true", help="When True, enables headless mode", default=False
+)
+args = parser.parse_args()
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(1, 0, 0, 0.8)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+def main():
+    usd_help = UsdHelper()
+    act_distance = 0.4
+    ignore_list = ["/World/target", "/World/defaultGroundPlane"]
+
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    radius = 0.1
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+
+    target = sphere.VisualSphere(
+        "/World/target",
+        position=np.array([0.5, 0, 1.0]) + pose.position[0].cpu().numpy(),
+        orientation=np.array([1, 0, 0, 0]),
+        radius=radius,
+        visual_material=target_material,
+    )
+
+    setup_curobo_logger("warn")
+
+    # warmup curobo instance
+
+    tensor_args = TensorDeviceType()
+    robot_file = "franka.yml"
+    world_file = ["collision_thin_walls.yml", "collision_test.yml"][-1]
+    collision_checker_type = CollisionCheckerType.MESH
+    world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+    world_cfg.objects[0].pose[2] += 0.2
+    vis_world_cfg = world_cfg
+
+    if args.nvblox:
+        world_file = "collision_nvblox.yml"
+        collision_checker_type = CollisionCheckerType.BLOX
+        world_cfg = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), world_file))
+        )
+        world_cfg.objects[0].pose[2] += 0.4
+        ignore_list.append(world_cfg.objects[0].name)
+        vis_world_cfg = world_cfg.get_mesh_world()
+        # world_cfg = vis_world_cfg
+
+    usd_help.add_world_to_stage(vis_world_cfg, base_frame="/World")
+    config = RobotWorldConfig.load_from_config(
+        robot_file,
+        world_cfg,
+        collision_activation_distance=act_distance,
+        collision_checker_type=collision_checker_type,
+    )
+    model = RobotWorld(config)
+    i = 0
+    x_sph = torch.zeros((1, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+
+        if step_index < 20:
+            continue
+        if step_index % 1000 == 0.0:
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                reference_prim_path="/World",
+                ignore_substring=ignore_list,
+            ).get_collision_check_world()
+
+            model.update_world(obstacles)
+            print("Updated World")
+
+        sp_buffer = []
+        sph_position, _ = target.get_local_pose()
+
+        x_sph[..., :3] = tensor_args.to_device(sph_position).view(1, 1, 1, 3)
+
+        d, d_vec = model.get_collision_vector(x_sph)
+
+        d = d.view(-1).cpu()
+
+        p = d.item()
+        p = max(1, p * 5)
+        if d.item() != 0.0:
+            draw_line(sph_position, d_vec[..., :3].view(3).cpu().numpy())
+            print(d, d_vec)
+
+        else:
+            # Third Party
+            from omni.isaac.debug_draw import _debug_draw
+
+            draw = _debug_draw.acquire_debug_draw_interface()
+            # if draw.get_num_points() > 0:
+            draw.clear_lines()
+        if d.item() == 0.0:
+            target_material.set_color(np.ravel([0, 1, 0]))
+        elif d.item() <= model.contact_distance:
+            target_material.set_color(np.array([0, 0, p]))
+        elif d.item() >= model.contact_distance:
+            target_material.set_color(np.array([p, 0, 0]))
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/helper.py

@@ -0,0 +1,134 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+from typing import Dict, List
+
+# Third Party
+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
+
+try:
+    # Third Party
+    from omni.isaac.urdf import _urdf  # isaacsim 2022.2
+except ImportError:
+    from omni.importer.urdf import _urdf  # isaac sim 2023.1
+# CuRobo
+from curobo.util_file import get_assets_path, get_filename, get_path_of_dir, join_path
+
+
+############################################################
+def add_robot_to_scene(
+    robot_config: Dict,
+    my_world: World,
+    load_from_usd: bool = False,
+    subroot: str = "",
+    robot_name: str = "robot",
+    position: np.array = np.array([0, 0, 0]),
+):
+    urdf_interface = _urdf.acquire_urdf_interface()
+
+    import_config = _urdf.ImportConfig()
+    import_config.merge_fixed_joints = False
+    import_config.convex_decomp = False
+    import_config.import_inertia_tensor = True
+    import_config.fix_base = True
+    import_config.make_default_prim = False
+    import_config.self_collision = False
+    import_config.create_physics_scene = True
+    import_config.import_inertia_tensor = False
+    import_config.default_drive_strength = 20000
+    import_config.default_position_drive_damping = 500
+    import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
+    import_config.distance_scale = 1
+    import_config.density = 0.0
+    full_path = join_path(get_assets_path(), robot_config["kinematics"]["urdf_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,
+    )
+
+    # prim_path = omni.usd.get_stage_next_free_path(
+    # my_world.scene.stage, str(my_world.scene.stage.GetDefaultPrim().GetPath()) + robot_path, False)
+    # print(prim_path)
+    # robot_prim = my_world.scene.stage.OverridePrim(prim_path)
+    # robot_prim.GetReferences().AddReference(dest_path)
+
+    robot = my_world.scene.add(
+        Robot(
+            prim_path=robot_path,
+            name=robot_name,
+            position=position,
+        )
+    )
+
+    return robot, robot_path
+
+
+class VoxelManager:
+    def __init__(
+        self,
+        num_voxels: int = 5000,
+        size: float = 0.02,
+        color: List[float] = [1, 1, 1],
+        prefix_path: str = "/World/curobo/voxel_",
+        material_path: str = "/World/looks/v_",
+    ) -> None:
+        self.cuboid_list = []
+        self.cuboid_material_list = []
+        self.disable_idx = num_voxels
+        for i in range(num_voxels):
+            target_material = OmniPBR("/World/looks/v_" + str(i), color=np.ravel(color))
+
+            cube = cuboid.VisualCuboid(
+                prefix_path + str(i),
+                position=np.array([0, 0, -10]),
+                orientation=np.array([1, 0, 0, 0]),
+                size=size,
+                visual_material=target_material,
+            )
+            self.cuboid_list.append(cube)
+            self.cuboid_material_list.append(target_material)
+            cube.set_visibility(True)
+
+    def update_voxels(self, voxel_position: np.ndarray, color_axis: int = 0):
+        max_index = min(voxel_position.shape[0], len(self.cuboid_list))
+
+        jet = cm.get_cmap("hot")  # .reversed()
+        z_val = voxel_position[:, 0]
+
+        jet_colors = jet(z_val)
+
+        for i in range(max_index):
+            self.cuboid_list[i].set_visibility(True)
+
+            self.cuboid_list[i].set_local_pose(translation=voxel_position[i])
+            self.cuboid_material_list[i].set_color(jet_colors[i][:3])
+
+        for i in range(max_index, len(self.cuboid_list)):
+            self.cuboid_list[i].set_local_pose(translation=np.ravel([0, 0, -10.0]))
+
+            # self.cuboid_list[i].set_visibility(False)
+
+    def clear(self):
+        for i in range(len(self.cuboid_list)):
+            self.cuboid_list[i].set_local_pose(translation=np.ravel([0, 0, -10.0]))

examples/isaac_sim/ik_reachability.py

@@ -0,0 +1,381 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+    "omni.isaac.urdf",
+]
+[enable_extension(x) for x in ext_list]
+simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+def get_pose_grid(n_x, n_y, n_z, max_x, max_y, max_z):
+    x = np.linspace(-max_x, max_x, n_x)
+    y = np.linspace(-max_y, max_y, n_y)
+    z = np.linspace(0, max_z, n_z)
+    x, y, z = np.meshgrid(x, y, z, indexing="ij")
+
+    position_arr = np.zeros((n_x * n_y * n_z, 3))
+    position_arr[:, 0] = x.flatten()
+    position_arr[:, 1] = y.flatten()
+    position_arr[:, 2] = z.flatten()
+    return position_arr
+
+
+def draw_points(pose, success):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    cpu_pos = pose.position.cpu().numpy()
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        if success[i].item():
+            colors += [(0, 1, 0, 0.25)]
+        else:
+            colors += [(1, 0, 0, 0.25)]
+    sizes = [40.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.002
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=20,
+        self_collision_check=True,
+        self_collision_opt=True,
+        tensor_args=tensor_args,
+        use_cuda_graph=True,
+        collision_checker_type=CollisionCheckerType.MESH,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        # use_fixed_samples=True,
+    )
+    ik_solver = IKSolver(ik_config)
+
+    # get pose grid:
+    position_grid_offset = tensor_args.to_device(get_pose_grid(10, 10, 5, 0.5, 0.5, 0.5))
+
+    # read current ik pose and warmup?
+    fk_state = ik_solver.fk(ik_solver.get_retract_config().view(1, -1))
+    goal_pose = fk_state.ee_pose
+    goal_pose = goal_pose.repeat(position_grid_offset.shape[0])
+    goal_pose.position += position_grid_offset
+
+    result = ik_solver.solve_batch(goal_pose)
+
+    print("Curobo is Ready")
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg, base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        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],
+                reference_prim_path=robot_prim_path,
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ],
+            ).get_collision_check_world()
+            print([x.name for x in obstacles.objects])
+            ik_solver.update_world(obstacles)
+            print("Updated World")
+            carb.log_info("Synced CuRobo world from stage.")
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(ik_solver.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = ik_solver.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        # print(sim_js.velocities)
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_pose.position[:] = ik_goal.position[:] + position_grid_offset
+            goal_pose.quaternion[:] = ik_goal.quaternion[:]
+            result = ik_solver.solve_batch(goal_pose)
+
+            succ = torch.any(result.success)
+            print(
+                "IK completed: Poses: "
+                + str(goal_pose.batch)
+                + " Time(s): "
+                + str(result.solve_time)
+            )
+            # get spheres and flags:
+            draw_points(goal_pose, result.success)
+
+            if succ:
+                # get all solutions:
+
+                cmd_plan = result.js_solution[result.success]
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None and step_index % 20 == 0 and True:
+            cmd_state = cmd_plan[cmd_idx]
+
+            robot.set_joint_positions(cmd_state.position.cpu().numpy(), idx_list)
+
+            # set desired joint angles obtained from IK:
+            # articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+                my_world.step(render=True)
+                robot.set_joint_positions(default_config, idx_list)
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/load_all_robots.py

@@ -0,0 +1,180 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+
+# Standard Library
+import argparse
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import (
+    get_motion_gen_robot_list,
+    get_robot_configs_path,
+    get_robot_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+)
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+args = parser.parse_args()
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+
+def main():
+    list_of_robots = get_motion_gen_robot_list()  # [:2]
+    usd_help = UsdHelper()
+
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    my_world.scene.add_default_ground_plane()
+
+    stage = my_world.stage
+    usd_help.load_stage(stage)
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+    # Make a target to follow
+    offset_y = 2
+    pose = Pose.from_list([0, 0, 0, 1, 0, 0, 0])
+
+    robot_cfg_list = []
+    robot_list = []
+    tensor_args = TensorDeviceType()
+    spheres = []
+
+    for i in range(len(list_of_robots)):
+        if i > 0:
+            pose.position[0, 1] += offset_y
+        if i == int(len(list_of_robots) / 2):
+            pose.position[0, 0] = -offset_y
+            pose.position[0, 1] = 0
+        robot_cfg = load_yaml(join_path(get_robot_configs_path(), list_of_robots[i]))["robot_cfg"]
+        robot_cfg_list.append(robot_cfg)
+        r = add_robot_to_scene(
+            robot_cfg,
+            my_world,
+            "/World/world_" + str(i) + "/",
+            robot_name="/World/world_" + str(i) + "/" "robot_" + str(i),
+            position=pose.position[0].cpu().numpy(),
+        )
+
+        robot_list.append(r[0])
+        robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+
+        kin_model = CudaRobotModel(robot_cfg.kinematics)
+        default_config = kin_model.cspace.retract_config
+
+        sph_list = kin_model.get_robot_as_spheres(default_config)
+        for si, s in enumerate(sph_list[0]):
+            sp = sphere.VisualSphere(
+                prim_path="/curobo/robot_sphere_" + str(i) + "_" + str(si),
+                position=np.ravel(s.position)
+                + pose.position[0].cpu().numpy()
+                + np.ravel([0, 0.5, 0.0]),
+                radius=float(s.radius),
+                color=np.array([0, 0.8, 0.2]),
+            )
+            spheres.append(sp)
+
+    setup_curobo_logger("warn")
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            for ri, robot in enumerate(robot_list):
+                j_names = robot_cfg_list[ri]["kinematics"]["cspace"]["joint_names"]
+                default_config = robot_cfg_list[ri]["kinematics"]["cspace"]["retract_config"]
+
+                idx_list = [robot.get_dof_index(x) for x in j_names]
+                robot.set_joint_positions(default_config, idx_list)
+                robot._articulation_view.set_max_efforts(
+                    values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+                )
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/motion_gen_reacher.py

@@ -0,0 +1,336 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+# simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        fixed_iters_trajopt=True,
+        finetune_dt_scale=1.05,
+        velocity_scale=[0.25, 1, 1, 1, 1.0, 1.0, 1.0, 1.0, 1.0],
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up...")
+    motion_gen.warmup(enable_graph=False, warmup_js_trajopt=False)
+
+    print("Curobo is Ready")
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg, base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index < 2:
+            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)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        if step_index == 50 or step_index % 1000 == 0.0:
+            print("Updating world, reading w.r.t.", robot_prim_path)
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                reference_prim_path=robot_prim_path,
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ],
+            ).get_collision_check_world()
+
+            motion_gen.update_world(obstacles)
+            print("Updated World")
+            carb.log_info("Synced CuRobo world from stage.")
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+
+            result = motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, plan_config)
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            for _ in range(2):
+                my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/motion_gen_reacher_nvblox.py

@@ -0,0 +1,313 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+# [enable_extension(x) for x in ext_list]
+# simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+############################################################
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, _ = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_nvblox.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        finetune_dt_scale=1.05,
+        fixed_iters_trajopt=True,
+        finetune_trajopt_iters=500,
+        minimize_jerk=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up...")
+    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
+
+    print("Curobo is Ready")
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg.get_mesh_world(), base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        # print(sim_js.velocities)
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+
+            result = motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, plan_config)
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            for _ in range(2):
+                my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/mpc_example.py

@@ -0,0 +1,402 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# script running (ubuntu):
+#
+
+############################################################
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+import pickle
+import shutil
+import sys
+
+## import curobo:
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+###########################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {"headless": False, "width": 1920, "height": 1080}
+)  # _make_simulation_app({"headless": False})
+# Third Party
+# Enable the layers and stage windows in the UI
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+simulation_app.update()
+# Standard Library
+import os
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere
+from omni.isaac.core.prims.xform_prim import XFormPrim
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+
+# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper
+from omni.isaac.core.utils.rotations import euler_angles_to_quat
+
+########### frame prim #################
+from omni.isaac.core.utils.stage import get_stage_units
+from omni.isaac.core.utils.types import ArticulationAction
+from omni.isaac.cortex.cortex_object import CortexObject
+from omni.isaac.franka import KinematicsSolver
+from omni.isaac.franka.franka import Franka
+from omni.isaac.franka.tasks import FollowTarget
+from pxr import Gf
+from scipy.spatial.transform import Rotation as R
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+###########
+EXT_DIR = os.path.abspath(os.path.join(os.path.abspath(os.path.dirname(__file__))))
+DATA_DIR = os.path.join(EXT_DIR, "data")
+########### frame prim #################;;;;;
+
+
+# Standard Library
+from typing import Optional
+
+# Third Party
+from helper import add_robot_to_scene
+from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial
+from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path
+from pxr import Sdf, UsdShade
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+from curobo.curobolib import geom_cu
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+############################################################
+
+
+def draw_points(rollouts: torch.Tensor):
+    if rollouts is None:
+        return
+    # Standard Library
+    import random
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    cpu_rollouts = rollouts.cpu().numpy()
+    b, h, _ = cpu_rollouts.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [
+            (cpu_rollouts[i, j, 0], cpu_rollouts[i, j, 1], cpu_rollouts[i, j, 2]) for j in range(h)
+        ]
+        colors += [(1.0 - (i + 1.0 / b), 0.3 * (i + 1.0 / b), 0.0, 0.1) for _ in range(h)]
+    sizes = [10.0 for _ in range(b * h)]
+    draw.draw_points(point_list, colors, sizes)
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] += 0.02
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    init_curobo = False
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].pose[2] = -10.0
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        self_collision_check=True,
+        collision_checker_type=CollisionCheckerType.MESH,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        use_mppi=True,
+        use_lbfgs=False,
+        use_es=False,
+        store_rollouts=True,
+        step_dt=0.02,
+    )
+
+    mpc = MpcSolver(mpc_config)
+
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.clone().unsqueeze(0)
+    joint_names = mpc.rollout_fn.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg, joint_names=joint_names)
+    )
+    current_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    goal = Goal(
+        current_state=current_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+    mpc.update_goal(goal_buffer)
+
+    usd_help.load_stage(my_world.stage)
+    init_world = False
+    cmd_state_full = None
+    step = 0
+    while simulation_app.is_running():
+        if not init_world:
+            for _ in range(10):
+                my_world.step(render=True)
+            init_world = True
+        draw_points(mpc.get_visual_rollouts())
+
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            continue
+
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if not init_curobo:
+            init_curobo = True
+        step += 1
+        step_index = step
+        if step_index % 1000 == 0:
+            print("Updating world")
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ],
+                reference_prim_path=robot_prim_path,
+            )
+            obstacles.add_obstacle(world_cfg_table.cuboid[0])
+            mpc.world_coll_checker.load_collision_model(obstacles)
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position + 1.0
+
+        if np.linalg.norm(cube_position - past_pose) > 1e-3:
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_buffer.goal_pose.copy_(ik_goal)
+            mpc.update_goal(goal_buffer)
+            past_pose = cube_position
+
+        # if not changed don't call curobo:
+
+        # get robot current state:
+        sim_js = robot.get_joints_state()
+        js_names = robot.dof_names
+        sim_js_names = robot.dof_names
+
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        if cmd_state_full is None:
+            current_state.copy_(cu_js)
+        else:
+            current_state_partial = cmd_state_full.get_ordered_joint_state(
+                mpc.rollout_fn.joint_names
+            )
+            current_state.copy_(current_state_partial)
+            current_state.joint_names = current_state_partial.joint_names
+            # current_state = current_state.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        common_js_names = []
+        current_state.copy_(cu_js)
+
+        mpc_result = mpc.step(current_state, max_attempts=2)
+        # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+        succ = True  # ik_result.success.item()
+        cmd_state_full = mpc_result.js_action
+        common_js_names = []
+        idx_list = []
+        for x in sim_js_names:
+            if x in cmd_state_full.joint_names:
+                idx_list.append(robot.get_dof_index(x))
+                common_js_names.append(x)
+
+        cmd_state = cmd_state_full.get_ordered_joint_state(common_js_names)
+        cmd_state_full = cmd_state
+
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=idx_list,
+        )
+        # positions_goal = articulation_action.joint_positions
+        if step_index % 1000 == 0:
+            print(mpc_result.metrics.feasible.item(), mpc_result.metrics.pose_error.item())
+
+        if succ:
+            # set desired joint angles obtained from IK:
+            for _ in range(3):
+                articulation_controller.apply_action(art_action)
+
+        else:
+            carb.log_warn("No action is being taken.")
+
+
+############################################################
+
+if __name__ == "__main__":
+    main()
+    simulation_app.close()

examples/isaac_sim/mpc_nvblox_example.py

@@ -0,0 +1,387 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# script running (ubuntu):
+#
+
+############################################################
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+import pickle
+import shutil
+import sys
+
+## import curobo:
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+args = parser.parse_args()
+
+###########################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {"headless": False, "width": 1920, "height": 1080}
+)  # _make_simulation_app({"headless": False})
+# Third Party
+# Enable the layers and stage windows in the UI
+from omni.isaac.core.utils.extensions import enable_extension
+
+ext_list = [
+    "omni.kit.asset_converter",
+    # "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+[enable_extension(x) for x in ext_list]
+simulation_app.update()
+# Standard Library
+import os
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere
+from omni.isaac.core.prims.xform_prim import XFormPrim
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+
+# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper
+from omni.isaac.core.utils.rotations import euler_angles_to_quat
+
+########### frame prim #################
+from omni.isaac.core.utils.stage import get_stage_units
+from omni.isaac.core.utils.types import ArticulationAction
+from omni.isaac.cortex.cortex_object import CortexObject
+from omni.isaac.franka import KinematicsSolver
+from omni.isaac.franka.franka import Franka
+from omni.isaac.franka.tasks import FollowTarget
+from pxr import Gf
+from scipy.spatial.transform import Rotation as R
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+###########
+EXT_DIR = os.path.abspath(os.path.join(os.path.abspath(os.path.dirname(__file__))))
+DATA_DIR = os.path.join(EXT_DIR, "data")
+########### frame prim #################;;;;;
+
+
+# Standard Library
+from typing import Optional
+
+# Third Party
+from helper import add_robot_to_scene
+from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial
+from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path
+from pxr import Sdf, UsdShade
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+from curobo.curobolib import geom_cu
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+############################################################
+
+
+def draw_points(rollouts: torch.Tensor):
+    if rollouts is None:
+        return
+    # Standard Library
+    import random
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    cpu_rollouts = rollouts.cpu().numpy()
+    b, h, _ = cpu_rollouts.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [
+            (cpu_rollouts[i, j, 0], cpu_rollouts[i, j, 1], cpu_rollouts[i, j, 2]) for j in range(h)
+        ]
+        colors += [(1.0 - (i + 1.0 / b), 0.3 * (i + 1.0 / b), 0.0, 0.1) for _ in range(h)]
+    sizes = [10.0 for _ in range(b * h)]
+    draw.draw_points(point_list, colors, sizes)
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=np.array([0.5, 0, 0.5]),
+        orientation=np.array([0, 1, 0, 0]),
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] += 0.02
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.04
+
+    init_curobo = False
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    # world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_nvblox.yml"))
+    )
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        self_collision_check=True,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_mppi=True,
+        use_lbfgs=False,
+        use_es=False,
+        store_rollouts=True,
+        step_dt=0.02,
+    )
+
+    mpc = MpcSolver(mpc_config)
+
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.clone().unsqueeze(0)
+    joint_names = mpc.rollout_fn.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg, joint_names=joint_names)
+    )
+    current_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    goal = Goal(
+        current_state=current_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+    mpc.update_goal(goal_buffer)
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg.get_mesh_world(), base_frame="/World")
+
+    init_world = False
+    cmd_state_full = None
+    step = 0
+    while simulation_app.is_running():
+        if not init_world:
+            for _ in range(10):
+                my_world.step(render=True)
+            init_world = True
+        draw_points(mpc.get_visual_rollouts())
+
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            continue
+
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if not init_curobo:
+            init_curobo = True
+        step += 1
+        step_index = step
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position + 1.0
+
+        if np.linalg.norm(cube_position - past_pose) > 1e-3:
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_buffer.goal_pose.copy_(ik_goal)
+            mpc.update_goal(goal_buffer)
+            past_pose = cube_position
+
+        # if not changed don't call curobo:
+
+        # get robot current state:
+        sim_js = robot.get_joints_state()
+        js_names = robot.dof_names
+        sim_js_names = robot.dof_names
+
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        if cmd_state_full is None:
+            current_state.copy_(cu_js)
+        else:
+            current_state_partial = cmd_state_full.get_ordered_joint_state(
+                mpc.rollout_fn.joint_names
+            )
+            current_state.copy_(current_state_partial)
+            current_state.joint_names = current_state_partial.joint_names
+            # current_state = current_state.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+        common_js_names = []
+        current_state.copy_(cu_js)
+
+        mpc_result = mpc.step(current_state, max_attempts=2)
+        # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+        succ = True  # ik_result.success.item()
+        cmd_state_full = mpc_result.js_action
+        common_js_names = []
+        idx_list = []
+        for x in sim_js_names:
+            if x in cmd_state_full.joint_names:
+                idx_list.append(robot.get_dof_index(x))
+                common_js_names.append(x)
+
+        cmd_state = cmd_state_full.get_ordered_joint_state(common_js_names)
+        cmd_state_full = cmd_state
+        # print(ee_translation_goal, ee_orientation_teleop_goal)
+
+        # Compute IK for given EE Teleop goals
+        # articulation_action, succ = my_controller.compute_inverse_kinematics(
+        #    ee_translation_goal,
+        #    ee_orientation_teleop_goal,
+        # )
+
+        # create articulation action:
+        # get full dof state
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=idx_list,
+        )
+        # positions_goal = articulation_action.joint_positions
+        if step_index % 1000 == 0:
+            print(mpc_result.metrics.feasible.item(), mpc_result.metrics.pose_error.item())
+
+        if succ:
+            # set desired joint angles obtained from IK:
+            for _ in range(3):
+                articulation_controller.apply_action(art_action)
+
+        else:
+            carb.log_warn("No action is being taken.")
+
+
+############################################################
+
+if __name__ == "__main__":
+    main()
+    simulation_app.close()

examples/isaac_sim/multi_arm_reacher.py

@@ -0,0 +1,379 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--headless", action="store_true", help="When True, enables headless mode", default=False
+)
+parser.add_argument(
+    "--visualize_spheres",
+    action="store_true",
+    help="When True, visualizes robot spheres",
+    default=False,
+)
+
+parser.add_argument(
+    "--robot", type=str, default="dual_ur10e.yml", help="robot configuration to load"
+)
+args = parser.parse_args()
+
+############################################################
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp(
+    {
+        "headless": args.headless,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+# Third Party
+from omni.isaac.core.utils.extensions import enable_extension
+
+# CuRobo
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+ext_list = [
+    "omni.kit.asset_converter",
+    "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+]
+# [enable_extension(x) for x in ext_list]
+# simulation_app.update()
+
+# Standard Library
+from typing import Dict
+
+# Third Party
+import carb
+import numpy as np
+from helper import add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.robots import Robot
+
+########### OV #################
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.logger import setup_curobo_logger
+from curobo.util.usd_helper import UsdHelper
+
+############################################################
+
+
+########### OV #################;;;;;
+
+
+############################################################
+
+
+def main():
+    # assuming obstacles are in objects_path:
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    stage.DefinePrim("/curobo", "Xform")
+    # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
+    stage = my_world.stage
+    # stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+    # Make a target to follow
+
+    setup_curobo_logger("warn")
+    past_pose = None
+    n_obstacle_cuboids = 30
+    n_obstacle_mesh = 10
+
+    # warmup curobo instance
+    usd_help = UsdHelper()
+    target_pose = None
+
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
+
+    articulation_controller = robot.get_articulation_controller()
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    )
+    world_cfg_table.cuboid[0].pose[2] -= 0.02
+
+    world_cfg1 = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+    ).get_mesh_world()
+    world_cfg1.mesh[0].name += "_mesh"
+    world_cfg1.mesh[0].pose[2] = -10.5
+
+    world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        maximum_trajectory_dt=0.2,
+        fixed_iters_trajopt=True,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up...")
+    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
+
+    print("Curobo is Ready")
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=10, enable_finetune_trajopt=True
+    )
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg, base_frame="/World")
+
+    cmd_plan = None
+    cmd_idx = 0
+    my_world.scene.add_default_ground_plane()
+    i = 0
+    spheres = None
+
+    # read number of targets in link names:
+    link_names = motion_gen.kinematics.link_names
+    ee_link_name = motion_gen.kinematics.ee_link
+    # get link poses at retract configuration:
+
+    kin_state = motion_gen.kinematics.get_state(motion_gen.get_retract_config().view(1, -1))
+
+    link_retract_pose = kin_state.link_pose
+    t_pos = np.ravel(kin_state.ee_pose.to_list())
+    target = cuboid.VisualCuboid(
+        "/World/target",
+        position=t_pos[:3],
+        orientation=t_pos[3:],
+        color=np.array([1.0, 0, 0]),
+        size=0.05,
+    )
+
+    # create new targets for new links:
+    ee_idx = link_names.index(ee_link_name)
+    target_links = {}
+    names = []
+    for i in link_names:
+        if i != ee_link_name:
+            k_pose = np.ravel(link_retract_pose[i].to_list())
+            color = np.random.randn(3) * 0.2
+            color[0] += 0.5
+            color[1] = 0.5
+            color[2] = 0.0
+            target_links[i] = cuboid.VisualCuboid(
+                "/World/target_" + i,
+                position=np.array(k_pose[:3]),
+                orientation=np.array(k_pose[3:]),
+                color=color,
+                size=0.05,
+            )
+            names.append("/World/target_" + i)
+    i = 0
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        step_index = my_world.current_time_step_index
+        # print(step_index)
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+        if step_index < 20:
+            continue
+
+        if step_index == 50 or step_index % 1000 == 0.0:
+            print("Updating world, reading w.r.t.", robot_prim_path)
+            obstacles = usd_help.get_obstacles_from_stage(
+                # only_paths=[obstacles_path],
+                reference_prim_path=robot_prim_path,
+                ignore_substring=[
+                    robot_prim_path,
+                    "/World/target",
+                    "/World/defaultGroundPlane",
+                    "/curobo",
+                ]
+                + names,
+            ).get_collision_check_world()
+
+            motion_gen.update_world(obstacles)
+            print("Updated World")
+            carb.log_info("Synced CuRobo world from stage.")
+
+        # position and orientation of target virtual cube:
+        cube_position, cube_orientation = target.get_world_pose()
+
+        if past_pose is None:
+            past_pose = cube_position
+        if target_pose is None:
+            target_pose = cube_position
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+        if args.visualize_spheres and step_index % 2 == 0:
+            sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position)
+
+            if spheres is None:
+                spheres = []
+                # create spheres:
+
+                for si, s in enumerate(sph_list[0]):
+                    sp = sphere.VisualSphere(
+                        prim_path="/curobo/robot_sphere_" + str(si),
+                        position=np.ravel(s.position),
+                        radius=float(s.radius),
+                        color=np.array([0, 0.8, 0.2]),
+                    )
+                    spheres.append(sp)
+            else:
+                for si, s in enumerate(sph_list[0]):
+                    spheres[si].set_world_pose(position=np.ravel(s.position))
+                    spheres[si].set_radius(float(s.radius))
+        # print(sim_js.velocities)
+        if (
+            np.linalg.norm(cube_position - target_pose) > 1e-3
+            and np.linalg.norm(past_pose - cube_position) == 0.0
+            and np.linalg.norm(sim_js.velocities) < 0.2
+        ):
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            # add link poses:
+            link_poses = {}
+            for i in target_links.keys():
+                c_p, c_rot = target_links[i].get_world_pose()
+                link_poses[i] = Pose(
+                    position=tensor_args.to_device(c_p),
+                    quaternion=tensor_args.to_device(c_rot),
+                )
+            result = motion_gen.plan_single(
+                cu_js.unsqueeze(0), ik_goal, plan_config.clone(), link_poses=link_poses
+            )
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+            target_pose = cube_position
+        past_pose = cube_position
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_idx += 1
+            for _ in range(2):
+                my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()

examples/isaac_sim/realsense_collision.py

@@ -0,0 +1,260 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+# Third Party
+import cv2
+import numpy as np
+import torch
+from matplotlib import cm
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.camera import CameraObservation
+from curobo.types.math import Pose
+from curobo.util_file import get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+simulation_app.update()
+# Third Party
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+
+
+def draw_points(voxels):
+    # Third Party
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    if len(voxels) == 0:
+        return
+
+    jet = cm.get_cmap("plasma").reversed()
+
+    cpu_pos = voxels[..., :3].view(-1, 3).cpu().numpy()
+    z_val = cpu_pos[:, 1]
+    # add smallest and largest values:
+    # z_val = np.append(z_val, 1.0)
+    # z_val = np.append(z_val,0.4)
+    # scale values
+    # z_val += 0.4
+    # z_val[z_val>1.0] = 1.0
+    # z_val = 1.0/z_val
+    # z_val = z_val/1.5
+    # z_val[z_val!=z_val] = 0.0
+    # z_val[z_val==0.0] = 0.4
+
+    jet_colors = jet(z_val)
+
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        colors += [(jet_colors[i][0], jet_colors[i][1], jet_colors[i][2], 1.0)]
+    sizes = [10.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def clip_camera(camera_data):
+    # clip camera image to bounding box:
+    h_ratio = 0.15
+    w_ratio = 0.15
+    depth = camera_data["raw_depth"]
+    depth_tensor = camera_data["depth"]
+    h, w = depth_tensor.shape
+    depth[: int(h_ratio * h), :] = 0.0
+    depth[int((1 - h_ratio) * h) :, :] = 0.0
+    depth[:, : int(w_ratio * w)] = 0.0
+    depth[:, int((1 - w_ratio) * w) :] = 0.0
+
+    depth_tensor[: int(h_ratio * h), :] = 0.0
+    depth_tensor[int(1 - h_ratio * h) :, :] = 0.0
+    depth_tensor[:, : int(w_ratio * w)] = 0.0
+    depth_tensor[:, int(1 - w_ratio * w) :] = 0.0
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(0.0, 0, 0.8, 0.9)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+if __name__ == "__main__":
+    radius = 0.05
+    act_distance = 0.4
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+    # my_world.scene.add_ground_plane(color=np.array([0.2,0.2,0.2]))
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+
+    target = sphere.VisualSphere(
+        "/World/target",
+        position=np.array([0.0, 0, 0.5]),
+        orientation=np.array([1, 0, 0, 0]),
+        radius=radius,
+        visual_material=target_material,
+    )
+
+    # Make a target to follow
+    camera_marker = cuboid.VisualCuboid(
+        "/World/camera_nvblox",
+        position=np.array([0.0, -0.1, 0.25]),
+        orientation=np.array([0.843, -0.537, 0.0, 0.0]),
+        color=np.array([0.1, 0.1, 0.5]),
+        size=0.03,
+    )
+    collision_checker_type = CollisionCheckerType.BLOX
+    world_cfg = WorldConfig.from_dict(
+        {
+            "blox": {
+                "world": {
+                    "pose": [0, 0, 0, 1, 0, 0, 0],
+                    "integrator_type": "occupancy",
+                    "voxel_size": 0.03,
+                }
+            }
+        }
+    )
+
+    config = RobotWorldConfig.load_from_config(
+        "franka.yml",
+        world_cfg,
+        collision_activation_distance=act_distance,
+        collision_checker_type=collision_checker_type,
+    )
+
+    model = RobotWorld(config)
+
+    realsense_data = RealsenseDataloader(clipping_distance_m=1.0)
+    data = realsense_data.get_data()
+
+    camera_pose = Pose.from_list([0, 0, 0, 0.707, 0.707, 0, 0])
+    i = 0
+    tensor_args = TensorDeviceType()
+    x_sph = torch.zeros((1, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    x_sph[..., 3] = radius
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+
+        sp_buffer = []
+        sph_position, _ = target.get_local_pose()
+
+        x_sph[..., :3] = tensor_args.to_device(sph_position).view(1, 1, 1, 3)
+
+        model.world_model.decay_layer("world")
+        data = realsense_data.get_data()
+        clip_camera(data)
+        cube_position, cube_orientation = camera_marker.get_local_pose()
+        camera_pose = Pose(
+            position=tensor_args.to_device(cube_position),
+            quaternion=tensor_args.to_device(cube_orientation),
+        )
+        # print(data["rgba"].shape, data["depth"].shape, data["intrinsics"])
+
+        data_camera = CameraObservation(  # rgb_image = data["rgba_nvblox"],
+            depth_image=data["depth"], intrinsics=data["intrinsics"], pose=camera_pose
+        )
+        data_camera = data_camera.to(device=model.tensor_args.device)
+        # print(data_camera.depth_image, data_camera.rgb_image, data_camera.intrinsics)
+        # print("got new message")
+        model.world_model.add_camera_frame(data_camera, "world")
+        # print("added camera frame")
+        model.world_model.process_camera_frames("world", False)
+        torch.cuda.synchronize()
+        model.world_model.update_blox_hashes()
+        bounding = Cuboid("t", dims=[1, 1, 1], pose=[0, 0, 0, 1, 0, 0, 0])
+        voxels = model.world_model.get_voxels_in_bounding_box(bounding, 0.025)
+        # print(data_camera.depth_image)
+        depth_image = data["raw_depth"]
+        color_image = data["raw_rgb"]
+        depth_colormap = cv2.applyColorMap(
+            cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS
+        )
+        images = np.hstack((color_image, depth_colormap))
+
+        cv2.namedWindow("Align Example", cv2.WINDOW_NORMAL)
+        cv2.imshow("Align Example", images)
+        key = cv2.waitKey(1)
+        # Press esc or 'q' to close the image window
+        if key & 0xFF == ord("q") or key == 27:
+            cv2.destroyAllWindows()
+            break
+
+        draw_points(voxels)
+        d, d_vec = model.get_collision_vector(x_sph)
+
+        p = d.item()
+        p = max(1, p * 5)
+        if d.item() == 0.0:
+            target_material.set_color(np.ravel([0, 1, 0]))
+        elif d.item() <= model.contact_distance:
+            target_material.set_color(np.array([0, 0, p]))
+        elif d.item() >= model.contact_distance:
+            target_material.set_color(np.array([p, 0, 0]))
+
+        if d.item() != 0.0:
+            print(d, d_vec)
+
+            draw_line(sph_position, d_vec[..., :3].view(3).cpu().numpy())
+        else:
+            # Third Party
+            from omni.isaac.debug_draw import _debug_draw
+
+            draw = _debug_draw.acquire_debug_draw_interface()
+            # if draw.get_num_points() > 0:
+            draw.clear_lines()
+
+    realsense_data.stop_device()
+    print("finished program")
+    simulation_app.close()

examples/isaac_sim/realsense_mpc.py

@@ -0,0 +1,484 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+# Third Party
+import cv2
+import numpy as np
+import torch
+from matplotlib import cm
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.camera import CameraObservation
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+simulation_app.update()
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+from helper import VoxelManager, add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.rollout.rollout_base import Goal
+from curobo.util.usd_helper import UsdHelper
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+parser = argparse.ArgumentParser()
+
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+parser.add_argument(
+    "--waypoints", action="store_true", help="When True, sets robot in static mode", default=False
+)
+parser.add_argument(
+    "--use-debug-draw",
+    action="store_true",
+    help="When True, sets robot in static mode",
+    default=False,
+)
+args = parser.parse_args()
+
+
+def draw_rollout_points(rollouts: torch.Tensor, clear: bool = False):
+    if rollouts is None:
+        return
+    # Standard Library
+    import random
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    N = 100
+    if clear:
+        draw.clear_points()
+    # if draw.get_num_points() > 0:
+    # draw.clear_points()
+    cpu_rollouts = rollouts.cpu().numpy()
+    b, h, _ = cpu_rollouts.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [
+            (cpu_rollouts[i, j, 0], cpu_rollouts[i, j, 1], cpu_rollouts[i, j, 2]) for j in range(h)
+        ]
+        colors += [(1.0 - (i + 1.0 / b), 0.3 * (i + 1.0 / b), 0.0, 0.1) for _ in range(h)]
+    sizes = [10.0 for _ in range(b * h)]
+    draw.draw_points(point_list, colors, sizes)
+
+
+def draw_points(voxels):
+    # Third Party
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    if len(voxels) == 0:
+        return
+
+    jet = cm.get_cmap("plasma").reversed()
+
+    cpu_pos = voxels[..., :3].view(-1, 3).cpu().numpy()
+    z_val = cpu_pos[:, 0]
+
+    jet_colors = jet(z_val)
+
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        colors += [(jet_colors[i][0], jet_colors[i][1], jet_colors[i][2], 0.8)]
+    sizes = [20.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def clip_camera(camera_data):
+    # clip camera image to bounding box:
+    h_ratio = 0.05
+    w_ratio = 0.05
+    depth = camera_data["raw_depth"]
+    depth_tensor = camera_data["depth"]
+    h, w = depth_tensor.shape
+    depth[: int(h_ratio * h), :] = 0.0
+    depth[int((1 - h_ratio) * h) :, :] = 0.0
+    depth[:, : int(w_ratio * w)] = 0.0
+    depth[:, int((1 - w_ratio) * w) :] = 0.0
+
+    depth_tensor[: int(h_ratio * h), :] = 0.0
+    depth_tensor[int(1 - h_ratio * h) :, :] = 0.0
+    depth_tensor[:, : int(w_ratio * w)] = 0.0
+    depth_tensor[:, int(1 - w_ratio * w) :] = 0.0
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(0.0, 0, 0.8, 0.9)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+if __name__ == "__main__":
+    radius = 0.05
+    act_distance = 0.4
+    voxel_size = 0.05
+    render_voxel_size = 0.02
+    clipping_distance = 0.7
+
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+    target_material_2 = OmniPBR("/World/looks/t2", color=np.array([0, 1, 0]))
+    if not args.waypoints:
+        target = cuboid.VisualCuboid(
+            "/World/target_1",
+            position=np.array([0.5, 0.0, 0.4]),
+            orientation=np.array([0, 1.0, 0, 0]),
+            size=0.04,
+            visual_material=target_material,
+        )
+
+    else:
+        target = cuboid.VisualCuboid(
+            "/World/target_1",
+            position=np.array([0.4, -0.5, 0.2]),
+            orientation=np.array([0, 1.0, 0, 0]),
+            size=0.04,
+            visual_material=target_material,
+        )
+
+    # Make a target to follow
+    target_2 = cuboid.VisualCuboid(
+        "/World/target_2",
+        position=np.array([0.4, 0.5, 0.2]),
+        orientation=np.array([0.0, 1, 0.0, 0.0]),
+        size=0.04,
+        visual_material=target_material_2,
+    )
+
+    # Make a target to follow
+    camera_marker = cuboid.VisualCuboid(
+        "/World/camera_nvblox",
+        position=np.array([-0.05, 0.0, 0.45]),
+        # orientation=np.array([0.793, 0, 0.609,0.0]),
+        orientation=np.array([0.5, -0.5, 0.5, -0.5]),
+        # orientation=np.array([0.561, -0.561, 0.431,-0.431]),
+        color=np.array([0, 0, 1]),
+        size=0.01,
+    )
+    camera_marker.set_visibility(False)
+    collision_checker_type = CollisionCheckerType.BLOX
+    world_cfg = WorldConfig.from_dict(
+        {
+            "blox": {
+                "world": {
+                    "pose": [0, 0, 0, 1, 0, 0, 0],
+                    "integrator_type": "occupancy",
+                    "voxel_size": 0.03,
+                }
+            }
+        }
+    )
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.02
+    robot, _ = add_robot_to_scene(robot_cfg, my_world, "/World/world_robot/")
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_wall.yml"))
+    )
+
+    world_cfg_table.cuboid[0].pose[2] -= 0.01
+    usd_help = UsdHelper()
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg_table.get_mesh_world(), base_frame="/World")
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+    world_cfg.add_obstacle(world_cfg_table.cuboid[1])
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        self_collision_check=True,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_mppi=True,
+        use_lbfgs=False,
+        use_es=False,
+        store_rollouts=True,
+        step_dt=0.02,
+    )
+
+    mpc = MpcSolver(mpc_config)
+
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.clone().unsqueeze(0)
+    joint_names = mpc.rollout_fn.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg, joint_names=joint_names)
+    )
+    current_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    goal = Goal(
+        current_state=current_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+    mpc.update_goal(goal_buffer)
+
+    world_model = mpc.world_collision
+    realsense_data = RealsenseDataloader(clipping_distance_m=clipping_distance)
+    data = realsense_data.get_data()
+
+    camera_pose = Pose.from_list([0, 0, 0, 0.707, 0.707, 0, 0])
+    i = 0
+    tensor_args = TensorDeviceType()
+    target_list = [target, target_2]
+    target_material_list = [target_material, target_material_2]
+    for material in target_material_list:
+        material.set_color(np.array([0.1, 0.1, 0.1]))
+    target_idx = 0
+    cmd_idx = 0
+    cmd_plan = None
+    articulation_controller = robot.get_articulation_controller()
+    cmd_state_full = None
+
+    cmd_step_idx = 0
+    current_error = 0.0
+    error_thresh = 0.01
+    first_target = False
+    if not args.use_debug_draw:
+        voxel_viewer = VoxelManager(100, size=render_voxel_size)
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+        step_index = my_world.current_time_step_index
+        if cmd_step_idx == 0:
+            draw_rollout_points(mpc.get_visual_rollouts(), clear=not args.use_debug_draw)
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if step_index % 2 == 0.0:
+            # camera data updation
+            world_model.decay_layer("world")
+            data = realsense_data.get_data()
+            clip_camera(data)
+            cube_position, cube_orientation = camera_marker.get_local_pose()
+            camera_pose = Pose(
+                position=tensor_args.to_device(cube_position),
+                quaternion=tensor_args.to_device(cube_orientation),
+            )
+
+            data_camera = CameraObservation(  # rgb_image = data["rgba_nvblox"],
+                depth_image=data["depth"], intrinsics=data["intrinsics"], pose=camera_pose
+            )
+            data_camera = data_camera.to(device=tensor_args.device)
+            world_model.add_camera_frame(data_camera, "world")
+            world_model.process_camera_frames("world", False)
+            torch.cuda.synchronize()
+            world_model.update_blox_hashes()
+
+            bounding = Cuboid("t", dims=[1, 1, 1.0], pose=[0, 0, 0, 1, 0, 0, 0])
+            voxels = world_model.get_voxels_in_bounding_box(bounding, voxel_size)
+            if voxels.shape[0] > 0:
+                voxels = voxels[voxels[:, 2] > voxel_size]
+                voxels = voxels[voxels[:, 0] > 0.0]
+                if args.use_debug_draw:
+                    draw_points(voxels)
+
+                else:
+                    voxels = voxels.cpu().numpy()
+                    voxel_viewer.update_voxels(voxels[:, :3])
+            else:
+                if not args.use_debug_draw:
+                    voxel_viewer.clear()
+
+        if True:
+            depth_image = data["raw_depth"]
+            color_image = data["raw_rgb"]
+            depth_colormap = cv2.applyColorMap(
+                cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS
+            )
+            color_image = cv2.flip(color_image, 1)
+            depth_colormap = cv2.flip(depth_colormap, 1)
+
+            images = np.hstack((color_image, depth_colormap))
+
+            cv2.namedWindow("NVBLOX Example", cv2.WINDOW_NORMAL)
+            cv2.imshow("NVBLOX Example", images)
+            key = cv2.waitKey(1)
+            # Press esc or 'q' to close the image window
+            if key & 0xFF == ord("q") or key == 27:
+                cv2.destroyAllWindows()
+                break
+
+        sim_js = robot.get_joints_state()
+        sim_js_names = robot.dof_names
+        cu_js = JointState(
+            position=tensor_args.to_device(sim_js.positions),
+            velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=sim_js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(mpc.rollout_fn.joint_names)
+
+        if cmd_state_full is None:
+            current_state.copy_(cu_js)
+        else:
+            current_state_partial = cmd_state_full.get_ordered_joint_state(
+                mpc.rollout_fn.joint_names
+            )
+            current_state.copy_(current_state_partial)
+            current_state.joint_names = current_state_partial.joint_names
+
+        if current_error <= error_thresh and (not first_target or args.waypoints):
+            first_target = True
+            # motion generation:
+            for ks in range(len(target_material_list)):
+                if ks == target_idx:
+                    target_material_list[ks].set_color(np.ravel([0, 1.0, 0]))
+                else:
+                    target_material_list[ks].set_color(np.ravel([0.1, 0.1, 0.1]))
+
+            cube_position, cube_orientation = target_list[target_idx].get_world_pose()
+
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+            goal_buffer.goal_pose.copy_(ik_goal)
+            mpc.update_goal(goal_buffer)
+            target_idx += 1
+            if target_idx >= len(target_list):
+                target_idx = 0
+
+        if cmd_step_idx == 0:
+            mpc_result = mpc.step(current_state, max_attempts=2)
+            current_error = mpc_result.metrics.pose_error.item()
+        cmd_state_full = mpc_result.js_action
+        common_js_names = []
+        idx_list = []
+        for x in sim_js_names:
+            if x in cmd_state_full.joint_names:
+                idx_list.append(robot.get_dof_index(x))
+                common_js_names.append(x)
+
+        cmd_state = cmd_state_full.get_ordered_joint_state(common_js_names)
+        cmd_state_full = cmd_state
+
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=idx_list,
+        )
+        articulation_controller.apply_action(art_action)
+
+        if cmd_step_idx == 2:
+            cmd_step_idx = 0
+
+        # positions_goal = a
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                # cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_step_idx += 1
+            # for _ in range(2):
+            #    my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    realsense_data.stop_device()
+    print("finished program")
+
+    simulation_app.close()

examples/isaac_sim/realsense_reacher.py

@@ -0,0 +1,414 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+# Third Party
+import cv2
+import numpy as np
+import torch
+from matplotlib import cm
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+from omni.isaac.kit import SimulationApp
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.camera import CameraObservation
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+simulation_app = SimulationApp(
+    {
+        "headless": False,
+        "width": "1920",
+        "height": "1080",
+    }
+)
+simulation_app.update()
+# Standard Library
+import argparse
+
+# Third Party
+import carb
+from helper import VoxelManager, add_robot_to_scene
+from omni.isaac.core import World
+from omni.isaac.core.materials import OmniPBR
+from omni.isaac.core.objects import cuboid, sphere
+from omni.isaac.core.utils.types import ArticulationAction
+
+# CuRobo
+from curobo.util.usd_helper import UsdHelper
+
+parser = argparse.ArgumentParser()
+
+
+parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
+parser.add_argument(
+    "--use-debug-draw",
+    action="store_true",
+    help="When True, sets robot in static mode",
+    default=False,
+)
+args = parser.parse_args()
+
+
+def draw_points(voxels):
+    # Third Party
+
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_points()
+    if len(voxels) == 0:
+        return
+
+    jet = cm.get_cmap("plasma").reversed()
+
+    cpu_pos = voxels[..., :3].view(-1, 3).cpu().numpy()
+    z_val = cpu_pos[:, 0]
+
+    jet_colors = jet(z_val)
+
+    b, _ = cpu_pos.shape
+    point_list = []
+    colors = []
+    for i in range(b):
+        # get list of points:
+        point_list += [(cpu_pos[i, 0], cpu_pos[i, 1], cpu_pos[i, 2])]
+        colors += [(jet_colors[i][0], jet_colors[i][1], jet_colors[i][2], 0.8)]
+    sizes = [20.0 for _ in range(b)]
+
+    draw.draw_points(point_list, colors, sizes)
+
+
+def clip_camera(camera_data):
+    # clip camera image to bounding box:
+    h_ratio = 0.05
+    w_ratio = 0.05
+    depth = camera_data["raw_depth"]
+    depth_tensor = camera_data["depth"]
+    h, w = depth_tensor.shape
+    depth[: int(h_ratio * h), :] = 0.0
+    depth[int((1 - h_ratio) * h) :, :] = 0.0
+    depth[:, : int(w_ratio * w)] = 0.0
+    depth[:, int((1 - w_ratio) * w) :] = 0.0
+
+    depth_tensor[: int(h_ratio * h), :] = 0.0
+    depth_tensor[int(1 - h_ratio * h) :, :] = 0.0
+    depth_tensor[:, : int(w_ratio * w)] = 0.0
+    depth_tensor[:, int(1 - w_ratio * w) :] = 0.0
+
+
+def draw_line(start, gradient):
+    # Third Party
+    from omni.isaac.debug_draw import _debug_draw
+
+    draw = _debug_draw.acquire_debug_draw_interface()
+    # if draw.get_num_points() > 0:
+    draw.clear_lines()
+    start_list = [start]
+    end_list = [start + gradient]
+
+    colors = [(0.0, 0, 0.8, 0.9)]
+
+    sizes = [10.0]
+    draw.draw_lines(start_list, end_list, colors, sizes)
+
+
+if __name__ == "__main__":
+    radius = 0.05
+    act_distance = 0.4
+    voxel_size = 0.05
+    render_voxel_size = 0.02
+    clipping_distance = 0.7
+    my_world = World(stage_units_in_meters=1.0)
+    stage = my_world.stage
+
+    stage = my_world.stage
+    my_world.scene.add_default_ground_plane()
+
+    xform = stage.DefinePrim("/World", "Xform")
+    stage.SetDefaultPrim(xform)
+    target_material = OmniPBR("/World/looks/t", color=np.array([0, 1, 0]))
+    target_material_2 = OmniPBR("/World/looks/t2", color=np.array([0, 1, 0]))
+
+    target = cuboid.VisualCuboid(
+        "/World/target_1",
+        position=np.array([0.4, -0.5, 0.2]),
+        orientation=np.array([0, 1.0, 0, 0]),
+        size=0.04,
+        visual_material=target_material,
+    )
+
+    # Make a target to follow
+    target_2 = cuboid.VisualCuboid(
+        "/World/target_2",
+        position=np.array([0.4, 0.5, 0.2]),
+        orientation=np.array([0.0, 1, 0.0, 0.0]),
+        size=0.04,
+        visual_material=target_material_2,
+    )
+
+    # Make a target to follow
+    camera_marker = cuboid.VisualCuboid(
+        "/World/camera_nvblox",
+        position=np.array([-0.05, 0.0, 0.45]),
+        # orientation=np.array([0.793, 0, 0.609,0.0]),
+        orientation=np.array([0.5, -0.5, 0.5, -0.5]),
+        # orientation=np.array([0.561, -0.561, 0.431,-0.431]),
+        color=np.array([0, 0, 1]),
+        size=0.01,
+    )
+    camera_marker.set_visibility(False)
+
+    collision_checker_type = CollisionCheckerType.BLOX
+    world_cfg = WorldConfig.from_dict(
+        {
+            "blox": {
+                "world": {
+                    "pose": [0, 0, 0, 1, 0, 0, 0],
+                    "integrator_type": "occupancy",
+                    "voxel_size": 0.03,
+                }
+            }
+        }
+    )
+    tensor_args = TensorDeviceType()
+
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
+
+    j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
+    default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
+
+    robot, _ = add_robot_to_scene(robot_cfg, my_world, "/World/world_robot/")
+
+    world_cfg_table = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), "collision_wall.yml"))
+    )
+
+    world_cfg_table.cuboid[0].pose[2] -= 0.01
+    usd_help = UsdHelper()
+
+    usd_help.load_stage(my_world.stage)
+    usd_help.add_world_to_stage(world_cfg_table.get_mesh_world(), base_frame="/World")
+    world_cfg.add_obstacle(world_cfg_table.cuboid[0])
+    world_cfg.add_obstacle(world_cfg_table.cuboid[1])
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=True,
+        num_trajopt_seeds=12,
+        num_graph_seeds=12,
+        interpolation_dt=0.03,
+        collision_activation_distance=0.025,
+        acceleration_scale=1.0,
+        self_collision_check=True,
+        maximum_trajectory_dt=0.25,
+        finetune_dt_scale=1.05,
+        fixed_iters_trajopt=True,
+        finetune_trajopt_iters=300,
+        minimize_jerk=True,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    print("warming up..")
+    motion_gen.warmup(warmup_js_trajopt=False)
+
+    world_model = motion_gen.world_collision
+    realsense_data = RealsenseDataloader(clipping_distance_m=clipping_distance)
+    data = realsense_data.get_data()
+
+    camera_pose = Pose.from_list([0, 0, 0, 0.707, 0.707, 0, 0])
+    i = 0
+    tensor_args = TensorDeviceType()
+    target_list = [target, target_2]
+    target_material_list = [target_material, target_material_2]
+    for material in target_material_list:
+        material.set_color(np.array([0.1, 0.1, 0.1]))
+    target_idx = 0
+    cmd_idx = 0
+    cmd_plan = None
+    articulation_controller = robot.get_articulation_controller()
+    plan_config = MotionGenPlanConfig(
+        enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
+    )
+    if not args.use_debug_draw:
+        voxel_viewer = VoxelManager(100, size=render_voxel_size)
+    cmd_step_idx = 0
+    while simulation_app.is_running():
+        my_world.step(render=True)
+
+        if not my_world.is_playing():
+            if i % 100 == 0:
+                print("**** Click Play to start simulation *****")
+            i += 1
+            # if step_index == 0:
+            #    my_world.play()
+            continue
+        step_index = my_world.current_time_step_index
+
+        if step_index == 0:
+            my_world.reset()
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+
+            robot._articulation_view.set_max_efforts(
+                values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list
+            )
+
+        if step_index % 5 == 0.0:
+            # camera data updation
+            world_model.decay_layer("world")
+            data = realsense_data.get_data()
+            clip_camera(data)
+            cube_position, cube_orientation = camera_marker.get_local_pose()
+            camera_pose = Pose(
+                position=tensor_args.to_device(cube_position),
+                quaternion=tensor_args.to_device(cube_orientation),
+            )
+
+            data_camera = CameraObservation(  # rgb_image = data["rgba_nvblox"],
+                depth_image=data["depth"], intrinsics=data["intrinsics"], pose=camera_pose
+            )
+            data_camera = data_camera.to(device=tensor_args.device)
+            world_model.add_camera_frame(data_camera, "world")
+            world_model.process_camera_frames("world", False)
+            torch.cuda.synchronize()
+            world_model.update_blox_hashes()
+
+            bounding = Cuboid("t", dims=[1, 1, 1.0], pose=[0, 0, 0, 1, 0, 0, 0])
+            voxels = world_model.get_voxels_in_bounding_box(bounding, voxel_size)
+            if voxels.shape[0] > 0:
+                voxels = voxels[voxels[:, 2] > voxel_size]
+                voxels = voxels[voxels[:, 0] > 0.0]
+                if args.use_debug_draw:
+                    draw_points(voxels)
+                else:
+                    voxels = voxels.cpu().numpy()
+                    voxel_viewer.update_voxels(voxels[:, :3])
+
+                voxel_viewer.update_voxels(voxels[:, :3])
+            else:
+                if not args.use_debug_draw:
+                    voxel_viewer.clear()
+            # draw_points(voxels)
+
+        if True:
+            depth_image = data["raw_depth"]
+            color_image = data["raw_rgb"]
+            depth_colormap = cv2.applyColorMap(
+                cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS
+            )
+            color_image = cv2.flip(color_image, 1)
+            depth_colormap = cv2.flip(depth_colormap, 1)
+
+            images = np.hstack((color_image, depth_colormap))
+
+            cv2.namedWindow("NVBLOX Example", cv2.WINDOW_NORMAL)
+            cv2.imshow("NVBLOX Example", images)
+            key = cv2.waitKey(1)
+            # Press esc or 'q' to close the image window
+            if key & 0xFF == ord("q") or key == 27:
+                cv2.destroyAllWindows()
+                break
+
+        if cmd_plan is None and step_index % 10 == 0:
+            # motion generation:
+            for ks in range(len(target_material_list)):
+                if ks == target_idx:
+                    target_material_list[ks].set_color(np.ravel([0, 1.0, 0]))
+                else:
+                    target_material_list[ks].set_color(np.ravel([0.1, 0.1, 0.1]))
+
+            sim_js = robot.get_joints_state()
+            sim_js_names = robot.dof_names
+            cu_js = JointState(
+                position=tensor_args.to_device(sim_js.positions),
+                velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
+                acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
+                jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
+                joint_names=sim_js_names,
+            )
+            cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
+
+            cube_position, cube_orientation = target_list[target_idx].get_world_pose()
+
+            # Set EE teleop goals, use cube for simple non-vr init:
+            ee_translation_goal = cube_position
+            ee_orientation_teleop_goal = cube_orientation
+
+            # compute curobo solution:
+            ik_goal = Pose(
+                position=tensor_args.to_device(ee_translation_goal),
+                quaternion=tensor_args.to_device(ee_orientation_teleop_goal),
+            )
+
+            result = motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, plan_config)
+            # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
+
+            succ = result.success.item()  # ik_result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                cmd_plan = motion_gen.get_full_js(cmd_plan)
+                # get only joint names that are in both:
+                idx_list = []
+                common_js_names = []
+                for x in sim_js_names:
+                    if x in cmd_plan.joint_names:
+                        idx_list.append(robot.get_dof_index(x))
+                        common_js_names.append(x)
+                # idx_list = [robot.get_dof_index(x) for x in sim_js_names]
+
+                cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names)
+
+                cmd_idx = 0
+                target_idx += 1
+                if target_idx >= len(target_list):
+                    target_idx = 0
+
+            else:
+                carb.log_warn("Plan did not converge to a solution.  No action is being taken.")
+        if cmd_plan is not None:
+            cmd_state = cmd_plan[cmd_idx]
+
+            # get full dof state
+            art_action = ArticulationAction(
+                cmd_state.position.cpu().numpy(),
+                cmd_state.velocity.cpu().numpy(),
+                joint_indices=idx_list,
+            )
+            # set desired joint angles obtained from IK:
+            articulation_controller.apply_action(art_action)
+            cmd_step_idx += 1
+            if cmd_step_idx == 2:
+                cmd_idx += 1
+                cmd_step_idx = 0
+            # for _ in range(2):
+            #    my_world.step(render=False)
+            if cmd_idx >= len(cmd_plan.position):
+                cmd_idx = 0
+                cmd_plan = None
+    realsense_data.stop_device()
+    print("finished program")
+
+    simulation_app.close()

examples/isaac_sim/realsense_viewer.py

@@ -0,0 +1,46 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import cv2
+import numpy as np
+from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
+
+
+def view_realsense():
+    realsense_data = RealsenseDataloader(clipping_distance_m=1.0)
+    # Streaming loop
+    try:
+        while True:
+            data = realsense_data.get_raw_data()
+            depth_image = data[0]
+            color_image = data[1]
+            # Render images:
+            #   depth align to color on left
+            #   depth on right
+            depth_colormap = cv2.applyColorMap(
+                cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_JET
+            )
+            images = np.hstack((color_image, depth_colormap))
+
+            cv2.namedWindow("Align Example", cv2.WINDOW_NORMAL)
+            cv2.imshow("Align Example", images)
+            key = cv2.waitKey(1)
+            # Press esc or 'q' to close the image window
+            if key & 0xFF == ord("q") or key == 27:
+                cv2.destroyAllWindows()
+                break
+    finally:
+        realsense_data.stop_device()
+
+
+if __name__ == "__main__":
+    view_realsense()

examples/isaac_sim/simple_stacking.py

@@ -0,0 +1,487 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(
+    4, device="cuda:0"
+)  # this is necessary to allow isaac sim to use this torch instance
+# Third Party
+import numpy as np
+
+np.set_printoptions(suppress=True)
+# Standard Library
+import os.path as osp
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp({"headless": False})  # This adds paths for the following importing
+# Standard Library
+from typing import Optional
+
+# Third Party
+import carb
+from omni.isaac.core import World
+from omni.isaac.core.controllers import BaseController
+from omni.isaac.core.tasks import Stacking as BaseStacking
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.prims import is_prim_path_valid
+from omni.isaac.core.utils.stage import get_stage_units
+from omni.isaac.core.utils.string import find_unique_string_name
+from omni.isaac.core.utils.types import ArticulationAction
+from omni.isaac.core.utils.viewports import set_camera_view
+from omni.isaac.franka import Franka
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.sphere_fit import SphereFitType
+from curobo.geom.types import WorldConfig
+from curobo.rollout.rollout_base import Goal
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.types.state import JointState
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import (
+    MotionGen,
+    MotionGenConfig,
+    MotionGenPlanConfig,
+    MotionGenResult,
+)
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+ext_list = [
+    "omni.kit.asset_converter",
+    "omni.kit.livestream.native",
+    "omni.kit.tool.asset_importer",
+    "omni.isaac.asset_browser",
+    # "omni.kit.window.sequencer.scripts.sequencer_extension",
+    "omni.kit.window.movie_capture",
+]
+[enable_extension(x) for x in ext_list]  # toggle this for remote streaming
+simulation_app.update()
+
+
+class CuroboController(BaseController):
+    def __init__(
+        self,
+        my_world: World,
+        my_task: BaseStacking,
+        name: str = "curobo_controller",
+    ) -> None:
+        BaseController.__init__(self, name=name)
+        self._save_log = False
+        self.my_world = my_world
+        self.my_task = my_task
+        n_obstacle_cuboids = 20
+        n_obstacle_mesh = 2
+        # warmup curobo instance
+        self.usd_help = UsdHelper()
+        self.init_curobo = False
+        self.world_file = "collision_table.yml"
+        self.cmd_js_names = [
+            "panda_joint1",
+            "panda_joint2",
+            "panda_joint3",
+            "panda_joint4",
+            "panda_joint5",
+            "panda_joint6",
+            "panda_joint7",
+        ]
+        self.tensor_args = TensorDeviceType()
+        self.robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+        self.robot_cfg["kinematics"][
+            "base_link"
+        ] = "panda_link0"  # controls which frame the controller is controlling
+
+        self.robot_cfg["kinematics"][
+            "ee_link"
+        ] = "panda_hand"  # controls which frame the controller is controlling
+        # self.robot_cfg["kinematics"]["cspace"]["max_acceleration"] = 10.0 # controls how fast robot moves
+        self.robot_cfg["kinematics"]["extra_collision_spheres"] = {"attached_object": 100}
+        self.robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0
+        self.robot_cfg["kinematics"]["collision_spheres"] = "spheres/franka_collision_mesh.yml"
+
+        world_cfg_table = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+        )
+        self._world_cfg_table = world_cfg_table
+
+        world_cfg1 = WorldConfig.from_dict(
+            load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
+        ).get_mesh_world()
+        world_cfg1.mesh[0].pose[2] = -10.5
+
+        self._world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
+
+        motion_gen_config = MotionGenConfig.load_from_robot_config(
+            self.robot_cfg,
+            self._world_cfg,
+            self.tensor_args,
+            trajopt_tsteps=32,
+            collision_checker_type=CollisionCheckerType.MESH,
+            use_cuda_graph=True,
+            num_ik_seeds=40,
+            num_trajopt_seeds=10,
+            num_graph_seeds=10,
+            interpolation_dt=0.01,
+            collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
+            store_ik_debug=self._save_log,
+            store_trajopt_debug=self._save_log,
+            state_finite_difference_mode="CENTRAL",
+            minimize_jerk=True,
+            acceleration_scale=0.3,
+            fixed_iters_trajopt=True,
+        )
+        self.motion_gen = MotionGen(motion_gen_config)
+        print("warming up...")
+        self.motion_gen.warmup()
+
+        self.plan_config = MotionGenPlanConfig(
+            enable_graph=False,
+            max_attempts=10,
+            enable_graph_attempt=None,
+            enable_finetune_trajopt=True,
+            partial_ik_opt=False,
+        )
+        self.usd_help.load_stage(self.my_world.stage)
+        self.cmd_plan = None
+        self.cmd_idx = 0
+        self.idx_list = None
+
+    def attach_obj(
+        self,
+        sim_js: JointState,
+        js_names: list,
+    ) -> None:
+        cube_name = self.my_task.get_cube_prim(self.my_task.target_cube)
+
+        cu_js = JointState(
+            position=self.tensor_args.to_device(sim_js.positions),
+            velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=js_names,
+        )
+
+        self.motion_gen.attach_objects_to_robot(
+            cu_js,
+            [cube_name],
+            sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE,
+            world_objects_pose_offset=Pose.from_list([0, 0, 0.005, 1, 0, 0, 0], self.tensor_args),
+        )
+
+    def detach_obj(self) -> None:
+        self.motion_gen.detach_object_from_robot()
+
+    def plan(
+        self,
+        ee_translation_goal: np.array,
+        ee_orientation_goal: np.array,
+        sim_js: JointState,
+        js_names: list,
+    ) -> MotionGenResult:
+        ik_goal = Pose(
+            position=self.tensor_args.to_device(ee_translation_goal),
+            quaternion=self.tensor_args.to_device(ee_orientation_goal),
+        )
+        cu_js = JointState(
+            position=self.tensor_args.to_device(sim_js.positions),
+            velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0,
+            joint_names=js_names,
+        )
+        cu_js = cu_js.get_ordered_joint_state(self.motion_gen.kinematics.joint_names)
+        result = self.motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, self.plan_config.clone())
+        if self._save_log:  # and not result.success.item(): # logging for debugging
+            UsdHelper.write_motion_gen_log(
+                result,
+                {"robot_cfg": self.robot_cfg},
+                self._world_cfg,
+                cu_js,
+                ik_goal,
+                join_path("log/usd/", "cube") + "_debug",
+                write_ik=False,
+                write_trajopt=True,
+                visualize_robot_spheres=True,
+                link_spheres=self.motion_gen.kinematics.kinematics_config.link_spheres,
+                grid_space=2,
+                write_robot_usd_path="log/usd/assets",
+            )
+        return result
+
+    def forward(
+        self,
+        sim_js: JointState,
+        js_names: list,
+    ) -> ArticulationAction:
+        assert self.my_task.target_position is not None
+        assert self.my_task.target_cube is not None
+
+        if self.cmd_plan is None:
+            self.cmd_idx = 0
+            # Set EE goals
+            ee_translation_goal = self.my_task.target_position
+            ee_orientation_goal = np.array([0, 0, -1, 0])
+            # compute curobo solution:
+            result = self.plan(ee_translation_goal, ee_orientation_goal, sim_js, js_names)
+            succ = result.success.item()
+            if succ:
+                cmd_plan = result.get_interpolated_plan()
+                self.idx_list = [i for i in range(len(self.cmd_js_names))]
+                self.cmd_plan = cmd_plan.get_ordered_joint_state(self.cmd_js_names)
+            else:
+                carb.log_warn("Plan did not converge to a solution.")
+                return None
+
+        cmd_state = self.cmd_plan[self.cmd_idx]
+        self.cmd_idx += 1
+        # get full dof state
+        art_action = ArticulationAction(
+            cmd_state.position.cpu().numpy(),
+            # cmd_state.velocity.cpu().numpy(),
+            joint_indices=self.idx_list,
+        )
+        if self.cmd_idx >= len(self.cmd_plan.position):
+            self.cmd_idx = 0
+            self.cmd_plan = None
+
+        return art_action
+
+    def reached_target(self, observations: dict) -> bool:
+        curr_ee_position = observations["my_franka"]["end_effector_position"]
+        if np.linalg.norm(
+            self.my_task.target_position - curr_ee_position
+        ) < 0.04 and (  # This is half gripper width, curobo succ threshold is 0.5 cm
+            self.cmd_plan is None
+        ):
+            if self.my_task.cube_in_hand is None:
+                print("reached picking target: ", self.my_task.target_cube)
+            else:
+                print("reached placing target: ", self.my_task.target_cube)
+            return True
+        else:
+            return False
+
+    def reset(
+        self,
+        ignore_substring: str,
+        robot_prim_path: str,
+    ) -> None:
+        # init
+        self.update(ignore_substring, robot_prim_path)
+        self.init_curobo = True
+        self.cmd_plan = None
+        self.cmd_idx = 0
+
+    def update(
+        self,
+        ignore_substring: str,
+        robot_prim_path: str,
+    ) -> None:
+        # print("updating world...")
+        obstacles = self.usd_help.get_obstacles_from_stage(
+            ignore_substring=ignore_substring, reference_prim_path=robot_prim_path
+        ).get_collision_check_world()
+        # add ground plane as it's not readable:
+        obstacles.add_obstacle(self._world_cfg_table.cuboid[0])
+        self.motion_gen.update_world(obstacles)
+        self._world_cfg = obstacles
+
+
+class MultiModalStacking(BaseStacking):
+    def __init__(
+        self,
+        name: str = "multi_modal_stacking",
+        offset: Optional[np.ndarray] = None,
+    ) -> None:
+        BaseStacking.__init__(
+            self,
+            name=name,
+            cube_initial_positions=np.array(
+                [
+                    [0.50, 0.0, 0.1],
+                    [0.50, -0.20, 0.1],
+                    [0.50, 0.20, 0.1],
+                    [0.30, -0.20, 0.1],
+                    [0.30, 0.0, 0.1],
+                    [0.30, 0.20, 0.1],
+                    [0.70, -0.20, 0.1],
+                    [0.70, 0.0, 0.1],
+                    [0.70, 0.20, 0.1],
+                ]
+            )
+            / get_stage_units(),
+            cube_initial_orientations=None,
+            stack_target_position=None,
+            cube_size=np.array([0.045, 0.045, 0.07]),
+            offset=offset,
+        )
+        self.cube_list = None
+        self.target_position = None
+        self.target_cube = None
+        self.cube_in_hand = None
+
+    def reset(self) -> None:
+        self.cube_list = self.get_cube_names()
+        self.target_position = None
+        self.target_cube = None
+        self.cube_in_hand = None
+
+    def update_task(self) -> bool:
+        # after detaching the cube in hand
+        assert self.target_cube is not None
+        assert self.cube_in_hand is not None
+        self.cube_list.insert(0, self.cube_in_hand)
+        self.target_cube = None
+        self.target_position = None
+        self.cube_in_hand = None
+        if len(self.cube_list) <= 1:
+            task_finished = True
+        else:
+            task_finished = False
+        return task_finished
+
+    def get_cube_prim(self, cube_name: str):
+        for i in range(self._num_of_cubes):
+            if cube_name == self._cubes[i].name:
+                return self._cubes[i].prim_path
+
+    def get_place_position(self, observations: dict) -> None:
+        assert self.target_cube is not None
+        self.cube_in_hand = self.target_cube
+        self.target_cube = self.cube_list[0]
+        ee_to_grasped_cube = (
+            observations["my_franka"]["end_effector_position"][2]
+            - observations[self.cube_in_hand]["position"][2]
+        )
+        self.target_position = observations[self.target_cube]["position"] + [
+            0,
+            0,
+            self._cube_size[2] + ee_to_grasped_cube + 0.02,
+        ]
+        self.cube_list.remove(self.target_cube)
+
+    def get_pick_position(self, observations: dict) -> None:
+        assert self.cube_in_hand is None
+        self.target_cube = self.cube_list[1]
+        self.target_position = observations[self.target_cube]["position"] + [
+            0,
+            0,
+            self._cube_size[2] / 2 + 0.092,
+        ]
+        self.cube_list.remove(self.target_cube)
+
+    def set_robot(self) -> Franka:
+        franka_prim_path = find_unique_string_name(
+            initial_name="/World/Franka", is_unique_fn=lambda x: not is_prim_path_valid(x)
+        )
+        franka_robot_name = find_unique_string_name(
+            initial_name="my_franka", is_unique_fn=lambda x: not self.scene.object_exists(x)
+        )
+        return Franka(
+            prim_path=franka_prim_path, name=franka_robot_name, end_effector_prim_name="panda_hand"
+        )
+
+
+robot_prim_path = "/World/Franka/panda_link0"
+ignore_substring = ["Franka", "TargetCube", "material", "Plane"]
+my_world = World(stage_units_in_meters=1.0)
+stage = my_world.stage
+stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
+
+my_task = MultiModalStacking()
+my_world.add_task(my_task)
+my_world.reset()
+robot_name = my_task.get_params()["robot_name"]["value"]
+my_franka = my_world.scene.get_object(robot_name)
+my_controller = CuroboController(my_world=my_world, my_task=my_task)
+articulation_controller = my_franka.get_articulation_controller()
+set_camera_view(eye=[2, 0, 1], target=[0.00, 0.00, 0.00], camera_prim_path="/OmniverseKit_Persp")
+wait_steps = 8
+
+initial_steps = 10
+################################################################
+print("Start simulation...")
+my_franka.gripper.open()
+for _ in range(wait_steps):
+    my_world.step(render=True)
+my_task.reset()
+task_finished = False
+observations = my_world.get_observations()
+my_task.get_pick_position(observations)
+robot = my_franka
+print("**********************")
+# print(robot.get_solver_position_iteration_count(), robot.get_solver_velocity_iteration_count())
+robot.enable_gravity()
+robot._articulation_view.set_gains(
+    kps=np.array([100000000, 10000000]), joint_indices=np.array([0, 2])
+)
+
+robot._articulation_view.set_max_efforts(
+    values=np.array([10000, 10000]), joint_indices=np.array([0, 2])
+)
+i = 0
+while simulation_app.is_running():
+    my_world.step(render=True)  # necessary to visualize changes
+    i += 1
+
+    if task_finished or i < initial_steps:
+        continue
+
+    if not my_controller.init_curobo:
+        my_controller.reset(ignore_substring, robot_prim_path)
+
+    step_index = my_world.current_time_step_index
+    observations = my_world.get_observations()
+    sim_js = my_franka.get_joints_state()
+
+    if my_controller.reached_target(observations):
+        if my_franka.gripper.get_joint_positions()[0] < 0.035:  # reached placing target
+            my_franka.gripper.open()
+            for _ in range(wait_steps):
+                my_world.step(render=True)
+            my_controller.detach_obj()
+            my_controller.update(
+                ignore_substring, robot_prim_path
+            )  # update world collision configuration
+            task_finished = my_task.update_task()
+            if task_finished:
+                print("\nTASK DONE\n")
+                for _ in range(wait_steps):
+                    my_world.step(render=True)
+                continue
+            else:
+                my_task.get_pick_position(observations)
+
+        else:  # reached picking target
+            my_franka.gripper.close()
+            for _ in range(wait_steps):
+                my_world.step(render=True)
+            sim_js = my_franka.get_joints_state()
+            my_controller.update(ignore_substring, robot_prim_path)
+            my_controller.attach_obj(sim_js, my_franka.dof_names)
+            my_task.get_place_position(observations)
+
+    else:  # target position has been set
+        sim_js = my_franka.get_joints_state()
+        art_action = my_controller.forward(sim_js, my_franka.dof_names)
+        if art_action is not None:
+            articulation_controller.apply_action(art_action)
+            for _ in range(2):
+                my_world.step(render=False)
+
+simulation_app.close()

examples/isaac_sim/util/convert_urdf_to_usd.py

@@ -0,0 +1,172 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Standard Library
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--robot",
+    type=str,
+    default="franka.yml",
+    help="Robot configuration to download",
+)
+parser.add_argument("--save_usd", default=False, action="store_true")
+args = parser.parse_args()
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp({"headless": args.save_usd})
+
+# Third Party
+import omni.usd
+from omni.isaac.core import World
+from omni.isaac.core.robots import Robot
+from omni.isaac.core.utils.types import ArticulationAction
+
+try:
+    # Third Party
+    from omni.isaac.urdf import _urdf  # isaacsim 2022.2
+except ImportError:
+    from omni.importer.urdf import _urdf  # isaac sim 2023.1
+
+# CuRobo
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import (
+    get_assets_path,
+    get_filename,
+    get_path_of_dir,
+    get_robot_configs_path,
+    join_path,
+    load_yaml,
+)
+
+
+def save_usd():
+    my_world = World(stage_units_in_meters=1.0)
+
+    import_config = _urdf.ImportConfig()
+    import_config.merge_fixed_joints = False
+    import_config.convex_decomp = False
+    import_config.import_inertia_tensor = True
+    import_config.fix_base = True
+    import_config.make_default_prim = True
+    import_config.self_collision = False
+    import_config.create_physics_scene = True
+    import_config.import_inertia_tensor = False
+    import_config.default_drive_strength = 10000
+    import_config.default_position_drive_damping = 100
+    import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
+    import_config.distance_scale = 1
+    import_config.density = 0.0
+    # Get the urdf file path
+    robot_config = load_yaml(join_path(get_robot_configs_path(), args.robot))
+    urdf_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    asset_path = join_path(
+        get_assets_path(), robot_config["robot_cfg"]["kinematics"]["asset_root_path"]
+    )
+    urdf_interface = _urdf.acquire_urdf_interface()
+    full_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    default_config = robot_config["robot_cfg"]["kinematics"]["cspace"]["retract_config"]
+    j_names = robot_config["robot_cfg"]["kinematics"]["cspace"]["joint_names"]
+
+    robot_path = get_path_of_dir(full_path)
+    filename = get_filename(full_path)
+    imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)
+    robot_path = urdf_interface.import_robot(
+        robot_path, filename, imported_robot, import_config, ""
+    )
+    robot = my_world.scene.add(Robot(prim_path=robot_path, name="robot"))
+    # robot.disable_gravity()
+    i = 0
+
+    my_world.reset()
+
+    usd_help = UsdHelper()
+    usd_help.load_stage(my_world.stage)
+    save_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["usd_path"])
+    usd_help.write_stage_to_file(save_path, True)
+    print("Wrote usd file to " + save_path)
+    simulation_app.close()
+
+
+def debug_usd():
+    my_world = World(stage_units_in_meters=1.0)
+
+    import_config = _urdf.ImportConfig()
+    import_config.merge_fixed_joints = False
+    import_config.convex_decomp = False
+    import_config.import_inertia_tensor = True
+    import_config.fix_base = True
+    import_config.make_default_prim = True
+    import_config.self_collision = False
+    import_config.create_physics_scene = True
+    import_config.import_inertia_tensor = False
+    import_config.default_drive_strength = 10000
+    import_config.default_position_drive_damping = 100
+    import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
+    import_config.distance_scale = 1
+    import_config.density = 0.0
+    # Get the urdf file path
+    robot_config = load_yaml(join_path(get_robot_configs_path(), args.robot))
+    urdf_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    asset_path = join_path(
+        get_assets_path(), robot_config["robot_cfg"]["kinematics"]["asset_root_path"]
+    )
+    urdf_interface = _urdf.acquire_urdf_interface()
+    full_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["urdf_path"])
+    default_config = robot_config["robot_cfg"]["kinematics"]["cspace"]["retract_config"]
+    j_names = robot_config["robot_cfg"]["kinematics"]["cspace"]["joint_names"]
+
+    robot_path = get_path_of_dir(full_path)
+    filename = get_filename(full_path)
+    imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)
+    robot_path = urdf_interface.import_robot(
+        robot_path, filename, imported_robot, import_config, ""
+    )
+    robot = my_world.scene.add(Robot(prim_path=robot_path, name="robot"))
+    # robot.disable_gravity()
+    i = 0
+
+    articulation_controller = robot.get_articulation_controller()
+    my_world.reset()
+
+    while simulation_app.is_running():
+        my_world.step(render=True)
+        if i == 0:
+            idx_list = [robot.get_dof_index(x) for x in j_names]
+            robot.set_joint_positions(default_config, idx_list)
+            i += 1
+        # if dof_n is not None:
+        #    dof_i = [robot.get_dof_index(x) for x in j_names]
+        #
+        #    robot.set_joint_positions(default_config, dof_i)
+        if robot.is_valid():
+            art_action = ArticulationAction(default_config, joint_indices=idx_list)
+            articulation_controller.apply_action(art_action)
+    usd_help = UsdHelper()
+    usd_help.load_stage(my_world.stage)
+    save_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["usd_path"])
+    usd_help.write_stage_to_file(save_path, True)
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    if args.save_usd:
+        save_usd()
+    else:
+        debug_usd()

examples/isaac_sim/util/dowload_assets.py

@@ -0,0 +1,69 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# This script downloads robot usd assets from isaac sim for using in CuRobo.
+
+
+# Third Party
+import torch
+
+a = torch.zeros(4, device="cuda:0")
+
+# Third Party
+from omni.isaac.kit import SimulationApp
+
+simulation_app = SimulationApp({"headless": True})
+# Third Party
+from omni.isaac.core import World
+from omni.isaac.core.robots import Robot
+from omni.isaac.core.utils.nucleus import get_assets_root_path as nucleus_path
+from omni.isaac.core.utils.stage import add_reference_to_stage
+
+# CuRobo
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import get_assets_path, get_robot_configs_path, join_path, load_yaml
+
+# supported robots:
+robots = ["franka.yml", "ur10.yml"]
+# Standard Library
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "--robot",
+    type=str,
+    default="franka.yml",
+    help="Robot configuration to download",
+)
+args = parser.parse_args()
+
+if __name__ == "__main__":
+    r = args.robot
+    my_world = World(stage_units_in_meters=1.0)
+    robot_config = load_yaml(join_path(get_robot_configs_path(), r))
+    usd_path = nucleus_path() + robot_config["robot_cfg"]["kinematics"]["isaac_usd_path"]
+
+    usd_help = UsdHelper()
+    robot_name = r
+    prim_path = robot_config["robot_cfg"]["kinematics"]["usd_robot_root"]
+    add_reference_to_stage(usd_path=usd_path, prim_path=prim_path)
+    robot = my_world.scene.add(Robot(prim_path=prim_path, name=robot_name))
+    usd_help.load_stage(my_world.stage)
+
+    my_world.reset()
+    articulation_controller = robot.get_articulation_controller()
+
+    # create a new stage and add robot to usd path:
+    save_path = join_path(get_assets_path(), robot_config["robot_cfg"]["kinematics"]["usd_path"])
+    usd_help.write_stage_to_file(save_path, True)
+    my_world.clear()
+    my_world.clear_instance()
+    simulation_app.close()