public void setCenterOfPressure(FramePoint2d centerOfPressure, RigidBody foot) { if (centerOfPressure != null) { centerOfPressure.checkReferenceFrameMatch(soleFrames.get(foot)); centerOfPressure.get(centerOfPressures.get(foot)); } else { centerOfPressures.get(foot).set(Double.NaN, Double.NaN); } }
public boolean isCellAtLocationOccupied(FramePoint2d location) { location.checkReferenceFrameMatch(soleFrame); location.get(tempPoint); tempPoint.setX(tempPoint.getX() - gridOrigin.getX()); tempPoint.setY(tempPoint.getY() - gridOrigin.getY()); int xIndex = findXIndex(tempPoint.getX()); int yIndex = findYIndex(tempPoint.getY()); if (checkIfIndicesAreValid(xIndex, yIndex)) return occupancyGrid.get(xIndex, yIndex) > 0.9; else return false; }
public void registerCenterOfPressureLocation(FramePoint2d copToRegister) { copToRegister.checkReferenceFrameMatch(soleFrame); copToRegister.get(tempPoint); tempPoint.setX(tempPoint.getX() - gridOrigin.getX()); tempPoint.setY(tempPoint.getY() - gridOrigin.getY()); int xIndex = findXIndex(tempPoint.getX()); int yIndex = findYIndex(tempPoint.getY()); currentXIndex.set(xIndex); currentYIndex.set(yIndex); areCurrentCoPIndicesValid.set(checkIfIndicesAreValid(xIndex, yIndex)); if (areCurrentCoPIndicesValid.getBooleanValue()) { counterGrid.add(xIndex, yIndex, 1.0); if (counterGrid.get(xIndex, yIndex) >= thresholdForCellActivation.getDoubleValue()) { occupancyGrid.set(xIndex, yIndex, 1.0); if (VISUALIZE) { getCellCenter(cellCenter, xIndex, yIndex); cellPosition.setXYIncludingFrame(cellCenter); cellViz[xIndex][yIndex].setAndMatchFrame(cellPosition); } } else { occupancyGrid.set(xIndex, yIndex, 0.0); } } }
public boolean isPointOnLeftSideOfLineSegment(FramePoint2d point) { checkReferenceFrameMatch(point); point.get(tempPoint2d); return this.lineSegment.isPointOnLeftSideOfLineSegment(tempPoint2d); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(point2d); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); framePoint2d.setIncludingFrame(currentReferenceFrame, point2d); framePoint2d.changeFrame(desiredFrame); framePoint2d.get(point2d); set(point2d); }
/** * Packs the linear velocity of a point2d that is fixed in bodyFrame but is expressed in baseFrame, * with respect to baseFrame, expressed in expressedInFrame */ public void getLineaVelocityOfPoint2dFixedInBodyFrame(FrameVector linearVelocityToPack, FramePoint2d point2dFixedInBodyFrame) { baseFrame.checkReferenceFrameMatch(expressedInFrame); point2dFixedInBodyFrame.checkReferenceFrameMatch(baseFrame); point2dFixedInBodyFrame.get(freeVector); linearVelocityToPack.setToZero(expressedInFrame); linearVelocityToPack.cross(angularPart, freeVector); linearVelocityToPack.add(linearPart); }
public CapturabilityBasedStatus updateAndReturnCapturabilityBasedStatus() { yoDesiredCapturePoint.getFrameTuple2dIncludingFrame(desiredCapturePoint2d); centerOfMassPosition.setToZero(centerOfMassFrame); centerOfMassPosition.changeFrame(worldFrame); momentumBasedController.getCapturePoint(capturePoint2d); capturePoint2d.checkReferenceFrameMatch(worldFrame); desiredCapturePoint2d.checkReferenceFrameMatch(worldFrame); SideDependentList<FrameConvexPolygon2d> footSupportPolygons = bipedSupportPolygons.getFootPolygonsInWorldFrame(); capturePoint2d.get(capturabilityBasedStatus.capturePoint); desiredCapturePoint2d.get(capturabilityBasedStatus.desiredCapturePoint); centerOfMassPosition.get(capturabilityBasedStatus.centerOfMass); for (RobotSide robotSide : RobotSide.values) { capturabilityBasedStatus.setSupportPolygon(robotSide, footSupportPolygons.get(robotSide)); } return capturabilityBasedStatus; }