Add feature: spawn assembly of shapes

docs/source/tutorials/00_sim/spawn_prims.rst

@@ -155,6 +155,23 @@ reflected in the scene in a non-destructive manner. For example, we can change t
 actually modifying the underlying file for the table asset directly. Only the changes are stored in the USD stage.
 
 
+Spawning assembly of shapes
+---------------------------
+
+Sometimes we want to abstract a robot as a rigid body, manually set its mass, inertia, collision properties, and generate
+its appearance according to parameters instead of using fixed appearance loaded from files. So we provide the
+:mod:`sim.spawners.assemblies` sub-module that hosts configurations and spawner functions for such robots. It includes an
+example for spawning racing quadcopters.
+
+Simular to spawning the third cone ``ConeRigid``, the exemplar rigid racing quadcopter can be spawned using the following code.
+All customizable properties are defined in the :class:`~sim.spawners.assemblies.RaceQuadcopterCfg` class.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/00_sim/spawn_prims.py
+   :language: python
+   :start-at: # spawn an assembly of shapes with collision and inertia properties
+   :end-at: cfg_assembly.func("/World/Objects/Quad", cfg_assembly, translation=(-0.5, 0.0, 1.05))
+
+
 Executing the Script
 ~~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/__init__.py

@@ -54,6 +54,7 @@ class and the function call in a single line of code.
 
 """
 
+from .assemblies import *  # noqa: F401, F403
 from .from_files import *  # noqa: F401, F403
 from .lights import *  # noqa: F401, F403
 from .materials import *  # noqa: F401, F403

source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/__init__.py

@@ -0,0 +1,13 @@
+# Copyright (c) 2022-2024, The ORBIT Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Sub-module for spawning shape assemblies in the simulation.
+
+For spawning complex robots abstracted by assemblies of shapes and USD files.
+
+"""
+
+from .race_quadcopter import spawn_race_quadcopter
+from .race_quadcopter_cfg import RaceQuadcopterCfg

source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter.py

