public static IBodyRestriction DontTwistHipRestriction(int degreesThreshold = 15)
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
var result = new SimpleBodyRestriction(body =>
{
// Shoulders, hips and feet must be aligned
Z3Point3D leftToRightShoulderVec = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight];
Z3Point3D leftToRightHipVec = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight];
Z3Point3D leftAnklePosition =
body.Joints[JointType.HipLeft] +
body.Joints[JointType.KneeLeft] +
body.Joints[JointType.AnkleLeft];
Z3Point3D rightAnklePosition =
body.Joints[JointType.HipRight] +
body.Joints[JointType.KneeRight] +
body.Joints[JointType.AnkleRight];
Z3Point3D leftToRightAnkleVec = rightAnklePosition - leftAnklePosition;
BoolExpr expr1 = leftToRightShoulderVec.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold);
BoolExpr expr2 = leftToRightShoulderVec.IsDegreesBetweenLessThan(leftToRightAnkleVec, degreesThreshold);
BoolExpr expr3 = leftToRightAnkleVec.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return(expr);
},
body => { return(1.0); });
return(result);
}
public static IBodyRestriction DontTwistHipRestriction(int angleThreshold = 15)
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
var result = new SimpleBodyRestriction(body =>
{
// Shoulders, hips and feet must be aligned
Z3Point3D leftToRightShoulderVec = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight];
Z3Point3D leftToRightHipVec = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight];
Z3Point3D leftAnklePosition =
body.Joints[JointType.HipLeft] +
body.Joints[JointType.KneeLeft] +
body.Joints[JointType.AnkleLeft];
Z3Point3D rightAnklePosition =
body.Joints[JointType.HipRight] +
body.Joints[JointType.KneeRight] +
body.Joints[JointType.AnkleRight];
Z3Point3D leftToRightAnkleVec = rightAnklePosition - leftAnklePosition;
BoolExpr expr1 = leftToRightShoulderVec.IsAngleBetweenLessThan(leftToRightHipVec, angleThreshold);
BoolExpr expr2 = leftToRightShoulderVec.IsAngleBetweenLessThan(leftToRightAnkleVec, angleThreshold);
BoolExpr expr3 = leftToRightAnkleVec.IsAngleBetweenLessThan(leftToRightHipVec, angleThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return expr;
});
return result;
}
public static IBodyRestriction DistributeWeightRestriction(int angleThreshold = 15)
{
var result = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
Z3Point3D legLeft = body.Joints[JointType.KneeLeft] + body.Joints[JointType.AnkleLeft];
Z3Point3D legRight = body.Joints[JointType.KneeRight] + body.Joints[JointType.AnkleRight];
Z3Point3D legsAverage = (legLeft + legRight) / 2;
BoolExpr expr = legsAverage.IsAngleBetweenLessThan(up, angleThreshold);
return(expr);
});
return(result);
}
public static IBodyRestriction DistributeWeightRestriction(int angleThreshold = 15)
{
var result = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
Z3Point3D legLeft = body.Joints[JointType.KneeLeft] + body.Joints[JointType.AnkleLeft];
Z3Point3D legRight = body.Joints[JointType.KneeRight] + body.Joints[JointType.AnkleRight];
Z3Point3D legsAverage = (legLeft + legRight) / 2;
BoolExpr expr = legsAverage.IsAngleBetweenLessThan(up, angleThreshold);
return expr;
});
return result;
}
// Therapy restrictions
public static IBodyRestriction StraightPostureRestriction(int angleThreshold = 15)
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
double distanceThreshold = TrigonometryHelper.GetSine(angleThreshold);
var result = new SimpleBodyRestriction(body =>
{
BoolExpr expr = body.Joints[JointType.SpineMid].IsNearerThan(up, distanceThreshold);
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.SpineShoulder].IsNearerThan(up, distanceThreshold));
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Neck].IsNearerThan(up, distanceThreshold));
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Head].IsNearerThan(up, distanceThreshold));
return(expr);
});
return(result);
}
public static IBodyRestriction ShouldersRelaxedRestriction(int angleThreshold = -5)
{
double maxY = TrigonometryHelper.GetSine(angleThreshold);
var result = new SimpleBodyRestriction
(body =>
{
// Check if both shoulders have a lower Y than maxY
BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.ShoulderLeft].Y, Z3Math.Real(maxY));
BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.ShoulderRight].Y, Z3Math.Real(maxY));
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return(expr);
});
return(result);
}
public static IBodyRestriction ShouldersRelaxedRestriction(int angleThreshold = -5)
{
double maxY = TrigonometryHelper.GetSine(angleThreshold);
var result = new SimpleBodyRestriction
(body =>
{
// Check if both shoulders have a lower Y than maxY
BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.ShoulderLeft].Y, Z3Math.Real(maxY));
BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.ShoulderRight].Y, Z3Math.Real(maxY));
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return expr;
});
return result;
}
public void And(SimpleBodyRestriction that)
{
this.Restrictions.Add(that);
}
public CompositeBodyRestriction(SimpleBodyRestriction soleBodyRestriction)
{
this.Restrictions = new List <SimpleBodyRestriction>();
this.Restrictions.Add(soleBodyRestriction);
}
public static IBodyRestriction TestBodyRestriction()
{
var result = new SimpleBodyRestriction(body =>
{
double distanceThreshold = 0.4;
ArithExpr distanceThresholdSquared = Z3Math.Real(distanceThreshold * distanceThreshold);
ArithExpr x1 = body.Joints[JointType.SpineMid].X;
ArithExpr y1 = body.Joints[JointType.SpineMid].Y;
ArithExpr z1 = body.Joints[JointType.SpineMid].Z;
ArithExpr x2 = Z3Math.Zero;
ArithExpr y2 = Z3Math.One;
ArithExpr z2 = Z3Math.Zero;
//ArithExpr x2 = body.Joints[JointType.SpineBase].X;
//ArithExpr y2 = body.Joints[JointType.SpineBase].Y;
//ArithExpr z2 = body.Joints[JointType.SpineBase].Z;
var xExpr = Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2));
var yExpr = Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2));
var zExpr = Z3Math.Mul(Z3Math.Sub(z1, z2), Z3Math.Sub(z1, z2));
var distanceSquared = Z3Math.Add(xExpr, yExpr, zExpr);
// This is runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), y2));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y2, y2)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y1)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Mul(y1, y2)));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2)));
// But this does not
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Add(y1, y2), Z3Math.Add(y1, y2)));
// This fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(0))));
// This does not
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1, 7))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 6)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 6))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 3)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 3))));
// This does not
// asserting: (x1 - 0)² + (y1 - 1/2)² <= 0.4²
// which is x1² + y1² - y/2 - y/2 + 1/4 <= 0.16
// which is x1² + y1² - y + 0.25 <= 0.16
// which is x1² + y1² - y <= -0.09
// if x1 = 0 and y1 = 0.1 we would have a solution
// 0 + 0.01 - 0.1 <= 0.09
// 0.09 <= 0.09
// but z3 is taking too long to solve this.
var addExpr = Z3Math.Add(
Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 2)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 2))));
// But this is not (Z3 just stops while solving it)
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2)));
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1)));
//BoolExpr expr = Z3.Context.MkLt(distanceSquared, distanceThresholdSquared);
BoolExpr expr = Z3.Context.MkLe(addExpr, distanceThresholdSquared);
// Why it complains about this Greater-Than case?
//BoolExpr expr = Z3.Context.MkGt(xExpr, distanceThresholdSquared);
return(expr);
},
body => { return(1.0); });
return(result);
}
public CompositeBodyRestriction(SimpleBodyRestriction soleBodyRestriction)
{
this.Restrictions = new List<SimpleBodyRestriction>();
this.Restrictions.Add(soleBodyRestriction);
}
/// <summary>
/// This is a basic safety check to make sure we don't break any bones.
/// </summary>
/// <returns>True if safe</returns>
public static CompositeBodyRestriction DefaultSafetyRestriction()
{
var result = new CompositeBodyRestriction();
//int inclinationThreshold = 45;
//// Head
//// Make sure neck is not inclinated beyond the threshold
//var head = new SimpleBodyRestriction(body =>
//{
// Z3Point3D up = new Z3Point3D(0, 1, 0);
// BoolExpr expr1 = body.Joints[JointType.Head].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr2 = body.Joints[JointType.Neck].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
// return expr;
//},
//body => { return 1.0; });
//result.And(head);
//// Spine
//// Make sure spine is not inclinated beyond the threshold
//var spine = new SimpleBodyRestriction(body =>
//{
// Z3Point3D up = new Z3Point3D(0, 1, 0);
// BoolExpr expr1 = body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr3 =
// body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(
// body.Joints[JointType.SpineShoulder], inclinationThreshold);
// BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
// return expr;
//},
//body => { return 1.0; });
//result.And(spine);
//// Shoulders
//// Make sure shoulders are not bent
//var shoulders = new SimpleBodyRestriction(body =>
//{
// BoolExpr expr =
// body.Joints[JointType.SpineMid].IsDegreesBetweenGreaterThan(
// body.Joints[JointType.SpineShoulder], 120);
// return expr;
//},
//body => { return 1.0; });
//result.And(shoulders);
// Elbows
// Make sure elbows are not behind the back
// And also that they are not on the top/back sub-space
var elbows1 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowRight);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderRight);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkGt(joint1Position.Y, joint2Position.Y)));
return(expr);
}, body => { return(1.0); },
"don't put your right elbow behind you and above your shoulders.\n");
var elbows2 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowLeft);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderLeft);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkGt(joint1Position.Y, joint2Position.Y)));
return(expr);
}, body => { return(1.0); },
"don't put your left elbow behind you and above your shoulders.\n");
var elbows3 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowRight);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderRight);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkLt(joint1Position.X, joint2Position.X)));
return(expr);
}, body => { return(1.0); },
"don't put your right elbow behind you crossing your back.\n");
var elbows4 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowLeft);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderLeft);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkGt(joint1Position.X, joint2Position.X)));
return(expr);
}, body => { return(1.0); },
"don't put your right elbow behind you crossing your back.\n");
result.And(elbows1);
result.And(elbows2);
result.And(elbows3);
result.And(elbows4);
//// Wrists
//// Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows
//// unless elbows are up or wrists are directed to torso
//// TODO
//// Hips
//// Make sure hips are aligned with the shoulders or at lest within the range
//var hips = new SimpleBodyRestriction(body =>
//{
// Z3Point3D shouldersSum =
// body.Joints[JointType.ShoulderLeft].GetInverted() +
// body.Joints[JointType.ShoulderRight];
// Z3Point3D hipsSum =
// body.Joints[JointType.HipLeft].GetInverted() +
// body.Joints[JointType.HipRight];
// BoolExpr expr = shouldersSum.IsDegreesBetweenLessThan(hipsSum, 45);
// return expr;
//},
//body => { return 1.0; });
//result.And(hips);
// Legs
// Make sure knees do not bent towards your back
var knees1 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.KneeLeft);
var joint2Position = body.GetJointZ3Position(JointType.HipLeft);
var joint3Position = body.GetJointZ3Position(JointType.AnkleLeft);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkLt(joint1Position.Z, joint3Position.Z)));
return(expr);
}, body => { return(1.0); },
"don't bend your left knee towards your back.\n");
var knees2 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.KneeRight);
var joint2Position = body.GetJointZ3Position(JointType.HipRight);
var joint3Position = body.GetJointZ3Position(JointType.AnkleRight);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkLt(joint1Position.Z, joint3Position.Z)));
return(expr);
}, body => { return(1.0); },
"don't bend your right knee towards your back.\n");
result.And(knees1);
result.And(knees2);
// Ankles
// Make sure ankles are not inclinated up more than knees
// unless ankles are pointing back
// TODO
return(result);
}
public static IBodyRestriction TestBodyRestriction()
{
var result = new SimpleBodyRestriction(body =>
{
double distanceThreshold = 0.4;
ArithExpr distanceThresholdSquared = Z3Math.Real(distanceThreshold * distanceThreshold);
ArithExpr x1 = body.Joints[JointType.SpineMid].X;
ArithExpr y1 = body.Joints[JointType.SpineMid].Y;
ArithExpr z1 = body.Joints[JointType.SpineMid].Z;
ArithExpr x2 = Z3Math.Zero;
ArithExpr y2 = Z3Math.One;
ArithExpr z2 = Z3Math.Zero;
//ArithExpr x2 = body.Joints[JointType.SpineBase].X;
//ArithExpr y2 = body.Joints[JointType.SpineBase].Y;
//ArithExpr z2 = body.Joints[JointType.SpineBase].Z;
var xExpr = Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2));
var yExpr = Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2));
var zExpr = Z3Math.Mul(Z3Math.Sub(z1, z2), Z3Math.Sub(z1, z2));
var distanceSquared = Z3Math.Add(xExpr, yExpr, zExpr);
// This is runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), y2));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y2, y2)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y1)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2)));
// This runs fine
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Mul(y1, y2)));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Add(y1, y2)));
// But this does not
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Add(y1, y2), Z3Math.Add(y1, y2)));
// This fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(0))));
// This does not
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, Z3.Context.MkReal(1, 7))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 6)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 6))));
// This runs fine
//var addExpr = Z3Math.Add(
// Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
// Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 3)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 3))));
// This does not
// asserting: (x1 - 0)² + (y1 - 1/2)² <= 0.4²
// which is x1² + y1² - y/2 - y/2 + 1/4 <= 0.16
// which is x1² + y1² - y + 0.25 <= 0.16
// which is x1² + y1² - y <= -0.09
// if x1 = 0 and y1 = 0.1 we would have a solution
// 0 + 0.01 - 0.1 <= 0.09
// 0.09 <= 0.09
// but z3 is taking too long to solve this.
var addExpr = Z3Math.Add(
Z3Math.Mul(Z3Math.Sub(x1, x2), Z3Math.Sub(x1, x2)),
Z3Math.Mul(Z3Math.Sub(y1, Z3.Context.MkReal(1, 2)), Z3Math.Sub(y1, Z3.Context.MkReal(1, 2))));
// But this is not (Z3 just stops while solving it)
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y2)));
//var addExpr = Z3Math.Add(xExpr, Z3Math.Mul(Z3Math.Sub(y1, y2), Z3Math.Sub(y1, y1)));
//BoolExpr expr = Z3.Context.MkLt(distanceSquared, distanceThresholdSquared);
BoolExpr expr = Z3.Context.MkLe(addExpr, distanceThresholdSquared);
// Why it complains about this Greater-Than case?
//BoolExpr expr = Z3.Context.MkGt(xExpr, distanceThresholdSquared);
return expr;
});
return result;
}
// Therapy restrictions
public static IBodyRestriction StraightPostureRestriction(int angleThreshold = 15)
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
double distanceThreshold = TrigonometryHelper.GetSine(angleThreshold);
var result = new SimpleBodyRestriction(body =>
{
BoolExpr expr = body.Joints[JointType.SpineMid].IsNearerThan(up, distanceThreshold);
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.SpineShoulder].IsNearerThan(up, distanceThreshold));
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Neck].IsNearerThan(up, distanceThreshold));
expr = Z3.Context.MkAnd(expr, body.Joints[JointType.Head].IsNearerThan(up, distanceThreshold));
return expr;
});
return result;
}
public void And(SimpleBodyRestriction that)
{
this.Restrictions.Add(that);
}
/// <summary>
/// This is a basic safety check to make sure we don't break any bones.
/// </summary>
/// <returns>True if safe</returns>
public static IBodyRestriction DefaultSafetyRestriction()
{
var result = new CompositeBodyRestriction();
int inclinationThreshold = 45;
// Head
// Make sure neck is not inclinated beyond the threshold
var head = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
BoolExpr expr1 = body.Joints[JointType.Head].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.Neck].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return(expr);
});
result.And(head);
// Spine
// Make sure spine is not inclinated beyond the threshold
var spine = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
BoolExpr expr1 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsAngleBetweenLessThan(up, inclinationThreshold);
BoolExpr expr3 =
body.Joints[JointType.SpineMid].IsAngleBetweenLessThan(
body.Joints[JointType.SpineShoulder], inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return(expr);
});
result.And(spine);
// Shoulders
// Make sure shoulders are not bent
var shoulders = new SimpleBodyRestriction(body =>
{
BoolExpr expr =
body.Joints[JointType.SpineMid].IsAngleBetweenGreaterThan(
body.Joints[JointType.SpineShoulder], 120);
return(expr);
});
result.And(shoulders);
// Elbows
// Make sure elbows are not behind the back
// And also that they are not on the top/back sub-space
var elbows = new SimpleBodyRestriction(body =>
{
BoolExpr exprRight1 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero),
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].X, Z3Math.Zero)));
BoolExpr exprRight2 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowRight].Y, Z3Math.Zero)));
BoolExpr exprLeft1 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].X, Z3Math.Zero)));
BoolExpr exprLeft2 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].Y, Z3Math.Zero)));
BoolExpr expr = Z3.Context.MkAnd(exprLeft1, exprLeft2, exprRight1, exprRight2);
return(expr);
});
result.And(elbows);
// Wrists
// Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows
// unless elbows are up or wrists are directed to torso
// TODO
// Hips
// Make sure hips are aligned with the shoulders or at lest within the range
var hips = new SimpleBodyRestriction(body =>
{
Z3Point3D shouldersSum =
body.Joints[JointType.ShoulderLeft].GetInverted() +
body.Joints[JointType.ShoulderRight];
Z3Point3D hipsSum =
body.Joints[JointType.HipLeft].GetInverted() +
body.Joints[JointType.HipRight];
BoolExpr expr = shouldersSum.IsAngleBetweenLessThan(hipsSum, 45);
return(expr);
});
result.And(hips);
// Legs
// Make sure legs are not higher than threshold
var legs = new SimpleBodyRestriction(body =>
{
BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.KneeLeft].Y, Z3Math.Real(0));
BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.KneeRight].Y, Z3Math.Real(0));
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return(expr);
});
result.And(legs);
// Ankles
// Make sure ankles are not inclinated up more than knees
// unless ankles are pointing back
// TODO
return(result);
}
/// <summary>
/// This is a basic safety check to make sure we don't break any bones.
/// </summary>
/// <returns>True if safe</returns>
public static IBodyRestriction DefaultSafetyRestriction()
{
var result = new CompositeBodyRestriction();
int inclinationThreshold = 45;
// Head
// Make sure neck is not inclinated beyond the threshold
var head = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
BoolExpr expr1 = body.Joints[JointType.Head].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.Neck].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return expr;
},
body => { return 1.0; });
result.And(head);
// Spine
// Make sure spine is not inclinated beyond the threshold
var spine = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
BoolExpr expr1 = body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr3 =
body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(
body.Joints[JointType.SpineShoulder], inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return expr;
},
body => { return 1.0; });
result.And(spine);
// Shoulders
// Make sure shoulders are not bent
var shoulders = new SimpleBodyRestriction(body =>
{
BoolExpr expr =
body.Joints[JointType.SpineMid].IsDegreesBetweenGreaterThan(
body.Joints[JointType.SpineShoulder], 120);
return expr;
},
body => { return 1.0; });
result.And(shoulders);
// Elbows
// Make sure elbows are not behind the back
// And also that they are not on the top/back sub-space
var elbows = new SimpleBodyRestriction(body =>
{
BoolExpr exprRight1 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero),
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].X, Z3Math.Zero)));
BoolExpr exprRight2 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowRight].Y, Z3Math.Zero)));
BoolExpr exprLeft1 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].X, Z3Math.Zero)));
BoolExpr exprLeft2 =
Z3.Context.MkNot(
Z3.Context.MkAnd(
Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero),
Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].Y, Z3Math.Zero)));
BoolExpr expr = Z3.Context.MkAnd(exprLeft1, exprLeft2, exprRight1, exprRight2);
return expr;
},
body => { return 1.0; });
result.And(elbows);
// Wrists
// Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows
// unless elbows are up or wrists are directed to torso
// TODO
// Hips
// Make sure hips are aligned with the shoulders or at lest within the range
var hips = new SimpleBodyRestriction(body =>
{
Z3Point3D shouldersSum =
body.Joints[JointType.ShoulderLeft].GetInverted() +
body.Joints[JointType.ShoulderRight];
Z3Point3D hipsSum =
body.Joints[JointType.HipLeft].GetInverted() +
body.Joints[JointType.HipRight];
BoolExpr expr = shouldersSum.IsDegreesBetweenLessThan(hipsSum, 45);
return expr;
},
body => { return 1.0; });
result.And(hips);
// Legs
// Make sure legs are not higher than threshold
var legs = new SimpleBodyRestriction(body =>
{
BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.KneeLeft].Y, Z3Math.Real(0));
BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.KneeRight].Y, Z3Math.Real(0));
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return expr;
},
body => { return 1.0; });
result.And(legs);
// Ankles
// Make sure ankles are not inclinated up more than knees
// unless ankles are pointing back
// TODO
return result;
}
