How to use

getFrameVertex

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

public FramePoint2d getFrameVertexCopy(int vertexIndex)
{
 FramePoint2d frameVertexCopy = new FramePoint2d();
 getFrameVertex(vertexIndex, frameVertexCopy);
 return frameVertexCopy;
}

public void getPreviousFrameVertex(int vertexIndex, FramePoint2d vertexToPack)
{
 getFrameVertex(convexPolygon.getPreviousVertexIndex(vertexIndex), vertexToPack);
}

public void getNextFrameVertex(int vertexIndex, FramePoint2d vertexToPack)
{
 getFrameVertex(convexPolygon.getNextVertexIndex(vertexIndex), vertexToPack);
}

@Override
public String toString()
{
 FramePoint2d vertex = new FramePoint2d();
 String ret = "";
 for (int i = 0; i < getNumberOfVertices(); i++)
 {
   this.getFrameVertex(i, vertex);
   ret = ret + vertex.toString() + "\n";
 }
 return ret;
}

public void updateCMPConstraintForSingleSupport(RobotSide supportSide, ICPOptimizationSolver solver)
{
 FrameConvexPolygon2d supportPolygon = bipedSupportPolygons.getFootPolygonInSoleFrame(supportSide);
 solver.setNumberOfCMPVertices(supportPolygon.getNumberOfVertices());
 for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++)
 {
   supportPolygon.getFrameVertex(i, tempVertex);
   solver.setSupportPolygonVertex(i, tempVertex, supportPolygon.getReferenceFrame(), maxCMPSingleSupportExitForward.getDoubleValue(),
      maxCMPSingleSupportExitSideways.getDoubleValue());
 }
}

for (int contactPointIndex = 0; contactPointIndex < footSupportPolygon.getNumberOfVertices(); contactPointIndex++)
  footSupportPolygon.getFrameVertex(contactPointIndex, contactPoint2d);
  findProjectionOntoPlaneFrame(soleFrame, contactPoint2d, contactPoints.add());

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

public void updateCMPConstraintForDoubleSupport(ICPOptimizationSolver solver)
{
 int numberOfVertices = 0;
 for (RobotSide robotSide : RobotSide.values)
   numberOfVertices += bipedSupportPolygons.getFootPolygonInMidFeetZUp(robotSide).getNumberOfVertices();
 solver.setNumberOfCMPVertices(numberOfVertices);
 numberOfVertices = 0;
 for (RobotSide robotSide : RobotSide.values)
 {
   FrameConvexPolygon2d supportPolygon = bipedSupportPolygons.getFootPolygonInMidFeetZUp(robotSide);
   for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++)
   {
    supportPolygon.getFrameVertex(i, tempVertex);
    solver.setSupportPolygonVertex(numberOfVertices + i, tempVertex, supportPolygon.getReferenceFrame(), maxCMPDoubleSupportExitForward.getDoubleValue(),
       maxCMPDoubleSupportExitSideways.getDoubleValue());
   }
   numberOfVertices += supportPolygon.getNumberOfVertices();
 }
}

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

private void computeToePoints(RobotSide supportSide)
{
 FrameConvexPolygon2d footDefaultPolygon = footDefaultPolygons.get(supportSide);
 toePoints[0].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0);
 toePoints[1].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0);
 for (int i = 0; i < footDefaultPolygon.getNumberOfVertices(); i++)
 {
   footDefaultPolygon.getFrameVertex(i, footPoint);
   if (footPoint.getX() > toePoints[0].getX())
   {
    toePoints[1].set(toePoints[0]);
    toePoints[0].setXY(footPoint);
   }
   else if (footPoint.getX() > toePoints[1].getX())
   {
    toePoints[1].setXY(footPoint);
   }
 }
 middleToePoint.setToZero(footDefaultPolygon.getReferenceFrame());
 middleToePoint.interpolate(toePoints[0], toePoints[1], 0.5);
}

yoCurrentCorner.set(corner);
supportPolygon.getFrameVertex(corner, currentCorner);
FramePoint2d centroid = supportPolygon.getCentroid();

supportPolygon.getFrameVertex(i, frameVertexBefore);
supportPolygon.getFrameVertex((i + 1) % numberOfVertices, frameVertexCurrentlyChecked);
supportPolygon.getFrameVertex((i + 2) % numberOfVertices, frameVertexAfter);

yoCurrentCorner.set(currentCornerIdx);
supportPolygon.getFrameVertex(currentCornerIdx, currentCorner);
FramePoint2d centroid = supportPolygon.getCentroid();