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);
@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)); }
plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(0.5, 0.05, 0.0);
ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); if (random.nextBoolean())
public Controller(YoGraphicsListRegistry graphicsListRegistry) { for (int i = 0; i < numberOfPoints; i++) { YoFramePoint3D yoPoint = new YoFramePoint3D("Position" + i, ReferenceFrame.getWorldFrame(), registry); YoGraphicPosition position = new YoGraphicPosition("Position" + i, yoPoint, 0.02, YoAppearance.Blue()); points.add(yoPoint); graphicsListRegistry.registerYoGraphic("BodyPath", position); } YoFrameConvexPolygon2D yoDefaultFootPolygon = new YoFrameConvexPolygon2D("DefaultFootPolygon", ReferenceFrame.getWorldFrame(), 4, registry); yoDefaultFootPolygon.set(PlannerTools.createDefaultFootPolygon()); for (RobotSide side : RobotSide.values) { AppearanceDefinition appearance = side == RobotSide.RIGHT ? YoAppearance.Green() : YoAppearance.Red(); ArrayList<YoFramePoseUsingYawPitchRoll> poses = new ArrayList<>(); for (int i = 0; i < stepsPerSide; i++) { YoFramePoseUsingYawPitchRoll yoFootstepPose = new YoFramePoseUsingYawPitchRoll("footPose" + side.getCamelCaseName() + i, ReferenceFrame.getWorldFrame(), registry); YoGraphicPolygon footstepViz = new YoGraphicPolygon("footstep" + side.getCamelCaseName() + i, yoDefaultFootPolygon, yoFootstepPose, 1.0, appearance); poses.add(yoFootstepPose); yoFootstepPose.setToNaN(); graphicsListRegistry.registerYoGraphic("viz", footstepViz); } yoSteps.put(side, poses); } planner.setTimeout(1.0); }
PlanarRegion region = new PlanarRegion(transformToWorld, planes); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0));
PlanarRegion region = new PlanarRegion(transformToWorld, planes); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimpleProjectionWithWiggleLimits() { ConvexPolygon2D plane = new ConvexPolygon2D(); plane.addVertex(0.0, 0.0); plane.addVertex(0.5, 0.0); plane.addVertex(0.0, 0.5); plane.addVertex(0.5, 0.5); plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0)); initialFootTransform.setTranslation(-0.1, -0.3, 0.0); initialFoot.applyTransform(initialFootTransform, false); WiggleParameters parameters = new WiggleParameters(); parameters.minX = 0.02; parameters.maxX = 0.02; ConvexPolygon2D foot = PolygonWiggler.wigglePolygon(initialFoot, plane, parameters); if (visualize) { addPolygonToArtifacts("Plane", plane, Color.BLACK); addPolygonToArtifacts("InitialFoot", initialFoot, Color.RED); addPolygonToArtifacts("Foot", foot, Color.BLUE); showPlotterAndSleep(artifacts); } assertTrue(foot == null); }
PlanarRegion region = new PlanarRegion(transformToWorld, planes); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimpleProjection() { ConvexPolygon2D plane = new ConvexPolygon2D(); plane.addVertex(0.0, 0.0); plane.addVertex(0.5, 0.0); plane.addVertex(0.0, 0.5); plane.addVertex(0.5, 0.5); plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0)); initialFootTransform.setTranslation(-0.1, -0.3, 0.0); initialFoot.applyTransform(initialFootTransform, false); WiggleParameters wiggleParameters = new WiggleParameters(); ConvexPolygon2D foot = PolygonWiggler.wigglePolygon(initialFoot, plane, 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)); }
PlanarRegion region = new PlanarRegion(transformToWorld, planes); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0));
PlanarRegion region = new PlanarRegion(transformToWorld, planes); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimpleProjectionWithDeltaInside() { ConvexPolygon2D plane = new ConvexPolygon2D(); plane.addVertex(0.0, 0.0); plane.addVertex(0.5, 0.0); plane.addVertex(0.0, 0.5); plane.addVertex(0.5, 0.5); plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setRotationYawAndZeroTranslation(Math.toRadians(-30.0)); initialFootTransform.setTranslation(-0.1, -0.3, 0.0); initialFoot.applyTransform(initialFootTransform, false); WiggleParameters wiggleParameters = new WiggleParameters(); wiggleParameters.deltaInside = 0.06; ConvexPolygon2D foot = PolygonWiggler.wigglePolygon(initialFoot, plane, wiggleParameters); if (visualize) { addPolygonToArtifacts("Plane", plane, Color.BLACK); addPolygonToArtifacts("InitialFoot", initialFoot, Color.RED); addPolygonToArtifacts("Foot", foot, Color.BLUE); showPlotterAndSleep(artifacts); } checkThatWiggledInsideJustTheRightAmount(plane, wiggleParameters, foot); assertTrue(ConvexPolygon2dCalculator.isPolygonInside(foot, 1.0e-5, plane)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testProjectionOnlyTranslation() { ConvexPolygon2D plane = new ConvexPolygon2D(); plane.addVertex(0.0, 0.0); plane.addVertex(0.5, 0.0); plane.addVertex(0.0, 0.5); plane.addVertex(0.5, 0.5); plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(-0.2, 0.25, 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)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testProjectionTranslationLimits() { ConvexPolygon2D plane = new ConvexPolygon2D(); plane.addVertex(0.0, 0.0); plane.addVertex(0.5, 0.0); plane.addVertex(0.0, 0.5); plane.addVertex(0.5, 0.5); plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(-0.2, 0.25, 0.0); initialFoot.applyTransform(initialFootTransform, false); WiggleParameters parameters = new WiggleParameters(); parameters.maxX = 0.1; ConvexPolygon2D foot = PolygonWiggler.wigglePolygon(initialFoot, plane, parameters); if (visualize) { addPolygonToArtifacts("Plane", plane, Color.BLACK); addPolygonToArtifacts("InitialFoot", initialFoot, Color.RED); addPolygonToArtifacts("Foot", foot, Color.BLUE); showPlotterAndSleep(artifacts); } assertTrue(foot == null); }
plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(0.5, 0.0, 0.0);
plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(1.0, 1.5, 0.0);
plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(1.0, 1.5, 0.0);
plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(0.5, 0.0, 0.0);
yoDefaultFootPolygon.set(PlannerTools.createDefaultFootPolygon());