Skip to content

xform_prim

XFormPrim

Bases: BasePrim

Provides high level functions to deal with an Xform prim and its attributes/ properties. If there is an Xform prim present at the path, it will use it. Otherwise, a new XForm prim at the specified prim path will be created when self.load(...) is called.

the prim will have "xformOp:orient", "xformOp:translate" and "xformOp:scale" only post init,

unless it is a non-root articulation link.

Parameters:

Name Type Description Default
relative_prim_path str

Scene-local prim path of the Prim to encapsulate or create.

required
name str

Name for the object. Names need to be unique per scene.

required
load_config None or dict

If specified, should contain keyword-mapped values that are relevant for loading this prim at runtime. For this xform prim, the below values can be specified:

scale (None or float or 3-array): If specified, sets the scale for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a 3-array specifies per-axis scaling.

None
Source code in OmniGibson/omnigibson/prims/xform_prim.py
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
class XFormPrim(BasePrim):
    """
    Provides high level functions to deal with an Xform prim and its attributes/ properties.
    If there is an Xform prim present at the path, it will use it. Otherwise, a new XForm prim at
    the specified prim path will be created when self.load(...) is called.

    Note: the prim will have "xformOp:orient", "xformOp:translate" and "xformOp:scale" only post init,
        unless it is a non-root articulation link.

    Args:
        relative_prim_path (str): Scene-local prim path of the Prim to encapsulate or create.
        name (str): Name for the object. Names need to be unique per scene.
        load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
            loading this prim at runtime. For this xform prim, the below values can be specified:

            scale (None or float or 3-array): If specified, sets the scale for this object. A single number corresponds
                to uniform scaling along the x,y,z axes, whereas a 3-array specifies per-axis scaling.
    """

    def __init__(
        self,
        relative_prim_path,
        name,
        load_config=None,
    ):
        # Other values that will be filled in at runtime
        self._material = None
        self.original_scale = None
        self._cached_scale = None

        # Run super method
        super().__init__(
            relative_prim_path=relative_prim_path,
            name=name,
            load_config=load_config,
        )

    def _load(self):
        with og.sim.editing_usd():
            return og.sim.stage.DefinePrim(self.prim_path, "Xform")

    def _post_load(self):
        # run super first
        super()._post_load()

        # Make sure all xforms have pose and scaling info
        # These only need to be done if we are creating this prim from scratch AND it is not an instanceable / proxy prim.
        # Pre-created OG objects' prims always have these things set up ahead of time.
        # Note that if this is an instanceable prim, we also don't need write these properties
        # TODO: This still breaks things downstream so we assert here to make sure we have backwards-compatibility with the expected prim types
        assert (
            not self._prim.IsInstanceable() and not self._prim.IsInstanceProxy()
        ), "Support for instanceable prims has not been implemented yet!"
        if not self._xform_props_pre_loaded and not self._prim.IsInstanceable() and not self._prim.IsInstanceProxy():
            self._set_xform_properties()

        # Cache the original scale from the USD so that when EntityPrim sets the scale for each link (Rigid/ClothPrim),
        # the new scale is with respect to the original scale. XFormPrim's scale always matches the scale in the USD.
        self.original_scale = th.tensor(self.get_attribute("xformOp:scale"))

        # Grab the attached material if it exists
        if self.has_material():
            material_prim_path = self._binding_api.GetDirectBinding().GetMaterialPath().pathString
            material_name = f"{self.name}:material"
            material = MaterialPrim.get_material(scene=self.scene, prim_path=material_prim_path, name=material_name)
            assert material.loaded, f"Material prim path {material_prim_path} doesn't exist on stage."
            material.add_user(self)
            self._material = material

        # Optionally set the scale, which is specified with respect to the original scale
        if "scale" in self._load_config and self._load_config["scale"] is not None:
            self.scale = self._load_config["scale"] * self.original_scale

    def remove(self):
        # Remove the material prim if one exists
        if self._material is not None:
            self._material.remove_user(self)

        # Remove the prim
        super().remove()

    def _set_xform_properties(self):
        current_position, current_orientation = self.get_position_orientation()
        properties_to_remove = [
            "xformOp:rotateX",
            "xformOp:rotateXZY",
            "xformOp:rotateY",
            "xformOp:rotateYXZ",
            "xformOp:rotateYZX",
            "xformOp:rotateZ",
            "xformOp:rotateZYX",
            "xformOp:rotateZXY",
            "xformOp:rotateXYZ",
            "xformOp:transform",
        ]
        prop_names = self.prim.GetPropertyNames()
        xformable = lazy.pxr.UsdGeom.Xformable(self.prim)

        with og.sim.editing_usd():
            xformable.ClearXformOpOrder()
            # TODO: wont be able to delete props for non root links on articulated objects
            for prop_name in prop_names:
                if prop_name in properties_to_remove:
                    self.prim.RemoveProperty(prop_name)
            if "xformOp:scale" not in prop_names:
                xform_op_scale = xformable.AddXformOp(
                    lazy.pxr.UsdGeom.XformOp.TypeScale, lazy.pxr.UsdGeom.XformOp.PrecisionDouble, ""
                )
                xform_op_scale.Set(lazy.pxr.Gf.Vec3d([1.0, 1.0, 1.0]))
            else:
                xform_op_scale = lazy.pxr.UsdGeom.XformOp(self._prim.GetAttribute("xformOp:scale"))

            if "xformOp:translate" not in prop_names:
                xform_op_translate = xformable.AddXformOp(
                    lazy.pxr.UsdGeom.XformOp.TypeTranslate, lazy.pxr.UsdGeom.XformOp.PrecisionDouble, ""
                )
            else:
                xform_op_translate = lazy.pxr.UsdGeom.XformOp(self._prim.GetAttribute("xformOp:translate"))

            if "xformOp:orient" not in prop_names:
                xform_op_rot = xformable.AddXformOp(
                    lazy.pxr.UsdGeom.XformOp.TypeOrient, lazy.pxr.UsdGeom.XformOp.PrecisionDouble, ""
                )
            else:
                xform_op_rot = lazy.pxr.UsdGeom.XformOp(self._prim.GetAttribute("xformOp:orient"))
            xformable.SetXformOpOrder([xform_op_translate, xform_op_rot, xform_op_scale])

        # This creates its own editing_usd context.
        XFormPrim.set_position_orientation(self, position=current_position, orientation=current_orientation)
        new_position, new_orientation = self.get_position_orientation()
        r1 = T.quat2mat(current_orientation)
        r2 = T.quat2mat(new_orientation)
        # Make sure setting is done correctly
        assert th.allclose(new_position, current_position, atol=1e-4) and th.allclose(r1, r2, atol=1e-3), (
            f"{self.prim_path}: old_pos: {current_position}, new_pos: {new_position}, "
            f"old_orn: {current_orientation}, new_orn: {new_orientation}"
        )

    @property
    def _collision_filter_api(self):
        return ensure_usd_api(self._prim, lazy.pxr.UsdPhysics.FilteredPairsAPI)

    @property
    def _binding_api(self):
        return ensure_usd_api(self.prim, lazy.pxr.UsdShade.MaterialBindingAPI)

    def has_material(self):
        """
        Returns:
            bool: True if there is a visual material bound to this prim. False otherwise
        """
        material_path = self._binding_api.GetDirectBinding().GetMaterialPath().pathString
        return material_path != "" and lazy.isaacsim.core.utils.prims.is_prim_path_valid(material_path)

    def set_position_orientation(
        self, position=None, orientation=None, frame: Literal["world", "parent", "scene"] = "world"
    ):
        """
        Sets prim's pose with respect to the specified frame

        Args:
            position (None or 3-array): if specified, (x,y,z) position in the world frame
                Default is None, which means left unchanged.
            orientation (None or 4-array): if specified, (x,y,z,w) quaternion orientation in the world frame.
                Default is None, which means left unchanged.
            frame (Literal): frame to set the pose with respect to, defaults to "world". parent frame
                set position relative to the object parent. scene frame set position relative to the scene.
        """
        assert frame in ["world", "parent", "scene"], f"Invalid frame '{frame}'. Must be 'world', 'parent', or 'scene'."

        # If no position or no orientation are given, get the current position and orientation of the object
        if position is None or orientation is None:
            current_position, current_orientation = self.get_position_orientation(frame=frame)
        position = current_position if position is None else position
        orientation = current_orientation if orientation is None else orientation

        # Convert to th.Tensor if necessary
        position = th.as_tensor(position, dtype=th.float32)
        orientation = th.as_tensor(orientation, dtype=th.float32)

        # Convert to from scene-relative to world if necessary
        if frame == "scene":
            assert self.scene is not None, "cannot set position and orientation relative to scene without a scene"
            position, orientation = self.scene.convert_scene_relative_pose_to_world(position, orientation)

        # If the current pose is not in parent frame, convert to parent frame since that's what we can set.
        if frame != "parent":
            world_transform = T.pose2mat((position, orientation))
            parent_path = str(lazy.isaacsim.core.utils.prims.get_prim_parent(self._prim).GetPath())
            parent_world_transform = get_world_pose_with_scale(parent_path)

            local_transform = th.linalg.inv_ex(parent_world_transform).inverse @ world_transform
            local_transform[:3, :3] /= th.linalg.norm(
                local_transform[:3, :3], dim=0
            )  # unscale local transform's rotation

            # Check that the local transform consists only of a position, scale and rotation
            product = local_transform[:3, :3] @ local_transform[:3, :3].T
            assert th.allclose(
                product, th.diag(th.diag(product)), atol=1e-3
            ), f"{self._prim.GetPath()} local transform is not orthogonal."

            # Return the local pose
            position, orientation = T.mat2pose(local_transform)

        # Assert validity of the orientation
        assert math.isclose(
            th.norm(orientation).item(), 1, abs_tol=1e-3
        ), f"{self.prim_path} desired orientation {orientation} is not a unit quaternion."

        # Actually set the local pose now.
        properties = self.prim.GetPropertyNames()
        position = lazy.pxr.Gf.Vec3d(*position.tolist())
        if "xformOp:translate" not in properties:
            logger.error("Translate property needs to be set for {} before setting its position".format(self.name))
        self.set_attribute("xformOp:translate", position)
        orientation = orientation[[3, 0, 1, 2]].tolist()
        if "xformOp:orient" not in properties:
            logger.error("Orient property needs to be set for {} before setting its orientation".format(self.name))
        xform_op = self._prim.GetAttribute("xformOp:orient")
        if xform_op.GetTypeName() == "quatf":
            rotq = lazy.pxr.Gf.Quatf(*orientation)
        else:
            rotq = lazy.pxr.Gf.Quatd(*orientation)
        with og.sim.editing_usd():
            xform_op.Set(rotq)

        og.sim.fabric_hierarchy.update_world_xforms()

    def get_position_orientation(self, frame: Literal["world", "scene", "parent"] = "world", clone=True):
        """
        Gets prim's pose with respect to the specified frame.

        Args:
            frame (Literal): frame to get the pose with respect to. Default to world.
                parent frame: get position relative to the object parent.
                scene frame: get position relative to the scene.
            clone (bool): Whether to clone the internal buffer or not when grabbing data

        Returns:
            2-tuple:
                - th.Tensor: (x,y,z) position in the specified frame
                - th.Tensor: (x,y,z,w) quaternion orientation in the specified frame
        """
        assert frame in ["world", "parent", "scene"], f"Invalid frame '{frame}'. Must be 'world', 'parent', or 'scene'."
        if frame == "world":
            return get_world_pose(self.prim_path)
        elif frame == "scene":
            assert self.scene is not None, "Cannot get position and orientation relative to scene without a scene"
            return self.scene.convert_world_pose_to_scene_relative(*get_world_pose(self.prim_path))
        else:
            return get_local_pose(self.prim_path)

    def set_position(self, position):
        """
        Set this prim's position with respect to the world frame

        Args:
            position (3-array): (x,y,z) global cartesian position to set
        """
        logger.warning(
            "set_position is deprecated and will be removed in a future release. Use set_position_orientation(position=position) instead"
        )
        return self.set_position_orientation(position=position)

    def get_position(self):
        """
        Get this prim's position with respect to the world frame

        Returns:
            3-array: (x,y,z) global cartesian position of this prim
        """
        logger.warning(
            "get_position is deprecated and will be removed in a future release. Use get_position_orientation()[0] instead."
        )
        return self.get_position_orientation()[0]

    def set_orientation(self, orientation):
        """
        Set this prim's orientation with respect to the world frame

        Args:
            orientation (4-array): (x,y,z,w) global quaternion orientation to set
        """
        logger.warning(
            "set_orientation is deprecated and will be removed in a future release. Use set_position_orientation(orientation=orientation) instead"
        )
        self.set_position_orientation(orientation=orientation)

    def get_orientation(self):
        """
        Get this prim's orientation with respect to the world frame

        Returns:
            4-array: (x,y,z,w) global quaternion orientation of this prim
        """
        logger.warning(
            "get_orientation is deprecated and will be removed in a future release. Use get_position_orientation()[1] instead"
        )
        return self.get_position_orientation()[1]

    def get_rpy(self):
        """
        Get this prim's orientation with respect to the world frame

        Returns:
            3-array: (roll, pitch, yaw) global euler orientation of this prim
        """
        return quat2euler(self.get_position_orientation()[1])

    def get_xy_orientation(self):
        """
        Get this prim's orientation on the XY plane of the world frame. This is obtained by
        projecting the forward vector onto the XY plane and then computing the angle.
        """
        return T.calculate_xy_plane_angle(self.get_position_orientation()[1])

    def get_local_pose(self):
        """
        Gets prim's pose with respect to the prim's local frame (its parent frame)

        Returns:
            2-tuple:
                - 3-array: (x,y,z) position in the local frame
                - 4-array: (x,y,z,w) quaternion orientation in the local frame
        """
        logger.warning(
            'get_local_pose is deprecated and will be removed in a future release. Use get_position_orientation(frame="parent") instead'
        )
        return self.get_position_orientation(frame="parent")

    def set_local_pose(self, position=None, orientation=None):
        """
        Sets prim's pose with respect to the local frame (the prim's parent frame).

        Args:
            position (None or 3-array): if specified, (x,y,z) position in the local frame of the prim
                (with respect to its parent prim). Default is None, which means left unchanged.
            orientation (None or 4-array): if specified, (x,y,z,w) quaternion orientation in the local frame of the prim
                (with respect to its parent prim). Default is None, which means left unchanged.
        """
        logger.warning(
            'set_local_pose is deprecated and will be removed in a future release. Use set_position_orientation(position=position, orientation=orientation, frame="parent") instead'
        )
        return self.set_position_orientation(position, orientation, frame="parent")

    @property
    def aabb(self):
        aabb_min, aabb_max = lazy.omni.usd.get_context().compute_path_world_bounding_box(self.prim_path)
        logger.warning(
            "Computing AABB of an XFormPrim using the USD context is slow and unreliable, especially when fabric is enabled. "
            "This is provided as a convenience for USD editing use cases and should generally not be used for physical objects."
        )
        return th.tensor(aabb_min), th.tensor(aabb_max)

    @property
    def aabb_center(self):
        min_corner, max_corner = self.aabb
        return (min_corner + max_corner) / 2

    @property
    def aabb_extent(self):
        min_corner, max_corner = self.aabb
        return max_corner - min_corner

    def get_world_scale(self):
        """
        Gets prim's scale with respect to the world's frame.

        Returns:
            th.tensor: scale applied to the prim's dimensions in the world frame. shape is (3, ).
        """
        prim_tf = lazy.pxr.UsdGeom.Xformable(self._prim).ComputeLocalToWorldTransform(lazy.pxr.Usd.TimeCode.Default())
        transform = lazy.pxr.Gf.Transform()
        transform.SetMatrix(prim_tf)
        return th.tensor(transform.GetScale())

    @property
    def scaled_transform(self):
        """
        Returns the scaled transform of this prim.
        """
        return get_world_pose_with_scale(self.prim_path)

    def transform_local_points_to_world(self, points):
        return T.transform_points(points, self.scaled_transform)

    @property
    def scale(self):
        """
        Gets prim's scale with respect to the local frame (the parent's frame).

        Returns:
            th.tensor: scale applied to the prim's dimensions in the local frame. shape is (3, ).
        """
        if self._cached_scale is not None:
            return self._cached_scale
        scale = self.get_attribute("xformOp:scale")
        assert scale is not None, "Attribute 'xformOp:scale' is None for prim {}".format(self.name)
        return th.tensor(scale)

    @scale.setter
    def scale(self, scale):
        """
        Sets prim's scale with respect to the local frame (the prim's parent frame).

        Args:
            scale (float or th.tensor): scale to be applied to the prim's dimensions. shape is (3, ).
                                          Defaults to None, which means left unchanged.
        """
        if isinstance(scale, th.Tensor):
            scale = scale
        elif isinstance(scale, Iterable):
            scale = th.tensor(scale, dtype=th.float32)
        else:
            scale = th.ones(3, dtype=th.float32) * scale
        assert th.all(scale > 0), f"Scale {scale} must consist of positive numbers."
        # Invalidate the cached scale
        self._cached_scale = None
        scale = lazy.pxr.Gf.Vec3d(*scale.tolist())
        properties = self.prim.GetPropertyNames()
        if "xformOp:scale" not in properties:
            logger.error("Scale property needs to be set for {} before setting its scale".format(self.name))
        self.set_attribute("xformOp:scale", scale)

    @property
    def material(self):
        """
        Returns:
            None or MaterialPrim: The bound material to this prim, if there is one
        """
        return self._material

    @material.setter
    def material(self, material):
        """
        Set the material @material for this prim. This will also bind the material to this prim

        Args:
            material (MaterialPrim): Material to bind to this prim
        """
        binding_api = self._binding_api
        with og.sim.editing_usd():
            binding_api.Bind(
                lazy.pxr.UsdShade.Material(material.prim),
                bindingStrength=lazy.pxr.UsdShade.Tokens.weakerThanDescendants,
            )
        self._material = material

    def add_filtered_collision_pair(self, prim):
        """
        Adds a collision filter pair with another prim

        Args:
            prim (XFormPrim): Another prim to filter collisions with
        """
        self_api = self._collision_filter_api
        other_api = prim._collision_filter_api
        with og.sim.editing_usd():
            # Add to both this prim's and the other prim's filtered pair
            self_api.GetFilteredPairsRel().AddTarget(prim.prim_path)
            other_api.GetFilteredPairsRel().AddTarget(self.prim_path)

    def remove_filtered_collision_pair(self, prim):
        """
        Removes a collision filter pair with another prim

        Args:
            prim (XFormPrim): Another prim to remove filter collisions with
        """
        self_api = self._collision_filter_api
        other_api = prim._collision_filter_api
        with og.sim.editing_usd():
            self_api.GetFilteredPairsRel().RemoveTarget(prim.prim_path)
            other_api.GetFilteredPairsRel().RemoveTarget(self.prim_path)

    def _dump_state(self):
        pos, ori = self.get_position_orientation()
        return dict(pos=pos, ori=ori)

    def _load_state(self, state):
        pos, orn = state["pos"], state["ori"]
        self.set_position_orientation(pos, orn)

    def serialize(self, state):
        return th.cat([state["pos"], state["ori"]])

    def deserialize(self, state):
        # We deserialize deterministically by knowing the order of values -- pos, ori
        return dict(pos=state[0:3], ori=state[3:7]), 7

