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 PoseSafetyException(string message, Pose pose, Z3Body body) : base(message) { this.Body = body; this.Pose = pose; }
public void AddPose(Pose pose) { this.DeclaredPoses.Add(pose); }
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); } }
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); } }
/// <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; } }