public void getPlane3d(FramePlane3d plane3dToPack) { plane3dToPack.setIncludingFrame(getReferenceFrame(), 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); }
public void getNormal3dVector(FrameVector normalToPack) { normalToPack.setIncludingFrame(getReferenceFrame(), 0.0, 0.0, 1.0); }
/** * This method does: * 1- {@code clear(firstPolygon.getReferenceFrame())}; * 2- {@code addVertices(firstPolygon)}; * 2- {@code addVertices(secondPolygon)}; * 3- {@code update()}. * <p/> TODO: Make this more efficient by finding the rotating calipers, as in the intersection method.<p/> * @param firstPolygon {@code FrameConvexPolygon2d} * @param secondPolygon {@code FrameConvexPolygon2d} * @throws ReferenceFrameMismatchException */ public void setIncludingFrameAndUpdate(FrameConvexPolygon2d firstPolygon, FrameConvexPolygon2d secondPolygon) { referenceFrame = firstPolygon.getReferenceFrame(); setAndUpdate(firstPolygon, secondPolygon); }
public FramePoint2d[] getLineOfSightVerticesCopy(FramePoint2d observer) { checkReferenceFrameMatch(observer); Point2d[] vertices = ConvexPolygon2dCalculator.getLineOfSightVerticesCopy(observer.getPoint(), convexPolygon); FramePoint2d point1 = new FramePoint2d(getReferenceFrame(), vertices[0]); FramePoint2d point2 = new FramePoint2d(getReferenceFrame(), vertices[1]); return new FramePoint2d[] {point1, point2}; }
@Override public FrameConvexPolygon2d intersectionWith(FrameConvexPolygon2d secondConvexPolygon) { checkReferenceFrameMatch(secondConvexPolygon); ConvexPolygon2d intersection = this.convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon); if (intersection == null) return null; return new FrameConvexPolygon2d(secondConvexPolygon.getReferenceFrame(), intersection); }
public boolean intersectionWith(FrameConvexPolygon2d secondConvexPolygon, FrameConvexPolygon2d intersectionToPack) { checkReferenceFrameMatch(secondConvexPolygon); intersectionToPack.clear(secondConvexPolygon.getReferenceFrame()); boolean success = convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon, intersectionToPack.convexPolygon); return success; }
/** * This method does: * 1- {@code clear(otherPolygon.getReferenceFrame())}; * 2- {@code addVertices(otherPolygon)}; * 3- {@code update()}. * <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/> * @param otherPolygon {@code FrameConvexPolygon2d} */ public void setIncludingFrameAndUpdate(FrameConvexPolygon2d otherPolygon) { clear(otherPolygon.getReferenceFrame()); addVertices(otherPolygon); update(); }
public void updatePointAndPolygon(FrameConvexPolygon2d polygon, FramePoint2d pointInAnyFrame) { FramePoint2d temp = new FramePoint2d(pointInAnyFrame); temp.changeFrame(polygon.getReferenceFrame()); updatePointAndPolygon(polygon, temp.getPoint()); }
@Override public void projectCMPIntoSupportPolygonIfOutside(FramePoint2d capturePoint, FrameConvexPolygon2d supportPolygon, FramePoint2d finalDesiredCapturePoint, FramePoint2d desiredCMPToPack) { ReferenceFrame returnFrame = desiredCMPToPack.getReferenceFrame(); desiredCMPToPack.changeFrame(supportPolygon.getReferenceFrame()); capturePoint.changeFrame(supportPolygon.getReferenceFrame()); projectedCMP.setToZero(supportPolygon.getReferenceFrame()); projectedCMP.setX(desiredCMPToPack.getX()); projectedCMP.setY(supportPolygon.getCentroid().getY()); projectCMPIntoSupportPolygonIfOutsideLocal(supportPolygon, desiredCMPToPack); desiredCMPToPack.changeFrame(returnFrame); capturePoint.changeFrame(returnFrame); }
public static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon) { ReferenceFrame originalReferenceFrame = point.getReferenceFrame(); point.changeFrame(polygon.getReferenceFrame()); FramePoint2d originalPoint = new FramePoint2d(point); polygon.orthogonalProjection(point); double distance = originalPoint.distance(point); if (distance > epsilon) throw new RuntimeException("point outside polygon by " + distance); point.changeFrame(originalReferenceFrame); }
/** * 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); } }
/** * This method does: * 1- {@code clear()}; * 2- {@code addVertices(otherPolygon)}; * 3- {@code update()}. * <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/> * @param otherPolygon {@code FrameConvexPolygon2d} * @throws ReferenceFrameMismatchException */ public void setAndUpdate(FrameConvexPolygon2d otherPolygon) { referenceFrame.checkReferenceFrameMatch(otherPolygon.getReferenceFrame()); clear(); addVertices(otherPolygon); update(); }
public static boolean combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2, FrameConvexPolygon2d combinedPolygonToPack, FrameLineSegment2d connectingEdge1ToPack, FrameLineSegment2d connectingEdge2ToPack) { combinedPolygonToPack.clear(polygon1.getReferenceFrame()); combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge1ToPack); combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge2ToPack); boolean success = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon, combinedPolygonToPack.convexPolygon, connectingEdge1ToPack.lineSegment, connectingEdge2ToPack.lineSegment); if (!success) return false; // combinedPolygonToPack.updateFramePoints(); combinedPolygonToPack.update(); return true; }
public void shrinkConstantDistanceInto(FrameConvexPolygon2d polygonQ, double distance, FrameConvexPolygon2d framePolygonToPack) { if (Math.abs(distance) < 1.0e-10) { framePolygonToPack.setIncludingFrameAndUpdate(polygonQ); return; } framePolygonToPack.clear(polygonQ.getReferenceFrame()); framePolygonToPack.update(); ConvexPolygon2d polygon2dToPack = framePolygonToPack.getConvexPolygon2d(); shrinkConstantDistanceInto(polygonQ.getConvexPolygon2dCopy(), distance, polygon2dToPack); // framePolygonToPack.updateFramePoints(); framePolygonToPack.update(); } }
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()); } }
/** * This function is called at beginning of every DoubleSupport, not frequent enough to preallocate eveything. */ public void updatePointAndPolygon(FrameConvexPolygon2d polygon, Point2d pointInPolygonFrame) { //copy external point back to polygon frame framePoint2d = new FramePoint2d(polygon.getReferenceFrame(), pointInPolygonFrame); //update polygon class point2dInConvexPolygon2d = new Point2dInConvexPolygon2d(polygon.getConvexPolygon2d(), framePoint2d.getX(), framePoint2d.getY()); angle.set(point2dInConvexPolygon2d.getAngle()); eccentricity.set(point2dInConvexPolygon2d.getEccentricity()); }
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); }
public void requestICPPlannerToHoldCurrentCoM() { centerOfMassPosition.setToZero(centerOfMassFrame); FrameConvexPolygon2d supportPolygonInMidFeetZUp = bipedSupportPolygons.getSupportPolygonInMidFeetZUp(); convexPolygonShrinker.shrinkConstantDistanceInto(supportPolygonInMidFeetZUp, distanceToShrinkSupportPolygonWhenHoldingCurrent.getDoubleValue(), shrunkSupportPolygon); centerOfMassPosition.changeFrame(shrunkSupportPolygon.getReferenceFrame()); centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition); shrunkSupportPolygon.orthogonalProjection(centerOfMassPosition2d); centerOfMassPosition.setXY(centerOfMassPosition2d); centerOfMassPosition.changeFrame(worldFrame); icpPlanner.holdCurrentICP(yoTime.getDoubleValue(), centerOfMassPosition); }
/** * generates garbage * @param polygon1 * @param polygon2 * @return */ //this should throw away points that are inside of the other polygon public static FrameConvexPolygon2dAndConnectingEdges combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2) { ReferenceFrame referenceFrame = polygon1.getReferenceFrame(); polygon2.checkReferenceFrameMatch(referenceFrame); ConvexPolygon2dAndConnectingEdges polygonAndEdges = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon); if (polygonAndEdges == null) return null; // Return null if not disjoint FrameConvexPolygon2d frameConvexPolygon2d = new FrameConvexPolygon2d(referenceFrame, polygonAndEdges.getConvexPolygon2d()); FrameLineSegment2d connectingEdge1 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge1()); FrameLineSegment2d connectingEdge2 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge2()); FrameConvexPolygon2dAndConnectingEdges ret = new FrameConvexPolygon2dAndConnectingEdges(polygon1, polygon2, frameConvexPolygon2d, connectingEdge1, connectingEdge2); return ret; }
private void intersectASinglePolygon(FrameConvexPolygon2d polygon, Pair<FramePoint, FramePoint> intersectionWithPolygon) { intersectionOfPlanes.changeFrame(polygon.getReferenceFrame()); intersectionOfPlanes.projectOntoXYPlane(planeIntersectionOnPolygonPlane); polygon.intersectionWith(planeIntersectionOnPolygonPlane, lineIntersectionOnPolygonPlane); if (lineIntersectionOnPolygonPlane.getFirst().containsNaN() && lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { noIntersection = true; } if (!lineIntersectionOnPolygonPlane.getFirst().containsNaN()) { intersectionWithPolygon.getFirst().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getFirst()); } if (!lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { intersectionWithPolygon.getSecond().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getSecond()); } }