How to use

setIncludingFrameAndUpdate

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

/**
* Creates an empty convex polygon attached to the reference frame of the frame vertex, adds N new vertices using a list of {@code FrameTuple2dArrayList<FramePoint2d>}, updates the vertices so they are clockwise ordered, and initializes some essential numbers such as the centroid.
* @param vertices {@code FrameTuple2dArrayList<FramePoint2d>} the list of points that is used to creates the vertices.
* @param numberOfVertices {@code int} that is used to determine the number of vertices of the polygon.
* Note the: pointList.size() can be greater or equal to numberOfVertices.
* @throws ReferenceFrameMismatchException
*/
public FrameConvexPolygon2d(FrameTuple2dArrayList<FramePoint2d> frameVertices)
{
 this();
 setIncludingFrameAndUpdate(frameVertices);
}

/**
* Creates an empty convex polygon attached to a reference frame, adds N new vertices using a list of Point2d, updates the vertices so they are clockwise ordered, and initializes some essential numbers such as the centroid.
* @param referenceFrame {@code ReferenceFrame} reference frame to which this polygon will be attached.
* @param vertices {@code List<Point2d>} the list of points that is used to creates the vertices.
* @param numberOfVertices {@code int} that is used to determine the number of vertices of the polygon.
* Note the: pointList.size() can be greater or equal to numberOfVertices.
*/
public FrameConvexPolygon2d(ReferenceFrame referenceFrame, List<Point2d> vertices)
{
 this(referenceFrame);
 setIncludingFrameAndUpdate(referenceFrame, vertices);
}

/**
* Creates an empty convex polygon attached to the reference frame of the frame vertex, adds N new vertices using a list of {@code List<FramePoint2d>}, updates the vertices so they are clockwise ordered, and initializes some essential numbers such as the centroid.
* @param vertices {@code List<FramePoint2d>} the list of points that is used to creates the vertices.
* @param numberOfVertices {@code int} that is used to determine the number of vertices of the polygon.
* Note the: pointList.size() can be greater or equal to numberOfVertices.
* @throws ReferenceFrameMismatchException
*/
public FrameConvexPolygon2d(List<FramePoint2d> frameVertices)
{
 this();
 setIncludingFrameAndUpdate(frameVertices);
}

public FrameConvexPolygon2d(ReferenceFrame referenceFrame, Point2d[] vertices)
{
 this(referenceFrame);
 setIncludingFrameAndUpdate(referenceFrame, vertices);
}

/**
* Creates an empty convex polygon attached to a reference frame, adds N new vertices using an array of Point2d, updates the vertices so they are clockwise ordered, and initializes some essential numbers such as the centroid.
* @param referenceFrame {@code ReferenceFrame} reference frame to which this polygon will be attached.
* @param vertices {@code double[N][>=2]} the array of points that is used to creates the vertices. Each row contains one point whereas the (at least) two columns contains the coordinates x and y.
* The number of vertices of this polygon will be equal to the length of the point array.
*/
public FrameConvexPolygon2d(ReferenceFrame referenceFrame, double[][] vertices)
{
 this(referenceFrame);
 setIncludingFrameAndUpdate(referenceFrame, vertices);
}

/**
* Creates a polygon with the same properties as the other polygon.
* @param otherPolygon {@code FrameConvexPolygon2d} the other convex polygon.
*/
public FrameConvexPolygon2d(FrameConvexPolygon2d otherPolygon)
{
 this();
 setIncludingFrameAndUpdate(otherPolygon);
}

/**
* Creates a new convex polygon by combining two other convex polygons. The result is the smallest convex hull that contains both polygons.
* Then it updates the vertices so they are clockwise ordered, and initializes some essential numbers such as the centroid.
* <p/> TODO: Make this more efficient by finding the rotating calipers, as in the intersection method.<p/>
*
* @param firstPolygon {@code ConvexPolygon2d}
* @param secondPolygon {@code ConvexPolygon2d}
* @throws ReferenceFrameMismatchException
*/
public FrameConvexPolygon2d(FrameConvexPolygon2d firstPolygon, FrameConvexPolygon2d secondPolygon)
{
 this();
 setIncludingFrameAndUpdate(firstPolygon, secondPolygon);
}

public void setSupportPolygon(FrameConvexPolygon2d supportPolygon)
{
 this.supportPolygon.setIncludingFrameAndUpdate(supportPolygon);
 this.supportPolygon.changeFrameAndProjectToXYPlane(worldFrame);
 supportUpToDate = true;
}

public void setCMPProjectionArea(FrameConvexPolygon2d areaToProjectInto, FrameConvexPolygon2d safeArea)
{
 this.areaToProjectInto.setIncludingFrameAndUpdate(areaToProjectInto);
 this.safeArea.setIncludingFrameAndUpdate(safeArea);
 yoSafeAreaPolygon.setFrameConvexPolygon2d(safeArea);
 yoProjectionPolygon.setFrameConvexPolygon2d(areaToProjectInto);
}

