diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py index 8866fea0..58df10a5 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 (URDF behavior). + Only effective for USD files, ignored for URDF 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 4251f05d..8099d6e4 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -582,41 +582,45 @@ 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() + if not cfg.use_usd_properties: + # 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() 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 37b53197..01bb222d 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -211,9 +211,10 @@ 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()) + if not cfg.use_usd_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) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 4a28ccc2..d4669a58 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -1030,9 +1030,31 @@ 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 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}.") + return None + 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) @@ -1092,9 +1114,31 @@ 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 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}.") + return None + 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 8086689c..f08c7433 100644 --- a/embodichain/lab/sim/utility/sim_utils.py +++ b/embodichain/lab/sim/utility/sim_utils.py @@ -219,6 +219,28 @@ def load_mesh_objects_from_cfg( compute_uv = cfg.shape.compute_uv + is_usd = fpath.endswith((".usd", ".usda", ".usdc")) + if is_usd: + # TODO: 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, dexsim.cuda.pybind.models.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..49e59833 --- /dev/null +++ b/scripts/tutorials/sim/import_usd.py @@ -0,0 +1,168 @@ +# ---------------------------------------------------------------------------- +# 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 create a simulation scene using SimulationManager. +It shows the basic setup of simulation context, adding objects, and sensors. +""" + +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 dexsim.utility.path import get_resources_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)" + ) + parser.add_argument( + "--enable_rt", + action="store_true", + default=True, + help="Enable ray tracing for better visuals", + ) + 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=args.enable_rt, # Enable ray tracing for better visuals + num_envs=1, + arena_space=3.0, + ) + + # Create the simulation instance + sim = SimulationManager(sim_cfg) + # Open window when the scene has been set up + if not args.headless: + sim.open_window() + + 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], + ) + ) + + usdpath = "/home/xiemh/model/004_sugar_box/004_sugar_box_xmh.usda" + sugar_box: RigidObject = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="sugar_box", + shape=MeshCfg(fpath=usdpath), + body_type="dynamic", + init_pos=[0.2, 0.2, 1.0], + use_usd_properties=True, + ) + ) + + # Add objects to the scene + h1: Articulation = sim.add_articulation( + cfg=ArticulationCfg( + uid="h1", + # fpath="/home/xiemh/model/Collected_ur10/ur10.usd", + fpath="/home/xiemh/model/Collected_h1/h1.usda", + build_pk_chain=False, + init_pos=[-0.2, -0.2, 1.0], + use_usd_properties=False, + ) + ) + + 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()