public void getSupportPolygon(FrameConvexPolygon2d polygonToPack) { polygonToPack.setIncludingFrame(shrunkFootPolygon); }
safeArea.setIncludingFrame(supportPolygon);
controllerFootPolygon.setIncludingFrame(shrunkFootPolygon); ConvexPolygonTools.limitVerticesConservative(controllerFootPolygon, footCornerPoints); controllerFootPolygonInWorld.setIncludingFrameAndUpdate(controllerFootPolygon);
private void doNothing() { footholdState.set(PartialFootholdState.FULL); yoUnsafePolygon.hide(); shrunkFootPolygonInWorld.setIncludingFrame(shrunkFootPolygon); shrunkFootPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame); yoShrunkFootPolygon.setFrameConvexPolygon2d(shrunkFootPolygonInWorld); unsafeArea.set(0.0); }
private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB) { footA.getCentroid(tempCentroid); firstCMP.setXYIncludingFrame(tempCentroid); firstCMP.changeFrame(worldFrame); footB.getCentroid(tempCentroid); secondCMP.setXYIncludingFrame(tempCentroid); secondCMP.changeFrame(worldFrame); upcomingSupport.clear(worldFrame); tempFootPolygon.setIncludingFrame(footA); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); tempFootPolygon.setIncludingFrame(footB); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); upcomingSupport.update(); entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); upcomingSupport.getCentroid(tempCentroid); tempCentroid3d.setXYIncludingFrame(tempCentroid); double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0); if (chicken <= 0.5) entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0); else entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0); exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex)); }
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(); } }
for (int j = 0; j < planarRegion.getNumberOfConvexPolygons(); j++) framePlanarRegion.setIncludingFrame(planarRegionReferenceFrame, planarRegion.getConvexPolygon(j));