material property writable

Returns:

Type Description
None or MaterialPrim

The bound material to this prim, if there is one

scale property writable

Gets prim's scale with respect to the local frame (the parent's frame).

Returns:

Type Description
tensor

scale applied to the prim's dimensions in the local frame. shape is (3, ).

scaled_transform property

Returns the scaled transform of this prim.

add_filtered_collision_pair(prim)

Adds a collision filter pair with another prim

Parameters:

Name Type Description Default
prim XFormPrim

Another prim to filter collisions with

required
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def add_filtered_collision_pair(self, prim):
    """
    Adds a collision filter pair with another prim

    Args:
        prim (XFormPrim): Another prim to filter collisions with
    """
    self_api = self._collision_filter_api
    other_api = prim._collision_filter_api
    with og.sim.editing_usd():
        # Add to both this prim's and the other prim's filtered pair
        self_api.GetFilteredPairsRel().AddTarget(prim.prim_path)
        other_api.GetFilteredPairsRel().AddTarget(self.prim_path)

get_local_pose()

Gets prim's pose with respect to the prim's local frame (its parent frame)

Returns:

Type Description
2 - tuple
  • 3-array: (x,y,z) position in the local frame
  • 4-array: (x,y,z,w) quaternion orientation in the local frame
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def get_local_pose(self):
    """
    Gets prim's pose with respect to the prim's local frame (its parent frame)

    Returns:
        2-tuple:
            - 3-array: (x,y,z) position in the local frame
            - 4-array: (x,y,z,w) quaternion orientation in the local frame
    """
    logger.warning(
        'get_local_pose is deprecated and will be removed in a future release. Use get_position_orientation(frame="parent") instead'
    )
    return self.get_position_orientation(frame="parent")

