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(); } }
@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; }