Skip to content

Commit

Permalink
formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@winthos
winthos committed Aug 21, 2024
1 parent 7bd5a59 commit 0b4e824
Show file tree
Hide file tree
Showing 2 changed files with 116 additions and 67 deletions.
37 changes: 18 additions & 19 deletions unity/Assets/Scripts/DebugInputField.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1110,32 +1110,31 @@ public void Execute(string command) {
}

case "ldtoggle": {
Dictionary<string, object> action = new Dictionary<string, object>();
Dictionary<string, object> action = new Dictionary<string, object>();

action["action"] = "ToggleLensDistortion";
if (splitcommand.Length > 1) {
action["state"] = bool.Parse(splitcommand[1]);
}
action["action"] = "ToggleLensDistortion";
if (splitcommand.Length > 1) {
action["state"] = bool.Parse(splitcommand[1]);
}

CurrentActiveController().ProcessControlCommand(action);
break;
}
CurrentActiveController().ProcessControlCommand(action);
break;
}

case "ssc": {
Dictionary<string, object> action = new Dictionary<string, object>();
Dictionary<string, object> action = new Dictionary<string, object>();

action["action"] = "SetUpSecondaryCamera";
CurrentActiveController().ProcessControlCommand(action);
break;
}
action["action"] = "SetUpSecondaryCamera";
CurrentActiveController().ProcessControlCommand(action);
break;
}
case "dsc": {
Dictionary<string, object> action = new Dictionary<string, object>();

action["action"] = "DisableSecondaryCamera";
CurrentActiveController().ProcessControlCommand(action);
break;
}
Dictionary<string, object> action = new Dictionary<string, object>();

action["action"] = "DisableSecondaryCamera";
CurrentActiveController().ProcessControlCommand(action);
break;
}

