diff --git a/docs/source/overview/sim/sim_articulation.md b/docs/source/overview/sim/sim_articulation.md index 5e4c0669..f35bb64b 100644 --- a/docs/source/overview/sim/sim_articulation.md +++ b/docs/source/overview/sim/sim_articulation.md @@ -10,10 +10,11 @@ The {class}`~objects.Articulation` class represents the fundamental physics enti Articulations are configured using the {class}`~cfg.ArticulationCfg` dataclass. | Parameter | Type | Default | Description | | :--- | :--- | :--- | :--- | -| `fpath` | `str` | `None` | Path to the asset file (URDF/MJCF). | +| `fpath` | `str` | `None` | Path to the asset file (URDF/USD). | | `init_pos` | `tuple` | `(0,0,0)` | Initial root position `(x, y, z)`. | | `init_rot` | `tuple` | `(0,0,0)` | Initial root rotation `(r, p, y)` in degrees. | | `fix_base` | `bool` | `True` | Whether to fix the base of the articulation. | +| `use_usd_properties` | `bool` | `False` | If True, use physical properties from USD file; if False, override with config values. Only effective for usd files. | | `init_qpos` | `List[float]` | `None` | Initial joint positions. | | `body_scale` | `List[float]` | `[1.0, 1.0, 1.0]` | Scaling factors for the articulation links. | | `disable_self_collisions` | `bool` | `True` | Whether to disable self-collisions. | @@ -61,6 +62,32 @@ articulation: Articulation = sim.add_articulation(cfg=art_cfg) # This performs a global reset of the simulation state sim.reset_objects_state() ``` + +### USD Import + +You can import USD files (`.usd`, `.usda`, `.usdc`) as articulations: + +```python +from embodichain.data import get_data_path + +# Import USD with properties from file +usd_art_cfg = ArticulationCfg( + fpath=get_data_path("path/to/robot.usd"), + init_pos=(0, 0, 0.5), + use_usd_properties=True # Keep USD drive/physics properties +) +usd_robot = sim.add_articulation(cfg=usd_art_cfg) + +# Or override USD properties with config (URDF behavior) +usd_art_cfg_override = ArticulationCfg( + fpath=get_data_path("path/to/robot.usd"), + init_pos=(0, 0, 0.5), + use_usd_properties=False, # Use config instead + drive_props=JointDrivePropertiesCfg(stiffness=5000, damping=500) +) +robot = sim.add_articulation(cfg=usd_art_cfg_override) +``` + ## Articulation Class State data is accessed via getter methods that return batched tensors. diff --git a/docs/source/overview/sim/sim_rigid_object.md b/docs/source/overview/sim/sim_rigid_object.md index 6d71465d..5bd4b417 100644 --- a/docs/source/overview/sim/sim_rigid_object.md +++ b/docs/source/overview/sim/sim_rigid_object.md @@ -16,6 +16,7 @@ Configured via the {class}`~cfg.RigidObjectCfg` class. | `attrs` | {class}`~cfg.RigidBodyAttributesCfg` | defaults in code | Physical attributes (mass, damping, friction, restitution, collision offsets, CCD, etc.). | | `init_pos` | `Sequence[float]` | `(0,0,0)` | Initial root position (x, y, z). | | `init_rot` | `Sequence[float]` | `(0,0,0)` (Euler degrees) | Initial root orientation (Euler angles in degrees) or provide `init_local_pose`. | +| `use_usd_properties` | `bool` | `False` | If True, use physical properties from USD file; if False, override with config values. Only effective for usd files. | | `uid` | `str` | `None` | Optional unique identifier for the object; manager will assign one if omitted. | ### Rigid Body Attributes ({class}`~cfg.RigidBodyAttributesCfg`) @@ -73,6 +74,31 @@ sim.update() > Note: `scripts/tutorials/sim/create_scene.py` provides a minimal working example of adding a rigid cube and running the simulation loop. +### USD Import + +You can import USD files (`.usd`, `.usda`, `.usdc`) as rigid objects: + +```python +from embodichain.data import get_data_path + +# Import USD with properties from file +usd_cfg = RigidObjectCfg( + shape=MeshCfg(fpath=get_data_path("path/to/object.usd")), + body_type="dynamic", + use_usd_properties=True # Keep USD properties +) +obj = sim.add_rigid_object(cfg=usd_cfg) + +# Or override USD properties with config +usd_cfg_override = RigidObjectCfg( + shape=MeshCfg(fpath=get_data_path("path/to/object.usd")), + body_type="dynamic", + use_usd_properties=False, # Use config instead + attrs=RigidBodyAttributesCfg(mass=2.0) +) +obj2 = sim.add_rigid_object(cfg=usd_cfg_override) +``` + ## Rigid Object Class — Common Methods & Attributes Rigid objects are observed and controlled via single poses and linear/angular velocities. Key APIs include: diff --git a/embodichain/data/assets/obj_assets.py b/embodichain/data/assets/obj_assets.py index 75ccf3d9..4790a26b 100644 --- a/embodichain/data/assets/obj_assets.py +++ b/embodichain/data/assets/obj_assets.py @@ -215,3 +215,15 @@ def __init__(self, data_root: str = None): path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root super().__init__(prefix, data_descriptor, path) + + +class SugarBox(EmbodiChainDataset): + def __init__(self, data_root: str = None): + data_descriptor = o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, obj_assets, "sugar_box_usd.zip"), + "a1bc5075512cedecd08af4f9c3e8f636", + ) + prefix = "SugarBox" + path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + + super().__init__(prefix, data_descriptor, path) diff --git a/embodichain/data/assets/robot_assets.py b/embodichain/data/assets/robot_assets.py index c979afbc..a817ffa5 100644 --- a/embodichain/data/assets/robot_assets.py +++ b/embodichain/data/assets/robot_assets.py @@ -124,6 +124,34 @@ def __init__(self, data_root: str = None): super().__init__(prefix, data_descriptor, path) +class UnitreeH1Usd(EmbodiChainDataset): + """Dataset class for the Unitree H1 robot USD version. + + Directory structure: + UnitreeH1Usd/H1_usd + h1.usd + h1.usda + + Example usage: + >>> from embodichain.data.robot_dataset import UnitreeH1Usd + >>> dataset = UnitreeH1Usd() + or + >>> from embodichain.data import get_data_path + >>> print(get_data_path("UnitreeH1Usd/H1_usd/h1.usd")) + >>> print(get_data_path("UnitreeH1Usd/H1_usd/h1.usda")) + """ + + def __init__(self, data_root: str = None): + data_descriptor = o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, robot_assets, "H1_usd.zip"), + "9fc19f8c8b4a49398ec661e6ea9877ee", + ) + prefix = "UnitreeH1Usd" + path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + + super().__init__(prefix, data_descriptor, path) + + class ABB(EmbodiChainDataset): """Dataset class for the ABB robot. diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py index 440e525c..4ed0e4f9 100644 --- a/embodichain/lab/sim/cfg.py +++ b/embodichain/lab/sim/cfg.py @@ -546,6 +546,14 @@ class RigidObjectCfg(ObjectBaseCfg): body_scale: Union[tuple, list] = (1.0, 1.0, 1.0) """Scale of the rigid body in the simulation world frame.""" + use_usd_properties: bool = False + """Whether to use physical properties from USD file instead of config. + + When True: Keep all physical properties (drive, physics attrs, etc.) from USD file. + When False (default): Override USD properties with config values. + Only effective for USD files. + """ + def to_dexsim_body_type(self) -> ActorType: """Convert the body type to dexsim ActorType.""" if self.body_type == "dynamic": @@ -1018,6 +1026,14 @@ class ArticulationCfg(ObjectBaseCfg): Currently, the uv mapping is computed for each link with projection uv mapping method. """ + use_usd_properties: bool = False + """Whether to use physical properties from USD file instead of config. + + When True: Keep all physical properties (drive, physics attrs, etc.) from USD file. + When False (default): Override USD properties with config values (URDF behavior). + Only effective for USD files, ignored for URDF files. + """ + @configclass class RobotCfg(ArticulationCfg): diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index d6ebe2aa..ac75d34e 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -582,41 +582,55 @@ def __init__( if self.cfg.init_qpos is None: self.cfg.init_qpos = torch.zeros(self.dof, dtype=torch.float32) - # Set articulation configuration in DexSim - set_dexsim_articulation_cfg(entities, self.cfg) - - # Init joint drive parameters. - num_entities = len(entities) - dof = self._data.dof - default_cfg = JointDrivePropertiesCfg() - self.default_joint_damping = torch.full( - (num_entities, dof), default_cfg.damping, dtype=torch.float32, device=device - ) - self.default_joint_stiffness = torch.full( - (num_entities, dof), - default_cfg.stiffness, - dtype=torch.float32, - device=device, - ) - self.default_joint_max_effort = torch.full( - (num_entities, dof), - default_cfg.max_effort, - dtype=torch.float32, - device=device, - ) - self.default_joint_max_velocity = torch.full( - (num_entities, dof), - default_cfg.max_velocity, - dtype=torch.float32, - device=device, - ) - self.default_joint_friction = torch.full( - (num_entities, dof), - default_cfg.friction, - dtype=torch.float32, - device=device, - ) - self._set_default_joint_drive() + # Determine if we should use USD properties or cfg properties. + is_usd_file = cfg.fpath.endswith((".usd", ".usda", ".usdc")) + use_cfg_properties = not (cfg.use_usd_properties and is_usd_file) + + if use_cfg_properties: + # Set articulation configuration in DexSim + set_dexsim_articulation_cfg(entities, self.cfg) + + num_entities = len(entities) + dof = self._data.dof + default_cfg = JointDrivePropertiesCfg() + self.default_joint_damping = torch.full( + (num_entities, dof), + default_cfg.damping, + dtype=torch.float32, + device=device, + ) + self.default_joint_stiffness = torch.full( + (num_entities, dof), + default_cfg.stiffness, + dtype=torch.float32, + device=device, + ) + self.default_joint_max_effort = torch.full( + (num_entities, dof), + default_cfg.max_effort, + dtype=torch.float32, + device=device, + ) + self.default_joint_max_velocity = torch.full( + (num_entities, dof), + default_cfg.max_velocity, + dtype=torch.float32, + device=device, + ) + self.default_joint_friction = torch.full( + (num_entities, dof), + default_cfg.friction, + dtype=torch.float32, + device=device, + ) + self._set_default_joint_drive() + else: + # Read current properties from USD-loaded entities + self.default_joint_stiffness = self._data.joint_stiffness.clone() + self.default_joint_damping = self._data.joint_damping.clone() + self.default_joint_friction = self._data.joint_friction.clone() + self.default_joint_max_effort = self._data.qf_limits.clone() + self.default_joint_max_velocity = self._data.qvel_limits.clone() self.pk_chain = None if self.cfg.build_pk_chain: diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index ac7723f5..4fec12b7 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -211,9 +211,18 @@ def __init__( self._visual_material: List[VisualMaterialInst] = [None] * len(entities) self.is_shared_visual_material = False - for entity in entities: - entity.set_body_scale(*cfg.body_scale) - entity.set_physical_attr(cfg.attrs.attr()) + # Determine if we should use USD properties or cfg properties. + from embodichain.lab.sim.shapes import MeshCfg + + is_usd_file = isinstance(cfg.shape, MeshCfg) and cfg.shape.fpath.endswith( + (".usd", ".usda", ".usdc") + ) + use_cfg_properties = not (cfg.use_usd_properties and is_usd_file) + + if use_cfg_properties: + for entity in entities: + entity.set_body_scale(*cfg.body_scale) + entity.set_physical_attr(cfg.attrs.attr()) if device.type == "cuda": self._world.update(0.001) @@ -869,7 +878,7 @@ def set_visible(self, visible: bool = True) -> None: def reset(self, env_ids: Sequence[int] | None = None) -> None: local_env_ids = self._all_indices if env_ids is None else env_ids num_instances = len(local_env_ids) - self.set_attrs(self.cfg.attrs, env_ids=local_env_ids) + # self.set_attrs(self.cfg.attrs, env_ids=local_env_ids) pos = torch.as_tensor( self.cfg.init_pos, dtype=torch.float32, device=self.device diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index eb29fea6..6ccd13cf 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -1041,9 +1041,32 @@ def add_articulation( env_list = [self._env] if len(self._arenas) == 0 else self._arenas obj_list = [] - for env in env_list: - art = env.load_urdf(cfg.fpath) - obj_list.append(art) + is_usd = cfg.fpath.endswith((".usd", ".usda", ".usdc")) + if is_usd: + # TODO: Currently add checking for num_envs when file is USD. After we support spawn via cloning, we can remove this. + if len(env_list) > 1: + logger.log_error(f"Currently not supporting multiple arenas for USD.") + env = self._env + results = env.import_from_usd_file(cfg.fpath, return_object=True) + # print("USD import results:", results) + + articulations_found = [] + for key, value in results.items(): + if isinstance(value, dexsim.engine.Articulation): + articulations_found.append(value) + + if len(articulations_found) == 0: + logger.log_error(f"No articulation found in USD file {cfg.fpath}.") + elif len(articulations_found) > 1: + logger.log_error( + f"Multiple articulations found in USD file {cfg.fpath}. " + ) + elif len(articulations_found) == 1: + obj_list.append(articulations_found[0]) + else: + for env in env_list: + art = env.load_urdf(cfg.fpath) + obj_list.append(art) articulation = Articulation(cfg=cfg, entities=obj_list, device=self.device) @@ -1109,9 +1132,32 @@ def add_robot(self, cfg: RobotCfg) -> Robot | None: env_list = [self._env] if len(self._arenas) == 0 else self._arenas obj_list = [] - for env in env_list: - art = env.load_urdf(cfg.fpath) - obj_list.append(art) + is_usd = cfg.fpath.endswith((".usd", ".usda", ".usdc")) + if is_usd: + # TODO: Currently add checking for num_envs when file is USD. After we support spawn via cloning, we can remove this. + if len(env_list) > 1: + logger.log_error(f"Currently not supporting multiple arenas for USD.") + env = self._env + results = env.import_from_usd_file(cfg.fpath, return_object=True) + # print("USD import results:", results) + + articulations_found = [] + for key, value in results.items(): + if isinstance(value, dexsim.engine.Articulation): + articulations_found.append(value) + + if len(articulations_found) == 0: + logger.log_error(f"No articulation found in USD file {cfg.fpath}.") + elif len(articulations_found) > 1: + logger.log_error( + f"Multiple articulations found in USD file {cfg.fpath}. " + ) + elif len(articulations_found) == 1: + obj_list.append(articulations_found[0]) + else: + for env in env_list: + art = env.load_urdf(cfg.fpath) + obj_list.append(art) robot = Robot(cfg=cfg, entities=obj_list, device=self.device) diff --git a/embodichain/lab/sim/utility/sim_utils.py b/embodichain/lab/sim/utility/sim_utils.py index 523c7fef..15e16edf 100644 --- a/embodichain/lab/sim/utility/sim_utils.py +++ b/embodichain/lab/sim/utility/sim_utils.py @@ -219,6 +219,30 @@ def load_mesh_objects_from_cfg( compute_uv = cfg.shape.compute_uv + is_usd = fpath.endswith((".usd", ".usda", ".usdc")) + if is_usd: + # TODO: Currently add checking for num_envs when file is USD. After we support spawn via cloning, we can remove this. + if len(env_list) > 1: + logger.log_error(f"Currently not supporting multiple arenas for USD.") + _env: dexsim.environment.Env = dexsim.default_world().get_env() + results = _env.import_from_usd_file(fpath, return_object=True) + # print(f"import usd result: {results}") + + rigidbodys_found = [] + for key, value in results.items(): + if isinstance(value, MeshObject): + rigidbodys_found.append(value) + if len(rigidbodys_found) == 0: + logger.log_error(f"No rigid body found in USD file: {fpath}") + elif len(rigidbodys_found) > 1: + logger.log_error(f"Multiple rigid bodies found in USD file: {fpath}.") + elif len(rigidbodys_found) == 1: + obj_list.append(rigidbodys_found[0]) + return obj_list + else: + # non-usd file does not support this option, will be ignored if set. + cfg.use_usd_properties = False + for i, env in enumerate(env_list): if max_convex_hull_num > 1: obj = env.load_actor_with_coacd( diff --git a/scripts/tutorials/sim/import_usd.py b/scripts/tutorials/sim/import_usd.py new file mode 100644 index 00000000..59dfac62 --- /dev/null +++ b/scripts/tutorials/sim/import_usd.py @@ -0,0 +1,166 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2025 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +""" +This script demonstrates how to import USD files into the scene. +Currently, it supports importing USD files as rigid objects or articulations. +Multiple arenas are not supported when importing USD files. +""" + +import argparse +import time + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import RigidBodyAttributesCfg +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg +from embodichain.lab.sim.objects import ( + RigidObject, + RigidObjectCfg, + ArticulationCfg, + Articulation, +) +from embodichain.data import get_data_path + + +def main(): + """Main function to create and run the simulation scene.""" + + # Parse command line arguments + parser = argparse.ArgumentParser( + description="Create a simulation scene with SimulationManager" + ) + parser.add_argument( + "--headless", + action="store_true", + default=False, + help="Run simulation in headless mode", + ) + parser.add_argument( + "--device", type=str, default="cpu", help="Simulation device (cuda or cpu)" + ) + args = parser.parse_args() + + # Configure the simulation + sim_cfg = SimulationManagerCfg( + width=1920, + height=1080, + headless=True, + physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) + sim_device=args.device, + enable_rt=True, # Enable ray tracing for better visuals + num_envs=1, + arena_space=3.0, + ) + + # Create the simulation instance + sim = SimulationManager(sim_cfg) + + cube: RigidObject = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=[0.1, 0.1, 0.1]), + body_type="dynamic", + attrs=RigidBodyAttributesCfg( + mass=1.0, + dynamic_friction=0.5, + static_friction=0.5, + restitution=0.1, + ), + init_pos=[0.0, 0.0, 1.0], + ) + ) + + sugar_box_path = get_data_path("SugarBox/sugar_box_usd/sugar_box.usda") + print(f"Loading USD file from: {sugar_box_path}") + sugar_box: RigidObject = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="sugar_box", + shape=MeshCfg(fpath=sugar_box_path), + body_type="dynamic", + init_pos=[0.2, 0.2, 1.0], + use_usd_properties=True, + ) + ) + + # Add objects to the scene + h1_path = get_data_path("UnitreeH1Usd/H1_usd/h1.usd") + print(f"Loading USD file from: {h1_path}") + h1: Articulation = sim.add_articulation( + cfg=ArticulationCfg( + uid="h1", + fpath=h1_path, + build_pk_chain=False, + init_pos=[-0.2, -0.2, 1.0], + use_usd_properties=False, + ) + ) + + # Open window when the scene has been set up + if not args.headless: + sim.open_window() + + print("[INFO]: Scene setup complete!") + print("[INFO]: Press Ctrl+C to stop the simulation") + + # Run the simulation + run_simulation(sim) + + +def run_simulation(sim: SimulationManager): + """Run the simulation loop. + + Args: + sim: The SimulationManager instance to run + """ + + # Initialize GPU physics if using CUDA + if sim.is_use_gpu_physics: + sim.init_gpu_physics() + + step_count = 0 + + try: + last_time = time.time() + last_step = 0 + while True: + # Update physics simulation + sim.update(step=1) + time.sleep(0.03) # Sleep to limit update rate (optional) + step_count += 1 + + # Print FPS every second + if step_count % 100 == 0: + current_time = time.time() + elapsed = current_time - last_time + fps = ( + sim.num_envs * (step_count - last_step) / elapsed + if elapsed > 0 + else 0 + ) + # print(f"[INFO]: Simulation step: {step_count}, FPS: {fps:.2f}") + last_time = current_time + last_step = step_count + + except KeyboardInterrupt: + print("\n[INFO]: Stopping simulation...") + finally: + # Clean up resources + sim.destroy() + print("[INFO]: Simulation terminated successfully") + + +if __name__ == "__main__": + main() diff --git a/tests/sim/objects/test_usd.py b/tests/sim/objects/test_usd.py new file mode 100644 index 00000000..06258c20 --- /dev/null +++ b/tests/sim/objects/test_usd.py @@ -0,0 +1,187 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +import pytest +import torch + +from embodichain.lab.sim import ( + SimulationManager, + SimulationManagerCfg, +) +from embodichain.lab.sim.objects import Articulation, RigidObject +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + RigidObjectCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, +) +from embodichain.lab.sim.shapes import MeshCfg +from embodichain.data import get_data_path + +NUM_ARENAS = 1 + + +class BaseUsdTest: + """Shared test logic for CPU and CUDA.""" + + def setup_simulation(self, sim_device): + config = SimulationManagerCfg( + headless=True, sim_device=sim_device, num_envs=NUM_ARENAS, enable_rt=False + ) + self.sim = SimulationManager(config) + + if sim_device == "cuda" and getattr(self.sim, "is_use_gpu_physics", False): + self.sim.init_gpu_physics() + + def test_import_rigid(self): + default_attr = RigidBodyAttributesCfg() + sugar_box_path = get_data_path("SugarBox/sugar_box_usd/sugar_box.usda") + sugar_box: RigidObject = self.sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="sugar_box", + shape=MeshCfg(fpath=sugar_box_path), + body_type="dynamic", + use_usd_properties=False, + init_pos=[0.0, 0.0, 0.1], + attrs=default_attr, + ) + ) + body0 = sugar_box._entities[0].get_physical_body() + print(sugar_box._entities[0].get_physical_attr()) + assert pytest.approx(body0.get_mass()) == default_attr.mass + assert pytest.approx(body0.get_linear_damping()) == default_attr.linear_damping + assert ( + pytest.approx(body0.get_angular_damping()) == default_attr.angular_damping + ) + assert body0.get_solver_iteration_counts() == ( + default_attr.min_position_iters, + default_attr.min_velocity_iters, + ) + + def test_import_articulation(self): + default_drive = JointDrivePropertiesCfg() + h1_path = get_data_path("UnitreeH1Usd/H1_usd/h1.usd") + h1: Articulation = self.sim.add_articulation( + cfg=ArticulationCfg( + uid="h1", + fpath=h1_path, + build_pk_chain=False, + use_usd_properties=False, + init_pos=[0.0, 0.0, 1.2], + drive_pros=default_drive, + ) + ) + + stiffness = h1.body_data.joint_stiffness + damping = h1.body_data.joint_damping + max_force = h1.body_data.qf_limits + print(f"All joint stiffness: {stiffness}") + print(f"All joint damping: {damping}") + print(f"All joint max force: {max_force}") + expected_stiffness = default_drive.stiffness + assert torch.allclose( + stiffness, torch.tensor(expected_stiffness, dtype=torch.float32) + ) + expectied_damping = default_drive.damping + assert torch.allclose( + damping, torch.tensor(expectied_damping, dtype=torch.float32) + ) + + def test_usd_properties(self): + """In this test, we set use_usd_properties=True to verify that the USD properties are correctly applied.""" + h1_path = get_data_path("UnitreeH1Usd/H1_usd/h1.usd") + h1: Articulation = self.sim.add_articulation( + cfg=ArticulationCfg( + uid="h1_beta", + fpath=h1_path, + build_pk_chain=False, + use_usd_properties=True, + init_pos=[0.0, 0.0, 1.2], + ) + ) + + stiffness = h1.body_data.joint_stiffness + damping = h1.body_data.joint_damping + max_force = h1.body_data.qf_limits + print(f"All joint stiffness: {stiffness}") + print(f"All joint damping: {damping}") + print(f"All joint max force: {max_force}") + expected_stiffness = 10000000.0 + assert torch.allclose( + stiffness, torch.tensor(expected_stiffness, dtype=torch.float32) + ) + + joint_names = h1.joint_names + print(f"Joint names: {joint_names}") + + target_joint_name = "left_hip_yaw_joint" + if target_joint_name in joint_names: + joint_idx = joint_names.index(target_joint_name) + # check for the first instance + assert torch.isclose( + stiffness[0, joint_idx], torch.tensor(10000000.0, dtype=torch.float32) + ) + assert torch.isclose( + damping[0, joint_idx], torch.tensor(0.0, dtype=torch.float32) + ) + assert torch.isclose( + max_force[0, joint_idx], torch.tensor(200.0, dtype=torch.float32) + ) + + sugar_box_path = get_data_path("SugarBox/sugar_box_usd/sugar_box.usda") + sugar_box: RigidObject = self.sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="sugar_box_beta", + shape=MeshCfg(fpath=sugar_box_path), + body_type="dynamic", + use_usd_properties=True, + init_pos=[0.0, 0.0, 0.1], + ) + ) + body0 = sugar_box._entities[0].get_physical_body() + print(sugar_box._entities[0].get_physical_attr()) + assert pytest.approx(body0.get_mass(), 0.001) == 0.514 + # TODO: nvidia physx attrs in usd currently are not fully suported + # assert(body0.get_linear_damping()==0) + # assert(body0.get_angular_damping()==0.05) + # assert(body0.get_solver_iteration_counts()==(4, 1)) + # assert(body0.get_max_angular_velocity()==100) + + def teardown_method(self): + """Clean up resources after each test method.""" + self.sim.destroy() + + +class TestUsdCPU(BaseUsdTest): + def setup_method(self): + self.setup_simulation("cpu") + + +@pytest.mark.skip(reason="Skipping CUDA tests temporarily") +class TestUsdCUDA(BaseUsdTest): + def setup_method(self): + self.setup_simulation("cuda") + + +if __name__ == "__main__": + test = TestUsdCPU() + test.setup_method() + test.test_import_rigid() + test.test_import_articulation() + test.test_usd_properties() + + # from IPython import embed + # embed()