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); }
/// <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); }