Unverified Commit 19a316d3 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Simplifies setting of physics material on prims (#30)

* adds kit utility to apply physics material on prims in a nested manner
* makes physics material cfg consistent across the assets classes
* adds function to sample from cylinder in the math utilities
* adds demo script to spawn ycb objects
* updates changelog
parent e00ec819
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.2.3"
version = "0.2.4"
# Description
title = "ORBIT framework for Robot Learning"
......
Changelog
---------
0.2.4 (2023-03-04)
~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added the :meth:`apply_physics_material` and :meth:`apply_nested_physics_material` to the ``omni.isaac.orbit.core.utils.kit``.
* Added the :meth:`sample_cylinder` to sample points from a cylinder's surface.
* Added documentation about the issue in using instanceable asset as markers.
Fixed
^^^^^
* Simplified the physics material application in the rigid object and legged robot classes.
0.2.3 (2023-02-24)
~~~~~~~~~~~~~~~~~~
......
......@@ -9,7 +9,7 @@ from typing import Optional, Sequence
import carb
import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.core.materials import PhysicsMaterial
from omni.isaac.core.prims import GeometryPrim, RigidPrim, RigidPrimView
from omni.isaac.core.prims import RigidPrim, RigidPrimView
import omni.isaac.orbit.utils.kit as kit_utils
......@@ -107,8 +107,7 @@ class RigidObject:
else:
carb.log_warn(f"A prim already exists at prim path: '{prim_path}'. Skipping...")
# TODO: What if prim already exists in the stage and spawn isn't called?
# apply rigid body properties
# apply rigid body properties API
RigidPrim(prim_path=prim_path)
# -- set rigid body properties
kit_utils.set_nested_rigid_body_properties(
......@@ -119,19 +118,20 @@ class RigidObject:
disable_gravity=self.cfg.rigid_props.disable_gravity,
)
# create physics material
material = PhysicsMaterial(
prim_path + self.cfg.material_props.material_path,
static_friction=self.cfg.material_props.static_friction,
dynamic_friction=self.cfg.material_props.dynamic_friction,
restitution=self.cfg.material_props.restitution,
)
# TODO: Iterate over the rigid prim and set this material to all collision shapes.
if self.cfg.meta_info.geom_prim_rel_path is not None:
object_geom = GeometryPrim(prim_path=prim_path + self.cfg.meta_info.geom_prim_rel_path)
else:
object_geom = GeometryPrim(prim_path=prim_path)
# apply physics material on object
object_geom.apply_physics_material(material)
if self.cfg.physics_material is not None:
# -- resolve material path
material_path = self.cfg.physics_material.prim_path
if not material_path.startswith("/"):
material_path = prim_path + "/" + prim_path
# -- create physics material
material = PhysicsMaterial(
prim_path=material_path,
static_friction=self.cfg.physics_material.static_friction,
dynamic_friction=self.cfg.physics_material.dynamic_friction,
restitution=self.cfg.physics_material.restitution,
)
# -- apply physics material
kit_utils.apply_nested_physics_material(prim_path, material.prim_path)
def initialize(self, prim_paths_expr: Optional[str] = None):
"""Initializes the PhysX handles and internal buffers.
......
......@@ -19,11 +19,6 @@ class RigidObjectCfg:
usd_path: str = MISSING
"""USD file to spawn asset from."""
geom_prim_rel_path: str = MISSING
"""Relative path to the collision geom from the default prim in the USD file.
This is used to apply physics material to the rigid body.
"""
scale: Tuple[float, float, float] = (1.0, 1.0, 1.0)
"""Scale to spawn the object with. Defaults to (1.0, 1.0, 1.0)."""
......@@ -41,17 +36,17 @@ class RigidObjectCfg:
"""Disable gravity for the actor. Defaults to False."""
@configclass
class PhysicsMaterialPropertiesCfg:
"""Properties to apply to the rigid body."""
class PhysicsMaterialCfg:
"""Physics material applied to the rigid object."""
static_friction: Optional[float] = 0.5
"""Static friction for the rigid body. Defaults to 0.5."""
dynamic_friction: Optional[float] = 0.5
"""Dynamic friction for the rigid body. Defaults to 0.5."""
restitution: Optional[float] = 0.0
"""Restitution for the rigid body. Defaults to 0.0."""
material_path: Optional[str] = "/physics_material"
"""Relative path to spawn the material for the rigid body. Defaults to "/physics_material"."""
prim_path: str = "/World/Materials/rigidMaterial"
"""Path to the physics material prim. Default: /World/Materials/rigidMaterial."""
static_friction: float = 0.5
"""Static friction coefficient. Defaults to 0.5."""
dynamic_friction: float = 0.5
"""Dynamic friction coefficient. Defaults to 0.5."""
restitution: float = 0.0
"""Restitution coefficient. Defaults to 0.0."""
@configclass
class InitialStateCfg:
......@@ -74,10 +69,13 @@ class RigidObjectCfg:
##
meta_info: MetaInfoCfg = MetaInfoCfg()
"""Meta-information about the rigid body."""
"""Meta-information about the rigid object."""
init_state: InitialStateCfg = InitialStateCfg()
"""Initial state of the rigid body."""
"""Initial state of the rigid object."""
rigid_props: RigidBodyPropertiesCfg = RigidBodyPropertiesCfg()
"""Properties to apply to the rigid body."""
material_props: PhysicsMaterialPropertiesCfg = PhysicsMaterialPropertiesCfg()
"""Properties to apply to the physics material on the rigid body."""
"""Properties to apply to all rigid bodies in the object."""
physics_material: Optional[PhysicsMaterialCfg] = PhysicsMaterialCfg()
"""Settings for the physics material to apply to the rigid object.
If set to None, no physics material will be created and applied.
"""
......@@ -8,7 +8,6 @@ from typing import Dict, Optional, Sequence
from omni.isaac.core.materials import PhysicsMaterial
from omni.isaac.core.prims import RigidPrimView
from omni.isaac.core.prims.geometry_prim_view import GeometryPrimView
from pxr import PhysxSchema
import omni.isaac.orbit.utils.kit as kit_utils
......@@ -66,21 +65,24 @@ class LeggedRobot(RobotBase):
# alter physics collision properties
kit_utils.set_nested_collision_properties(prim_path, contact_offset=0.02, rest_offset=0.0)
# add physics material to the feet bodies!
# TODO: Make this something configurable?
foot_material = PhysicsMaterial(
prim_path=self.cfg.physics_material.prim_path,
static_friction=self.cfg.physics_material.static_friction,
dynamic_friction=self.cfg.physics_material.dynamic_friction,
restitution=self.cfg.physics_material.restitution,
)
# enable patch-friction: yields better results!
physx_material_api = PhysxSchema.PhysxMaterialAPI.Apply(foot_material.prim)
physx_material_api.CreateImprovePatchFrictionAttr().Set(True)
# add to bodies
body_names = [foot_cfg.body_name for foot_cfg in self.cfg.feet_info.values()]
# bind materials
geom_prim = GeometryPrimView(f"{prim_path}/{body_names}/collisions", reset_xform_properties=False)
geom_prim.apply_physics_materials(foot_material, weaker_than_descendants=False)
if self.cfg.physics_material is not None:
# -- resolve material path
material_path = self.cfg.physics_material.prim_path
if not material_path.startswith("/"):
material_path = prim_path + "/" + material_path
# -- create material
material = PhysicsMaterial(
prim_path=material_path,
static_friction=self.cfg.physics_material.static_friction,
dynamic_friction=self.cfg.physics_material.dynamic_friction,
restitution=self.cfg.physics_material.restitution,
)
# -- enable patch-friction: yields better results!
physx_material_api = PhysxSchema.PhysxMaterialAPI.Apply(material.prim)
physx_material_api.CreateImprovePatchFrictionAttr().Set(True)
# -- bind material to feet
for foot_cfg in self.cfg.feet_info.values():
kit_utils.apply_nested_physics_material(f"{prim_path}/{foot_cfg.body_name}", material.prim_path)
def initialize(self, prim_paths_expr: Optional[str] = None):
# initialize parent handles
......
......@@ -4,7 +4,7 @@
# SPDX-License-Identifier: BSD-3-Clause
from dataclasses import MISSING
from typing import Dict, Tuple
from typing import Dict, Optional, Tuple
from omni.isaac.orbit.utils import configclass
......@@ -48,5 +48,8 @@ class LeggedRobotCfg(RobotBaseCfg):
The returned tensor for feet state is in the same order as that of the provided list.
"""
physics_material: PhysicsMaterialCfg = PhysicsMaterialCfg()
"""Settings for the physics material to apply to feet."""
physics_material: Optional[PhysicsMaterialCfg] = PhysicsMaterialCfg()
"""Settings for the physics material to apply to feet.
If set to None, no physics material will be created and applied.
"""
......@@ -13,7 +13,7 @@ import omni.isaac.core.utils.prims as prim_utils
import omni.kit
from omni.isaac.core.materials import PhysicsMaterial
from omni.isaac.core.prims import GeometryPrim
from pxr import Gf, PhysxSchema, UsdPhysics
from pxr import Gf, PhysxSchema, UsdPhysics, UsdShade
def create_ground_plane(
......@@ -444,6 +444,49 @@ def set_collision_properties(
physx_collision_api.GetMinTorsionalPatchRadiusAttr().Set(min_torsional_patch_radius)
def apply_physics_material(prim_path: str, material_path: str, weaker_than_descendants: bool = False):
"""Apply a physics material to a prim.
Physics material can be applied only to a prim with physics-enabled on them. This includes having
a collision APIs, or deformable body APIs, or being a particle system.
Args:
prim_path (str): The prim path of parent.
material_path (str): The prim path of the material to apply.
Raises:
ValueError: If the material path does not exist on stage.
ValueError: When prim at specified path is not physics-enabled.
"""
# check if material exists
if not prim_utils.is_prim_path_valid(material_path):
raise ValueError(f"Physics material '{material_path}' does not exist.")
# get USD prim
prim = prim_utils.get_prim_at_path(prim_path)
# check if prim has collision applied on it
has_collider = prim.HasAPI(UsdPhysics.CollisionAPI)
has_deformable_body = prim.HasAPI(PhysxSchema.PhysxDeformableBodyAPI)
has_particle_system = prim.IsA(PhysxSchema.PhysxParticleSystem)
if not (has_collider or has_deformable_body or has_particle_system):
raise ValueError(
f"Cannot apply physics material on prim '{prim_path}'. It is neither a collider, nor a deformable body, nor a particle system."
)
# obtain material binding API
if prim.HasAPI(UsdShade.MaterialBindingAPI):
material_binding_api = UsdShade.MaterialBindingAPI(prim)
else:
material_binding_api = UsdShade.MaterialBindingAPI.Apply(prim)
# obtain the material prim
material = UsdShade.Material(prim_utils.get_prim_at_path(material_path))
# resolve token for weaker than descendants
if weaker_than_descendants:
binding_strength = UsdShade.Tokens.weakerThanDescendants
else:
binding_strength = UsdShade.Tokens.strongerThanDescendants
# apply the material
material_binding_api.Bind(material, bindingStrength=binding_strength, materialPurpose="physics")
def set_nested_articulation_properties(prim_path: str, **kwargs) -> None:
"""Set PhysX parameters on all articulations under specified prim-path.
......@@ -551,3 +594,35 @@ def set_nested_collision_properties(prim_path: str, **kwargs):
set_collision_properties(prim_utils.get_prim_path(child_prim), **kwargs)
# add all children to tree
all_prims += child_prim.GetChildren()
def apply_nested_physics_material(prim_path: str, material_path: str, weaker_than_descendants: bool = False):
"""Apply the physics material on all meshes under a specified prim path.
Physics material can be applied only to a prim with physics-enabled on them. This includes having
a collision APIs, or deformable body APIs, or being a particle system.
Args:
prim_path (str): The prim path under which to search and apply physics material.
material_path (str): The path to the physics material to apply.
weaker_than_descendants (bool, optional): Whether the material should override the
descendants materials. Defaults to False.
Raises:
ValueError: If the material path does not exist on stage.
"""
# check if material exists
if not prim_utils.is_prim_path_valid(material_path):
raise ValueError(f"Physics material '{material_path}' does not exist.")
# get USD prim
prim = prim_utils.get_prim_at_path(prim_path)
# iterate over all prims under prim-path
all_prims = [prim]
while len(all_prims) > 0:
# get current prim
child_prim = all_prims.pop(0)
# set physics material
with contextlib.suppress(ValueError):
apply_physics_material(prim_utils.get_prim_path(child_prim), material_path, weaker_than_descendants)
# add all children to tree
all_prims += child_prim.GetChildren()
......@@ -586,3 +586,40 @@ def sample_uniform(
size = (size,)
# return tensor
return torch.rand(*size, device=device) * (upper - lower) + lower
def sample_cylinder(
radius: float, h_range: Tuple[float, float], size: Union[int, Tuple[int, ...]], device: str
) -> torch.Tensor:
"""Sample 3D points uniformly on a cylinder's surface.
The cylinder is centered at the origin and aligned with the z-axis. The height of the cylinder is
sampled uniformly from the range :obj:`h_range`, while the radius is fixed to :obj:`radius`.
The sampled points are returned as a tensor of shape :obj:`(*size, 3)`, i.e. the last dimension
contains the x, y, and z coordinates of the sampled points.
Args:
radius (float): The radius of the cylinder.
h_range (Tuple[float, float]): The minimum and maximum height of the cylinder.
size (Union[int, Tuple[int, ...]]): The shape of the tensor.
device (str): Device to create tensor on.
Returns:
torch.Tensor: Sampled tensor of shape :obj:`(*size, 3)`.
"""
# sample angles
angles = (torch.rand(size, device=device) * 2 - 1) * np.pi
h_min, h_max = h_range
# add shape
if isinstance(size, int):
size = (size, 3)
else:
size += (3,)
# allocate a tensor
xyz = torch.zeros(size, device=device)
xyz[..., 0] = radius * torch.cos(angles)
xyz[..., 1] = radius * torch.sin(angles)
xyz[..., 2].uniform_(h_min, h_max)
# return positions
return xyz
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.2.1"
version = "0.2.2"
# Description
title = "ORBIT Environments"
......
Changelog
---------
0.2.2 (2023-03-04)
~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
* Fixed the issue with rigid object not working in the ``Isaac-Lift-Franka-v0`` environment.
0.2.1 (2023-03-01)
~~~~~~~~~~~~~~~~~~
......
......@@ -31,7 +31,6 @@ class ManipulationObjectCfg(RigidObjectCfg):
meta_info = RigidObjectCfg.MetaInfoCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd",
geom_prim_rel_path="/collisions",
scale=(0.8, 0.8, 0.8),
)
init_state = RigidObjectCfg.InitialStateCfg(
......@@ -43,8 +42,8 @@ class ManipulationObjectCfg(RigidObjectCfg):
max_depenetration_velocity=10.0,
disable_gravity=False,
)
material_props = RigidObjectCfg.PhysicsMaterialPropertiesCfg(
static_friction=0.5, dynamic_friction=0.5, restitution=0.0, material_path="/physics_material"
physics_material = RigidObjectCfg.PhysicsMaterialCfg(
static_friction=0.5, dynamic_friction=0.5, restitution=0.0, prim_path="/World/Materials/cubeMaterial"
)
......@@ -53,9 +52,9 @@ class GoalMarkerCfg:
"""Properties for visualization marker."""
# usd file to import
usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd"
usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd"
# scale of the asset at import
scale = [0.8, 0.8, 0.8] # x,y,z
scale = [0.05, 0.05, 0.05] # x,y,z
@configclass
......
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This script demonstrates how to use the rigid objects class.
"""
"""Launch Isaac Sim Simulator first."""
import argparse
from omni.isaac.kit import SimulationApp
# add argparse arguments
parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
args_cli = parser.parse_args()
# launch omniverse app
config = {"headless": args_cli.headless}
simulation_app = SimulationApp(config)
"""Rest everything follows."""
import scipy.spatial.transform as tf
import torch
import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.core.simulation_context import SimulationContext
from omni.isaac.core.utils.viewports import set_camera_view
import omni.isaac.orbit.utils.kit as kit_utils
from omni.isaac.orbit.objects.rigid import RigidObject, RigidObjectCfg
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
from omni.isaac.orbit.utils.math import convert_quat, quat_mul, random_yaw_orientation, sample_cylinder
"""
Helpers
"""
def design_scene():
"""Add prims to the scene."""
# Ground-plane
kit_utils.create_ground_plane(
"/World/defaultGroundPlane",
static_friction=0.5,
dynamic_friction=0.5,
restitution=0.8,
improve_patch_friction=True,
)
# Lights-1
prim_utils.create_prim(
"/World/Light/GreySphere",
"SphereLight",
translation=(4.5, 3.5, 10.0),
attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)},
)
# Lights-2
prim_utils.create_prim(
"/World/Light/WhiteSphere",
"SphereLight",
translation=(-4.5, 3.5, 10.0),
attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)},
)
"""
Main
"""
def main():
"""Imports all legged robots supported in Orbit and applies zero actions."""
# Load kit helper
sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01, backend="torch", device="cpu")
# Set main camera
set_camera_view(eye=[1.5, 1.5, 1.5], target=[0.0, 0.0, 0.0])
# Spawn things into stage
# design props
design_scene()
# add YCB objects
ycb_usd_paths = {
"crackerBox": f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/003_cracker_box.usd",
"sugarBox": f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/004_sugar_box.usd",
"tomatoSoupCan": f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/005_tomato_soup_can.usd",
"mustardBottle": f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/006_mustard_bottle.usd",
}
for key, usd_path in ycb_usd_paths.items():
translation = torch.rand(3).tolist()
prim_utils.create_prim(f"/World/Objects/{key}", usd_path=usd_path, translation=translation)
# Setup rigid object
cfg = RigidObjectCfg()
# -- usd path
cfg.meta_info.usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/003_cracker_box.usd"
# -- rotate the object to align with the ground plane
cfg.init_state.rot = convert_quat(tf.Rotation.from_euler("XYZ", (-90, 90, 0), degrees=True).as_quat(), to="wxyz")
# Create rigid object handler
rigid_object = RigidObject(cfg)
# Spawn rigid object
# note: Spawning object like this will apply rigid object properties and physics material configurations.
rigid_object.spawn("/World/Objects/crackerBox2")
# Play the simulator
sim.reset()
# Initialize handles
# note: We desire view over all the objects in the scene.
rigid_object.initialize("/World/Objects/.*")
# Now we are ready!
print("[INFO]: Setup complete...")
# Define simulation stepping
sim_dt = sim.get_physics_dt()
sim_time = 0.0
count = 0
# Simulate physics
while simulation_app.is_running():
# If simulation is stopped, then exit.
if sim.is_stopped():
break
# If simulation is paused, then skip.
if not sim.is_playing():
sim.step(render=not args_cli.headless)
continue
# reset
if count % 250 == 0:
# reset counters
sim_time = 0.0
count = 0
# reset root state
root_state = rigid_object.get_default_root_state()
# -- position
root_state[:, :3] = sample_cylinder(
radius=0.5, h_range=(0.15, 0.25), size=rigid_object.count, device=rigid_object.device
)
# -- orientation: apply yaw rotation
root_state[:, 3:7] = quat_mul(
random_yaw_orientation(rigid_object.count, rigid_object.device), root_state[:, 3:7]
)
# -- set root state
rigid_object.set_root_state(root_state)
# reset buffers
rigid_object.reset_buffers()
print(">>>>>>>> Reset!")
# perform step
sim.step()
# update sim-time
sim_time += sim_dt
count += 1
# note: to deal with timeline events such as stopping, we need to check if the simulation is playing
if sim.is_playing():
# update buffers
rigid_object.update_buffers(sim_dt)
if __name__ == "__main__":
# Run the main function
main()
# Close the simulator
simulation_app.close()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment