Skip to content

Commit

Permalink
teleport updates
Browse files Browse the repository at this point in the history 
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
  • Loading branch information
@winthos
winthos committed Sep 24, 2024
1 parent 4a067f1 commit 605b595
Show file tree
Hide file tree
Showing 6 changed files with 113 additions and 82 deletions.
2 changes: 1 addition & 1 deletion unity/Assets/Scripts/AgentManager.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
80 changes: 80 additions & 0 deletions unity/Assets/Scripts/ArmAgentController.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
96 changes: 21 additions & 75 deletions unity/Assets/Scripts/PhysicsRemoteFPSAgentController.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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
Expand All @@ -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,
Expand Down Expand Up @@ -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
Expand All @@ -1802,36 +1765,19 @@ 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,
bool? standing = null,
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
);
}
Expand Down
8 changes: 4 additions & 4 deletions unity/Assets/Scripts/StretchAgentController.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,10 +60,6 @@ public override ActionFinished InitializeBody(ServerAction initializeAction) {
m_Camera.GetComponent<PostProcessVolume>().enabled = true;
m_Camera.GetComponent<PostProcessLayer>().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;

Expand Down Expand Up @@ -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);
Expand Down
7 changes: 7 additions & 0 deletions unity/Assets/Scripts/UtilityFunctions.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -484,6 +484,13 @@ public static List<LightParameters> 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<LightParameters> lights) {
Expand Down
2 changes: 0 additions & 2 deletions unity/Assets/UnitTests/Procedural/Movement/TestProceduralTeleport.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 }
}
);

Expand Down

0 comments on commit 605b595

Please sign in to comment.