From d997ba36c95f208a5afe6bbbb01872213755e857 Mon Sep 17 00:00:00 2001 From: "Ryan Y. Liu" Date: Wed, 10 Apr 2024 15:18:06 +0200 Subject: [PATCH] Add feature: spawn assembly of shapes --- docs/source/tutorials/00_sim/spawn_prims.rst | 17 ++ .../omni/isaac/orbit/sim/spawners/__init__.py | 1 + .../orbit/sim/spawners/assemblies/__init__.py | 13 + .../spawners/assemblies/race_quadcopter.py | 222 ++++++++++++++++++ .../assemblies/race_quadcopter_cfg.py | 86 +++++++ .../tutorials/00_sim/spawn_prims.py | 8 + 6 files changed, 347 insertions(+) create mode 100644 source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/__init__.py create mode 100644 source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter.py create mode 100644 source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter_cfg.py diff --git a/docs/source/tutorials/00_sim/spawn_prims.rst b/docs/source/tutorials/00_sim/spawn_prims.rst index 444ca93529..8269bc154f 100644 --- a/docs/source/tutorials/00_sim/spawn_prims.rst +++ b/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 ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/__init__.py index 432ee6d5ed..ee10da52c5 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/__init__.py +++ b/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 diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/__init__.py new file mode 100644 index 0000000000..cedaaa54ee --- /dev/null +++ b/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 diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter.py new file mode 100644 index 0000000000..5ef843f039 --- /dev/null +++ b/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) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/assemblies/race_quadcopter_cfg.py new file mode 100644 index 0000000000..993e4bc4d3 --- /dev/null +++ b/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.""" diff --git a/source/standalone/tutorials/00_sim/spawn_prims.py b/source/standalone/tutorials/00_sim/spawn_prims.py index f9a4e65f07..746deca4c0 100644 --- a/source/standalone/tutorials/00_sim/spawn_prims.py +++ b/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."""