Skip to content

Commit

Permalink
Fixes invisible to secondary rays for markers (#316)
Browse files Browse the repository at this point in the history
# Description

Earlier, the markers set "invisible" to secondary rays on the
wrong prim (XForm instead of mesh). For context, this property is used
when we don't want to see the marker prims on secondary rendering
operations used to compute depth and semantic images. However,
the markers are still visible on RGB.

This MR makes sure that the marker prims are invisible to these rays by
setting the property on the correct prim.

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## Screenshots

| Before | After |
| ------ | ----- |
|
![earlier](https://github.com/isaac-orbit/orbit/assets/12863862/140f0afb-86bc-48e9-9812-7e7d9489cecb)
|
![now](https://github.com/isaac-orbit/orbit/assets/12863862/643b1ed3-b454-4a94-b2e7-256aed012d13)
|

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there
  • Loading branch information
Mayankm96 authored Dec 19, 2023
1 parent 679dbe8 commit 5710bc4
Show file tree
Hide file tree
Showing 5 changed files with 235 additions and 14 deletions.
2 changes: 1 addition & 1 deletion source/extensions/omni.isaac.orbit/config/extension.toml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
[package]

# Note: Semantic Versioning is used: https://semver.org/
version = "0.10.5"
version = "0.10.6"

# Description
title = "ORBIT framework for Robot Learning"
Expand Down
18 changes: 18 additions & 0 deletions source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,6 +1,24 @@
Changelog
---------

0.10.6 (2023-12-19)
~~~~~~~~~~~~~~~~~~~

Added
^^^^^

* Added support for using articulations as visualization markers. This disables all physics APIs from
the articulation and allows the user to use it as a visualization marker. It is useful for creating
visualization markers for the end-effectors or base of the robot.

Fixed
^^^^^

* Fixed hiding of debug markers from secondary images when using the
:class:`omni.isaac.orbit.markers.VisualizationMarkers` class. Earlier, the properties were applied on
the XForm prim instead of the Mesh prim.


0.10.5 (2023-12-18)
~~~~~~~~~~~~~~~~~~~

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
import omni.isaac.core.utils.stage as stage_utils
import omni.kit.commands
import omni.physx.scripts.utils as physx_utils
from pxr import Gf, Sdf, UsdGeom, Vt
from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, Vt

import omni.isaac.orbit.sim as sim_utils
from omni.isaac.orbit.sim.spawners import SpawnerCfg
Expand Down Expand Up @@ -347,17 +347,9 @@ def _add_markers_prototypes(self, markers_cfg: dict[str, sim_utils.SpawnerCfg]):
prim = cfg.func(prim_path=marker_prim_path, cfg=cfg)
# make the asset uninstanceable (in case it is)
# point instancer defines its own prototypes so if an asset is already instanced, this doesn't work.
sim_utils.make_uninstanceable(marker_prim_path)
self._process_prototype_prim(prim)
# remove any physics on the markers because they are only for visualization!
physx_utils.removeRigidBodySubtree(prim)
# make marker invisible to secondary rays
omni.kit.commands.execute(
"ChangePropertyCommand",
prop_path=Sdf.Path(f"{marker_prim_path}.primvars:invisibleToSecondaryRays"),
value=True,
prev=None,
type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool,
)
# add child reference to point instancer
self._instancer_manager.GetPrototypesRel().AddTarget(marker_prim_path)
# check that we loaded all the prototypes
Expand All @@ -366,3 +358,51 @@ def _add_markers_prototypes(self, markers_cfg: dict[str, sim_utils.SpawnerCfg]):
raise RuntimeError(
f"Failed to load all the prototypes. Expected: {len(markers_cfg)}. Received: {len(prototypes)}."
)

def _process_prototype_prim(self, prim: Usd.Prim):
"""Process a prim and its descendants to make them suitable for defining prototypes.
Point instancer defines its own prototypes so if an asset is already instanced, this doesn't work.
This function checks if the prim at the specified prim path and its descendants are instanced.
If so, it makes the respective prim uninstanceable by disabling instancing on the prim.
Additionally, it makes the prim invisible to secondary rays. This is useful when we do not want
to see the marker prims on camera images.
Args:
prim_path: The prim path to check.
stage: The stage where the prim exists.
Defaults to None, in which case the current stage is used.
"""
# check if prim is valid
if not prim.IsValid():
raise ValueError(f"Prim at path '{prim.GetPrimAtPath()}' is not valid.")
# iterate over all prims under prim-path
all_prims = [prim]
while len(all_prims) > 0:
# get current prim
child_prim = all_prims.pop(0)
# check if it is physics body -> if so, remove it
if child_prim.HasAPI(UsdPhysics.ArticulationRootAPI):
child_prim.RemoveAPI(UsdPhysics.ArticulationRootAPI)
child_prim.RemoveAPI(PhysxSchema.PhysxArticulationAPI)
if child_prim.HasAPI(UsdPhysics.RigidBodyAPI):
child_prim.RemoveAPI(UsdPhysics.RigidBodyAPI)
child_prim.RemoveAPI(PhysxSchema.PhysxRigidBodyAPI)
if child_prim.IsA(UsdPhysics.Joint):
child_prim.GetAttribute("physics:jointEnabled").Set(False)
# check if prim is instanced -> if so, make it uninstanceable
if child_prim.IsInstance():
child_prim.SetInstanceable(False)
# check if prim is a mesh -> if so, make it invisible to secondary rays
if child_prim.IsA(UsdGeom.Gprim):
# invisible to secondary rays such as depth images
omni.kit.commands.execute(
"ChangePropertyCommand",
prop_path=Sdf.Path(f"{child_prim.GetPrimPath().pathString}.primvars:invisibleToSecondaryRays"),
value=True,
prev=None,
type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool,
)
# add children to list
all_prims += child_prim.GetChildren()
Original file line number Diff line number Diff line change
@@ -0,0 +1,158 @@
# Copyright (c) 2022-2023, The ORBIT Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

"""
This script checks if the debug markers are visible from the camera.
To check if the markers are visible on different rendering modalities, you can switch them by going
through the synthetic data generation tool in the Isaac Sim UI. For more information,
please check: https://www.youtube.com/watch?v=vLk-f9LWj48&ab_channel=NVIDIAOmniverse
.. code-block:: bash
# Usage
./orbit.sh -p source/extensions/omni.isaac.orbit/test/markers/check_markers_visibility.py
"""

from __future__ import annotations

"""Launch Isaac Sim Simulator first."""


import argparse

from omni.isaac.orbit.app import AppLauncher

# add argparse arguments
parser = argparse.ArgumentParser(description="Check if the debug markers are visible from the camera.")
parser.add_argument("--num_envs", type=int, default=2, help="Number of environments to spawn.")
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()

# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

"""Rest everything follows."""

import traceback

import carb

import omni.isaac.orbit.sim as sim_utils
from omni.isaac.orbit.assets import ArticulationCfg, AssetBaseCfg
from omni.isaac.orbit.assets.config.anymal import ANYMAL_C_CFG
from omni.isaac.orbit.scene import InteractiveScene, InteractiveSceneCfg
from omni.isaac.orbit.sensors import RayCasterCfg, patterns
from omni.isaac.orbit.utils import configclass


@configclass
class SensorsSceneCfg(InteractiveSceneCfg):
"""Design the scene with sensors on the robot."""

# ground plane
ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg())

# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)

# robot
robot: ArticulationCfg = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")

# sensors
height_scanner = RayCasterCfg(
prim_path="{ENV_REGEX_NS}/Robot/base",
update_period=0.02,
offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)),
attach_yaw_only=True,
pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]),
debug_vis=True,
mesh_prim_paths=["/World/defaultGroundPlane"],
)


