@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 Footstep generateFootstepUsingHeightMap(FramePose2D desiredSolePosition, RigidBodyBasics 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 Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { FramePose2d footPose2d = new FramePose2d(ReferenceFrame.getWorldFrame(), new Point2d(soleX, soleY), yaw); return generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap); }
@Override public Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { FramePose2D footPose2d = new FramePose2D(ReferenceFrame.getWorldFrame(), new Point2D(soleX, soleY), yaw); return generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap); }
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); } }