public FrameConvexPolygon2d getContactPolygonCopy() { return new FrameConvexPolygon2d(soleFrame, contactPoints); }
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 setSupportPolygon(FrameConvexPolygon2d supportPolygon) { this.supportPolygon.setIncludingFrameAndUpdate(supportPolygon); this.supportPolygon.changeFrameAndProjectToXYPlane(worldFrame); supportUpToDate = true; }
@Override public FrameConvexPolygon2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FrameConvexPolygon2d ret = new FrameConvexPolygon2d(this); ret.changeFrameAndProjectToXYPlane(desiredFrame); return ret; }
@Override public FrameConvexPolygon2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform) { FrameConvexPolygon2d copy = new FrameConvexPolygon2d(this); copy.applyTransformAndProjectToXYPlane(transform); return copy; }
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); }
safeArea.setIncludingFrame(supportPolygon); tempPolygon1.setIncludingFrameAndUpdate(defaultFootPolygons.get(nextFootstep.getRobotSide())); tempPolygon2.setIncludingFrameAndUpdate(nextFootstep.getSoleReferenceFrame(), tempPolygon1.getConvexPolygon2d()); tempPolygon2.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); polygonShrinker.shrinkConstantDistanceInto(tempPolygon2, -distanceToExtendUpcomingFoothold.getDoubleValue(), tempPolygon1); safeArea.addVertices(tempPolygon1); safeArea.update(); safeCMPArea.setIncludingFrameAndUpdate(safeArea); safeArea.setIncludingFrameAndUpdate(tempPolygon1); safeArea.setIncludingFrameAndUpdate(tempPolygon1); safeArea.changeFrameAndProjectToXYPlane(worldFrame); capturePoint2d.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); boolean icpInSafeArea = safeArea.isPointInside(capturePoint2d);
this.supportFootPolygon.setIncludingFrameAndUpdate(footPolygon); this.supportFootPolygon.changeFrameAndProjectToXYPlane(supportAnkleZUp); previousSwingSide = swingSide; supportFootPolygon.getCentroid(footCentroid); rawCaptureRegion.clear(supportAnkleZUp); captureRegionPolygon.clear(supportAnkleZUp); ArrayList<FramePoint2d> extremesOfFeasibleCOP = supportFootPolygon.getAllVisibleVerticesFromOutsideLeftToRightCopy(capturePoint); if (extremesOfFeasibleCOP == null) captureRegionPolygon.setIncludingFrameAndUpdate(reachableRegions.get(swingSide.getOppositeSide())); updateVisualizer(); return; copExtreme.changeFrame(supportAnkleZUp); CaptureRegionMathTools.predictCapturePoint(capturePoint, copExtreme, swingTimeRemaining, omega0, predictedICP); rawCaptureRegion.addVertexChangeFrameAndProjectToXYPlane(predictedICP); captureRegionPolygon.update(); return; rawCaptureRegion.addVertexChangeFrameAndProjectToXYPlane(kinematicExtreme); captureRegionMath.getPointBetweenVectorsAtDistanceFromOriginCircular(firstKinematicExtremeDirection, lastKinematicExtremeDirection, alphaFromAToB, APPROXIMATION_MULTILIER * kinematicStepRange, footCentroid, additionalKinematicPoint); rawCaptureRegion.addVertexAndChangeFrame(additionalKinematicPoint); if (!rawCaptureRegion.isEmpty())
if (projectionArea.isPointInside(desiredCMP)) projectionArea.getBoundingBox(tempBoundingBox); if (tempBoundingBox.getDiagonalLengthSquared() < 0.01 * 0.01) projectionArea.getCentroid(projectedCMP); activeProjection.set(ProjectionMethod.SMALL_AREA_CENTROID); return; if (projectionArea.distance(capturePoint) < 1.0e-6) projectionArea.getCentroid(centroid); centroid.sub(capturePoint); centroid.scale(1.0e-6); if (projectionArea.intersectionWithRay(icpToCMPLine, intersection1, intersection2) > 0) icpToCMPVector.setToZero(projectionArea.getReferenceFrame()); icpToCMPVector.sub(desiredCMP, capturePoint); icpToCandidateVector.setToZero(projectionArea.getReferenceFrame()); icpToCandidateVector.sub(candidate, capturePoint); if (projectionArea.intersectionWithRay(finalICPToICPLine, intersection1, intersection2) > 0) finalICPToICPVector.setToZero(projectionArea.getReferenceFrame()); finalICPToICPVector.sub(capturePoint, finalCapturePoint); icpToCandidateVector.setToZero(projectionArea.getReferenceFrame()); icpToCandidateVector.sub(candidate, capturePoint);
shrunkFootPolygon.set(backupFootPolygon); return false; shrunkFootPolygon.set(backupFootPolygon); return false; shrunkFootPolygon.set(backupFootPolygon); return false; controllerFootPolygon.setIncludingFrame(shrunkFootPolygon); ConvexPolygonTools.limitVerticesConservative(controllerFootPolygon, footCornerPoints); controllerFootPolygonInWorld.setIncludingFrameAndUpdate(controllerFootPolygon); controllerFootPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame); fullSupportAfterShrinking.setIncludingFrameAndUpdate(oppositeFootPolygon); fullSupportAfterShrinking.changeFrameAndProjectToXYPlane(worldFrame); fullSupportAfterShrinking.addVertices(controllerFootPolygonInWorld); fullSupportAfterShrinking.update(); momentumBasedController.getCapturePoint(capturePoint); yoFullSupportAfterShrinking.setFrameConvexPolygon2d(fullSupportAfterShrinking); for (int i = 0; i < controllerFootPolygon.getNumberOfVertices(); i++) controllerFootPolygon.getFrameVertexXY(i, tempPosition); contactPoints.get(i).setPosition(tempPosition); backupFootPolygon.set(shrunkFootPolygon); shrinkCounter.increment(); 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(); } }
cmpProjectedToVertex.set(false); supportPolygon.getBoundingBox(tempBoundingBox); double diagonalLengthSquared = tempBoundingBox.getDiagonalLengthSquared(); supportPolygon.getCentroid(desiredCMP); return; if (supportPolygon.isPointInside(desiredCMP)) return; int intersections = supportPolygon.intersectionWithRay(icpToCMPLine, intersection1, intersection2); rayFromICPAwayFromFinalDesiredICP.setIncludingFrame(capturePoint, finalDesiredICPToICPDirection); FramePoint2d[] finalDesiredICPToICPIntersections = supportPolygon.intersectionWith(rayFromICPAwayFromFinalDesiredICP); boolean success = supportPolygon.getClosestPointWithRay(desiredCMP, rayFromICPAwayFromFinalDesiredICP); if (!success) supportPolygon.getClosestVertex(desiredCMP, capturePoint); return; supportPolygon.orthogonalProjection(desiredCMP);
if (predictedContactPoints != null && !predictedContactPoints.isEmpty()) nextFootstepPolygon.setIncludingFrameAndUpdate(footstepSoleFrame, predictedContactPoints); ConvexPolygon2d footPolygon = footDefaultPolygons.get(nextFootstep.getRobotSide()).getConvexPolygon2d(); nextFootstepPolygon.setIncludingFrameAndUpdate(footstepSoleFrame, footPolygon); nextFootstepPolygon.changeFrameAndProjectToXYPlane(worldFrame); isDesiredECMPOKForToeOff.set(isOnExitCMP && onToesSupportPolygon.isPointInside(desiredECMP)); else isDesiredECMPOKForToeOff.set(isOnExitCMP); onToesSupportPolygon.isPointInside(desiredICP) && nextFootstepPolygon.distance(desiredICP) < (icpProximityToLeadingFootForSSToeOff.getDoubleValue()); isCurrentICPOKForToeOff = onToesSupportPolygon.isPointInside(currentICP) && nextFootstepPolygon.distance(currentICP) < (icpProximityToLeadingFootForSSToeOff.getDoubleValue()); isDesiredICPOKForToeOff = nextFootstepPolygon.isPointInside(desiredICP); isCurrentICPOKForToeOff = nextFootstepPolygon.isPointInside(currentICP);
int corners = supportPolygon.getNumberOfVertices(); currentCornerIdx = (int) (timeExploring / timeToExploreCorner); int corner = currentCornerIdx % corners; yoCurrentCorner.set(corner); supportPolygon.getFrameVertex(corner, currentCorner); FramePoint2d centroid = supportPolygon.getCentroid();
private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB) { footA.getCentroid(tempCentroid); firstCMP.setXYIncludingFrame(tempCentroid); firstCMP.changeFrame(worldFrame); footB.getCentroid(tempCentroid); secondCMP.setXYIncludingFrame(tempCentroid); secondCMP.changeFrame(worldFrame); upcomingSupport.clear(worldFrame); tempFootPolygon.setIncludingFrame(footA); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); tempFootPolygon.setIncludingFrame(footB); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); upcomingSupport.update(); entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); upcomingSupport.getCentroid(tempCentroid); tempCentroid3d.setXYIncludingFrame(tempCentroid); double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0); if (chicken <= 0.5) entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0); else entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0); exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex)); }
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); } }
private void updateOnToesSupportPolygon(RobotSide trailingSide, FrameConvexPolygon2d leadingFootSupportPolygon) { computeToePoints(trailingSide); middleToePoint.changeFrame(worldFrame); onToesSupportPolygon.setIncludingFrameAndUpdate(leadingFootSupportPolygon); onToesSupportPolygon.changeFrameAndProjectToXYPlane(worldFrame); onToesSupportPolygon.addVertexByProjectionOntoXYPlane(middleToePoint); onToesSupportPolygon.update(); } }
leadingFootSupportPolygon.setIncludingFrameByProjectionOntoXYPlaneAndUpdate(worldFrame, contactStatePoints); leadingFootSupportPolygon.setIncludingFrameAndUpdate(footDefaultPolygons.get(leadingLeg)); leadingFootSupportPolygon.changeFrameAndProjectToXYPlane(worldFrame); isDesiredECMPOKForToeOff.set(onToesSupportPolygon.isPointInside(desiredECMP)); else isDesiredECMPOKForToeOff.set(true); onToesSupportPolygon.isPointInside(desiredICP) && leadingFootSupportPolygon.distance(desiredICP) < (icpProximityToLeadingFootForDSToeOff.getDoubleValue()); isCurrentICPOKForToeOff = onToesSupportPolygon.isPointInside(currentICP) && leadingFootSupportPolygon.distance(currentICP) < (icpProximityToLeadingFootForDSToeOff.getDoubleValue()); isDesiredICPOKForToeOff = leadingFootSupportPolygon.isPointInside(desiredICP); isCurrentICPOKForToeOff = leadingFootSupportPolygon.isPointInside(currentICP);
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 static void movePointInsidePolygonAlongVector(FramePoint2d pointToMove, FrameVector2d vector, FrameConvexPolygon2d polygon, double distanceToBeInside) if (polygon.getNumberOfVertices() < 2) FramePoint2d[] intersections = polygon.intersectionWith(line); boolean insidePolygon = polygon.isPointInside(pointToMove); if (!insidePolygon) pointToMove.set(polygon.getClosestVertexCopy(line));