public static bool IsInternallyValid(Pose pose, out string firstBadStatement)
{
bool result = true;
firstBadStatement = "";
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
var restrictions = pose.Restriction.Restrictions;
var composite = new CompositeBodyRestriction();
foreach (var restriction in restrictions)
{
composite.And(restriction);
BoolExpr transformedRestricted = composite.Evaluate(transformed);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(transformedRestricted);
if (solverResult.Status == Status.UNSATISFIABLE)
{
firstBadStatement = restriction.ToString();
result = false;
break;
}
}
return(result);
}
public static bool IsWithinDefaultSafetyRestrictions(Pose pose, out string firstBadStatement)
{
bool result = true;
firstBadStatement = "";
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
var restrictions = Safety.DefaultSafetyRestriction().Restrictions;
var composite = new CompositeBodyRestriction();
foreach (var restriction in restrictions)
{
composite.And(restriction);
BoolExpr inputSafe = composite.Evaluate(transformed);
BoolExpr expr = Z3.Context.MkAnd(transformedRestricted, inputSafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.UNSATISFIABLE)
{
firstBadStatement = ((SimpleBodyRestriction)restriction).Message;
result = false;
break;
}
}
return(result);
}
public bool IsTransformAcceptedByRestriction()
{
Z3Body body = Z3Body.MkZ3Const();
Z3Body transformedBody = this.Transform.Transform(body);
BoolExpr expr = this.Restriction.Evaluate(transformedBody);
SolverCheckResult checkResult = Z3AnalysisInterface.CheckStatus(expr);
return(checkResult.Status != Status.UNSATISFIABLE);
}
public Z3Target CalcNearestTargetBody(Z3Body startBody)
{
// Create binary search to look for nearest body
int numSteps = 6;
int angleThreshold = 90;
int angleIncrement = 90;
Z3Target target = null;
for (int i = 0; i < numSteps; ++i)
{
// Ask for a witness which is within the range
target = Z3AnalysisInterface.GenerateTarget(
this.Transform,
this.Restriction,
startBody,
angleThreshold);
// Update angle threshold
angleIncrement /= 2;
if (target != null)
{
angleThreshold -= angleIncrement;
}
else
{
angleThreshold += angleIncrement;
}
}
// If target still null it probably means Z3 was unable to solve the restrictions
// This way we generate a target using onlye the transforms
if (target == null)
{
target = new Z3Target();
target.Body = this.Transform.Transform(startBody);
target.TransformedJoints = this.Transform.GetJointTypes();
}
// If target still null assing a new body as an error proof policy
if (target == null)
{
target = new Z3Target();
target.Body = startBody;
}
return(target);
}
/// <summary>
/// Checks if the pose is within default safety
/// restrictions when the transform and restrictions
/// are applied.
/// </summary>
/// <returns>True if it's safe</returns>
public static bool IsWithinSafetyRestrictions(Pose pose, out Z3Body witness)
{
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
IBodyRestriction safe = Safety.DefaultSafetyRestriction();
BoolExpr inputSafe = safe.Evaluate(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
// Try to generate a unsafe witness using the transform
BoolExpr outputUnsafe = Z3.Context.MkNot(safe.Evaluate(transformed));
// Put together all expressions and search for unsat
BoolExpr expr = Z3.Context.MkAnd(inputSafe, transformedRestricted, outputUnsafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.SATISFIABLE)
{
//Z3Body
witness =
Z3AnalysisInterface.CreateBodyWitness(
transformed,
solverResult.Model,
pose.GetAllJointTypes(),
JointTypeHelper.CreateDefaultZ3Body());
return(false);
}
else if (solverResult.Status == Status.UNKNOWN)
{
//Z3Body
witness = JointTypeHelper.CreateDefaultZ3Body();
return(false);
}
else
{
Contract.Assert(solverResult.Status == Status.UNSATISFIABLE);
witness = null;
return(true);
}
}
public static bool IsInternallyValid(Pose pose)
{
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
// We have to check that the pose is within the default safety restriction
IBodyRestriction safe = Safety.DefaultSafetyRestriction();
BoolExpr inputSafe = safe.Evaluate(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
// Try to generate a safe witness using the transform
BoolExpr outputSafe = safe.Evaluate(transformed);
// Check to see if the transform is not satisfiable -- if so, then it is not internally valid
BoolExpr expr = Z3.Context.MkAnd(inputSafe, transformedRestricted, outputSafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.SATISFIABLE)
{
// We can create a witness - therefore the pose must be valid
return(true);
}
else if (solverResult.Status == Status.UNKNOWN)
{
return(false);
}
else
{
Contract.Assert(solverResult.Status == Status.UNSATISFIABLE);
// Pose is not internally valid and as a result there can be no witness created
return(false);
}
}
//public ArithExpr CalcApproximateDistanceBetweenNormalized(Z3Point3D that)
//{
// Z3Point3D thisNormalized = this.GetApproximateNormalized();
// Z3Point3D thatNormalized = that.GetApproximateNormalized();
// ArithExpr result = thisNormalized.CalcApproximateDistance(thatNormalized);
// return result;
//}
// Assumes vectors are normalized
public BoolExpr IsAngleBetweenLessThan(Z3Point3D that, int angleThreshold)
{
double distanceThreshold = TrigonometryHelper.GetDistance(angleThreshold);
ArithExpr distance = this.CalcApproximateDistance(that);
BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold));
// TODO remove this, test code
SolverCheckResult checkResult = Z3AnalysisInterface.CheckStatus(result);
if (checkResult.Status == Status.SATISFIABLE)
{
var joint = new Z3Point3D(
checkResult.Model.Evaluate(this.X, true) as ArithExpr,
checkResult.Model.Evaluate(this.Y, true) as ArithExpr,
checkResult.Model.Evaluate(this.Z, true) as ArithExpr);
var distanceSolvedExpr = checkResult.Model.Evaluate(distance, true) as ArithExpr;
var distanceValue = Z3Math.GetRealValue(distanceSolvedExpr);
}
// end of test code
return(result);
}
/// <summary>
/// Check for pairwise ambiguity
/// </summary>
/// <returns></returns>
public static bool HasPairwiseConflicts(App app, out List <PairwiseConflictException> allExceptions, out List <AmbiguityTime> ambiguityTimes, int precision = 15)
{
List <Gesture> conflictGestures = (List <Gesture>)app.Gestures;
bool result = false;
allExceptions = new List <PairwiseConflictException>();
ambiguityTimes = new List <AmbiguityTime>();
for (int i = 0; i < conflictGestures.Count - 1; i++)
{
for (int j = i + 1; j < conflictGestures.Count; j++)
{
var gesture1 = conflictGestures[i];
var gesture2 = conflictGestures[j];
// Create const input body
var input = Z3Body.MkZ3Const();
var allJoints = EnumUtil.GetValues <JointType>().ToList();
Z3Body transformed1 = null;
Z3Body transformed2 = null;
BoolExpr evaluation1 = Z3Math.True;
BoolExpr evaluation2 = Z3Math.True;
// Pass input through both gestures
gesture1.FinalResult(input, out transformed1, out evaluation1);
gesture2.FinalResult(input, out transformed2, out evaluation2);
// Check if it is possible that both outputs are equals
// This is performed by checking if is possible that all expressions are true
var isNearExpr = transformed1.IsNearerThan(
transformed2, precision);
var expr = Z3.Context.MkAnd(isNearExpr, evaluation1, evaluation2);
// If we are dumping Z3 constraints, then convert the expression to a SMTLIB formatted string
// and dump it to disk. Note this is not included in the timing for the individual pair of gestures,
// but it _is_ included in the timing for the app overall.
if (DumpZ3Constraints)
{
string exprName = String.Join("X", gesture1.Name, gesture2.Name);
string exprPath = exprName + ".smt2";
Z3AnalysisInterface.WriteExprToDisk(expr, exprName, exprPath);
}
// Check if we have an ambiguity conflict. Record the time it takes.
var stopwatch = new Stopwatch();
stopwatch.Start();
var checkResult = Z3AnalysisInterface.CheckStatus(expr);
stopwatch.Stop();
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = Z3AnalysisInterface.CreateBodyWitness(
input,
checkResult.Model,
allJoints,
JointTypeHelper.CreateDefaultZ3Body());
var exception = new PairwiseConflictException(
"Conflict detected between pair of gestures",
gesture1,
gesture2,
witness);
allExceptions.Add(exception);
result = true;
}
// TODO the witness here should exist, this case shouldn't be needed
else if (checkResult.Status == Status.UNKNOWN)
{
var witness = JointTypeHelper.CreateDefaultZ3Body();
var exception = new PairwiseConflictException(
"Conflict detected between pair of gestures, the reason is unknown",
gesture1,
gesture2,
witness);
allExceptions.Add(exception);
result = true;
}
ambiguityTimes.Add(new AmbiguityTime
{
Gesture1 = gesture1,
Gesture2 = gesture2,
Time = stopwatch.ElapsedMilliseconds,
Conflict = result,
CheckResult = checkResult
});
}
}
return(result);
}
/// <summary>
/// Check for pairwise ambiguity
/// </summary>
/// <returns></returns>
public static bool HasPairwiseConflicts(
App app,
out string errorMessage,
out List <AmbiguityTime> ambiguityTimes,
int precision = 15)
{
errorMessage = "";
List <Gesture> gestures = (List <Gesture>)app.Gestures;
bool result = false;
ambiguityTimes = new List <AmbiguityTime>();
for (int i = 0; i < gestures.Count - 1; i++)
{
for (int j = i + 1; j < gestures.Count; j++)
{
var gesture1 = gestures[i];
var gesture2 = gestures[j];
var gesture1CurrentStep = 0;
var gesture2CurrentStep = 0;
var gesture1LastConflictedStep = 0;
var gesture2LastConflictedStep = 0;
var gesture1NumSteps = gesture1.Steps.Count;
var gesture2NumSteps = gesture2.Steps.Count;
var advanceOn = 1;
var conflictCount = 0;
while (gesture1CurrentStep < gesture1NumSteps && gesture2CurrentStep < gesture2NumSteps)
{
var expr1 = ExpressionFromCurrentStep(gesture1, gesture1CurrentStep);
var expr2 = ExpressionFromCurrentStep(gesture2, gesture2CurrentStep);
var expr = Z3.Context.MkAnd(expr1, expr2);
var solver = Z3AnalysisInterface.CheckStatus(expr);
if (solver.Status == Status.SATISFIABLE)
{
gesture1LastConflictedStep = gesture1CurrentStep;
gesture2LastConflictedStep = gesture2CurrentStep;
++gesture1CurrentStep;
++gesture2CurrentStep;
++conflictCount;
advanceOn = 1;
}
else if (solver.Status == Status.UNSATISFIABLE)
{
if (gesture1CurrentStep == gesture1NumSteps - 1)
{
gesture1CurrentStep = gesture1LastConflictedStep;
advanceOn = 2;
}
else if (gesture2CurrentStep == gesture2NumSteps - 1)
{
conflictCount = 0;
}
if (advanceOn == 1)
{
++gesture1CurrentStep;
}
else if (advanceOn == 2)
{
++gesture2CurrentStep;
}
}
}
var conflicted = (conflictCount > 0);
if (conflicted)
{
result = true;
var full = false;
if (conflictCount >= Math.Min(gesture1NumSteps, gesture2NumSteps))
{
full = true;
}
if (full)
{
errorMessage += "\tFull conflict found between gestures " + gesture1.Name + " and " + gesture2.Name + ",\n";
errorMessage += "\twhich means that one gesture may be fully executed whithin the other.\n";
}
else
{
errorMessage += "\tPartial conflict found between gestures " + gesture1.Name + " and " + gesture2.Name + ",\n";
errorMessage += "\twhich means that one gesture may be started before the other one ended.\n";
errorMessage += "\tIn total " + conflictCount + " sequential steps conflicted between those gestures.\n";
}
}
}
}
return(result);
}
