@Override public FrameConvexPolygon2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FrameConvexPolygon2d ret = new FrameConvexPolygon2d(this); ret.changeFrameAndProjectToXYPlane(desiredFrame); return ret; }
public void setSupportPolygon(FrameConvexPolygon2d supportPolygon) { this.supportPolygon.setIncludingFrameAndUpdate(supportPolygon); this.supportPolygon.changeFrameAndProjectToXYPlane(worldFrame); supportUpToDate = true; }
public void update() { nextFootstepPolygon.setIncludingFrameAndUpdate(footstepAdjustor.getTouchdownFootPolygon()); nextFootstepPolygon.changeFrameAndProjectToXYPlane(worldFrame); try { yoNextFootstepPolygon.setFrameConvexPolygon2d(nextFootstepPolygon); } catch (Exception e) { e.printStackTrace(); } } }
public void update() { captureRegionPolygon.setIncludingFrameAndUpdate(captureRegionCalculator.getCaptureRegion()); captureRegionPolygon.changeFrameAndProjectToXYPlane(worldFrame); if (yoCaptureRegionPolygon != null) { try { yoCaptureRegionPolygon.setFrameConvexPolygon2d(captureRegionPolygon); } catch (Exception e) { System.out.println(e); } } } }
private void doNothing() { footholdState.set(PartialFootholdState.FULL); yoUnsafePolygon.hide(); shrunkFootPolygonInWorld.setIncludingFrame(shrunkFootPolygon); shrunkFootPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame); yoShrunkFootPolygon.setFrameConvexPolygon2d(shrunkFootPolygonInWorld); unsafeArea.set(0.0); }
private void updateOnToesSupportPolygon(RobotSide trailingSide, FrameConvexPolygon2d leadingFootSupportPolygon) { computeToePoints(trailingSide); middleToePoint.changeFrame(worldFrame); onToesSupportPolygon.setIncludingFrameAndUpdate(leadingFootSupportPolygon); onToesSupportPolygon.changeFrameAndProjectToXYPlane(worldFrame); onToesSupportPolygon.addVertexByProjectionOntoXYPlane(middleToePoint); onToesSupportPolygon.update(); } }
private void updateSupportPolygon(boolean inDoubleSupport, boolean neitherFootIsSupportingFoot, RobotSide supportSide) { // Get the support polygon. If in double support, it is the combined polygon. // FIXME: Assumes the individual feet polygons are disjoint for faster computation. Will crash if the feet overlap. // If in single support, then the support polygon is just the foot polygon of the supporting foot. if (neitherFootIsSupportingFoot) throw new RuntimeException("neither foot is a supporting foot!"); if (inDoubleSupport) { supportPolygonInMidFeetZUp.setIncludingFrameAndUpdate(footPolygonsInMidFeetZUp.get(RobotSide.LEFT), footPolygonsInMidFeetZUp.get(RobotSide.RIGHT)); } else { supportPolygonInMidFeetZUp.setIncludingFrameAndUpdate(footPolygonsInMidFeetZUp.get(supportSide)); } supportPolygonInWorld.setIncludingFrameAndUpdate(supportPolygonInMidFeetZUp); supportPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame); }
public void setNextFootstep(Footstep nextFootstep) { isUsingNextFootstep.set(nextFootstep != null); if (isUsingNextFootstep.getBooleanValue()) { ReferenceFrame footstepSoleFrame = nextFootstep.getSoleReferenceFrame(); ConvexPolygon2d footPolygon = footPolygons.get(nextFootstep.getRobotSide()).getConvexPolygon2d(); nextFootstepPolygon.setIncludingFrameAndUpdate(footstepSoleFrame, footPolygon); nextFootstepPolygon.changeFrameAndProjectToXYPlane(worldFrame); } }
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 updateCombinedPolygon() { for (RobotSide robotSide : RobotSide.values) { FrameConvexPolygon2d footPolygonInWorldFrame = footPolygonsInWorldFrame.get(robotSide); footPolygonInWorldFrame.setIncludingFrameAndUpdate(footPolygons.get(robotSide)); footPolygonInWorldFrame.changeFrameAndProjectToXYPlane(worldFrame); } combinedFootPolygon.setIncludingFrameAndUpdate(footPolygonsInWorldFrame.get(RobotSide.LEFT), footPolygonsInWorldFrame.get(RobotSide.RIGHT)); // If there is a nextFootstep, increase the polygon to include it. if (isUsingNextFootstep.getBooleanValue()) combinedFootPolygonWithNextFootstep.setIncludingFrameAndUpdate(combinedFootPolygon, nextFootstepPolygon); }
/** * This function takes a footstep and calculates the touch-down polygon in the * desired reference frame */ private void calculateTouchdownFootPolygon(Footstep footstep, ReferenceFrame desiredFrame, FrameConvexPolygon2d polygonToPack) { footstep.getPositionIncludingFrame(centroid3d); centroid3d.getFramePoint2d(centroid2d); centroid2d.changeFrame(desiredFrame); polygonToPack.setIncludingFrameAndUpdate(footstep.getSoleReferenceFrame(), defaultSupportPolygons.get(footstep.getRobotSide())); polygonToPack.changeFrameAndProjectToXYPlane(desiredFrame); // shrink the polygon for safety by pulling all the corner points towards the center polygonToPack.scale(centroid2d, SHRINK_TOUCHDOWN_POLYGON_FACTOR); }
public void getCMPProjectionArea(FrameConvexPolygon2d areaToProjectInto, FrameConvexPolygon2d safeArea) { if (usingUpperBodyMomentum.getBooleanValue()) { polygonShrinker.shrinkConstantDistanceInto(supportPolygon, -maxDistanceCMPSupport.getDoubleValue(), extendedSupportPolygon); areaToProjectInto.setIncludingFrameAndUpdate(extendedSupportPolygon); safeCMPArea.changeFrameAndProjectToXYPlane(worldFrame); safeArea.setIncludingFrameAndUpdate(safeCMPArea); } else { areaToProjectInto.setIncludingFrameAndUpdate(supportPolygon); safeArea.clearAndUpdate(worldFrame); } if (yoSafeCMPArea != null) yoSafeCMPArea.setFrameConvexPolygon2d(safeArea); if (yoProjectionArea != null) yoProjectionArea.setFrameConvexPolygon2d(areaToProjectInto); }
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(); } }
tempPolygon2.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); polygonShrinker.shrinkConstantDistanceInto(tempPolygon2, -distanceToExtendUpcomingFoothold.getDoubleValue(), tempPolygon1); safeArea.addVertices(tempPolygon1); safeArea.changeFrameAndProjectToXYPlane(worldFrame);
ConvexPolygonTools.limitVerticesConservative(controllerFootPolygon, footCornerPoints); controllerFootPolygonInWorld.setIncludingFrameAndUpdate(controllerFootPolygon); controllerFootPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame); fullSupportAfterShrinking.changeFrameAndProjectToXYPlane(worldFrame); fullSupportAfterShrinking.addVertices(controllerFootPolygonInWorld); fullSupportAfterShrinking.update();
private void sequenceShiftWeight() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitPelvisHomeCommand(false); }
private void sequenceMediumWarmup() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameVector2d shiftScaleVector = new FrameVector2d(midFeetZUpFrame, 0.1, 0.7); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, shiftScaleVector.getX() * desiredPelvisOffset.getX(), shiftScaleVector.getY() * desiredPelvisOffset.getY(), 0.0); sequenceSquats(); sequenceChestRotations(0.55); //TODO increase/decrease limit? sequencePelvisRotations(0.3); //TODO increase/decrease limit? } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitChestHomeCommand(false); submitPelvisHomeCommand(false); }
footPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame);
this.projectionArea.changeFrameAndProjectToXYPlane(worldFrame); this.desiredCMP.changeFrameAndProjectToXYPlane(worldFrame); projectedCMP.changeFrameAndProjectToXYPlane(worldFrame);
footPolygon.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); // this works if the soleFrames are z up.