FootstepDataMessage currentFaceFootstep = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(x, y, facePlane.getZOnPlane(x, y)), newOrientation); currentPredictedContactPoints = getPredictedContactPointsForFootstep(currentFaceFootstep, points, distanceTolerance);
RotationTools.computeQuaternionFromYawAndZNormal(yaw, facePlane.getNormalCopy(), newOrientation); FootstepDataMessage currentFaceFootstep = HumanoidMessageTools.createFootstepDataMessage(RobotSide.fromByte(footstep.getRobotSide()), new Point3D(x, y, facePlane.getZOnPlane(x, y)), newOrientation); currentPredictedContactPoints = getPredictedContactPointsForFootstep(currentFaceFootstep, points, distanceTolerance);