public void addVertices(FramePoint2d[] vertices) { for (int i = 0; i < vertices.length; i++) { FramePoint2d vertex = vertices[i]; addVertex(vertex); } }
/** * Adds N new vertices to this polygon using a list of {@code FramePoint2d}. * Note that this method recycles memory. * @param vertices {@code List<FramePoint2d>} the list of new vertices (it is not modified). * @param numberOfVertices {@code int} that is used to determine the number of vertices to add to this polygon. Note the: {@code vertices.size()} can be greater or equal to numberOfVertices. * @throws ReferenceFrameMismatchException */ public void addVertices(List<FramePoint2d> vertices, int numberOfVertices) { for (int i = 0; i < numberOfVertices; i++) { FramePoint2d vertex = vertices.get(i); addVertex(vertex); } }
/** * Adds N new vertices to this polygon using a list of {@code FramePoint2d}. * Note that this method recycles memory. * @param vertices {@code FrameTuple2dArrayList<FramePoint2d>} the list of new vertices (it is not modified). * @param numberOfVertices {@code int} that is used to determine the number of vertices to add to this polygon. Note the: {@code vertices.size()} can be greater or equal to numberOfVertices. * @throws ReferenceFrameMismatchException */ public void addVertices(FrameTuple2dArrayList<FramePoint2d> vertices, int numberOfVertices) { for (int i = 0; i < numberOfVertices; i++) { FramePoint2d vertex = vertices.get(i); addVertex(vertex); } }
/** * Add a vertex to this polygon. * Note that this method recycles memory. * @param newVertex {@code FramePoint2d} the new vertex (it is not modified). * @throws ReferenceFrameMismatchException */ public void addVertex(FramePoint2d vertex) { vertex.checkReferenceFrameMatch(referenceFrame); addVertex(vertex.getPoint()); }
private void putYoValuesIntoFrameConvexPolygon2d() { try { convexPolygon2dForReading.clear(referenceFrame); for (int i = 0; i < numVertices.getIntegerValue(); i++) convexPolygon2dForReading.addVertex(yoFramePoints.get(i).getFrameTuple2d()); convexPolygon2dForReading.update(); } catch (Exception e) { System.err.println("In YoFrameConvexPolygon2d.java: " + e.getClass().getSimpleName() + " while calling putYoValuesIntoFrameConvexPolygon2d()."); } }
/** * 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(ConvexPolygon2d otherPolygon) { for (int i = 0; i < otherPolygon.getNumberOfVertices(); i++) { Point2d vertex = otherPolygon.getVertex(i); addVertex(vertex); } }
/** * This method does: * 1- {@code clear()}; * 2- {@code addVertex(vertices.get(i)}; * 3- {@code update()}. * @param vertices {@code FramePoint2d[]} the array of points that is used to creates the vertices. */ public void setAndUpdate(FramePoint2d[] vertices) { clear(); for (int i = 0; i < vertices.length; i++) { addVertex(vertices[i]); } update(); }
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); } }
/** * Add a vertex to this polygon after doing {@code newVertex2d.changeFrameAndProjectToXYPlane(this.referenceFrame)}. * Note that this method recycles memory. * @param newVertex {@code FramePoint} the new vertex (it is not modified). */ public void addVertexByProjectionOntoXYPlane(FramePoint newVertex) { tempPoint.setIncludingFrame(newVertex); tempPoint.changeFrame(referenceFrame); addVertex(referenceFrame, tempPoint.getX(), tempPoint.getY()); }
public void computeConvexHull(FrameConvexPolygon2d convexHullToPack) { convexHullToPack.clear(soleFrame); tempCellCenter.setToZero(soleFrame); for (int xIndex = 0; xIndex < nLengthSubdivisions.getIntegerValue(); xIndex++) { for (int yIndex = 0; yIndex < nWidthSubdivisions.getIntegerValue(); yIndex++) { if (isCellOccupied(xIndex, yIndex)) { getCellCenter(tempCellCenter, xIndex, yIndex); convexHullToPack.addVertex(tempCellCenter); } } } convexHullToPack.update(); }
gridBoundaries.addVertex(new FramePoint2d(soleFrame, -footLength / 2.0, -footWidth / 2.0)); gridBoundaries.addVertex(new FramePoint2d(soleFrame, -footLength / 2.0, footWidth / 2.0)); gridBoundaries.addVertex(new FramePoint2d(soleFrame, footLength / 2.0, -footWidth / 2.0)); gridBoundaries.addVertex(new FramePoint2d(soleFrame, footLength / 2.0, footWidth / 2.0)); gridBoundaries.update();
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); footPolygon.clear(soleFrame); for (int i = 0; i < contactPoints.size(); i++) { contactPoints.get(i).getPosition2d(toeOffContactPoint2d); footPolygon.addVertex(toeOffContactPoint2d); } footPolygon.update(); FramePoint2d rayOrigin; if (!exitCMP2d.containsNaN() && footPolygon.isPointInside(exitCMP2d)) rayOrigin = exitCMP2d; else rayOrigin = footPolygon.getCentroid(); rayThroughExitCMP.set(rayOrigin, exitCMPRayDirection2d); frameConvexPolygonWithRayIntersector2d.intersectWithRay(footPolygon, rayThroughExitCMP); toeOffContactPoint2d.set(frameConvexPolygonWithRayIntersector2d.getIntersectionPointOne()); contactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); contactPointPosition.changeFrame(contactableFoot.getRigidBody().getBodyFixedFrame()); pointFeedbackControlCommand.setBodyFixedPointToControl(contactPointPosition); desiredContactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); desiredContactPointPosition.changeFrame(worldFrame); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredYawToHold = desiredOrientation.getYaw(); desiredRollToHold = desiredOrientation.getRoll(); }