Esempio n. 1
0
        public static IEnumerable <Pose> ShoulderAbduction(int numberOfRepetitions = 3)
        {
            BodyTransform leftUp = new BodyTransform();

            leftUp = leftUp.Compose(JointType.ElbowLeft, new SetJointDirectionTransform(-1, 0, 0));
            leftUp = leftUp.Compose(JointType.WristLeft, new SetJointDirectionTransform(-1, 0, 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(1, 0, 0));
            rightUp = rightUp.Compose(JointType.WristRight, new SetJointDirectionTransform(1, 0, 0));
            Pose rightPose = new Pose("rightUp", rightUp);

            for (int i = 0; i < numberOfRepetitions; ++i)
            {
                yield return(leftPose);

                yield return(rightPose);
            }
        }
Esempio n. 2
0
        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);
            }
        }
Esempio n. 3
0
        public static IEnumerable <Pose> ElbowFlexion(int numberOfRepetitions = 8)
        {
            // First create the start pose and set both arms to point down
            var startTransform = BodyTransformBuilder.ArmsDownTransform();

            // The create a new pose for the rise of the left arm
            BodyTransform leftUpTransform = new BodyTransform();

            leftUpTransform = startTransform.Compose(JointType.WristLeft, new SetJointDirectionTransform(0, 1, 0));

            // And another for the rise of the right arm
            BodyTransform rightUpTransform = new BodyTransform();

            rightUpTransform = startTransform.Compose(JointType.WristRight, new SetJointDirectionTransform(0, 1, 0));

            // Add the needed
            yield return(new Pose("", startTransform));

            for (int i = 0; i < numberOfRepetitions; ++i)
            {
                yield return(new Pose("", leftUpTransform));

                yield return(new Pose("", rightUpTransform));
            }
        }
Esempio n. 4
0
        /// <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);
            }
        }
Esempio n. 5
0
        public BodyTransform Aggregate(Func <JointType, BodyTransform> func)
        {
            var result = new BodyTransform();

            foreach (var joint in this.Joints)
            {
                var that = func(joint);
                result = result.Compose(that);
            }

            return(result);
        }
Esempio n. 6
0
        public Pose(string name, BodyTransform transform, IBodyRestriction restriction)
        {
            this.Name        = name;
            this.Transform   = transform;
            this.Restriction = (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");
            }
        }
Esempio n. 7
0
        public Pose(string name, BodyTransform transform, IBodyRestriction restriction)
        {
            this.Name = name;
            this.Transform = transform;
            this.Restriction = (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");
            }
        }
Esempio n. 8
0
        public BodyTransform ComposeJointTransform(JointType jointType, JointTransform point3DTransform)
        {
            BodyTransform composed = new BodyTransform(this.JointTransforms);

            if (!this.JointTransforms.ContainsKey(jointType))
            {
                composed.JointTransforms.Add(jointType, point3DTransform);
            }
            else
            {
                composed.JointTransforms[jointType] = this.JointTransforms[jointType].Compose(point3DTransform);
            }

            return(composed);
        }
Esempio n. 9
0
        // 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);
            }
        }
Esempio n. 10
0
        public BodyTransform Compose(BodyTransform that)
        {
            BodyTransform composed = new BodyTransform();

            var jointTypes = EnumUtil.GetValues <JointType>();

            foreach (var jointType in jointTypes)
            {
                // Both BodyTransforms have a transform for this joint, so compose them
                if (this.JointTransforms.ContainsKey(jointType) &&
                    that.JointTransforms.ContainsKey(jointType))
                {
                    composed.JointTransforms.Add(jointType,
                                                 this.JointTransforms[jointType].Compose(
                                                     that.JointTransforms[jointType]));
                }

                // Only this have the transform, so add it
                else if (this.JointTransforms.ContainsKey(jointType) &&
                         !that.JointTransforms.ContainsKey(jointType))
                {
                    composed.JointTransforms.Add(jointType,
                                                 this.JointTransforms[jointType]);
                }

                // Only that have the transform
                else if (!this.JointTransforms.ContainsKey(jointType) &&
                         that.JointTransforms.ContainsKey(jointType))
                {
                    composed.JointTransforms.Add(jointType,
                                                 that.JointTransforms[jointType]);
                }

                // Do nothing in case neither this or that have the transform for the joint
            }

            return(composed);
        }
