protected override void GUIBehaviorInput() { if (GUI.Button(new Rect(10, 10, 120, buttonHeight), "Idle")) { MSkeletonAccess.Iface skeletonAccess = this.avatar.GetSkeletonAccess(); skeletonAccess.SetChannelData(this.avatar.GetPosture()); MInstruction instruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); //MInstruction instruction = new MInstruction(MInstructionFactory.GenerateID(), "MMUTest", "Object/Move"); MSimulationState simstate = new MSimulationState(this.avatar.GetPosture(), this.avatar.GetPosture()); this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(instruction, simstate); } if (GUI.Button(new Rect(140, 10, 120, buttonHeight), "Walk to")) { MInstruction walkInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Walk", MOTION_WALK) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance.GetSceneObjectByName("WalkTarget").ID) }; MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE) { //Start idle after walk has been finished StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End //synchronization constraint similar to bml "id:End" (bml original: <bml start="id:End"/> }; this.CoSimulator.Abort(); MSimulationState currentState = new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }; //Assign walk and idle instruction this.CoSimulator.AssignInstruction(walkInstruction, currentState); this.CoSimulator.AssignInstruction(idleInstruction, currentState); this.CoSimulator.MSimulationEventHandler += this.CoSimulator_MSimulationEventHandler; } if (GUI.Button(new Rect(270, 10, 120, buttonHeight), "Reach Object")) { MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); MInstruction reachRight = new MInstruction(MInstructionFactory.GenerateID(), "reach right", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetR"].ID, "Hand", "Right", "MinDistance", 2.0.ToString()), }; //MInstruction reachLeft = new MInstruction(MInstructionFactory.GenerateID(), "reach left", "Pose/Reach") //{ // Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetL"].ID, "Hand", "Left"), //}; //this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(reachRight, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); //this.CoSimulator.AssignInstruction(reachLeft, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(400, 10, 120, buttonHeight), "Move Object")) { MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); MInstruction moveObject = new MInstruction(MInstructionFactory.GenerateID(), "move object", MOTION_MOVE) { Properties = PropertiesCreator.Create("SubjectID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Right", "TargetID", UnitySceneAccess.Instance["PositioningTarget"].ID), }; this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(moveObject, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(530, 10, 120, buttonHeight), "Pick-up")) { MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); MInstruction reachInstruction = new MInstruction(MInstructionFactory.GenerateID(), "reach", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetR"].ID, "Hand", "Right"), }; carryID = MInstructionFactory.GenerateID(); MInstruction carryInstruction = new MInstruction(carryID, "carry object", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Right"), StartCondition = reachInstruction.ID + ":" + mmiConstants.MSimulationEvent_End + "+ 0.01" }; this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(reachInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(carryInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.MSimulationEventHandler += this.CoSimulator_MSimulationEventHandler; } if (GUI.Button(new Rect(660, 10, 120, buttonHeight), "Carry Object")) { carryID = MInstructionFactory.GenerateID(); MInstruction carryInstruction = new MInstruction(carryID, "carry object", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Right")//, "CarryTarget", UnitySceneAccess.Instance["CarryTarget"].ID), }; this.CoSimulator.AssignInstruction(carryInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(790, 10, 120, buttonHeight), "Release Object")) { MInstruction releaseRight = new MInstruction(MInstructionFactory.GenerateID(), "release object", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Right", CoSimTopic.OnStart, carryID + ":" + CoSimAction.EndInstruction), }; MInstruction releaseLeft = new MInstruction(MInstructionFactory.GenerateID(), "release object", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Left", CoSimTopic.OnStart, carryID + ":" + CoSimAction.EndInstruction), }; this.CoSimulator.AssignInstruction(releaseRight, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(releaseLeft, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(10, 40, 160, buttonHeight), "Concurrent scenario")) { MInstruction walkInstruction1 = new MInstruction(MInstructionFactory.GenerateID(), "Walk", MOTION_WALK) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance.GetSceneObjectByName("WalkTargetConcurrent").ID), }; MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE) { StartCondition = walkInstruction1.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction reachRight = new MInstruction(MInstructionFactory.GenerateID(), "reach right", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTarget2R"].ID, "Hand", "Right"), StartCondition = walkInstruction1.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction reachLeft = new MInstruction(MInstructionFactory.GenerateID(), "reach left", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetL"].ID, "Hand", "Left"), StartCondition = walkInstruction1.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction carryLeft = new MInstruction(MInstructionFactory.GenerateID(), "carry left", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Left", "AddOffset", false.ToString()), StartCondition = reachLeft.ID + ":" + mmiConstants.MSimulationEvent_End + "+ 0.01" }; MInstruction carryRight = new MInstruction(MInstructionFactory.GenerateID(), "carry right", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject2"].ID, "Hand", "Right", "AddOffset", false.ToString()), StartCondition = reachRight.ID + ":" + mmiConstants.MSimulationEvent_End + "+ 0.01" }; MInstruction walkInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Walk", MOTION_WALK) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance.GetSceneObjectByName("WalkTarget2").ID), //Both carries must me finished with positioning StartCondition = carryRight.ID + ":" + "PositioningFinished" + " && " + carryLeft.ID + ":" + "PositioningFinished", //Finish the idle instruction Action = CoSimTopic.OnStart + "->" + idleInstruction.ID + ":" + CoSimAction.EndInstruction, }; MInstruction idleInstruction2 = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE) { StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End, }; MInstruction moveRight = new MInstruction(MInstructionFactory.GenerateID(), "move right", MOTION_MOVE) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["PositioningTargetRight"].ID, "SubjectID", UnitySceneAccess.Instance["GraspObject2"].ID, "Hand", "Right", CoSimTopic.OnStart, carryRight.ID + ":" + CoSimAction.EndInstruction), StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction moveLeft = new MInstruction(MInstructionFactory.GenerateID(), "move left", MOTION_MOVE) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["PositioningTargetLeft"].ID, "SubjectID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Left", CoSimTopic.OnStart, carryLeft.ID + ":" + CoSimAction.EndInstruction), StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction releaseRight = new MInstruction(MInstructionFactory.GenerateID(), "release right", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Right"), StartCondition = moveRight.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction releaseLeft = new MInstruction(MInstructionFactory.GenerateID(), "release left", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Left"), StartCondition = moveLeft.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction walkInstruction2 = new MInstruction(MInstructionFactory.GenerateID(), "Walk2", MOTION_WALK) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance.GetSceneObjectByName("WalkTargetConcurrent").ID, CoSimTopic.OnStart, idleInstruction.ID + ":" + CoSimAction.EndInstruction), StartCondition = releaseLeft.ID + ":" + mmiConstants.MSimulationEvent_End// + "|" + releaseLeft.ID + ":" + mmiConstants.MSimulationEvent_End, }; MInstruction idleInstruction3 = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE) { StartCondition = walkInstruction2.ID + ":" + mmiConstants.MSimulationEvent_End, }; MInstruction reachRight2 = new MInstruction(MInstructionFactory.GenerateID(), "reach right", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTarget3R"].ID, "Hand", "Right"), StartCondition = walkInstruction2.ID + ":" + mmiConstants.MSimulationEvent_End// + "|>" + releaseLeft.ID + ":" + mmiConstants.MSimulationEvent_End, }; MInstruction carryRight2 = new MInstruction(MInstructionFactory.GenerateID(), "carry right", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject3"].ID, "Hand", "Right", "AddOffset", false.ToString()), StartCondition = reachRight2.ID + ":" + mmiConstants.MSimulationEvent_End + "+ 0.01" }; MInstruction walkInstruction3 = new MInstruction(MInstructionFactory.GenerateID(), "Walk2", MOTION_WALK) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance.GetSceneObjectByName("WalkTarget2").ID, CoSimTopic.OnStart, idleInstruction3.ID + ":" + CoSimAction.EndInstruction), StartCondition = carryRight2.ID + ":" + "PositioningFinished"// + "|" + releaseLeft.ID + ":" + mmiConstants.MSimulationEvent_End, }; MInstruction idleInstruction4 = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE) { StartCondition = walkInstruction3.ID + ":" + mmiConstants.MSimulationEvent_End, }; MSimulationState state = new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }; this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(walkInstruction1, state); this.CoSimulator.AssignInstruction(idleInstruction, state); this.CoSimulator.AssignInstruction(reachRight, state); this.CoSimulator.AssignInstruction(reachLeft, state); this.CoSimulator.AssignInstruction(carryRight, state); this.CoSimulator.AssignInstruction(carryLeft, state); this.CoSimulator.AssignInstruction(walkInstruction, state); this.CoSimulator.AssignInstruction(idleInstruction2, state); this.CoSimulator.AssignInstruction(moveRight, state); this.CoSimulator.AssignInstruction(moveLeft, state); this.CoSimulator.AssignInstruction(releaseLeft, state); this.CoSimulator.AssignInstruction(releaseRight, state); this.CoSimulator.AssignInstruction(walkInstruction2, state); this.CoSimulator.AssignInstruction(idleInstruction3, state); this.CoSimulator.AssignInstruction(reachRight2, state); this.CoSimulator.AssignInstruction(carryRight2, state); this.CoSimulator.AssignInstruction(walkInstruction3, state); this.CoSimulator.AssignInstruction(idleInstruction4, state); } if (GUI.Button(new Rect(10, 70, 220, buttonHeight), "Pickup large object")) { MInstruction walkInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Walk", MOTION_WALK) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance.GetSceneObjectByName("WalkTargetLargeObject").ID) }; MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE) { StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction reachLeft = new MInstruction(MInstructionFactory.GenerateID(), "reachLeft", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["LargeObjectGraspL"].ID, "Hand", "Left"), StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction reachRight = new MInstruction(MInstructionFactory.GenerateID(), "reachRight", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["LargeObjectGraspR"].ID, "Hand", "Right"), StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End }; carryID = MInstructionFactory.GenerateID(); MInstruction carryInstruction = new MInstruction(carryID, "carry object", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["LargeObject"].ID, "Hand", "Both", "CarryDistance", 0.4f.ToString(), "CarryTarget", UnitySceneAccess.Instance["CarryTarget"].ID), StartCondition = reachLeft.ID + ":" + mmiConstants.MSimulationEvent_End + " && " + reachRight.ID + ":" + mmiConstants.MSimulationEvent_End }; this.CoSimulator.AssignInstruction(walkInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(reachLeft, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(reachRight, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(carryInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.MSimulationEventHandler += this.CoSimulator_MSimulationEventHandler; } if (GUI.Button(new Rect(240, 70, 220, buttonHeight), "Move Large Object both handed")) { MInstruction moveInstruction = new MInstruction(MInstructionFactory.GenerateID(), "move object", MOTION_MOVE) { Properties = PropertiesCreator.Create("SubjectID", UnitySceneAccess.Instance["LargeObject"].ID, "Hand", "Both", "TargetID", UnitySceneAccess.Instance["LargeObjectPositioningTarget"].ID, "HoldDuration", 1.0f.ToString()), //Terminate the carry Action = CoSimTopic.OnStart + "->" + carryID + ":" + CoSimAction.EndInstruction, }; MInstruction releaseLeft = new MInstruction(MInstructionFactory.GenerateID(), "release object left", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Left"), StartCondition = moveInstruction.ID + ":" + mmiConstants.MSimulationEvent_End }; MInstruction releaseRight = new MInstruction(MInstructionFactory.GenerateID(), "release object right", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Right"), StartCondition = moveInstruction.ID + ":" + mmiConstants.MSimulationEvent_End }; this.CoSimulator.AssignInstruction(moveInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(releaseLeft, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(releaseRight, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(470, 70, 220, buttonHeight), "Carry Object both handed")) { carryID = MInstructionFactory.GenerateID(); MInstruction carryInstruction = new MInstruction(carryID, "carry object", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Both", "CarryTarget", UnitySceneAccess.Instance["CarryTarget"].ID), }; this.CoSimulator.AssignInstruction(carryInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(700, 70, 210, buttonHeight), "Reach Object Single")) { MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); MInstruction reachRight = new MInstruction(MInstructionFactory.GenerateID(), "reach right", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetR"].ID, "Hand", "Right"), }; this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(reachRight, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(180, 40, 160, buttonHeight), "Abort")) { this.CoSimulator.Abort(); } }
protected override void GUIBehaviorInput() { if (GUI.Button(new Rect(10, 10, 120, 50), "Idle")) { MSkeletonAccess.Iface skeletonAccess = this.avatar.GetSkeletonAccess(); skeletonAccess.SetChannelData(this.avatar.GetPosture()); MInstruction instruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); //MInstruction instruction = new MInstruction(MInstructionFactory.GenerateID(), "MMUTest", "Object/Move"); MSimulationState simstate = new MSimulationState(this.avatar.GetPosture(), this.avatar.GetPosture()); this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(instruction, simstate); } if (GUI.Button(new Rect(140, 10, 120, 50), "Walk to")) { MInstruction walkInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Walk", MOTION_WALK) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance.GetSceneObjectByName("WalkTarget").ID) }; MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE) { //Start idle after walk has been finished StartCondition = walkInstruction.ID + ":" + mmiConstants.MSimulationEvent_End //synchronization constraint similar to bml "id:End" (bml original: <bml start="id:End"/> }; this.CoSimulator.Abort(); MSimulationState currentState = new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }; //Assign walk and idle instruction this.CoSimulator.AssignInstruction(walkInstruction, currentState); this.CoSimulator.AssignInstruction(idleInstruction, currentState); this.CoSimulator.MSimulationEventHandler += this.CoSimulator_MSimulationEventHandler; } if (GUI.Button(new Rect(270, 10, 120, 50), "Reach Object")) { MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); MInstruction reachRight = new MInstruction(MInstructionFactory.GenerateID(), "reach right", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetR"].ID, "Hand", "Right", "MinDistance", 2.0.ToString()), }; //MInstruction reachLeft = new MInstruction(MInstructionFactory.GenerateID(), "reach left", "Pose/Reach") //{ // Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetL"].ID, "Hand", "Left"), //}; //this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(reachRight, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); //this.CoSimulator.AssignInstruction(reachLeft, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(400, 10, 120, 50), "Move Object")) { MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); MInstruction moveObject = new MInstruction(MInstructionFactory.GenerateID(), "move object", MOTION_MOVE) { Properties = PropertiesCreator.Create("SubjectID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Right", "TargetID", UnitySceneAccess.Instance["PositioningTarget"].ID), }; this.CoSimulator.Abort(); this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(moveObject, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(530, 10, 120, 50), "Pick-up")) { MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", MOTION_IDLE); MInstruction reachInstruction = new MInstruction(MInstructionFactory.GenerateID(), "reach", MOTION_REACH) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetR"].ID, "Hand", "Right"), }; carryID = MInstructionFactory.GenerateID(); MInstruction carryInstruction = new MInstruction(carryID, "carry object", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Right"), StartCondition = reachInstruction.ID + ":" + mmiConstants.MSimulationEvent_End + "+ 0.01" }; this.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(reachInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(carryInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.MSimulationEventHandler += this.CoSimulator_MSimulationEventHandler; } if (GUI.Button(new Rect(660, 10, 120, 50), "Carry Object")) { carryID = MInstructionFactory.GenerateID(); MInstruction carryInstruction = new MInstruction(carryID, "carry object", MOTION_CARRY) { Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspObject"].ID, "Hand", "Right")//, "CarryTarget", UnitySceneAccess.Instance["CarryTarget"].ID), }; this.CoSimulator.AssignInstruction(carryInstruction, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } if (GUI.Button(new Rect(790, 10, 120, 50), "Release Object")) { MInstruction releaseRight = new MInstruction(MInstructionFactory.GenerateID(), "release object", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Right", CoSimTopic.OnStart, carryID + ":" + CoSimAction.EndInstruction), }; MInstruction releaseLeft = new MInstruction(MInstructionFactory.GenerateID(), "release object", MOTION_RELEASE) { Properties = PropertiesCreator.Create("Hand", "Left", CoSimTopic.OnStart, carryID + ":" + CoSimAction.EndInstruction), }; this.CoSimulator.AssignInstruction(releaseRight, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); this.CoSimulator.AssignInstruction(releaseLeft, new MSimulationState() { Initial = this.avatar.GetPosture(), Current = this.avatar.GetPosture() }); } }
/// <summary> /// Returns all ik constraints which are violated (avove specified threshold) /// </summary> /// <param name="constraints"></param> /// <param name="currentPosture"></param> /// <returns></returns> private List <MConstraint> GetViolatedIKConstraints(List <MConstraint> constraints, MAvatarPostureValues currentPosture) { List <MConstraint> violated = new List <MConstraint>(); if (constraints != null) { // Apply result posture values to the skeleton skeletonAccess.SetChannelData(currentPosture); string avatarID = currentPosture.AvatarID; //Check each joint constraint foreach (MConstraint mconstraint in constraints) { if (mconstraint.JointConstraint != null) { MJointConstraint endeffectorConstraint = mconstraint.JointConstraint; //Skip if no gemometry constraint is defined if (endeffectorConstraint.GeometryConstraint == null) { continue; } double distance = 0f; double angularDistance = 0f; //Default (parent to constraint is set) if (endeffectorConstraint.GeometryConstraint.ParentToConstraint != null) { MVector3 position = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Position; MQuaternion rotation = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Rotation; switch (endeffectorConstraint.JointType) { case MJointType.LeftWrist: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist)); break; case MJointType.RightWrist: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist)); break; case MJointType.LeftBall: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle)); break; case MJointType.RightBall: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle)); break; case MJointType.PelvisCentre: distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre)); angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre)); break; } } //Legacy fallback mechanism -> Remove in future else { MTranslationConstraint positionConstraint = endeffectorConstraint.GeometryConstraint.TranslationConstraint; if (endeffectorConstraint.GeometryConstraint.TranslationConstraint != null) { switch (endeffectorConstraint.JointType) { case MJointType.LeftWrist: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist)); break; case MJointType.RightWrist: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist)); break; case MJointType.LeftBall: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle)); break; case MJointType.RightBall: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle)); break; case MJointType.PelvisCentre: distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre)); break; } } //Handle the rotation constraint if (endeffectorConstraint.GeometryConstraint.RotationConstraint != null) { MRotationConstraint rotationConstraint = endeffectorConstraint.GeometryConstraint.RotationConstraint; //Compute a quaternion based on the euler angles MQuaternion quaternion = MQuaternionExtensions.FromEuler(new MVector3(rotationConstraint.X(), rotationConstraint.Y(), rotationConstraint.Z())); if (endeffectorConstraint.GeometryConstraint.ParentObjectID == null || endeffectorConstraint.GeometryConstraint.ParentObjectID == "") { switch (endeffectorConstraint.JointType) { case MJointType.LeftWrist: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist)); break; case MJointType.RightWrist: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist)); break; case MJointType.LeftBall: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle)); break; case MJointType.RightBall: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle)); break; case MJointType.PelvisCentre: angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre)); break; } } } } //Check if solving is required if (distance > this.PositionThreshold || angularDistance > this.RotationThreshold) { violated.Add(mconstraint); } } } } return(violated); }