get_orientation()

Get this prim's orientation with respect to the world frame

Returns:

Type Description
4 - array

(x,y,z,w) global quaternion orientation of this prim

Source code in OmniGibson/omnigibson/prims/xform_prim.py
def get_orientation(self):
    """
    Get this prim's orientation with respect to the world frame

    Returns:
        4-array: (x,y,z,w) global quaternion orientation of this prim
    """
    logger.warning(
        "get_orientation is deprecated and will be removed in a future release. Use get_position_orientation()[1] instead"
    )
    return self.get_position_orientation()[1]

get_position()

Get this prim's position with respect to the world frame

Returns:

Type Description
3 - array

(x,y,z) global cartesian position of this prim

Source code in OmniGibson/omnigibson/prims/xform_prim.py
def get_position(self):
    """
    Get this prim's position with respect to the world frame

    Returns:
        3-array: (x,y,z) global cartesian position of this prim
    """
    logger.warning(
        "get_position is deprecated and will be removed in a future release. Use get_position_orientation()[0] instead."
    )
    return self.get_position_orientation()[0]

get_position_orientation(frame='world', clone=True)

Gets prim's pose with respect to the specified frame.

Parameters:

Name Type Description Default
frame Literal

frame to get the pose with respect to. Default to world. parent frame: get position relative to the object parent. scene frame: get position relative to the scene.

