Ejemplo n.º 1
0
        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;
            }
        }
Ejemplo n.º 2
0
 public PoseSafetyException(string message, Pose pose, Z3Body body)
     : base(message)
 {
     this.Body = body;
     this.Pose = pose;
 }
Ejemplo n.º 3
0
 public void AddPose(Pose pose)
 {
     this.DeclaredPoses.Add(pose);
 }
Ejemplo n.º 4
0
        public static IEnumerable<Pose> ShoulderFlexion(int numberOfRepetitions = 3)
        {
            BodyTransform leftUp = new BodyTransform();
            leftUp = leftUp.Compose(JointType.ElbowLeft, new SetJointDirectionTransform(0, 1, 0));
            leftUp = leftUp.Compose(JointType.WristLeft, new SetJointDirectionTransform(0, 1, 0));
            leftUp = leftUp.Compose(JointType.ElbowRight, new SetJointDirectionTransform(0, -1, 0));
            leftUp = leftUp.Compose(JointType.WristRight, new SetJointDirectionTransform(0, -1, 0));
            Pose leftPose = new Pose("leftUp", leftUp);

            BodyTransform rightUp = new BodyTransform();
            rightUp = rightUp.Compose(JointType.ElbowLeft, new SetJointDirectionTransform(0, -1, 0));
            rightUp = rightUp.Compose(JointType.WristLeft, new SetJointDirectionTransform(0, -1, 0));
            rightUp = rightUp.Compose(JointType.ElbowRight, new SetJointDirectionTransform(0, 1, 0));
            rightUp = rightUp.Compose(JointType.WristRight, new SetJointDirectionTransform(0, 1, 0));
            Pose rightPose = new Pose("rightUp", rightUp);

            for (int i = 0; i < numberOfRepetitions; ++i)
            {
                yield return (leftPose);
                yield return (rightPose);
            }
        }
Ejemplo n.º 5
0
        public static IEnumerable<Pose> PassiveExternalRotation(int numberOfRepetitions = 4)
        {
            // Create the left pose and set both arms to point down
            BodyTransform leftTransform = new BodyTransform();
            leftTransform = leftTransform.Compose(JointType.ElbowLeft, new SetJointDirectionTransform(0, -1, 0));
            leftTransform = leftTransform.Compose(JointType.WristLeft, new SetJointDirectionTransform(-0.7, 0, 0.7));
            leftTransform = leftTransform.Compose(JointType.ElbowRight, new SetJointDirectionTransform(0, -1, 0));
            leftTransform = leftTransform.Compose(JointType.WristRight, new SetJointDirectionTransform(0, 0, 1));
            Pose leftPose = new Pose("DontTwistHipRestriction-l", leftTransform, BodyRestrictionBuilder.DontTwistHipRestriction(5));

            // Create the left pose and set both arms to point down
            BodyTransform rightTransform = new BodyTransform();
            rightTransform = rightTransform.Compose(JointType.ElbowLeft, new SetJointDirectionTransform(0, -1, 0));
            rightTransform = rightTransform.Compose(JointType.WristLeft, new SetJointDirectionTransform(0, 0, 1));
            rightTransform = rightTransform.Compose(JointType.ElbowRight, new SetJointDirectionTransform(0, -1, 0));
            rightTransform = rightTransform.Compose(JointType.WristRight, new SetJointDirectionTransform(0.7, 0, 0.7));
            Pose rightPose = new Pose("DontTwistHipRestriction-r", rightTransform, BodyRestrictionBuilder.DontTwistHipRestriction(5));

            for (int i = 0; i < numberOfRepetitions; ++i)
            {
                yield return (leftPose);
                yield return (rightPose);
            }
        }
Ejemplo n.º 6
0
        /// <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;
            }
        }