public void getDefaultFootPolygon(RobotSide robotSide, FrameConvexPolygon2d polygonToPack) { polygonToPack.set(defaultFootPolygons.get(robotSide)); }
private void computeShrunkFoothold(FramePoint2d desiredCenterOfPressure) { boolean wasCoPInThatRegion = false; if (useCoPOccupancyGrid.getBooleanValue()) { numberOfCellsOccupiedOnSideOfLine.set(footCoPOccupancyGrid.computeNumberOfCellsOccupiedOnSideOfLine(lineOfRotation, RobotSide.RIGHT, distanceFromLineOfRotationToComputeCoPOccupancy.getDoubleValue())); wasCoPInThatRegion = numberOfCellsOccupiedOnSideOfLine.getIntegerValue() >= thresholdForCoPRegionOccupancy.getIntegerValue(); } unsafeArea.set(unsafePolygon.getArea()); boolean areaBigEnough = unsafeArea.getDoubleValue() >= minAreaToConsider.getDoubleValue(); unsafeAreaAboveThreshold.set(areaBigEnough); boolean desiredCopInPolygon = unsafePolygon.isPointInside(desiredCenterOfPressure, 0.0e-3); if (desiredCopInPolygon && !wasCoPInThatRegion && areaBigEnough) { backupFootPolygon.set(shrunkFootPolygon); ConvexPolygonTools.cutPolygonWithLine(lineOfRotation, shrunkFootPolygon, RobotSide.RIGHT); unsafePolygon.changeFrameAndProjectToXYPlane(worldFrame); yoUnsafePolygon.setFrameConvexPolygon2d(unsafePolygon); shrunkFootPolygonInWorld.setIncludingFrame(shrunkFootPolygon); shrunkFootPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame); yoShrunkFootPolygon.setFrameConvexPolygon2d(shrunkFootPolygonInWorld); } else { doNothing(); } }
private void fitLine() { if (!footCoPOccupancyGrid.fitLineToData(line)) return; lineL.setIncludingFrame(line); lineL.shiftToLeft(width/2.0); lineR.setIncludingFrame(line); lineR.shiftToRight(width/2.0); backupFootPolygon.set(shrunkFootPolygon); shrunkFootPolygon.clear(); shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineL)); shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineR)); shrunkFootPolygon.update(); }