Exemple #1
0
        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);
        }
Exemple #2
0
        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);
        }
Exemple #3
0
        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);
        }
Exemple #4
0
        /// <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);
        }
Exemple #5
0
        /// <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;
        }
Exemple #6
0
        /// <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);
        }