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