How to use

us.ihmc.robotics.geometry.FrameConvexPolygon2d

constructor

public FrameConvexPolygon2d getContactPolygonCopy()
{
 return new FrameConvexPolygon2d(soleFrame, contactPoints);
}

@Override
public FrameConvexPolygon2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame)
{
 FrameConvexPolygon2d ret = new FrameConvexPolygon2d(this);
 ret.changeFrameAndProjectToXYPlane(desiredFrame);
 return ret;
}

@Override
public FrameConvexPolygon2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform)
{
 FrameConvexPolygon2d copy = new FrameConvexPolygon2d(this);
 copy.applyTransformAndProjectToXYPlane(transform);
 return copy;
}

public FrameConvexPolygon2d getFootSupportPolygon(RobotSide robotSide)
{
 if (robotSide == RobotSide.LEFT && leftFootSupportPolygonLength != 0)
   return new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), Arrays.copyOf(leftFootSupportPolygonStore, leftFootSupportPolygonLength));
 else if (rightFootSupportPolygonStore != null)
   return new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), Arrays.copyOf(rightFootSupportPolygonStore, rightFootSupportPolygonLength));
 else
   return new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame());
}

@Override
public FrameConvexPolygon2d applyTransformCopy(RigidBodyTransform transform)
{
 FrameConvexPolygon2d copy = new FrameConvexPolygon2d(this);
 copy.applyTransform(transform);
 return copy;
}

public YoFrameConvexPolygon2d(ArrayList<YoFramePoint2d> yoFramePoints, IntegerYoVariable yoNumVertices, ReferenceFrame referenceFrame)
{
 this.numVertices = yoNumVertices;
 numVertices.addVariableChangedListener(this);
 this.referenceFrame = referenceFrame;
 for (YoFramePoint2d point : yoFramePoints)
 {
   point.attachVariableChangedListener(this);
   this.yoFramePoints.add(point);
 }
 convexPolygon2dForReading = new FrameConvexPolygon2d(referenceFrame);
 convexPolygon2dForWriting = new FrameConvexPolygon2d(referenceFrame);
}

public synchronized void addConvexPolygons(ArrayList<ConvexPolygon2d> convexPolygons, Color color)
{
 FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
 if (frameConvexPolygonGroup == null)
 {
   frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
   frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
 }
 ArrayList<FrameConvexPolygon2d> frameConvexPolygons = new ArrayList<FrameConvexPolygon2d>();
 for (ConvexPolygon2d polygon : convexPolygons)
 {
   frameConvexPolygons.add(new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), polygon));
 }
 frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons);
}

public synchronized void addConvexPolygon(ConvexPolygon2d convexPolygon, Color color)
{
 FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
 if (frameConvexPolygonGroup == null)
 {
   frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
   frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
 }
 ArrayList<FrameConvexPolygon2d> frameConvexPolygons = new ArrayList<FrameConvexPolygon2d>();
 if (convexPolygon == null)
   return;
 frameConvexPolygons.add(new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), convexPolygon));
 frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons);
}

public synchronized void addConvexPolygons(ConvexPolygon2d[] convexPolygons, Color color)
{
 FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
 if (frameConvexPolygonGroup == null)
 {
   frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
   frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
 }
 ArrayList<FrameConvexPolygon2d> frameConvexPolygons = new ArrayList<FrameConvexPolygon2d>();
 for (ConvexPolygon2d polygon : convexPolygons)
 {
   if (polygon == null)
    continue;
   frameConvexPolygons.add(new FrameConvexPolygon2d(ReferenceFrame.getWorldFrame(), polygon));
 }
 frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons);
}

public YoFrameConvexPolygon2d(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, int maxNumberOfVertices, YoVariableRegistry registry)
{
 this.numVertices = new IntegerYoVariable(namePrefix + "NumVertices" + nameSuffix, registry);
 numVertices.addVariableChangedListener(this);
 this.referenceFrame = referenceFrame;
 for (int i = 0; i < maxNumberOfVertices; i++)
 {
   YoFramePoint2d point = new YoFramePoint2d(namePrefix + "_" + i + "_", nameSuffix, referenceFrame, registry);
   point.attachVariableChangedListener(this);
   yoFramePoints.add(point);
 }
 convexPolygon2dForReading = new FrameConvexPolygon2d(referenceFrame);
 convexPolygon2dForWriting = new FrameConvexPolygon2d(referenceFrame);
}

