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