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