@Override public double getMinArea() { return supportPolygon.getArea() * 0.01; } }
@Override public double getMinArea() { return supportPolygon.getArea() * 0.25; } }
public double getArea() { return convexPolygon.getArea(); }
private boolean checkArea(FootstepDataMessage footstep, double percentageToCheckAgainst, SteppingParameters parameters){ if (footstep.predictedContactPoints == null) return true; ConvexPolygon2d convexPolygon2d = new ConvexPolygon2d(footstep.getPredictedContactPoints()); convexPolygon2d.update(); double area = convexPolygon2d.getArea(); double percentage = area / parameters.getFootstepArea(); if (percentage + EPSILON < percentageToCheckAgainst) return false; return true; } }
/** * Returns a transform that will move the given polygon into a planar region. Problematic if the planar region consists of * multiple sub convex polygons. The polygon to wiggle must have the same transform to world as the planar region. * * @param polygonToWiggleInRegionFrame * @param regionToWiggleInto * @param wiggleParameters * @return */ public static RigidBodyTransform wigglePolygonIntoRegion(ConvexPolygon2d polygonToWiggleInRegionFrame, PlanarRegion regionToWiggleInto, WiggleParameters parameters) { // find the part of the region that has the biggest intersection with the polygon ConvexPolygon2d bestMatch = null; double overlap = 0.0; for (int i = 0; i < regionToWiggleInto.getNumberOfConvexPolygons(); i++) { ConvexPolygon2d intersection = new ConvexPolygon2d(); regionToWiggleInto.getConvexPolygon(i).intersectionWith(polygonToWiggleInRegionFrame, intersection); if (intersection.getArea() > overlap) { overlap = intersection.getArea(); bestMatch = regionToWiggleInto.getConvexPolygon(i); } } if (bestMatch == null) return null; return findWiggleTransform(polygonToWiggleInRegionFrame, bestMatch, parameters); }
@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; }
intersectionArea += intersectingPolygon.getArea();
private boolean checkIfEnoughArea(BipedalFootstepPlannerNode nodeToExpand, PlanarRegion planarRegion) { RigidBodyTransform nodeToExpandTransform = new RigidBodyTransform(); nodeToExpand.getSoleTransform(nodeToExpandTransform); ConvexPolygon2d snappedPolygon = controllerPolygonsInSoleFrame.get(nodeToExpand.getRobotSide()); snappedPolygon.update(); footArea.set(snappedPolygon.getArea()); ConvexPolygon2d footholdPolygon = new ConvexPolygon2d(); totalArea.set(planarRegion.getPolygonIntersectionAreaWhenSnapped(snappedPolygon, nodeToExpandTransform, footholdPolygon)); nodeToExpand.setPercentageOfFoothold(totalArea.getDoubleValue() / footArea.getDoubleValue()); if (nodeToExpand.isPartialFoothold()) { nodeToExpand.setPartialFootholdPolygon(footholdPolygon); } if (totalArea.getDoubleValue() < parameters.getMinimumFootholdPercent() * footArea.getDoubleValue()) { notifyListenerNodeUnderConsiderationWasRejected(nodeToExpand, BipedalFootstepPlannerNodeRejectionReason.NOT_ENOUGH_AREA); return false; } return true; }