@Override public void setMask(List<Point2D> footShape) { footstepMask = new BasicFootstepMask(footShape, maskBuffer); }
@Override public void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; if (footstepMask != null) { footstepMask.setSafetyBuffer(maskBuffer); } searchWidth = boundingBoxDimension; searchLength = boundingBoxDimension; }
public BasicFootstepMask(List<Point2D> footstepShape, double maskBufferSize) { this.safetyBuffer = maskBufferSize; footShapeWithBufferPolygon.clear(); for (Point2D vertex : footstepShape) { footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
footstepMask.setPositionAndYaw(position.getX(), position.getY(), yaw); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), searchWidth, searchLength, footstepMask);
public BasicFootstepMask(List<Point2d> footstepShape, double maskBufferSize) { this.safetyBuffer = maskBufferSize; footShapeWithBufferPolygon.clear(); for (Point2d vertex : footstepShape) { footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
footstepMask.setPositionAndYaw(position, footPose2d.getYaw()); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), searchWidth, searchLength, footstepMask);
@Override public void setMask(List<Point2d> footShape) { this.footstepMask = new BasicFootstepMask(footShape, maskBuffer); }
public BasicFootstepMask(ContactablePlaneBody foot, double maskBufferSize) { this.safetyBuffer = maskBufferSize; footShapeWithBufferPolygon.clear(); for (FramePoint2d vertex : foot.getContactPoints2d()) { footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
footstepMask.setPositionAndYaw(position, footPose2d.getYaw()); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), searchWidth, searchLength, footstepMask);
@Override public void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; if (footstepMask != null) { footstepMask.setSafetyBuffer(maskBuffer); } searchWidth = boundingBoxDimension; searchLength = boundingBoxDimension; }
@Override public void setMask(List<Point2D> footShape) { this.footstepMask = new BasicFootstepMask(footShape, maskBuffer); }
public BasicFootstepMask(ConvexPolygon2d footstepShape, double maskBufferSize) { int numVertices = footstepShape.getNumberOfVertices(); Point2d vertex; for (int i = 0; i < numVertices; i++) { vertex = footstepShape.getVertex(i); footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
footstepMask.setPositionAndYaw(position.getX(), position.getY(), yaw); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), searchWidth, searchLength, footstepMask);
@Override public void setMask(List<Point2d> footShape) { footstepMask = new BasicFootstepMask(footShape, maskBuffer); }
public BasicFootstepMask(ContactablePlaneBody foot, double maskBufferSize) { this.safetyBuffer = maskBufferSize; footShapeWithBufferPolygon.clear(); for (FramePoint2D vertex : foot.getContactPoints2d()) { footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
@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); }
@Override public void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }
public BasicFootstepMask(ConvexPolygon2D footstepShape, double maskBufferSize) { int numVertices = footstepShape.getNumberOfVertices(); Point2DReadOnly vertex; for (int i = 0; i < numVertices; i++) { vertex = footstepShape.getVertex(i); footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
@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); }
@Override public void setUseMask(boolean useMask, double maskSafetyBuffer, double boundingBoxDimension) { this.useMask = useMask; maskBuffer = maskSafetyBuffer; this.footstepMask = new BasicFootstepMask(parameters.getCollisionPolygon(), maskBuffer); }