@@ -0,0 +1,222 @@
+# Copyright (c) 2022-2024, The ORBIT Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from __future__ import annotations
+
+import math
+from typing import TYPE_CHECKING
+
+import omni.isaac.core.utils.prims as prim_utils
+from pxr import Gf, Usd, UsdPhysics
+from warp.utils import quat_rpy
+
+from omni.isaac.orbit.sim import schemas
+from omni.isaac.orbit.sim.utils import clone, safe_set_attribute_on_usd_schema
+
+if TYPE_CHECKING:
+    from . import race_quadcopter_cfg
+
+
+@clone
+def spawn_race_quadcopter(
+    prim_path: str,
+    cfg: race_quadcopter_cfg.RaceQuadcopterCfg,
+    translation: tuple[float, float, float] | None = None,
+    orientation: tuple[float, float, float, float] | None = None,
+) -> Usd.Prim:
+    # body frame FLU-xyz
+    prim_utils.create_prim(
+        prim_path,
+        prim_type="Xform",
+        translation=translation,
+        orientation=orientation,
+    )
+
+    # visual shapes
+    visual_prim_path = prim_path + "/visual"
+    prim_utils.create_prim(visual_prim_path, prim_type="Xform")
+
+    # central body cube
+    prim_utils.create_prim(
+        visual_prim_path + "/central_body",
+        prim_type="Cube",
+        attributes={"size": 1.0},
+        scale=(
+            cfg.central_body_length_x,
+            cfg.central_body_length_y,
+            cfg.central_body_length_z,
+        ),
+        translation=(
+            cfg.central_body_center_x,
+            cfg.central_body_center_y,
+            cfg.central_body_center_z,
+        ),
+    )
+
+    # arms
+    arm_move_length = (cfg.arm_length_front + cfg.arm_length_rear) / 2 - cfg.arm_length_rear
+
+    # arms 1, 4
+    qx, qy, qz, qw = quat_rpy(0.0, 0.0, cfg.arm_front_angle / 2)
+    prim_utils.create_prim(
+        visual_prim_path + "/arm_1_4",
+        prim_type="Cube",
+        attributes={"size": 1.0},
+        scale=(
+            cfg.arm_length_front + cfg.arm_length_rear,
+            cfg.motor_diameter,
+            cfg.arm_thickness,
+        ),
+        orientation=(qw, qx, qy, qz),
+        translation=(
+            math.cos(cfg.arm_front_angle / 2) * arm_move_length,
+            math.sin(cfg.arm_front_angle / 2) * arm_move_length,
+            -cfg.arm_thickness / 2,
+        ),
+    )
+
+    # arms 2, 3
+    qx, qy, qz, qw = quat_rpy(0.0, 0.0, -cfg.arm_front_angle / 2)
+    prim_utils.create_prim(
+        visual_prim_path + "/arm_2_3",
+        prim_type="Cube",
+        attributes={"size": 1.0},
+        scale=(
+            cfg.arm_length_front + cfg.arm_length_rear,
+            cfg.motor_diameter,
+            cfg.arm_thickness,
+        ),
+        orientation=(qw, qx, qy, qz),
+        translation=(
+            math.cos(cfg.arm_front_angle / 2) * arm_move_length,
+            -math.sin(cfg.arm_front_angle / 2) * arm_move_length,
+            -cfg.arm_thickness / 2,
+        ),
+    )
+
+    # rotors
+    rotor_angles = [
+        cfg.arm_front_angle / 2 + math.pi,
+        -cfg.arm_front_angle / 2,
+        -cfg.arm_front_angle / 2 + math.pi,
+        cfg.arm_front_angle / 2,
+    ]
+    for i in [1, 2, 3, 4]:
+        arm_length = None
+        if i == 1 or i == 3:
+            arm_length = cfg.arm_length_rear
+        else:
+            arm_length = cfg.arm_length_front
+        prim_utils.create_prim(
+            visual_prim_path + "/motor_" + str(i),
+            prim_type="Cylinder",
+            attributes={
+                "radius": cfg.motor_diameter / 2,
+                "height": cfg.motor_height + cfg.arm_thickness,
+            },
+            translation=(
+                math.cos(rotor_angles[i - 1]) * arm_length,
+                math.sin(rotor_angles[i - 1]) * arm_length,
+                (cfg.motor_height + cfg.arm_thickness) / 2 - cfg.arm_thickness,
+            ),
+        )
+    for i in [1, 2, 3, 4]:
+        arm_length = None
+        if i == 1 or i == 3:
+            arm_length = cfg.arm_length_rear
+        else:
+            arm_length = cfg.arm_length_front
+        prim_utils.create_prim(
+            visual_prim_path + "/propeller_" + str(i),
+            prim_type="Cylinder",
+            attributes={
+                "radius": cfg.propeller_diameter / 2,
+                "height": cfg.propeller_height,
+            },
+            translation=(
+                math.cos(rotor_angles[i - 1]) * arm_length,
+                math.sin(rotor_angles[i - 1]) * arm_length,
+                cfg.propeller_height / 2 + cfg.motor_height,
+            ),
+        )
+
+    # collision box
+    collision_prim_path = prim_path + "/collision"
+    prim_utils.create_prim(collision_prim_path, prim_type="Xform")
+
+    # collision box size
+    positive_x_extend = max(
+        cfg.central_body_center_x + cfg.central_body_length_x / 2,
+        cfg.arm_length_front * math.cos(cfg.arm_front_angle / 2) + cfg.propeller_diameter / 2,
+    )
+    negative_x_extend = -min(
+        cfg.central_body_center_x - cfg.central_body_length_x / 2,
+        cfg.arm_length_rear * math.cos(cfg.arm_front_angle / 2 + math.pi) - cfg.propeller_diameter / 2,
+    )
+    collision_bbox_length_x = positive_x_extend + negative_x_extend
+    collision_bbox_center_x = -negative_x_extend + collision_bbox_length_x / 2
+
+    positive_y_extend = max(
+        cfg.central_body_center_y + cfg.central_body_length_y / 2,
+        cfg.arm_length_front * math.sin(cfg.arm_front_angle / 2) + cfg.propeller_diameter / 2,
+        cfg.arm_length_rear * math.sin(math.pi - cfg.arm_front_angle / 2) + cfg.propeller_diameter / 2,
+    )
+    collision_bbox_length_y = 2 * positive_y_extend
+    collision_bbox_center_y = 0.0
+
+    positive_z_extend = max(
+        cfg.central_body_center_z + cfg.central_body_length_z / 2,
+        cfg.motor_height + cfg.propeller_height,
+    )
+    negative_z_extend = -min(cfg.central_body_center_z - cfg.central_body_length_z / 2, -cfg.arm_thickness)
+    collision_bbox_length_z = positive_z_extend + negative_z_extend
+    collision_bbox_center_z = -negative_z_extend + collision_bbox_length_z / 2
+
+    prim_utils.create_prim(
+        collision_prim_path + "/bbox",
+        prim_type="Cube",
+        attributes={"size": 1.0, "purpose": "guide"},
+        scale=(
+            collision_bbox_length_x,
+            collision_bbox_length_y,
+            collision_bbox_length_z,
+        ),
+        translation=(
+            collision_bbox_center_x,
+            collision_bbox_center_y,
+            collision_bbox_center_z,
+        ),
+    )
+    schemas.define_collision_properties(collision_prim_path + "/bbox", cfg.collision_props)
+
+    # other properties
+    if cfg.mass_props is not None:
+        schemas.define_mass_properties(prim_path, cfg.mass_props)
+    if cfg.rigid_props is not None:
+        schemas.define_rigid_body_properties(prim_path, cfg.rigid_props)
+
+    # additional mass properties
+    mass_api = UsdPhysics.MassAPI(prim_utils.get_prim_at_path(prim_path))
+    safe_set_attribute_on_usd_schema(mass_api, "center_of_mass", cfg.center_of_mass, True)
+    safe_set_attribute_on_usd_schema(mass_api, "diagonal_inertia", cfg.diagonal_inertia, True)
+    safe_set_attribute_on_usd_schema(
+        mass_api,
+        "principal_axes",
+        Gf.Quatf(
+            cfg.principal_axes_rotation[0],
+            cfg.principal_axes_rotation[1],
+            cfg.principal_axes_rotation[2],
+            cfg.principal_axes_rotation[3],
+        ),
+        True,
+    )
+
+    # instance-able
+    visual = prim_utils.get_prim_at_path(visual_prim_path)
+    collision = prim_utils.get_prim_at_path(collision_prim_path)
+    visual.SetInstanceable(True)
+    collision.SetInstanceable(True)
+
+    return prim_utils.get_prim_at_path(prim_path)

