public Pose(
string name,
CompositeBodyTransform transform,
IBodyRestriction restriction,
CompositeDelayedStatement delayed)
: this(name, transform, restriction)
{
this.Delayed = delayed;
}
public Pose(
string name,
CompositeBodyTransform transform,
IBodyRestriction restriction,
CompositeDelayedStatement delayed) :
this(name, transform, restriction)
{
this.Delayed = delayed;
}
public Pose(string name, CompositeBodyTransform transform, IBodyRestriction restriction)
: this(name, transform)
{
this.mRestriction = (restriction is CompositeBodyRestriction) ?
(CompositeBodyRestriction)restriction :
new CompositeBodyRestriction((SimpleBodyRestriction)restriction);
// Check if restriction allows transform
if (!this.IsTransformAcceptedByRestriction())
{
throw new ArgumentException("The restriction does not allow the transform.", "restriction");
}
}
public Pose(string name, CompositeBodyTransform transform, IBodyRestriction restriction)
: this(name, transform)
{
this.mRestriction = (restriction is CompositeBodyRestriction) ?
(CompositeBodyRestriction)restriction :
new CompositeBodyRestriction((SimpleBodyRestriction)restriction);
// Check if restriction allows transform
if (!this.IsTransformAcceptedByRestriction())
{
throw new ArgumentException("The restriction does not allow the transform.", "restriction");
}
}
/// <summary>
/// Generates a witness body tha matches the restrictions that
/// are passed in.
/// </summary>
/// <param name="bodyRestriction"></param>
/// <returns></returns>
public static Z3Body GenerateWitness(IBodyRestriction bodyRestriction)
{
var body = Z3Body.MkZ3Const();
var expr = bodyRestriction.Evaluate(body);
var checkResult = CheckStatus(expr);
var witness = CreateBodyWitness(
body,
checkResult.Model,
bodyRestriction.GetJointTypes(),
JointTypeHelper.CreateDefaultZ3Body());
return(witness);
}
public CompositeBodyRestriction And(IBodyRestriction that)
{
CompositeBodyRestriction result = new CompositeBodyRestriction();
result.Restrictions.AddRange(this.Restrictions);
if (that is CompositeBodyRestriction)
{
result.Restrictions.AddRange(((CompositeBodyRestriction)that).Restrictions);
}
else
{
result.Restrictions.Add(((SimpleBodyRestriction)that));
}
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);
}
}
public void Compose(IBodyRestriction newRestriction)
{
this.mRestriction.And(newRestriction);
}
public void Compose(IBodyRestriction newRestriction)
{
this.mRestriction.And(newRestriction);
}
/// <summary>
/// Generates a witness body tha matches the restrictions that
/// are passed in.
/// </summary>
/// <param name="bodyRestriction"></param>
/// <returns></returns>
public static Z3Body GenerateWitness(IBodyRestriction bodyRestriction)
{
var body = Z3Body.MkZ3Const();
var expr = bodyRestriction.Evaluate(body);
var checkResult = CheckStatus(expr);
var witness = CreateBodyWitness(
body,
checkResult.Model,
bodyRestriction.GetJointTypes(),
JointTypeHelper.CreateDefaultZ3Body());
return witness;
}
internal Wrapper(IBodyRestriction restriction)
{
this.value = restriction;
}
public CompositeBodyRestriction And(IBodyRestriction that)
{
CompositeBodyRestriction result = new CompositeBodyRestriction();
result.Restrictions.AddRange(this.Restrictions);
if (that is CompositeBodyRestriction)
{
result.Restrictions.AddRange(((CompositeBodyRestriction)that).Restrictions);
}
else
{
result.Restrictions.Add(((SimpleBodyRestriction)that));
}
return result;
}
