private FootstepPlanner getPlanner(FootstepPlannerParameters parameters, VisibilityGraphsParameters visibilityGraphsParameters, YoGraphicsListRegistry graphicsListRegistry, YoVariableRegistry registry) { SideDependentList<ConvexPolygon2D> footPloygons = PlannerTools.createDefaultFootPolygons(); return new VisibilityGraphWithAStarPlanner(parameters, visibilityGraphsParameters, footPloygons, graphicsListRegistry, registry); }
@Override public void setupInternal() { planner = new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), PlannerTools.createDefaultFootPolygons()); }
private BodyPathAndFootstepPlanner createPlanner() { SideDependentList<ConvexPolygon2D> contactPointsInSoleFrame = PlannerTools.createDefaultFootPolygons(); YoVariableRegistry registry = new YoVariableRegistry("visualizerRegistry"); switch (footstepPlannerTypeReference.get()) { case PLANAR_REGION_BIPEDAL: return createPlanarRegionBipedalPlanner(contactPointsInSoleFrame, registry); case PLAN_THEN_SNAP: return new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), contactPointsInSoleFrame); case A_STAR: return createAStarPlanner(contactPointsInSoleFrame, registry); case SIMPLE_BODY_PATH: return new SplinePathWithAStarPlanner(parameters.get(), contactPointsInSoleFrame, registry, null); case VIS_GRAPH_WITH_A_STAR: return new VisibilityGraphWithAStarPlanner(parameters.get(), visibilityGraphsParameters.get(), contactPointsInSoleFrame, null, registry); default: throw new RuntimeException("Planner type " + footstepPlannerTypeReference.get() + " is not valid!"); } }
@Before public void setup() { visualize = visualize && !ContinuousIntegrationTools.isRunningOnContinuousIntegrationServer(); DefaultFootstepPlanningParameters parameters = new DefaultFootstepPlanningParameters(); SideDependentList<ConvexPolygon2D> footPolygons = PlannerTools.createDefaultFootPolygons(); ParameterBasedNodeExpansion expansion = new ParameterBasedNodeExpansion(parameters); planner = AStarFootstepPlanner.createPlanner(parameters, null, footPolygons, expansion, registry); }
@Before public void setup() { FootstepPlannerParameters parameters = new BestEffortPlannerParameters(3); SideDependentList<ConvexPolygon2D> footPolygons = PlannerTools.createDefaultFootPolygons(); ParameterBasedNodeExpansion expansion = new ParameterBasedNodeExpansion(parameters); this.planner = AStarFootstepPlanner.createPlanner(parameters, null, footPolygons, expansion, registry); planner.setTimeout(5.0); }
@Override public void setupInternal() { SideDependentList<ConvexPolygon2D> footPolygons = PlannerTools.createDefaultFootPolygons(); ParameterBasedNodeExpansion expansion = new ParameterBasedNodeExpansion(getPlannerParameters()); planner = AStarFootstepPlanner .createPlanner(getPlannerParameters(), visualization, footPolygons, expansion, new YoVariableRegistry("TestRegistry")); }
@Before public void createPlanner() { if (visualizePlanner) visualization = new FootstepNodeVisualization(1000, 1.0, null); SideDependentList<ConvexPolygon2D> footPolygons = PlannerTools.createDefaultFootPolygons(); FootstepPlannerParameters parameters = new DefaultFootstepPlanningParameters(); SimpleSideBasedExpansion expansion = new SimpleSideBasedExpansion(parameters); planner = AStarFootstepPlanner.createPlanner(parameters, visualization, footPolygons, expansion, new YoVariableRegistry("TestRegistry")); }
public FootstepPlanningToolboxController(RobotContactPointParameters<RobotSide> contactPointParameters, FootstepPlannerParameters footstepPlannerParameters, VisibilityGraphsParameters visibilityGraphsParameters, StatusMessageOutputManager statusOutputManager, YoVariableRegistry parentRegistry, YoGraphicsListRegistry graphicsListRegistry, double dt) { super(statusOutputManager, parentRegistry); this.dt = dt; this.yoGraphicPlanarRegionsList = new YoGraphicPlanarRegionsList("FootstepPlannerToolboxPlanarRegions", 200, 30, registry); SideDependentList<ConvexPolygon2D> contactPointsInSoleFrame; if (contactPointParameters == null) contactPointsInSoleFrame = PlannerTools.createDefaultFootPolygons(); else contactPointsInSoleFrame = createFootPolygonsFromContactPoints(contactPointParameters); footstepPlanningParameters = new YoFootstepPlannerParameters(registry, footstepPlannerParameters); this.visibilityGraphsParameters = new YoVisibilityGraphParameters(visibilityGraphsParameters, registry); plannerMap.put(FootstepPlannerType.PLANAR_REGION_BIPEDAL, createPlanarRegionBipedalPlanner(contactPointsInSoleFrame)); plannerMap.put(FootstepPlannerType.PLAN_THEN_SNAP, new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), contactPointsInSoleFrame)); plannerMap.put(FootstepPlannerType.A_STAR, createAStarPlanner(contactPointsInSoleFrame)); plannerMap .put(FootstepPlannerType.SIMPLE_BODY_PATH, new SplinePathWithAStarPlanner(footstepPlanningParameters, contactPointsInSoleFrame, parentRegistry, null)); plannerMap.put(FootstepPlannerType.VIS_GRAPH_WITH_A_STAR, new VisibilityGraphWithAStarPlanner(footstepPlanningParameters, this.visibilityGraphsParameters, contactPointsInSoleFrame, graphicsListRegistry, parentRegistry)); activePlanner.set(FootstepPlannerType.PLANAR_REGION_BIPEDAL); graphicsListRegistry.registerYoGraphic("footstepPlanningToolbox", yoGraphicPlanarRegionsList); isDone.set(true); planId.set(FootstepPlanningRequestPacket.NO_PLAN_ID); }
contactPointsInSoleFrame = PlannerTools.createDefaultFootPolygons(); else contactPointsInSoleFrame = createFootPolygonsFromContactPoints(contactPointParameters);
@Override public void setupInternal() { YoVariableRegistry registry = new YoVariableRegistry("test"); parameters = new YoFootstepPlannerParameters(registry, new DefaultFootstepPlanningParameters()); SideDependentList<ConvexPolygon2D> footPolygonsInSoleFrame = PlannerTools.createDefaultFootPolygons(); FootstepNodeSnapAndWiggler snapper = new FootstepNodeSnapAndWiggler(footPolygonsInSoleFrame, parameters); SnapBasedNodeChecker nodeChecker = new SnapBasedNodeChecker(parameters, footPolygonsInSoleFrame, snapper); ConstantFootstepCost stepCostCalculator = new ConstantFootstepCost(1.0); planner = new DepthFirstFootstepPlanner(parameters, snapper, nodeChecker, stepCostCalculator, registry); planner.setFeetPolygons(footPolygonsInSoleFrame); planner.setMaximumNumberOfNodesToExpand(100); }
@Before public void setupPlanner() { registry = new YoVariableRegistry("test"); parameters = new YoFootstepPlannerParameters(registry, new DefaultFootstepPlanningParameters()); SideDependentList<ConvexPolygon2D> footPolygonsInSoleFrame = PlannerTools.createDefaultFootPolygons(); FlatGroundFootstepNodeSnapper snapper = new FlatGroundFootstepNodeSnapper(); AlwaysValidNodeChecker nodeChecker = new AlwaysValidNodeChecker(); ConstantFootstepCost footstepCost = new ConstantFootstepCost(1.0); planner = new DepthFirstFootstepPlanner(parameters, snapper, nodeChecker, footstepCost, registry); planner.setFeetPolygons(footPolygonsInSoleFrame); planner.setMaximumNumberOfNodesToExpand(1000); }