case "rpanm": {
Dictionary<string, object> action = new Dictionary<string, object>();
Expand Down
146 changes: 98 additions & 48 deletions unity/Assets/Scripts/StretchAgentController.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,19 @@ public partial class StretchAgentController : ArmAgentController {
//define default parameters for both main camera and secondary camera, specific to real-life stretch bot rig
//these are kind of magic numbers, but can be adjusted via UpdateMainCamera and UpdateThirdPartyCamera as needed if our
//real rig changes
private Vector3 defaultMainCameraLocalPosition = new Vector3(0.001920350f, 0.544700900f, 0.067880400f);
private Vector3 defaultMainCameraLocalPosition = new Vector3(
0.001920350f,
0.544700900f,
0.067880400f
);
private Vector3 defaultMainCameraLocalRotation = new Vector3(30f, 0, 0);
private float defaultMainCameraFieldOfView = 59f;
private Vector3 defaultSecondaryCameraLocalPosition = new Vector3(0.053905130f, 0.523833600f, -0.058848570f);
private Vector3 defaultSecondaryCameraLocalRotation =new Vector3(50f, 90f, 0);
private Vector3 defaultSecondaryCameraLocalPosition = new Vector3(
0.053905130f,
0.523833600f,
-0.058848570f
);
private Vector3 defaultSecondaryCameraLocalRotation = new Vector3(50f, 90f, 0);
private float defaultSecondaryCameraFieldOfView = 59f;
private Vector3[] defaultGoProCameraLocalPositions = new Vector3[]
{
Expand Down Expand Up @@ -98,7 +106,6 @@ public override ActionFinished InitializeBody(ServerAction initializeAction) {
standingLocalCameraPosition = m_Camera.transform.localPosition;
crouchingLocalCameraPosition = m_Camera.transform.localPosition;


var secondaryCameraName = "SecondaryCamera";

// activate arm-camera
Expand Down Expand Up @@ -131,7 +138,8 @@ public override ActionFinished InitializeBody(ServerAction initializeAction) {
agentManager.registerAsThirdPartyCamera(goProCameras[i]);
if (initializeAction.antiAliasing != null) {
agentManager.updateAntiAliasing(
postProcessLayer: goProCameras[i].gameObject.GetComponentInChildren<PostProcessLayer>(),
postProcessLayer: goProCameras[i]
.gameObject.GetComponentInChildren<PostProcessLayer>(),
antiAliasing: initializeAction.antiAliasing
);
}
Expand Down Expand Up @@ -714,12 +722,14 @@ public void RotateCameraMount(float degrees, bool secondary = false) {
}
}

public void SetUpSecondaryCamera(ServerAction initializeAction){
public void SetUpSecondaryCamera(ServerAction initializeAction) {
if (agentManager.thirdPartyCameras.Count == 0) {
var secondaryCameraName = "SecondaryCamera";

// activate arm-camera
Camera fp_camera_2 = m_CharacterController.transform.Find(secondaryCameraName).GetComponent<Camera>();
Camera fp_camera_2 = m_CharacterController
.transform.Find(secondaryCameraName)
.GetComponent<Camera>();
fp_camera_2.gameObject.SetActive(true);
agentManager.registerAsThirdPartyCamera(fp_camera_2);
if (initializeAction.antiAliasing != null) {
Expand Down Expand Up @@ -748,7 +758,10 @@ public void SetUpSecondaryCamera(ServerAction initializeAction){
}

var secondaryCameraParams = new CameraParameters();
var setSecondaryParams = initializeAction.thirdPartyCameraParameters?.TryGetValue(secondaryCameraName, out secondaryCameraParams);
var setSecondaryParams = initializeAction.thirdPartyCameraParameters?.TryGetValue(
secondaryCameraName,
out secondaryCameraParams
);

if (setSecondaryParams.GetValueOrDefault()) {
CameraParameters.setCameraParameters(fp_camera_2, secondaryCameraParams);
Expand All @@ -757,23 +770,27 @@ public void SetUpSecondaryCamera(ServerAction initializeAction){
actionFinished(true);
}

public void DisableSecondaryCamera(){
public void DisableSecondaryCamera() {
if (agentManager.thirdPartyCameras.Count > 0) {
var secondaryCameraName = "SecondaryCamera";
Camera fp_camera_2 = m_CharacterController.transform.Find(secondaryCameraName).GetComponent<Camera>();
Camera fp_camera_2 = m_CharacterController
.transform.Find(secondaryCameraName)
.GetComponent<Camera>();
fp_camera_2.gameObject.SetActive(false);
agentManager.thirdPartyCameras.Remove(fp_camera_2);
}
actionFinished(true);
}

public void ToggleLensDistortion (bool state) {
public void ToggleLensDistortion(bool state) {
toggleLensDistortion(state);
actionFinished(true);
}

public void toggleLensDistortion (bool state) {
PostProcessVolume ppVolume = GameObject.Find("ppProfileSecondary").GetComponent<PostProcessVolume>();
public void toggleLensDistortion(bool state) {
PostProcessVolume ppVolume = GameObject
.Find("ppProfileSecondary")
.GetComponent<PostProcessVolume>();
ppVolume.enabled = state;

// // Keeping this here in case we ever decide unravel the secrets of this obnoxious post-processing instancing when you change anything about its settings
Expand All @@ -790,16 +807,19 @@ public IEnumerator MoveAheadQuick(
bool returnToStart = true
) {
bool quickMoveSuccess = MoveRelative(
z: 1.0f,
moveMagnitude: moveMagnitude,
noise: noise,
forceAction: forceAction
z: 1.0f,
moveMagnitude: moveMagnitude,
noise: noise,
forceAction: forceAction
);
Physics.SyncTransforms();
if (quickMoveSuccess){

if (quickMoveSuccess) {
Debug.Log("Use quick MoveAhead");
yield return new ActionFinished() {success = true, actionReturn = "Use quick MoveAhead"};
yield return new ActionFinished() {
success = true,
actionReturn = "Use quick MoveAhead"
};
} else {
Debug.Log("Use slow MoveAhead");
yield return base.MoveAgent(
Expand All @@ -818,16 +838,19 @@ public IEnumerator MoveBackQuick(
bool returnToStart = true
) {
bool quickMoveSuccess = MoveRelative(
z: -1.0f,
moveMagnitude: moveMagnitude,
noise: noise,
forceAction: forceAction
z: -1.0f,
moveMagnitude: moveMagnitude,
noise: noise,
forceAction: forceAction
);
Physics.SyncTransforms();
if (quickMoveSuccess){

if (quickMoveSuccess) {
Debug.Log("Use quick MoveBack");
yield return new ActionFinished() {success = true, actionReturn = "Use quick MoveBack"};
yield return new ActionFinished() {
success = true,
actionReturn = "Use quick MoveBack"
};
} else {
Debug.Log("Use slow MoveBack");
yield return base.MoveAgent(
Expand All @@ -844,11 +867,17 @@ public IEnumerator RotateRightQuick(
float speed = 1.0f,
bool returnToStart = true
) {
bool quickRotateSuccess = Rotate(rotation: new Vector3(0, degrees.GetValueOrDefault(rotateStepDegrees), 0), noise: noise);
bool quickRotateSuccess = Rotate(
rotation: new Vector3(0, degrees.GetValueOrDefault(rotateStepDegrees), 0),
noise: noise
);
Physics.SyncTransforms();
if (quickRotateSuccess){
if (quickRotateSuccess) {
Debug.Log("Use quick RotateRight");
yield return new ActionFinished() {success = true, actionReturn = "Use quick RotateRight"};
yield return new ActionFinished() {
success = true,
actionReturn = "Use quick RotateRight"
};
} else {
Debug.Log("Use slow RotateRight");
yield return base.RotateAgent(
Expand All @@ -865,11 +894,17 @@ public IEnumerator RotateLeftQuick(
float speed = 1.0f,
bool returnToStart = true
) {
bool quickRotateSuccess = Rotate(rotation: new Vector3(0, -degrees.GetValueOrDefault(rotateStepDegrees), 0), noise: noise);
bool quickRotateSuccess = Rotate(
rotation: new Vector3(0, -degrees.GetValueOrDefault(rotateStepDegrees), 0),
noise: noise
);
Physics.SyncTransforms();
if (quickRotateSuccess){
if (quickRotateSuccess) {
Debug.Log("Use quick RotateLeft");
yield return new ActionFinished() {success = true, actionReturn = "Use quick RotateLeft"};
yield return new ActionFinished() {
success = true,
actionReturn = "Use quick RotateLeft"
};
} else {
Debug.Log("Use slow RotateLeft");
yield return base.RotateAgent(
Expand All @@ -887,24 +922,27 @@ public bool MoveRelative(
float noise = 0f,
bool forceAction = false
) {

if (!moveMagnitude.HasValue) {
moveMagnitude = gridSize;
} else if (moveMagnitude.Value <= 0f) {
throw new InvalidOperationException("moveMagnitude must be null or >= 0.");
}

if (!allowHorizontalMovement && Math.Abs(x) > 0) {
throw new InvalidOperationException("Controller does not support horizontal movement. Set AllowHorizontalMovement to true on the Controller.");
throw new InvalidOperationException(
"Controller does not support horizontal movement. Set AllowHorizontalMovement to true on the Controller."
);
}

var moveLocal = new Vector3(x, 0, z);
float xzMag = moveLocal.magnitude;
if (xzMag > 1e-5f) {
// rotate a small amount with every movement since robot doesn't always move perfectly straight
if (this.applyActionNoise) {
var rotateNoise = (float)systemRandom.NextGaussian(rotateGaussianMu, rotateGaussianSigma / 2.0f);
transform.rotation = transform.rotation * Quaternion.Euler(new Vector3(0.0f, rotateNoise, 0.0f));
var rotateNoise = (float)
systemRandom.NextGaussian(rotateGaussianMu, rotateGaussianSigma / 2.0f);
transform.rotation =
transform.rotation * Quaternion.Euler(new Vector3(0.0f, rotateNoise, 0.0f));
Physics.SyncTransforms();
}

Expand All @@ -914,7 +952,10 @@ public bool MoveRelative(
noise: noise
);

Debug.Log("Move Direction:" + this.transform.rotation * (moveLocalNorm * magnitudeWithNoise));
Debug.Log(
"Move Direction:"
+ this.transform.rotation * (moveLocalNorm * magnitudeWithNoise)
);
return base.moveInDirection(
direction: this.transform.rotation * (moveLocalNorm * magnitudeWithNoise),
forceAction: forceAction
Expand All @@ -926,7 +967,9 @@ public bool MoveRelative(
}

protected float GetMoveMagnitudeWithNoise(float moveMagnitude, float noise) {
float internalNoise = applyActionNoise ? (float)systemRandom.NextGaussian(movementGaussianMu, movementGaussianSigma) : 0;
float internalNoise = applyActionNoise
? (float)systemRandom.NextGaussian(movementGaussianMu, movementGaussianSigma)
: 0;
return moveMagnitude + noise + (float)internalNoise;
}

Expand All @@ -939,10 +982,12 @@ protected bool moveInDirection(
HashSet<Collider> ignoreColliders = null
) {
Vector3 targetPosition = transform.position + direction;
if (checkIfSceneBoundsContainTargetPosition(targetPosition) &&
CheckIfItemBlocksAgentMovement(direction, forceAction) && // forceAction = true allows ignoring movement restrictions caused by held objects
CheckIfAgentCanMove(direction, ignoreColliders)) {

if (
checkIfSceneBoundsContainTargetPosition(targetPosition)
&& CheckIfItemBlocksAgentMovement(direction, forceAction)
&& // forceAction = true allows ignoring movement restrictions caused by held objects
CheckIfAgentCanMove(direction, ignoreColliders)
) {
// only default hand if not manually interacting with things
if (!manualInteract) {
DefaultAgentHand();
Expand All @@ -960,7 +1005,8 @@ protected bool moveInDirection(
}
SimObjPhysics sop = physicsSceneManager.ObjectIdToSimObjPhysics[objectId];
if (distanceToObject(sop) > maxDistanceToObject) {
errorMessage = "Agent movement would bring it beyond the max distance of " + objectId;
errorMessage =
"Agent movement would bring it beyond the max distance of " + objectId;
transform.position = oldPosition;
return false;
}
Expand All @@ -977,12 +1023,14 @@ public bool Rotate(Vector3 rotation, float noise, bool manualInteract = false) {
DefaultAgentHand();
}

float rotateAmountDegrees = GetRotateMagnitudeWithNoise(rotation: rotation, noise: noise);
float rotateAmountDegrees = GetRotateMagnitudeWithNoise(
rotation: rotation,
noise: noise
);

// multiply quaternions to apply rotation based on rotateAmountDegrees
transform.rotation = (
transform.rotation
* Quaternion.Euler(new Vector3(0.0f, rotateAmountDegrees, 0.0f))
transform.rotation * Quaternion.Euler(new Vector3(0.0f, rotateAmountDegrees, 0.0f))
);
Physics.SyncTransforms();
if (base.isAgentCapsuleColliding()) {
Expand All @@ -997,7 +1045,9 @@ public bool Rotate(Vector3 rotation, float noise, bool manualInteract = false) {
}

protected float GetRotateMagnitudeWithNoise(Vector3 rotation, float noise) {
float internalNoise = applyActionNoise ? (float)systemRandom.NextGaussian(rotateGaussianMu, rotateGaussianSigma) : 0;
float internalNoise = applyActionNoise
? (float)systemRandom.NextGaussian(rotateGaussianMu, rotateGaussianSigma)
: 0;
return rotation.y + noise + (float)internalNoise;
}
}
Expand Down

0 comments on commit 0b4e824

Please sign in to comment.