Fix more API calls

This commit is contained in:
Cem Gökmen 2024-04-18 03:00:19 -07:00
parent d334904979
commit 8fd93a4dbf
25 changed files with 115 additions and 71 deletions

View File

@ -154,6 +154,7 @@ def create_projection_visualization(
# Override the prototype with our own sphere with optional material
prototype_path = "/".join(sprite_path.split("/")[:-1]) + "/prototype"
create_primitive_mesh(prototype_path, primitive_type="Sphere")
# TODO(parallel): Update
prototype = VisualGeomPrim(prim_path=prototype_path, name=f"{projection_name}_prototype")
prototype.initialize()
# Set the scale (native scaling --> radius 0.5) and possibly update the material
@ -386,7 +387,11 @@ class ParticleModifier(IntrinsicObjectState, LinkBasedStateMixin, UpdateStateMix
)
# Create the visual geom instance referencing the generated mesh prim, and then hide it
self.projection_mesh = VisualGeomPrim(prim_path=mesh_prim_path, name=f"{name_prefix}_projection_mesh")
# TODO(parallel): Update
self.projection_mesh = VisualGeomPrim(
relative_prim_path=self.obj.absolute_prim_path_to_scene_relative(mesh_prim_path),
name=f"{name_prefix}_projection_mesh",
)
self.projection_mesh.initialize()
self.projection_mesh.visible = False
@ -1061,9 +1066,11 @@ class ParticleApplier(ParticleModifier):
parent_scale=self.link.scale,
material=system.material,
)
# TODO(parallel): Update this
self.projection_system_prim = BasePrim(
prim_path=self.projection_system.GetPrimPath().pathString, name=projection_name
)
# TODO(parallel): Update this
# Create the visual geom instance referencing the generated source mesh prim, and then hide it
self.projection_source_sphere = VisualGeomPrim(
prim_path=projection_visualization_path, name=f"{name_prefix}_projection_source_sphere"

View File

@ -114,6 +114,7 @@ class ToggledOn(AbsoluteObjectState, BooleanStateMixin, LinkBasedStateMixin, Upd
self.scale = np.array(pre_existing_mesh.GetAttribute("xformOp:scale").Get())
# Create the visual geom instance referencing the generated mesh prim
# TODO(parallel): Update this
self.visual_marker = VisualGeomPrim(prim_path=mesh_prim_path, name=f"{self.obj.name}_visual_marker")
self.visual_marker.scale = self.scale
self.visual_marker.initialize()

View File

@ -118,7 +118,7 @@ class LightObject(StatefulObject):
# Grab reference to light link
self._light_link = XFormPrim(
relative_prim_path=f"{self.relative_prim_path}/base_link/light", name=f"{self.name}:light_link"
relative_prim_path=f"{self._relative_prim_path}/base_link/light", name=f"{self.name}:light_link"
)
self._light_link.load(self.scene)

View File

@ -252,11 +252,11 @@ class EntityPrim(XFormPrim):
"kinematic_only": (
self._load_config.get("kinematic_only", False) if link_name == self._root_link_name else False
),
"is_part_of_articulation": self.articulation_view is not None,
"is_part_of_articulation": self._articulation_view is not None,
"remesh": self._load_config.get("remesh", True),
}
self._links[link_name] = link_cls(
relative_prim_path=self.scene.absolute_prim_path_to_relative(prim.GetPrimPath().__str__()),
relative_prim_path=self.absolute_prim_path_to_scene_relative(prim.GetPrimPath().__str__()),
name=f"{self._name}:{link_name}",
load_config=link_load_config,
)
@ -289,7 +289,7 @@ class EntityPrim(XFormPrim):
joint_dof_offset = self._articulation_view._metadata.joint_dof_offsets[i]
joint_path = self._articulation_view._dof_paths[0][joint_dof_offset]
joint = JointPrim(
relative_prim_path=self.scene.absolute_prim_path_to_relative(joint_path),
relative_prim_path=self.absolute_prim_path_to_scene_relative(joint_path),
name=f"{self._name}:joint_{joint_name}",
articulation_view=self._articulation_view_direct,
)
@ -1512,7 +1512,7 @@ class EntityPrim(XFormPrim):
# Create a attachment point link
link = RigidPrim(
prim_path=link_prim.GetPrimPath().__str__(),
relative_prim_path=self.absolute_prim_path_to_scene_relative(link_prim.GetPrimPath().pathString),
name=f"{self._name}:{link_name}",
)
link.disable_collisions()

View File

@ -152,14 +152,48 @@ class BasePrim(Serializable, UniquelyNamed, Recreatable, ABC):
Returns:
str: prim path in the stage.
"""
return self.scene_relative_prim_path_to_absolute(self._relative_prim_path)
def scene_relative_prim_path_to_absolute(self, relative_prim_path):
"""
Converts a scene-relative prim path to an absolute prim path.
Args:
relative_prim_path (str): Relative prim path in the scene
Returns:
str: Absolute prim path in the stage
"""
assert self._scene_assigned, f"A scene was not yet set for prim with {self._relative_prim_path}"
# When the scene is set to None, this prim is not in a scene but is global e.g. like the
# viewer camera or one of the scene prims.
if self._scene is None:
return "/World" + self._relative_prim_path
return "/World" + relative_prim_path
return self._scene.relative_prim_path_to_absolute(self._relative_prim_path)
return self._scene.relative_prim_path_to_absolute(relative_prim_path)
def absolute_prim_path_to_scene_relative(self, absolute_prim_path):
"""
Converts an absolute prim path to a scene-relative prim path.
Args:
absolute_prim_path (str): Absolute prim path in the stage
Returns:
str: Relative prim path in the scene
"""
assert self._scene_assigned, f"A scene was not yet set for prim with {self._relative_prim_path}"
# When the scene is set to None, this prim is not in a scene but is global e.g. like the
# viewer camera or one of the scene prims.
if self._scene is None:
assert absolute_prim_path.startswith(
"/World"
), f"Expected absolute prim path to start with /World, got {absolute_prim_path}"
return absolute_prim_path[len("/World") :]
return self._scene.absolute_prim_path_to_relative(absolute_prim_path)
@property
def name(self):

View File

@ -201,7 +201,7 @@ class RigidPrim(XFormPrim):
mesh_prim = lazy.omni.isaac.core.utils.prims.get_prim_at_path(prim_path=mesh_path)
is_collision = mesh_prim.HasAPI(lazy.pxr.UsdPhysics.CollisionAPI)
mesh_kwargs = {
"relative_prim_path": self.scene.absolute_prim_path_to_relative(mesh_path),
"relative_prim_path": self.absolute_prim_path_to_scene_relative(mesh_path),
"name": f"{self._name}:{'collision' if is_collision else 'visual'}_{mesh_name}",
}
if is_collision:

View File

@ -58,7 +58,7 @@ class BehaviorRobot(ManipulationRobot, LocomotionRobot, ActiveCameraRobot):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -89,7 +89,7 @@ class BehaviorRobot(ManipulationRobot, LocomotionRobot, ActiveCameraRobot):
"""
super(BehaviorRobot, self).__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,
@ -117,13 +117,18 @@ class BehaviorRobot(ManipulationRobot, LocomotionRobot, ActiveCameraRobot):
self.parts[arm_name] = BRPart(
name=arm_name,
parent=self,
prim_path=f"{arm_name}_palm",
relative_prim_path=f"/{arm_name}_palm",
eef_type="hand",
offset_to_body=m.HAND_TO_BODY_OFFSET[arm_name],
**kwargs,
)
self.parts["head"] = BRPart(
name="head", parent=self, prim_path="eye", eef_type="head", offset_to_body=m.HEAD_TO_BODY_OFFSET, **kwargs
name="head",
parent=self,
relative_prim_path="/eye",
eef_type="head",
offset_to_body=m.HEAD_TO_BODY_OFFSET,
**kwargs,
)
# whether to use ghost hands (visual markers to help visualize current vr hand pose)
@ -531,7 +536,7 @@ class BRPart(ABC):
if self.eef_type == "hand" and self.parent._use_ghost_hands:
gh_name = f"ghost_hand_{self.name}"
self.ghost_hand = USDObject(
prim_path=f"/World/{gh_name}",
relative_prim_path=f"/{gh_name}",
usd_path=os.path.join(gm.ASSET_PATH, f"models/behavior_robot/usd/{gh_name}.usd"),
name=gh_name,
scale=0.001,

View File

@ -38,7 +38,7 @@ class Fetch(ManipulationRobot, TwoWheelRobot, ActiveCameraRobot):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -135,7 +135,7 @@ class Fetch(ManipulationRobot, TwoWheelRobot, ActiveCameraRobot):
# Run super init
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,

View File

@ -16,7 +16,7 @@ class FrankaPanda(ManipulationRobot):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -89,7 +89,7 @@ class FrankaPanda(ManipulationRobot):
"""
# Run super init
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,

View File

@ -16,7 +16,7 @@ class FrankaAllegro(ManipulationRobot):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -90,7 +90,7 @@ class FrankaAllegro(ManipulationRobot):
# Run super init
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,

View File

@ -18,7 +18,7 @@ class FrankaLeap(ManipulationRobot):
# Shared kwargs in hierarchy
name,
hand="right",
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -94,7 +94,7 @@ class FrankaLeap(ManipulationRobot):
self.hand = hand
# Run super init
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,

View File

@ -64,7 +64,7 @@ class ManipulationRobot(BaseRobot):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -163,7 +163,7 @@ class ManipulationRobot(BaseRobot):
# Call super() method
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,

View File

@ -48,7 +48,7 @@ class BaseRobot(USDObject, ControllableObject, GymObservable):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -139,7 +139,7 @@ class BaseRobot(USDObject, ControllableObject, GymObservable):
# Run super init
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
usd_path=self.usd_path,
name=name,
category=m.ROBOT_CATEGORY,
@ -188,7 +188,7 @@ class BaseRobot(USDObject, ControllableObject, GymObservable):
dummy_prim = add_asset_to_stage(asset_path=self._dummy_usd_path, prim_path=dummy_path)
self._dummy = BaseObject(
name=f"{self.name}_dummy",
relative_prim_path=self.scene.absolute_prim_path_to_relative(dummy_path),
relative_prim_path=self.absolute_prim_path_to_scene_relative(dummy_path),
scale=self._load_config.get("scale", None),
visible=False,
fixed_base=True,
@ -258,7 +258,7 @@ class BaseRobot(USDObject, ControllableObject, GymObservable):
# Create the sensor and store it internally
sensor = create_sensor(
sensor_type=prim_type,
prim_path=str(prim.GetPrimPath()),
relative_prim_path=self.absolute_prim_path_to_scene_relative(str(prim.GetPrimPath())),
name=f"{self.name}:{link_name}:{prim_type}:{sensor_counts[prim_type]}",
**sensor_kwargs,
)

View File

@ -47,7 +47,7 @@ class Tiago(ManipulationRobot, LocomotionRobot, ActiveCameraRobot):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -152,7 +152,7 @@ class Tiago(ManipulationRobot, LocomotionRobot, ActiveCameraRobot):
# Run super init
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,

View File

@ -17,7 +17,7 @@ class VX300S(ManipulationRobot):
self,
# Shared kwargs in hierarchy
name,
prim_path=None,
relative_prim_path=None,
uuid=None,
scale=None,
visible=True,
@ -90,7 +90,7 @@ class VX300S(ManipulationRobot):
"""
# Run super init
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
uuid=uuid,
scale=scale,

View File

@ -13,7 +13,14 @@ SENSOR_PRIMS_TO_SENSOR_CLS = {
def create_sensor(
sensor_type, prim_path, name, modalities="all", enabled=True, sensor_kwargs=None, noise_type=None, noise_kwargs=None
sensor_type,
relative_prim_path,
name,
modalities="all",
enabled=True,
sensor_kwargs=None,
noise_type=None,
noise_kwargs=None,
):
"""
Create a sensor of type @sensor_type with optional keyword args @sensor_kwargs that should be passed to the
@ -23,7 +30,7 @@ def create_sensor(
Args:
sensor_type (str): Type of sensor to create. Should be either one of SENSOR_PRIM_TO_SENSOR.keys() or
one of REGISTERED_SENSORS (i.e.: the string name of the desired class to create)
prim_path (str): prim path of the Sensor to encapsulate or create.
relative_prim_path (str): prim path of the Sensor to encapsulate or create.
name (str): Name for the sensor. Names need to be unique per scene.
modalities (str or list of str): Modality(s) supported by this sensor. Valid options are part of
sensor.all_modalities. Default is "all", which corresponds to all modalities being used
@ -58,7 +65,7 @@ def create_sensor(
# Create the sensor
sensor_kwargs = dict() if sensor_kwargs is None else sensor_kwargs
sensor = sensor_cls(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
modalities=modalities,
enabled=enabled,

View File

@ -15,7 +15,7 @@ class ScanSensor(BaseSensor):
General 2D LiDAR range sensor and occupancy grid sensor.
Args:
prim_path (str): prim path of the Prim to encapsulate or create.
relative_prim_path (str): prim path of the Prim to encapsulate or create.
name (str): Name for the object. Names need to be unique per scene.
modalities (str or list of str): Modality(s) supported by this sensor. Default is "all", which corresponds
to all modalities being used. Otherwise, valid options should be part of cls.all_modalities.
@ -49,7 +49,7 @@ class ScanSensor(BaseSensor):
def __init__(
self,
prim_path,
relative_prim_path,
name,
modalities="all",
enabled=True,
@ -102,7 +102,7 @@ class ScanSensor(BaseSensor):
# Run super method
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
modalities=modalities,
enabled=enabled,

View File

@ -19,7 +19,7 @@ class BaseSensor(XFormPrim, GymObservable, Registerable, metaclass=ABCMeta):
Sensor-specific get_obs method is implemented in subclasses
Args:
prim_path (str): prim path of the Sensor to encapsulate or create.
relative_prim_path (str): prim path of the Sensor to encapsulate or create.
name (str): Name for the sensor. Names need to be unique per scene.
modalities (str or list of str): Modality(s) supported by this sensor. Default is "all", which corresponds
to all modalities being used. Otherwise, valid options should be part of cls.all_modalities.
@ -31,7 +31,7 @@ class BaseSensor(XFormPrim, GymObservable, Registerable, metaclass=ABCMeta):
def __init__(
self,
prim_path,
relative_prim_path,
name,
modalities="all",
enabled=True,
@ -51,7 +51,7 @@ class BaseSensor(XFormPrim, GymObservable, Registerable, metaclass=ABCMeta):
# Run super method
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
load_config=load_config,
)

View File

@ -45,7 +45,7 @@ class VisionSensor(BaseSensor):
- Camera state
Args:
prim_path (str): prim path of the Prim to encapsulate or create.
relative_prim_path (str): prim path of the Prim to encapsulate or create.
name (str): Name for the object. Names need to be unique per scene.
modalities (str or list of str): Modality(s) supported by this sensor. Default is "all", which corresponds
to all modalities being used. Otherwise, valid options should be part of cls.all_modalities.
@ -112,7 +112,7 @@ class VisionSensor(BaseSensor):
def __init__(
self,
prim_path,
relative_prim_path,
name,
modalities="all",
enabled=True,
@ -170,7 +170,7 @@ class VisionSensor(BaseSensor):
# Run super method
super().__init__(
prim_path=prim_path,
relative_prim_path=relative_prim_path,
name=name,
modalities=modalities,
enabled=enabled,

View File

@ -253,7 +253,7 @@ def launch_simulator(*args, **kwargs):
# Here we assign self as the Simulator instance and as og.sim, because certain functions
# called downstream during the initialization of this object will try to access og.sim.
# This makes that possible (and also introduces possible issues around circular dependencies)
Simulator._instance = self
assert og.sim is None, "Only one Simulator instance can be created at a time!"
og.sim = self
# Store vars needed for initialization
@ -312,9 +312,6 @@ def launch_simulator(*args, **kwargs):
state for state in self.object_state_types if issubclass(state, JointBreakSubscribedStateMixin)
}
# Auto-load the dummy stage
self.clear()
# Set the viewer dimensions
if gm.RENDER_VIEWER_CAMERA:
self.viewer_width = viewer_width
@ -333,21 +330,11 @@ def launch_simulator(*args, **kwargs):
def __new__(
cls,
gravity=9.81,
physics_dt=1.0 / 120.0,
rendering_dt=1.0 / 30.0,
stage_units_in_meters=1.0,
viewer_width=gm.DEFAULT_VIEWER_WIDTH,
viewer_height=gm.DEFAULT_VIEWER_HEIGHT,
device_idx=0,
*args,
**kwargs,
):
# Overwrite since we have different kwargs
if Simulator._instance is None:
# The init function assigns itself as Simulator._instance as soon as it starts.
object.__new__(cls)
else:
lazy.carb.log_info("Simulator is defined already, returning the previously defined one")
return Simulator._instance
# Override the SimulationContext instancing system
return object.__new__(cls, *args, **kwargs)
def _set_viewer_camera(self, relative_prim_path="/viewer_camera", viewport_name="Viewport"):
"""
@ -1184,6 +1171,8 @@ def launch_simulator(*args, **kwargs):
scene.clear()
self._scenes = []
# TODO(parallel): Clear scene prims
# Clear all vision sensors and remove viewer camera reference and camera mover reference
VisionSensor.clear()
self._viewer_camera = None
@ -1216,7 +1205,7 @@ def launch_simulator(*args, **kwargs):
self._callbacks_on_remove_obj = dict()
# Load dummy stage, but don't clear sim to prevent circular loops
self._open_new_stage()
self._init_stage(physics_dt=self.get_physics_dt(), rendering_dt=self.get_rendering_dt())
def write_metadata(self, key, data):
"""
@ -1479,7 +1468,7 @@ def launch_simulator(*args, **kwargs):
# Also add skybox if requested
if self._use_skybox:
self._skybox = LightObject(
prim_path="/World/skybox",
relative_prim_path="/skybox",
name="skybox",
category="background",
light_type="Dome",

View File

@ -445,6 +445,7 @@ class MacroVisualParticleSystem(MacroParticleSystem, VisualParticleSystem):
semantic_label=cls.name,
type_label="class",
)
# TODO(parallel): Fix these
return VisualGeomPrim(prim_path=prim_path, name=name)
@classmethod

View File

@ -165,7 +165,7 @@ class PointNavigationTask(BaseTask):
"""
if self._visualize_goal:
self._initial_pos_marker = PrimitiveObject(
prim_path="/World/task_initial_pos_marker",
relative_prim_path="/task_initial_pos_marker",
primitive_type="Cylinder",
name="task_initial_pos_marker",
radius=self._goal_tolerance,
@ -174,7 +174,7 @@ class PointNavigationTask(BaseTask):
rgba=np.array([1, 0, 0, 0.3]),
)
self._goal_pos_marker = PrimitiveObject(
prim_path="/World/task_goal_pos_marker",
relative_prim_path="/task_goal_pos_marker",
primitive_type="Cylinder",
name="task_goal_pos_marker",
radius=self._goal_tolerance,
@ -192,7 +192,7 @@ class PointNavigationTask(BaseTask):
waypoints = []
for i in range(self._n_vis_waypoints):
waypoint = PrimitiveObject(
prim_path=f"/World/task_waypoint_marker{i}",
relative_prim_path=f"/task_waypoint_marker{i}",
primitive_type="Cylinder",
name=f"task_waypoint_marker{i}",
radius=self._waypoint_width,

View File

@ -500,7 +500,7 @@ class CollisionAPI:
# Create the group
col_group_prim_path = f"/World/collision_groups/{col_group}"
group = lazy.pxr.UsdPhysics.CollisionGroup.Define(stage, col_group_prim_path)
group = lazy.pxr.UsdPhysics.CollisionGroup.Define(og.sim.stage, col_group_prim_path)
if filter_self_collisions:
# Do not collide with self
group.GetFilteredGroupsRel().AddTarget(col_group_prim_path)

View File

@ -48,14 +48,14 @@ def benchmark_scene(scene_name, non_rigid_simulation=False, import_robot=True):
print(time.time() - start)
if import_robot:
turtlebot = Turtlebot(prim_path="/World/robot", name="agent", obs_modalities=["rgb"])
turtlebot = Turtlebot(relative_prim_path="/robot", name="agent", obs_modalities=["rgb"])
scene.add_object(turtlebot)
og.sim.step()
if non_rigid_simulation:
cloth = DatasetObject(
name="cloth",
prim_path="/World/cloth",
relative_prim_path="/cloth",
category="t_shirt",
model="kvidcx",
prim_type=PrimType.CLOTH,

View File

@ -54,7 +54,7 @@ def benchmark_scene(sim):
for j in range(NUM_OBJS_PER_ITER):
obj_idx = i * NUM_OBJS_PER_ITER + j
obj = PrimitiveObject(
prim_path=f"/World/obj{obj_idx}",
relative_prim_path=f"/obj{obj_idx}",
primitive_type="Sphere",
name=f"obj{obj_idx}",
scale=OBJ_SCALE,