Skip to content

Commit

Permalink
adds spawning of multi usd files
Browse files Browse the repository at this point in the history
  • Loading branch information
Mayankm96 committed Oct 5, 2024
1 parent ab9f892 commit d919efe
Show file tree
Hide file tree
Showing 5 changed files with 255 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,5 @@
different configurations.
"""

from .wrappers import spawn_multi_asset
from .wrappers_cfg import MultiAssetSpawnerCfg
from .wrappers import spawn_multi_asset, spawn_multi_usd_file
from .wrappers_cfg import MultiAssetSpawnerCfg, MultiUsdFileCfg
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
from pxr import Sdf, Usd

import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.sim.spawners.from_files import UsdFileCfg

if TYPE_CHECKING:
from . import wrappers_cfg
Expand Down Expand Up @@ -69,10 +70,11 @@ def spawn_multi_asset(
else:
asset_cfg.semantic_tags += cfg.semantic_tags
# override settings for properties
for name in ["mass_props", "rigid_props", "collision_props", "activate_contact_sensors", "deformable_props"]:
value = getattr(cfg, name)
if hasattr(asset_cfg, name) and value is not None:
setattr(asset_cfg, name, value)
attr_names = ["mass_props", "rigid_props", "collision_props", "activate_contact_sensors", "deformable_props"]
for attr_name in attr_names:
attr_value = getattr(cfg, attr_name)
if hasattr(asset_cfg, attr_name) and attr_value is not None:
setattr(asset_cfg, attr_name, attr_value)
# spawn single instance
proto_prim_path = f"/World/Dataset/Asset_{index:04d}"
asset_cfg.func(proto_prim_path, asset_cfg, translation=translation, orientation=orientation)
Expand Down Expand Up @@ -102,3 +104,54 @@ def spawn_multi_asset(

# return the prim
return prim_utils.get_prim_at_path(prim_paths[0])


def spawn_multi_usd_file(
prim_path: str,
cfg: wrappers_cfg.MultiUsdFileCfg,
translation: tuple[float, float, float] | None = None,
orientation: tuple[float, float, float, float] | None = None,
) -> Usd.Prim:
"""Spawn multiple USD files based on the provided configurations.
This function spawns multiple assets based on the provided configurations. The assets are spawned
in the order they are provided in the list. If the `random_choice` parameter is set to True, a random
asset configuration is selected for each spawn.
Args:
prim_path: The prim path to spawn the assets.
cfg: The configuration for spawning the assets.
translation: The translation of the spawned assets. Default is None.
orientation: The orientation of the spawned assets. Default is None.
Returns:
The created prim at the first prim path.
"""
# needed here to avoid circular imports
from .wrappers_cfg import MultiAssetSpawnerCfg

# parse all the usd files
if isinstance(cfg.usd_path, str):
usd_paths = [cfg.usd_path]
else:
usd_paths = cfg.usd_path

# make a template usd config
usd_template_cfg = UsdFileCfg()
for attr_name, attr_value in cfg.__dict__.items():
# skip names we know are not present
if attr_name in ["func", "usd_path", "random_choice"]:
continue
# set the attribute into the template
setattr(usd_template_cfg, attr_name, attr_value)

# create multi asset configuration of USD files
multi_asset_cfg = MultiAssetSpawnerCfg(assets_cfg=[])
for usd_path in usd_paths:
usd_cfg = usd_template_cfg.replace(usd_path=usd_path)
multi_asset_cfg.assets_cfg.append(usd_cfg)
# set random choice
multi_asset_cfg.random_choice = cfg.random_choice

# call the original function
return spawn_multi_asset(prim_path, multi_asset_cfg, translation, orientation)
Original file line number Diff line number Diff line change
Expand Up @@ -5,23 +5,23 @@

from dataclasses import MISSING

from omni.isaac.lab.utils import configclass
from omni.isaac.lab.sim.spawners.spawner_cfg import DeformableObjectSpawnerCfg, RigidObjectSpawnerCfg, SpawnerCfg
from omni.isaac.lab.sim.spawners.from_files import UsdFileCfg
from omni.isaac.lab.sim.spawners.spawner_cfg import DeformableObjectSpawnerCfg, RigidObjectSpawnerCfg, SpawnerCfg
from omni.isaac.lab.utils import configclass

from . import wrappers


@configclass
class MultiAssetSpawnerCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg):
"""Configuration parameters for loading multiple assets from their individual configurations.
Specifying values for any properties at the configuration level will override the settings of
individual assets' configuration. For instance if the attribute
:attr:`MultiAssetSpawnerCfg.mass_props` is specified, its value will overwrite the values of the
mass properties in each configuration inside :attr:`assets_cfg` (wherever applicable).
This is done to simplify configuring similar properties globally. By default, all properties are set to None.
The following is an exception to the above:
* :attr:`visible`: This parameter is ignored. Its value for the individual assets is used.
Expand All @@ -40,3 +40,20 @@ class MultiAssetSpawnerCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg):
If False, the asset configurations are spawned in the order they are provided in the list.
If True, a random asset configuration is selected for each spawn.
"""


@configclass
class MultiUsdFileCfg(UsdFileCfg):
"""Configuration parameters for loading multiple USDs from their individual configurations."""

func = wrappers.spawn_multi_usd_file

usd_path: str | list[str] = MISSING
"""Path or a list of paths to the USD files to spawn asset from."""

random_choice: bool = True
"""Whether to randomly select an asset configuration. Default is True.
If False, the asset configurations are spawned in the order they are provided in the list.
If True, a random asset configuration is selected for each spawn.
"""
8 changes: 4 additions & 4 deletions source/standalone/demos/multi_asset.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
.. code-block:: bash
# Usage
./isaaclab.sh -p source/standalone/demos/multi_object.py --num_envs 2048
./isaaclab.sh -p source/standalone/demos/multi_asset.py --num_envs 2048
"""

