release repository

examples/collision_check_example.py

@@ -0,0 +1,57 @@
+#
+# 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.
+#
+""" Example computing collisions using curobo
+
+"""
+# Third Party
+import torch
+
+# CuRobo
+from curobo.types.base import TensorDeviceType
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+if __name__ == "__main__":
+    robot_file = "franka.yml"
+    world_file = "collision_test.yml"
+    tensor_args = TensorDeviceType()
+    # config = RobotWorldConfig.load_from_config(robot_file, world_file, pose_weight=[10, 200, 1, 10],
+    #                                           collision_activation_distance=0.0)
+    # curobo_fn = RobotWorld(config)
+    robot_file = "franka.yml"
+    world_config = {
+        "cuboid": {
+            "table": {"dims": [2, 2, 0.2], "pose": [0.4, 0.0, 0.3, 1, 0, 0, 0]},
+            "cube_1": {"dims": [0.1, 0.1, 0.2], "pose": [0.4, 0.0, 0.5, 1, 0, 0, 0]},
+        },
+        "mesh": {
+            "scene": {
+                "pose": [1.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
+                "file_path": "scene/nvblox/srl_ur10_bins.obj",
+            }
+        },
+    }
+    tensor_args = TensorDeviceType()
+    config = RobotWorldConfig.load_from_config(
+        robot_file, world_file, collision_activation_distance=0.0
+    )
+    curobo_fn = RobotWorld(config)
+
+    q_sph = torch.randn((10, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
+    q_sph[..., 3] = 0.2
+    d = curobo_fn.get_collision_distance(q_sph)
+    print(d)
+
+    q_s = curobo_fn.sample(5, mask_valid=False)
+
+    d_world, d_self = curobo_fn.get_world_self_collision_distance_from_joints(q_s)
+    print("Collision Distance:")
+    print("World:", d_world)
+    print("Self:", d_self)

examples/ik_example.py

@@ -0,0 +1,234 @@
+#
+# 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
+import time
+
+# Third Party
+import torch
+
+# CuRobo
+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.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+
+def demo_basic_ik():
+    tensor_args = TensorDeviceType()
+
+    config_file = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))
+    urdf_file = config_file["robot_cfg"]["kinematics"][
+        "urdf_path"
+    ]  # Send global path starting with "/"
+    base_link = config_file["robot_cfg"]["kinematics"]["base_link"]
+    ee_link = config_file["robot_cfg"]["kinematics"]["ee_link"]
+    robot_cfg = RobotConfig.from_basic(urdf_file, base_link, ee_link, tensor_args)
+
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        None,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=20,
+        self_collision_check=False,
+        self_collision_opt=False,
+        tensor_args=tensor_args,
+        use_cuda_graph=False,
+    )
+    ik_solver = IKSolver(ik_config)
+
+    # print(kin_state)
+    for _ in range(10):
+        q_sample = ik_solver.sample_configs(5000)
+        kin_state = ik_solver.fk(q_sample)
+        goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+        st_time = time.time()
+        result = ik_solver.solve_batch(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s) ",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            q_sample.shape[0] / (time.time() - st_time),
+            torch.mean(result.position_error) * 100.0,
+            torch.mean(result.rotation_error) * 100.0,
+        )
+
+
+def demo_full_config_collision_free_ik():
+    tensor_args = TensorDeviceType()
+    world_file = "collision_cage.yml"
+
+    robot_file = "franka.yml"
+    robot_cfg = RobotConfig.from_dict(
+        load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    )
+    world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+    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,
+        # use_fixed_samples=True,
+    )
+    ik_solver = IKSolver(ik_config)
+
+    # print(kin_state)
+    print("Running Single IK")
+    for _ in range(10):
+        q_sample = ik_solver.sample_configs(5000)
+        kin_state = ik_solver.fk(q_sample)
+        goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+        st_time = time.time()
+        result = ik_solver.solve_batch(goal)
+        torch.cuda.synchronize()
+        total_time = (time.time() - st_time) / q_sample.shape[0]
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            total_time,
+            1.0 / total_time,
+            torch.mean(result.position_error) * 100.0,
+            torch.mean(result.rotation_error) * 100.0,
+        )
+    exit()
+    print("Running Batch IK (10 goals)")
+    q_sample = ik_solver.sample_configs(10)
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+    print("Running Goalset IK (10 goals in 1 set)")
+    q_sample = ik_solver.sample_configs(10)
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(kin_state.ee_position.unsqueeze(0), kin_state.ee_quaternion.unsqueeze(0))
+
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_goalset(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(result.success),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+    print("Running Batch Goalset IK (10 goals in 10 sets)")
+    q_sample = ik_solver.sample_configs(100)
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(
+        kin_state.ee_position.view(10, 10, 3).contiguous(),
+        kin_state.ee_quaternion.view(10, 10, 4).contiguous(),
+    )
+
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch_goalset(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(result.success.view(-1)),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+
+def demo_full_config_batch_env_collision_free_ik():
+    tensor_args = TensorDeviceType()
+    world_file = ["collision_test.yml", "collision_cubby.yml"]
+
+    robot_file = "franka.yml"
+    robot_cfg = RobotConfig.from_dict(
+        load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    )
+    world_cfg = [
+        WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), x))) for x in world_file
+    ]
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=100,
+        self_collision_check=True,
+        self_collision_opt=True,
+        tensor_args=tensor_args,
+        use_cuda_graph=False,
+        # use_fixed_samples=True,
+    )
+    ik_solver = IKSolver(ik_config)
+    q_sample = ik_solver.sample_configs(len(world_file))
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+    print("Running Batch Env IK")
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch_env(goal)
+        print(result.success)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(q_sample),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+    q_sample = ik_solver.sample_configs(10 * len(world_file))
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(
+        kin_state.ee_position.view(len(world_file), 10, 3),
+        kin_state.ee_quaternion.view(len(world_file), 10, 4),
+    )
+
+    print("Running Batch Env Goalset IK")
+    for _ in range(3):
+        st_time = time.time()
+        result = ik_solver.solve_batch_env_goalset(goal)
+        torch.cuda.synchronize()
+        print(
+            "Success, Solve Time(s), Total Time(s)",
+            torch.count_nonzero(result.success).item() / len(result.success.view(-1)),
+            result.solve_time,
+            time.time() - st_time,
+        )
+
+
+if __name__ == "__main__":
+    demo_basic_ik()
+    # demo_full_config_collision_free_ik()
+    # demo_full_config_batch_env_collision_free_ik()

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()

