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