protected Footstep createFootstep(RobotSide currentFootstepSide, FramePose2D solePose) { RigidBodyBasics foot = feet.get(currentFootstepSide); ReferenceFrame soleFrame = soleFrames.get(currentFootstepSide); Footstep footstep; try { if (heightMap != null) { footstep = footstepSnapper.generateFootstepUsingHeightMap(solePose, foot, soleFrame, currentFootstepSide, heightMap); } else { FramePoint3D soleFrameInWorldPoint = new FramePoint3D(soleFrame); soleFrameInWorldPoint.changeFrame(WORLD_FRAME); footstep = footstepSnapper.generateFootstepWithoutHeightMap(solePose, foot, soleFrame, currentFootstepSide, soleFrameInWorldPoint.getZ(), new Vector3D(0.0, 0.0, 1.0)); if (VERBOSE_ERROR_PRINTS) System.err.println("AbstractFootstepGenerator: Grid data unavailable. Using best guess for ground height."); } } catch (InsufficientDataException e) { footstep = footstepSnapper.generateFootstepWithoutHeightMap(solePose, foot, soleFrame, currentFootstepSide, 0, new Vector3D(0.0, 0.0, 1.0)); if (VERBOSE_DEBUG) { System.err.println("AbstractFootstepGenerator: No lidar data found for step. Using best guess."); } } return footstep; }