Merge pull request #636 from StanfordVL/sanity-check-transforms

Require positive scales and validate local transforms

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: