public static bool IsWithinDefaultSafetyRestrictions(Pose pose, out string firstBadStatement)
{
bool result = true;
firstBadStatement = "";
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
var restrictions = Safety.DefaultSafetyRestriction().Restrictions;
var composite = new CompositeBodyRestriction();
foreach (var restriction in restrictions)
{
composite.And(restriction);
BoolExpr inputSafe = composite.Evaluate(transformed);
BoolExpr expr = Z3.Context.MkAnd(transformedRestricted, inputSafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.UNSATISFIABLE)
{
firstBadStatement = ((SimpleBodyRestriction)restriction).Message;
result = false;
break;
}
}
return(result);
}
public static bool IsInternallyValid(Pose pose, out string firstBadStatement)
{
bool result = true;
firstBadStatement = "";
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
var restrictions = pose.Restriction.Restrictions;
var composite = new CompositeBodyRestriction();
foreach (var restriction in restrictions)
{
composite.And(restriction);
BoolExpr transformedRestricted = composite.Evaluate(transformed);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(transformedRestricted);
if (solverResult.Status == Status.UNSATISFIABLE)
{
firstBadStatement = restriction.ToString();
result = false;
break;
}
}
return(result);
}
public CompositeBodyRestriction Aggregate(Func <JointType, IBodyRestriction> func)
{
var result = new CompositeBodyRestriction();
foreach (var joint in this.Joints)
{
var that = func(joint);
result = result.And(that);
}
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 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);
}
/// <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;
}
/// <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);
}