public void checkIfRobotIsFalling(FramePoint2d capturePoint2d, FramePoint2d desiredCapturePoint2d) { updateCombinedPolygon(); if (isUsingNextFootstep.getBooleanValue()) icpDistanceFromFootPolygon.set(combinedFootPolygonWithNextFootstep.distance(capturePoint2d)); else icpDistanceFromFootPolygon.set(combinedFootPolygon.distance(capturePoint2d)); // TODO need to investigate this method, seems to be buggy // boolean isCapturePointCloseToFootPolygon = combinedFootPolygon.isPointInside(capturePoint, icpDistanceFromFootPolygonThreshold.getDoubleValue()); boolean isCapturePointCloseToFootPolygon = icpDistanceFromFootPolygon.getDoubleValue() < icpDistanceFromFootPolygonThreshold.getDoubleValue(); boolean isCapturePointCloseToDesiredCapturePoint = desiredCapturePoint2d.distance(capturePoint2d) < icpDistanceFromFootPolygonThreshold.getDoubleValue(); isRobotFalling.set(!isCapturePointCloseToFootPolygon && !isCapturePointCloseToDesiredCapturePoint); if (isRobotFalling.getBooleanValue()) { tempFallingDirection.set(capturePoint2d); FramePoint2d footCenter = combinedFootPolygon.getCentroid(); tempFallingDirection.changeFrame(ReferenceFrame.getWorldFrame()); footCenter.changeFrame(ReferenceFrame.getWorldFrame()); tempFallingDirection.sub(footCenter); fallingDirection.set(tempFallingDirection); } }
onToesSupportPolygon.isPointInside(desiredICP) && leadingFootSupportPolygon.distance(desiredICP) < (icpProximityToLeadingFootForDSToeOff.getDoubleValue()); isCurrentICPOKForToeOff = onToesSupportPolygon.isPointInside(currentICP) && leadingFootSupportPolygon.distance(currentICP) < (icpProximityToLeadingFootForDSToeOff.getDoubleValue());
onToesSupportPolygon.isPointInside(desiredICP) && nextFootstepPolygon.distance(desiredICP) < (icpProximityToLeadingFootForSSToeOff.getDoubleValue()); isCurrentICPOKForToeOff = onToesSupportPolygon.isPointInside(currentICP) && nextFootstepPolygon.distance(currentICP) < (icpProximityToLeadingFootForSSToeOff.getDoubleValue());
if (projectionArea.distance(capturePoint) < 1.0e-6)