public FootstepAdjusterVisualizer(FootstepAdjustor footstepAdjustor, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry)
{
 this.footstepAdjustor = footstepAdjustor;
 String nextFootstepCaption = "DesiredTouchdown";
 yoNextFootstepPolygon = new YoFrameConvexPolygon2d(nextFootstepCaption, "", worldFrame, 8, registry);
 nextFootstepPolygon = new FrameConvexPolygon2d(worldFrame);
 nextFootstepPolygonArtifact = new YoArtifactPolygon(nextFootstepCaption, yoNextFootstepPolygon, colorDefault, false);
 nextFootstepPolygonArtifact.setVisible(false);
 yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), nextFootstepPolygonArtifact);
 parentRegistry.addChild(registry);
}

@Override
public FrameConvexPolygon2d intersectionWith(FrameConvexPolygon2d secondConvexPolygon)
{
 checkReferenceFrameMatch(secondConvexPolygon);
 ConvexPolygon2d intersection = this.convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon);
 if (intersection == null)
   return null;
 return new FrameConvexPolygon2d(secondConvexPolygon.getReferenceFrame(), intersection);
}

public SimpleFootstepMask(ReferenceFrame yawFrame2D, ContactablePlaneBody foot, double footKernelMaskSafetyBuffer)
{
 this.yawFrame2d = yawFrame2D;
 this.safetyBuffer = footKernelMaskSafetyBuffer;
 ArrayList<FramePoint2d> contactPoints = new ArrayList<FramePoint2d>();
 for (FramePoint2d point : foot.getContactPoints2d())
 {
   contactPoints.add(new FramePoint2d(yawFrame2D, inflate(point.getX()), inflate(point.getY())));
 }
 footPolygon = new FrameConvexPolygon2d(contactPoints);
 if (DEBUG)
   System.out.println("SimpleFootstepMask: footPolygon is \n" + footPolygon + " \nand yawFrame2d = \n"
            + yawFrame2d.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame()));
}

private void calculateReachableRegions()
{
 for (RobotSide side : RobotSide.values)
 {
   FrameConvexPolygon2d reachableRegion = new FrameConvexPolygon2d();
   reachableRegion.clear(ankleZUpFrames.get(side));
   double sign = side == RobotSide.RIGHT ? 1.0 : -1.0;
   for (int i = 0; i < MAX_CAPTURE_REGION_POLYGON_POINTS - 1; i++)
   {
    double angle = sign * reachableRegionCutoffAngle * Math.PI * ((double) i) / ((double) (MAX_CAPTURE_REGION_POLYGON_POINTS - 2));
    double x = kinematicStepRange * Math.cos(angle) + midFootAnkleXOffset;
    double y = kinematicStepRange * Math.sin(angle);
    if (Math.abs(y) < footWidth / 2.0)
      y = sign * footWidth / 2.0;
    reachableRegion.addVertex(ankleZUpFrames.get(side), x, y);
   }
   reachableRegion.addVertex(ankleZUpFrames.get(side), midFootAnkleXOffset, sign * footWidth / 2.0);
   reachableRegion.update();
   reachableRegions.set(side, reachableRegion);
 }
}

public FootstepAdjustor(SideDependentList<? extends ContactablePlaneBody> contactableFeet, YoVariableRegistry parentRegistry,
   YoGraphicsListRegistry yoGraphicsListRegistry)
{
 parentRegistry.addChild(registry);
 if (yoGraphicsListRegistry != null && VISUALIZE)
 {
   footstepAdjusterVisualizer = new FootstepAdjusterVisualizer(this, yoGraphicsListRegistry, registry);
 }
 defaultSupportPolygons = new SideDependentList<>();
 for (RobotSide robotSide : RobotSide.values)
 {
   defaultSupportPolygons.put(robotSide, new FrameConvexPolygon2d(contactableFeet.get(robotSide).getContactPoints2d()).getConvexPolygon2d());
 }
}

