Пример #1
0
        /// <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);
            }
        }
Пример #2
0
        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);
        }
Пример #3
0
        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);
        }
Пример #4
0
        // 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);
            }
        }
Пример #5
0
        /// <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;
            }
        }
Пример #6
0
        // 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;
            }
        }