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); }
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 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); }
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); } }
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 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 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); }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, FloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector .createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED, 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED , 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
contactVector.setColor(Color.BLUE); contactVector.setLevel(4); graphicsListRegistry.registerArtifact("Contact State", contactVector); PolygonArtifact polygonViz = new PolygonArtifact("SupportPolygon", true, Color.LIGHT_GRAY, supportPolygon); polygonViz.setLevel(1); graphicsListRegistry.registerArtifact("Contact State", polygonViz); graphicsListRegistry.registerArtifact("CoPs", copViz); desiredForce.setColor(Color.RED); desiredForce.setLevel(2); graphicsListRegistry.registerArtifact("Forces", desiredForce); vectorCount++; achievedForce.setColor(Color.GREEN); achievedForce.setLevel(3); graphicsListRegistry.registerArtifact("Forces", achievedForce); vectorCount++;
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 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); }
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()); }
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()); }
YoArtifactPosition planePointArtifact = planePointViz.createArtifact(); planePointArtifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, planePointArtifact); new YoArtifactLineSegment2d(caption, yoLineOfRotation, Color.GREEN, 0.01, 0.01); lineOfRotationArtifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, lineOfRotationArtifact);
public ExplorationHelper(ContactableFoot contactableFoot, FootControlHelper footControlHelper, String prefix, YoVariableRegistry registry) { footholdExplorationActive = new BooleanYoVariable(prefix + "FootholdExplorationActive", registry); timeExploring = new DoubleYoVariable(prefix + "TimeExploring", registry); startTime = new DoubleYoVariable(prefix + "StartTime", registry); yoCurrentCorner = new IntegerYoVariable(prefix + "CurrentCornerExplored", registry); centerOfPressureCommand.setContactingRigidBody(contactableFoot.getRigidBody()); explorationParameters = footControlHelper.getWalkingControllerParameters().getOrCreateExplorationParameters(registry); if (explorationParameters != null) copCommandWeight = explorationParameters.getCopCommandWeight(); else copCommandWeight = null; soleFrame = footControlHelper.getContactableFoot().getSoleFrame(); partialFootholdControlModule = footControlHelper.getPartialFootholdControlModule(); YoGraphicsListRegistry graphicObjectsListRegistry = footControlHelper.getMomentumBasedController().getDynamicGraphicObjectsListRegistry(); if (graphicObjectsListRegistry != null) { yoDesiredCop = new YoFramePoint2d(prefix + "DesiredExplorationCop", ReferenceFrame.getWorldFrame(), registry); String name = prefix + "Desired Center of Pressure for Exploration"; YoArtifactPosition artifact = new YoArtifactPosition(name, yoDesiredCop.getYoX(), yoDesiredCop.getYoY(), GraphicType.BALL, Color.BLUE, 0.003); graphicObjectsListRegistry.registerArtifact(prefix + getClass().getSimpleName(), artifact); } else { yoDesiredCop = null; } }
yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), lineOfRotationArtifact);
yoGraphicsListRegistry.registerArtifact(prefix + "feetArtifact", footPositionGraphic.createArtifact());