@Override public void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
public ConvexHullFootstepSnapper(FootstepValueFunction valueFunction, QuadTreeFootstepSnappingParameters parameters) { this.footstepValueFunction = valueFunction; this.parameters = parameters; this.useMask = true; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); if (writer == null) writer = new PointWriter("DataFromConvexHullSnapper"); }
public void updateParameters(QuadTreeFootstepSnappingParameters newParameters) { parameters.updateParameters(newParameters); footstepValueFunction.updateFunction(); this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
private List<Point2D> reduceListOfPointsToFourFootstepBased(List<? extends Point2DReadOnly> listOfPoints) { ConvexPolygon2D basePolygon = parameters.getCollisionPolygon(); ConvexPolygon2D supportPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(listOfPoints)); supportPolygon.update(); ArrayList<Point2D> finalListOfSupportPoints = new ArrayList<Point2D>(); //for each vertex of the basePolygon, find the closest point inside the support polygon. int size = basePolygon.getNumberOfVertices(); for (int i = 0; i < size; i++) { Point2DReadOnly vertex = basePolygon.getVertex(i); Point2D correspondingSupportPoint = getPointInPolygonNearestPoint(supportPolygon, vertex); finalListOfSupportPoints.add(correspondingSupportPoint); } return finalListOfSupportPoints; }
private SimpleFootstepSnapper createSimpleFootstepSnapper() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); BasicFootstepMask footstepMask = new BasicFootstepMask(snappingParameters.getCollisionPolygon(), 0.0); SimpleFootstepSnapper footstepSnapper = new SimpleFootstepSnapper(); boolean useMask = true; double kernelMaskSafetyBuffer = 0.15; double boundingBoxDimension = 0.15; footstepSnapper.setUseMask(useMask, kernelMaskSafetyBuffer, boundingBoxDimension); footstepSnapper.setMask(footstepMask); return footstepSnapper; }
private SimpleFootstepSnapper createSimpleFootstepSnapper() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); BasicFootstepMask footstepMask = new BasicFootstepMask(snappingParameters.getCollisionPolygon(), 0.0); SimpleFootstepSnapper footstepSnapper = new SimpleFootstepSnapper(); boolean useMask = true; double kernelMaskSafetyBuffer = 0.15; double boundingBoxDimension = 0.15; footstepSnapper.setUseMask(useMask, kernelMaskSafetyBuffer, boundingBoxDimension); footstepSnapper.setMask(footstepMask); return footstepSnapper; }
private void addLowerBoundaryPointsToHullPointList(List<Point3D> convexHullPointsList, double xOrigin, double yOrigin, double yaw) { double minHeight = Double.POSITIVE_INFINITY; for (Point3D point : convexHullPointsList) { if (point.getZ() < minHeight) minHeight = point.getZ(); } ConvexPolygon2D lowerBoundPolygon = parameters.getCollisionPolygon(); double dropDistance = parameters.getBoundingSquareSizeLength(); double cosYaw = Math.cos(yaw); double sinYaw = Math.sin(yaw); int numVertices = lowerBoundPolygon.getNumberOfVertices(); Point2DReadOnly vertex; double xCoord; double yCoord; for (int i = 0; i < numVertices; i++) { vertex = lowerBoundPolygon.getVertex(i); xCoord = xOrigin + cosYaw * vertex.getX() - sinYaw * vertex.getY(); yCoord = yOrigin + cosYaw * vertex.getY() + sinYaw * vertex.getX(); convexHullPointsList.add(new Point3D(xCoord, yCoord, minHeight - dropDistance)); } }
public void updateParameters(QuadTreeFootstepSnappingParameters newParameters){ collisionPolygon = newParameters.getCollisionPolygon(); supportPolygon = newParameters.getSupportPolygon(); boundingSquareSizeLength = newParameters.getBoundingSquareSizeLength(); maxAngle = newParameters.getMaxAngle(); minArea = newParameters.getMinArea(); zDistanceTolerance = newParameters.getZDistanceTolerance(); this.distanceAdjustment = newParameters.getDistanceAdjustment(); this.angleAdjustment = newParameters.getAngleAdjustment(); this.badnumberOfPointsthreshold = newParameters.getBadnumberOfPointsthreshold(); } }