public static List<FramePoint> calculateExpectedContactPoints(Footstep atFootstep, ContactablePlaneBody foot)
{
if (!atFootstep.getBody().getName().equals(foot.getRigidBody().getName()))
throw new RuntimeException("The RigidBodies are not the same.");
List<FramePoint> ret = foot.getContactPointsCopy();
ReferenceFrame soleFrame = atFootstep.getSoleReferenceFrame();
for (int i = 0; i < ret.size(); i++)
{
FramePoint contactPoint = ret.get(i);
contactPoint.checkReferenceFrameMatch(foot.getSoleFrame());
contactPoint.setIncludingFrame(soleFrame, contactPoint.getPoint());
}
return ret;
}
}