examples/kinematics_example.py

@@ -0,0 +1,66 @@
+#
+# 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.
+#
+""" Example computing fk using curobo
+
+"""
+# Third Party
+import torch
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.robot import RobotConfig
+from curobo.util.logger import setup_curobo_logger
+from curobo.util_file import get_robot_path, join_path, load_yaml
+
+
+def demo_basic_robot():
+    tensor_args = TensorDeviceType()
+
+    # load a urdf:
+    config_file = load_yaml(join_path(get_robot_path(), "franka.yml"))
+
+    urdf_file = config_file["robot_cfg"]["kinematics"][
+        "urdf_path"
+    ]  # Send global path starting with "/"
+    base_link = config_file["robot_cfg"]["kinematics"]["base_link"]
+    ee_link = config_file["robot_cfg"]["kinematics"]["ee_link"]
+    robot_cfg = RobotConfig.from_basic(urdf_file, base_link, ee_link, tensor_args)
+
+    kin_model = CudaRobotModel(robot_cfg.kinematics)
+
+    # compute forward kinematics:
+
+    q = torch.rand((10, kin_model.get_dof()), **vars(tensor_args))
+    out = kin_model.get_state(q)
+    # here is the kinematics state:
+    # print(out)
+
+
+def demo_full_config_robot():
+    setup_curobo_logger("info")
+    tensor_args = TensorDeviceType()
+    # load a urdf:
+    config_file = load_yaml(join_path(get_robot_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(config_file, tensor_args)
+
+    kin_model = CudaRobotModel(robot_cfg.kinematics)
+
+    # compute forward kinematics:
+    q = torch.rand((10, kin_model.get_dof()), **vars(tensor_args))
+    out = kin_model.get_state(q)
+    # here is the kinematics state:
+    # print(out)
+
+
+if __name__ == "__main__":
+    demo_basic_robot()
+    demo_full_config_robot()

examples/motion_gen_api_example.py

@@ -0,0 +1,101 @@
+#
+# 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
+
+# CuRobo
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState
+from curobo.util.usd_helper import UsdHelper
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig
+
+
+def demo_motion_gen_api():
+    tensor_args = TensorDeviceType(device=torch.device("cuda:0"))
+
+    interpolation_dt = 0.02
+    collision_activation_distance = 0.02  # meters
+    # create motion gen with a cuboid cache to be able to load obstacles later:
+    motion_gen_cfg = MotionGenConfig.load_from_robot_config(
+        "franka.yml",
+        "collision_table.yml",
+        tensor_args,
+        trajopt_tsteps=34,
+        interpolation_steps=5000,
+        num_ik_seeds=50,
+        num_trajopt_seeds=6,
+        grad_trajopt_iters=500,
+        trajopt_dt=0.5,
+        interpolation_dt=interpolation_dt,
+        evaluate_interpolated_trajectory=True,
+        js_trajopt_dt=0.5,
+        js_trajopt_tsteps=34,
+        collision_activation_distance=collision_activation_distance,
+    )
+    motion_gen = MotionGen(motion_gen_cfg)
+
+    motion_gen.warmup()
+
+    # create world representation:
+    motion_gen.world_coll_checker.clear_cache()
+    motion_gen.reset(reset_seed=False)
+    cuboids = [Cuboid(name="obs_1", pose=[0.9, 0.0, 0.5, 1, 0, 0, 0], dims=[0.1, 0.5, 0.5])]
+    world = WorldConfig(cuboid=cuboids)
+
+    motion_gen.update_world(world)
+
+    q_start = JointState.from_position(
+        tensor_args.to_device([[0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0.0]]),
+        joint_names=[
+            "panda_joint1",
+            "panda_joint2",
+            "panda_joint3",
+            "panda_joint4",
+            "panda_joint5",
+            "panda_joint6",
+            "panda_joint7",
+        ],
+    )
+
+    goal_pose = Pose(
+        position=tensor_args.to_device([[0.5, 0.0, 0.3]]),
+        quaternion=tensor_args.to_device([[1, 0, 0, 0]]),
+    )
+
+    result = motion_gen.plan_single(q_start, goal_pose)
+
+    if result.success.item():
+        # get result:
+        # this contains trajectory with 34 tsteps and the final
+        # result.optimized_plan
+        # result.optimized_dt
+
+        # this contains a linearly interpolated trajectory with fixed dt
+        interpolated_solution = result.get_interpolated_plan()
+
+        UsdHelper.write_trajectory_animation_with_robot_usd(
+            "franka.yml",
+            world,
+            q_start,
+            interpolated_solution,
+            dt=result.interpolation_dt,
+            save_path="demo.usd",
+            base_frame="/world_base",
+        )
+    else:
+        print("Failed")
+
+
+if __name__ == "__main__":
+    demo_motion_gen_api()

examples/motion_gen_example.py

@@ -0,0 +1,366 @@
+#
+# 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
+
+# Third Party
+import torch
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.geom.types import Cuboid, WorldConfig
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util.logger import setup_curobo_logger
+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
+
+
+def plot_traj(trajectory, dt):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(4, 1)
+    q = trajectory.position.cpu().numpy()
+    qd = trajectory.velocity.cpu().numpy()
+    qdd = trajectory.acceleration.cpu().numpy()
+    qddd = trajectory.jerk.cpu().numpy()
+    timesteps = [i * dt for i in range(q.shape[0])]
+    for i in range(q.shape[-1]):
+        axs[0].plot(timesteps, q[:, i], label=str(i))
+        axs[1].plot(timesteps, qd[:, i], label=str(i))
+        axs[2].plot(timesteps, qdd[:, i], label=str(i))
+        axs[3].plot(timesteps, qddd[:, i], label=str(i))
+
+    plt.legend()
+    # plt.savefig("test.png")
+    plt.show()
+
+
+def plot_iters_traj(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(len(trajectory), 1)
+    if len(trajectory) == 1:
+        axs = [axs]
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            axs[k].plot(
+                q[i][seed, :-1, d_id].cpu(),
+                "r+-",
+                label=str(i),
+                alpha=0.1 + min(0.9, float(i) / (len(q))),
+            )
+    plt.legend()
+    plt.show()
+
+
+def plot_iters_traj_3d(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    ax = plt.axes(projection="3d")
+    c = 0
+    h = trajectory[0][0].shape[1] - 1
+    x = [x for x in range(h)]
+
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            # ax.plot3D(x,[c for _ in range(h)],  q[i][seed, :, d_id].cpu())#, 'r')
+            ax.scatter3D(
+                x, [c for _ in range(h)], q[i][seed, :h, d_id].cpu(), c=q[i][seed, :, d_id].cpu()
+            )
+            # @plt.show()
+            c += 1
+    # plt.legend()
+    plt.show()
+
+
+def demo_motion_gen_mesh():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_mesh_scene.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=40,
+        collision_checker_type=CollisionCheckerType.MESH,
+        use_cuda_graph=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = robot_cfg.retract_config
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.5)
+    result = motion_gen.plan(
+        start_state,
+        retract_pose,
+        enable_graph=False,
+        enable_opt=True,
+        max_attempts=1,
+        num_trajopt_seeds=10,
+        num_graph_seeds=10,
+    )
+    print(result.status, result.attempts, result.trajopt_time)
+    traj = result.raw_plan  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    if PLOT:
+        plot_traj(traj.cpu().numpy())
+
+
+def demo_motion_gen():
+    # Standard Library
+    PLOT = False
+    tensor_args = TensorDeviceType()
+    world_file = "collision_table.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        interpolation_dt=0.01,
+    )
+
+    motion_gen = MotionGen(motion_gen_config)
+
+    motion_gen.warmup(enable_graph=False)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = motion_gen.get_retract_config()
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
+    result = motion_gen.plan_single(start_state, retract_pose, MotionGenPlanConfig(max_attempts=1))
+    traj = result.get_interpolated_plan()
+    print("Trajectory Generated: ", result.success, result.optimized_dt.item())
+    if PLOT:
+        plot_traj(traj, result.interpolation_dt)
+        # plt.save("test.png")
+        # plt.close()
+        # traj = result.debug_info["opt_solution"].position.view(-1,7)
+        # plot_traj(traj.cpu().numpy())
+
+
+def demo_motion_gen_debug():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_cubby.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=24,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=True,
+        num_trajopt_seeds=1,
+        num_graph_seeds=1,
+        store_ik_debug=True,
+        store_trajopt_debug=True,
+        trajopt_particle_opt=False,
+        grad_trajopt_iters=100,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = robot_cfg.cspace.retract_config
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.4)
+    result = motion_gen.plan(start_state, retract_pose, enable_graph=True, enable_opt=True)
+    if result.status not in [None, "Opt Fail"]:
+        return
+    traj = result.plan.view(-1, 7)  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    trajectory_iter_steps = result.debug_info["trajopt_result"].debug_info["solver"]["steps"]
+
+    if PLOT:
+        plot_iters_traj_3d(trajectory_iter_steps, d_id=6)
+        # plot_traj(traj.cpu().numpy())
+
+
+def demo_motion_gen_batch():
+    PLOT = False
+    tensor_args = TensorDeviceType()
+    world_file = "collision_cubby.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=30,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=True,
+        num_trajopt_seeds=4,
+        num_graph_seeds=1,
+        num_ik_seeds=30,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = motion_gen.get_retract_config()
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.6)
+
+    retract_pose = retract_pose.repeat_seeds(2)
+    retract_pose.position[0, 0] = -0.3
+    for _ in range(2):
+        result = motion_gen.plan_batch(
+            start_state.repeat_seeds(2),
+            retract_pose,
+            MotionGenPlanConfig(enable_graph=False, enable_opt=True),
+        )
+    traj = result.optimized_plan.position.view(2, -1, 7)  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    if PLOT:
+        plot_traj(traj[0, : result.path_buffer_last_tstep[0], :].cpu().numpy())
+        plot_traj(traj[1, : result.path_buffer_last_tstep[1], :].cpu().numpy())
+
+
+def demo_motion_gen_api():
+    tensor_args = TensorDeviceType(device=torch.device("cuda:0"))
+    interpolation_dt = 0.02
+    # create motion gen with a cuboid cache to be able to load obstacles later:
+    motion_gen_cfg = MotionGenConfig.load_from_robot_config(
+        "franka.yml",
+        "collision_table.yml",
+        tensor_args,
+        trajopt_tsteps=34,
+        interpolation_steps=5000,
+        num_ik_seeds=50,
+        num_trajopt_seeds=6,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        grad_trajopt_iters=500,
+        trajopt_dt=0.5,
+        interpolation_dt=interpolation_dt,
+        evaluate_interpolated_trajectory=True,
+        js_trajopt_dt=0.5,
+        js_trajopt_tsteps=34,
+    )
+    motion_gen = MotionGen(motion_gen_cfg)
+
+    motion_gen.warmup(warmup_js_trajopt=False)
+
+    # create world representation:
+
+    cuboids = [Cuboid(name="obs_1", pose=[0, 0, 0, 1, 0, 0, 0], dims=[0.1, 0.1, 0.1])]
+    world = WorldConfig(cuboid=cuboids)
+
+    motion_gen.update_world(world)
+
+    q_start = JointState.from_position(
+        tensor_args.to_device([[0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0.0]]),
+        joint_names=[
+            "panda_joint1",
+            "panda_joint2",
+            "panda_joint3",
+            "panda_joint4",
+            "panda_joint5",
+            "panda_joint6",
+            "panda_joint7",
+        ],
+    )
+
+    goal_pose = Pose(
+        position=tensor_args.to_device([[0.5, 0.0, 0.3]]),
+        quaternion=tensor_args.to_device([[1, 0, 0, 0]]),
+    )
+    print(goal_pose)
+
+    result = motion_gen.plan_single(q_start, goal_pose)
+
+    # get result:
+
+    interpolated_solution = result.get_interpolated_plan()
+
+    print(result.get_interpolated_plan())
+
+
+def demo_motion_gen_batch_env(n_envs: int = 10):
+    tensor_args = TensorDeviceType()
+    world_files = ["collision_table.yml" for _ in range(int(n_envs / 2))] + [
+        "collision_test.yml" for _ in range(int(n_envs / 2))
+    ]
+
+    world_cfg = [
+        WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+        for world_file in world_files
+    ]
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_cfg,
+        tensor_args,
+        trajopt_tsteps=30,
+        use_cuda_graph=False,
+        num_trajopt_seeds=4,
+        num_ik_seeds=30,
+        num_batch_ik_seeds=30,
+        evaluate_interpolated_trajectory=True,
+        interpolation_dt=0.05,
+        interpolation_steps=500,
+        grad_trajopt_iters=30,
+    )
+    motion_gen_batch_env = MotionGen(motion_gen_config)
+    motion_gen_batch_env.reset()
+    motion_gen_batch_env.warmup(
+        enable_graph=False, batch=n_envs, warmup_js_trajopt=False, batch_env_mode=True
+    )
+    retract_cfg = motion_gen_batch_env.get_retract_config().clone()
+    state = motion_gen_batch_env.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    goal_pose = Pose(
+        state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze()
+    ).repeat_seeds(n_envs)
+
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3).repeat_seeds(n_envs)
+
+    goal_pose.position[1, 0] -= 0.2
+
+    m_config = MotionGenPlanConfig(
+        False, True, max_attempts=1, enable_graph_attempt=None, enable_finetune_trajopt=False
+    )
+    result = motion_gen_batch_env.plan_batch_env(start_state, goal_pose, m_config)
+
+    print(n_envs, result.total_time, result.total_time / n_envs)
+
+
+if __name__ == "__main__":
+    setup_curobo_logger("error")
+    demo_motion_gen()
+    n = [2, 10]
+    for n_envs in n:
+        demo_motion_gen_batch_env(n_envs=n_envs)

