How to use

changeFrameAndProjectToXYPlane

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

@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.