public CaptureRegionVisualizer(OneStepCaptureRegionCalculator captureRegionCalculator, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.captureRegionCalculator = captureRegionCalculator; yoCaptureRegionPolygon = new YoFrameConvexPolygon2d(caption, "", worldFrame, 30, registry); YoArtifactPolygon dynamicGraphicYoPolygonArtifact = new YoArtifactPolygon(caption, yoCaptureRegionPolygon, color, false); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), dynamicGraphicYoPolygonArtifact); parentRegistry.addChild(registry); }
@Override public YoArtifact duplicate(YoVariableRegistry newRegistry) { return new YoArtifactPolygon(getName(), convexPolygon.duplicate(newRegistry), color, fill, lineWidth, dashedLine); } }
private void addPolygonToArtifacts(String name, ConvexPolygon2D polygon, Color color) { YoFrameConvexPolygon2D yoPlanePolygon = new YoFrameConvexPolygon2D(name + "Polygon", worldFrame, 10, registry); artifacts.add(new YoArtifactPolygon(name, yoPlanePolygon , color, false)); yoPlanePolygon.set(polygon); }
private static YoArtifactPolygon yoArtifactPolygonFromMessage(String name, YoVariable<?>[] vars, double[] consts, AppearanceDefinition appearance) { ReferenceFrame referenceFrame = ReferenceFrame.getWorldFrame(); int i = 0; YoInteger yoNumVertices = (YoInteger) vars[i++]; ArrayList<YoFramePoint2D> yoFramePoints = new ArrayList<YoFramePoint2D>(); while (i < vars.length) { yoFramePoints.add(new YoFramePoint2D((YoDouble) vars[i++], (YoDouble) vars[i++], referenceFrame)); } YoFrameConvexPolygon2D convexPolygon2d = new YoFrameConvexPolygon2D(yoFramePoints, yoNumVertices, referenceFrame); return new YoArtifactPolygon(name, convexPolygon2d, appearance.getColor().get(), consts[0] > 0); }
public FootstepAdjusterVisualizer(FootstepAdjustor footstepAdjustor, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.footstepAdjustor = footstepAdjustor; String nextFootstepCaption = "DesiredTouchdown"; yoNextFootstepPolygon = new YoFrameConvexPolygon2d(nextFootstepCaption, "", worldFrame, 8, registry); nextFootstepPolygon = new FrameConvexPolygon2d(worldFrame); nextFootstepPolygonArtifact = new YoArtifactPolygon(nextFootstepCaption, yoNextFootstepPolygon, colorDefault, false); nextFootstepPolygonArtifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), nextFootstepPolygonArtifact); parentRegistry.addChild(registry); }
public SmartCMPProjector(YoGraphicsListRegistry graphicsListRegistry, YoVariableRegistry parentRegistry) { activeProjection.set(ProjectionMethod.NONE); if (parentRegistry != null) parentRegistry.addChild(registry); if (graphicsListRegistry != null) { YoArtifactPosition desiredCMPViz = new YoArtifactPosition("Desired CMP Position", yoDesiredCMP, GraphicType.SOLID_BALL, DarkRed().getAwtColor(), 0.008); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), desiredCMPViz); YoArtifactPosition projectedCMPViz = new YoArtifactPosition("Projected CMP Position", yoProjectedCMP, GraphicType.BALL_WITH_ROTATED_CROSS, DarkRed().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), projectedCMPViz); YoArtifactPolygon projectionAreaViz = new YoArtifactPolygon("CMP Projection Area", yoProjectionArea, Blue().getAwtColor(), false); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), projectionAreaViz); } }
private void createGraphicsAndArtifacts(YoGraphicsListRegistry yoGraphicsListRegistry) { yoGraphicsListRegistry.registerYoGraphic("Frames", leftMidZUpFrameViz); yoGraphicsListRegistry.registerYoGraphic("Frames", rightMidZUpFrameViz); yoGraphicsListRegistry.registerYoGraphic("target", targetViz); yoGraphicsListRegistry.registerArtifact("target", targetViz.createArtifact()); yoGraphicsListRegistry.registerArtifact("centroidViz", centroidViz.createArtifact()); yoGraphicsListRegistry.registerYoGraphic("nominalYaw", nominalYawGraphic); YoArtifactPolygon supportPolygonArtifact = new YoArtifactPolygon("quadSupportPolygonArtifact", supportPolygon, Color.blue, false); YoArtifactPolygon currentTriplePolygonArtifact = new YoArtifactPolygon("currentTriplePolygonArtifact", currentTriplePolygon, Color.GREEN, false); yoGraphicsListRegistry.registerArtifact("nominalYawArtifact", nominalYawArtifact); yoGraphicsListRegistry.registerArtifact("supportPolygon", supportPolygonArtifact); yoGraphicsListRegistry.registerArtifact("currentTriplePolygon", currentTriplePolygonArtifact); yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(true); }
private void setupSupportViz() { SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); supportPolygons = new SideDependentList<YoFrameConvexPolygon2D>(); supportPolygons.set(RobotSide.LEFT, new YoFrameConvexPolygon2D("FootPolygonLeft", "", worldFrame, 4, registry)); supportPolygons.set(RobotSide.RIGHT, new YoFrameConvexPolygon2D("FootPolygonRight", "", worldFrame, 4, registry)); footContactsInAnkleFrame = new SideDependentList<ArrayList<Point2D>>(); footContactsInAnkleFrame.set(RobotSide.LEFT, null); footContactsInAnkleFrame.set(RobotSide.RIGHT, null); yoGraphicsListRegistry.registerArtifact("SupportLeft", new YoArtifactPolygon("SupportLeft", supportPolygons.get(RobotSide.LEFT), Color.BLACK, false)); yoGraphicsListRegistry.registerArtifact("SupportRight", new YoArtifactPolygon("SupportRight", supportPolygons.get(RobotSide.RIGHT), Color.BLACK, false)); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); drcSimulationTestHelper.addRobotControllerOnControllerThread(new VizUpdater()); }
public CapturePointUpdatable(CapturabilityBasedStatusSubscriber capturabilityBasedStatusSubsrciber, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.capturabilityBasedStatusSubsrciber = capturabilityBasedStatusSubsrciber; YoGraphicPosition capturePointViz = new YoGraphicPosition("Capture Point", yoCapturePoint, 0.01, YoAppearance.Blue(), GraphicType.ROTATED_CROSS); yoGraphicsListRegistry.registerArtifact("Capturability", capturePointViz.createArtifact()); YoGraphicPosition desiredCapturePointViz = new YoGraphicPosition("Desired Capture Point", yoDesiredCapturePoint, 0.01, YoAppearance.Yellow(), GraphicType.ROTATED_CROSS); yoGraphicsListRegistry.registerArtifact("Capturability", desiredCapturePointViz.createArtifact()); YoArtifactPolygon supportPolygonViz = new YoArtifactPolygon("Combined Polygon", yoSupportPolygon, Color.pink, false); yoGraphicsListRegistry.registerArtifact("Capturability", supportPolygonViz); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); String name = sidePrefix + "FootSupportPolygon"; YoFrameConvexPolygon2d yoFootSupportPolygon = new YoFrameConvexPolygon2d(name, "", worldFrame, 4, registry); yoFootSupportPolygons.put(robotSide, yoFootSupportPolygon); Color color = FootstepListVisualizer.defaultFeetColors.get(robotSide); YoArtifactPolygon footSupportPolygonViz = new YoArtifactPolygon(sidePrefix + "Foot Polygon", yoFootSupportPolygon, color, false); yoGraphicsListRegistry.registerArtifact("Capturability", footSupportPolygonViz); } parentRegistry.addChild(registry); }
public BipedSupportPolygons(SideDependentList<ReferenceFrame> ankleZUpFrames, ReferenceFrame midFeetZUpFrame, SideDependentList<ReferenceFrame> soleZUpFrames, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { this.ankleZUpFrames = ankleZUpFrames; this.midFeetZUp = midFeetZUpFrame; this.soleZUpFrames = soleZUpFrames; supportPolygonViz = new YoFrameConvexPolygon2d("combinedPolygon", "", worldFrame, 2 * maxNumberOfContactPointsPerFoot, registry); ArtifactList artifactList = new ArtifactList(getClass().getSimpleName()); YoArtifactPolygon supportPolygonArtifact = new YoArtifactPolygon("Combined Polygon", supportPolygonViz, Color.pink, false); artifactList.add(supportPolygonArtifact); for (RobotSide robotSide : RobotSide.values) { footPolygonsInWorldFrame.put(robotSide, new FrameConvexPolygon2d()); footPolygonsInSoleFrame.put(robotSide, new FrameConvexPolygon2d()); footPolygonsInSoleZUpFrame.put(robotSide, new FrameConvexPolygon2d()); footPolygonsInAnkleZUp.put(robotSide, new FrameConvexPolygon2d()); footPolygonsInMidFeetZUp.put(robotSide, new FrameConvexPolygon2d()); String robotSidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); YoFrameConvexPolygon2d footPolygonViz = new YoFrameConvexPolygon2d(robotSidePrefix + "FootPolygon", "", worldFrame, maxNumberOfContactPointsPerFoot, registry); footPolygonsViz.put(robotSide, footPolygonViz); YoArtifactPolygon footPolygonArtifact = new YoArtifactPolygon(robotSide.getCamelCaseNameForMiddleOfExpression() + " Foot Polygon", footPolygonViz, defaultFeetColors.get(robotSide), false); artifactList.add(footPolygonArtifact); } if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.registerArtifactList(artifactList); } parentRegistry.addChild(registry); }
public CapturePointUpdatable(CapturabilityBasedStatusSubscriber capturabilityBasedStatusSubsrciber, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.capturabilityBasedStatusSubsrciber = capturabilityBasedStatusSubsrciber; YoGraphicPosition capturePointViz = new YoGraphicPosition("Capture Point", yoCapturePoint, 0.01, YoAppearance.Blue(), GraphicType.ROTATED_CROSS); yoGraphicsListRegistry.registerArtifact("Capturability", capturePointViz.createArtifact()); YoGraphicPosition desiredCapturePointViz = new YoGraphicPosition("Desired Capture Point", yoDesiredCapturePoint, 0.01, YoAppearance.Yellow(), GraphicType.ROTATED_CROSS); yoGraphicsListRegistry.registerArtifact("Capturability", desiredCapturePointViz.createArtifact()); YoArtifactPolygon supportPolygonViz = new YoArtifactPolygon("Combined Polygon", yoSupportPolygon, Color.pink, false); yoGraphicsListRegistry.registerArtifact("Capturability", supportPolygonViz); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); String name = sidePrefix + "FootSupportPolygon"; YoFrameConvexPolygon2D yoFootSupportPolygon = new YoFrameConvexPolygon2D(name, "", worldFrame, 4, registry); yoFootSupportPolygons.put(robotSide, yoFootSupportPolygon); Color color = FootstepListVisualizer.defaultFeetColors.get(robotSide); YoArtifactPolygon footSupportPolygonViz = new YoArtifactPolygon(sidePrefix + "Foot Polygon", yoFootSupportPolygon, color, false); yoGraphicsListRegistry.registerArtifact("Capturability", footSupportPolygonViz); } parentRegistry.addChild(registry); }
artifacts.add(new YoArtifactPolygon("Safe Area", yoSafeArea, Color.GREEN, false)); artifacts.add(new YoArtifactPolygon("Safe CMP Area", yoSafeCMPArea, Color.CYAN, false)); artifacts.add(new YoArtifactPolygon("Projection Area", yoProjectionArea, Color.BLUE, false));
private void setupSupportViz() { SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); supportPolygons = new SideDependentList<YoFrameConvexPolygon2D>(); supportPolygons.set(RobotSide.LEFT, new YoFrameConvexPolygon2D("FootPolygonLeft", "", worldFrame, 4, registry)); supportPolygons.set(RobotSide.RIGHT, new YoFrameConvexPolygon2D("FootPolygonRight", "", worldFrame, 4, registry)); footContactsInAnkleFrame = new SideDependentList<>(); footContactsInAnkleFrame.set(RobotSide.LEFT, null); footContactsInAnkleFrame.set(RobotSide.RIGHT, null); yoGraphicsListRegistry.registerArtifact("SupportLeft", new YoArtifactPolygon("SupportLeft", supportPolygons.get(RobotSide.LEFT), Color.BLACK, false)); yoGraphicsListRegistry.registerArtifact("SupportRight", new YoArtifactPolygon("SupportRight", supportPolygons.get(RobotSide.RIGHT), Color.BLACK, false)); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); drcSimulationTestHelper.addRobotControllerOnControllerThread(new VizUpdater()); }
YoArtifactPolygon projectionAreaViz = new YoArtifactPolygon("CMP Projection Area", yoProjectionArea, Blue().getAwtColor(), false); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), projectionAreaViz); yoProjectionArea.set(projectionArea); YoArtifactPolygon expectedAreaViz = new YoArtifactPolygon("Expected CMP Area", yoExpectedArea, LawnGreen().getAwtColor(), false, 4); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), expectedAreaViz); yoExpectedArea.set(expectedArea);
return new YoArtifactPolygon(name, convexPolygon2d, appearance.getColor().get(), consts[0] > 0);
YoArtifactPolygon yoGraphicPolygon = new YoArtifactPolygon(namePrefix + "UnsafeRegion", yoUnsafePolygon, Color.RED, false); yoGraphicPolygon.setVisible(false); yoGraphicsListRegistry.registerArtifact(listName, yoGraphicPolygon); YoArtifactPolygon yoShrunkPolygon = new YoArtifactPolygon(namePrefix + "ShrunkPolygon", yoShrunkFootPolygon, Color.CYAN, false); yoShrunkPolygon.setVisible(false); yoGraphicsListRegistry.registerArtifact(listName, yoShrunkPolygon);
else footColor = Color.green; YoArtifactPolygon footArtifact = new YoArtifactPolygon(robotSide.getCamelCaseNameForStartOfExpression(), yoFootPolygon, footColor, false); yoGraphicsListRegistry.registerArtifact("Feet", footArtifact); footArtifacts.put(robotSide, footArtifact); YoArtifactPolygon captureRegionArtifact = new YoArtifactPolygon("CaptureRegion", yoCaptureRegion, Color.BLACK, false); yoGraphicsListRegistry.registerArtifact("Capture", captureRegionArtifact); final YoEnum<RobotSide> yoSupportSide = new YoEnum<>("supportSide", registry, RobotSide.class);
String Prefix = robotSide.getCamelCaseNameForMiddleOfExpression(); YoFrameConvexPolygon2D yoFootPolygon = new YoFrameConvexPolygon2D(prefix + "FootPolygon", worldFrame, 10, registry); artifacts.add(new YoArtifactPolygon(Prefix + " Foot Polygon", yoFootPolygon, Color.BLACK, false, 1)); yoFootPolygon.set(footPolygonInWorld);
plotter.addArtifact(new YoArtifactLineSegment2d("lineSegment1", lineSegment, Color.DARK_GRAY, 0.1, 0.1)); plotter.addArtifact(new YoArtifactLine2d("line1", line, Color.GREEN)); plotter.addArtifact(new YoArtifactPolygon("emptyPolygon1", polygon, Color.MAGENTA, false)); plotter.addArtifact(new YoArtifactPolygon("onePointPolygon", polygon2, Color.MAGENTA, false)); plotter.addArtifact(new YoArtifactPolygon("twoPointPolygon", polygon3, Color.BLUE, true)); plotter.addArtifact(new YoArtifactPosition("pointZ", pointZ, GraphicType.BALL, Color.DARK_GRAY, 0.1)); plotter.addArtifact(new YoArtifactPosition("point4", point4, GraphicType.BALL_WITH_CROSS, Color.DARK_GRAY, 0.1));