examples/motion_gen_profile.py

@@ -0,0 +1,81 @@
+#
+# 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
+
+
+# CuRobo
+
+# Third Party
+from torch.profiler import ProfilerActivity, profile
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util_file import get_robot_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig
+
+
+def plot_traj(trajectory):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(1, 1)
+    q = trajectory
+
+    for i in range(q.shape[-1]):
+        axs.plot(q[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def demo_motion_gen():
+    PLOT = False
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=40,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=False,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = robot_cfg.retract_config
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.5)
+    result = motion_gen.plan(start_state, retract_pose, enable_graph=False)
+
+    # profile:
+    with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+        result = motion_gen.plan(start_state, retract_pose, enable_graph=False)
+
+    print("Exporting the trace..")
+    prof.export_chrome_trace("trace.json")
+    exit(10)
+    traj = result.raw_plan  # optimized plan
+    print("Trajectory Generated: ", result.success)
+    if PLOT:
+        plot_traj(traj.cpu().numpy())
+
+
+if __name__ == "__main__":
+    demo_motion_gen()

examples/mpc_example.py

@@ -0,0 +1,131 @@
+#
+# 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
+import time
+
+# Third Party
+import numpy as np
+import torch
+
+# CuRobo
+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.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
+
+
+def plot_traj(trajectory, dof):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(3, 1)
+    q = trajectory[:, :dof]
+    qd = trajectory[:, dof : dof * 2]
+    qdd = trajectory[:, dof * 2 : dof * 3]
+
+    for i in range(q.shape[-1]):
+        axs[0].plot(q[:, i], label=str(i))
+        axs[1].plot(qd[:, i], label=str(i))
+        axs[2].plot(qdd[:, i], label=str(i))
+    plt.legend()
+    plt.savefig("test.png")
+    # plt.show()
+
+
+def demo_full_config_mpc():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+
+    mpc_config = MpcSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_file,
+        use_cuda_graph=True,
+        use_cuda_graph_metrics=True,
+        use_cuda_graph_full_step=False,
+        use_lbfgs=False,
+        use_es=False,
+        use_mppi=True,
+        store_rollouts=True,
+        step_dt=0.03,
+    )
+    mpc = MpcSolver(mpc_config)
+
+    # retract_cfg = robot_cfg.cspace.retract_config.view(1, -1)
+    retract_cfg = mpc.rollout_fn.dynamics_model.retract_config.unsqueeze(0)
+    joint_names = mpc.joint_names
+
+    state = mpc.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg + 0.5, joint_names=joint_names)
+    )
+    retract_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+    start_state = JointState.from_position(retract_cfg, joint_names=joint_names)
+
+    goal = Goal(
+        current_state=start_state,
+        goal_state=JointState.from_position(retract_cfg, joint_names=joint_names),
+        goal_pose=retract_pose,
+    )
+    goal_buffer = mpc.setup_solve_single(goal, 1)
+
+    # test_q = tensor_args.to_device( [2.7735, -1.6737,  0.4998, -2.9865,  0.3386,  0.8413,  0.4371])
+    # start_state.position[:] = test_q
+    converged = False
+    tstep = 0
+    traj_list = []
+    mpc_time = []
+    mpc.update_goal(goal_buffer)
+    current_state = start_state  # .clone()
+    while not converged:
+        st_time = time.time()
+        # current_state.position += 0.1
+        print(current_state.position)
+        result = mpc.step(current_state, 1)
+
+        print(mpc.get_visual_rollouts().shape)
+        # exit()
+        torch.cuda.synchronize()
+        if tstep > 5:
+            mpc_time.append(time.time() - st_time)
+        # goal_buffer.current_state.position[:] = result.action.position
+        # result.action.position += 0.1
+        current_state.copy_(result.action)
+        # goal_buffer.current_state.velocity[:] = result.action.vel
+        traj_list.append(result.action.get_state_tensor())
+        tstep += 1
+        # if tstep % 10 == 0:
+        #    print(result.metrics.pose_error.item(), result.solve_time, mpc_time[-1])
+        if result.metrics.pose_error.item() < 0.01:
+            converged = True
+        if tstep > 1000:
+            break
+    print(
+        "MPC (converged, error, steps, opt_time, mpc_time): ",
+        converged,
+        result.metrics.pose_error.item(),
+        tstep,
+        result.solve_time,
+        np.mean(mpc_time),
+    )
+    if PLOT:
+        plot_traj(torch.cat(traj_list, dim=0).cpu().numpy(), dof=retract_cfg.shape[-1])
+
+
+if __name__ == "__main__":
+    demo_full_config_mpc()
+    # demo_full_config_mesh_mpc()

