/** * This is where the end-effectors needing a visualization are registered, if you need more, add * it there. * * @param rigidBodies all the rigid bodies for which the desired and actual pose will be * displayed using graphical coordinate systems. */ public void setupVisualization(RigidBodyBasics... rigidBodies) { AppearanceDefinition desiredAppearance = YoAppearance.Red(); AppearanceDefinition currentAppearance = YoAppearance.Blue(); for (RigidBodyBasics rigidBody : rigidBodies) { YoGraphicCoordinateSystem desiredCoodinateSystem = createCoodinateSystem(rigidBody, Type.DESIRED, desiredAppearance); YoGraphicCoordinateSystem currentCoodinateSystem = createCoodinateSystem(rigidBody, Type.CURRENT, currentAppearance); desiredCoodinateSystems.put(rigidBody, desiredCoodinateSystem); currentCoodinateSystems.put(rigidBody, currentCoodinateSystem); yoGraphicsListRegistry.registerYoGraphic("CoordinateSystems", desiredCoodinateSystem); yoGraphicsListRegistry.registerYoGraphic("CoordinateSystems", currentCoodinateSystem); } }
private YoGraphicsListRegistry createYoGraphicsListRegistryWithObject() { YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoGraphicsList yoGraphicsList = new YoGraphicsList(yoGraphicsListName); yoGraphicsList.add(yoGraphic); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); return yoGraphicsListRegistry; }
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 setupYoRegistries() { yoGraphicsListRegistry = new YoGraphicsListRegistry(); yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(true); yoGraphicsListRegistryForDetachedOverhead = new YoGraphicsListRegistry(); }
private void initializeCollisionManager() { if (useShapeCollision) { double coefficientOfRestitution = 0.0; double coefficientOfFriction = 0.9; HybridImpulseSpringDamperCollisionHandler collisionHandler = new HybridImpulseSpringDamperCollisionHandler(coefficientOfRestitution, coefficientOfFriction, simulationConstructionSet.getRootRegistry(), new YoGraphicsListRegistry()); collisionHandler.setKp(100000); collisionHandler.setKd(500); CollisionManager collisionManager = new CollisionManager(commonAvatarEnvironment.get().getTerrainObject3D(), collisionHandler); simulationConstructionSet.initializeShapeCollision(collisionManager); } }
public void registerYoGraphicsLists(ArrayList<YoGraphicsList> yoGraphicsLists) { for (YoGraphicsList yoGraphicsList : yoGraphicsLists) { registerYoGraphicsList(yoGraphicsList); } }
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; } }
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()); }
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); }
yoGraphicsListRegistry.registerYoGraphicsList(dgoListMap.get(list)); yoGraphicsListRegistry.registerArtifactList(artifactList);
final SideDependentList<YoFrameConvexPolygon2D> yoFootPolygons = new SideDependentList<>(); YoVariableRegistry registry = robot.getRobotsYoVariableRegistry(); final YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); final SideDependentList<YoArtifactPolygon> footArtifacts = new SideDependentList<>(); for (final RobotSide robotSide : RobotSide.values) footColor = Color.green; YoArtifactPolygon footArtifact = new YoArtifactPolygon(robotSide.getCamelCaseNameForStartOfExpression(), yoFootPolygon, footColor, false); yoGraphicsListRegistry.registerArtifact("Feet", footArtifact); footArtifacts.put(robotSide, footArtifact); yoGraphicsListRegistry.registerArtifact("Capture", captureRegionArtifact); final YoEnum<RobotSide> yoSupportSide = new YoEnum<>("supportSide", registry, RobotSide.class); final YoDouble swingTimeRemaining = new YoDouble("swingTimeRemaining", registry); final YoFramePoint2D yoICP = new YoFramePoint2D("ICP", worldFrame, registry); yoGraphicsListRegistry.registerArtifact("ICP", new YoGraphicPosition("ICP", yoICP, 0.02, YoAppearance.Blue(), GraphicType.CROSS).createArtifact()); final double omega0 = 3.4; scs.addExtraJpanel(scrollPane, "Plotter Legend", false); yoGraphicsListRegistry.update(); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry, false); yoGraphicsListRegistry.addArtifactListsToPlotter(simulationOverheadPlotter.getPlotter()); Thread myThread = new Thread(scs); myThread.start();
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); }
public void registerArtifactLists(ArrayList<ArtifactList> artifactLists) { for (ArtifactList artifactList : artifactLists) { registerArtifactList(artifactList); } }
yoGraphicsListRegistry.getRegisteredArtifactLists(artifactLists); for (ArtifactList artifactList : artifactLists) frame.setVisible(true); yoGraphicsListRegistry.addArtifactListsToPlotter(plotter);
private void updateYoGraphics(YoGraphicsListRegistry yoGraphicsListRegistry) { if (yoGraphicsListRegistry == null) return; List<YoGraphicsList> yoGraphicsLists = yoGraphicsListRegistry.getYoGraphicsLists(); for (YoGraphicsList yoGraphicsList : yoGraphicsLists) { ArrayList<YoGraphic> yoGraphics = yoGraphicsList.getYoGraphics(); for (YoGraphic yoGraphic : yoGraphics) yoGraphic.update(); } yoGraphicsListRegistry.update(); }
private void doControl() { updateRobotState(); updateControlStatus(); updateRequestedBehavior(); callUpdatables(); stateMachine.checkTransitionConditions(); stateMachine.doAction(); yoGraphicsListRegistry.update(); }
@Override public void updateYoGraphicsListRegistry() { if(robotControlElement.getYoGraphicsListRegistry() != null) { if(registry!=null) registry.updateChangedValues(); if (simulatedRobot != null) { simulatedRobot.getRootJoints().get(0).getTransformToWorld(transformToWorld); robotControlElement.getYoGraphicsListRegistry().setSimulationTransformToWorld(transformToWorld); } robotControlElement.getYoGraphicsListRegistry().update(); } }
public final void setLogDataProcessor(LogDataProcessorFunction logDataProcessor) { YoGraphicsListRegistry yoGraphicsListRegistry = logDataProcessor.getYoGraphicsListRegistry(); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.addArtifactListsToPlotter(plotter); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } logDataProcessorWrapper.addLogDataProcessor(logDataProcessor); }
public static PlanarRegionBipedalFootstepPlannerVisualizer createWithYoVariableServer(double dtForViz, FullRobotModel fullRobotModel, LogModelProvider logModelProvider, SideDependentList<ConvexPolygon2d> footPolygonsInSoleFrame, String namePrefix) { YoVariableRegistry registry = new YoVariableRegistry(PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName()); YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); PlanarRegionBipedalFootstepPlannerVisualizer footstepPlannerVisualizer = new PlanarRegionBipedalFootstepPlannerVisualizer(10, footPolygonsInSoleFrame, registry, graphicsListRegistry); PeriodicThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler("PlannerScheduler"); YoVariableServer yoVariableServer = new YoVariableServer(namePrefix + PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName(), scheduler, logModelProvider, LogSettings.FOOTSTEP_PLANNER, dtForViz); yoVariableServer.setSendKeepAlive(true); footstepPlannerVisualizer.setTickAndUpdatable(yoVariableServer); yoVariableServer.setMainRegistry(registry, fullRobotModel, graphicsListRegistry); yoVariableServer.start(); return footstepPlannerVisualizer; }
public void registerYoGraphicsLists(ArrayList<YoGraphicsList> yoGraphicsLists) { for (YoGraphicsList yoGraphicsList : yoGraphicsLists) { registerYoGraphicsList(yoGraphicsList); } }