diff --git a/unity/Assets/Scripts/AgentManager.cs b/unity/Assets/Scripts/AgentManager.cs index 9e0efc8904..1b869c9fad 100644 --- a/unity/Assets/Scripts/AgentManager.cs +++ b/unity/Assets/Scripts/AgentManager.cs @@ -624,10 +624,10 @@ private void updateCameraProperties( camera.transform.localScale = Vector3.one; } else { if (position.HasValue) { - camera.gameObject.transform.position = position.Value; + camera.transform.position = position.Value; } if (rotation.HasValue) { - camera.gameObject.transform.eulerAngles = rotation.Value; + camera.transform.eulerAngles = rotation.Value; } } @@ -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()) { - 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.parentRelativeThirdPartyCameraPosition = + camera.transform.localPosition; + + //get third party camera rotation as quaternion in parent space + cMetadata.parentRelativeThirdPartyCameraRotation = + camera.transform.localEulerAngles; + } else { + //if not parented, default position and rotation to world coordinate space + cMetadata.parentObjectName = ""; + cMetadata.parentRelativeThirdPartyCameraPosition = camera.transform.position; + cMetadata.parentRelativeThirdPartyCameraRotation = camera.transform.rotation.eulerAngles; + } + + //if this camera is part of the agent's hierarchy at all, get agent relative info + if (camera.GetComponentInParent() != null) { + GameObject agent = camera.GetComponentInParent().gameObject; + + cMetadata.agentRelativeThirdPartyCameraPosition = + agent.transform.InverseTransformPoint(camera.transform.position); + + var agentSpaceCameraRotationAsQuaternion = + Quaternion.Inverse(agent.transform.rotation) + * worldSpaceCameraRotationAsQuaternion; + cMetadata.agentRelativeThirdPartyCameraRotation = + agentSpaceCameraRotationAsQuaternion.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; + //if this third party camera is not a child of the agent, we don't need agent-relative coordinates + //Note: We don't default this to world space because in the case of a multi-agent scenario, the agent + //to be relative to is ambiguous and UHHHHH + cMetadata.agentRelativeThirdPartyCameraPosition = null; + cMetadata.agentRelativeThirdPartyCameraRotation = 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? agentRelativeThirdPartyCameraPosition; + public Vector3? agentRelativeThirdPartyCameraRotation; + + //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 parentRelativeThirdPartyCameraPosition; + public Vector3 parentRelativeThirdPartyCameraRotation; + 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; diff --git a/unity/Assets/Scripts/BaseFPSAgentController.cs b/unity/Assets/Scripts/BaseFPSAgentController.cs index a2ef43c4fc..1ad6e89011 100644 --- a/unity/Assets/Scripts/BaseFPSAgentController.cs +++ b/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; diff --git a/unity/Assets/Scripts/StretchAgentController.cs b/unity/Assets/Scripts/StretchAgentController.cs index ba7967f92f..0c9f8e4fe8 100644 --- a/unity/Assets/Scripts/StretchAgentController.cs +++ b/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 = diff --git a/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs b/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs index 38a70f6897..cd20598ace 100644 --- a/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs +++ b/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs @@ -312,5 +312,349 @@ public IEnumerator TestUpdateMainCamera() result = Mathf.Approximately(camera.transform.localEulerAngles.z, 0.0f); Assert.AreEqual(result, true); } + + //test main camera metadata + [UnityTest] + public IEnumerator TestMainCameraMetadataReturn() + { + bool result = false; + Dictionary action = new Dictionary(); + + action["action"] = "Initialize"; + action["fieldOfView"] = 90f; + action["snapToGrid"] = true; + yield return step(action); + + action.Clear(); + + MetadataWrapper metadata = getLastActionMetadata(); + + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.x, -1.0000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.y, 1.5759990000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.z, 1.0000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraRotation.x, 0.0000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraRotation.y, 270.0000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraRotation.z, 0.0000000000f); + Assert.AreEqual(result, true); + + result = Mathf.Approximately( + metadata.agentPositionRelativeCameraPosition.x, + 0.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentPositionRelativeCameraPosition.y, + 0.6750000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentPositionRelativeCameraPosition.z, + 0.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentRotationRelativeCameraRotation.x, + 0.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentRotationRelativeCameraRotation.y, + 0.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentRotationRelativeCameraRotation.z, + 0.0000000000f + ); + Assert.AreEqual(result, true); + + action["action"] = "UpdateMainCamera"; + action["position"] = new Vector3(0.5f, 0.5f, 0.5f); + action["rotation"] = new Vector3(30f, 10f, 12f); + yield return step(action); + + metadata = getLastActionMetadata(); + + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.x, -1.5000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.y, 1.4009990000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.z, 1.5000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraRotation.x, 30.0000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraRotation.y, 280.0000000000f); + Assert.AreEqual(result, true); + result = Mathf.Approximately(metadata.worldRelativeCameraRotation.z, 12.0000000000f); + Assert.AreEqual(result, true); + + result = Mathf.Approximately( + metadata.agentPositionRelativeCameraPosition.x, + 0.5000001000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentPositionRelativeCameraPosition.y, + 0.4999999000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentPositionRelativeCameraPosition.z, + 0.5000002000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentRotationRelativeCameraRotation.x, + 30.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentRotationRelativeCameraRotation.y, + 10.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.agentRotationRelativeCameraRotation.z, + 12.0000000000f + ); + Assert.AreEqual(result, true); + } + + //test third party camera metadata + [UnityTest] + public IEnumerator TestThirdPartyCameraMetadataReturn() + { + bool result = false; + + Dictionary action = new Dictionary(); + + action["action"] = "Initialize"; + action["fieldOfView"] = 90f; + action["snapToGrid"] = true; + yield return step(action); + + action.Clear(); + + action["action"] = "AddThirdPartyCamera"; + action["position"] = new Vector3(3, 2, 1); + action["rotation"] = new Vector3(10, 20, 30); + action["parent"] = "world"; + yield return step(action); + + MetadataWrapper metadata = getLastActionMetadata(); + + // World Relative Position + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraPosition.x, + 3.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraPosition.y, + 2.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraPosition.z, + 1.0000000000f + ); + Assert.AreEqual(result, true); + // World Relative Rotation + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraRotation.x, + 10.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraRotation.y, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraRotation.z, + 30.0000000000f + ); + Assert.AreEqual(result, true); + //Agent Relative Position + Assert.AreEqual( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraPosition, + null + ); + Assert.AreEqual( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraRotation, + null + ); + //Parent Relative Position + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraPosition.x, + 3.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraPosition.y, + 2.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraPosition.z, + 1.0000000000f + ); + Assert.AreEqual(result, true); + //Parent Relative Rotation + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation.x, + 10.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation.y, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation.z, + 30.0000000000f + ); + Assert.AreEqual(result, true); + Assert.AreEqual(metadata.thirdPartyCameras[0].parentObjectName, ""); + action.Clear(); + + //use update third party camera to change camera to be attached to agent + action["action"] = "UpdateThirdPartyCamera"; + action["thirdPartyCameraId"] = 0; + action["position"] = new Vector3(1, 2, 3); + action["rotation"] = new Vector3(20, 20, 20); + action["parent"] = "agent"; + action["agentPositionRelativeCoordinates"] = true; + yield return step(action); + + metadata = getLastActionMetadata(); + + // //world relative + // Debug.Log($"world relative camera pos: {metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraPosition:F10}"); + // Debug.Log($"world relative camera rot: {metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraRotation:F10}"); + // //agent relative + // Debug.Log($"agent relative camera pos: {metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraPosition:F10}"); + // Debug.Log($"agent relative camera rot: {metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraRotation:F10}"); + // //parent relative + // Debug.Log($"parent relative camera rot: {metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraPosition:F10}"); + // Debug.Log($"parent relative camera rot: {metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation:F10}"); + // Debug.Log($"parent object name: {metadata.thirdPartyCameras[0].parentObjectName}"); + + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraPosition.x, + -4.0000010000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraPosition.y, + 2.9009990000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraPosition.z, + 2.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraRotation.x, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraRotation.y, + 290.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].worldRelativeThirdPartyCameraRotation.z, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraPosition.Value.x, + 1.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraPosition.Value.y, + 2.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraPosition.Value.z, + 3.0000020000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraRotation.Value.x, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraRotation.Value.y, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata.thirdPartyCameras[0].agentRelativeThirdPartyCameraRotation.Value.z, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata + .thirdPartyCameras[0] + .parentRelativeThirdPartyCameraPosition + .x, + 1.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata + .thirdPartyCameras[0] + .parentRelativeThirdPartyCameraPosition + .y, + 2.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata + .thirdPartyCameras[0] + .parentRelativeThirdPartyCameraPosition + .z, + 3.0000020000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata + .thirdPartyCameras[0] + .parentRelativeThirdPartyCameraRotation + .x, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata + .thirdPartyCameras[0] + .parentRelativeThirdPartyCameraRotation + .y, + 20.0000000000f + ); + Assert.AreEqual(result, true); + result = Mathf.Approximately( + metadata + .thirdPartyCameras[0] + .parentRelativeThirdPartyCameraRotation + .z, + 20.0000000000f + ); + Assert.AreEqual(result, true); + Assert.AreEqual(metadata.thirdPartyCameras[0].parentObjectName, "FPSController"); + } } }