/** * Create a Bag of Balls with alternating ball color going through the cycle of the colors of the * rainbow. * * @param numberOfBalls int Number of balls to create. * @param sizeInMeters double Size of each ball in meters. * @param name String Name of the BagOfBalls to create. * @param parentYoVariableRegistry YoVariableRegistry to register the BagOfBalls with. * @param yoGraphicsListRegistry YoGraphicsListRegistry to register the BagOfBalls * with. * @return BagOfBalls */ public static BagOfBalls createRainbowBag(int numberOfBalls, double sizeInMeters, String name, YoVariableRegistry parentYoVariableRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { AppearanceDefinition[] rainbow = YoAppearance.getStandardRoyGBivRainbow(); ArrayList<AppearanceDefinition> appearances = new ArrayList<>(); for (int i = 0; i < numberOfBalls; i++) { appearances.add(rainbow[i % rainbow.length]); } return new BagOfBalls(sizeInMeters, name, appearances, parentYoVariableRegistry, yoGraphicsListRegistry); }
public void setBall(double x, double y, double z, AppearanceDefinition appearance) { setBall(x, y, z, appearance, index); index++; }
/** * Sets the next ball to the given location. If all the balls have been set, then loops to the first ball. * @param location FramePoint to set the next ball to. */ public void setBallLoop(FramePoint location) { setBallLoop(location, null); }
private void updateICPCornerPoints() { List<? extends FramePoint3DReadOnly> icpInitialDesiredPositions = planner.getReferenceICPGenerator().getICPPositionDesiredInitialList(); List<? extends FramePoint3DReadOnly> icpFinalDesiredPositions = planner.getReferenceICPGenerator().getICPPositionDesiredFinalList(); icpInitialCornerPoints.reset(); for (int i = 0; i < icpInitialDesiredPositions.size(); i++) { icpInitialCornerPoints.setBall(icpInitialDesiredPositions.get(i)); } icpFinalCornerPoints.reset(); for (int i = 0; i < icpFinalDesiredPositions.size(); i++) { icpFinalCornerPoints.setBall(icpFinalDesiredPositions.get(i)); } }
private static void visualizeBodyPath(List<Point3DReadOnly> bodyPath, BagOfBalls vizToUpdate) { int numberOfBalls = vizToUpdate.getNumberOfBalls(); double bodyPathLength = 0.0; for (int i = 0; i < bodyPath.size() - 1; i++) { bodyPathLength += bodyPath.get(i).distance(bodyPath.get(i + 1)); } double distanceToTravel = bodyPathLength / (numberOfBalls - 1.0); Point3D position = new Point3D(bodyPath.get(0)); vizToUpdate.setBall(position); for (int i = 0; i < numberOfBalls - 1; i++) { position = travelAlongBodyPath(distanceToTravel, position, bodyPath); vizToUpdate.setBall(position, i); } }
bagOfBalls = new BagOfBalls(robot.getRobotsYoVariableRegistry(), yoGraphicsListRegistry); surfaceNormalPointForViz = new YoFramePoint3D("surfaceNormalPointForViz", ReferenceFrame.getWorldFrame(), robot.getRobotsYoVariableRegistry()); surfaceNormalViz = new YoFrameVector3D("surfaceNormalVector", ReferenceFrame.getWorldFrame(), robot.getRobotsYoVariableRegistry()); bagOfBalls.setBallLoop(new FramePoint3D(ReferenceFrame.getWorldFrame(), queryPoint), appearance); scs.setTime(scs.getTime() + 0.001); scs.tickAndUpdate();
graphicsListRegistry.registerYoGraphic("viz", currentPositionVisualization); bodyPathViz = new BagOfBalls(bodyPathVisualizationResolution, 0.01, "bodyPath", registry, graphicsListRegistry); bodyPathWaypointsViz = new BagOfBalls(100, 0.025, YoAppearance.Yellow(), registry, graphicsListRegistry); bodyPathWaypointsViz.hideAll(); bodyPath.forEach(bodyPathWaypointsViz::setBall); scs.setTime(iteration);
bagOfBalls.reset(); int numberOfPoints = 30; CoMHeightPartialDerivativesData coMHeightPartialDerivativesData = new CoMHeightPartialDerivativesData(); tempFramePointForViz2.setY(tempFramePointForViz1.getY()); bagOfBalls.setBallLoop(tempFramePointForViz2);
public void variableChanged(YoVariable<?> v) { boolean visible = showViz.getBooleanValue(); currentPositionViz.setVisible(visible); initialPositionViz.setVisible(visible); finalPositionViz.setVisible(visible); initialDirectionViz.setVisible(visible); bagOfBalls.setVisible(visible); if (!visible) bagOfBalls.hideAll(); } });
@Override public void hideVisualization() { if (trajectoryViz == null) return; trajectoryViz.hideAll(); }
private void updateCoMCornerPoints() { List<? extends FramePoint3DReadOnly> comInitialDesiredPositions = planner.getReferenceCoMGenerator().getCoMPositionDesiredInitialList(); List<? extends FramePoint3DReadOnly> comFinalDesiredPositions = planner.getReferenceCoMGenerator().getCoMPositionDesiredFinalList(); comInitialCornerPoints.reset(); for (int i = 0; i < comInitialDesiredPositions.size(); i++) { comInitialCornerPoints.setBall(comInitialDesiredPositions.get(i)); } comFinalCornerPoints.reset(); for (int i = 0; i < comFinalDesiredPositions.size(); i++) { comFinalCornerPoints.setBall(comFinalDesiredPositions.get(i)); } }
@Override public void variableChanged(YoVariable<?> v) { boolean visible = showViz.getBooleanValue(); currentPositionViz.setVisible(visible); initialPositionViz.setVisible(visible); finalPositionViz.setVisible(visible); initialDirectionViz.setVisible(visible); finalDirectionViz.setVisible(visible); distortedPlaneViz.setVisible(visible); bagOfBalls.setVisible(visible); if (!visible) bagOfBalls.hideAll(); } });
@Override public void hideVisualization() { if (trajectoryViz == null) return; trajectoryViz.hideAll(); }
/** * Create a Bag of Balls with alternating ball color going through the cycle of the colors of the rainbow. * * @param numberOfBalls int Number of balls to create. * @param sizeInMeters double Size of each ball in meters. * @param name String Name of the BagOfBalls to create. * @param parentYoVariableRegistry YoVariableRegistry to register the BagOfBalls with. * @param yoGraphicsListRegistry DynamicGraphicObjectsListRegistry to register the BagOfBalls with. * @return BagOfBalls */ public static BagOfBalls createRainbowBag(int numberOfBalls, double sizeInMeters, String name, YoVariableRegistry parentYoVariableRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { AppearanceDefinition[] rainbow = YoAppearance.getStandardRoyGBivRainbow(); ArrayList<AppearanceDefinition> appearances = new ArrayList<AppearanceDefinition>(); for (int i = 0; i < numberOfBalls; i++) { appearances.add(rainbow[i % rainbow.length]); } return new BagOfBalls(sizeInMeters, name, appearances, parentYoVariableRegistry, yoGraphicsListRegistry); }
public void setBall(FramePoint3DReadOnly location, AppearanceDefinition appearance) { setBall(location, appearance, index); index++; }
private void updateCoPCornerPoints() { List<CoPPointsInFoot> copCornerPointPositions = planner.getReferenceCoPGenerator().getWaypoints(); copCornerPoints.reset(); for (int i = 0; i < copCornerPointPositions.size(); i++) { CoPPointsInFoot copPoints = copCornerPointPositions.get(i); for (int j = 0; j < copPoints.getNumberOfCoPPoints(); j++) { copCornerPoints.setBall(copPoints.getWaypointInWorld(j)); } } }
/** * Sets the next ball to the given location. If all the balls have been set, then loops to the * first ball. * * @param location FramePoint to set the next ball to. */ public void setBallLoop(FramePoint3DReadOnly location) { setBallLoop(location, null); }
private void setupTrackBallsVisualization() { comTrack = new BagOfBalls(numberOfTrackBalls, trackBallSize, "CoMTrack", new YoAppearanceRGBColor(comPointsColor, 0.0), registry, graphicsListRegistry); icpTrack = new BagOfBalls(numberOfTrackBalls, trackBallSize, "ICPTrack", new YoAppearanceRGBColor(icpPointsColor, 0.0), registry, graphicsListRegistry); cmpTrack = new BagOfBalls(numberOfTrackBalls, trackBallSize, "CMPTrack", new YoAppearanceRGBColor(cmpPointsColor, 0.0), registry, graphicsListRegistry); copTrack = new BagOfBalls(numberOfTrackBalls, trackBallSize, "CoPTrack", new YoAppearanceRGBColor(copPointsColor, 0.0), registry, graphicsListRegistry); }
public void setBall(double x, double y, double z, AppearanceDefinition appearance) { setBall(x, y, z, appearance, index); index++; }