public boolean isPointInside(FramePoint2d framePoint) { return isPointInside(framePoint, 0.0); }
private void drawTestPoints(ArrayList<FramePoint2d> testPoints, FrameConvexPolygon2d polygon, Graphics graphics) { for (FramePoint2d testPoint : testPoints) { boolean isInside = polygon.isPointInside(testPoint); if (isInside) { drawInsidePoint(testPoint, graphics); } else { drawOutsidePoint(testPoint, graphics); } } }
@Override public boolean isDone() { if (!balanceManager.isICPPlanDone()) return false; balanceManager.getCapturePoint(capturePoint2d); FrameConvexPolygon2d supportPolygonInWorld = momentumBasedController.getBipedSupportPolygons().getSupportPolygonInWorld(); boolean isICPInsideSupportPolygon = supportPolygonInWorld.isPointInside(capturePoint2d); if (!isICPInsideSupportPolygon) return true; else return balanceManager.isTransitionToSingleSupportSafe(transferToSide); }
public void update() { momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); if (desiredCoP.containsNaN()) isDesiredCoPOnEdge.set(false); else { double epsilon = isDesiredCoPOnEdge.getBooleanValue() ? EPSILON_POINT_ON_EDGE_WITH_HYSTERESIS : EPSILON_POINT_ON_EDGE; FrameConvexPolygon2d footSupportPolygon = bipedSupportPolygons.getFootPolygonInSoleFrame(robotSide); isDesiredCoPOnEdge.set(!footSupportPolygon.isPointInside(desiredCoP, -epsilon)); // Minus means that the check is done with a smaller polygon } }
@Override public void computeCMPInternal(FramePoint2d desiredCMPPreviousValue) { icpOptimizationController.compute(yoTime.getDoubleValue(), desiredCapturePoint, desiredCapturePointVelocity, capturePoint, omega0); icpOptimizationController.getDesiredCMP(desiredCMP); yoUnprojectedDesiredCMP.set(desiredCMP); // do projection here: if (!areaToProjectInto.isEmpty()) { desiredCMPinSafeArea.set(safeArea.isPointInside(desiredCMP)); if (safeArea.isPointInside(desiredCMP)) { supportPolygon.setIncludingFrameAndUpdate(bipedSupportPolygons.getSupportPolygonInMidFeetZUp()); areaToProjectInto.setIncludingFrameAndUpdate(supportPolygon); } cmpProjector.projectCMPIntoSupportPolygonIfOutside(capturePoint, areaToProjectInto, finalDesiredCapturePoint, desiredCMP); } }
boolean isCoPInsideFoot = footPolygon.isPointInside(tempCoP2d); if (!isCoPInsideFoot)
public boolean isIncluded(Point3d testInput) { FramePoint test = new FramePoint(ReferenceFrame.getWorldFrame(),testInput); if (DEBUG) System.out.printf("SimpleFootstepMask: testing point input is <%.2f,%.2f> in frame %s", test.getX(), test.getY(), test.getReferenceFrame()); test.changeFrame(yawFrame2d); boolean isPointInside = footPolygon.isPointInside(test.toFramePoint2d()); if (DEBUG) System.out.printf("SimpleFootstepMask: testing point <%.2f,%.2f> in frame %s. Point is in is %s. ", test.getX(), test.getY(), test.getReferenceFrame(),isPointInside ? "true":"false"); return isPointInside; }
@Override public void doAction() { super.doAction(); if (walkingMessageHandler.hasFootTrajectoryForFlamingoStance(swingSide)) { while(walkingMessageHandler.hasFootTrajectoryForFlamingoStance(swingSide)) feetManager.handleFootTrajectoryCommand(walkingMessageHandler.pollFootTrajectoryForFlamingoStance(swingSide)); } balanceManager.getCapturePoint(capturePoint2d); boolean icpErrorIsTooLarge = balanceManager.getICPErrorMagnitude() > 0.05; if (icpErrorIsTooLarge) { FrameConvexPolygon2d supportPolygonInWorld = bipedSupportPolygons.getSupportPolygonInWorld(); FrameConvexPolygon2d combinedFootPolygon = failureDetectionControlModule.getCombinedFootPolygon(); if (!supportPolygonInWorld.isPointInside(capturePoint2d, 2.0e-2) && combinedFootPolygon.isPointInside(capturePoint2d)) { feetManager.requestMoveStraightTouchdownForDisturbanceRecovery(swingSide); initiateFootLoadingProcedure(swingSide); balanceManager.requestICPPlannerToHoldCurrentCoMInNextDoubleSupport(); } } walkingMessageHandler.clearFootTrajectory(supportSide); }
public void computeCMPInternal(FramePoint2d desiredCMPPreviousValue) { if (supportSide != supportLegPreviousTick.getEnumValue()) { icpProportionalController.reset(); } desiredCMP.set(icpProportionalController.doProportionalControl(desiredCMPPreviousValue, capturePoint, desiredCapturePoint, finalDesiredCapturePoint, desiredCapturePointVelocity, perfectCMP, omega0)); yoUnprojectedDesiredCMP.set(desiredCMP); // do projection here: if (!areaToProjectInto.isEmpty()) { desiredCMPinSafeArea.set(safeArea.isPointInside(desiredCMP)); if (safeArea.isPointInside(desiredCMP)) { supportPolygon.setIncludingFrameAndUpdate(bipedSupportPolygons.getSupportPolygonInMidFeetZUp()); areaToProjectInto.setIncludingFrameAndUpdate(supportPolygon); } cmpProjector.projectCMPIntoSupportPolygonIfOutside(capturePoint, areaToProjectInto, finalDesiredCapturePoint, desiredCMP); if (cmpProjector.getWasCMPProjected()) icpProportionalController.bleedOffIntegralTerm(); } }
private void projectCMPIntoSupportPolygonIfOutsideLocal(FrameConvexPolygon2d supportPolygon, FramePoint2d desiredCMPToPack) { cmpProjected.set(false); if (supportPolygon.getArea() < 1.0e-3) { supportPolygon.getCentroid(desiredCMPToPack); return; } if (supportPolygon.isPointInside(projectedCMP)) { return; } else { supportPolygon.getClosestVertex(projectedCMP, projectedCMP); cmpProjected.set(true); } desiredCMPToPack.setX(projectedCMP.getX()); }
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);
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);
@Deprecated public static void movePointInsidePolygonAlongLine(FramePoint2d point, FrameConvexPolygon2d polygon, FrameLine2d line, double amountToBeInside) if (!polygon.isPointInside(point))
@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(); }
private void checkIfUseUpperBodyMomentumDoubleSupport() { safeArea.setIncludingFrameAndUpdate(supportPolygon); safeCMPArea.setIncludingFrameAndUpdate(safeArea); // hysteresis: // shrink the safe area if we are already using upper body momentum if (usingUpperBodyMomentum.getBooleanValue()) { polygonShrinker.shrinkConstantDistanceInto(safeArea, distanceToShrinkSafeAreaIfRecoveringDS.getDoubleValue(), tempPolygon1); safeArea.setIncludingFrameAndUpdate(tempPolygon1); } else { polygonShrinker.shrinkConstantDistanceInto(safeArea, distanceToShrinkSafeAreaDS.getDoubleValue(), tempPolygon1); safeArea.setIncludingFrameAndUpdate(tempPolygon1); } capturePoint2d.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); boolean icpInSafeArea = safeArea.isPointInside(capturePoint2d); if (!icpInSafeArea) { usingUpperBodyMomentum.set(true); } else { usingUpperBodyMomentum.set(false); } }
private void computeShrunkFoothold(FramePoint2d desiredCenterOfPressure) { boolean wasCoPInThatRegion = false; if (useCoPOccupancyGrid.getBooleanValue()) { numberOfCellsOccupiedOnSideOfLine.set(footCoPOccupancyGrid.computeNumberOfCellsOccupiedOnSideOfLine(lineOfRotation, RobotSide.RIGHT, distanceFromLineOfRotationToComputeCoPOccupancy.getDoubleValue())); wasCoPInThatRegion = numberOfCellsOccupiedOnSideOfLine.getIntegerValue() >= thresholdForCoPRegionOccupancy.getIntegerValue(); } unsafeArea.set(unsafePolygon.getArea()); boolean areaBigEnough = unsafeArea.getDoubleValue() >= minAreaToConsider.getDoubleValue(); unsafeAreaAboveThreshold.set(areaBigEnough); boolean desiredCopInPolygon = unsafePolygon.isPointInside(desiredCenterOfPressure, 0.0e-3); if (desiredCopInPolygon && !wasCoPInThatRegion && areaBigEnough) { backupFootPolygon.set(shrunkFootPolygon); ConvexPolygonTools.cutPolygonWithLine(lineOfRotation, shrunkFootPolygon, RobotSide.RIGHT); unsafePolygon.changeFrameAndProjectToXYPlane(worldFrame); yoUnsafePolygon.setFrameConvexPolygon2d(unsafePolygon); shrunkFootPolygonInWorld.setIncludingFrame(shrunkFootPolygon); shrunkFootPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame); yoShrunkFootPolygon.setFrameConvexPolygon2d(shrunkFootPolygonInWorld); } else { doNothing(); } }
boolean icpInSafeArea = safeArea.isPointInside(capturePoint2d);
if (supportPolygon.isPointInside(desiredCMP)) return;
return; boolean isCoPOnEdge = !footPolygon.isPointInside(cop, -EPSILON);
isICPOutside.set(!supportPolygonInMidFeetZUp.isPointInside(this.capturePoint2d));