/// <summary>
/// Produces a witness body given input restrictions, output
/// restrictions and a transform.
/// </summary>
/// <param name="inputBodyRestriction"></param>
/// <param name="bodyTransform"></param>
/// <param name="outputBodyRestriction"></param>
/// <returns></returns>
public static Z3Body GenerateWitness(
CompositeBodyRestriction inputBodyRestriction,
BodyTransform bodyTransform,
CompositeBodyRestriction outputBodyRestriction)
{
var body = Z3Body.MkZ3Const();
var transformedBody = bodyTransform.Transform(body);
var expr1 = inputBodyRestriction.Evaluate(body);
var expr2 = outputBodyRestriction.Evaluate(transformedBody);
var expr = Z3.Context.MkAnd(expr1, expr2);
var evaluatedJoints =
JointTypeHelper.MergeJointTypeLists(
inputBodyRestriction.GetJointTypes(),
bodyTransform.GetJointTypes(),
outputBodyRestriction.GetJointTypes());
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
body,
checkResult.Model,
evaluatedJoints,
JointTypeHelper.CreateDefaultZ3Body());
return(witness);
}
else
{
return(null);
}
}
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 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);
}
// Generates a body witness which satisfies two conditions
// 1. It is within a range (angle threshold) of a transform from a start body
// 2. It is within the considered restrictions
public static Z3Target GenerateTarget(
BodyTransform transform,
CompositeBodyRestriction restriction,
Z3Body startBody,
int angleThreshold)
{
var z3ConstBody = Z3Body.MkZ3Const();
z3ConstBody.Norms = startBody.Norms;
var transformedBody = transform.Transform(startBody);
var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList();
var isNearExpr = z3ConstBody.IsAngleBetweenLessThan(transformedBody, joints, angleThreshold);
var evaluateExpr = restriction.Evaluate(z3ConstBody);
//var normsExpr = BodyRestrictionBuilder.EvaluateNorms(startBody, z3ConstBody);
//var expr = Z3.Context.MkAnd(isNearExpr, evaluateExpr, normsExpr);
//var expr = Z3.Context.MkAnd(evaluateExpr, normsExpr);
var expr = Z3.Context.MkAnd(evaluateExpr, isNearExpr);
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
z3ConstBody,
checkResult.Model,
restriction.GetJointTypes(),
startBody);
var target = new Z3Target();
target.Body = witness;
target.RestrictedJoints = restriction.GetJointTypes();
target.TransformedJoints = transform.GetJointTypes();
foreach (var jointType in transform.GetJointTypes())
{
target.Body.Joints[jointType] = transformedBody.Joints[jointType];
}
return(target);
}
else
{
return(null);
}
}
/// <summary>
/// Produces a witness body given input restrictions, output
/// restrictions and a transform.
/// </summary>
/// <param name="inputBodyRestriction"></param>
/// <param name="bodyTransform"></param>
/// <param name="outputBodyRestriction"></param>
/// <returns></returns>
public static Z3Body GenerateWitness(
CompositeBodyRestriction inputBodyRestriction,
BodyTransform bodyTransform,
CompositeBodyRestriction outputBodyRestriction)
{
var body = Z3Body.MkZ3Const();
var transformedBody = bodyTransform.Transform(body);
var expr1 = inputBodyRestriction.Evaluate(body);
var expr2 = outputBodyRestriction.Evaluate(transformedBody);
var expr = Z3.Context.MkAnd(expr1, expr2);
var evaluatedJoints =
JointTypeHelper.MergeJointTypeLists(
inputBodyRestriction.GetJointTypes(),
bodyTransform.GetJointTypes(),
outputBodyRestriction.GetJointTypes());
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
body,
checkResult.Model,
evaluatedJoints,
JointTypeHelper.CreateDefaultZ3Body());
return witness;
}
else
{
return null;
}
}
// Generates a body witness which satisfies two conditions
// 1. It is within a range (angle threshold) of a transform from a start body
// 2. It is within the considered restrictions
public static Z3Target GenerateTarget(
BodyTransform transform,
CompositeBodyRestriction restriction,
Z3Body startBody,
int angleThreshold)
{
var z3ConstBody = Z3Body.MkZ3Const();
z3ConstBody.Norms = startBody.Norms;
var transformedBody = transform.Transform(startBody);
var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList();
var isNearExpr = z3ConstBody.IsAngleBetweenLessThan(transformedBody, joints, angleThreshold);
var evaluateExpr = restriction.Evaluate(z3ConstBody);
//var normsExpr = BodyRestrictionBuilder.EvaluateNorms(startBody, z3ConstBody);
//var expr = Z3.Context.MkAnd(isNearExpr, evaluateExpr, normsExpr);
//var expr = Z3.Context.MkAnd(evaluateExpr, normsExpr);
var expr = Z3.Context.MkAnd(evaluateExpr, isNearExpr);
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
z3ConstBody,
checkResult.Model,
restriction.GetJointTypes(),
startBody);
var target = new Z3Target();
target.Body = witness;
target.RestrictedJoints = restriction.GetJointTypes();
target.TransformedJoints = transform.GetJointTypes();
foreach (var jointType in transform.GetJointTypes())
target.Body.Joints[jointType] = transformedBody.Joints[jointType];
return target;
}
else
{
return null;
}
}