examples/nvblox_example.py

@@ -0,0 +1,120 @@
+#
+# 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
+
+
+# CuRobo
+
+# CuRobo
+from curobo.geom.sdf.world import CollisionCheckerType
+from curobo.types.base import TensorDeviceType
+from curobo.types.math import Pose
+from curobo.types.robot import JointState, RobotConfig
+from curobo.util_file import get_robot_configs_path, join_path, load_yaml
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+
+def plot_traj(trajectory):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(1, 1)
+    q = trajectory
+
+    for i in range(q.shape[-1]):
+        axs.plot(q[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def plot_iters_traj(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(len(trajectory), 1)
+    if len(trajectory) == 1:
+        axs = [axs]
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            axs[k].plot(
+                q[i][seed, :-1, d_id].cpu(),
+                "r+-",
+                label=str(i),
+                alpha=0.1 + min(0.9, float(i) / (len(q))),
+            )
+    plt.legend()
+    plt.show()
+
+
+def plot_iters_traj_3d(trajectory, d_id=1, dof=7, seed=0):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    ax = plt.axes(projection="3d")
+    c = 0
+    h = trajectory[0][0].shape[1] - 1
+    x = [x for x in range(h)]
+
+    for k in range(len(trajectory)):
+        q = trajectory[k]
+
+        for i in range(len(q)):
+            # ax.plot3D(x,[c for _ in range(h)],  q[i][seed, :, d_id].cpu())#, 'r')
+            ax.scatter3D(
+                x, [c for _ in range(h)], q[i][seed, :h, d_id].cpu(), c=q[i][seed, :, d_id].cpu()
+            )
+            # @plt.show()
+            c += 1
+    # plt.legend()
+    plt.show()
+
+
+def demo_motion_gen_nvblox():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_nvblox.yml"
+    robot_file = "franka.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=32,
+        collision_checker_type=CollisionCheckerType.BLOX,
+        use_cuda_graph=False,
+        num_trajopt_seeds=2,
+        num_graph_seeds=2,
+        evaluate_interpolated_trajectory=True,
+    )
+    goals = tensor_args.to_device(
+        [
+            [0.5881, 0.0589, 0.3055],
+            [0.5881, 0.4155, 0.3055],
+            [0.5881, 0.4155, 0.1238],
+            [0.5881, -0.4093, 0.1238],
+            [0.7451, 0.0287, 0.2539],
+        ]
+    ).view(-1, 3)
+
+    motion_gen = MotionGen(motion_gen_config)
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    motion_gen.warmup()
+    print("ready")
+    # print("Trajectory Generated: ", result.success)
+    # if PLOT:
+    #    plot_traj(traj.cpu().numpy())
+
+
+if __name__ == "__main__":
+    demo_motion_gen_nvblox()

examples/torch_layers_example.py

@@ -0,0 +1,229 @@
+#
+# 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 example will use CuRobo's functions as pytorch layers as part of a NN."""
+# Standard Library
+import uuid
+from typing import Optional
+
+# Third Party
+import numpy as np
+import torch
+from torch import nn
+from torch.utils.tensorboard import SummaryWriter
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import WorldConfig
+from curobo.geom.types import Mesh
+from curobo.types.math import Pose
+from curobo.types.state import JointState
+from curobo.util.usd_helper import UsdHelper
+from curobo.util_file import get_assets_path, get_world_configs_path, join_path, load_yaml
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+
+
+class CuroboTorch(torch.nn.Module):
+    def __init__(self, robot_world: RobotWorld):
+        """Build a simple structured NN:
+
+        q_current -> kinematics -> sdf -> features
+        [features, x_des] -> NN -> kinematics -> sdf -> [sdf, pose_distance] -> NN -> q_out
+        loss = (fk(q_out) - x_des) + (q_current - q_out) + valid(q_out)
+        """
+        super(CuroboTorch, self).__init__()
+
+        feature_dims = robot_world.kinematics.kinematics_config.link_spheres.shape[0] * 5 + 7 + 1
+        q_feature_dims = 7
+        final_feature_dims = feature_dims + 1 + 7
+        output_dims = robot_world.kinematics.get_dof()
+
+        # build neural network:
+        self._robot_world = robot_world
+
+        self._feature_mlp = nn.Sequential(
+            nn.Linear(q_feature_dims, 512),
+            nn.ReLU6(),
+            nn.Linear(512, 512),
+            nn.ReLU6(),
+            nn.Linear(512, 512),
+            nn.ReLU6(),
+            nn.Linear(512, output_dims),
+            nn.Tanh(),
+        )
+        self._final_mlp = nn.Sequential(
+            nn.Linear(final_feature_dims, 256),
+            nn.ReLU6(),
+            nn.Linear(256, 256),
+            nn.ReLU6(),
+            nn.Linear(256, 64),
+            nn.ReLU6(),
+            nn.Linear(64, output_dims),
+            nn.Tanh(),
+        )
+
+    def get_features(self, q: torch.Tensor, x_des: Optional[Pose] = None):
+        kin_state = self._robot_world.get_kinematics(q)
+        spheres = kin_state.link_spheres_tensor.unsqueeze(2)
+        q_sdf = self._robot_world.get_collision_distance(spheres)
+        q_self = self._robot_world.get_self_collision_distance(
+            kin_state.link_spheres_tensor.unsqueeze(1)
+        )
+
+        features = [
+            kin_state.link_spheres_tensor.view(q.shape[0], -1),
+            q_sdf,
+            q_self,
+            kin_state.ee_position,
+            kin_state.ee_quaternion,
+        ]
+        if x_des is not None:
+            pose_distance = self._robot_world.pose_distance(x_des, kin_state.ee_pose)
+            features.append(pose_distance)
+            features.append(x_des.position)
+            features.append(x_des.quaternion)
+        features = torch.cat(features, dim=-1)
+
+        return features
+
+    def forward(self, q: torch.Tensor, x_des: Pose):
+        """Forward for neural network
+
+        Args:
+            q (torch.Tensor): _description_
+            x_des (torch.Tensor): _description_
+        """
+        # get features for input:
+        in_features = torch.cat([x_des.position, x_des.quaternion], dim=-1)
+        # pass through initial mlp:
+        q_mid = self._feature_mlp(in_features)
+
+        q_scale = self._robot_world.bound_scale * q_mid
+        # get new features:
+        mid_features = self.get_features(q_scale, x_des=x_des)
+        q_out = self._final_mlp(mid_features)
+        q_out = self._robot_world.bound_scale * q_out
+        return q_out
+
+    def loss(self, x_des: Pose, q: torch.Tensor, q_in: torch.Tensor):
+        kin_state = self._robot_world.get_kinematics(q)
+        distance = self._robot_world.pose_distance(x_des, kin_state.ee_pose)
+        d_sdf = self._robot_world.collision_constraint(kin_state.link_spheres_tensor.unsqueeze(1))
+        d_self = self._robot_world.self_collision_cost(kin_state.link_spheres_tensor.unsqueeze(1))
+        loss = 0.1 * torch.linalg.norm(q_in - q, dim=-1) + distance + 100.0 * (d_self + d_sdf)
+        return loss
+
+    def val_loss(self, x_des: Pose, q: torch.Tensor, q_in: torch.Tensor):
+        kin_state = self._robot_world.get_kinematics(q)
+        distance = self._robot_world.pose_distance(x_des, kin_state.ee_pose)
+        d_sdf = self._robot_world.collision_constraint(kin_state.link_spheres_tensor.unsqueeze(1))
+        d_self = self._robot_world.self_collision_cost(kin_state.link_spheres_tensor.unsqueeze(1))
+        loss = 10.0 * (d_self + d_sdf) + distance
+        return loss
+
+
+if __name__ == "__main__":
+    update_goal = False
+    write_usd = False
+    writer = SummaryWriter("log/runs/ik/" + str(uuid.uuid4()))
+    robot_file = "franka.yml"
+    world_file = "collision_table.yml"
+    config = RobotWorldConfig.load_from_config(robot_file, world_file, pose_weight=[10, 200, 1, 10])
+    curobo_fn = RobotWorld(config)
+
+    model = CuroboTorch(curobo_fn)
+    model.cuda()
+    with torch.no_grad():
+        # q_train = curobo_fn.sample(10000)
+        q_val = curobo_fn.sample(100)
+        q_train = curobo_fn.sample(5 * 2048)
+    usd_list = []
+    optimizer = torch.optim.Adam(model.parameters(), lr=1e-3, weight_decay=1e-8)
+    batch_size = 512
+    batch_start = torch.arange(0, q_train.shape[0], batch_size)
+    with torch.no_grad():
+        x_des = curobo_fn.get_kinematics(q_train[1:2]).ee_pose
+        x_des_train = curobo_fn.get_kinematics(q_train).ee_pose
+        x_des_val = x_des.repeat(q_val.shape[0])
+    q_debug = []
+    bar = tqdm(range(500))
+    for e in bar:
+        model.train()
+        for j in range(batch_start.shape[0]):
+            x_train = q_train[batch_start[j] : batch_start[j] + batch_size]
+            if x_train.shape[0] != batch_size:
+                continue
+            x_des_batch = x_des_train[batch_start[j] : batch_start[j] + batch_size]
+            q = model.forward(x_train, x_des_batch)
+            loss = model.loss(x_des_batch, q, x_train)
+            loss = torch.mean(loss)
+
+            optimizer.zero_grad()
+            loss.backward()
+            optimizer.step()
+        writer.add_scalar("training loss", loss.item(), e)
+        model.eval()
+        with torch.no_grad():
+            q_pred = model.forward(q_val, x_des_val)
+            val_loss = model.val_loss(x_des_val, q_pred, q_val)
+            val_loss = torch.mean(val_loss)
+            q_debug.append(q_pred[0:1].clone())
+        writer.add_scalar("validation loss", val_loss.item(), e)
+        bar.set_description("t: " + str(val_loss.item()))
+        if e % 100 == 0 and len(q_debug) > 1:
+            if write_usd:
+                q_traj = torch.cat(q_debug, dim=0)
+                world_model = WorldConfig.from_dict(
+                    load_yaml(join_path(get_world_configs_path(), world_file))
+                )
+                gripper_mesh = Mesh(
+                    name="target_gripper",
+                    file_path=join_path(
+                        get_assets_path(),
+                        "robot/franka_description/meshes/visual/hand_ee_link.dae",
+                    ),
+                    color=[0.0, 0.8, 0.1, 1.0],
+                    pose=x_des[0].tolist(),
+                )
+                world_model.add_obstacle(gripper_mesh)
+                save_name = "e_" + str(e)
+                UsdHelper.write_trajectory_animation_with_robot_usd(
+                    "franka.yml",
+                    world_model,
+                    JointState(position=q_traj[0]),
+                    JointState(position=q_traj),
+                    dt=1.0,
+                    visualize_robot_spheres=False,
+                    save_path=save_name + ".usd",
+                    base_frame="/" + save_name,
+                )
+                usd_list.append(save_name + ".usd")
+            if update_goal:
+                with torch.no_grad():
+                    rand_perm = torch.randperm(q_val.shape[0])
+                    q_val = q_val[rand_perm].clone()
+                    x_des = curobo_fn.get_kinematics(q_val[0:1]).ee_pose
+                    x_des_val = x_des.repeat(q_val.shape[0])
+            q_debug = []
+
+    # create loss function:
+    if write_usd:
+        UsdHelper.create_grid_usd(
+            usd_list,
+            "epoch_grid.usd",
+            "/world",
+            max_envs=len(usd_list),
+            max_timecode=len(q_debug),
+            x_space=2.0,
+            y_space=2.0,
+            x_per_row=int(np.sqrt(len(usd_list))) + 1,
+            local_asset_path="",
+            dt=1,
+        )

examples/trajopt_example.py

@@ -0,0 +1,122 @@
+#
+# 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
+import time
+
+# Third Party
+import torch
+
+# CuRobo
+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.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.trajopt import TrajOptSolver, TrajOptSolverConfig
+
+
+def plot_js(trajectory: JointState):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(4, 1)
+    q = trajectory.position.cpu().numpy()
+    qd = trajectory.velocity.cpu().numpy()
+    qdd = trajectory.acceleration.cpu().numpy()
+    qddd = None
+    if trajectory.jerk is not None:
+        qddd = trajectory.jerk.cpu().numpy()
+
+    for i in range(q.shape[-1]):
+        axs[0].plot(q[:, i], label=str(i))
+        axs[1].plot(qd[:, i], label=str(i))
+        axs[2].plot(qdd[:, i], label=str(i))
+        if qddd is not None:
+            axs[3].plot(qddd[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def plot_traj(trajectory):
+    # Third Party
+    import matplotlib.pyplot as plt
+
+    _, axs = plt.subplots(1, 1)
+    q = trajectory
+
+    for i in range(q.shape[-1]):
+        axs.plot(q[:, i], label=str(i))
+    plt.legend()
+    plt.show()
+
+
+def demo_trajopt_collision_free():
+    PLOT = True
+    tensor_args = TensorDeviceType()
+    world_file = "collision_table.yml"
+
+    robot_file = "franka.yml"
+    robot_cfg = RobotConfig.from_dict(
+        load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    )
+    world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
+
+    trajopt_config = TrajOptSolverConfig.load_from_robot_config(
+        robot_cfg,
+        world_cfg,
+        tensor_args,
+        use_cuda_graph=False,
+    )
+    trajopt_solver = TrajOptSolver(trajopt_config)
+    q_start = trajopt_solver.retract_config
+
+    q_goal = q_start.clone() + 0.1
+    # q_goal[...,-1] -=0.2
+    kin_state = trajopt_solver.fk(q_goal)
+    goal_pose = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+    goal_state = JointState.from_position(q_goal)
+    current_state = JointState.from_position(q_start)
+    # do single planning:
+    # print("Running Goal State trajopt")
+    # js_goal = Goal(goal_state=goal_state, current_state=current_state)
+    # result = trajopt_solver.solve_single(js_goal)
+    # traj = result.solution.position
+    # print(result.success, result.cspace_error)
+    # print(goal_state.position)
+    # print(traj[...,-1,:])
+    # print(torch.linalg.norm((goal_state.position - traj[...,-1,:])).item())
+
+    # exit()
+    # if PLOT:
+    #    #plot_traj(traj)
+    #    plot_js(result.solution)
+    # exit()
+    print("Running Goal Pose trajopt")
+    js_goal = Goal(goal_pose=goal_pose, current_state=current_state)
+    result = trajopt_solver.solve_single(js_goal)
+    print(result.success)
+    if PLOT:
+        plot_js(result.solution)
+
+    # run goalset planning:
+    print("Running Goal Pose Set trajopt")
+    # g_set = Pose(kin_state.ee_position, kin_state.ee_quaternion.repeat(2,1).view())
+    # js_goal = Goal(goal_pose=goal_pose, current_state=current_state)
+    # result = trajopt_solver.solve_single(js_goal)
+
+
+if __name__ == "__main__":
+    # demo_basic_ik()
+    # demo_full_config_collision_free_ik()
+    # demo_full_config_batch_env_collision_free_ik()
+    demo_trajopt_collision_free()

examples/usd_example.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
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+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.util.usd_helper import UsdHelper
+from curobo.util_file import (
+    get_assets_path,
+    get_robot_configs_path,
+    get_world_configs_path,
+    join_path,
+    load_yaml,
+)
+from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+
+
+def save_curobo_world_to_usd():
+    world_file = "collision_table.yml"
+    world_cfg = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), world_file))
+    ).get_mesh_world(process=False)
+    usd_helper = UsdHelper()
+    usd_helper.create_stage()
+
+    usd_helper.add_obstacles_to_stage(world_cfg)
+
+    usd_helper.write_stage_to_file("test.usd")
+
+
+def get_trajectory(robot_file="franka.yml", dt=1.0 / 60.0):
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    motion_gen_config = MotionGenConfig.load_from_robot_config(
+        robot_file,
+        world_file,
+        tensor_args,
+        trajopt_tsteps=24,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        use_cuda_graph=True,
+        num_trajopt_seeds=2,
+        num_graph_seeds=2,
+        evaluate_interpolated_trajectory=True,
+        interpolation_dt=dt,
+        self_collision_check=True,
+    )
+    motion_gen = MotionGen(motion_gen_config)
+    motion_gen.warmup()
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    retract_cfg = motion_gen.get_retract_config()
+    state = motion_gen.rollout_fn.compute_kinematics(
+        JointState.from_position(retract_cfg.view(1, -1))
+    )
+
+    retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
+    start_state = JointState.from_position(retract_cfg.view(1, -1).clone() + 0.5)
+    # start_state.position[0,2] = 0.5
+    # start_state.position[0,1] = 0.5
+    # start_state.position[0,0] = 0.5
+    # print(start_state.position)
+    result = motion_gen.plan_single(start_state, retract_pose)
+    # print(result.plan_state.position)
+    print(result.success)
+    # print(result.position_error)
+    # result = motion_gen.plan(
+    #    start_state, retract_pose, enable_graph=False, enable_opt=True, max_attempts=10
+    # )
+    traj = result.get_interpolated_plan()  # optimized plan
+    return traj
+
+
+def save_curobo_robot_world_to_usd(robot_file="franka.yml"):
+    tensor_args = TensorDeviceType()
+    world_file = "collision_test.yml"
+    world_model = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), world_file))
+    ).get_obb_world()
+    dt = 1 / 60.0
+
+    q_traj = get_trajectory(robot_file, dt)
+    q_start = q_traj[0]
+
+    UsdHelper.write_trajectory_animation_with_robot_usd(
+        robot_file, world_model, q_start, q_traj, save_path="test.usd"
+    )
+
+
+def save_curobo_robot_to_usd(robot_file="franka.yml"):
+    # print(robot_file)
+    tensor_args = TensorDeviceType()
+    robot_cfg_y = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg_y["kinematics"]["use_usd_kinematics"] = True
+    print(
+        len(robot_cfg_y["kinematics"]["cspace"]["null_space_weight"]),
+        len(robot_cfg_y["kinematics"]["cspace"]["retract_config"]),
+        len(robot_cfg_y["kinematics"]["cspace"]["joint_names"]),
+    )
+    # print(robot_cfg_y)
+    robot_cfg = RobotConfig.from_dict(robot_cfg_y, tensor_args)
+    start = JointState.from_position(robot_cfg.cspace.retract_config)
+    retract_cfg = robot_cfg.cspace.retract_config.clone()
+    retract_cfg[0] = 0.5
+
+    # print(retract_cfg)
+    kin_model = CudaRobotModel(robot_cfg.kinematics)
+    position = retract_cfg
+    q_traj = JointState.from_position(position.unsqueeze(0))
+    q_traj.joint_names = kin_model.joint_names
+    # print(q_traj.joint_names)
+    usd_helper = UsdHelper()
+    # usd_helper.create_stage(
+    #    "test.usd", timesteps=q_traj.position.shape[0] + 1, dt=dt, interpolation_steps=10
+    # )
+    world_file = "collision_table.yml"
+    world_model = WorldConfig.from_dict(
+        load_yaml(join_path(get_world_configs_path(), world_file))
+    ).get_obb_world()
+
+    # print(q_traj.position.shape)
+    # usd_helper.load_robot_usd(robot_cfg.kinematics.usd_path, js)
+    usd_helper.write_trajectory_animation_with_robot_usd(
+        {"robot_cfg": robot_cfg_y},
+        world_model,
+        start,
+        q_traj,
+        save_path="test.usd",
+        # robot_asset_prim_path="/robot"
+    )
+
+    # usd_helper.save()
+    # usd_helper.write_stage_to_file("test.usda")
+
+
+def read_world_from_usd(file_path: str):
+    usd_helper = UsdHelper()
+    usd_helper.load_stage_from_file(file_path)
+    # world_model = usd_helper.get_obstacles_from_stage(reference_prim_path="/Root/world_obstacles")
+    world_model = usd_helper.get_obstacles_from_stage(
+        only_paths=["/world/obstacles"], reference_prim_path="/world"
+    )
+    # print(world_model)
+    for x in world_model.cuboid:
+        print(x.name + ":")
+        print("  pose: ", x.pose)
+        print("  dims: ", x.dims)
+
+
+def read_robot_from_usd(robot_file: str = "franka.yml"):
+    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
+    robot_cfg["kinematics"]["use_usd_kinematics"] = True
+    robot_cfg = RobotConfig.from_dict(robot_cfg, TensorDeviceType())
+
+
+if __name__ == "__main__":
+    save_curobo_world_to_usd()

