public static FootstepPlan runPlanner(FootstepPlanner planner, FramePose3D initialStanceFootPose, RobotSide initialStanceSide, FramePose3D goalPose, PlanarRegionsList planarRegionsList) { return runPlanner(planner, initialStanceFootPose, initialStanceSide, goalPose, planarRegionsList, true); }
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); }
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); }
@ContinuousIntegrationTest(estimatedDuration = 7.0) @Test(timeout = 30000) public void testWaypointPathOnFlat() { YoVariableRegistry registry = new YoVariableRegistry(name.getMethodName()); FootstepPlannerParameters parameters = new DefaultFootstepPlanningParameters(); double defaultStepWidth = parameters.getIdealFootstepWidth(); double goalDistance = 5.0; FramePose3D initialStanceFootPose = new FramePose3D(); RobotSide initialStanceFootSide = RobotSide.LEFT; initialStanceFootPose.setY(initialStanceFootSide.negateIfRightSide(defaultStepWidth / 2.0)); FramePose3D goalPose = new FramePose3D(); goalPose.setX(goalDistance); WaypointDefinedBodyPathPlanner bodyPath = new WaypointDefinedBodyPathPlanner(); List<Point3D> waypoints = new ArrayList<>(); waypoints.add(new Point3D(0.0, 0.0, 0.0)); waypoints.add(new Point3D(goalDistance / 8.0, 2.0, 0.0)); waypoints.add(new Point3D(2.0 * goalDistance / 3.0, -2.0, 0.0)); waypoints.add(new Point3D(7.0 * goalDistance / 8.0, -2.0, 0.0)); waypoints.add(new Point3D(goalDistance, 0.0, 0.0)); bodyPath.setWaypoints(waypoints); bodyPath.compute(); FootstepPlanner planner = createBodyPathBasedPlanner(registry, parameters, bodyPath); FootstepPlan footstepPlan = PlannerTools.runPlanner(planner, initialStanceFootPose, initialStanceFootSide, goalPose, null, true); if (visualize) PlanningTestTools.visualizeAndSleep(null, footstepPlan, goalPose, bodyPath); }
@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)); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testPartialFootholds() { double steppingStoneWidth = footWidth * Math.sqrt(percentageFootholdToTest); double steppingStoneLength = footLength * Math.sqrt(percentageFootholdToTest); int numberOfSteps = 6; PlanarRegionsList planarRegionsList = PlanarRegionsListExamples.generateSteppingStoneField(steppingStoneWidth, steppingStoneLength, parameters.getIdealFootstepWidth(), parameters.getIdealFootstepLength(), numberOfSteps); planner.setPlanarRegions(planarRegionsList); Point2D goalPosition = new Point2D(0.6 + (numberOfSteps + 1) * parameters.getIdealFootstepLength(), 0.0); FramePose2D goalPose = new FramePose2D(ReferenceFrame.getWorldFrame(), goalPosition, 0.0); FramePose2D initialStanceFootPose = new FramePose2D(ReferenceFrame.getWorldFrame(), new Point2D(), 0.0); RobotSide initialStanceFootSide = RobotSide.LEFT; FramePose3D initialStanceFootPose3d = FlatGroundPlanningUtils.poseFormPose2d(initialStanceFootPose); FramePose3D goalPose3d = FlatGroundPlanningUtils.poseFormPose2d(goalPose); FootstepPlan footstepPlan = PlannerTools.runPlanner(planner, initialStanceFootPose3d, initialStanceFootSide, goalPose3d, planarRegionsList, !visualize); if (visualize) PlanningTestTools.visualizeAndSleep(planarRegionsList, footstepPlan, goalPose3d); }
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); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testStartNodeInAVoid() { ConvexPolygon2D groundPlane = new ConvexPolygon2D(); groundPlane.addVertex(-1.0, -1.0); groundPlane.addVertex(-1.0, 1.0); groundPlane.addVertex(1.0, -1.0); groundPlane.addVertex(1.0, 1.1); groundPlane.update(); PlanarRegion planarRegion = new PlanarRegion(new RigidBodyTransform(), groundPlane); PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion); Point2D goalPosition = new Point2D(0.5, 0.0); FramePose2D goalPose = new FramePose2D(ReferenceFrame.getWorldFrame(), goalPosition, 0.0); FramePose2D initialStanceFootPose = new FramePose2D(ReferenceFrame.getWorldFrame(), new Point2D(-1.2, 0.0), 0.0); RobotSide initialStanceFootSide = RobotSide.LEFT; FramePose3D initialStanceFootPose3d = FlatGroundPlanningUtils.poseFormPose2d(initialStanceFootPose); FramePose3D goalPose3d = FlatGroundPlanningUtils.poseFormPose2d(goalPose); FootstepPlan footstepPlan = PlannerTools.runPlanner(planner, initialStanceFootPose3d, initialStanceFootSide, goalPose3d, planarRegionsList, !visualize); if (visualize) PlanningTestTools.visualizeAndSleep(planarRegionsList, footstepPlan, goalPose3d); } }
AStarFootstepPlanner planner = createBodyPathBasedPlanner(registry, parameters, bodyPath); planner.setTimeout(1.0); FootstepPlan footstepPlan = PlannerTools.runPlanner(planner, initialStanceFootPose, initialStanceFootSide, goalPose, planarRegionsList, true);
FramePose3D goalPose3d = FlatGroundPlanningUtils.poseFormPose2d(goalPose); FootstepPlan footstepPlan = PlannerTools.runPlanner(planner, initialStanceFootPose3d, initialStanceFootSide, goalPose3d, planarRegionsList, false);