source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter_cfg.py

@@ -0,0 +1,86 @@
+# Copyright (c) 2022-2024, The ORBIT Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from __future__ import annotations
+
+import math
+from collections.abc import Callable
+
+from omni.isaac.orbit.sim.spawners.spawner_cfg import RigidObjectSpawnerCfg
+from omni.isaac.orbit.utils import configclass
+
+from . import race_quadcopter
+
+
+@configclass
+class RaceQuadcopterCfg(RigidObjectSpawnerCfg):
+    """Configuration for the quadcopter in FLU body frame convention.
+
+    The center of body frame is the crossing point of the arms,
+    on the upper surface of the arm rectangles.
+
+    Collision shape is the minimum bounding box of the quadcopter,
+    and is automatically computed.
+
+    Additional mass properties are defined here instead of in `MassPropertiesCfg`
+    to avoid breaking existing code and tests.
+    """
+
+    func: Callable = race_quadcopter.spawn_race_quadcopter
+
+    # visual
+
+    arm_length_rear: float = 0.14
+    """Length of the two rear arms [m]."""
+
+    arm_length_front: float = 0.14
+    """Length of the two front arms [m]."""
+
+    arm_thickness: float = 0.01
+    """Thickness of the arm plate [m]."""
+
+    arm_front_angle: float = 100.0 * math.pi / 180
+    """Separation angle between two front arms [rad]."""
+
+    motor_diameter: float = 0.023
+    """Diameter of the motor cylinder [m]."""
+
+    motor_height: float = 0.006
+    """Height of the motor cylinder [m]."""
+
+    central_body_length_x: float = 0.15
+    """X-dimension of the cnetral body cuboid [m]."""
+
+    central_body_length_y: float = 0.05
+    """Y-dimension of the cnetral body cuboid [m]."""
+
+    central_body_length_z: float = 0.05
+    """Z-dimension of the cnetral body cuboid [m]."""
+
+    central_body_center_x: float = 0.0
+    """X-position of the cnetral body cuboid [m]."""
+
+    central_body_center_y: float = 0.0
+    """Y-position of the cnetral body cuboid [m]."""
+
+    central_body_center_z: float = 0.015
+    """Y-position of the cnetral body cuboid [m]."""
+
+    propeller_diameter: float = 6 * 2.54 * 0.01
+    """Diameter of the propeller cylinder [m]."""
+
+    propeller_height: float = 0.01
+    """Height of the propeller cylinder [m]."""
+
+    # additional mass properties
+
+    center_of_mass: tuple[float, float, float] = (0.0, 0.0, 0.0)
+    """Center of mass position in body frame [m]."""
+
+    diagonal_inertia: tuple[float, float, float] = (0.0025, 0.0025, 0.0045)
+    """Diagonal inertia [kg m^2]."""
+
+    principal_axes_rotation: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)
+    """Quaternion representing the same rotation as the principal axes matrix."""

source/standalone/tutorials/00_sim/spawn_prims.py

@@ -78,6 +78,14 @@ def design_scene():
     cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd")
     cfg.func("/World/Objects/Table", cfg, translation=(0.0, 0.0, 1.05))
 
+    # spawn an assembly of shapes with collision and inertia properties
+    cfg_assembly = sim_utils.RaceQuadcopterCfg(
+        mass_props=sim_utils.MassPropertiesCfg(mass=0.752),
+        rigid_props=sim_utils.RigidBodyPropertiesCfg(),
+        collision_props=sim_utils.CollisionPropertiesCfg(),
+    )
+    cfg_assembly.func("/World/Objects/Quad", cfg_assembly, translation=(-0.5, 0.0, 1.05))
+
 
 def main():
     """Main function."""