Esempio n. 11
0
        public BodyTransform Compose(BodyTransform that)
        {
            BodyTransform composed = new BodyTransform();

            var jointTypes = EnumUtil.GetValues<JointType>();

            foreach (var jointType in jointTypes)
            {
                // Both BodyTransforms have a transform for this joint, so compose them
                if (this.JointTransforms.ContainsKey(jointType) &&
                    that.JointTransforms.ContainsKey(jointType))
                {
                    composed.JointTransforms.Add(jointType,
                        this.JointTransforms[jointType].Compose(
                        that.JointTransforms[jointType]));
                }

                // Only this have the transform, so add it
                else if (this.JointTransforms.ContainsKey(jointType) &&
                    !that.JointTransforms.ContainsKey(jointType))
                {
                    composed.JointTransforms.Add(jointType,
                        this.JointTransforms[jointType]);
                }

                // Only that have the transform
                else if (!this.JointTransforms.ContainsKey(jointType) &&
                    that.JointTransforms.ContainsKey(jointType))
                {
                    composed.JointTransforms.Add(jointType,
                        that.JointTransforms[jointType]);
                }

                // Do nothing in case neither this or that have the transform for the joint
            }

            return composed;
        }
Esempio n. 12
0
        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);
            }
        }
Esempio n. 13
0
        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);
            }
        }
Esempio n. 14
0
        public static IEnumerable<Pose> ElbowFlexion(int numberOfRepetitions = 8)
        {
            // First create the start pose and set both arms to point down
            var startTransform = BodyTransformBuilder.ArmsDownTransform();

            // The create a new pose for the rise of the left arm
            BodyTransform leftUpTransform = new BodyTransform();
            leftUpTransform = startTransform.Compose(JointType.WristLeft, new SetJointDirectionTransform(0, 1, 0));

            // And another for the rise of the right arm
            BodyTransform rightUpTransform = new BodyTransform();
            rightUpTransform = startTransform.Compose(JointType.WristRight, new SetJointDirectionTransform(0, 1, 0));

            // Add the needed
            yield return new Pose("", startTransform);
            for (int i = 0; i < numberOfRepetitions; ++i)
            {
                yield return (new Pose("", leftUpTransform));
                yield return (new Pose("", rightUpTransform));
            }
        }
Esempio n. 15
0
 public void Compose(BodyTransform newTransform)
 {
     this.Transform.Compose(newTransform);
 }
Esempio n. 16
0
 public BodyTransform Compose(JointType jointType, JointTransform point3DTransform)
 {
     BodyTransform that = new BodyTransform(jointType, point3DTransform);
     return this.Compose(that);
 }
Esempio n. 17
0
 public Pose(string name, BodyTransform transform)
 {
     this.Name        = name;
     this.Transform   = transform;
     this.Restriction = new CompositeBodyRestriction(new NoBodyRestriction());
 }
Esempio n. 18
0
        public BodyTransform Compose(JointType jointType, JointTransform point3DTransform)
        {
            BodyTransform that = new BodyTransform(jointType, point3DTransform);

            return(this.Compose(that));
        }
Esempio n. 19
0
 public Pose(string name, BodyTransform transform)
 {
     this.Name = name;
     this.Transform = transform;
     this.Restriction = new CompositeBodyRestriction(new NoBodyRestriction());
 }
Esempio n. 20
0
        // 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;
            }
        }
Esempio n. 21
0
        public BodyTransform ComposeJointTransform(JointType jointType, JointTransform point3DTransform)
        {
            BodyTransform composed = new BodyTransform(this.JointTransforms);

            if (!this.JointTransforms.ContainsKey(jointType))
                composed.JointTransforms.Add(jointType, point3DTransform);
            else
                composed.JointTransforms[jointType] = this.JointTransforms[jointType].Compose(point3DTransform);

            return composed;
        }
Esempio n. 22
0
        /// <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;
            }
        }
Esempio n. 23
0
 public void Compose(BodyTransform newTransform)
 {
     this.Transform.Compose(newTransform);
 }