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(); } }
public AdjustingFootstepSnapper(FootstepValueFunction valueFunction, QuadTreeFootstepSnappingParameters parameters) { convexHullFootstepSnapper = new ConvexHullFootstepSnapper(valueFunction, parameters); this.footstepSnappingParameters = parameters; this.distanceAdjustment = parameters.getDistanceAdjustment(); this.angleAdjustment = parameters.getAngleAdjustment(); }
public void updateParameters(QuadTreeFootstepSnappingParameters newParameters) { parameters.updateParameters(newParameters); footstepValueFunction.updateFunction(); this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
@Override public void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
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)); } }
@Override public double getFootstepValue(FootstepDataMessage footstepData) { RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.set(footstepData.getOrientation()); Vector3D footstepNormal = new Vector3D(); rotationMatrix.getColumn(2, footstepNormal); double offHorizontalAngle = Math.acos(footstepNormal.getZ()); if (offHorizontalAngle > parameters.getMaxAngle() || footstepNormal.getZ() < 0) return Double.NEGATIVE_INFINITY; double value = slopeGain * offHorizontalAngle; if (footstepData.getPredictedContactPoints2d() == null || footstepData.getPredictedContactPoints2d().isEmpty()) return Double.NEGATIVE_INFINITY; ConvexPolygon2D supportPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(HumanoidMessageTools.unpackPredictedContactPoints(footstepData))); supportPolygon.update(); double inPlaneArea = supportPolygon.getArea(); double horizonalArea = inPlaneArea * footstepNormal.getZ(); if (horizonalArea < parameters.getMinArea()) return Double.NEGATIVE_INFINITY; value += horizonalArea * areaGain; return value; }
boolean badPlane = false; int badnumberOfPointsthreshold = parameters.getBadnumberOfPointsthreshold(); double tolerance = parameters.getZDistanceTolerance(); double errorThreshold = 1.0;
@Override public Footstep.FootstepType snapFootstep(FootstepDataMessage footstep, HeightMapWithPoints heightMap) { FootstepDataMessage originalFootstepFound = new FootstepDataMessage(footstep); Point3D position = originalFootstepFound.getLocation(); double yaw = originalFootstepFound.getOrientation().getYaw(); if (!useMask) { pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength()); } else { footstepMask.setPositionAndYaw(position.getX(), position.getY(), yaw); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength(), footstepMask); } double height = heightMap.getHeightAtPoint(position.getX(), position.getY()); return snapFootstep(footstep, pointList, height); }
ConvexPolygon2D maxSupportPolygon = parameters.getSupportPolygon(); ConvexPolygon2D actualSupportPolygon = new ConvexPolygon2D(); for (Point2D point2d : pointsInFootstepFrame)
double distanceTolerance = parameters.getZDistanceTolerance();
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"); }
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; }