/// <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 Pose(string name) { this.Name = name; this.mTransform = new CompositeBodyTransform(); this.mRestriction = new CompositeBodyRestriction(); this.Delayed = new CompositeDelayedStatement(); }
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); }
public CompositeBodyRestriction Aggregate(Func <JointType, IBodyRestriction> func) { var result = new CompositeBodyRestriction(); foreach (var joint in this.Joints) { var that = func(joint); result = result.And(that); } return(result); }
public Pose(string name, CompositeBodyTransform transform, IBodyRestriction restriction) : this(name, transform) { this.mRestriction = (restriction is CompositeBodyRestriction) ? (CompositeBodyRestriction)restriction : new CompositeBodyRestriction((SimpleBodyRestriction)restriction); // Check if restriction allows transform if (!this.IsTransformAcceptedByRestriction()) { throw new ArgumentException("The restriction does not allow the transform.", "restriction"); } }
public CompositeBodyRestriction And(IBodyRestriction that) { CompositeBodyRestriction result = new CompositeBodyRestriction(); result.Restrictions.AddRange(this.Restrictions); if (that is CompositeBodyRestriction) { result.Restrictions.AddRange(((CompositeBodyRestriction)that).Restrictions); } else { result.Restrictions.Add(((SimpleBodyRestriction)that)); } 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; } }
/// <summary> /// This is a basic safety check to make sure we don't break any bones. /// </summary> /// <returns>True if safe</returns> public static CompositeBodyRestriction DefaultSafetyRestriction() { var result = new CompositeBodyRestriction(); //int inclinationThreshold = 45; //// Head //// Make sure neck is not inclinated beyond the threshold //var head = new SimpleBodyRestriction(body => //{ // Z3Point3D up = new Z3Point3D(0, 1, 0); // BoolExpr expr1 = body.Joints[JointType.Head].IsDegreesBetweenLessThan(up, inclinationThreshold); // BoolExpr expr2 = body.Joints[JointType.Neck].IsDegreesBetweenLessThan(up, inclinationThreshold); // BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); // return expr; //}, //body => { return 1.0; }); //result.And(head); //// Spine //// Make sure spine is not inclinated beyond the threshold //var spine = new SimpleBodyRestriction(body => //{ // Z3Point3D up = new Z3Point3D(0, 1, 0); // BoolExpr expr1 = body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(up, inclinationThreshold); // BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsDegreesBetweenLessThan(up, inclinationThreshold); // BoolExpr expr3 = // body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan( // body.Joints[JointType.SpineShoulder], inclinationThreshold); // BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3); // return expr; //}, //body => { return 1.0; }); //result.And(spine); //// Shoulders //// Make sure shoulders are not bent //var shoulders = new SimpleBodyRestriction(body => //{ // BoolExpr expr = // body.Joints[JointType.SpineMid].IsDegreesBetweenGreaterThan( // body.Joints[JointType.SpineShoulder], 120); // return expr; //}, //body => { return 1.0; }); //result.And(shoulders); // Elbows // Make sure elbows are not behind the back // And also that they are not on the top/back sub-space var elbows1 = new SimpleBodyRestriction(body => { var joint1Position = body.GetJointZ3Position(JointType.ElbowRight); var joint2Position = body.GetJointZ3Position(JointType.ShoulderRight); BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd( Z3.Context.MkLt(joint1Position.Z, joint2Position.Z), Z3.Context.MkGt(joint1Position.Y, joint2Position.Y))); return(expr); }, body => { return(1.0); }, "don't put your right elbow behind you and above your shoulders.\n"); var elbows2 = new SimpleBodyRestriction(body => { var joint1Position = body.GetJointZ3Position(JointType.ElbowLeft); var joint2Position = body.GetJointZ3Position(JointType.ShoulderLeft); BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd( Z3.Context.MkLt(joint1Position.Z, joint2Position.Z), Z3.Context.MkGt(joint1Position.Y, joint2Position.Y))); return(expr); }, body => { return(1.0); }, "don't put your left elbow behind you and above your shoulders.\n"); var elbows3 = new SimpleBodyRestriction(body => { var joint1Position = body.GetJointZ3Position(JointType.ElbowRight); var joint2Position = body.GetJointZ3Position(JointType.ShoulderRight); BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd( Z3.Context.MkLt(joint1Position.Z, joint2Position.Z), Z3.Context.MkLt(joint1Position.X, joint2Position.X))); return(expr); }, body => { return(1.0); }, "don't put your right elbow behind you crossing your back.\n"); var elbows4 = new SimpleBodyRestriction(body => { var joint1Position = body.GetJointZ3Position(JointType.ElbowLeft); var joint2Position = body.GetJointZ3Position(JointType.ShoulderLeft); BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd( Z3.Context.MkLt(joint1Position.Z, joint2Position.Z), Z3.Context.MkGt(joint1Position.X, joint2Position.X))); return(expr); }, body => { return(1.0); }, "don't put your right elbow behind you crossing your back.\n"); result.And(elbows1); result.And(elbows2); result.And(elbows3); result.And(elbows4); //// Wrists //// Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows //// unless elbows are up or wrists are directed to torso //// TODO //// Hips //// Make sure hips are aligned with the shoulders or at lest within the range //var hips = new SimpleBodyRestriction(body => //{ // Z3Point3D shouldersSum = // body.Joints[JointType.ShoulderLeft].GetInverted() + // body.Joints[JointType.ShoulderRight]; // Z3Point3D hipsSum = // body.Joints[JointType.HipLeft].GetInverted() + // body.Joints[JointType.HipRight]; // BoolExpr expr = shouldersSum.IsDegreesBetweenLessThan(hipsSum, 45); // return expr; //}, //body => { return 1.0; }); //result.And(hips); // Legs // Make sure knees do not bent towards your back var knees1 = new SimpleBodyRestriction(body => { var joint1Position = body.GetJointZ3Position(JointType.KneeLeft); var joint2Position = body.GetJointZ3Position(JointType.HipLeft); var joint3Position = body.GetJointZ3Position(JointType.AnkleLeft); BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd( Z3.Context.MkLt(joint1Position.Z, joint2Position.Z), Z3.Context.MkLt(joint1Position.Z, joint3Position.Z))); return(expr); }, body => { return(1.0); }, "don't bend your left knee towards your back.\n"); var knees2 = new SimpleBodyRestriction(body => { var joint1Position = body.GetJointZ3Position(JointType.KneeRight); var joint2Position = body.GetJointZ3Position(JointType.HipRight); var joint3Position = body.GetJointZ3Position(JointType.AnkleRight); BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd( Z3.Context.MkLt(joint1Position.Z, joint2Position.Z), Z3.Context.MkLt(joint1Position.Z, joint3Position.Z))); return(expr); }, body => { return(1.0); }, "don't bend your right knee towards your back.\n"); result.And(knees1); result.And(knees2); // Ankles // Make sure ankles are not inclinated up more than knees // unless ankles are pointing back // TODO return(result); }
/// <summary> /// This is a basic safety check to make sure we don't break any bones. /// </summary> /// <returns>True if safe</returns> public static IBodyRestriction DefaultSafetyRestriction() { var result = new CompositeBodyRestriction(); int inclinationThreshold = 45; // Head // Make sure neck is not inclinated beyond the threshold var head = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.Head].IsDegreesBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.Neck].IsDegreesBetweenLessThan(up, inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return expr; }, body => { return 1.0; }); result.And(head); // Spine // Make sure spine is not inclinated beyond the threshold var spine = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsDegreesBetweenLessThan(up, inclinationThreshold); BoolExpr expr3 = body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan( body.Joints[JointType.SpineShoulder], inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3); return expr; }, body => { return 1.0; }); result.And(spine); // Shoulders // Make sure shoulders are not bent var shoulders = new SimpleBodyRestriction(body => { BoolExpr expr = body.Joints[JointType.SpineMid].IsDegreesBetweenGreaterThan( body.Joints[JointType.SpineShoulder], 120); return expr; }, body => { return 1.0; }); result.And(shoulders); // Elbows // Make sure elbows are not behind the back // And also that they are not on the top/back sub-space var elbows = new SimpleBodyRestriction(body => { BoolExpr exprRight1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkLt(body.Joints[JointType.ElbowRight].X, Z3Math.Zero))); BoolExpr exprRight2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowRight].Y, Z3Math.Zero))); BoolExpr exprLeft1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].X, Z3Math.Zero))); BoolExpr exprLeft2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].Y, Z3Math.Zero))); BoolExpr expr = Z3.Context.MkAnd(exprLeft1, exprLeft2, exprRight1, exprRight2); return expr; }, body => { return 1.0; }); result.And(elbows); // Wrists // Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows // unless elbows are up or wrists are directed to torso // TODO // Hips // Make sure hips are aligned with the shoulders or at lest within the range var hips = new SimpleBodyRestriction(body => { Z3Point3D shouldersSum = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight]; Z3Point3D hipsSum = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight]; BoolExpr expr = shouldersSum.IsDegreesBetweenLessThan(hipsSum, 45); return expr; }, body => { return 1.0; }); result.And(hips); // Legs // Make sure legs are not higher than threshold var legs = new SimpleBodyRestriction(body => { BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.KneeLeft].Y, Z3Math.Real(0)); BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.KneeRight].Y, Z3Math.Real(0)); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return expr; }, body => { return 1.0; }); result.And(legs); // Ankles // Make sure ankles are not inclinated up more than knees // unless ankles are pointing back // TODO return result; }
/// <summary> /// This is a basic safety check to make sure we don't break any bones. /// </summary> /// <returns>True if safe</returns> public static IBodyRestriction DefaultSafetyRestriction() { var result = new CompositeBodyRestriction(); int inclinationThreshold = 45; // Head // Make sure neck is not inclinated beyond the threshold var head = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.Head].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.Neck].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return(expr); }); result.And(head); // Spine // Make sure spine is not inclinated beyond the threshold var spine = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr3 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan( body.Joints[JointType.SpineShoulder], inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3); return(expr); }); result.And(spine); // Shoulders // Make sure shoulders are not bent var shoulders = new SimpleBodyRestriction(body => { BoolExpr expr = body.Joints[JointType.SpineMid].IsAngleBetweenGreaterThan( body.Joints[JointType.SpineShoulder], 120); return(expr); }); result.And(shoulders); // Elbows // Make sure elbows are not behind the back // And also that they are not on the top/back sub-space var elbows = new SimpleBodyRestriction(body => { BoolExpr exprRight1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkLt(body.Joints[JointType.ElbowRight].X, Z3Math.Zero))); BoolExpr exprRight2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowRight].Y, Z3Math.Zero))); BoolExpr exprLeft1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].X, Z3Math.Zero))); BoolExpr exprLeft2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].Y, Z3Math.Zero))); BoolExpr expr = Z3.Context.MkAnd(exprLeft1, exprLeft2, exprRight1, exprRight2); return(expr); }); result.And(elbows); // Wrists // Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows // unless elbows are up or wrists are directed to torso // TODO // Hips // Make sure hips are aligned with the shoulders or at lest within the range var hips = new SimpleBodyRestriction(body => { Z3Point3D shouldersSum = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight]; Z3Point3D hipsSum = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight]; BoolExpr expr = shouldersSum.IsAngleBetweenLessThan(hipsSum, 45); return(expr); }); result.And(hips); // Legs // Make sure legs are not higher than threshold var legs = new SimpleBodyRestriction(body => { BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.KneeLeft].Y, Z3Math.Real(0)); BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.KneeRight].Y, Z3Math.Real(0)); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return(expr); }); result.And(legs); // Ankles // Make sure ankles are not inclinated up more than knees // unless ankles are pointing back // TODO return(result); }
public CompositeBodyRestriction And(IBodyRestriction that) { CompositeBodyRestriction result = new CompositeBodyRestriction(); result.Restrictions.AddRange(this.Restrictions); if (that is CompositeBodyRestriction) { result.Restrictions.AddRange(((CompositeBodyRestriction)that).Restrictions); } else { result.Restrictions.Add(((SimpleBodyRestriction)that)); } return result; }