private void getYoValuesFromFrameConvexPolygon2d() { numVertices.set(convexPolygon2dForWriting.getNumberOfVertices()); try { for (int i = 0; i < numVertices.getIntegerValue(); i++) { yoFramePoints.get(i).checkReferenceFrameMatch(convexPolygon2dForWriting); yoFramePoints.get(i).set(convexPolygon2dForWriting.getVertex(i)); } } catch (Exception e) { System.err.println("In YoFrameConvexPolygon2d.java: " + e.getClass().getSimpleName() + " while calling getYoValuesFromFrameConvexPolygon2d()."); e.printStackTrace(); } }
@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 setSupportPolygon(RobotSide robotSide, FrameConvexPolygon2d footPolygon) { int numberOfVertices = footPolygon.getNumberOfVertices(); if (numberOfVertices > MAXIMUM_NUMBER_OF_VERTICES) { numberOfVertices = MAXIMUM_NUMBER_OF_VERTICES; } if (robotSide == RobotSide.LEFT) { leftFootSupportPolygonLength = numberOfVertices; } else { rightFootSupportPolygonLength = numberOfVertices; } for (int i = 0; i < numberOfVertices; i++) { if (robotSide == RobotSide.LEFT) { footPolygon.getVertex(i, leftFootSupportPolygonStore[i]); } else { footPolygon.getVertex(i, rightFootSupportPolygonStore[i]); } } }
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()); } }
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(); } }
public void resetFootSupportPolygon(RobotSide robotSide) { YoPlaneContactState contactState = footContactStates.get(robotSide); List<YoContactPoint> contactPoints = contactState.getContactPoints(); FrameConvexPolygon2d defaultSupportPolygon = defaultFootPolygons.get(robotSide); for (int i = 0; i < defaultSupportPolygon.getNumberOfVertices(); i++) { defaultSupportPolygon.getFrameVertexXY(i, tempPosition); contactPoints.get(i).setPosition(tempPosition); } contactState.notifyContactStateHasChanged(); }
/** * Adds new vertices to this polygon from another convex polygon. * Note that this method recycles memory. * @param otherPolygon {@code FrameConvexPolygon2d} the other convex polygon that is used to add new vertices to this polygon. * @throws ReferenceFrameMismatchException */ public void addVertices(FrameConvexPolygon2d otherPolygon) { referenceFrame.checkReferenceFrameMatch(otherPolygon.getReferenceFrame()); for (int i = 0; i < otherPolygon.getNumberOfVertices(); i++) { Point2d vertex = otherPolygon.convexPolygon.getVertex(i); convexPolygon.addVertex(vertex); } }
private void drawPolygon(FrameConvexPolygon2d frameConvexPolygon2d, Graphics graphics) { Polygon polygon = new Polygon(); for (int i = 0; i < frameConvexPolygon2d.getNumberOfVertices(); i++) { Point2d vertex = frameConvexPolygon2d.getVertex(i); int xInt = doubleToInt(vertex.getX(), xCenter, scale, getWidth()); int yInt = doubleToInt(vertex.getY(), yCenter, -scale, getHeight()); polygon.addPoint(xInt, yInt); } graphics.drawPolygon(polygon); }
for (int contactPointIndex = 0; contactPointIndex < footSupportPolygon.getNumberOfVertices(); contactPointIndex++)
public static void movePointInsidePolygonAlongVector(FramePoint2d pointToMove, FrameVector2d vector, FrameConvexPolygon2d polygon, double distanceToBeInside) if (polygon.getNumberOfVertices() < 2)
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); }
int corners = supportPolygon.getNumberOfVertices(); currentCornerIdx = (int) (timeExploring / timeToExploreCorner); int corner = currentCornerIdx % corners;
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); }
for (int i = 0; i < controllerFootPolygon.getNumberOfVertices(); i++)
int numberOfVertices = supportPolygon.getNumberOfVertices(); ArrayList<FramePoint2d> supportCornerPoints = new ArrayList<>();
int corners = supportPolygon.getNumberOfVertices(); currentCornerIdx = (int) (timeExploring / timeToExploreCorner); currentCornerIdx = currentCornerIdx % corners;
for (int i = 0; i < projectionArea.getNumberOfVertices(); i++)