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 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 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 void Compose(CompositeBodyTransform newTransform)
{
this.mTransform = this.mTransform.Compose(newTransform);
}
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;
}
public Pose(string name, CompositeBodyTransform transform) : this(name)
{
this.mTransform = transform;
}
// 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);
}
}
