diff --git a/unity/Assets/Scripts/ArmAgentController.cs b/unity/Assets/Scripts/ArmAgentController.cs index 53e9d9cdc3..222be0febe 100644 --- a/unity/Assets/Scripts/ArmAgentController.cs +++ b/unity/Assets/Scripts/ArmAgentController.cs @@ -20,6 +20,47 @@ protected IK_Robot_Arm_Controller getArm() { return arm; } + + /* + This function is identical to `MoveArm` except that rather than + giving a target position you instead give an "offset" w.r.t. + the arm's (i.e. wrist's) current location. + + Thus if you want to increase the + arms x position (in world coordinates) by 0.1m you should + pass in `offset=Vector3(0.1f, 0f, 0f)` and `coordinateSpace="world"`. + If you wanted to move the arm 0.1m to the "right" from the agent's + perspective then you would pass in the same offset but set + `coordinateSpace="armBase"`. Note that this last movement is **not** + the same as passing `position=Vector3(0.1f, 0f, 0f)` to the `MoveArm` + action with `coordinateSpace="wrist"` as, if the wrist has been rotated, + right need not mean the same thing to the arm base as it does to the wrist. + + Finally note that when `coordinateSpace="wrist"` then both `MoveArm` and + `MoveArmRelative` are identical. + */ + public void MoveArmRelative( + Vector3 offset, + float speed = 1, + float? fixedDeltaTime = null, + bool returnToStart = true, + string coordinateSpace = "armBase", + bool restrictMovement = false, + bool disableRendering = true + ) { + IK_Robot_Arm_Controller arm = getArm(); + arm.moveArmRelative( + controller: this, + offset: offset, + unitsPerSecond: speed, + fixedDeltaTime: fixedDeltaTime.GetValueOrDefault(Time.fixedDeltaTime), + returnToStart: returnToStart, + coordinateSpace: coordinateSpace, + restrictTargetPosition: restrictMovement, + disableRendering: disableRendering + ); + } + public void MoveArm( Vector3 position, float speed = 1, diff --git a/unity/Assets/Scripts/DebugDiscreteAgentController.cs b/unity/Assets/Scripts/DebugDiscreteAgentController.cs index e696af41f7..15a34abbd5 100644 --- a/unity/Assets/Scripts/DebugDiscreteAgentController.cs +++ b/unity/Assets/Scripts/DebugDiscreteAgentController.cs @@ -2,11 +2,16 @@ using System.Collections.Generic; using UnityEngine; using UnityEngine.UI; +using System; namespace UnityStandardAssets.Characters.FirstPerson { public class DebugDiscreteAgentController : MonoBehaviour { public GameObject InputFieldObj = null; public PhysicsRemoteFPSAgentController PhysicsController = null; + public StochasticRemoteFPSAgentController StochasticController = null; + public DroneFPSAgentController DroneController = null; + public ArmAgentController ArmController = null; + public AgentManager AManager = null; private InputField inputField; [SerializeField] private GameObject InputMode_Text = null; @@ -15,7 +20,13 @@ void Start() { InputFieldObj = GameObject.Find("DebugCanvasPhysics/InputField"); var Debug_Canvas = GameObject.Find("DebugCanvasPhysics"); inputField = InputFieldObj.GetComponent(); - PhysicsController = gameObject.GetComponent(); + + GameObject fpsController = GameObject.FindObjectOfType().gameObject; + PhysicsController = fpsController.GetComponent(); + StochasticController = fpsController.GetComponent(); + DroneController = fpsController.GetComponent(); + ArmController = fpsController.GetComponent(); + AManager = GameObject.Find("PhysicsSceneManager").GetComponentInChildren(); Cursor.visible = true; Cursor.lockState = CursorLockMode.None; @@ -28,6 +39,19 @@ void Start() { } + BaseFPSAgentController CurrentActiveController() { + if (PhysicsController.enabled) { + return PhysicsController; + } else if (StochasticController.enabled) { + return StochasticController; + } else if (DroneController.enabled) { + return DroneController; + } else if (ArmController.enabled) { + return ArmController; + } + throw new InvalidOperationException("No controller is active!"); + } + public void OnEnable() { InputMode_Text = GameObject.Find("DebugCanvasPhysics/InputModeText"); if (InputMode_Text) { @@ -56,7 +80,7 @@ void Update() { // ServerAction action = new ServerAction(); // action.action = "ThrowObject"; // action.moveMagnitude = 600f; - // PhysicsController.ProcessControlCommand(action); + // PhysicsController.ProcessControlCommand(action); // } // if(Input.GetKeyDown(KeyCode.U)) @@ -73,157 +97,192 @@ void Update() { // } // if we press enter, select the input field - if (PhysicsController.ReadyForCommand) { + if (CurrentActiveController().ReadyForCommand) { if (Input.GetKeyDown(KeyCode.Return)) { UnityEngine.EventSystems.EventSystem.current.SetSelectedGameObject(InputFieldObj); } - if (!inputField.isFocused) { - // float FlyMagnitude = 1.0f; - float WalkMagnitude = 0.25f; - if (Input.GetKeyDown(KeyCode.W)) { - ServerAction action = new ServerAction(); - // if(PhysicsController.FlightMode) - // { - // action.action = "FlyAhead"; - // action.moveMagnitude = FlyMagnitude; - // PhysicsController.ProcessControlCommand(action); - // } - - // else - // { - action.action = "MoveAhead"; - action.moveMagnitude = WalkMagnitude; - PhysicsController.ProcessControlCommand(action); - // } - } - - if (Input.GetKeyDown(KeyCode.S)) { - ServerAction action = new ServerAction(); - // if(PhysicsController.FlightMode) - // { - // action.action = "FlyBack"; - // action.moveMagnitude = FlyMagnitude; - // PhysicsController.ProcessControlCommand(action); - // } - - // else - // { - action.action = "MoveBack"; - action.moveMagnitude = WalkMagnitude; - PhysicsController.ProcessControlCommand(action); - // } - } - - if (Input.GetKeyDown(KeyCode.A)) { - ServerAction action = new ServerAction(); - // if(PhysicsController.FlightMode) + bool armMode = Input.GetKey(KeyCode.LeftShift); + if (!armMode) { + // float FlyMagnitude = 1.0f; + float WalkMagnitude = 0.25f; + if (Input.GetKeyDown(KeyCode.W)) { + ServerAction action = new ServerAction(); + // if(PhysicsController.FlightMode) + // { + // action.action = "FlyAhead"; + // action.moveMagnitude = FlyMagnitude; + // PhysicsController.ProcessControlCommand(action); + // } + + // else + // { + action.action = "MoveAhead"; + action.moveMagnitude = WalkMagnitude; + PhysicsController.ProcessControlCommand(action); + // } + } + + if (Input.GetKeyDown(KeyCode.S)) { + ServerAction action = new ServerAction(); + // if(PhysicsController.FlightMode) + // { + // action.action = "FlyBack"; + // action.moveMagnitude = FlyMagnitude; + // PhysicsController.ProcessControlCommand(action); + // } + + // else + // { + action.action = "MoveBack"; + action.moveMagnitude = WalkMagnitude; + PhysicsController.ProcessControlCommand(action); + // } + } + + if (Input.GetKeyDown(KeyCode.A)) { + ServerAction action = new ServerAction(); + // if(PhysicsController.FlightMode) + // { + // action.action = "FlyLeft"; + // action.moveMagnitude = FlyMagnitude; + // PhysicsController.ProcessControlCommand(action); + // } + + // else + // { + action.action = "MoveLeft"; + action.moveMagnitude = WalkMagnitude; + PhysicsController.ProcessControlCommand(action); + // } + } + + if (Input.GetKeyDown(KeyCode.D)) { + ServerAction action = new ServerAction(); + // if(PhysicsController.FlightMode) + // { + // action.action = "FlyRight"; + // action.moveMagnitude = FlyMagnitude; + // PhysicsController.ProcessControlCommand(action); + // } + + // else + // { + action.action = "MoveRight"; + action.moveMagnitude = WalkMagnitude; + PhysicsController.ProcessControlCommand(action); + // } + } + + // if(Input.GetKeyDown(KeyCode.I)) // { - // action.action = "FlyLeft"; - // action.moveMagnitude = FlyMagnitude; - // PhysicsController.ProcessControlCommand(action); + // if(PhysicsController.FlightMode) + // { + // ServerAction action = new ServerAction(); + // action.action = "FlyUp"; + // action.moveMagnitude = FlyMagnitude; + // PhysicsController.ProcessControlCommand(action); + // } // } - // else + // if(Input.GetKeyDown(KeyCode.K)) // { - action.action = "MoveLeft"; - action.moveMagnitude = WalkMagnitude; - PhysicsController.ProcessControlCommand(action); + // if(PhysicsController.FlightMode) + // { + // ServerAction action = new ServerAction(); + // action.action = "FlyDown"; + // action.moveMagnitude = FlyMagnitude; + // PhysicsController.ProcessControlCommand(action); + // } // } - } - if (Input.GetKeyDown(KeyCode.D)) { - ServerAction action = new ServerAction(); - // if(PhysicsController.FlightMode) + if (Input.GetKeyDown(KeyCode.UpArrow)) { + ServerAction action = new ServerAction(); + action.action = "LookUp"; + PhysicsController.ProcessControlCommand(action); + } + + if (Input.GetKeyDown(KeyCode.DownArrow)) { + ServerAction action = new ServerAction(); + action.action = "LookDown"; + PhysicsController.ProcessControlCommand(action); + } + + if (Input.GetKeyDown(KeyCode.LeftArrow))//|| Input.GetKeyDown(KeyCode.J)) + { + ServerAction action = new ServerAction(); + action.action = "RotateLeft"; + PhysicsController.ProcessControlCommand(action); + } + + if (Input.GetKeyDown(KeyCode.RightArrow))//|| Input.GetKeyDown(KeyCode.L)) + { + ServerAction action = new ServerAction(); + action.action = "RotateRight"; + PhysicsController.ProcessControlCommand(action); + } + + // if(Input.GetKeyDown(KeyCode.Space)) // { - // action.action = "FlyRight"; - // action.moveMagnitude = FlyMagnitude; - // PhysicsController.ProcessControlCommand(action); + // if(PhysicsController.FlightMode) + // { + // ServerAction action = new ServerAction(); + // action.action = "LaunchDroneObject"; + // action.moveMagnitude = 200f; + // // action. = new Vector3(0, 1, -1); + // action.x = 0; + // action.y = 1; + // action.z = -1; + // PhysicsController.ProcessControlCommand(action); + // } // } - // else + // if(Input.GetKeyDown(KeyCode.O)) // { - action.action = "MoveRight"; - action.moveMagnitude = WalkMagnitude; - PhysicsController.ProcessControlCommand(action); + // if(PhysicsController.FlightMode) + // { + // ServerAction action = new ServerAction(); + // action.action = "CheckDroneCaught"; + // PhysicsController.ProcessControlCommand(action); + // } // } + } else { + var actionName = "MoveArmRelative"; + var localPos = new Vector3(0, 0, 0); + float ArmMoveMagnitude = 0.05f; + + if (Input.GetKeyDown(KeyCode.W)) { + localPos.y += ArmMoveMagnitude; + } else if (Input.GetKeyDown(KeyCode.S)) { + localPos.y -= ArmMoveMagnitude; + } else if (Input.GetKeyDown(KeyCode.UpArrow)) { + localPos.z += ArmMoveMagnitude; + } else if (Input.GetKeyDown(KeyCode.DownArrow)) { + localPos.z -= ArmMoveMagnitude; + } else if (Input.GetKeyDown(KeyCode.LeftArrow)) { + localPos.x -= ArmMoveMagnitude; + } else if (Input.GetKeyDown(KeyCode.RightArrow)) { + localPos.x += ArmMoveMagnitude; + } else if (Input.GetKeyDown(KeyCode.P)) { + actionName = "PickupObject"; + } else if (Input.GetKeyDown(KeyCode.D)) { + actionName = "ReleaseObject"; + } else { + actionName = ""; + } + + if (actionName != "") { + Dictionary action = new Dictionary(); + action["action"] = actionName; + if (localPos.magnitude != 0) { + action["offset"] = localPos; + } + this.CurrentActiveController().ProcessControlCommand(action); + } } - - // if(Input.GetKeyDown(KeyCode.I)) - // { - // if(PhysicsController.FlightMode) - // { - // ServerAction action = new ServerAction(); - // action.action = "FlyUp"; - // action.moveMagnitude = FlyMagnitude; - // PhysicsController.ProcessControlCommand(action); - // } - // } - - // if(Input.GetKeyDown(KeyCode.K)) - // { - // if(PhysicsController.FlightMode) - // { - // ServerAction action = new ServerAction(); - // action.action = "FlyDown"; - // action.moveMagnitude = FlyMagnitude; - // PhysicsController.ProcessControlCommand(action); - // } - // } - - if (Input.GetKeyDown(KeyCode.UpArrow)) { - ServerAction action = new ServerAction(); - action.action = "LookUp"; - PhysicsController.ProcessControlCommand(action); - } - - if (Input.GetKeyDown(KeyCode.DownArrow)) { - ServerAction action = new ServerAction(); - action.action = "LookDown"; - PhysicsController.ProcessControlCommand(action); - } - - if (Input.GetKeyDown(KeyCode.LeftArrow))//|| Input.GetKeyDown(KeyCode.J)) - { - ServerAction action = new ServerAction(); - action.action = "RotateLeft"; - PhysicsController.ProcessControlCommand(action); - } - - if (Input.GetKeyDown(KeyCode.RightArrow))//|| Input.GetKeyDown(KeyCode.L)) - { - ServerAction action = new ServerAction(); - action.action = "RotateRight"; - PhysicsController.ProcessControlCommand(action); - } - - // if(Input.GetKeyDown(KeyCode.Space)) - // { - // if(PhysicsController.FlightMode) - // { - // ServerAction action = new ServerAction(); - // action.action = "LaunchDroneObject"; - // action.moveMagnitude = 200f; - // // action. = new Vector3(0, 1, -1); - // action.x = 0; - // action.y = 1; - // action.z = -1; - // PhysicsController.ProcessControlCommand(action); - // } - // } - - // if(Input.GetKeyDown(KeyCode.O)) - // { - // if(PhysicsController.FlightMode) - // { - // ServerAction action = new ServerAction(); - // action.action = "CheckDroneCaught"; - // PhysicsController.ProcessControlCommand(action); - // } - // } } } } } -} \ No newline at end of file +} diff --git a/unity/Assets/Scripts/IK_Robot_Arm_Controller.cs b/unity/Assets/Scripts/IK_Robot_Arm_Controller.cs index add2bf221e..3f5fef409d 100644 --- a/unity/Assets/Scripts/IK_Robot_Arm_Controller.cs +++ b/unity/Assets/Scripts/IK_Robot_Arm_Controller.cs @@ -228,6 +228,58 @@ private bool validArmTargetPosition(Vector3 targetWorldPosition) { return targetShoulderSpace.z >= 0.0f && targetShoulderSpace.magnitude <= extendedArmLength; } + protected IEnumerator resetArmTargetPositionAsLastStep(IEnumerator steps) { + while (steps.MoveNext()) { + yield return steps.Current; + } + armTarget.position = handCameraTransform.transform.position; + } + + + /* + See the documentation of the `MoveArmRelative` function + in the ArmAgentController. + */ + public void moveArmRelative( + PhysicsRemoteFPSAgentController controller, + Vector3 offset, + float unitsPerSecond, + float fixedDeltaTime = 0.02f, + bool returnToStart = false, + string coordinateSpace = "arm", + bool restrictTargetPosition = false, + bool disableRendering = false + ) { + Vector3 offsetWorldPos = Vector3.zero; + switch (coordinateSpace) { + case "world": + // world space, can be used to move directly toward positions + // returned by sim objects + offsetWorldPos = offset; + break; + case "wrist": + // space relative to base of the wrist, where the camera is + offsetWorldPos = handCameraTransform.TransformPoint(offset) - handCameraTransform.TransformPoint(Vector3.zero); + break; + case "armBase": + // space relative to the root of the arm, joint 1 + offsetWorldPos = this.transform.TransformPoint(offset) - this.transform.TransformPoint(Vector3.zero); + break; + default: + throw new ArgumentException("Invalid coordinateSpace: " + coordinateSpace); + } + moveArmTarget( + controller: controller, + target: armTarget.position + offsetWorldPos, + unitsPerSecond: unitsPerSecond, + fixedDeltaTime: fixedDeltaTime, + returnToStart: returnToStart, + coordinateSpace: "world", + restrictTargetPosition: restrictTargetPosition, + disableRendering: disableRendering + ); + } + public void moveArmTarget( PhysicsRemoteFPSAgentController controller, Vector3 target, @@ -285,18 +337,17 @@ public void moveArmTarget( ); } - Vector3 originalPos = armTarget.position; - Vector3 targetDirectionWorld = (targetWorldPos - originalPos).normalized; - - IEnumerator moveCall = ContinuousMovement.move( - controller, - collisionListener, - armTarget, - targetWorldPos, - disableRendering ? fixedDeltaTime : Time.fixedDeltaTime, - unitsPerSecond, - returnToStart, - false + IEnumerator moveCall = resetArmTargetPositionAsLastStep( + ContinuousMovement.move( + controller, + collisionListener, + armTarget, + targetWorldPos, + disableRendering ? fixedDeltaTime : Time.fixedDeltaTime, + unitsPerSecond, + returnToStart, + false + ) ); if (disableRendering) {