@Override public YoArtifactPosition createArtifact() { return new YoArtifactPosition(getName(), x, y, type, getColor(), scale); }
@Override public void drawHistoryEntry(Graphics2DAdapter graphics, double[] entry) { tempPoint.set(entry[0], entry[1]); drawLocal(graphics); }
@Override public YoArtifact duplicate(YoVariableRegistry newRegistry) { return new YoArtifactPosition(getName(), point.duplicate(newRegistry), graphicType, color, radii.getX()); } }
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; } }
YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copPositionsInWorld.get(foot), 0.005, YoAppearance.DeepPink()); YoArtifactPosition artifact = copInWorld.createArtifact(); artifact.setVisible(false); yoGraphicsListRegistry.registerArtifact("StateEstimator", artifact);
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; } }
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 YoArtifactPosition createArtifact() { return new YoArtifactPosition(getName(), x, y, type, this.getColor(), scale); }
yoGraphicsListRegistry.registerYoGraphic(listName, copViz); YoArtifactPosition artifact = copViz.createArtifact(); artifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, artifact);
@Override public void drawHistoryEntry(Graphics2DAdapter graphics, double[] entry) { tempPoint.set(entry[0], entry[1]); drawLocal(graphics); }
private static YoArtifactPosition yoArtifactPositionFromMessage(String name, YoVariable<?>[] vars, double[] consts, AppearanceDefinition appearance) { return new YoArtifactPosition(name, (YoDouble) vars[0], (YoDouble) vars[1], YoGraphicPosition.GraphicType.values()[(int) (double) consts[1]], appearance.getColor().get(), consts[0]); }
new YoGraphicPosition(caption, yoPlanePoint, 0.005, YoAppearance.Blue(), GraphicType.SOLID_BALL); YoArtifactPosition planePointArtifact = planePointViz.createArtifact(); planePointArtifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, planePointArtifact);
@Override public void draw(Graphics2DAdapter graphics) { tempPoint.set(point); drawLocal(graphics); }
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); } }
YoGraphicPosition unprojectedDesiredCMPViz = new YoGraphicPosition("Unprojected Desired CMP", yoUnprojectedDesiredCMP, 0.008, Purple(), YoGraphicPosition.GraphicType.BALL_WITH_ROTATED_CROSS); YoArtifactPosition artifact = unprojectedDesiredCMPViz.createArtifact(); artifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(graphicListName, artifact);
@Override public void draw(Graphics2DAdapter graphics) { point.get(tempPoint); drawLocal(graphics); }
YoArtifactPosition capturePointViz = new YoArtifactPosition("Capture Point", yoCapturePoint, GraphicType.BALL_WITH_ROTATED_CROSS, Blue().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), capturePointViz); 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); YoArtifactPosition expectedCMPViz = new YoArtifactPosition("Expected Projection", yoExpectedCMP, GraphicType.BALL, LawnGreen().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), expectedCMPViz); YoArtifactPosition finalCapturePointViz = new YoArtifactPosition("Final Capture Point", yoFinalCapturePoint, GraphicType.BALL_WITH_ROTATED_CROSS, Beige().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), finalCapturePointViz);
yoGraphicsListRegistry.registerArtifact(graphicListName, achievedCMPViz.createArtifact()); YoArtifactPosition perfectCMPArtifact = perfectCMPViz.createArtifact(); perfectCMPArtifact.setVisible(false); yoGraphicsListRegistry.registerArtifact(graphicListName, perfectCMPArtifact);
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; } }
return new YoArtifactPosition(name, (DoubleYoVariable) vars[0], (DoubleYoVariable) vars[1], YoGraphicPosition.GraphicType.values()[(int) (double) consts[1]], appearance.getColor().get(), consts[0]);