public void setContactPoints(List<Point2d> contactPointLocations)
{
 int contactPointLocationsSize = contactPointLocations.size();
 if (contactPointLocationsSize != totalNumberOfContactPoints)
   throw new RuntimeException("contactPointLocationsSize != totalNumberOfContactPoints");
 for (int i = 0; i < contactPointLocationsSize; i++)
 {
   Point2d contactPointLocation = contactPointLocations.get(i);
   YoContactPoint yoContactPoint = contactPoints.get(i);
   yoContactPoint.setPosition2d(contactPointLocation);
 }
 contactPointsPolygon.setIncludingFrameAndUpdate(planeFrame, contactPointLocations);
 this.contactPointCentroid.set(contactPointsPolygon.getCentroid());
}

public void setContactFramePoints(List<FramePoint2d> contactPointLocations)
{
 int contactPointLocationsSize = contactPointLocations.size();
 if (contactPointLocationsSize != totalNumberOfContactPoints)
   throw new RuntimeException("contactPointLocationsSize != totalNumberOfContactPoints");
 for (int i = 0; i < contactPointLocationsSize; i++)
 {
   FramePoint2d contactPointLocation = contactPointLocations.get(i);
   YoContactPoint yoContactPoint = contactPoints.get(i);
   yoContactPoint.setPosition(contactPointLocation);
 }
 contactPointsPolygon.setIncludingFrameAndUpdate(contactPointLocations);
 this.contactPointCentroid.set(contactPointsPolygon.getCentroid());
}

/**
* Creates an empty convex polygon attached to a reference frame, adds N new vertices using the vertices of another {code ConvexPolygon2d}, updates the vertices so they are clockwise ordered, and initializes some essential numbers such as the centroid.
* @param referenceFrame {@code ReferenceFrame} reference frame to which this polygon will be attached.
* @param otherPolygon {@code ConvexPolygon2d} the other convex polygon.
*/
public FrameConvexPolygon2d(ReferenceFrame referenceFrame, ConvexPolygon2d otherPolygon)
{
 super(referenceFrame, new ConvexPolygon2d());
 this.convexPolygon = this.getGeometryObject();
 setIncludingFrameAndUpdate(referenceFrame, otherPolygon);
}

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

private void computeEntryCMPForFootstep(FramePoint entryCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot,
   YoFramePoint previousExitCMP)
{
 ReferenceFrame soleFrame = footstep.getSoleReferenceFrame();
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 RobotSide robotSide = footstep.getRobotSide();
 if (predictedContactPoints != null)
   tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints);
 else
   tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide));
 computeEntryCMP(entryCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfPreviousSupportFoot, previousExitCMP);
}

public void updateCaptureRegion(double swingTimeRemaining, double omega0, RobotSide swingSide, FramePoint2d capturePoint2d)
{
 footPolygon.setIncludingFrameAndUpdate(bipedSupportPolygon.getFootPolygonInAnkleZUp(swingSide.getOppositeSide()));
 captureRegionCalculator.calculateCaptureRegion(swingSide, swingTimeRemaining, capturePoint2d, omega0, footPolygon);
}

private void computeEntryCMPForSupportFoot(FramePoint entryCMPToPack, RobotSide robotSide, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot,
   YoFramePoint previousLateCMP)
{
 ReferenceFrame soleFrame = soleZUpFrames.get(robotSide);
 tempSupportPolygon.setIncludingFrameAndUpdate(supportFootPolygonsInSoleZUpFrame.get(robotSide));
 tempSupportPolygon.changeFrame(soleFrame);
 computeEntryCMP(entryCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfPreviousSupportFoot, previousLateCMP);
}

  private void updateOnToesSupportPolygon(RobotSide trailingSide, FrameConvexPolygon2d leadingFootSupportPolygon)
  {
   computeToePoints(trailingSide);
   middleToePoint.changeFrame(worldFrame);

   onToesSupportPolygon.setIncludingFrameAndUpdate(leadingFootSupportPolygon);
   onToesSupportPolygon.changeFrameAndProjectToXYPlane(worldFrame);
   onToesSupportPolygon.addVertexByProjectionOntoXYPlane(middleToePoint);
   onToesSupportPolygon.update();
  }
}

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

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

private void putExitCMPOnToes(FrameConvexPolygon2d footSupportPolygon, FramePoint2d exitCMPToPack)
{
 // Set x to have the CMP slightly inside the support polygon
 exitCMPToPack.setToZero(footSupportPolygon.getReferenceFrame());
 exitCMPToPack.setX(footSupportPolygon.getMaxX() - 1.6e-2);
 exitCMPToPack.setY(footSupportPolygon.getCentroid().getY());
 // Then constrain the computed CMP to be inside a safe support region
 tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon);
 convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdgesWhenSteppingDown.getDoubleValue(),
    footSupportPolygon);
 footSupportPolygon.orthogonalProjection(exitCMPToPack);
}