/// <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 CompositeBodyTransform CrossoverArmStretchTransform(JointSide sideOfStretchedArm) { JointSide oppositeSide; JointTransform stretchDirectionTransform; // First set the stretch direction and the opposite side (helper arm side) if (sideOfStretchedArm == JointSide.Left) { oppositeSide = JointSide.Right; stretchDirectionTransform = new SetJointDirectionTransform(1, 0, 0); } else { oppositeSide = JointSide.Left; stretchDirectionTransform = new SetJointDirectionTransform(-1, 0, 0); } // Get all joint types for both stretched and support arm JointType stretchedElbow = JointTypeHelper.GetSidedJointType(SidedJointName.Elbow, sideOfStretchedArm); JointType stretchedWrist = JointTypeHelper.GetSidedJointType(SidedJointName.Wrist, sideOfStretchedArm); JointType supportElbow = JointTypeHelper.GetSidedJointType(SidedJointName.Elbow, oppositeSide); JointType supportWrist = JointTypeHelper.GetSidedJointType(SidedJointName.Wrist, oppositeSide); // Apply the transforms in both arms return(new CompositeBodyTransform() .Compose(stretchedElbow, stretchDirectionTransform) .Compose(stretchedWrist, stretchDirectionTransform) .Compose(supportElbow, new SetJointDirectionTransform(0, -1, 0)) .Compose(supportWrist, new SetJointDirectionTransform(0, 1, 0))); }
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false) : base(body => { JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide); Z3Point3D joint1Position = body.GetJointPosition(jointType); Z3Point3D joint2Position = body.GetJointPosition(sidedHandType); BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold); if (dont) { expr = Z3.Context.MkNot(expr); } return(expr); }, jointType, JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide)) { this.JointType = jointType; this.HandSide = handSide; if (dont) { isNegated = true; } else { isNegated = false; } }
internal static Z3Body CreateDefaultZ3Body() { var joints = JointTypeHelper.CreateSyntheticJoints(); var norms = JointTypeHelper.CreateSyntheticNorms(); var vectors = JointTypeHelper.CreateSyntheticVectors(); var positions = JointTypeHelper.CreateSyntheticPositions(); return(new Z3Body(positions, vectors, joints, norms)); }
private static void SumWithFatherPosition( Dictionary <Microsoft.Kinect.JointType, Vector3D> jointVectors, List <PreposeGestures.JointType> targetCalculatedJoints, Microsoft.Kinect.JointType jointType) { if (targetCalculatedJoints.Contains((PreposeGestures.JointType)jointType)) { jointVectors[jointType] += jointVectors[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather((PreposeGestures.JointType)jointType)]; } }
public override string ToString() { var result = string.Format("align your {0} and your {1}", JointTypeHelper.JointToString(this.JointType1), JointTypeHelper.JointToString(this.JointType2)); if (this.isNegated) { result = "don't " + result; } return(result); }
public override string ToString() { var result = string.Format("touch your {0} with your {1} hand", JointTypeHelper.JointToString(this.JointType), EnumUtil.GetDescription <JointSide>(this.HandSide).ToLower()); if (this.isNegated) { result = "don't " + result; } return(result); }
public override string ToString() { var result = string.Format("put your {0} {1} {2}", JointTypeHelper.JointToString(this.JointType1), EnumUtil.GetDescription <RelativeDirection>(this.Direction).ToLower(), JointTypeHelper.JointToString(this.JointType2)); if (this.isNegated) { result = "don't " + result; } return(result); }
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false) : base(body => { JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide); Z3Point3D joint1Position = body.GetJointZ3Position(jointType); Z3Point3D joint2Position = body.GetJointZ3Position(sidedHandType); BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold); if (dont) { expr = Z3.Context.MkNot(expr); } return(expr); }, body => { var stopwatch = new Stopwatch(); stopwatch.Start(); var sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide); var p1 = body.Positions[jointType]; var p2 = body.Positions[sidedHandType]; var distance = Math.Max(0.00000001, p1.DistanceTo(p2)); var percentage = Math.Min(1.0, distanceThreshold / distance); if (dont) { percentage = Math.Min(1.0, distance / distanceThreshold); } //Console.WriteLine("tch " + stopwatch.ElapsedTicks); return(percentage); }, jointType, JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide)) { this.JointType = jointType; this.HandSide = handSide; if (dont) { isNegated = true; } else { isNegated = false; } }
//public Dictionary<JointType, double> GrabDistances(Z3Body that, List<JointType> joints) //{ // var distanceExprs = this.GrabDistancePoint3Ds(that, joints); // var result = new Dictionary<JointType, double>(); // foreach (var jointType in joints) // result.Add(jointType, Z3Math.GetRealValue(distanceExprs[jointType])); // return result; //} //public List<JointType> GetList public Z3Point3D GetJointZ3Position(JointType jointType) { Z3Point3D result = new Z3Point3D(0, 0, 0); JointType currentJointType = jointType; // SpineBase is the root of the coordinate system while (currentJointType != JointType.SpineBase) { result = result + (this.Joints[currentJointType] * this.Norms[currentJointType]); currentJointType = JointTypeHelper.GetFather(currentJointType); } return(result); }
/// <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); }
/// <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); } }
private static void AddZ3JointToVectors3D( Z3Body targetBody, IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints, Dictionary <Microsoft.Kinect.JointType, Vector3D> jointVectors, PreposeGestures.JointType jointType) { var vector3D = new Vector3D( targetBody.Joints[jointType].GetXValue(), targetBody.Joints[jointType].GetYValue(), -targetBody.Joints[jointType].GetZValue()); //var norm = Z3Math.GetRealValue(targetBody.Norms[jointType]); var norm = new Vector3D( baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.X, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Y, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Z).Length; vector3D = vector3D * norm; jointVectors.Add((Microsoft.Kinect.JointType)jointType, vector3D); }
public override string ToString() { return(string.Format("rotate your {0} {1} degrees {2}", JointTypeHelper.JointToString(this.JointType), this.Degrees, EnumUtil.GetDescription <Direction>(this.Direction).ToLower())); }
/// <summary> /// Check for pairwise ambiguity /// </summary> /// <returns></returns> public static bool HasPairwiseConflicts(App app, out List <PairwiseConflictException> allExceptions, out List <AmbiguityTime> ambiguityTimes, int precision = 15) { List <Gesture> conflictGestures = (List <Gesture>)app.Gestures; bool result = false; allExceptions = new List <PairwiseConflictException>(); ambiguityTimes = new List <AmbiguityTime>(); for (int i = 0; i < conflictGestures.Count - 1; i++) { for (int j = i + 1; j < conflictGestures.Count; j++) { var gesture1 = conflictGestures[i]; var gesture2 = conflictGestures[j]; // Create const input body var input = Z3Body.MkZ3Const(); var allJoints = EnumUtil.GetValues <JointType>().ToList(); Z3Body transformed1 = null; Z3Body transformed2 = null; BoolExpr evaluation1 = Z3Math.True; BoolExpr evaluation2 = Z3Math.True; // Pass input through both gestures gesture1.FinalResult(input, out transformed1, out evaluation1); gesture2.FinalResult(input, out transformed2, out evaluation2); // Check if it is possible that both outputs are equals // This is performed by checking if is possible that all expressions are true var isNearExpr = transformed1.IsNearerThan( transformed2, precision); var expr = Z3.Context.MkAnd(isNearExpr, evaluation1, evaluation2); // If we are dumping Z3 constraints, then convert the expression to a SMTLIB formatted string // and dump it to disk. Note this is not included in the timing for the individual pair of gestures, // but it _is_ included in the timing for the app overall. if (DumpZ3Constraints) { string exprName = String.Join("X", gesture1.Name, gesture2.Name); string exprPath = exprName + ".smt2"; Z3AnalysisInterface.WriteExprToDisk(expr, exprName, exprPath); } // Check if we have an ambiguity conflict. Record the time it takes. var stopwatch = new Stopwatch(); stopwatch.Start(); var checkResult = Z3AnalysisInterface.CheckStatus(expr); stopwatch.Stop(); if (checkResult.Status == Status.SATISFIABLE) { var witness = Z3AnalysisInterface.CreateBodyWitness( input, checkResult.Model, allJoints, JointTypeHelper.CreateDefaultZ3Body()); var exception = new PairwiseConflictException( "Conflict detected between pair of gestures", gesture1, gesture2, witness); allExceptions.Add(exception); result = true; } // TODO the witness here should exist, this case shouldn't be needed else if (checkResult.Status == Status.UNKNOWN) { var witness = JointTypeHelper.CreateDefaultZ3Body(); var exception = new PairwiseConflictException( "Conflict detected between pair of gestures, the reason is unknown", gesture1, gesture2, witness); allExceptions.Add(exception); result = true; } ambiguityTimes.Add(new AmbiguityTime { Gesture1 = gesture1, Gesture2 = gesture2, Time = stopwatch.ElapsedMilliseconds, Conflict = result, CheckResult = checkResult }); } } return(result); }
internal static Z3Body CreateDefaultZ3Body() { return(new Z3Body(JointTypeHelper.CreateSyntheticJoints(), JointTypeHelper.CreateSyntheticNorms())); }