protected Footstep createFootstep(RobotSide currentFootstepSide, FramePose2d solePose)
{
RigidBody foot = feet.get(currentFootstepSide);
ReferenceFrame soleFrame = soleFrames.get(currentFootstepSide);
Footstep footstep;
try
{
if (heightMap != null)
{
footstep = footstepSnapper.generateFootstepUsingHeightMap(solePose, foot, soleFrame, currentFootstepSide, heightMap);
}
else
{
FramePoint soleFrameInWorldPoint = new FramePoint(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;
}