From 605b595a70e5cc9fe34245bedc4e92cd4ede4b01 Mon Sep 17 00:00:00 2001 From: Winson Han Date: Tue, 24 Sep 2024 16:06:00 -0700 Subject: [PATCH 1/7] teleport updates The purpose of these updates is to prevent an edge case where, when using an arm-typed agent, calling the Teleport action would cause the isStanding value to default in a weird way, causing the main camera of the arm-typed agent to be changed unexpectedly back to a default position that only has context for the high-level-agent. This was mainly due to the arm agents being derived classes of the high-level-agent's `PhysicsRemoteFPSAgentController` class - adding an override to the Teleport/TeleportFull actions in the `ArmAgentController` class so that an error can be thrown if passing in the `standing` bool since arm agents don't have a concept of a standing vs crouching camera position - updating the isStanding() helper function to throw an exception in cases where the High Level Agent's main camera is not in either the hard-coded standing or crouching local position. This has some issues as if UpdateMainCamera is called while using the HighLevelAgent, it causes an incompatible state --- unity/Assets/Scripts/AgentManager.cs | 2 +- unity/Assets/Scripts/ArmAgentController.cs | 80 ++++++++++++++++ .../PhysicsRemoteFPSAgentController.cs | 96 ++++--------------- .../Assets/Scripts/StretchAgentController.cs | 8 +- unity/Assets/Scripts/UtilityFunctions.cs | 7 ++ .../Movement/TestProceduralTeleport.cs | 2 - 6 files changed, 113 insertions(+), 82 deletions(-) diff --git a/unity/Assets/Scripts/AgentManager.cs b/unity/Assets/Scripts/AgentManager.cs index 1b869c9fad..36d2070205 100644 --- a/unity/Assets/Scripts/AgentManager.cs +++ b/unity/Assets/Scripts/AgentManager.cs @@ -320,7 +320,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) { diff --git a/unity/Assets/Scripts/ArmAgentController.cs b/unity/Assets/Scripts/ArmAgentController.cs index 5e3aea7317..0c317ade63 100644 --- a/unity/Assets/Scripts/ArmAgentController.cs +++ b/unity/Assets/Scripts/ArmAgentController.cs @@ -266,6 +266,86 @@ 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 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 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 (isHandObjectColliding(ignoreAgent: true)) { + throw new InvalidOperationException( + "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) { + 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/PhysicsRemoteFPSAgentController.cs b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs index a827d8200a..7daea12c8f 100644 --- a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs +++ b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs @@ -174,6 +174,21 @@ public void SetMassProperties(string objectId, float mass, float drag, float ang } 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 false; + } + + if(UtilityFunctions.ArePositionsApproximatelyEqual(m_Camera.transform.localPosition, standingLocalCameraPosition) != true && + UtilityFunctions.ArePositionsApproximatelyEqual(m_Camera.transform.localPosition, crouchingLocalCameraPosition) != true) { + throw new InvalidOperationException( + $"Camera position is not equal to standing or crouching position. camera local position: {m_Camera.transform.localPosition}, standing local position: {standingLocalCameraPosition}, crouching local position: {crouchingLocalCameraPosition}" + ); + } + return (m_Camera.transform.localPosition - standingLocalCameraPosition).magnitude < 0.1f; } @@ -1625,23 +1640,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 +1662,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, @@ -1763,23 +1743,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 +1765,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 +1774,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 ); } 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 18e1e8cbd5..c05a2cba83 100644 --- a/unity/Assets/Scripts/UtilityFunctions.cs +++ b/unity/Assets/Scripts/UtilityFunctions.cs @@ -484,6 +484,13 @@ public static List GetLightPropertiesOfScene() { return allOfTheLights; } + public static bool ArePositionsApproximatelyEqual(Vector3 position1, Vector3 position2) { + // 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) < Vector3.kEpsilon + && Mathf.Abs(position1.y - position2.y) < Vector3.kEpsilon + && Mathf.Abs(position1.z - position2.z) < Vector3.kEpsilon; + } + #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 } } ); From df6c14ff7be8f3977a4f7f24ffd5689029c0fb23 Mon Sep 17 00:00:00 2001 From: Winson Han Date: Wed, 25 Sep 2024 13:55:10 -0700 Subject: [PATCH 2/7] allowing null return for isStanding trying a fix where we allow returning a null value for `isStanding` for cases where the camera position has changed via UpdateMainCamera or for agents that don't have a meaningful use for isStanding like stretch agents --- .../DiscreteHidenSeekAgentController.cs | 5 +- .../PhysicsRemoteFPSAgentController.cs | 49 ++++++++++--------- .../TestThirdPartyCameraAndMainCamera.cs | 4 ++ 3 files changed, 34 insertions(+), 24 deletions(-) diff --git a/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs b/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs index 354b1045a5..43c4f575da 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 7daea12c8f..7e7aa365af 100644 --- a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs +++ b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs @@ -173,24 +173,28 @@ public void SetMassProperties(string objectId, float mass, float drag, float ang return; } - public bool isStanding() { + 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; } - if(UtilityFunctions.ArePositionsApproximatelyEqual(m_Camera.transform.localPosition, standingLocalCameraPosition) != true && - UtilityFunctions.ArePositionsApproximatelyEqual(m_Camera.transform.localPosition, crouchingLocalCameraPosition) != true) { - throw new InvalidOperationException( - $"Camera position is not equal to standing or crouching position. camera local position: {m_Camera.transform.localPosition}, standing local position: {standingLocalCameraPosition}, crouching local position: {crouchingLocalCameraPosition}" - ); + else { + return null; } - return (m_Camera.transform.localPosition - standingLocalCameraPosition).magnitude - < 0.1f; } public override MetadataWrapper generateMetadataWrapper() { @@ -1671,7 +1675,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; @@ -1720,9 +1724,9 @@ public virtual void TeleportFull( base.assertTeleportedNearGround(targetPosition: position); } } catch (InvalidOperationException e) { - if (wasStanding) { + if (wasStanding == true) { stand(); - } else { + } else if (wasStanding == false) { crouch(); } if (ItemInHand != null) { @@ -2345,9 +2349,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); } @@ -6026,7 +6031,7 @@ public virtual void stand() { } public void Crouch() { - if (!isStanding()) { + if (isStanding() == false) { errorMessage = "Already crouching."; actionFinished(false); } else if (!CheckIfItemBlocksAgentStandOrCrouch()) { @@ -6038,7 +6043,7 @@ public void Crouch() { } public void Stand() { - if (isStanding()) { + if (isStanding() == true) { errorMessage = "Already standing."; actionFinished(false); } else if (!CheckIfItemBlocksAgentStandOrCrouch()) { @@ -6240,7 +6245,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) { @@ -6331,7 +6336,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) { @@ -6376,9 +6381,9 @@ SimObjPhysics sop in GetAllVisibleSimObjPhysics( go.SetActive(true); } - if (wasStanding) { + if (wasStanding == true) { stand(); - } else { + } else if(wasStanding == false){ crouch(); } transform.position = oldPosition; @@ -6532,7 +6537,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; @@ -6640,9 +6645,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/UnitTests/TestThirdPartyCameraAndMainCamera.cs b/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs index cd20598ace..0332d494b0 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); From 33ef2df112a4db2cdddd589958cc41115bbbea89 Mon Sep 17 00:00:00 2001 From: Winson Han Date: Wed, 25 Sep 2024 14:57:18 -0700 Subject: [PATCH 3/7] isolating held object checks to high level agent... ... that uses the high level "hand" rather than a physical arm also updating errorMessage for armed agents --- unity/Assets/Scripts/ArmAgentController.cs | 9 ++------- unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs | 9 --------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/unity/Assets/Scripts/ArmAgentController.cs b/unity/Assets/Scripts/ArmAgentController.cs index 0c317ade63..f851d2caf9 100644 --- a/unity/Assets/Scripts/ArmAgentController.cs +++ b/unity/Assets/Scripts/ArmAgentController.cs @@ -275,7 +275,7 @@ public override void Teleport( ) { //non-high level agents cannot set standing if(standing != null) { - errorMessage = "Cannot set standing for stretch agent"; + errorMessage = "Cannot set standing for arm/stretch agent"; actionFinishedEmit(success:false, actionReturn:null, errorMessage:errorMessage); return; } @@ -298,7 +298,7 @@ public override void TeleportFull( ) { //non-high level agents cannot set standing if(standing != null) { - errorMessage = "Cannot set standing for stretch agent"; + errorMessage = "Cannot set standing for arm/stretch agent"; actionFinishedEmit(success:false, actionReturn:null, errorMessage:errorMessage); return; } @@ -318,11 +318,6 @@ public override void TeleportFull( // add arm value cases if (!forceAction) { - if (isHandObjectColliding(ignoreAgent: true)) { - throw new InvalidOperationException( - "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." diff --git a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs index 7e7aa365af..ff3a92003c 100644 --- a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs +++ b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs @@ -1712,15 +1712,6 @@ 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) { From 1b9a9865bb45548df30c2eaab51fb0f4e4c37494 Mon Sep 17 00:00:00 2001 From: Winson Han Date: Wed, 25 Sep 2024 16:04:11 -0700 Subject: [PATCH 4/7] formatting --- unity/Assets/Scripts/AgentManager.cs | 17 ++++++---- unity/Assets/Scripts/ArmAgentController.cs | 21 ++++++------ .../DiscreteHidenSeekAgentController.cs | 2 +- .../PhysicsRemoteFPSAgentController.cs | 31 ++++++++++-------- .../TestThirdPartyCameraAndMainCamera.cs | 32 ++++--------------- 5 files changed, 46 insertions(+), 57 deletions(-) diff --git a/unity/Assets/Scripts/AgentManager.cs b/unity/Assets/Scripts/AgentManager.cs index 36d2070205..b3dc6c1a97 100644 --- a/unity/Assets/Scripts/AgentManager.cs +++ b/unity/Assets/Scripts/AgentManager.cs @@ -1310,17 +1310,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 @@ -1337,7 +1342,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 f851d2caf9..de21938e37 100644 --- a/unity/Assets/Scripts/ArmAgentController.cs +++ b/unity/Assets/Scripts/ArmAgentController.cs @@ -274,9 +274,9 @@ public override void Teleport( bool forceAction = false ) { //non-high level agents cannot set standing - if(standing != null) { + if (standing != null) { errorMessage = "Cannot set standing for arm/stretch agent"; - actionFinishedEmit(success:false, actionReturn:null, errorMessage:errorMessage); + actionFinishedEmit(success: false, actionReturn: null, errorMessage: errorMessage); return; } @@ -290,19 +290,19 @@ public override void Teleport( } public override void TeleportFull( - Vector3? position = null, - Vector3? rotation = null, - float? horizon = null, - bool? standing = null, + 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) { + if (standing != null) { errorMessage = "Cannot set standing for arm/stretch agent"; - actionFinishedEmit(success:false, actionReturn:null, errorMessage:errorMessage); + 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; @@ -315,7 +315,7 @@ public override void TeleportFull( horizon: horizon, forceAction: forceAction ); - + // add arm value cases if (!forceAction) { if (Arm != null && Arm.IsArmColliding()) { @@ -329,7 +329,6 @@ public override void TeleportFull( } base.assertTeleportedNearGround(targetPosition: position); } - } catch (InvalidOperationException e) { transform.position = oldPosition; transform.rotation = oldRotation; diff --git a/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs b/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs index 43c4f575da..6d4deb2c37 100644 --- a/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs +++ b/unity/Assets/Scripts/DiscreteHidenSeekAgentController.cs @@ -268,7 +268,7 @@ void Update() { if (wasStanding == true) { action.action = "Crouch"; PhysicsController.ProcessControlCommand(action); - } else if(wasStanding == false) { + } 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 ff3a92003c..8f91afd3b5 100644 --- a/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs +++ b/unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs @@ -174,27 +174,30 @@ public void SetMassProperties(string objectId, float mass, float drag, float ang } 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)) - { + 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) { + if ( + UtilityFunctions.ArePositionsApproximatelyEqual( + m_Camera.transform.localPosition, + standingLocalCameraPosition + ) == true + ) { return true; - } - - else if(UtilityFunctions.ArePositionsApproximatelyEqual(m_Camera.transform.localPosition, crouchingLocalCameraPosition) == true) { + } else if ( + UtilityFunctions.ArePositionsApproximatelyEqual( + m_Camera.transform.localPosition, + crouchingLocalCameraPosition + ) == true + ) { return false; - } - - else { + } else { return null; } - } public override MetadataWrapper generateMetadataWrapper() { @@ -2343,7 +2346,7 @@ public bool CheckIfItemBlocksAgentStandOrCrouch() { bool? standState = isStanding(); if (standState == true) { dir = new Vector3(0.0f, -1f, 0.0f); - } else if (standState == false){ + } else if (standState == false) { dir = new Vector3(0.0f, 1f, 0.0f); } @@ -6374,7 +6377,7 @@ SimObjPhysics sop in GetAllVisibleSimObjPhysics( if (wasStanding == true) { stand(); - } else if(wasStanding == false){ + } else if (wasStanding == false) { crouch(); } transform.position = oldPosition; @@ -6638,7 +6641,7 @@ public virtual List> getInteractablePoses( // restore old agent pose if (wasStanding == true) { stand(); - } else if(wasStanding == false) { + } else if (wasStanding == false) { crouch(); } SetTransform( diff --git a/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs b/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs index 0332d494b0..098a25c202 100644 --- a/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs +++ b/unity/Assets/UnitTests/TestThirdPartyCameraAndMainCamera.cs @@ -524,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(); @@ -611,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); From 778be8518cd8137b25b30bfa33fed90ea7f1e993 Mon Sep 17 00:00:00 2001 From: Winson Han Date: Thu, 26 Sep 2024 11:13:05 -0700 Subject: [PATCH 5/7] updating unit tests to account for new isStanding return --- ai2thor/tests/data/arm-metadata-schema.json | 2 +- ai2thor/tests/test_unity.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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..6210509170 100644 --- a/ai2thor/tests/test_unity.py +++ b/ai2thor/tests/test_unity.py @@ -1418,7 +1418,7 @@ 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"]: From 5bf2d3e970d39ee9fcddc92bbbb60a2698eb3d63 Mon Sep 17 00:00:00 2001 From: Winson Han Date: Thu, 26 Sep 2024 11:51:51 -0700 Subject: [PATCH 6/7] reformatting and updating stretch teleport unit test --- ai2thor/tests/test_unity.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ai2thor/tests/test_unity.py b/ai2thor/tests/test_unity.py index 6210509170..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 None, agent + " cannot stand so this should be None/null!" + 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']}") From 1545ca219d349cd24bafc57c6989cf5a72433042 Mon Sep 17 00:00:00 2001 From: Winson Han Date: Thu, 3 Oct 2024 16:23:54 -0700 Subject: [PATCH 7/7] Update UtilityFunctions.cs allowing epsilon to be parameterized, default to Unity's Vector3.kEpsilon, which apparently is built into the Vector3 class specifically because Unity's representation of floats is the 32-bit variety and that epsilon value is tuned for that??? So seems like a good default still --- unity/Assets/Scripts/UtilityFunctions.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/unity/Assets/Scripts/UtilityFunctions.cs b/unity/Assets/Scripts/UtilityFunctions.cs index c05a2cba83..10d14ea27d 100644 --- a/unity/Assets/Scripts/UtilityFunctions.cs +++ b/unity/Assets/Scripts/UtilityFunctions.cs @@ -484,11 +484,11 @@ public static List GetLightPropertiesOfScene() { return allOfTheLights; } - public static bool ArePositionsApproximatelyEqual(Vector3 position1, Vector3 position2) { + 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) < Vector3.kEpsilon - && Mathf.Abs(position1.y - position2.y) < Vector3.kEpsilon - && Mathf.Abs(position1.z - position2.z) < Vector3.kEpsilon; + 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