'world'
clone bool

Whether to clone the internal buffer or not when grabbing data

True

Returns:

Type Description
2 - tuple
  • th.Tensor: (x,y,z) position in the specified frame
  • th.Tensor: (x,y,z,w) quaternion orientation in the specified frame
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def get_position_orientation(self, frame: Literal["world", "scene", "parent"] = "world", clone=True):
    """
    Gets prim's pose with respect to the specified frame.

    Args:
        frame (Literal): frame to get the pose with respect to. Default to world.
            parent frame: get position relative to the object parent.
            scene frame: get position relative to the scene.
        clone (bool): Whether to clone the internal buffer or not when grabbing data

    Returns:
        2-tuple:
            - th.Tensor: (x,y,z) position in the specified frame
            - th.Tensor: (x,y,z,w) quaternion orientation in the specified frame
    """
    assert frame in ["world", "parent", "scene"], f"Invalid frame '{frame}'. Must be 'world', 'parent', or 'scene'."
    if frame == "world":
        return get_world_pose(self.prim_path)
    elif frame == "scene":
        assert self.scene is not None, "Cannot get position and orientation relative to scene without a scene"
        return self.scene.convert_world_pose_to_scene_relative(*get_world_pose(self.prim_path))
    else:
        return get_local_pose(self.prim_path)

