diff --git a/omnigibson/prims/xform_prim.py b/omnigibson/prims/xform_prim.py index 516b2af89..76721777f 100644 --- a/omnigibson/prims/xform_prim.py +++ b/omnigibson/prims/xform_prim.py @@ -187,6 +187,10 @@ def set_position_orientation(self, position=None, orientation=None): parent_world_transform = PoseAPI.get_world_pose_with_scale(parent_path) local_transform = np.linalg.inv(parent_world_transform) @ my_world_transform + product = local_transform[:3, :3] @ local_transform[:3, :3].T + assert np.allclose( + product, np.diag(np.diag(product)), atol=1e-3 + ), f"{self.prim_path} local transform is not diagonal." self.set_local_pose(*T.mat2pose(local_transform)) def get_position_orientation(self): @@ -359,6 +363,7 @@ def scale(self, scale): Defaults to None, which means left unchanged. """ scale = np.array(scale, dtype=float) if isinstance(scale, Iterable) else np.ones(3) * scale + assert np.all(scale > 0), f"Scale {scale} must consist of positive numbers." scale = lazy.pxr.Gf.Vec3d(*scale) properties = self.prim.GetPropertyNames() if "xformOp:scale" not in properties: