public static IEnumerable <Pose> PassiveExternalRotation(int numberOfRepetitions = 4) { // Create the left pose and set both arms to point down CompositeBodyTransform leftTransform = new CompositeBodyTransform(); 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 CompositeBodyTransform rightTransform = new CompositeBodyTransform(); 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> ShoulderAbduction(int numberOfRepetitions = 3) { CompositeBodyTransform leftUp = new CompositeBodyTransform(); 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); CompositeBodyTransform rightUp = new CompositeBodyTransform(); 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); } }
/// <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, CompositeBodyTransform 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 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 CompositeBodyTransform leftUpTransform = new CompositeBodyTransform(); leftUpTransform = startTransform.Compose(JointType.WristLeft, new SetJointDirectionTransform(0, 1, 0)); // And another for the rise of the right arm CompositeBodyTransform rightUpTransform = new CompositeBodyTransform(); 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 Pose( string name, CompositeBodyTransform transform, IBodyRestriction restriction, CompositeDelayedStatement delayed) : this(name, transform, restriction) { this.Delayed = delayed; }
public CompositeBodyTransform Aggregate(Func <JointType, CompositeBodyTransform> func) { var result = new CompositeBodyTransform(); foreach (var joint in this.Joints) { var that = func(joint); result = result.Compose(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 CompositeBodyTransform ComposeJointTransform(JointType jointType, JointTransform point3DTransform) { CompositeBodyTransform composed = new CompositeBodyTransform(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 (degrees threshold) of a transform from a start body // 2. It is within the considered restrictions public static Z3Target GenerateTarget( CompositeBodyTransform transform, CompositeBodyRestriction restriction, Z3Body startBody, int degreesThreshold) { var z3ConstBody = Z3Body.MkZ3ConstJoints(startBody.Norms); var transformedBody = transform.Transform(startBody); var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList(); var isNearExpr = z3ConstBody.IsDegreesBetweenLessThan(transformedBody, joints, degreesThreshold); 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); } }
internal void Update(Z3Body body) { this.Restriction = new CompositeBodyRestriction(); this.Transform = new CompositeBodyTransform(); // treating rotate direction // for each definition // if a joint appears once // restrict // else // transform // the rotate direction can be a transform or a restriction foreach (var definition in this.Statements) { var jointCount = 0; foreach (var definitionTemp in this.Statements) { if (definition.JointType == definitionTemp.JointType) jointCount++; } // if for a single joint there is only one rotate direction // then the restriction should be applied to give more // freedom for the user while performing the gesture if(jointCount == 1) { this.Restriction.And(new RotateDirectionRestriction( definition.JointType, body.Joints[definition.JointType], definition.Degrees, definition.Direction)); } // else if for that joint there is more than one rotate direction // then we create a transform to guarantee that all directions // are well represented else if(jointCount > 1) { this.Transform = this.Transform.Compose( new CompositeBodyTransform(definition.JointType, new RotateJointTransform(definition.Degrees, definition.Direction))); } } }
public CompositeBodyTransform Compose(CompositeBodyTransform that) { CompositeBodyTransform composed = new CompositeBodyTransform(); 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); }
/// <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, CompositeBodyTransform 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 CompositeBodyTransform Compose(JointType jointType, JointTransform point3DTransform) { CompositeBodyTransform that = new CompositeBodyTransform(jointType, point3DTransform); return(this.Compose(that)); }
public Pose(string name, CompositeBodyTransform transform) : this(name) { this.mTransform = transform; }
public void Compose(CompositeBodyTransform newTransform) { this.mTransform = this.mTransform.Compose(newTransform); }
public void Compose(JointType jointType, JointTransform point3DTransform) { this.mTransform = this.mTransform.Compose(jointType, point3DTransform); }
public CompositeBodyTransform Compose(JointType jointType, JointTransform point3DTransform) { CompositeBodyTransform that = new CompositeBodyTransform(jointType, point3DTransform); return this.Compose(that); }
public static IEnumerable<Pose> PassiveExternalRotation(int numberOfRepetitions = 4) { // Create the left pose and set both arms to point down CompositeBodyTransform leftTransform = new CompositeBodyTransform(); 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 CompositeBodyTransform rightTransform = new CompositeBodyTransform(); 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 CompositeBodyTransform Compose(CompositeBodyTransform that) { CompositeBodyTransform composed = new CompositeBodyTransform(); 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 CompositeBodyTransform ComposeJointTransform(JointType jointType, JointTransform point3DTransform) { CompositeBodyTransform composed = new CompositeBodyTransform(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 (degrees threshold) of a transform from a start body // 2. It is within the considered restrictions public static Z3Target GenerateTarget( CompositeBodyTransform transform, CompositeBodyRestriction restriction, Z3Body startBody, int degreesThreshold) { var z3ConstBody = Z3Body.MkZ3ConstJoints(startBody.Norms); var transformedBody = transform.Transform(startBody); var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList(); var isNearExpr = z3ConstBody.IsDegreesBetweenLessThan(transformedBody, joints, degreesThreshold); 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 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 CompositeBodyTransform leftUpTransform = new CompositeBodyTransform(); leftUpTransform = startTransform.Compose(JointType.WristLeft, new SetJointDirectionTransform(0, 1, 0)); // And another for the rise of the right arm CompositeBodyTransform rightUpTransform = new CompositeBodyTransform(); 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 static IEnumerable<Pose> ShoulderFlexion(int numberOfRepetitions = 3) { CompositeBodyTransform leftUp = new CompositeBodyTransform(); 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); CompositeBodyTransform rightUp = new CompositeBodyTransform(); 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); } }