get_rpy()

Get this prim's orientation with respect to the world frame

Returns:

Type Description
3 - array

(roll, pitch, yaw) global euler orientation of this prim

Source code in OmniGibson/omnigibson/prims/xform_prim.py
def get_rpy(self):
    """
    Get this prim's orientation with respect to the world frame

    Returns:
        3-array: (roll, pitch, yaw) global euler orientation of this prim
    """
    return quat2euler(self.get_position_orientation()[1])

get_world_scale()

Gets prim's scale with respect to the world's frame.

Returns:

Type Description
tensor

scale applied to the prim's dimensions in the world frame. shape is (3, ).

Source code in OmniGibson/omnigibson/prims/xform_prim.py
def get_world_scale(self):
    """
    Gets prim's scale with respect to the world's frame.

    Returns:
        th.tensor: scale applied to the prim's dimensions in the world frame. shape is (3, ).
    """
    prim_tf = lazy.pxr.UsdGeom.Xformable(self._prim).ComputeLocalToWorldTransform(lazy.pxr.Usd.TimeCode.Default())
    transform = lazy.pxr.Gf.Transform()
    transform.SetMatrix(prim_tf)
    return th.tensor(transform.GetScale())

get_xy_orientation()

Get this prim's orientation on the XY plane of the world frame. This is obtained by projecting the forward vector onto the XY plane and then computing the angle.

