private FootstepPlanner getPlanner(FootstepPlannerParameters parameters, VisibilityGraphsParameters visibilityGraphsParameters, YoGraphicsListRegistry graphicsListRegistry, YoVariableRegistry registry) { SideDependentList<ConvexPolygon2D> footPloygons = PlannerTools.createDefaultFootPolygons(); return new VisibilityGraphWithAStarPlanner(parameters, visibilityGraphsParameters, footPloygons, graphicsListRegistry, registry); }
private String assertPlanIsValid(String datasetName, FootstepPlanningResult result, FootstepPlan plan, Point3D goal) { String errorMessage = ""; if(!result.validForExecution()) { errorMessage = "Planning result for " + datasetName + " is invalid, result was " + result; } else if(!PlannerTools.isGoalNextToLastStep(goal, plan)) { errorMessage = datasetName + " did not reach goal. Made it to " + PlannerTools.getEndPosition(plan) + ", trying to get to " + goal; } if((VISUALIZE || DEBUG) && !errorMessage.isEmpty()) LogTools.error(errorMessage); return errorMessage; }
public static SideDependentList<ConvexPolygon2D> createDefaultFootPolygons() { return createFootPolygons(footLength, footWidth); }
@ContinuousIntegrationTest(estimatedDuration = 2.0) @Test(timeout = 30000) public void testRandomPoses() { boolean assertPlannerReturnedResult = assertPlannerReturnedResult(); double xGoal = random.nextDouble(); double yGoal = random.nextDouble(); double yawGoal = 0.0; Point2D goalPosition = new Point2D(xGoal, yGoal); FramePose2D goalPose = new FramePose2D(ReferenceFrame.getWorldFrame(), goalPosition, yawGoal); double xInitialStanceFoot = random.nextDouble(); double yInitialStanceFoot = random.nextDouble(); double yawInitial = 0.0; Point2D initialStanceFootPosition = new Point2D(xInitialStanceFoot, yInitialStanceFoot); FramePose2D initialStanceFootPose = new FramePose2D(ReferenceFrame.getWorldFrame(), initialStanceFootPosition, yawInitial); RobotSide initialStanceFootSide = RobotSide.generateRandomRobotSide(random); FramePose3D initialStanceFootPose3d = FlatGroundPlanningUtils.poseFormPose2d(initialStanceFootPose); FramePose3D goalPose3d = FlatGroundPlanningUtils.poseFormPose2d(goalPose); FootstepPlan footstepPlan = PlannerTools.runPlanner(getPlanner(), initialStanceFootPose3d, initialStanceFootSide, goalPose3d, null, assertPlannerReturnedResult); if (visualize()) PlanningTestTools.visualizeAndSleep(null, footstepPlan, goalPose3d); if (assertPlannerReturnedResult) assertTrue(PlannerTools.isGoalNextToLastStep(goalPose3d, footstepPlan)); } }
public static FootstepPlan runPlanner(FootstepPlanner planner, FramePose3D initialStanceFootPose, RobotSide initialStanceSide, FramePose3D goalPose, PlanarRegionsList planarRegionsList) { return runPlanner(planner, initialStanceFootPose, initialStanceSide, goalPose, planarRegionsList, true); }
public static boolean isGoalNextToLastStep(FramePose3D goalPose, FootstepPlan footstepPlan) { return isGoalNextToLastStep(goalPose, footstepPlan, 0.5); }
public void testImpossibleCases() ConvexPolygon2D plane = PlannerTools.createDefaultFootPolygon(); plane.scale(0.9); addPolygonToArtifacts("Plane", plane, Color.BLACK); for (int i = 0; i < 1000; i ++) ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); double x = 5.0 * (random.nextDouble() - 0.5);
public static boolean isGoalNextToLastStep(Point3D desiredPosition, FootstepPlan footstepPlan, double epsilon) { Point3D goalPosition = getEndPosition(footstepPlan); if (goalPosition.epsilonEquals(desiredPosition, epsilon)) return true; else return false; }
PlannerTools.addGoalViz(goalPose, vizRegistry, vizGraphicsListRegistry); yoDefaultFootPolygon.set(PlannerTools.createDefaultFootPolygon());
public static ConvexPolygon2D createDefaultFootPolygon() { return createFootPolygon(footLength, footWidth); }
public void testJustTurnInPlace() { boolean assertPlannerReturnedResult = assertPlannerReturnedResult(); double xGoal = 0.0; double yGoal = -stepWidth/2.0; double yawGoal = Math.toRadians(160.0); Point2D goalPosition = new Point2D(xGoal, yGoal); FramePose2D goalPose = new FramePose2D(ReferenceFrame.getWorldFrame(), goalPosition, yawGoal); double xInitialStanceFoot = 0.0; double yInitialStanceFoot = 0.0; double yawInitial = 0.0; Point2D initialStanceFootPosition = new Point2D(xInitialStanceFoot, yInitialStanceFoot); FramePose2D initialStanceFootPose = new FramePose2D(ReferenceFrame.getWorldFrame(), initialStanceFootPosition, yawInitial); RobotSide initialStanceFootSide = RobotSide.LEFT; FramePose3D initialStanceFootPose3d = FlatGroundPlanningUtils.poseFormPose2d(initialStanceFootPose); FramePose3D goalPose3d = FlatGroundPlanningUtils.poseFormPose2d(goalPose); FootstepPlan footstepPlan = PlannerTools.runPlanner(getPlanner(), initialStanceFootPose3d, initialStanceFootSide, goalPose3d, null, assertPlannerReturnedResult); if (visualize()) PlanningTestTools.visualizeAndSleep(null, footstepPlan, goalPose3d); if (assertPlannerReturnedResult) assertTrue(PlannerTools.isGoalNextToLastStep(goalPose3d, footstepPlan)); }
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); }
public static boolean isGoalNextToLastStep(Point3D goalPosition, FootstepPlan footstepPlan) { return isGoalNextToLastStep(goalPosition, footstepPlan, 0.5); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testProjectionThatRequiredRotation() { ConvexPolygon2D plane = PlannerTools.createDefaultFootPolygon(); plane.scale(1.1); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-13.0)); initialFootTransform.setTranslation(-0.1, -0.3, 0.0); initialFoot.applyTransform(initialFootTransform, false); ConvexPolygon2D foot = PolygonWiggler.wigglePolygon(initialFoot, plane, new WiggleParameters()); if (visualize) { addPolygonToArtifacts("Plane", plane, Color.BLACK); addPolygonToArtifacts("InitialFoot", initialFoot, Color.RED); addPolygonToArtifacts("Foot", foot, Color.BLUE); showPlotterAndSleep(artifacts); } assertTrue(ConvexPolygon2dCalculator.isPolygonInside(foot, 1.0e-5, plane)); }
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; }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testStraightLineWithInitialTurn() { boolean assertPlannerReturnedResult = assertPlannerReturnedResult(); double xGoal = 5.0; double yGoal = -stepWidth/2.0; double yawGoal = Math.toRadians(20.0); Point2D goalPosition = new Point2D(xGoal, yGoal); FramePose2D goalPose = new FramePose2D(ReferenceFrame.getWorldFrame(), goalPosition, yawGoal); double xInitialStanceFoot = 0.0; double yInitialStanceFoot = 0.0; double yawInitial = Math.toRadians(20.0); Point2D initialStanceFootPosition = new Point2D(xInitialStanceFoot, yInitialStanceFoot); FramePose2D initialStanceFootPose = new FramePose2D(ReferenceFrame.getWorldFrame(), initialStanceFootPosition, yawInitial); RobotSide initialStanceFootSide = RobotSide.LEFT; FramePose3D initialStanceFootPose3d = FlatGroundPlanningUtils.poseFormPose2d(initialStanceFootPose); FramePose3D goalPose3d = FlatGroundPlanningUtils.poseFormPose2d(goalPose); FootstepPlan footstepPlan = PlannerTools.runPlanner(getPlanner(), initialStanceFootPose3d, initialStanceFootSide, goalPose3d, null, assertPlannerReturnedResult); if (visualize()) PlanningTestTools.visualizeAndSleep(null, footstepPlan, goalPose3d); if (assertPlannerReturnedResult) assertTrue(PlannerTools.isGoalNextToLastStep(goalPose3d, footstepPlan)); }
protected String assertPlanIsValid(String datasetName, FootstepPlanningResult result, FootstepPlan plan, Point3D goal) { String errorMessage = ""; if(!result.validForExecution()) { errorMessage = "Planning result for " + datasetName + " is invalid, result was " + result; } else if(!PlannerTools.isGoalNextToLastStep(goal, plan)) { errorMessage = datasetName + " did not reach goal. Made it to " + PlannerTools.getEndPosition(plan) + ", trying to get to " + goal; } if((VISUALIZE || DEBUG) && !errorMessage.isEmpty()) LogTools.error(errorMessage); return errorMessage; }
@Override public void setupInternal() { planner = new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), PlannerTools.createDefaultFootPolygons()); }
private void iterateOnPlan(PlannerTestEnvironments.PlannerTestData testData) { PrintTools.info("Iterating"); FootstepPlan footstepPlan = PlannerTools .runPlanner(getPlanner(), testData.getStartPose(), testData.getStartSide(), testData.getGoalPose(), testData.getPlanarRegionsList(), assertPlannerReturnedResult()); messager.submitMessage(FootstepPlannerMessagerAPI.FootstepPlanTopic, footstepPlan); }
private void runTestAndAssert(PlannerTestEnvironments.PlannerTestData testData) { submitInfoToUI(testData); ThreadTools.sleep(10); AtomicReference<Boolean> receivedPlan = new AtomicReference<>(false); AtomicReference<Boolean> receivedResult = new AtomicReference<>(false); messager.registerTopicListener(FootstepPlanTopic, request -> receivedPlan.set(true)); messager.registerTopicListener(PlanningResultTopic, request -> receivedResult.set(true)); AtomicReference<FootstepPlan> footstepPlanReference = messager.createInput(FootstepPlanTopic); AtomicReference<FootstepPlanningResult> footstepPlanningResult = messager.createInput(PlanningResultTopic); messager.submitMessage(ComputePathTopic, true); double timeout = 5.0; double totalTimeTaken = 0.0; long sleepDuration = 10; while (!receivedPlan.get() || !receivedResult.get() || footstepPlanReference.get() == null) { if (totalTimeTaken > timeout + 5.0) throw new RuntimeException("Waited too long for a result."); ThreadTools.sleep(sleepDuration); totalTimeTaken += Conversions.millisecondsToSeconds(sleepDuration); } ThreadTools.sleep(10); assertTrue("Planning result is invalid, result was " + footstepPlanningResult.get(), footstepPlanningResult.get().validForExecution()); assertTrue(PlannerTools.isGoalNextToLastStep(testData.getGoalPose(), footstepPlanReference.get())); if (keepUIUp) ThreadTools.sleepForever(); }