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;
}
