public static boolean isGoalNextToLastStep(FramePose3D goalPose, FootstepPlan footstepPlan) { return isGoalNextToLastStep(goalPose, footstepPlan, 0.5); }
public static FootstepPlan runPlanner(FootstepPlanner planner, FramePose3D initialStanceFootPose, RobotSide initialStanceSide, FramePose3D goalPose, PlanarRegionsList planarRegionsList) { return runPlanner(planner, initialStanceFootPose, initialStanceSide, goalPose, planarRegionsList, true); }
public static SideDependentList<ConvexPolygon2D> createDefaultFootPolygons() { return createFootPolygons(footLength, footWidth); }
protected String assertPlanIsValid(String datasetName, FootstepPlanningResult result, FootstepPlan plan, Point3D goal) { String errorMessage = ""; errorMessage += assertTrue(datasetName, "Planning result for " + datasetName + " is invalid, result was " + result, result.validForExecution()); if (result.validForExecution()) { errorMessage += assertTrue(datasetName, datasetName + " did not reach goal. Made it to " + PlannerTools.getEndPosition(plan) + ", trying to get to " + goal, PlannerTools.isGoalNextToLastStep(goal, plan)); } return errorMessage; }
private FootstepPlanner getPlanner(FootstepPlannerParameters parameters, VisibilityGraphsParameters visibilityGraphsParameters, YoGraphicsListRegistry graphicsListRegistry, YoVariableRegistry registry) { SideDependentList<ConvexPolygon2D> footPloygons = PlannerTools.createDefaultFootPolygons(); return new VisibilityGraphWithAStarPlanner(parameters, visibilityGraphsParameters, footPloygons, graphicsListRegistry, registry); }
public static boolean isGoalNextToLastStep(Point3D desiredPosition, FootstepPlan footstepPlan, double epsilon) { Point3D goalPosition = getEndPosition(footstepPlan); if (goalPosition.epsilonEquals(desiredPosition, epsilon)) return true; else return false; }
@Test(timeout = 500000) @ContinuousIntegrationTest(estimatedDuration = 13.0) public void testDatasetsWithoutOcclusion() { List<FootstepPlannerUnitTestDataset> allDatasets = FootstepPlannerIOTools .loadAllFootstepPlannerDatasetsWithoutOcclusions(FootstepPlannerDataExporter.class); runAssertionsOnAllDatasets(this::runAssertions, allDatasets); }
public static List<FootstepPlannerUnitTestDataset> loadAllFootstepPlannerDatasetsWithoutOcclusions(Class<?> loadingClass) { return loadAllFootstepPlannerDatasets(loadingClass, TEST_DATA_URL); }
public static ConvexPolygon2D createDefaultFootPolygon() { return createFootPolygon(footLength, footWidth); }
public static FootstepPlannerUnitTestDataset loadDataset(Class<?> clazz, String datasetResourceName) { return new FootstepPlannerUnitTestDataset(clazz, datasetResourceName); }
private static List<FootstepPlannerType> parsePlannerTypes(String stringPlannerTypes) { List<FootstepPlannerType> footstepPlannerTypes = new ArrayList<>(); boolean containsData = true; while (containsData) { stringPlannerTypes = parsePlannerType(footstepPlannerTypes, stringPlannerTypes); if (stringPlannerTypes == null) containsData = false; } return footstepPlannerTypes; }
private static Double[] parsePlannerTimeouts(String stringTimeouts) { List<Double> footstepPlannerTimeouts = new ArrayList<>(); boolean containsData = true; while (containsData) { stringTimeouts = parsePlannerTimeout(footstepPlannerTimeouts, stringTimeouts); if (stringTimeouts == null) containsData = false; } Double[] typeArray = new Double[footstepPlannerTimeouts.size()]; footstepPlannerTimeouts.toArray(typeArray); return typeArray; }
private void exportFootstepPlannerData(boolean export) { PlanarRegionsList planarRegionData = planarRegionsState.get(); executor.execute( () -> executeOnThread(planarRegionData, startPosition.get(), startOrientation.get(), goalPosition.get(), goalOrientation.get(), plannerType.get(), timeout.get())); }
@Test(timeout = 500000) @ContinuousIntegrationTest(estimatedDuration = 13.0, categoriesOverride = IntegrationCategory.IN_DEVELOPMENT) public void testDatasetsWithoutOcclusionInDevelopment() { List<FootstepPlannerUnitTestDataset> allDatasets = FootstepPlannerIOTools .loadAllFootstepPlannerDatasetsWithoutOcclusionsInDevelopment(FootstepPlannerDataExporter.class); runAssertionsOnAllDatasets(this::runAssertions, allDatasets); }
public static boolean isGoalNextToLastStep(Point3D goalPosition, FootstepPlan footstepPlan) { return isGoalNextToLastStep(goalPosition, footstepPlan, 0.5); }
@Override public void setupInternal() { planner = new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), PlannerTools.createDefaultFootPolygons()); }
public static FootstepPlan runPlanner(FootstepPlanner planner, FramePose3D initialStanceFootPose, RobotSide initialStanceSide, FramePose3D goalPose, PlanarRegionsList planarRegionsList, boolean assertPlannerReturnedResult) { FootstepPlannerGoal goal = new FootstepPlannerGoal(); goal.setFootstepPlannerGoalType(FootstepPlannerGoalType.POSE_BETWEEN_FEET); goal.setGoalPoseBetweenFeet(goalPose); goal.setXYGoal(new Point2D(goalPose.getX(), goalPose.getY()), 0.5); return runPlanner(planner, initialStanceFootPose, initialStanceSide, goal, planarRegionsList, assertPlannerReturnedResult); }
@Test(timeout = 500000) @ContinuousIntegrationTest(estimatedDuration = 13.0) public void testDatasetsWithoutOcclusion() { List<FootstepPlannerUnitTestDataset> allDatasets = FootstepPlannerIOTools .loadAllFootstepPlannerDatasetsWithoutOcclusions(FootstepPlannerDataExporter.class); runAssertionsOnAllDatasets(this::runAssertions, allDatasets); }
public static List<FootstepPlannerUnitTestDataset> loadAllFootstepPlannerDatasetsWithoutOcclusionsInDevelopment(Class<?> loadingClass) { return loadAllFootstepPlannerDatasets(loadingClass, IN_DEVELOPMENT_TEST_DATA_URL); }
public static SideDependentList<ConvexPolygon2D> createFootPolygons(double footLength, double footWidth) { SideDependentList<ConvexPolygon2D> footPolygons = new SideDependentList<>(); for (RobotSide side : RobotSide.values) footPolygons.put(side, createFootPolygon(footLength, footWidth)); return footPolygons; }