examples/world_representation_example.py

@@ -0,0 +1,105 @@
+#
+# 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.
+#
+
+# CuRobo
+from curobo.geom.types import Capsule, Cuboid, Cylinder, Mesh, Sphere, WorldConfig
+from curobo.util_file import get_assets_path, join_path
+
+
+def approximate_geometry():
+    # CuRobo
+    from curobo.geom.sphere_fit import SphereFitType
+    from curobo.geom.types import Capsule, WorldConfig
+
+    obstacle_capsule = Capsule(
+        name="capsule",
+        radius=0.2,
+        base=[0, 0, 0],
+        tip=[0, 0, 0.5],
+        pose=[0.0, 5, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    sph = obstacle_capsule.get_bounding_spheres(
+        500, 0.005, SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE
+    )
+
+    WorldConfig(sphere=sph).save_world_as_mesh("bounding_spheres.obj")
+
+    WorldConfig(capsule=[obstacle_capsule]).save_world_as_mesh("capsule.obj")
+
+
+def doc_example():
+    # describe a cuboid obstacle
+
+    obstacle_1 = Cuboid(
+        name="cube_1",
+        pose=[0.0, 0.0, 0.0, 0.043, -0.471, 0.284, 0.834],
+        dims=[0.2, 1.0, 0.2],
+        color=[0.8, 0.0, 0.0, 1.0],
+    )
+
+    # describe a mesh obstacle
+    # import a mesh file:
+
+    mesh_file = join_path(get_assets_path(), "scene/nvblox/srl_ur10_bins.obj")
+
+    obstacle_2 = Mesh(
+        name="mesh_1",
+        pose=[0.0, 2, 0.5, 0.043, -0.471, 0.284, 0.834],
+        file_path=mesh_file,
+        scale=[0.5, 0.5, 0.5],
+    )
+
+    obstacle_3 = Capsule(
+        name="capsule",
+        radius=0.2,
+        base=[0, 0, 0],
+        tip=[0, 0, 0.5],
+        pose=[0.0, 5, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    obstacle_4 = Cylinder(
+        name="cylinder_1",
+        radius=0.2,
+        height=0.5,
+        pose=[0.0, 6, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    obstacle_5 = Sphere(
+        name="sphere_1",
+        radius=0.2,
+        pose=[0.0, 7, 0.0, 0.043, -0.471, 0.284, 0.834],
+        color=[0, 1.0, 0, 1.0],
+    )
+
+    world_model = WorldConfig(
+        mesh=[obstacle_2],
+        cuboid=[obstacle_1],
+        capsule=[obstacle_3],
+        cylinder=[obstacle_4],
+        sphere=[obstacle_5],
+    )
+    world_model.randomize_color(r=[0.2, 0.7], g=[0.4, 0.8], b=[0.0, 0.4])
+    file_path = "debug_mesh.obj"
+    world_model.save_world_as_mesh(file_path)
+
+    cuboid_world = WorldConfig.create_obb_world(world_model)
+    cuboid_world.save_world_as_mesh("debug_cuboid_mesh.obj")
+
+    collision_support_world = WorldConfig.create_collision_support_world(world_model)
+    collision_support_world.save_world_as_mesh("debug_collision_mesh.obj")
+
+
+if __name__ == "__main__":
+    approximate_geometry()