public WalkingFailureDetectionControlModule(SideDependentList<? extends ContactablePlaneBody> contactableFeet, YoVariableRegistry parentRegistry)
{
 for (RobotSide robotSide : RobotSide.values)
 {
   FrameConvexPolygon2d footPolygonInFootFrame = new FrameConvexPolygon2d(contactableFeet.get(robotSide).getContactPoints2d());
   footPolygons.put(robotSide, footPolygonInFootFrame);
   FrameConvexPolygon2d footPolygonInWorldFrame = new FrameConvexPolygon2d();
   footPolygonsInWorldFrame.put(robotSide, footPolygonInWorldFrame);
 }
 // Just for allocating memory for the nextFootstepPolygon
 nextFootstepPolygon.setIncludingFrameAndUpdate(footPolygons.get(RobotSide.LEFT));
 isUsingNextFootstep = new BooleanYoVariable("isFallDetectionUsingNextFootstep", registry);
 isUsingNextFootstep.set(false);
 updateCombinedPolygon();
 isFallDetectionActivated = new BooleanYoVariable("isFallDetectionActivated", registry);
 isFallDetectionActivated.set(true);
 icpDistanceFromFootPolygonThreshold = new DoubleYoVariable("icpDistanceFromFootPolygonThreshold", registry);
 icpDistanceFromFootPolygonThreshold.set(0.05);
 icpDistanceFromFootPolygon = new DoubleYoVariable("icpDistanceFromFootPolygon", registry);
 isRobotFalling = new BooleanYoVariable("isRobotFalling", registry);
 parentRegistry.addChild(registry);
}

/**
* generates garbage
* @param polygon1
* @param polygon2
* @return
*/
//this should throw away points that are inside of the other polygon
public static FrameConvexPolygon2dAndConnectingEdges combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2)
{
 ReferenceFrame referenceFrame = polygon1.getReferenceFrame();
 polygon2.checkReferenceFrameMatch(referenceFrame);
 ConvexPolygon2dAndConnectingEdges polygonAndEdges = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon);
 if (polygonAndEdges == null)
   return null; // Return null if not disjoint
 FrameConvexPolygon2d frameConvexPolygon2d = new FrameConvexPolygon2d(referenceFrame, polygonAndEdges.getConvexPolygon2d());
 FrameLineSegment2d connectingEdge1 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge1());
 FrameLineSegment2d connectingEdge2 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge2());
 FrameConvexPolygon2dAndConnectingEdges ret = new FrameConvexPolygon2dAndConnectingEdges(polygon1, polygon2, frameConvexPolygon2d, connectingEdge1,
    connectingEdge2);
 return ret;
}

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);
}

public WalkOnTheEdgesManager(FullHumanoidRobotModel fullRobotModel, WalkingControllerParameters walkingControllerParameters,
   SideDependentList<? extends ContactablePlaneBody> feet, SideDependentList<YoPlaneContactState> footContactStates,
   YoVariableRegistry parentRegistry)
{
 this.doToeOffIfPossible.set(walkingControllerParameters.doToeOffIfPossible());
 this.doToeOffIfPossibleInSingleSupport.set(walkingControllerParameters.doToeOffIfPossibleInSingleSupport());
 this.doToeOffWhenHittingAnkleLimit.set(walkingControllerParameters.doToeOffWhenHittingAnkleLimit());
 this.ankleLowerLimitToTriggerToeOff.set(walkingControllerParameters.getAnkleLowerLimitToTriggerToeOff());
 this.icpProximityToLeadingFootForDSToeOff.set(walkingControllerParameters.getICPProximityToLeadingFootForToeOff());
 this.icpPercentOfStanceForSSToeOff.set(walkingControllerParameters.getICPPercentOfStanceForSSToeOff());
 this.walkingControllerParameters = walkingControllerParameters;
 this.fullRobotModel = fullRobotModel;
 this.feet = feet;
 this.inPlaceWidth = walkingControllerParameters.getInPlaceWidth();
 this.footLength = walkingControllerParameters.getFootBackwardOffset() + walkingControllerParameters.getFootForwardOffset();
 extraCoMMaxHeightWithToes.set(0.08);
 minStepLengthForToeOff.set(walkingControllerParameters.getMinStepLengthForToeOff());
 minStepHeightForToeOff.set(walkingControllerParameters.getMinStepHeightForToeOff());
 isRearAnklePitchHittingLimit = new BooleanYoVariable("isRearAnklePitchHittingLimit", registry);
 isRearAnklePitchHittingLimitFilt = new GlitchFilteredBooleanYoVariable("isRearAnklePitchHittingLimitFilt", registry, isRearAnklePitchHittingLimit, 10);
 footDefaultPolygons = new SideDependentList<>();
 for (RobotSide robotSide : RobotSide.values)
 {
   footDefaultPolygons.put(robotSide, new FrameConvexPolygon2d(feet.get(robotSide).getContactPoints2d()));
 }
 this.footContactStates = footContactStates;
 parentRegistry.addChild(registry);
}