public static boolean isGoalNextToLastStep(FramePose3D goalPose, FootstepPlan footstepPlan) { return isGoalNextToLastStep(goalPose, footstepPlan, 0.5); }
public static boolean isGoalNextToLastStep(Point3D goalPosition, FootstepPlan footstepPlan) { return isGoalNextToLastStep(goalPosition, footstepPlan, 0.5); }
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; }
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; }
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(); }
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; }
@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)); } }
@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)); }
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)); }
private void runTestAndAssert(PlannerTestEnvironments.PlannerTestData testData) { if (messager != null && visualize()) submitInfoToUI(testData); Random random = new Random(324); testData.getPlanarRegionsList().getPlanarRegionsAsList().forEach(region -> region.setRegionId(random.nextInt())); FootstepPlanner planner = getPlanner(); FootstepPlan footstepPlan = PlannerTools .runPlanner(planner, testData.getStartPose(), testData.getStartSide(), testData.getGoalPose(), testData.getPlanarRegionsList(), assertPlannerReturnedResult()); if (assertPlannerReturnedResult()) assertTrue(PlannerTools.isGoalNextToLastStep(testData.getGoalPose(), footstepPlan)); if (messager != null && visualize()) { messager.submitMessage(FootstepPlannerMessagerAPI.PlannerStatusTopic, FootstepPlannerStatus.PLANNING_STEPS); messager.registerTopicListener(PlannerParametersTopic, message -> { // TODO set parameters from message }); messager.submitMessage(FootstepPlannerMessagerAPI.FootstepPlanTopic, footstepPlan); messager.submitMessage(FootstepPlannerMessagerAPI.PlannerStatusTopic, FootstepPlannerStatus.IDLE); messager.submitMessage(FootstepPlannerMessagerAPI.PlannerTimeTakenTopic, planner.getPlanningDuration()); ThreadTools.sleep(10); messager.registerTopicListener(ComputePathTopic, request -> iterateOnPlan(testData)); if (keepUp()) ThreadTools.sleepForever(); } }
public void runJustStraightLine(boolean assertPlannerReturnedResult) { double xGoal = 5.0; double yGoal = -stepWidth/2.0; double yawGoal = 0.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)); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout = 30000) public void testATightTurn() { boolean assertPlannerReturnedResult = assertPlannerReturnedResult(); double xGoal = 1.0; double yGoal = 0.5; double yawGoal = 0.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 (assertPlannerReturnedResult) assertTrue(PlannerTools.isGoalNextToLastStep(goalPose3d, footstepPlan)); if (assertPlannerReturnedResult) assertTrue(footstepPlan.getNumberOfSteps() < 30); if (visualize()) PlanningTestTools.visualizeAndSleep(null, footstepPlan, goalPose3d); }