private void createABall(String name, int i, double sizeInMeters, AppearanceDefinition appearance, YoVariableRegistry registry) { YoFramePoint yoFramePoint = new YoFramePoint(name + i, "", ReferenceFrame.getWorldFrame(), registry); Double scale = new Double(sizeInMeters); YoGraphicPosition newPosition = new YoGraphicPosition(name + i, yoFramePoint, scale, appearance); dynamicGraphicPositions.add(newPosition); }
private void updatePositionGraphics() { comPositionGraphic.setPosition(icpPlannerData1.comPosition); icpPositionGraphic.setPosition(icpPlannerData1.icpPosition); cmpPositionGraphic.setPosition(icpPlannerData1.cmpPosition); copPositionGraphic.setPosition(icpPlannerData1.copPosition); }
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 hideGraphics(YoGraphicPosition graphics) { graphics.setPositionToNaN(); graphics.update(); }
public void setBall(double x, double y, double z, AppearanceDefinition appearance, int ballIndex) { if (ballIndex < dynamicGraphicPositions.size()) { YoGraphicPosition dynamicGraphicPosition = dynamicGraphicPositions.get(ballIndex); dynamicGraphicPosition.setPosition(x, y, z); if (appearance != null) dynamicGraphicPosition.setAppearance(appearance); } else { if (!this.outOfBallsWarning) { // System.err.println("Bag of Balls doesn't have enough footstep graphic positions!"); this.outOfBallsWarning = true; } } }
private void setGraphics(YoGraphicPosition graphics, FootstepNode node) { graphics.setPosition(node.getX(), node.getY(), 0.0); graphics.update(); }
YoGraphicPosition footPositionGraphic = new YoGraphicPosition(prefix + "footPositionViz", footPosition, 0.02, yoAppearance, GraphicType.BALL_WITH_CROSS); footPositionGraphics.set(robotQuadrant, footPositionGraphic); yoGraphicsListRegistry.registerArtifact(prefix + "feetArtifact", footPositionGraphic.createArtifact()); footPositionGraphics.get(robotQuadrant).update();
YoGraphicPosition copViz = new YoGraphicPosition(copName, cop, 0.005, YoAppearance.Navy(), YoGraphicPosition.GraphicType.BALL); copViz.setVisible(VISUALIZE); yoGraphicsListRegistry.registerYoGraphic(listName, copViz); YoArtifactPosition artifact = copViz.createArtifact(); artifact.setVisible(VISUALIZE); yoGraphicsListRegistry.registerArtifact(listName, artifact);
YoGraphicPosition pointOnAViz = new YoGraphicPosition("pointOnA", "_" + i, robot.getRobotsYoVariableRegistry(), 0.03, YoAppearance.Purple()); YoGraphicPosition pointOnBViz = new YoGraphicPosition("pointOnB", "_" + i, robot.getRobotsYoVariableRegistry(), 0.03, YoAppearance.Gold()); pointsOnA.get(j).setPosition(Double.NaN, Double.NaN, Double.NaN); pointsOnB.get(j).setPosition(Double.NaN, Double.NaN, Double.NaN); pointsOnA.get(vizIndex).setPosition(pointOnA); pointsOnB.get(vizIndex).setPosition(pointOnB);
@Override public YoGraphicPosition duplicate(YoVariableRegistry newRegistry) { YoDouble x = (YoDouble) newRegistry.getVariable(this.x.getFullNameWithNameSpace()); YoDouble y = (YoDouble) newRegistry.getVariable(this.y.getFullNameWithNameSpace()); YoDouble z = (YoDouble) newRegistry.getVariable(this.z.getFullNameWithNameSpace()); return new YoGraphicPosition(getName(), x, y, z, scale, appearance, type); } }
framePointS0.setZ(s0.getY() + offsetHeightAboveGround.getDoubleValue()); framePointS0.changeFrame(worldFrame); pointS0Viz.setPosition(framePointS0); pointS0MinViz.setPosition(framePointS0); pointS0MaxViz.setPosition(framePointS0); framePointD0.setZ(d0.getY() + offsetHeightAboveGround.getDoubleValue()); framePointD0.changeFrame(worldFrame); pointD0Viz.setPosition(framePointD0); pointD0MinViz.setPosition(framePointD0); pointD0MaxViz.setPosition(framePointD0); framePointDF.setZ(dF.getY() + offsetHeightAboveGround.getDoubleValue()); framePointDF.changeFrame(worldFrame); pointDFViz.setPosition(framePointDF); pointDFMinViz.setPosition(framePointDF); pointDFMaxViz.setPosition(framePointDF); pointSFViz.setPosition(framePointSF); pointSFMinViz.setPosition(framePointSF); pointSFMaxViz.setPosition(framePointSF);
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); } }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).set(externalForcePoints.get(i).getYoPosition()); desiredHeight.add(initialPositions.get(i).getZ() / initialPositions.size()); efp_positionViz.get(i).update(); } for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); doControl(); }
private void updateContactPointDynamicGraphicObjects(int i, ContactPointInterface contactPoint) { if (contactPoint.isInContact()) { contactPoint.getPosition(tempFramePoint); tempFramePoint.changeFrame(worldFrame); contactPointsWorld.get(i).set(tempFramePoint); normalVectors.get(i).set(tempFrameVector); dynamicGraphicPositions.get(i).showGraphicObject(); dynamicGraphicVectors.get(i).showGraphicObject(); } else { contactPointsWorld.get(i).setToNaN(); normalVectors.get(i).set(Double.NaN, Double.NaN, Double.NaN); dynamicGraphicPositions.get(i).hideGraphicObject(); dynamicGraphicVectors.get(i).hideGraphicObject(); } dynamicGraphicPositions.get(i).update(); dynamicGraphicVectors.get(i).update(); } }
/** * Hides all the balls. */ public void hideAll() { index = 0; for (int i = 0; i < yoGraphicPositions.size(); i++) { yoGraphicPositions.get(i).setPositionToNaN(); } }
public void setBall(double x, double y, double z, AppearanceDefinition appearance, int ballIndex) { if (ballIndex < yoGraphicPositions.size()) { YoGraphicPosition yoGraphicPosition = yoGraphicPositions.get(ballIndex); yoGraphicPosition.setPosition(x, y, z); if (appearance != null) yoGraphicPosition.setAppearance(appearance); } else { if (!outOfBallsWarning) { // System.err.println("Bag of Balls doesn't have enough footstep graphic positions!"); outOfBallsWarning = true; } } }
private boolean isPositionReachable(int xIndex, int yIndex, int zIndex) { voxel3dGrid.getVoxel(voxelLocation, xIndex, yIndex, zIndex); modifiableVoxelLocation.setIncludingFrame(voxelLocation); modifiableVoxelLocation.changeFrame(ReferenceFrame.getWorldFrame()); currentEvaluationPosition.setPosition(modifiableVoxelLocation); currentEvaluationPosition.update(); return solver.solveFor(voxelLocation); }
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); }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { externalForcePoints.get(i).getYoPosition().get(initialPositions.get(i)); desiredHeight.add(initialPositions.get(i).getZ() / initialPositions.size()); efp_positionViz.get(i).update(); } for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); doControl(); }
/** * Hides all the balls. */ public void hideAll() { index = 0; for (int i = 0; i < dynamicGraphicPositions.size(); i++) { dynamicGraphicPositions.get(i).setPositionToNaN(); } }