diff --git a/ai2thor/tests/data/arm-metadata-schema.json b/ai2thor/tests/data/arm-metadata-schema.json index 7c4bb2a144..b590fc7718 100644 --- a/ai2thor/tests/data/arm-metadata-schema.json +++ b/ai2thor/tests/data/arm-metadata-schema.json @@ -462,7 +462,7 @@ "type": "number" }, "isStanding": { - "type": "boolean" + "type": ["boolean", "null"] }, "inHighFrictionArea": { "type": "boolean" diff --git a/ai2thor/tests/test_unity.py b/ai2thor/tests/test_unity.py index 80437433e5..0b086c8554 100644 --- a/ai2thor/tests/test_unity.py +++ b/ai2thor/tests/test_unity.py @@ -1418,16 +1418,17 @@ def test_teleport_stretch(controller): agent = "stretch" event = controller.reset(agentMode=agent) - assert event.metadata["agent"]["isStanding"] is False, agent + " cannot stand!" + assert event.metadata["agent"]["isStanding"] is None, ( + agent + " cannot stand so this should be None/null!" + ) - # Only degrees of freedom on the locobot for action in ["Teleport", "TeleportFull"]: event = controller.step( action=action, position=dict(x=-1.5, y=0.9, z=-1.5), rotation=dict(x=0, y=90, z=0), horizon=30, - standing=True, + standing=None, ) print(f"Error Message: {event.metadata['errorMessage']}") diff --git a/unity/Assets/Scripts/AgentManager.cs b/unity/Assets/Scripts/AgentManager.cs index 41aa2259b4..3d8721be27 100644 --- a/unity/Assets/Scripts/AgentManager.cs +++ b/unity/Assets/Scripts/AgentManager.cs @@ -321,7 +321,7 @@ public void Initialize(ServerAction action) { : action.dynamicServerAction.agentInitializationParams ); Debug.Log( - $"Initialize of AgentController. lastActionSuccess: {primaryAgent.lastActionSuccess}, errorMessage: {primaryAgent.errorMessage}, actionReturn: {primaryAgent.actionReturn}, agentState: {primaryAgent.agentState}" + $"Initialize of AgentController.lastAction: {primaryAgent.lastAction} lastActionSuccess: {primaryAgent.lastActionSuccess}, errorMessage: {primaryAgent.errorMessage}, actionReturn: {primaryAgent.actionReturn}, agentState: {primaryAgent.agentState}" ); Time.fixedDeltaTime = action.fixedDeltaTime.GetValueOrDefault(Time.fixedDeltaTime); if (action.targetFrameRate > 0) { @@ -1333,17 +1333,22 @@ bool shouldRenderImageSynthesis if (camera.transform.parent != null) { cMetadata.parentObjectName = camera.transform.parent.name; - cMetadata.parentRelativeThirdPartyCameraPosition = - camera.transform.localPosition; + cMetadata.parentRelativeThirdPartyCameraPosition = camera + .transform + .localPosition; //get third party camera rotation as quaternion in parent space - cMetadata.parentRelativeThirdPartyCameraRotation = - camera.transform.localEulerAngles; + 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; + cMetadata.parentRelativeThirdPartyCameraRotation = camera + .transform + .rotation + .eulerAngles; } //if this camera is part of the agent's hierarchy at all, get agent relative info @@ -1360,7 +1365,7 @@ bool shouldRenderImageSynthesis agentSpaceCameraRotationAsQuaternion.eulerAngles; } else { //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 + //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; diff --git a/unity/Assets/Scripts/ArmAgentController.cs b/unity/Assets/Scripts/ArmAgentController.cs index 5e3aea7317..de21938e37 100644 --- a/unity/Assets/Scripts/ArmAgentController.cs +++ b/unity/Assets/Scripts/ArmAgentController.cs @@ -266,6 +266,80 @@ public virtual IEnumerator RotateAgent( ); } + public override void Teleport( + Vector3? position = null, + Vector3? rotation = null, + float? horizon = null, + bool? standing = null, + bool forceAction = false + ) { + //non-high level agents cannot set standing + if (standing != null) { + errorMessage = "Cannot set standing for arm/stretch agent"; + actionFinishedEmit(success: false, actionReturn: null, errorMessage: errorMessage); + return; + } + + TeleportFull( + position: position, + rotation: rotation, + horizon: horizon, + standing: standing, + forceAction: forceAction + ); + } + + public override void TeleportFull( + Vector3? position = null, + Vector3? rotation = null, + float? horizon = null, + bool? standing = null, + bool forceAction = false + ) { + //non-high level agents cannot set standing + if (standing != null) { + errorMessage = "Cannot set standing for arm/stretch agent"; + actionFinishedEmit(success: false, actionReturn: null, errorMessage: errorMessage); + return; + } + + //cache old values in case there is a failure + Vector3 oldPosition = transform.position; + Quaternion oldRotation = transform.rotation; + Quaternion oldCameraRotation = m_Camera.transform.localRotation; + + try { + base.teleportFull( + position: position, + rotation: rotation, + horizon: horizon, + forceAction: forceAction + ); + + // add arm value cases + if (!forceAction) { + if (Arm != null && Arm.IsArmColliding()) { + throw new InvalidOperationException( + "Mid Level Arm is actively clipping with some geometry in the environment. TeleportFull fails in this position." + ); + } else if (SArm != null && SArm.IsArmColliding()) { + throw new InvalidOperationException( + "Stretch Arm is actively clipping with some geometry in the environment. TeleportFull fails in this position." + ); + } + base.assertTeleportedNearGround(targetPosition: position); + } + } catch (InvalidOperationException e) { + transform.position = oldPosition; + transform.rotation = oldRotation; + m_Camera.transform.localRotation = oldCameraRotation; + + throw new InvalidOperationException(e.Message); + } + + actionFinished(success: true); + } + /* Rotates the wrist (in a relative fashion) given some input pitch, yaw, and roll offsets. Easiest to see how this works by diff --git a/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs b/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs index 354b1045a5..6d4deb2c37 100644 --- a/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs +++ b/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs @@ -264,10 +264,11 @@ void Update() { ) && PhysicsController.ReadyForCommand ) { ServerAction action = new ServerAction(); - if (this.PhysicsController.isStanding()) { + bool? wasStanding = this.PhysicsController.isStanding(); + if (wasStanding == true) { action.action = "Crouch"; PhysicsController.ProcessControlCommand(action); - } else { + } else if (wasStanding == false) { action.action = "Stand"; PhysicsController.ProcessControlCommand(action); } diff --git a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs index b37962eecf..2b0b75845b 100644 --- a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs +++ b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs @@ -173,9 +173,31 @@ public void SetMassProperties(string objectId, float mass, float drag, float ang return; } - public bool isStanding() { - return (m_Camera.transform.localPosition - standingLocalCameraPosition).magnitude - < 0.1f; + public bool? isStanding() { + //default to not standing if this isn't literally the PhysicsRemoteFPSAgentController + //this means the metadat for isStanding should always be false for all derived classes + if (this.GetType() != typeof(PhysicsRemoteFPSAgentController)) { + return null; + } + + //if camera is in neither the standing or crouching predetermined locations, return null + if ( + UtilityFunctions.ArePositionsApproximatelyEqual( + m_Camera.transform.localPosition, + standingLocalCameraPosition + ) == true + ) { + return true; + } else if ( + UtilityFunctions.ArePositionsApproximatelyEqual( + m_Camera.transform.localPosition, + crouchingLocalCameraPosition + ) == true + ) { + return false; + } else { + return null; + } } public override MetadataWrapper generateMetadataWrapper() { @@ -1625,23 +1647,6 @@ public void TeleportObjectToFloor(ServerAction action) { ////////////// TELEPORT FULL ////////////// /////////////////////////////////////////// - // [ObsoleteAttribute(message: "This action is deprecated. Call TeleportFull(position, ...) instead.", error: false)] - // public void TeleportFull( - // float x, float y, float z, - // float rotation, - // float horizon, - // bool standing, - // bool forceAction = false - // ) { - // TeleportFull( - // position: new Vector3(x, y, z), - // rotation: new Vector3(0, rotation, 0), - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - [ObsoleteAttribute( message: "This action is deprecated. Call TeleportFull(position, ...) instead.", error: false @@ -1664,24 +1669,6 @@ public void TeleportFull( ); } - // keep undocumented until float: rotation is added to Stochastic - // public void TeleportFull( - // Vector3 position, - // float rotation, - // float horizon, - // bool standing, - // bool forceAction = false - // ) { - // TeleportFull( - // position: position, - // rotation: new Vector3(0, rotation, 0), - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - - // has to consider both the arm and standing public virtual void TeleportFull( Vector3? position, Vector3? rotation, @@ -1691,7 +1678,7 @@ public virtual void TeleportFull( ) { Debug.Log($"------- Teleport Full physicsFPS type {this.GetType()}"); // cache old values in case there's a failure - bool wasStanding = isStanding(); + bool? wasStanding = isStanding(); Vector3 oldPosition = transform.position; Quaternion oldRotation = transform.rotation; Vector3 oldCameraLocalEulerAngle = m_Camera.transform.localEulerAngles; @@ -1728,21 +1715,12 @@ public virtual void TeleportFull( "Cannot teleport due to hand object collision." ); } - if (Arm != null && Arm.IsArmColliding()) { - throw new InvalidOperationException( - "Mid Level Arm is actively clipping with some geometry in the environment. TeleportFull fails in this position." - ); - } else if (SArm != null && SArm.IsArmColliding()) { - throw new InvalidOperationException( - "Stretch Arm is actively clipping with some geometry in the environment. TeleportFull fails in this position." - ); - } base.assertTeleportedNearGround(targetPosition: position); } } catch (InvalidOperationException e) { - if (wasStanding) { + if (wasStanding == true) { stand(); - } else { + } else if (wasStanding == false) { crouch(); } if (ItemInHand != null) { @@ -1763,23 +1741,6 @@ public virtual void TeleportFull( //////////////// TELEPORT ///////////////// /////////////////////////////////////////// - // [ObsoleteAttribute(message: "This action is deprecated. Call Teleport(position, ...) instead.", error: false)] - // public void Teleport( - // float x, float y, float z, - // float? rotation = null, - // float? horizon = null, - // bool? standing = null, - // bool forceAction = false - // ) { - // Teleport( - // position: new Vector3(x, y, z), - // rotation: rotation, - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - [ObsoleteAttribute( message: "This action is deprecated. Call Teleport(position, ...) instead.", error: false @@ -1802,25 +1763,8 @@ public void Teleport( ); } - // keep undocumented until float: rotation is added to Stochastic - // DO NOT add float: rotation to base. - // public void Teleport( - // Vector3? position = null, - // float? rotation = null, - // float? horizon = null, - // bool? standing = null, - // bool forceAction = false - // ) { - // Teleport( - // position: position, - // rotation: rotation == null ? m_Camera.transform.localEulerAngles : new Vector3(0, (float) rotation, 0), - // horizon: horizon, - // standing: standing, - // forceAction: forceAction - // ); - // } - - public void Teleport( + //keeping 'Teleport' for backwards compatibility + public virtual void Teleport( Vector3? position = null, Vector3? rotation = null, float? horizon = null, @@ -1828,10 +1772,10 @@ public void Teleport( bool forceAction = false ) { TeleportFull( - position: position == null ? transform.position : (Vector3)position, - rotation: rotation == null ? transform.eulerAngles : (Vector3)rotation, - horizon: horizon == null ? m_Camera.transform.localEulerAngles.x : (float)horizon, - standing: standing == null ? isStanding() : (bool)standing, + position: position, + rotation: rotation, + horizon: horizon, + standing: standing, forceAction: forceAction ); } @@ -2399,9 +2343,10 @@ public bool CheckIfItemBlocksAgentStandOrCrouch() { else { Vector3 dir = new Vector3(); - if (isStanding()) { + bool? standState = isStanding(); + if (standState == true) { dir = new Vector3(0.0f, -1f, 0.0f); - } else { + } else if (standState == false) { dir = new Vector3(0.0f, 1f, 0.0f); } @@ -6080,7 +6025,7 @@ public virtual void stand() { } public void Crouch() { - if (!isStanding()) { + if (isStanding() == false) { errorMessage = "Already crouching."; actionFinished(false); } else if (!CheckIfItemBlocksAgentStandOrCrouch()) { @@ -6092,7 +6037,7 @@ public void Crouch() { } public void Stand() { - if (isStanding()) { + if (isStanding() == true) { errorMessage = "Already standing."; actionFinished(false); } else if (!CheckIfItemBlocksAgentStandOrCrouch()) { @@ -6256,7 +6201,7 @@ public void ExhaustiveSearchForItem(ServerAction action) { positions = getReachablePositions(); } - bool wasStanding = isStanding(); + bool? wasStanding = isStanding(); Vector3 oldPosition = transform.position; Quaternion oldRotation = transform.rotation; if (ItemInHand != null) { @@ -6347,7 +6292,7 @@ public void ExhaustiveSearchForItem(ServerAction action) { } protected HashSet getAllItemsVisibleFromPositions(Vector3[] positions) { - bool wasStanding = isStanding(); + bool? wasStanding = isStanding(); Vector3 oldPosition = transform.position; Quaternion oldRotation = transform.rotation; if (ItemInHand != null) { @@ -6392,9 +6337,9 @@ SimObjPhysics sop in GetAllVisibleSimObjPhysics( go.SetActive(true); } - if (wasStanding) { + if (wasStanding == true) { stand(); - } else { + } else if (wasStanding == false) { crouch(); } transform.position = oldPosition; @@ -6548,7 +6493,7 @@ public virtual List> getInteractablePoses( } // save current agent pose - bool wasStanding = isStanding(); + bool? wasStanding = isStanding(); Vector3 oldPosition = transform.position; Quaternion oldRotation = transform.rotation; Vector3 oldHorizon = m_Camera.transform.localEulerAngles; @@ -6656,9 +6601,9 @@ public virtual List> getInteractablePoses( } // restore old agent pose - if (wasStanding) { + if (wasStanding == true) { stand(); - } else { + } else if (wasStanding == false) { crouch(); } SetTransform( diff --git a/unity/Assets/Scripts/StretchAgentController.cs b/unity/Assets/Scripts/StretchAgentController.cs index 0c9f8e4fe8..74e9510ad5 100644 --- a/unity/Assets/Scripts/StretchAgentController.cs +++ b/unity/Assets/Scripts/StretchAgentController.cs @@ -60,10 +60,6 @@ public override ActionFinished InitializeBody(ServerAction initializeAction) { m_Camera.GetComponent().enabled = true; m_Camera.GetComponent().enabled = true; - // set camera stand/crouch local positions for Tall mode - standingLocalCameraPosition = m_Camera.transform.localPosition; - crouchingLocalCameraPosition = m_Camera.transform.localPosition; - // set up main camera parameters m_Camera.fieldOfView = 65f; @@ -115,6 +111,10 @@ out secondaryCameraParams CameraParameters.setCameraParameters(fp_camera_2, secondaryCameraParams); } + // set camera stand/crouch local positions for stretch mode even though they arent used + standingLocalCameraPosition = m_Camera.transform.localPosition; + crouchingLocalCameraPosition = m_Camera.transform.localPosition; + // enable stretch arm component Debug.Log("initializing stretch arm"); StretchArm.SetActive(true); diff --git a/unity/Assets/Scripts/UtilityFunctions.cs b/unity/Assets/Scripts/UtilityFunctions.cs index 0f053a17ab..7351abdcdc 100644 --- a/unity/Assets/Scripts/UtilityFunctions.cs +++ b/unity/Assets/Scripts/UtilityFunctions.cs @@ -505,6 +505,13 @@ public static List GetLightPropertiesOfScene() { return allOfTheLights; } + public static bool ArePositionsApproximatelyEqual(Vector3 position1, Vector3 position2, float epsilon = Vector3.kEpsilon) { + // Compare each component (x, y, z) of the two positions to see if they are approximately equal via the epsilon value + return Mathf.Abs(position1.x - position2.x) < epsilon + && Mathf.Abs(position1.y - position2.y) < epsilon + && Mathf.Abs(position1.z - position2.z) < epsilon; + } + #if UNITY_EDITOR public static void debugGetLightPropertiesOfScene(List lights) { diff --git a/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs b/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs index 4f08054e24..d9b367a6ac 100644 --- a/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs +++ b/unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs @@ -174,9 +174,7 @@ public IEnumerator TestTeleport() { "action", "TeleportFull" }, { "position", new Vector3(3f, 0.91f, 1.0f) }, //adjusting Y value to be within the error (0.05) of the floor. { "rotation", new Vector3(0f, 180f, 0f) }, - // {"forceAction", true}, { "horizon", -20f }, - { "standing", true } } ); diff --git a/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs b/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs index cd20598ace..098a25c202 100644 --- a/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs +++ b/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs @@ -329,6 +329,8 @@ public IEnumerator TestMainCameraMetadataReturn() MetadataWrapper metadata = getLastActionMetadata(); + Debug.Log($"what was standing: {metadata.agent.isStanding}"); + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.x, -1.0000000000f); Assert.AreEqual(result, true); result = Mathf.Approximately(metadata.worldRelativeCameraPosition.y, 1.5759990000f); @@ -380,6 +382,8 @@ public IEnumerator TestMainCameraMetadataReturn() metadata = getLastActionMetadata(); + Debug.Log($"what was standing after UpdateMainCamera: {metadata.agent.isStanding}"); + result = Mathf.Approximately(metadata.worldRelativeCameraPosition.x, -1.5000000000f); Assert.AreEqual(result, true); result = Mathf.Approximately(metadata.worldRelativeCameraPosition.y, 1.4009990000f); @@ -520,7 +524,7 @@ public IEnumerator TestThirdPartyCameraMetadataReturn() metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation.z, 30.0000000000f ); - Assert.AreEqual(result, true); + Assert.AreEqual(result, true); Assert.AreEqual(metadata.thirdPartyCameras[0].parentObjectName, ""); action.Clear(); @@ -607,50 +611,32 @@ public IEnumerator TestThirdPartyCameraMetadataReturn() ); Assert.AreEqual(result, true); result = Mathf.Approximately( - metadata - .thirdPartyCameras[0] - .parentRelativeThirdPartyCameraPosition - .x, + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraPosition.x, 1.0000000000f ); Assert.AreEqual(result, true); result = Mathf.Approximately( - metadata - .thirdPartyCameras[0] - .parentRelativeThirdPartyCameraPosition - .y, + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraPosition.y, 2.0000000000f ); Assert.AreEqual(result, true); result = Mathf.Approximately( - metadata - .thirdPartyCameras[0] - .parentRelativeThirdPartyCameraPosition - .z, + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraPosition.z, 3.0000020000f ); Assert.AreEqual(result, true); result = Mathf.Approximately( - metadata - .thirdPartyCameras[0] - .parentRelativeThirdPartyCameraRotation - .x, + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation.x, 20.0000000000f ); Assert.AreEqual(result, true); result = Mathf.Approximately( - metadata - .thirdPartyCameras[0] - .parentRelativeThirdPartyCameraRotation - .y, + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation.y, 20.0000000000f ); Assert.AreEqual(result, true); result = Mathf.Approximately( - metadata - .thirdPartyCameras[0] - .parentRelativeThirdPartyCameraRotation - .z, + metadata.thirdPartyCameras[0].parentRelativeThirdPartyCameraRotation.z, 20.0000000000f ); Assert.AreEqual(result, true);