public void getScanPoint(int index, FramePoint scanPointToPack) { index *= 3; scanPointToPack.setToZero(ReferenceFrame.getWorldFrame()); scanPointToPack.setX(scan[index++]); scanPointToPack.setY(scan[index++]); scanPointToPack.setZ(scan[index++]); }
public void getBasePosition(FramePoint framePoint) { framePoint.setX(this.baseX.getDoubleValue()); framePoint.setY(this.baseY.getDoubleValue()); framePoint.setZ(this.baseZ.getDoubleValue()); }
public void getBasePosition(FramePoint framePoint) { framePoint.setX(this.baseX.getDoubleValue()); framePoint.setY(this.baseY.getDoubleValue()); framePoint.setZ(this.baseZ.getDoubleValue()); }
public void getPosition(FramePoint framePoint) { framePoint.setX(this.getX()); framePoint.setY(this.getY()); if (this.z != null) framePoint.setZ(this.getZ()); else framePoint.setZ(0.0); }
public void getVoxel(FramePoint voxelLocationToPack, int xIndex, int yIndex, int zIndex) { voxelLocationToPack.setToZero(referenceFrame); voxelLocationToPack.setX(getCoordinateFromIndex(xIndex)); voxelLocationToPack.setY(getCoordinateFromIndex(yIndex)); voxelLocationToPack.setZ(getCoordinateFromIndex(zIndex)); }
public void setSupportPolygonVertex(int vertexIndex, FramePoint2d vertexLocation, ReferenceFrame frame, double xBuffer, double yBuffer) { tmpPoint.setToZero(frame); tmpPoint.setXY(vertexLocation); if (tmpPoint.getX() > 0.0) tmpPoint.setX(tmpPoint.getX() + xBuffer); else tmpPoint.setX(tmpPoint.getX() - xBuffer); if (tmpPoint.getY() > 0.0) tmpPoint.setY(tmpPoint.getY() + yBuffer); else tmpPoint.setY(tmpPoint.getY() - yBuffer); tmpPoint.changeFrame(worldFrame); cmpVertexLocations.get(vertexIndex).set(0, 0, tmpPoint.getX()); cmpVertexLocations.get(vertexIndex).set(1, 0, tmpPoint.getY()); }
framePoint.setY(listOfPoints[1][i].getY()); framePoint.setZ(listOfPoints[2][i].getY());
public static FramePoint getCenterOfFootstep(Footstep stance, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter) { FramePoint stanceCenter = new FramePoint(stance.getSoleReferenceFrame()); stanceCenter.getReferenceFrame().checkReferenceFrameMatch(stance.getSoleReferenceFrame()); stanceCenter.setX(stanceCenter.getX() + centimetersForwardFromCenter); stanceCenter.setY(stanceCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter)); stanceCenter.changeFrame(worldFrame); return stanceCenter; }
public void getClosestVoxel(FramePoint voxelLocationToPack, FramePoint inputPoint) { checkReferenceFrameMatch(inputPoint); if (!boundingBox.isInside(inputPoint.getPoint())) throw new RuntimeException("The given point is outside the grid"); voxelLocationToPack.setToZero(getReferenceFrame()); voxelLocationToPack.setX(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getX()))); voxelLocationToPack.setY(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getY()))); voxelLocationToPack.setZ(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getZ()))); }
public void initialize(FramePoint initialPosition, FramePoint finalPosition, double heightAtParameter, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); FramePoint intermediatePosition = new FramePoint(referenceFrame); intermediatePosition.setX(initialPosition.getX() + q * (finalPosition.getX() - initialPosition.getX())); intermediatePosition.setY(initialPosition.getY() + q * (finalPosition.getY() - initialPosition.getY())); intermediatePosition.setZ(heightAtParameter); initialize(initialPosition, intermediatePosition, finalPosition, q); }
coMHeightPartialDerivativesData.getCoMHeight(tempFramePointForViz2); tempFramePointForViz2.setX(tempFramePointForViz1.getX()); tempFramePointForViz2.setY(tempFramePointForViz1.getY());