// Use this for initialization void Start() { if (!this._graspable_object) { this._graspable_object = this.GetComponent <GraspableObject>(); } if (!this._gripper) { this._gripper = FindObjectOfType <ScriptedGripper>(); } this._grasp = this._graspable_object.GetOptimalGrasp(this._gripper).First; this._rigid_body = this._grasp.GetComponentInParent <Rigidbody>(); this._rigid_bodies = this._graspable_object.GetComponentsInChildren <Rigidbody>(); this._initial_position = this._rigid_body.transform.position; this._initial_rotation = this._rigid_body.transform.rotation; NeodroidUtilities.RegisterCollisionTriggerCallbacksOnChildren( this, this.transform, this.OnCollisionEnterChild, this.OnTriggerEnterChild, this.OnCollisionExitChild, this.OnTriggerExitChild, this.OnCollisionStayChild, this.OnTriggerStayChild, this._debugging); }
// Notify graspable objects when they are ready to grasp protected void Hover() { Collider hover = this.FindClosestGrabbableObject(CurrentGraspCenter); if (hover != this.ActiveObject && this.ActiveObject != null) { GraspableObject oldGraspable = ActiveObject.GetComponent <GraspableObject>(); if (oldGraspable != null) { oldGraspable.OnStopHover(); } } if (hover != null) { GraspableObject newGraspable = hover.GetComponent <GraspableObject>(); if (newGraspable != null) { newGraspable.OnStartHover(); } } this.ActiveObject = hover; }
// this happens on release protected void ContinueSoftGrasp() { Quaternion targetRotation = this.PalmRotation * this.RotationFromPalm; Vector3 targetPosition = this.SmoothedGraspPosition + targetRotation * this.GraspOffset; Vector3 deltaPosition = targetPosition - this.ActiveObject.transform.position; float strength = (this.ReleaseBreakDistance - deltaPosition.magnitude) / this.ReleaseBreakDistance; strength = this.ReleaseStrengthCurve.Evaluate(strength); GraspableObject graspable = this.ActiveObject.gameObject.GetComponent <GraspableObject>(); if (graspable != null) { this.ActiveObject.GetComponent <Rigidbody>().AddForce(deltaPosition.normalized * strength * this.PositionFiltering, ForceMode.Acceleration); } Quaternion deltaRotation = targetRotation * Quaternion.Inverse(this.ActiveObject.transform.rotation); float angle = 0.0f; Vector3 axis = Vector3.zero; deltaRotation.ToAngleAxis(out angle, out axis); if (graspable != null) { this.ActiveObject.GetComponent <Rigidbody>().AddTorque(strength * RotationFiltering * angle * axis, ForceMode.Acceleration); } }
// Finds the closest graspable object within range of the grasp. protected Collider FindClosestGrabbableObject(Vector3 graspPosition) { Collider closest = null; float minGraspDistance = this.GraspObjectDistance * this.GraspObjectDistance; Collider[] graspCandidates = Physics.OverlapSphere(graspPosition, GraspObjectDistance, GraspableLayers); for (int j = 0; j < graspCandidates.Length; ++j) { float squareDistance = (graspPosition - graspCandidates[j].transform.position).sqrMagnitude; if (graspCandidates[j].GetComponent <Rigidbody>() != null && squareDistance < minGraspDistance && !graspCandidates[j].transform.IsChildOf(this.transform) && graspCandidates[j].tag != "NotGrabbable") { GraspableObject grabbable = graspCandidates[j].GetComponent <GraspableObject>(); if (grabbable == null || !grabbable.IsGrabbed()) { closest = graspCandidates[j]; minGraspDistance = squareDistance; } } } return(closest); }
private void OnTriggerEnter(Collider other) { if (ContainerController.Verbose) { UnityEngine.Debug.Log("collision with: " + other.name + "..."); } // TODO: if the other gameobject has 1. a certain tag ("Target") and 2. has a GraspableObject script // attached to it and 3. is grasped, then the ObjectWasReleased event should be invoked, further, // the initiateDestruction method should be called, please set the validOrientation flag to // true in both cases if (ContainerController.Verbose) { UnityEngine.Debug.Log("collision with: " + other.gameObject.name + "..."); } if (other.gameObject.tag == "Target") { GraspableObject graspable = other.gameObject.GetComponent <GraspableObject>(); if (graspable != null) { if (graspable.IsGrabbed()) { bool validOrientation = true; this.initiateDestruction(other.gameObject, validOrientation); this.ObjectWasReleased(other.gameObject, validOrientation); } else { if (ContainerController.Verbose) { UnityEngine.Debug.Log("target object was not grapsed: " + other.gameObject.name + "..."); } } } else { if (ContainerController.Verbose) { UnityEngine.Debug.Log("target object is no graspable: " + other.gameObject.name + "..."); } } } }
public void OnRelease() { if (this.ActiveObject != null) { // Notify the grabbable object that is was released. GraspableObject graspable = this.ActiveObject.GetComponent <GraspableObject>(); if (graspable != null) { graspable.OnRelease(this.gameObject); } if (graspable == null || graspable.RotateQuickly) { ActiveObject.GetComponent <Rigidbody>().maxAngularVelocity = ObjectMaxAngularVelocity; } Leap.Utils.IgnoreCollisions(gameObject, ActiveObject.gameObject, false); } this.ActiveObject = null; this.Hover(); }
// Use this for initialization void Start() { if (!this._graspable_object) { this._graspable_object = this.GetComponent <GraspableObject>(); } if (!this._grasping) { this._grasping = FindObjectOfType <ScriptedGrasping>(); } this._grasp = this._graspable_object.GetOptimalGrasp(grasping: this._grasping).Item1; this._rigid_body = this._grasp.GetComponentInParent <Rigidbody>(); this._rigid_bodies = this._graspable_object.GetComponentsInChildren <Rigidbody>(); var transform1 = this._rigid_body.transform; this._initial_position = transform1.position; this._initial_rotation = transform1.rotation; NeodroidRegistrationUtilities .RegisterCollisionTriggerCallbacksOnChildren <ChildCollider3DSensor, Collider, Collision>(caller: this, parent: this .transform, on_collision_enter_child: this .OnCollisionEnterChild, on_trigger_enter_child: this .OnTriggerEnterChild, on_collision_exit_child: this .OnCollisionExitChild, on_trigger_exit_child: this .OnTriggerExitChild, on_collision_stay_child: this .OnCollisionStayChild, on_trigger_stay_child: this .OnTriggerStayChild, debug: this ._debugging); }
protected void ContinueHardGrasp() { // default mode Quaternion targetRotation = this.PalmRotation * this.RotationFromPalm; GraspableObject graspable = ActiveObject.gameObject.GetComponent <GraspableObject>(); if (graspable != null) { Vector3 targetPosition = this.SmoothedGraspPosition + targetRotation * this.GraspOffset; targetPosition.x = Mathf.Clamp(targetPosition.x, MinMovement.x, MaxMovement.x); targetPosition.y = Mathf.Clamp(targetPosition.y, MinMovement.y, MaxMovement.y); targetPosition.z = Mathf.Clamp(targetPosition.z, MinMovement.z, MaxMovement.z); Vector3 velocity = (targetPosition - ActiveObject.transform.position) / Time.deltaTime; this.ActiveObject.GetComponent <Rigidbody>().velocity = velocity; } Quaternion deltaRotation = targetRotation * Quaternion.Inverse(this.ActiveObject.transform.rotation); if (graspable != null) { float angle = 0.0f; Vector3 axis = Vector3.zero; deltaRotation.ToAngleAxis(out angle, out axis); if (angle >= 180) { angle = 360 - angle; axis = -axis; } if (angle != 0) { this.ActiveObject.GetComponent <Rigidbody>().angularVelocity = angle * axis; } } }
public void TrialUpdate() { if (!SpeechTrainingController.IsActive) { this.Started = false; return; } if (SpeechTrainingController.Verbose) { UnityEngine.Debug.Log("timer called, in state = " + CurrentTrialState.TrialState.ToString() + "..."); } switch (this.CurrentTrialState.TrialState) { case TrainingStates.STARTUP: if (SpeechTrainingController.Verbose) { UnityEngine.Debug.Log("startup..."); } this.HandPoseController.resetInitialPoseChecks(); this.HandPoseController.setControllerMode(PoseController.PoseControllerMode.CHECK); this.FeedbackDisplay.text = ""; if (SpeechTrainingController.Verbose) { UnityEngine.Debug.Log("startup done..."); } break; case TrainingStates.INIT: this.CurrentCongruencyCondition = ""; this.CorrectResponse = false; this.StimulationOnset = -1L; this.VerbalResponseTime = -1L; this.CurrentTrialState.TrialState = TrainingStates.FIXATION; this.FixationObject.transform.position = this.InitialTargetPosition; if (SpeechTrainingController.Verbose) { UnityEngine.Debug.Log("dummy fixation interval"); } StartCoroutine(CoroutineTimer.Start(CurrentTrialState.getSleepInterval() * .001f, this.TrialUpdateDelegate)); break; case TrainingStates.FIXATION: this.CurrentTrialState.TrialState = TrainingStates.SOA; this.FixationObject.transform.position = new Vector3(0, -10, 0); StartCoroutine(CoroutineTimer.Start(CurrentTrialState.getSleepInterval() * .001f, this.TrialUpdateDelegate)); // stimulation int stimulationTime = UnityEngine.Random.Range(-150, 150); float timeout = (CurrentTrialState.getSleepInterval() + stimulationTime) * .001f; StartCoroutine(this.TimeBasedStimulation(timeout)); break; case TrainingStates.SOA: this.messageObtained = false; if (this.Target.GetComponent <GraspableObject>().IsGrabbed()) { GraspController[] hands = GameObject.FindObjectsOfType <GraspController>(); foreach (GraspController hand in hands) { hand.requestRelease(); } } this.HandPoseController.setControllerMode(PoseController.PoseControllerMode.IDLE); GraspableObject graspable = this.Target.GetComponent <GraspableObject>(); graspable.ResetVariables(); bool rotate = UnityEngine.Random.value > .5f; graspable.ResetPositionAndOrientation(rotate ? 180.0f : 0.0f, this.InitialTargetPosition); if (rotate) { if (SpeechTrainingController.Verbose) { Debug.Log("rotated target..."); } // this.Target.transform.Rotate(new Vector3(180.0f, 0.0f, 0.0f)); // this.Target.transform.position = this.InitialTargetPosition; } else { if (SpeechTrainingController.Verbose) { Debug.Log("upright target..."); } // this.Target.transform.Rotate(new Vector3(0.0f, 0.0f, 0.0f)); // this.Target.transform.position = this.InitialTargetPosition; } this.Target.GetComponent <Rigidbody>().isKinematic = false; // edit JL this.Target.GetComponent <Rigidbody>().useGravity = true; this.CurrentTrialState.TrialState = TrainingStates.RESPONSE; // enable range check this.InteractionState = SampleTrialController.InteractionStates.NONE; this.CheckInteractionState = true; if (SpeechTrainingController.Verbose) { Debug.Log("Current Trial State: " + CurrentTrialState.TrialState); } break; case TrainingStates.RESPONSE: if (SpeechTrainingController.Verbose) { Debug.Log("response state..."); } this.CheckInteractionState = false; //method that saves the interaction in the TrialData // no verbal reponse... if (this.VerbalResponseTime == -1L) { this.InteractionState = SampleTrialController.InteractionStates.VERBAL_TIME_OUT; } if (this.VerbalResponseTime != -1L && !this.CorrectResponse) { this.InteractionState = SampleTrialController.InteractionStates.VERBAL_WRONG_RESPONSE; } if (this.InteractionState != SampleTrialController.InteractionStates.IN_BOX) { this.cancelTrial(); } else { this.FeedbackDisplay.text = this.positiveFeedbackTemplates[UnityEngine.Random.Range(0, this.positiveFeedbackTemplates.Length - 1)]; this.deactivateTarget(); this.InteractionState = SampleTrialController.InteractionStates.NONE; // clear monitor this.messageObtained = false; this.PoseCheckTimeStamp = -1L; } this.CurrentCongruencyCondition = ""; this.CorrectResponse = false; this.StimulationOnset = -1L; this.VerbalResponseTime = -1L; this.LastErrorCode = "none"; this.CurrentTrialState.TrialState = TrainingStates.WAITING; StartCoroutine(this.FeedbackInterval()); break; } }
/// <summary> /// this is the state machine that controls the interaction /// </summary> public void InteractionStateUpdate() { if (!GraspStateController.IsActive) { this.Started = false; return; } if (GraspStateController.Verbose) { UnityEngine.Debug.Log("timer called, in state = " + CurrentInteractionState.InteractionStage.ToString() + "..."); } switch (this.CurrentInteractionState.InteractionStage) { case InteractionStage.STARTUP: if (GraspStateController.Verbose) { UnityEngine.Debug.Log("startup..."); } this.HandPoseController.resetInitialPoseChecks(); this.HandPoseController.setControllerMode(PoseController.PoseControllerMode.CHECK); this.FeedbackDisplay.text = ""; if (GraspStateController.Verbose) { UnityEngine.Debug.Log("startup done..."); } break; case InteractionStage.TARGET_PRESENTATION: // just paranoia, it is highly unlikely that something like this happens if (this.Target.GetComponent <GraspableObject>().IsGrabbed()) { GraspController[] hands = GameObject.FindObjectsOfType <GraspController>(); foreach (GraspController hand in hands) { hand.requestRelease(); } } // disable the pose check this.HandPoseController.setControllerMode(PoseController.PoseControllerMode.IDLE); GraspableObject graspable = this.Target.GetComponent <GraspableObject>(); graspable.ResetVariables(); // TODO: display the bottle either upright or upside down, depending on the // bottleOrientationMode string, you can use the InitialTargetPosition variable // and the ResetPositionAndOrientation of the Graspable script // TODO: enable the physics of the bottle if (this.bottleOrientationMode == "upright") { graspable.ResetPositionAndOrientation(0.0f, this.InitialTargetPosition); if (GraspStateController.Verbose) { Debug.Log("upright target..."); } } else if (this.bottleOrientationMode == "upsidedown") { graspable.ResetPositionAndOrientation(180.0f, this.InitialTargetPosition); if (GraspStateController.Verbose) { Debug.Log("rotated target..."); } } this.Target.GetComponent <Rigidbody>().isKinematic = false; this.Target.GetComponent <Rigidbody>().useGravity = true; this.CurrentInteractionState.InteractionStage = InteractionStage.GRASPING_INTERACTION; // enable range check this.InteractionState = Interactioncontroller.InteractionStates.NONE; this.CheckInteractionState = true; this.RangeCheck.clearMonitor(); // TODO: add the target to the monitor, have a look at the EffectorRangeCheck script this.RangeCheck.monitorObject(this.Target); if (Verbose) { Debug.Log("current interaction state: " + CurrentInteractionState.InteractionStage); } break; case InteractionStage.GRASPING_INTERACTION: if (GraspStateController.Verbose) { Debug.Log("response state..."); } this.CheckInteractionState = false; if (this.InteractionState != Interactioncontroller.InteractionStates.IN_BOX) { this.cancelInteractionSequence(); } else { this.FeedbackDisplay.text = this.positiveFeedbackTemplates[UnityEngine.Random.Range(0, this.positiveFeedbackTemplates.Length - 1)]; this.deactivateTarget(); this.InteractionState = Interactioncontroller.InteractionStates.NONE; // clear monitor this.RangeCheck.clearMonitor(); this.PoseCheckTimeStamp = -1L; } this.CurrentInteractionState.InteractionStage = InteractionStage.WAITING; StartCoroutine(this.FeedbackInterval()); break; } }
protected void StartGrasp() { // Only grasp if we're hovering over an object. if (this.ActiveObject == null) { return; } HandModel handModel = this.GetComponent <HandModel>(); GraspableObject graspable = this.ActiveObject.GetComponent <GraspableObject>(); Leap.Utils.IgnoreCollisions(this.gameObject, this.ActiveObject.gameObject, true); // Setup initial position and rotation conditions. this.PalmRotation = handModel.GetPalmRotation(); this.GraspOffset = Vector3.zero; // If we don't center the object, find the closest point in the collider for our grab point. if (graspable == null || !graspable.CenterGraspedObject) { Vector3 deltaPosition = ActiveObject.transform.position - this.CurrentGraspCenter; Ray graspRay = new Ray(this.CurrentGraspCenter, deltaPosition); RaycastHit graspHit; // If the raycast hits the object, we are outside the collider so grab the hit point. // If not, we are inside the collider so just use the grasp position. if (ActiveObject.Raycast(graspRay, out graspHit, GraspObjectDistance)) { this.GraspOffset = this.ActiveObject.transform.position - graspHit.point; } else { this.GraspOffset = this.ActiveObject.transform.position - CurrentGraspCenter; } } this.SmoothedGraspPosition = this.ActiveObject.transform.position - this.GraspOffset; this.GraspOffset = Quaternion.Inverse(this.ActiveObject.transform.rotation) * GraspOffset; this.RotationFromPalm = Quaternion.Inverse(this.PalmRotation) * this.ActiveObject.transform.rotation; // If we can rotate the object quickly, increase max angular velocity for now. if (graspable == null || graspable.RotateQuickly) { this.ObjectMaxAngularVelocity = this.ActiveObject.GetComponent <Rigidbody>().maxAngularVelocity; this.ActiveObject.GetComponent <Rigidbody>().maxAngularVelocity = Mathf.Infinity; } if (graspable != null) { // Notify grabbable object that it was grabbed. if (graspable.GetResponseTime() < 0L) { if (handModel != null) { Vector3 normal = handModel.GetPalmNormal(); float angleLeft = Vector3.Angle(normal, Vector3.left); float angleRight = Vector3.Angle(normal, Vector3.right); if (GraspController.Verbose) { Debug.LogWarning("hand normal: " + normal + ", to right: " + angleRight + ", to left: " + angleLeft); } if (angleLeft < angleRight) { graspable.setGraspDirection(GraspableObject.GraspDirection.UPRIGHT); } else { graspable.setGraspDirection(GraspableObject.GraspDirection.ROTATED); } } else { if (GraspController.Verbose) { Debug.LogWarning("no hand found..."); } } } graspable.OnGrasp(this.gameObject); if (graspable.UseAxisAlignment) { // If this option is enabled we only want to align the object axis with the palm axis, // hence we cancel out any rotation about the aligned axis. Vector3 palmVector = graspable.RightHandAxis; if (handModel.GetLeapHand().IsLeft) { palmVector = Vector3.Scale(palmVector, new Vector3(-1, 1, 1)); } Vector3 axisInPalm = this.RotationFromPalm * graspable.ObjectAxis; Quaternion axisCorrection = Quaternion.FromToRotation(axisInPalm, palmVector); if (Vector3.Dot(axisInPalm, palmVector) < 0) { axisCorrection = Quaternion.FromToRotation(axisInPalm, -palmVector); } this.RotationFromPalm = axisCorrection * this.RotationFromPalm; } } }
public void TrialUpdate() { if (!GraspTrainingController.IsActive) { this.Started = false; return; } if (this.CurrentTrialState.TrialState == TrainingStates.RESPONSE && SampleTrialController.Millis - this.TargetOnsetTime < 1000L) { return; } if (GraspTrainingController.Verbose) { UnityEngine.Debug.Log("timer called, in state = " + CurrentTrialState.TrialState.ToString() + "..."); } switch (this.CurrentTrialState.TrialState) { case TrainingStates.STARTUP: if (GraspTrainingController.Verbose) { UnityEngine.Debug.Log("startup..."); } this.HandPoseController.resetInitialPoseChecks(); this.HandPoseController.setControllerMode(PoseController.PoseControllerMode.CHECK); this.FeedbackDisplay.text = ""; if (this.variableMapping) { this.OffsetController.DriftFactor = this.master.visualOffsets[UnityEngine.Random.Range(0, this.master.visualOffsets.Length)]; } else { this.OffsetController.DriftFactor = 0.0f; } if (GraspTrainingController.Verbose) { UnityEngine.Debug.Log("startup done..."); } break; case TrainingStates.INIT: this.CurrentTrialState.TrialState = TrainingStates.FIXATION; this.FixationObject.transform.position = this.InitialTargetPosition; this.master.GraspTrainingTrialStart(); if (GraspTrainingController.Verbose) { UnityEngine.Debug.Log("dummy fixation interval"); } StartCoroutine(CoroutineTimer.Start(CurrentTrialState.getSleepInterval() * .001f, this.TrialUpdateDelegate)); break; case TrainingStates.FIXATION: this.CurrentTrialState.TrialState = TrainingStates.SOA; this.FixationObject.transform.position = new Vector3(0, -10, 0); if (this.variableMapping) { this.OffsetController.ApplyDrift = true; } else { this.OffsetController.ApplyDrift = false; } StartCoroutine(CoroutineTimer.Start(CurrentTrialState.getSleepInterval() * .001f, this.TrialUpdateDelegate)); break; case TrainingStates.SOA: if (this.Target.GetComponent <GraspableObject>().IsGrabbed()) { GraspController[] hands = GameObject.FindObjectsOfType <GraspController>(); foreach (GraspController hand in hands) { hand.requestRelease(); } } this.HandPoseController.setControllerMode(PoseController.PoseControllerMode.IDLE); GraspableObject graspable = this.Target.GetComponent <GraspableObject>(); graspable.ResetVariables(); if (this.bottleOrientationMode == "upright") { graspable.ResetPositionAndOrientation(0.0f, this.InitialTargetPosition); if (GraspTrainingController.Verbose) { Debug.Log("upright target..."); } // this.Target.transform.Rotate(new Vector3(0.0f, 0.0f, 0.0f)); // this.Target.transform.position = this.InitialTargetPosition; } else if (this.bottleOrientationMode == "upsidedown") { graspable.ResetPositionAndOrientation(180.0f, this.InitialTargetPosition); if (GraspTrainingController.Verbose) { Debug.Log("rotated target..."); } // this.Target.transform.Rotate(new Vector3(180.0f, 0.0f, 0.0f)); // this.Target.transform.position = this.InitialTargetPosition; } else if (this.bottleOrientationMode == "random") { if (UnityEngine.Random.value > .5f) { graspable.ResetPositionAndOrientation(180.0f, this.InitialTargetPosition); if (GraspTrainingController.Verbose) { Debug.Log("rotated target..."); } // this.Target.transform.Rotate(new Vector3(180.0f, 0.0f, 0.0f)); // this.Target.transform.position = this.InitialTargetPosition; } else { graspable.ResetPositionAndOrientation(0.0f, this.InitialTargetPosition); if (GraspTrainingController.Verbose) { Debug.Log("upright target..."); } // this.Target.transform.Rotate(new Vector3(0.0f, 0.0f, 0.0f)); // this.Target.transform.position = this.InitialTargetPosition; } } else { UnityEngine.Debug.Log("unknown orientation mode: " + this.bottleOrientationMode + "..."); } this.Target.GetComponent <Rigidbody>().isKinematic = false; // edit JL this.Target.GetComponent <Rigidbody>().useGravity = true; this.CurrentTrialState.TrialState = TrainingStates.RESPONSE; // enable range check this.InteractionState = SampleTrialController.InteractionStates.NONE; this.CheckInteractionState = true; this.RangeCheck.clearMonitor(); this.RangeCheck.monitorObject(this.Target); this.TargetOnsetTime = SampleTrialController.Millis; if (Verbose) { Debug.Log("Current Trial State: " + CurrentTrialState.TrialState); } break; case TrainingStates.RESPONSE: if (GraspTrainingController.Verbose) { Debug.Log("response state..."); } this.CheckInteractionState = false; this.OffsetController.ApplyDrift = false; if (this.InteractionState != SampleTrialController.InteractionStates.IN_BOX) { this.cancelTrial(); // reset trial state... //this.CurrentTrialState.TrialState = TrainingStates.STARTUP; //StartCoroutine(CoroutineTimer.Start(CurrentTrialState.getSleepInterval() * .001f, this.TrialUpdateDelegate)); } else { this.FeedbackDisplay.text = this.positiveFeedbackTemplates[UnityEngine.Random.Range(0, this.positiveFeedbackTemplates.Length - 1)]; this.deactivateTarget(); this.InteractionState = SampleTrialController.InteractionStates.NONE; // clear monitor this.RangeCheck.clearMonitor(); this.PoseCheckTimeStamp = -1L; //this.CurrentTrialState.TrialState = TrainingStates.STARTUP; //StartCoroutine(CoroutineTimer.Start(CurrentTrialState.getSleepInterval() * .001f, this.TrialUpdateDelegate)); } this.CurrentTrialState.TrialState = TrainingStates.WAITING; StartCoroutine(this.FeedbackInterval()); break; } }