@Override public Footstep generateFootstepUsingHeightMap(FramePose2d desiredSolePosition, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { Footstep toReturn = convexHullFootstepSnapper.generateFootstepUsingHeightMap(desiredSolePosition, foot, soleFrame, robotSide, heightMap); snapFootstep(toReturn, heightMap); return toReturn; }
@Override public void setUseMask(boolean useMask, double kernelMaskSafetyBuffer, double boundingBoxDimension) { convexHullFootstepSnapper.setUseMask(useMask, kernelMaskSafetyBuffer, boundingBoxDimension); }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal) { convexHullFootstepSnapper.adjustFootstepWithoutHeightmap(footstep, height, planeNormal); }
@Override public Footstep generateFootstepUsingHeightMap(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { Footstep toReturn = generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, 0.0, new Vector3D(0.0, 0.0, 1.0)); snapFootstep(toReturn, heightMap); return toReturn; }
public void testFootstepAndPointsFromDataFile() throws NumberFormatException, InsufficientDataException, IOException { QuadTreeFootstepSnappingParameters snappingParameters = new AtlasFootstepSnappingParameters(); ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension); String baseName = "footstepListsForTesting/"; String resourceName = baseName + "DataFromConvexHullSnapper1422988400956.txt"; InputStream resourceAsStream = getClass().getClassLoader().getResourceAsStream(resourceName); FootstepPointsDataReader dataReader = new FootstepPointsDataReader(resourceAsStream); FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.setRobotSide(RobotSide.LEFT.toByte()); FootSpoof spoof = new FootSpoof("basicSpoof"); FramePose2D desiredPose = new FramePose2D(ReferenceFrame.getWorldFrame()); List<Point3D> listOfPoints = new ArrayList<>(); while (dataReader.hasAnotherFootstepAndPoints()) { listOfPoints = dataReader.getNextSetPointsAndFootstep(footstepData); desiredPose.setIncludingFrame(ReferenceFrame.getWorldFrame(), footstepData.getLocation().getX(), footstepData.getLocation().getY(), footstepData.getOrientation().getYaw()); Footstep footstep = footstepSnapper.generateFootstepUsingHeightMap(desiredPose, spoof.getRigidBody(), spoof.getSoleFrame(), RobotSide.fromByte(footstepData.getRobotSide()), listOfPoints, 0.0); assertTrue(footstep.getFootstepType() != Footstep.FootstepType.BAD_FOOTSTEP); } }
public AdjustingFootstepSnapper(FootstepValueFunction valueFunction, QuadTreeFootstepSnappingParameters parameters) { convexHullFootstepSnapper = new ConvexHullFootstepSnapper(valueFunction, parameters); this.footstepSnappingParameters = parameters; this.distanceAdjustment = parameters.getDistanceAdjustment(); this.angleAdjustment = parameters.getAngleAdjustment(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testBasicCropping() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); List<Point2D> pointsToCrop = new ArrayList<Point2D>(); pointsToCrop.add(new Point2D(1,1)); pointsToCrop.add(new Point2D(-1,1)); pointsToCrop.add(new Point2D(-1,-1)); pointsToCrop.add(new Point2D(1,-1)); pointsToCrop.add(new Point2D(1.1,0)); List<Point2D> finalPoints = footstepSnapper.reduceListOfPointsByArea(pointsToCrop, 4); assertTrue(finalPoints.size() == 4.0); ConvexPolygon2D endPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(finalPoints)); assertEquals(4.0, endPolygon.getArea(), 1e-15); }
ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension);
if (cropNumber == 4) finalSupportPoints = reduceListOfPointsToFourFootstepBased(supportPoints); finalSupportPoints = reduceListOfPointsByArea(supportPoints, cropNumber);
adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); return Footstep.FootstepType.BAD_FOOTSTEP; adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); footstep.predictedContactPoints = null; return Footstep.FootstepType.FULL_FOOTSTEP; boolean footstepFound = computePartialFootstepFromPoints(footstep, pointList); height = 0.0; adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); return Footstep.FootstepType.BAD_FOOTSTEP;
Quat4d orientation = footstep.getOrientation(); double yaw = RotationTools.computeYaw(orientation); addLowerBoundaryPointsToHullPointList(convexHullPointsList, position.getX(), position.getY(), yaw); FootstepDataMessage currentFaceFootstep = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(x, y, facePlane.getZOnPlane(x, y)), newOrientation); currentPredictedContactPoints = getPredictedContactPointsForFootstep(currentFaceFootstep, points, distanceTolerance);
@Override public Footstep generateFootstepWithoutHeightMap(FramePose2D desiredSolePosition, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal) { return convexHullFootstepSnapper.generateFootstepWithoutHeightMap(desiredSolePosition, foot, soleFrame, robotSide, height, planeNormal); }
private void addLowerBoundaryPointsToHullPointList(List<Point3D> convexHullPointsList, FramePose2D positioning) { addLowerBoundaryPointsToHullPointList(convexHullPointsList, positioning.getX(), positioning.getY(), positioning.getYaw()); }
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; }
public AdjustingFootstepSnapper(FootstepValueFunction valueFunction, FootstepSnappingParameters parameters) { convexHullFootstepSnapper = new ConvexHullFootstepSnapper(valueFunction, parameters); this.footstepSnappingParameters = parameters; this.distanceAdjustment = parameters.getDistanceAdjustment(); this.angleAdjustment = parameters.getAngleAdjustment(); }
ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); List<Point2D> pointsToCrop = new ArrayList<Point2D>(); Random random = new Random(82368L); double startArea = startPolygon.getArea(); ConvexPolygon2D intermediateStepPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(footstepSnapper.reduceListOfPointsByArea(pointsToCrop, Math.max(4, startPolygon.getNumberOfVertices() / 2)))); intermediateStepPolygon.update(); double intermediateStepArea = intermediateStepPolygon.getArea(); assertTrue(intermediateStepPolygon.getNumberOfVertices() <= Math.max(4, startPolygon.getNumberOfVertices())); ConvexPolygon2D endPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(footstepSnapper.reduceListOfPointsByArea(pointsToCrop, 4))); endPolygon.update(); double endArea = endPolygon.getArea();
@Override public Footstep generateFootstepUsingHeightMap(FramePose2d footPose2d, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { Footstep toReturn = generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, 0.0, new Vector3d(0.0, 0.0, 1.0)); snapFootstep(toReturn, heightMap); return toReturn; }
if (cropNumber == 4) finalSupportPoints = reduceListOfPointsToFourFootstepBased(supportPoints); finalSupportPoints = reduceListOfPointsByArea(supportPoints, cropNumber);
adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); return Footstep.FootstepType.BAD_FOOTSTEP; adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); footstep.getPredictedContactPoints2d().clear(); return Footstep.FootstepType.FULL_FOOTSTEP; boolean footstepFound = computePartialFootstepFromPoints(footstep, pointList); height = 0.0; adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); return Footstep.FootstepType.BAD_FOOTSTEP;
Quaternion orientation = footstep.getOrientation(); double yaw = orientation.getYaw(); addLowerBoundaryPointsToHullPointList(convexHullPointsList, position.getX(), position.getY(), yaw); RotationTools.computeQuaternionFromYawAndZNormal(yaw, facePlane.getNormalCopy(), newOrientation); FootstepDataMessage currentFaceFootstep = HumanoidMessageTools.createFootstepDataMessage(RobotSide.fromByte(footstep.getRobotSide()), new Point3D(x, y, facePlane.getZOnPlane(x, y)), newOrientation); currentPredictedContactPoints = getPredictedContactPointsForFootstep(currentFaceFootstep, points, distanceTolerance);