Source code in OmniGibson/omnigibson/prims/xform_prim.py
def get_xy_orientation(self):
    """
    Get this prim's orientation on the XY plane of the world frame. This is obtained by
    projecting the forward vector onto the XY plane and then computing the angle.
    """
    return T.calculate_xy_plane_angle(self.get_position_orientation()[1])

has_material()

Returns:

Type Description
bool

True if there is a visual material bound to this prim. False otherwise

Source code in OmniGibson/omnigibson/prims/xform_prim.py
def has_material(self):
    """
    Returns:
        bool: True if there is a visual material bound to this prim. False otherwise
    """
    material_path = self._binding_api.GetDirectBinding().GetMaterialPath().pathString
    return material_path != "" and lazy.isaacsim.core.utils.prims.is_prim_path_valid(material_path)

remove_filtered_collision_pair(prim)

Removes a collision filter pair with another prim

Parameters:

Name Type Description Default
prim XFormPrim

Another prim to remove filter collisions with

required
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def remove_filtered_collision_pair(self, prim):
    """
    Removes a collision filter pair with another prim

    Args:
        prim (XFormPrim): Another prim to remove filter collisions with
    """
    self_api = self._collision_filter_api
    other_api = prim._collision_filter_api
    with og.sim.editing_usd():
        self_api.GetFilteredPairsRel().RemoveTarget(prim.prim_path)
        other_api.GetFilteredPairsRel().RemoveTarget(self.prim_path)