Expand Down Expand Up @@ -38,7 +38,7 @@
import random

import omni.usd
from pxr import Sdf, Gf
from pxr import Gf, Sdf

import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.assets import AssetBaseCfg, RigidObjectCfg
Expand Down Expand Up @@ -87,9 +87,9 @@ class MultiObjectSceneCfg(InteractiveSceneCfg):
prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)

# articulation
# rigid object
object: RigidObjectCfg = RigidObjectCfg(
prim_path="/World/envs/env_.*/Objects",
prim_path="/World/envs/env_.*/Object",
spawn=sim_utils.MultiAssetSpawnerCfg(
assets_cfg=[
sim_utils.ConeCfg(
Expand Down
171 changes: 171 additions & 0 deletions source/standalone/demos/multi_usd_asset.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,171 @@
# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

"""This script demonstrates how to spawn multiple objects in multiple environments.
.. code-block:: bash
# Usage
./isaaclab.sh -p source/standalone/demos/multi_usd_asset.py --num_envs 2048
"""

from __future__ import annotations

"""Launch Isaac Sim Simulator first."""


import argparse

from omni.isaac.lab.app import AppLauncher

# add argparse arguments
parser = argparse.ArgumentParser(description="Demo on spawning different objects in multiple environments.")
parser.add_argument("--num_envs", type=int, default=1024, help="Number of environments to spawn.")
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()

# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

"""Rest everything follows."""

import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.assets import AssetBaseCfg, ArticulationCfg
from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg
from omni.isaac.lab.sim import SimulationContext
from omni.isaac.lab.utils import Timer, configclass
from omni.isaac.lab.utils.assets import ISAACLAB_NUCLEUS_DIR
from omni.isaac.lab.actuators import ActuatorNetLSTMCfg


##
# Scene Configuration
##


@configclass
class MultiObjectSceneCfg(InteractiveSceneCfg):
"""Configuration for a multi-object scene."""

# ground plane
ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg())

# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)

# articulation
robot: ArticulationCfg = ArticulationCfg(
prim_path="/World/envs/env_.*/Robot",
spawn=sim_utils.MultiUsdFileCfg(
usd_path=[
f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd",
f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd",
],
random_choice=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
retain_accelerations=False,
linear_damping=0.0,
angular_damping=0.0,
max_linear_velocity=1000.0,
max_angular_velocity=1000.0,
max_depenetration_velocity=1.0,
),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0
),
activate_contact_sensors=True,
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.6),
joint_pos={
".*HAA": 0.0, # all HAA
".*F_HFE": 0.4, # both front HFE
".*H_HFE": -0.4, # both hind HFE
".*F_KFE": -0.8, # both front KFE
".*H_KFE": 0.8, # both hind KFE
},
),
actuators={
"legs": ActuatorNetLSTMCfg(
joint_names_expr=[".*HAA", ".*HFE", ".*KFE"],
network_file=f"{ISAACLAB_NUCLEUS_DIR}/ActuatorNets/ANYbotics/anydrive_3_lstm_jit.pt",
saturation_effort=120.0,
effort_limit=80.0,
velocity_limit=7.5,
)
},
)


##
# Simulation Loop
##


def run_simulator(sim: SimulationContext, scene: InteractiveScene):
"""Runs the simulation loop."""
# Extract scene entities
# note: we only do this here for readability.
robot = scene["robot"]
# Define simulation stepping
sim_dt = sim.get_physics_dt()
count = 0
# Simulation loop
while simulation_app.is_running():
# Reset
if count % 500 == 0:
# reset counter
count = 0
# reset the scene entities
# root state
root_state = robot.data.default_root_state.clone()
root_state[:, :3] += scene.env_origins
robot.write_root_state_to_sim(root_state)
# clear internal buffers
scene.reset()
print("[INFO]: Resetting robot state...")
# Write data to sim
scene.write_data_to_sim()
# Perform step
sim.step()
# Increment counter
count += 1
# Update buffers
scene.update(sim_dt)


def main():
"""Main function."""
# Load kit helper
sim_cfg = sim_utils.SimulationCfg(device=args_cli.device)
sim = SimulationContext(sim_cfg)
# Set main camera
sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0])

# Design scene
scene_cfg = MultiObjectSceneCfg(num_envs=args_cli.num_envs, env_spacing=1.5, replicate_physics=False)
with Timer("[INFO] Time to create scene: "):
scene = InteractiveScene(scene_cfg)

# Play the simulator
sim.reset()
# Now we are ready!
print("[INFO]: Setup complete...")
# Run the simulator
run_simulator(sim, scene)


if __name__ == "__main__":
# run the main execution
main()
# close sim app
simulation_app.close()

0 comments on commit d919efe

Please sign in to comment.