public static void SyncExecuteBehavior(string contextServer, MotionBasedBehavior behavior) { try { if (!string.IsNullOrEmpty(contextServer) && behavior != null) { // Execute Init actions only once if (!behavior.InitActionsComplete) { SyncExecuteBehavior(contextServer, behavior.InitActions); behavior.InitActionsComplete = true; } // Evaluate Expression string humanInfo = GetHumanInfo(contextServer, behavior.Id); var count = GestureTriggerCount(humanInfo, behavior.Trigger, behavior.ConfidenceLevel); // Increment count once count = count + 1; if (!string.IsNullOrEmpty(humanInfo)) { //Log.Info(humanInfo); } if (!behavior.CyclicActionsComplete) { foreach (var behaviorItem in behavior.RobotActions) { var behaviorInfo = behaviorItem.Clone() as BehaviorInfo; // Before executing each action get the latest information about the human humanInfo = GetHumanInfo(contextServer, behavior.Id); if (!string.IsNullOrEmpty(humanInfo)) { //Log.Info(humanInfo); } if (behaviorInfo != null) { Log.InfoFormat("Behavior Detail: {0}", behaviorInfo.ToString()); if (behaviorInfo.BehaviorName == "Say Expressively") { var msg = string.Empty; var arg = string.Empty; var msgDict = behaviorInfo.Parameters.TryGetAndReturn("msg") as Dictionary<string, object>; var argDict = behaviorInfo.Parameters.TryGetAndReturn("arg") as Dictionary<string, object>; if (msgDict != null && argDict != null) { msg = msgDict.TryGetAndReturn("value") as string; arg = argDict.TryGetAndReturn("value") as string; } Log.InfoFormat("Msg: {0}, Arg: {1}", msg, arg); if (!string.IsNullOrEmpty(msg) && !string.IsNullOrEmpty(arg) && !string.IsNullOrEmpty(behavior.TriggerCountVariable)) { var newMsg = string.Empty; if (arg == behavior.TriggerCountVariable) { newMsg = string.Format(msg, count); if (msgDict != null) msgDict["value"] = newMsg; //behaviorInfo.Parameters["msg"] = newMsg; } Log.InfoFormat("New Msg: {0}, Count : {1}", newMsg, count); } } if (behaviorInfo.BehaviorName == "Move To") { var humanDict = behaviorInfo.Parameters.TryGetAndReturn("human") as Dictionary<string, object>; var rotDict = behaviorInfo.Parameters.TryGetAndReturn("rotation") as Dictionary<string, object>; var transDict = behaviorInfo.Parameters.TryGetAndReturn("translation") as Dictionary<string, object>; var distDict = behaviorInfo.Parameters.TryGetAndReturn("dist") as Dictionary<string, object>; float isHuman, isRotation, isTranslation; float.TryParse(humanDict.TryGetAndReturn("value").ToString(), out isHuman); float.TryParse(transDict.TryGetAndReturn("value").ToString(), out isTranslation); float.TryParse(rotDict.TryGetAndReturn("value").ToString(), out isRotation); if (isHuman > 0 && isRotation > 0) { // Get Robot String var robotString = GetRobotInfo(contextServer); // Get World String var worldFrame = GetWorldFrame(contextServer); Matrix3x3 robotRot; Matrix3x3 worldRot; Matrix3x3 humanRot; Quaternion robotQ; Quaternion worldQ; Quaternion humanQ; Vector3 robotTrans; Vector3 worldTrans; Vector3 humanTrans; // Get the transformation from the JSON strings GetHumanPoseFromJson(humanInfo, out humanTrans, out humanRot, out humanQ); GetLocalizationFromRobotJson(robotString, out robotTrans, out robotRot, out robotQ); GetWorldFrameMatrix(worldFrame, out worldTrans, out worldRot, out worldQ); var humanMat = GetMatrixFromPose(humanQ, new Vector3(0, 0, 0)); var worldMat = GetMatrixFromPose(worldQ, new Vector3(0, 0, 0)); var robotMat = GetMatrixFromPose(robotQ, new Vector3(0, 0, 0)); worldMat.Invert(); var hWorld = worldMat*humanMat; var hRobot = worldMat*robotMat; var hWorldQ = Quaternion.RotationMatrix(hWorld); var rWorldQ = Quaternion.RotationMatrix(hRobot); var hWorldYaw = GetHeading(hWorldQ); var rWorldYaw = GetHeading(rWorldQ); // Compute the displacement of Robot and Human wrt to world frame worldRot.Invert(); var hDisp = new Vector3(Vector3.Dot(worldRot.Column1, humanTrans), Vector3.Dot(worldRot.Column2, humanTrans), Vector3.Dot(worldRot.Column3, humanTrans)) - new Vector3(Vector3.Dot(worldRot.Column1, worldTrans), Vector3.Dot(worldRot.Column2, worldTrans), Vector3.Dot(worldRot.Column3, worldTrans)); var rDisp = new Vector3(Vector3.Dot(worldRot.Column1, robotTrans), Vector3.Dot(worldRot.Column2, robotTrans), Vector3.Dot(worldRot.Column3, robotTrans)) - new Vector3(Vector3.Dot(worldRot.Column1, worldTrans), Vector3.Dot(worldRot.Column2, worldTrans), Vector3.Dot(worldRot.Column3, worldTrans)); // We get the unit vector from Robot pointing towards human var toHumanVec = new Vector2(hDisp.X, hDisp.Y) - new Vector2(rDisp.X, rDisp.Y); toHumanVec.Normalize(); // Next we would like to align the X-Axis of the robot with that of this unit vector var yUnit = Vector2.UnitY; var xUnit = Vector2.UnitX; // Now we find the angle of rotation needed to do this alignment //var angle = Math.Acos(Vector2.Dot(toHumanVec, yUnit)); var angle = GetRelativeAngle(xUnit, toHumanVec); if (rWorldYaw > 0) angle += rWorldYaw; else angle -= rWorldYaw; // Update the values of X,Y,Theta var xDict = behaviorInfo.Parameters.TryGetAndReturn("x") as Dictionary<string, object>; var yDict = behaviorInfo.Parameters.TryGetAndReturn("y") as Dictionary<string, object>; var thetaDict = behaviorInfo.Parameters.TryGetAndReturn("theta") as Dictionary<string, object>; Log.InfoFormat("Move To Rotation Angle : {0}", MathUtil.RadiansToDegrees((float) angle)); if (xDict != null) { xDict["value"] = 0.0f; } if (yDict != null) { yDict["value"] = 0.0f; } if (thetaDict != null) { thetaDict["value"] = angle; } } else if (isHuman > 0 && isTranslation > 0) { // Get Robot String var robotString = GetRobotInfo(contextServer); // Get World String var worldFrame = GetWorldFrame(contextServer); Matrix3x3 robotRot; Matrix3x3 worldRot; Matrix3x3 humanRot; Quaternion robotQ; Quaternion worldQ; Quaternion humanQ; Vector3 robotTrans; Vector3 worldTrans; Vector3 humanTrans; // Get the transformation from the JSON strings GetHumanPoseFromJson(humanInfo, out humanTrans, out humanRot, out humanQ); GetLocalizationFromRobotJson(robotString, out robotTrans, out robotRot, out robotQ); GetWorldFrameMatrix(worldFrame, out worldTrans, out worldRot, out worldQ); var humanMat = GetMatrixFromPose(humanQ, new Vector3(0, 0, 0)); var worldMat = GetMatrixFromPose(worldQ, new Vector3(0, 0, 0)); var robotMat = GetMatrixFromPose(robotQ, new Vector3(0, 0, 0)); worldMat.Invert(); var hWorld = worldMat*humanMat; var hRobot = worldMat*robotMat; var hWorldQ = Quaternion.RotationMatrix(hWorld); var rWorldQ = Quaternion.RotationMatrix(hRobot); var hWorldYaw = GetHeading(hWorldQ); var rWorldYaw = GetHeading(rWorldQ); // Compute the displacement of Robot and Human wrt to world frame worldRot.Invert(); var hDisp = new Vector3(Vector3.Dot(worldRot.Column1, humanTrans), Vector3.Dot(worldRot.Column2, humanTrans), Vector3.Dot(worldRot.Column3, humanTrans)) - new Vector3(Vector3.Dot(worldRot.Column1, worldTrans), Vector3.Dot(worldRot.Column2, worldTrans), Vector3.Dot(worldRot.Column3, worldTrans)); var rDisp = new Vector3(Vector3.Dot(worldRot.Column1, robotTrans), Vector3.Dot(worldRot.Column2, robotTrans), Vector3.Dot(worldRot.Column3, robotTrans)) - new Vector3(Vector3.Dot(worldRot.Column1, worldTrans), Vector3.Dot(worldRot.Column2, worldTrans), Vector3.Dot(worldRot.Column3, worldTrans)); // We get the unit vector from Robot pointing towards human var toHumanVec = new Vector2(hDisp.X, hDisp.Y) - new Vector2(rDisp.X, rDisp.Y); var distance = toHumanVec.Length(); var threshold = distance; if (distDict != null) { float.TryParse(humanDict.TryGetAndReturn("value").ToString(), out threshold); } var residual = 0.0f; if (distance > threshold) { residual = distance - threshold; } // Unit Vector toHumanVec.Normalize(); //var residualVector = toHumanVec*residual; var residualVector = Vector2.UnitX*residual; // Update the values of X,Y,Theta var xDict = behaviorInfo.Parameters.TryGetAndReturn("x") as Dictionary<string, object>; var yDict = behaviorInfo.Parameters.TryGetAndReturn("y") as Dictionary<string, object>; var thetaDict = behaviorInfo.Parameters.TryGetAndReturn("theta") as Dictionary<string, object>; Log.InfoFormat("Move To X : {0}, Y:{1}", residualVector.X, residualVector.Y); if (xDict != null) { xDict["value"] = 0.75*residualVector.X; } if (yDict != null) { yDict["value"] = 0.75*residualVector.Y; } if (thetaDict != null) { thetaDict["value"] = 0.0f; } } } SyncExecuteBehavior(behaviorInfo); } } } if (behavior.BehaviorType == BehaviorType.Behavior) { if (behavior.ExecutionLifetime == BehaviorExecutionLifetime.once) { behavior.CyclicActionsComplete = true; } if (behavior.RobotActions.Count == 0) { behavior.CyclicActionsComplete = true; } if (behavior.ExecutionLifetime == BehaviorExecutionLifetime.until) { if (!string.IsNullOrEmpty(behavior.ExecutionEvalExpression) && !string.IsNullOrEmpty(behavior.TriggerCountVariable)) { // Evaluating Termination condition var expression = new Expression(behavior.ExecutionEvalExpression); expression.Parameters.Add(behavior.TriggerCountVariable, count + 1); var result = expression.Evaluate(); bool complete; if (bool.TryParse(result.ToString(), out complete)) { behavior.CyclicActionsComplete = !complete; } } } } // Execute exit actions only once if (behavior.CyclicActionsComplete && !behavior.ExitActionsComplete) { SyncExecuteBehavior(contextServer, behavior.ExitActions); behavior.ExitActionsComplete = true; } } } catch (Exception ex) { Log.ErrorFormat("Simple behavior error : {0}", ex.Message); Log.ErrorFormat("Simple behavior stack_trace : {0}", ex.StackTrace); } }
private static bool ScheduleBehaviorExecution(IScheduler scheduler, MotionBasedBehavior behavior, string triggerName, IDictionary<string, object> props) { bool ret = false; Console.WriteLine(@"Behavior Execution for trigger : {0}", triggerName); if (behavior != null && behavior.RobotActions.Count > 0 && scheduler != null) { var moduleName = behavior.RobotActions[0].ModuleName; var jobKey = JobKey.Create(string.Format("Task_{0}", moduleName), moduleName); if (!scheduler.CheckExists(jobKey)) { IJobDetail detail = JobBuilder.Create<MotionBehaviorTask>() .WithIdentity(jobKey) .Build(); detail.JobDataMap.Add("MotionBasedBehavior", behavior); var contextServer = GetValue(props, "ContextServer", "tcp://*****:*****@"New job about to be scheduled Job : {0}, Module : {1}", jobKey.Name, jobKey.Group); ret = true; } } return ret; }
public static MotionBasedBehavior GetMotionBasedBehavior(XElement block) { if (block == null) { return null; } var motionBehavior = new MotionBasedBehavior {Guid = Guid.NewGuid()}; var fields = LinqXmlUtil.GetElementsAnyNS(block, "field"); foreach (var field in fields) { var nameAttribute = field.Attribute("name"); if (nameAttribute != null) { if (nameAttribute.Value == "behavior_name") { motionBehavior.Name = field.Value; } else if (nameAttribute.Value == "priorities") { BehaviorExecutionPriority priority = BehaviorExecutionPriority.normal; Enum.TryParse(field.Value, out priority); motionBehavior.Priority = priority; } else if (nameAttribute.Value == "trigger_count") { motionBehavior.TriggerCountVariable = field.Value; } else if (nameAttribute.Value == "execution") { BehaviorExecutionLifetime execution = BehaviorExecutionLifetime.forever; Enum.TryParse(field.Value, out execution); motionBehavior.ExecutionLifetime = execution; if (execution == BehaviorExecutionLifetime.until) { var mutationBlock = LinqXmlUtil.GetElementsAnyNS(block, "mutation").ToList(); if (mutationBlock.Count > 0) { var mutation = mutationBlock[0]; var runLogicAttr = mutation.Attribute("run_logic"); if (runLogicAttr != null) { motionBehavior.ExecutionEvalExpression = runLogicAttr.Value; } } } } else if (nameAttribute.Value == "triggers") { motionBehavior.Trigger = field.Value; } else if (nameAttribute.Value == "confidence_levels") { int confidence = 90; int.TryParse(field.Value, out confidence); motionBehavior.ConfidenceLevel = confidence; } } } var statements = LinqXmlUtil.GetElementsAnyNS(block, "statement").ToArray(); for (int s = 0; s < statements.Length; s++) { var currStatement = statements[s]; IList<BehaviorInfo> tempList = null; if (currStatement.Attribute("name").Value == "INIT_DO") { tempList = motionBehavior.InitActions; } if (currStatement.Attribute("name").Value == "DO") { tempList = motionBehavior.RobotActions; } if (currStatement.Attribute("name").Value == "EXIT_DO") { tempList = motionBehavior.ExitActions; } if (tempList != null) { var statementBlocks = LinqXmlUtil.GetElementsAnyNS(currStatement, "block").ToArray(); for (int i = 0; i < statementBlocks.Length; i++) { var tempBlock = statementBlocks[i]; var behaviorInfo = GetBehaviorInfo(tempBlock); if (behaviorInfo != null) { foreach (var info in behaviorInfo) { tempList.Add(info); } var nextBlock = LinqXmlUtil.GetElementsAnyNS(tempBlock, "next"); while (nextBlock != null && nextBlock.Any()) { var childBlock = LinqXmlUtil.GetElementsAnyNS(nextBlock, "block").FirstOrDefault(); if (childBlock != null) { behaviorInfo = GetBehaviorInfo(childBlock); foreach (var info in behaviorInfo) { tempList.Add(info); } Console.WriteLine(childBlock); nextBlock = LinqXmlUtil.GetElementsAnyNS(childBlock, "next"); Console.WriteLine(nextBlock); } else { nextBlock = null; } } } } } } return motionBehavior; }