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); } }
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); } }
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)); } }
/// <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 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); }
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"); } }
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); }
// 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); } }
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); }
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; }
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); } }
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); } }
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)); } }
public void Compose(BodyTransform newTransform) { this.Transform.Compose(newTransform); }
public BodyTransform Compose(JointType jointType, JointTransform point3DTransform) { BodyTransform that = new BodyTransform(jointType, point3DTransform); return this.Compose(that); }
public Pose(string name, BodyTransform transform) { this.Name = name; this.Transform = transform; this.Restriction = new CompositeBodyRestriction(new NoBodyRestriction()); }
public BodyTransform Compose(JointType jointType, JointTransform point3DTransform) { BodyTransform that = new BodyTransform(jointType, point3DTransform); return(this.Compose(that)); }
// 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; } }
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; }
/// <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; } }