def run_simulator(
sim: sim_utils.SimulationContext,
scene: InteractiveScene,
):
"""Run the simulator."""

# Define simulation stepping
sim_dt = sim.get_physics_dt()
sim_time = 0.0
count = 0

# Simulate physics
while simulation_app.is_running():
# Reset
if count % 500 == 0:
# reset counter
count = 0
# reset the scene entities
# root state
root_state = scene["robot"].data.default_root_state.clone()
root_state[:, :3] += scene.env_origins
scene["robot"].write_root_state_to_sim(root_state)
# set joint positions with some noise
joint_pos, joint_vel = (
scene["robot"].data.default_joint_pos.clone(),
scene["robot"].data.default_joint_vel.clone(),
)
scene["robot"].write_joint_state_to_sim(joint_pos, joint_vel)
# clear internal buffers
scene.reset()
print("[INFO]: Resetting robot state...")
# Apply default actions to the robot
# -- generate actions/commands
targets = scene["robot"].data.default_joint_pos
# -- apply action to the robot
scene["robot"].set_joint_position_target(targets)
# -- write data to sim
scene.write_data_to_sim()
# perform step
sim.step()
# update sim-time
sim_time += sim_dt
count += 1
# update buffers
scene.update(sim_dt)


def main():
"""Main function."""

# Initialize the simulation context
sim_cfg = sim_utils.SimulationCfg(dt=0.005, substeps=1)
sim = sim_utils.SimulationContext(sim_cfg)
# Set main camera
sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0])
# design scene
scene_cfg = SensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
scene = InteractiveScene(scene_cfg)
# Play the simulator
sim.reset()
# Now we are ready!
print("[INFO]: Setup complete...")
# Run the simulator
run_simulator(sim, scene)


if __name__ == "__main__":
try:
# run the main execution
main()
except Exception as err:
carb.log_error(err)
carb.log_error(traceback.format_exc())
raise
finally:
# close sim app
simulation_app.close()
11 changes: 8 additions & 3 deletions source/standalone/demos/markers.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
import omni.isaac.orbit.sim as sim_utils
from omni.isaac.orbit.markers import VisualizationMarkers, VisualizationMarkersCfg
from omni.isaac.orbit.sim import SimulationContext
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR, ISAAC_ORBIT_NUCLEUS_DIR
from omni.isaac.orbit.utils.math import quat_from_angle_axis


Expand Down Expand Up @@ -87,6 +87,11 @@ def spawn_markers():
scale=(10.0, 10.0, 10.0),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.25, 0.0)),
),
"robot_mesh": sim_utils.UsdFileCfg(
usd_path=f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd",
scale=(2.0, 2.0, 2.0),
visual_material=sim_utils.GlassMdlCfg(glass_color=(0.0, 0.1, 0.0)),
),
},
)
return VisualizationMarkers(marker_cfg)
Expand All @@ -97,11 +102,11 @@ def main():
# Load kit helper
sim = SimulationContext(sim_utils.SimulationCfg(dt=0.01, substeps=1))
# Set main camera
sim.set_camera_view([0.0, 17.0, 12.0], [0.0, 2.0, 0.0])
sim.set_camera_view([0.0, 18.0, 12.0], [0.0, 3.0, 0.0])

# Spawn things into stage
# Lights
cfg = sim_utils.DistantLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
cfg = sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
cfg.func("/World/Light", cfg)

# create markers
Expand Down

0 comments on commit 5710bc4

Please sign in to comment.