public void setPoseFinderParams(double kernelMaskSafetyBuffer, double kernelSize) { footstepSnapper.setUseMask(USE_MASK, kernelMaskSafetyBuffer, kernelSize); }
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; }
private SideDependentList<Footstep> updatedStartFeet(double startX, double startY, double startYaw) { double leftXFactor = initialDeltaFeetLocalX / 2 * Math.cos(startYaw) - initialDeltaFeetLocalY / 2 * Math.sin(startYaw); double leftYFactor = initialDeltaFeetLocalX / 2 * Math.sin(startYaw) + initialDeltaFeetLocalY / 2 * Math.cos(startYaw); QuadTreeFootstepSnapper footstepSnapper = new SimpleFootstepSnapper(); Point2D leftFootStartPoint = new Point2D(startX + leftXFactor, startY + leftYFactor); FramePose2D leftFootPose2d = new FramePose2D(WORLD_FRAME, leftFootStartPoint, startYaw + initialDeltaFeetYaw / 2); Point2D rightFootStartPoint = new Point2D(startX - leftXFactor, startY - leftYFactor); FramePose2D rightFootPose2d = new FramePose2D(WORLD_FRAME, rightFootStartPoint, startYaw - initialDeltaFeetYaw / 2); FramePoint3D leftPosition = new FramePoint3D(WORLD_FRAME, leftFootStartPoint.getX(), leftFootStartPoint.getY(), 0); FramePoint3D rightPosition = new FramePoint3D(WORLD_FRAME, rightFootStartPoint.getX(), rightFootStartPoint.getY(), 0); FrameQuaternion leftOrientation = new FrameQuaternion(WORLD_FRAME, leftFootPose2d.getYaw(), 0.0, 0.0); FrameQuaternion rightOrientation = new FrameQuaternion(WORLD_FRAME, rightFootPose2d.getYaw(), 0.0, 0.0); leftContactableFoot.setSoleFrame(leftPosition, leftOrientation); rightContactableFoot.setSoleFrame(rightPosition, rightOrientation); Footstep leftStart = footstepSnapper.generateFootstepWithoutHeightMap(leftFootPose2d, feet.get(RobotSide.LEFT), contactableFeet.get(RobotSide.LEFT).getSoleFrame(), RobotSide.LEFT, GROUND_HEIGHT, PLANE_NORMAL); Footstep rightStart = footstepSnapper.generateFootstepWithoutHeightMap(rightFootPose2d, feet.get(RobotSide.RIGHT), contactableFeet.get(RobotSide.RIGHT).getSoleFrame(), RobotSide.RIGHT, GROUND_HEIGHT, PLANE_NORMAL); SideDependentList<Footstep> startFeet = new SideDependentList<Footstep>(leftStart, rightStart); return startFeet; }
Footstep generatedSnappedFootstep = footstepSnapper.generateSnappedFootstep(soleX.getDoubleValue(), soleY.getDoubleValue(), soleYaw.getDoubleValue(), footstepBody.getRigidBody(), footstepBody.getSoleFrame(), robotSide, heightMap); boolean pointsBelowPlane = pointsBelowPlane(footstepSnapper.getPointList(), solePlane, tolerance); assertTrue("queryPoint = " + queryPoint + " yaw = " + soleYaw.getDoubleValue() + " planeNormal = " + planeNormal + ", pointsBelowPlane = " + pointsBelowPlane, (planeNormal.getZ() >= 0.98) || pointsBelowPlane); List<Point3D> pointList = footstepSnapper.getPointList(); for (Point3D point : pointList)
Footstep leftStart = footstepSnapper.generateFootstepWithoutHeightMap(leftFootPose2d, feetRigidBodies.get(RobotSide.LEFT), soleFrames.get(RobotSide.LEFT), RobotSide.LEFT, 0.0, new Vector3D(0.0, 0.0, 1.0)); FramePose2D rightFootPose2d = new FramePose2D(worldFrame, new Point2D(0.0, -0.1), 0); Footstep rightStart = footstepSnapper.generateFootstepWithoutHeightMap(rightFootPose2d, feetRigidBodies.get(RobotSide.RIGHT), soleFrames.get(RobotSide.RIGHT), RobotSide.RIGHT, 0.0, new Vector3D(0.0, 0.0, 1.0));