set_local_pose(position=None, orientation=None)

Sets prim's pose with respect to the local frame (the prim's parent frame).

Parameters:

Name Type Description Default
position None or 3 - array

if specified, (x,y,z) position in the local frame of the prim (with respect to its parent prim). Default is None, which means left unchanged.

None
orientation None or 4 - array

if specified, (x,y,z,w) quaternion orientation in the local frame of the prim (with respect to its parent prim). Default is None, which means left unchanged.

None
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def set_local_pose(self, position=None, orientation=None):
    """
    Sets prim's pose with respect to the local frame (the prim's parent frame).

    Args:
        position (None or 3-array): if specified, (x,y,z) position in the local frame of the prim
            (with respect to its parent prim). Default is None, which means left unchanged.
        orientation (None or 4-array): if specified, (x,y,z,w) quaternion orientation in the local frame of the prim
            (with respect to its parent prim). Default is None, which means left unchanged.
    """
    logger.warning(
        'set_local_pose is deprecated and will be removed in a future release. Use set_position_orientation(position=position, orientation=orientation, frame="parent") instead'
    )
    return self.set_position_orientation(position, orientation, frame="parent")

set_orientation(orientation)

Set this prim's orientation with respect to the world frame

Parameters:

Name Type Description Default
orientation 4 - array

(x,y,z,w) global quaternion orientation to set

required
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def set_orientation(self, orientation):
    """
    Set this prim's orientation with respect to the world frame

    Args:
        orientation (4-array): (x,y,z,w) global quaternion orientation to set
    """
    logger.warning(
        "set_orientation is deprecated and will be removed in a future release. Use set_position_orientation(orientation=orientation) instead"
    )
    self.set_position_orientation(orientation=orientation)

set_position(position)

Set this prim's position with respect to the world frame

Parameters:

Name Type Description Default
position 3 - array

(x,y,z) global cartesian position to set

required
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def set_position(self, position):
    """
    Set this prim's position with respect to the world frame

    Args:
        position (3-array): (x,y,z) global cartesian position to set
    """
    logger.warning(
        "set_position is deprecated and will be removed in a future release. Use set_position_orientation(position=position) instead"
    )
    return self.set_position_orientation(position=position)

