Updating Agent Metadata and ThirdPartyCamera Metadata

unity/Assets/Scripts/AgentManager.cs

@@ -1295,31 +1295,52 @@ bool shouldRenderImageSynthesis
                 Camera camera = thirdPartyCameras.ToArray()[i];
                 cMetadata.thirdPartyCameraId = i;
 
-                cMetadata.position = camera.gameObject.transform.position;
-                cMetadata.rotation = camera.gameObject.transform.eulerAngles;
-
-                //agent relative third party camera metadata here
-                //if the camera is a child of the base agent, then return local space values
-                if (camera.GetComponentInParent<BaseAgentComponent>()) {
-                    cMetadata.agentPositionRelativeThirdPartyCameraPosition = camera
-                        .gameObject
-                        .transform
-                        .localPosition;
-                    cMetadata.agentPositionRelativeThirdPartyCameraRotation = camera
-                        .gameObject
-                        .transform
-                        .localEulerAngles;
+                //to be deprecated at some point, will be replaced by more descriptive worldRelativeThirdPartyCamera....
+                cMetadata.position = camera.transform.position;
+                cMetadata.rotation = camera.transform.eulerAngles;
+                //currently redundant data as it is the same as metadata.position/rotation, but more descriptive naming convention
+                cMetadata.worldRelativeThirdPartyCameraPosition = camera.transform.position;
+                cMetadata.worldRelativeThirdPartyCameraRotation = camera.transform.eulerAngles;
+
+                //grab this for parent or agent relative stuff
+                var worldSpaceCameraRotationAsQuaternion = camera.transform.rotation;
+
+                //this may be the same info as the agentPositionRelative values since ThirdPartyCameras often are children of the agent
+                //but in cases where a third party camera is attached to an arm joint or the like, this can be useful
+                if (camera.transform.parent != null) {
+                    cMetadata.parentObjectName = camera.transform.parent.name;
+
+                    cMetadata.parentPositionRelativeThirdPartyCameraPosition =
+                        camera.transform.parent.InverseTransformPoint(camera.transform.position);
+
+                    //get third party camera rotation as quaternion in parent space
+                    var parentSpaceCameraRotationAsQuaternion =
+                        Quaternion.Inverse(camera.transform.parent.rotation)
+                        * worldSpaceCameraRotationAsQuaternion;
+                    cMetadata.parentPositionRelativeThirdPartyCameraRotation =
+                        parentSpaceCameraRotationAsQuaternion.eulerAngles;
                 } else {
-                    //if this third party camera is not a child of the agent, then the agent relative coordinates
-                    //are the same as the world coordinates so
-                    cMetadata.agentPositionRelativeThirdPartyCameraPosition = camera
-                        .gameObject
-                        .transform
-                        .position;
-                    cMetadata.agentPositionRelativeThirdPartyCameraRotation = camera
-                        .gameObject
-                        .transform
-                        .eulerAngles;
+                    cMetadata.parentObjectName = "";
+                    cMetadata.parentPositionRelativeThirdPartyCameraPosition = null;
+                    cMetadata.parentPositionRelativeThirdPartyCameraRotation = null;
+                }
+
+                //if this camera is part of the agent's hierarchy at all, get agent relative info
+                if (camera.GetComponentInParent<BaseAgentComponent>() != null) {
+                    GameObject agent = camera.GetComponentInParent<BaseAgentComponent>().gameObject;
+
+                    cMetadata.agentPositionRelativeThirdPartyCameraPosition =
+                        agent.transform.InverseTransformPoint(camera.gameObject.transform.position);
+
+                    var agentSpaceCameraRotationAsQuaternion =
+                        Quaternion.Inverse(agent.transform.rotation)
+                        * worldSpaceCameraRotationAsQuaternion;
+                    cMetadata.agentRotationRelativeThirdPartyCameraRotation =
+                        agentSpaceCameraRotationAsQuaternion.eulerAngles;
+                } else {
+                    //if this third party camera is not a child of the agent, we don't need agent-relative coordinates
+                    cMetadata.agentPositionRelativeThirdPartyCameraPosition = null;
+                    cMetadata.agentRotationRelativeThirdPartyCameraRotation = null;
                 }
 
                 cMetadata.fieldOfView = camera.fieldOfView;
@@ -1846,14 +1867,21 @@ public class MultiAgentMetadata {
 [MessagePackObject(keyAsPropertyName: true)]
 public class ThirdPartyCameraMetadata {
     public int thirdPartyCameraId;
-    public Vector3 position;
-    public Vector3 rotation;
+    public Vector3 position; //to be deprecated
+    public Vector3 rotation; //to be deprecated
+    public Vector3 worldRelativeThirdPartyCameraPosition;
+    public Vector3 worldRelativeThirdPartyCameraRotation;
     public float fieldOfView;
 
     //note these should only be returned with values
     //if the third party camera is a child of the agent
-    public Vector3 agentPositionRelativeThirdPartyCameraPosition;
-    public Vector3 agentPositionRelativeThirdPartyCameraRotation;
+    public Vector3? agentPositionRelativeThirdPartyCameraPosition;
+    public Vector3? agentRotationRelativeThirdPartyCameraRotation;
+
+    //return the local space coordinates if this third party camera has a parent object, this may be the same as agentPositionRelative depending on how things are parented
+    public Vector3? parentPositionRelativeThirdPartyCameraPosition;
+    public Vector3? parentPositionRelativeThirdPartyCameraRotation;
+    public string parentObjectName; //if this third party camera is in a hierarchy, return the name of the parent object
 }
 
 [Serializable]
@@ -2280,10 +2308,12 @@ public struct MetadataWrapper {
     public ArmMetadata arm;
     public ArticulationArmMetadata articulationArm;
     public float fov;
-    public Vector3 cameraPosition;
-    public Vector3 cameraRotation;
+    public Vector3 cameraPosition; //to be deprecated
+    public Vector3 cameraRotation; //to be deprecated
+    public Vector3 worldRelativeCameraPosition;
+    public Vector3 worldRelativeCameraRotation;
     public Vector3 agentPositionRelativeCameraPosition;
-    public Vector3 agentPositionRelativeCameraRotation;
+    public Vector3 agentRotationRelativeCameraRotation;
     public float cameraOrthSize;
     public ThirdPartyCameraMetadata[] thirdPartyCameras;
     public bool collided;

unity/Assets/Scripts/BaseFPSAgentController.cs

@@ -2627,8 +2627,10 @@ public virtual MetadataWrapper generateMetadataWrapper() {
             metaMessage.screenWidth = Screen.width;
             metaMessage.screenHeight = Screen.height;
 
-            metaMessage.cameraPosition = m_Camera.transform.position;
-            metaMessage.cameraRotation = m_Camera.transform.eulerAngles;
+            metaMessage.cameraPosition = m_Camera.transform.position; //to be deprecated
+            metaMessage.cameraRotation = m_Camera.transform.eulerAngles; //to be deprecated
+            metaMessage.worldRelativeCameraPosition = m_Camera.transform.position;
+            metaMessage.worldRelativeCameraRotation = m_Camera.transform.eulerAngles;
 
             //we need to transform these relative to the agent position
             //main camera's local space coordinates need to be translated to world space first
@@ -2643,9 +2645,9 @@ public virtual MetadataWrapper generateMetadataWrapper() {
             var worldSpaceCameraRotationAsQuaternion = m_Camera.transform.rotation;
             var localSpaceCameraRotationAsQuaternion =
                 Quaternion.Inverse(transform.rotation) * worldSpaceCameraRotationAsQuaternion;
-            metaMessage.agentPositionRelativeCameraRotation =
+            metaMessage.agentRotationRelativeCameraRotation =
                 localSpaceCameraRotationAsQuaternion.eulerAngles;
-            //Debug.Log($"agentRelativeCameraRotation: {metaMessage.agentPositionRelativeCameraRotation}");
+            //Debug.Log($"agentRelativeCameraRotation: {metaMessage.agentRotationRelativeCameraRotation}");
 
             metaMessage.cameraOrthSize = cameraOrthSize;
             cameraOrthSize = -1f;

unity/Assets/Scripts/StretchAgentController.cs

@@ -739,9 +739,7 @@ public void PlaceObjectIntoGripper(string objectId, bool grasp = true) {
 
                 Physics.SyncTransforms();
 
-                Collider c = UtilityFunctions.firstColliderObjectCollidingWith(
-                    go: sop.gameObject
-                );
+                Collider c = UtilityFunctions.firstColliderObjectCollidingWith(go: sop.gameObject);
                 if (c != null) {
                     SimObjPhysics collisionSop = ancestorSimObjPhysics(c.gameObject);
                     errorMessage =