private void visualize() { supportPolygonViz.setFrameConvexPolygon2d(supportPolygonInWorld); for (RobotSide robotSide : RobotSide.values) { YoFrameConvexPolygon2d footPolygonViz = footPolygonsViz.get(robotSide); FrameConvexPolygon2d footPolygon = footPolygonsInWorldFrame.get(robotSide); if (footPolygon.isEmpty()) footPolygonViz.hide(); else footPolygonViz.setFrameConvexPolygon2d(footPolygon); } }
if (captureRegion.isEmpty())
@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); } }
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(); } }
isCaptureRegionEmpty.set(captureRegion.isEmpty()); if (!recovering.getBooleanValue())
if (!rawCaptureRegion.isEmpty())