set_position_orientation(position=None, orientation=None, frame='world')

Sets prim's pose with respect to the specified frame

Parameters:

Name Type Description Default
position None or 3 - array

if specified, (x,y,z) position in the world frame Default is None, which means left unchanged.

None
orientation None or 4 - array

if specified, (x,y,z,w) quaternion orientation in the world frame. Default is None, which means left unchanged.

None
frame Literal

frame to set the pose with respect to, defaults to "world". parent frame set position relative to the object parent. scene frame set position relative to the scene.

'world'
Source code in OmniGibson/omnigibson/prims/xform_prim.py
def set_position_orientation(
    self, position=None, orientation=None, frame: Literal["world", "parent", "scene"] = "world"
):
    """
    Sets prim's pose with respect to the specified frame

    Args:
        position (None or 3-array): if specified, (x,y,z) position in the world frame
            Default is None, which means left unchanged.
        orientation (None or 4-array): if specified, (x,y,z,w) quaternion orientation in the world frame.
            Default is None, which means left unchanged.
        frame (Literal): frame to set the pose with respect to, defaults to "world". parent frame
            set position relative to the object parent. scene frame set position relative to the scene.
    """
    assert frame in ["world", "parent", "scene"], f"Invalid frame '{frame}'. Must be 'world', 'parent', or 'scene'."

    # If no position or no orientation are given, get the current position and orientation of the object
    if position is None or orientation is None:
        current_position, current_orientation = self.get_position_orientation(frame=frame)
    position = current_position if position is None else position
    orientation = current_orientation if orientation is None else orientation

    # Convert to th.Tensor if necessary
    position = th.as_tensor(position, dtype=th.float32)
    orientation = th.as_tensor(orientation, dtype=th.float32)

    # Convert to from scene-relative to world if necessary
    if frame == "scene":
        assert self.scene is not None, "cannot set position and orientation relative to scene without a scene"
        position, orientation = self.scene.convert_scene_relative_pose_to_world(position, orientation)

    # If the current pose is not in parent frame, convert to parent frame since that's what we can set.
    if frame != "parent":
        world_transform = T.pose2mat((position, orientation))
        parent_path = str(lazy.isaacsim.core.utils.prims.get_prim_parent(self._prim).GetPath())
        parent_world_transform = get_world_pose_with_scale(parent_path)

        local_transform = th.linalg.inv_ex(parent_world_transform).inverse @ world_transform
        local_transform[:3, :3] /= th.linalg.norm(
            local_transform[:3, :3], dim=0
        )  # unscale local transform's rotation

        # Check that the local transform consists only of a position, scale and rotation
        product = local_transform[:3, :3] @ local_transform[:3, :3].T
        assert th.allclose(
            product, th.diag(th.diag(product)), atol=1e-3
        ), f"{self._prim.GetPath()} local transform is not orthogonal."

        # Return the local pose
        position, orientation = T.mat2pose(local_transform)

    # Assert validity of the orientation
    assert math.isclose(
        th.norm(orientation).item(), 1, abs_tol=1e-3
    ), f"{self.prim_path} desired orientation {orientation} is not a unit quaternion."

    # Actually set the local pose now.
    properties = self.prim.GetPropertyNames()
    position = lazy.pxr.Gf.Vec3d(*position.tolist())
    if "xformOp:translate" not in properties:
        logger.error("Translate property needs to be set for {} before setting its position".format(self.name))
    self.set_attribute("xformOp:translate", position)
    orientation = orientation[[3, 0, 1, 2]].tolist()
    if "xformOp:orient" not in properties:
        logger.error("Orient property needs to be set for {} before setting its orientation".format(self.name))
    xform_op = self._prim.GetAttribute("xformOp:orient")
    if xform_op.GetTypeName() == "quatf":
        rotq = lazy.pxr.Gf.Quatf(*orientation)
    else:
        rotq = lazy.pxr.Gf.Quatd(*orientation)
    with og.sim.editing_usd():
        xform_op.Set(rotq)

    og.sim.fabric_hierarchy.update_world_xforms()