private void registerArtifacts(String name, YoGraphicsListRegistry yoGraphicsListRegistry) { if (yoGraphicsListRegistry != null) { artifactList = new ArtifactList(name + "Balls"); for (YoGraphicPosition yoGraphicPosition : yoGraphicPositions) { artifactList.add(yoGraphicPosition.createArtifact()); } yoGraphicsListRegistry.registerArtifactList(artifactList); } }
public void createVisualizerForConstantCMPs(YoGraphicsList yoGraphicsList, ArtifactList artifactList) { for (int i = 0; i < entryCMPs.size(); i++) { YoGraphicPosition entryCMPViz = new YoGraphicPosition("Entry CMP" + i, entryCMPsInWorldFrameReadOnly.get(i), CMP_POINT_SIZE, YoAppearance.Green(), GraphicType.SOLID_BALL); yoGraphicsList.add(entryCMPViz); artifactList.add(entryCMPViz.createArtifact()); } for (int i = 0; i < exitCMPs.size(); i++) { YoGraphicPosition exitCMPViz = new YoGraphicPosition("Exit CMP" + i, exitCMPsInWorldFrameReadOnly.get(i), CMP_POINT_SIZE, YoAppearance.Green(), GraphicType.BALL); yoGraphicsList.add(exitCMPViz); artifactList.add(exitCMPViz.createArtifact()); } }
public void createVisualizers(YoGraphicsList yoGraphicsList, ArtifactList artifactList) { YoGraphicPosition startOfSplineICPViz = new YoGraphicPosition("singleSupportInitialSplineICP", yoStartOfSplineICP, 0.004, YoAppearance.Black(), GraphicType.SOLID_BALL); yoGraphicsList.add(startOfSplineICPViz); artifactList.add(startOfSplineICPViz.createArtifact()); YoGraphicPosition endOfSplineICPViz = new YoGraphicPosition("singleSupportFinalSplineICP", yoEndOfSplineICP, 0.004, YoAppearance.Black(), GraphicType.BALL); yoGraphicsList.add(endOfSplineICPViz); artifactList.add(endOfSplineICPViz.createArtifact()); }
private void setupVisualizers(YoGraphicsListRegistry yoGraphicsListRegistry, boolean visualize) { YoGraphicsList yoGraphicsList = new YoGraphicsList(getClass().getSimpleName()); ArtifactList artifactList = new ArtifactList(getClass().getSimpleName()); YoGraphicPosition previousExitCMP = new YoGraphicPosition("previousExitCMP", previousStanceExitCMP, 0.01, YoAppearance.Red(), GraphicType.SQUARE); YoGraphicPosition entryCMP = new YoGraphicPosition("entryCMP", stanceEntryCMP, 0.01, YoAppearance.Red(), GraphicType.SQUARE); YoGraphicPosition exitCMP = new YoGraphicPosition("exitCMP", stanceExitCMP, 0.01, YoAppearance.Red(), GraphicType.SQUARE); YoGraphicPosition finalICP = new YoGraphicPosition("finalICP", this.finalICP, 0.005, YoAppearance.Black(), GraphicType.SOLID_BALL); yoGraphicsList.add(finalICP); artifactList.add(previousExitCMP.createArtifact()); artifactList.add(entryCMP.createArtifact()); artifactList.add(exitCMP.createArtifact()); artifactList.add(finalICP.createArtifact()); artifactList.setVisible(visualize); yoGraphicsList.setVisible(visualize); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerArtifactList(artifactList); }
public CenterOfPressureVisualizer(Map<RigidBody, FootSwitchInterface> footSwitches, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.footSwitches = footSwitches; footRigidBodies = footSwitches.keySet(); for (RigidBody rigidBody : footRigidBodies) { String rigidBodyName = rigidBody.getName(); rigidBodyName = WordUtils.capitalize(rigidBodyName); YoFramePoint rawCoPPositionInWorld = new YoFramePoint("raw" + rigidBodyName + "CoPPositionsInWorld", worldFrame, registry); footRawCoPPositionsInWorld.put(rigidBody, rawCoPPositionInWorld); YoGraphicPosition copDynamicGraphic = new YoGraphicPosition("Meas " + rigidBodyName + "CoP", rawCoPPositionInWorld, 0.008, YoAppearance.DarkRed(), GraphicType.DIAMOND); YoArtifactPosition copArtifact = copDynamicGraphic.createArtifact(); yoGraphicsListRegistry.registerArtifact("StateEstimator", copArtifact); footList.add(rigidBody); } overallRawCoPPositionInWorld = new YoFramePoint("overallRawCoPPositionInWorld", worldFrame, registry); YoGraphicPosition overallRawCoPDynamicGraphic = new YoGraphicPosition("Meas CoP", overallRawCoPPositionInWorld, 0.015, YoAppearance.DarkRed(), GraphicType.DIAMOND); YoArtifactPosition overallRawCoPArtifact = overallRawCoPDynamicGraphic.createArtifact(); overallRawCoPArtifact.setVisible(false); yoGraphicsListRegistry.registerArtifact("StateEstimator", overallRawCoPArtifact); parentRegistry.addChild(registry); }
private void setupVisualizers(YoGraphicsListRegistry yoGraphicsListRegistry, boolean visualize) { YoGraphicsList yoGraphicsList = new YoGraphicsList(getClass().getSimpleName()); ArtifactList artifactList = new ArtifactList(getClass().getSimpleName()); YoGraphicPosition actualEndOfStateICP = new YoGraphicPosition("actualEndOfStateICP", this.actualEndOfStateICP, 0.005, YoAppearance.Aquamarine(), GraphicType.SOLID_BALL); YoGraphicPosition nominalReferenceICP = new YoGraphicPosition("nominalReferenceICP", this.nominalReferenceICP, 0.01, YoAppearance.LightYellow(), GraphicType.BALL); YoGraphicPosition nominalEndOfStateICP = new YoGraphicPosition("nominalEndOfStateICP", this.nominalEndOfStateICP, 0.01, YoAppearance.Green(), GraphicType.SOLID_BALL); yoGraphicsList.add(actualEndOfStateICP); yoGraphicsList.add(nominalReferenceICP); yoGraphicsList.add(nominalEndOfStateICP); artifactList.add(actualEndOfStateICP.createArtifact()); artifactList.add(nominalReferenceICP.createArtifact()); artifactList.add(nominalEndOfStateICP.createArtifact()); yoGraphicsList.setVisible(visualize); artifactList.setVisible(visualize); YoGraphicPosition referenceICP = new YoGraphicPosition("controllerReferenceICP", controllerReferenceICP, 0.005, YoAppearance.Yellow(), GraphicType.BALL_WITH_CROSS); YoGraphicPosition referenceCMP = new YoGraphicPosition("controllerReferenceCMP", controllerReferenceCMP, 0.005, YoAppearance.Beige(), GraphicType.BALL_WITH_CROSS); String name = "ICPOptimization"; yoGraphicsListRegistry.registerArtifact(name, referenceICP.createArtifact()); yoGraphicsListRegistry.registerArtifact(name, referenceCMP.createArtifact()); yoGraphicsListRegistry.registerYoGraphic(name, referenceICP); yoGraphicsListRegistry.registerYoGraphic(name, referenceCMP); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerArtifactList(artifactList); }
private void setupVisualizers(YoGraphicsListRegistry yoGraphicsListRegistry) { YoGraphicsList yoGraphicsList = new YoGraphicsList(getClass().getSimpleName()); ArtifactList artifactList = new ArtifactList(getClass().getSimpleName()); referenceCMPsCalculator.createVisualizerForConstantCMPs(yoGraphicsList, artifactList); for (int i = 0; i < numberFootstepsToConsider.getIntegerValue() - 1; i++) { YoGraphicPosition icpEarlyCornerPointsViz = new YoGraphicPosition("EntryCornerPoints" + i, entryCornerPoints.get(i).buildUpdatedYoFramePointForVisualizationOnly(), ICP_CORNER_POINT_SIZE, YoAppearance.Blue(), GraphicType.SOLID_BALL); yoGraphicsList.add(icpEarlyCornerPointsViz); artifactList.add(icpEarlyCornerPointsViz.createArtifact()); YoGraphicPosition lateCornerPointsViz = new YoGraphicPosition("ExitCornerPoints" + i, exitCornerPoints.get(i).buildUpdatedYoFramePointForVisualizationOnly(), ICP_CORNER_POINT_SIZE, YoAppearance.Blue(), GraphicType.BALL); yoGraphicsList.add(lateCornerPointsViz); artifactList.add(lateCornerPointsViz.createArtifact()); } YoGraphicPosition singleSupportInitialICPViz = new YoGraphicPosition("singleSupportInitialICP", singleSupportInitialICP.buildUpdatedYoFramePointForVisualizationOnly(), 0.004, YoAppearance.Chocolate(), GraphicType.SOLID_BALL); yoGraphicsList.add(singleSupportInitialICPViz); artifactList.add(singleSupportInitialICPViz.createArtifact()); YoGraphicPosition singleSupportFinalICPViz = new YoGraphicPosition("singleSupportFinalICP", singleSupportFinalICP.buildUpdatedYoFramePointForVisualizationOnly(), 0.004, YoAppearance.Chocolate(), GraphicType.BALL); yoGraphicsList.add(singleSupportFinalICPViz); artifactList.add(singleSupportFinalICPViz.createArtifact()); icpSingleSupportTrajectoryGenerator.createVisualizers(yoGraphicsList, artifactList); artifactList.setVisible(VISUALIZE); yoGraphicsList.setVisible(VISUALIZE); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerArtifactList(artifactList); }
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); }
String sidePrefix = foot.getName(); YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copPositionsInWorld.get(foot), 0.005, YoAppearance.DeepPink()); YoArtifactPosition artifact = copInWorld.createArtifact(); artifact.setVisible(false); yoGraphicsListRegistry.registerArtifact("StateEstimator", artifact);
copViz.setVisible(VISUALIZE); yoGraphicsListRegistry.registerYoGraphic(listName, copViz); YoArtifactPosition artifact = copViz.createArtifact(); artifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, artifact);
private void setupPositionGraphics() { YoFramePoint3D yoCoMPosition = new YoFramePoint3D("CoMPositionForViz", worldFrame, registry); comPositionGraphic = new YoGraphicPosition("CoMPositionGraphic", yoCoMPosition, trackBallSize * 2, new YoAppearanceRGBColor(comPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); YoFramePoint3D yoICPPosition = new YoFramePoint3D("ICPPositionForViz", worldFrame, registry); icpPositionGraphic = new YoGraphicPosition("ICPPositionGraphic", yoICPPosition, trackBallSize * 2, new YoAppearanceRGBColor(icpPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); YoFramePoint3D yoCMPPosition = new YoFramePoint3D("CMPPositionForViz", worldFrame, registry); cmpPositionGraphic = new YoGraphicPosition("CMPPositionGraphic", yoCMPPosition, trackBallSize * 2, new YoAppearanceRGBColor(cmpPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); YoFramePoint3D yoCoPPosition = new YoFramePoint3D("CoPPositionForViz", worldFrame, registry); copPositionGraphic = new YoGraphicPosition("CoPPositionGraphic", yoCoPPosition, trackBallSize * 2, new YoAppearanceRGBColor(copPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); graphicsListRegistry.registerYoGraphic("GraphicPositions", comPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", comPositionGraphic.createArtifact()); graphicsListRegistry.registerYoGraphic("GraphicPositions", icpPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", icpPositionGraphic.createArtifact()); graphicsListRegistry.registerYoGraphic("GraphicPositions", cmpPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", cmpPositionGraphic.createArtifact()); graphicsListRegistry.registerYoGraphic("GraphicPositions", copPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", copPositionGraphic.createArtifact()); }
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 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 static void showPlotter(WaypointDefinedBodyPathPlanner plan, String testName) { int markers = 5; YoVariableRegistry registry = new YoVariableRegistry(testName); YoGraphicsListRegistry graphicsList = new YoGraphicsListRegistry(); for (int i = 0; i < markers; i++) { double alpha = (double) i / (double) (markers - 1); Pose2D pose = new Pose2D(); plan.getPointAlongPath(alpha, pose); YoFramePoint3D yoStartPoint = new YoFramePoint3D("PointStart" + i, worldFrame, registry); yoStartPoint.set(pose.getX(), pose.getY(), 0.0); double length = 0.1; YoFrameVector3D direction = new YoFrameVector3D("Direction" + i, worldFrame, registry); direction.set(length * Math.cos(pose.getYaw()), length * Math.sin(pose.getYaw()), 0.0); YoFramePoint3D yoEndPoint = new YoFramePoint3D("PointEnd" + i, worldFrame, registry); yoEndPoint.set(yoStartPoint); yoEndPoint.add(direction); YoGraphicPosition poseStartVis = new YoGraphicPosition("PointStart" + i, yoStartPoint, 0.01, YoAppearance.Blue()); YoGraphicPosition poseEndVis = new YoGraphicPosition("PointEnd" + i, yoEndPoint, 0.01, YoAppearance.Red()); graphicsList.registerArtifact(testName, poseStartVis.createArtifact()); graphicsList.registerArtifact(testName, poseEndVis.createArtifact()); } showPlotter(graphicsList, testName); }
yoGraphicsListRegistry.registerArtifact(graphicListName, adjustedDesiredCapturePointViz.createArtifact()); yoGraphicsListRegistry.registerArtifact(graphicListName, centerOfMassViz.createArtifact()); yoGraphicsListRegistry.registerArtifact(graphicListName, desiredCapturePointViz.createArtifact()); yoGraphicsListRegistry.registerArtifact(graphicListName, finalDesiredCapturePointViz.createArtifact()); yoGraphicsListRegistry.registerArtifact(graphicListName, desiredCMPViz.createArtifact()); yoGraphicsListRegistry.registerArtifact(graphicListName, achievedCMPViz.createArtifact()); YoArtifactPosition perfectCMPArtifact = perfectCMPViz.createArtifact(); perfectCMPArtifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(graphicListName, perfectCMPArtifact);
YoGraphicPosition planePointViz = new YoGraphicPosition(caption, yoPlanePoint, 0.005, YoAppearance.Blue(), GraphicType.SOLID_BALL); YoArtifactPosition planePointArtifact = planePointViz.createArtifact(); planePointArtifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, planePointArtifact);
YoArtifactPosition artifact = unprojectedDesiredCMPViz.createArtifact(); artifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(graphicListName, artifact);
yoGraphicsListRegistry.registerArtifact("copSnapping", centerOfMassViz.createArtifact()); yoGraphicsListRegistry.registerArtifact("copSnapping", desiredCenterOfPressureViz.createArtifact()); yoGraphicsListRegistry.registerArtifact("copSnapping", snappedDesiredCenterOfPressureViz.createArtifact()); yoGraphicsListRegistry.registerArtifact("centroid", centroidGraphic.createArtifact()); yoGraphicsListRegistry.registerArtifact("weightedCentroid", weightedCentroidGraphic.createArtifact()); yoGraphicsListRegistry.registerArtifact("inscribedCircleCenter", circleCenterGraphic.createArtifact()); yoGraphicsListRegistry.registerArtifact("miniCircle", miniCircle);
.createArtifact(); yoGraphicsListRegistry.registerArtifact("StateEstimator", comArtifact);
yoGraphicsListRegistry.registerArtifact(prefix + "feetArtifact", footPositionGraphic.createArtifact());