public static bool IsInternallyValid(Pose pose)
{
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
// We have to check that the pose is within the default safety restriction
IBodyRestriction safe = Safety.DefaultSafetyRestriction();
BoolExpr inputSafe = safe.Evaluate(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
// Try to generate a safe witness using the transform
BoolExpr outputSafe = safe.Evaluate(transformed);
// Check to see if the transform is not satisfiable -- if so, then it is not internally valid
BoolExpr expr = Z3.Context.MkAnd(inputSafe, transformedRestricted, outputSafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.SATISFIABLE)
{
// We can create a witness - therefore the pose must be valid
return true;
}
else if (solverResult.Status == Status.UNKNOWN)
{
return false;
}
else
{
Contract.Assert(solverResult.Status == Status.UNSATISFIABLE);
// Pose is not internally valid and as a result there can be no witness created
return false;
}
}
public PoseSafetyException(string message, Pose pose, Z3Body body)
: base(message)
{
this.Body = body;
this.Pose = pose;
}
public void AddPose(Pose pose)
{
this.DeclaredPoses.Add(pose);
}
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);
}
}
/// <summary>
/// Checks if the pose is within default safety
/// restrictions when the transform and restrictions
/// are applied.
/// </summary>
/// <returns>True if it's safe</returns>
public static bool IsWithinSafetyRestrictions(Pose pose, out Z3Body witness)
{
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
IBodyRestriction safe = Safety.DefaultSafetyRestriction();
BoolExpr inputSafe = safe.Evaluate(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
// Try to generate a unsafe witness using the transform
BoolExpr outputUnsafe = Z3.Context.MkNot(safe.Evaluate(transformed));
// Put together all expressions and search for unsat
BoolExpr expr = Z3.Context.MkAnd(inputSafe, transformedRestricted, outputUnsafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.SATISFIABLE)
{
//Z3Body
witness =
Z3AnalysisInterface.CreateBodyWitness(
transformed,
solverResult.Model,
pose.GetAllJointTypes(),
JointTypeHelper.CreateDefaultZ3Body());
return false;
}
else if (solverResult.Status == Status.UNKNOWN)
{
//Z3Body
witness = JointTypeHelper.CreateDefaultZ3Body();
return false;
}
else
{
Contract.Assert(solverResult.Status == Status.UNSATISFIABLE);
witness = null;
return true;
}
}
