public void updateParameters(FootstepSnappingParameters 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 void updateParameters(FootstepSnappingParameters newParameters) { parameters.updateParameters(newParameters); footstepValueFunction.updateFunction(); this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
public AdjustingFootstepSnapper(FootstepValueFunction valueFunction, FootstepSnappingParameters parameters) { convexHullFootstepSnapper = new ConvexHullFootstepSnapper(valueFunction, parameters); this.footstepSnappingParameters = parameters; this.distanceAdjustment = parameters.getDistanceAdjustment(); this.angleAdjustment = parameters.getAngleAdjustment(); }
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(); Point2d 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) { Matrix3d rotationMatrix = new Matrix3d(); 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.predictedContactPoints == null || footstepData.predictedContactPoints.isEmpty()) return Double.NEGATIVE_INFINITY; ConvexPolygon2d supportPolygon = new ConvexPolygon2d(footstepData.getPredictedContactPoints()); 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 void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
@Override public Footstep.FootstepType snapFootstep(FootstepDataMessage footstep, HeightMapWithPoints heightMap) { FootstepDataMessage originalFootstepFound = new FootstepDataMessage(footstep); Point3d position = originalFootstepFound.getLocation(); double yaw = RotationTools.computeYaw(originalFootstepFound.getOrientation()); 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, FootstepSnappingParameters 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 ArrayList<Point2d> reduceListOfPointsToFourFootstepBased(List<Point2d> listOfPoints) { ConvexPolygon2d basePolygon = parameters.getCollisionPolygon(); ConvexPolygon2d supportPolygon = new ConvexPolygon2d(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++) { Point2d vertex = basePolygon.getVertex(i); Point2d correspondingSupportPoint = getPointInPolygonNearestPoint(supportPolygon, vertex); finalListOfSupportPoints.add(correspondingSupportPoint); } return finalListOfSupportPoints; }