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 static YoGraphicPosition yoGraphicPositionFromMessage(String name, YoVariable<?>[] vars, double[] consts, AppearanceDefinition appearance) { return new YoGraphicPosition(name, (YoDouble) vars[0], (YoDouble) vars[1], (YoDouble) getVariableOrNull(vars, 2), consts[0], appearance, YoGraphicPosition.GraphicType.values()[(int) (double) consts[1]]); }
public BagOfBalls(double sizeInMeters, String name, List<AppearanceDefinition> appearances, GraphicType graphicType, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { YoVariableRegistry registry = new YoVariableRegistry(name + "Balls"); for (int i = 0; i < appearances.size(); i++) { YoFramePoint3D yoFramePoint = new YoFramePoint3D(name + i, "", worldFrame, registry); YoGraphicPosition newPosition; if (graphicType != null) newPosition = new YoGraphicPosition(name + i, yoFramePoint, sizeInMeters, appearances.get(i), graphicType); else newPosition = new YoGraphicPosition(name + i, yoFramePoint, sizeInMeters, appearances.get(i)); yoGraphicPositions.add(newPosition); } index = 0; registerYoGraphics(name, registry, parentRegistry, yoGraphicsListRegistry); if (graphicType != null) registerArtifacts(name, yoGraphicsListRegistry); parentRegistry.addChild(registry); }
public PlanarRegionBaseOfCliffAvoider(YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { parentRegistry.addChild(registry); if (yoGraphicsListRegistry == null) { visualize = false; } if (visualize) { beforeAdjustmentPosition = new YoGraphicPosition("beforeAdjustmentPosition", "", registry, 0.02, YoAppearance.Red()); afterAdjustmentPosition = new YoGraphicPosition("afterAdjustmentPosition", "", registry, 0.02, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), beforeAdjustmentPosition); yoGraphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), afterAdjustmentPosition); } else { beforeAdjustmentPosition = null; afterAdjustmentPosition = null; } }
public void createVisualizerForConstantCMPs(YoGraphicsList yoGraphicsList, ArtifactList artifactList) { for (int i = 0; i < entryCMPs.size(); i++) { YoGraphicPosition entryCMPViz = new YoGraphicPosition("Entry CMP" + i, entryCMPsInWorldFrameReadOnly.get(i), CMP_POINT_SIZE, YoAppearance.Green(), GraphicType.SOLID_BALL); yoGraphicsList.add(entryCMPViz); artifactList.add(entryCMPViz.createArtifact()); } for (int i = 0; i < exitCMPs.size(); i++) { YoGraphicPosition exitCMPViz = new YoGraphicPosition("Exit CMP" + i, exitCMPsInWorldFrameReadOnly.get(i), CMP_POINT_SIZE, YoAppearance.Green(), GraphicType.BALL); yoGraphicsList.add(exitCMPViz); artifactList.add(exitCMPViz.createArtifact()); } }
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()); }
public ContactPointVisualizer(ArrayList<? extends PlaneContactState> contactStates, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.contactStates = contactStates; int totalNumberOfContactPoints = 0; for (int i = 0; i < contactStates.size(); i++) totalNumberOfContactPoints += contactStates.get(i).getTotalNumberOfContactPoints(); maxNumberOfDynamicGraphicPositions = totalNumberOfContactPoints; for (int i = 0; i < maxNumberOfDynamicGraphicPositions; i++) { YoFramePoint contactPointWorld = new YoFramePoint("contactPoint" + i, worldFrame, this.registry); contactPointsWorld.add(contactPointWorld); YoGraphicPosition dynamicGraphicPosition = new YoGraphicPosition("contactViz" + i, contactPointWorld, 0.01, YoAppearance.Crimson()); dynamicGraphicPositions.add(dynamicGraphicPosition); yoGraphicsListRegistry.registerYoGraphic("contactPoints", dynamicGraphicPosition); YoFrameVector normalVector = new YoFrameVector("contactNormal" + i, worldFrame, registry); normalVectors.add(normalVector); YoGraphicVector dynamicGraphicVector = new YoGraphicVector("contactNormalViz" + i, contactPointWorld, normalVector, YoAppearance.Crimson()); dynamicGraphicVectors.add(dynamicGraphicVector); yoGraphicsListRegistry.registerYoGraphic("contactPoints", dynamicGraphicVector); } parentRegistry.addChild(registry); }
private void setupVisualizers(YoGraphicsListRegistry yoGraphicsListRegistry, boolean visualize) { YoGraphicsList yoGraphicsList = new YoGraphicsList(getClass().getSimpleName()); ArtifactList artifactList = new ArtifactList(getClass().getSimpleName()); YoGraphicPosition previousExitCMP = new YoGraphicPosition("previousExitCMP", previousStanceExitCMP, 0.01, YoAppearance.Red(), GraphicType.SQUARE); YoGraphicPosition entryCMP = new YoGraphicPosition("entryCMP", stanceEntryCMP, 0.01, YoAppearance.Red(), GraphicType.SQUARE); YoGraphicPosition exitCMP = new YoGraphicPosition("exitCMP", stanceExitCMP, 0.01, YoAppearance.Red(), GraphicType.SQUARE); YoGraphicPosition finalICP = new YoGraphicPosition("finalICP", this.finalICP, 0.005, YoAppearance.Black(), GraphicType.SOLID_BALL); yoGraphicsList.add(finalICP); artifactList.add(previousExitCMP.createArtifact()); artifactList.add(entryCMP.createArtifact()); artifactList.add(exitCMP.createArtifact()); artifactList.add(finalICP.createArtifact()); artifactList.setVisible(visualize); yoGraphicsList.setVisible(visualize); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerArtifactList(artifactList); }
private void setupVisualization(String prefix, YoGraphicsListRegistry graphicsListRegistry, YoVariableRegistry registry) { YoGraphicsList yoGraphicsList = new YoGraphicsList(prefix + "VisGraph"); for (int i = 0; i < bodyPathPointsForVisualization; i++) { YoFramePoint3D point = new YoFramePoint3D(prefix + "BodyPathPoint" + i, ReferenceFrame.getWorldFrame(), registry); point.setToNaN(); bodyPathPoints.add(point); YoGraphicPosition pointVisualization = new YoGraphicPosition(prefix + "BodyPathPoint" + i, point, 0.02, YoAppearance.Yellow()); yoGraphicsList.add(pointVisualization); } graphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
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 LineTrajectory(double controlDT, Tuple3DReadOnly initialPosition, YoVariableRegistry parentRegistry, YoGraphicsListRegistry graphicsListRegistry) { this.controlDT = controlDT; Vector3D initialPositionA = new Vector3D(initialPosition); Vector3D initialPositionB = new Vector3D(initialPosition); initialPositionA.addX(0.025); initialPositionB.addX(-0.025); pointA = new ParameterVector3D("PointA", initialPositionA, registry); pointB = new ParameterVector3D("PointB", initialPositionB, registry); frequency = new DoubleParameter("Frequency", registry, 0.25); maxVelocity = new DoubleParameter("MaxVelocity", registry, 0.1); limitedPointA = new RateLimitedYoFramePoint("PointALim", "", registry, maxVelocity, controlDT, createFrameTuple(ReferenceFrame.getWorldFrame(), pointA)); limitedPointB = new RateLimitedYoFramePoint("PointBLim", "", registry, maxVelocity, controlDT, createFrameTuple(ReferenceFrame.getWorldFrame(), pointB)); pointAViz = new YoGraphicPosition("PointAViz", limitedPointA, 0.025, YoAppearance.Blue()); pointBViz = new YoGraphicPosition("PointBViz", limitedPointB, 0.025, YoAppearance.Blue()); graphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), pointAViz); graphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), pointBViz); parentRegistry.addChild(registry); }
@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); } }
public static void addGoalViz(FramePose3D goalPose, YoVariableRegistry registry, YoGraphicsListRegistry graphicsListRegistry) { YoFramePoint3D yoGoal = new YoFramePoint3D("GoalPosition", worldFrame, registry); yoGoal.set(goalPose.getPosition()); graphicsListRegistry.registerYoGraphic("viz", new YoGraphicPosition("GoalViz", yoGoal, 0.05, YoAppearance.Yellow())); YoFramePoint3D yoStart = new YoFramePoint3D("StartPosition", worldFrame, registry); graphicsListRegistry.registerYoGraphic("viz", new YoGraphicPosition("StartViz", yoStart, 0.05, YoAppearance.Blue())); PoseReferenceFrame goalFrame = new PoseReferenceFrame("GoalFrame", goalPose); FrameVector3D goalOrientation = new FrameVector3D(goalFrame, 0.5, 0.0, 0.0); goalOrientation.changeFrame(worldFrame); YoFrameVector3D yoGoalOrientation = new YoFrameVector3D("GoalVector", worldFrame, registry); yoGoalOrientation.set(goalOrientation); // graphicsListRegistry.registerYoGraphic("vizOrientation", new YoGraphicVector("GoalOrientationViz", yoGoal, yoGoalOrientation, 1.0, YoAppearance.White())); }
private void addSupportBaseGraphics(YoGraphicsListRegistry yoGraphicsListRegistry,YoFramePoint3D[] basePoints, ArrayList<YoGraphic> basePointsList, ArrayList<YoGraphic> linesList, String namePrefix,AppearanceDefinition appearance) { AppearanceDefinition[] colors = { YoAppearance.Red(), YoAppearance.Green(), YoAppearance.Blue(), YoAppearance.Yellow() }; YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "Points"); for (int i = 0; i < basePoints.length; i++) { YoGraphicPosition baseControlPointViz = new YoGraphicPosition(namePrefix + "Point" + i, basePoints[i], 0.01, colors[i]); yoGraphicsList.add(baseControlPointViz); basePointsList.add(baseControlPointViz); for (int j = i + 1; j < basePoints.length; j++) { YoGraphicLineSegment yoGraphicLineSegment = new YoGraphicLineSegment(namePrefix + "SupportLine", basePoints[i], basePoints[j], 1.0, appearance, false); yoGraphicsList.add(yoGraphicLineSegment); linesList.add(yoGraphicLineSegment); } } if (yoGraphicsListRegistry != null) yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsList.hideYoGraphics(); }
private void addSupportBaseGraphics(YoGraphicsListRegistry yoGraphicsListRegistry,YoFramePoint[] basePoints, ArrayList<YoGraphic> basePointsList, ArrayList<YoGraphic> linesList, String namePrefix,AppearanceDefinition appearance) { AppearanceDefinition[] colors = { YoAppearance.Red(), YoAppearance.Green(), YoAppearance.Blue(), YoAppearance.Yellow() }; YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "Points"); for (int i = 0; i < basePoints.length; i++) { YoGraphicPosition baseControlPointViz = new YoGraphicPosition(namePrefix + "Point" + i, basePoints[i], 0.01, colors[i]); yoGraphicsList.add(baseControlPointViz); basePointsList.add(baseControlPointViz); for (int j = i + 1; j < basePoints.length; j++) { YoGraphicLineSegment dynamicGraphicLineSegment = new YoGraphicLineSegment(namePrefix + "SupportLine", basePoints[i], basePoints[j], 1.0, appearance, false); yoGraphicsList.add(dynamicGraphicLineSegment); linesList.add(dynamicGraphicLineSegment); } } if (yoGraphicsListRegistry != null) yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsList.hideYoGraphics(); }
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 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 MomentumVisualizer(String name, TwistCalculator twistCalculator, YoVariableRegistry registry, YoGraphicsListRegistry graphicsRegistry, RigidBody... rigidBodies) { comCalculator = new CenterOfMassCalculator(rigidBodies, ReferenceFrame.getWorldFrame()); momentumCalculator = new MomentumCalculator(twistCalculator, rigidBodies); centerOfMass = new YoFramePoint(name + "CoM", ReferenceFrame.getWorldFrame(), registry); linearMomentum = new YoFrameVector(name + "Momentum", ReferenceFrame.getWorldFrame(), registry); YoGraphicPosition yoCoMGraphics = new YoGraphicPosition(name + "CoM", centerOfMass, 0.05, YoAppearance.Brown()); YoGraphicVector yoMomentumGraphics = new YoGraphicVector(name + "Momentum", centerOfMass, linearMomentum, 0.05, YoAppearance.Brown()); graphicsRegistry.registerYoGraphic(name, yoCoMGraphics); graphicsRegistry.registerYoGraphic(name, yoMomentumGraphics); }
public void createAvailableContactPoints(int groupIdentifier, int totalContactPointsAvailable, double forceVectorScale, boolean addYoGraphicForceVectorsForceVectors) { YoGraphicsListRegistry yoGraphicsListRegistry = null; if (addYoGraphicForceVectorsForceVectors) yoGraphicsListRegistry = new YoGraphicsListRegistry(); for (int i = 0; i < totalContactPointsAvailable; i++) { GroundContactPoint contactPoint = new GroundContactPoint("contact_" + name + "_" + i, robot.getRobotsYoVariableRegistry()); getJoint().addGroundContactPoint(groupIdentifier, contactPoint); allGroundContactPoints.add(contactPoint); YoBoolean contactAvailable = new YoBoolean("contact_" + name + "_" + i + "_avail", robot.getRobotsYoVariableRegistry()); contactAvailable.set(true); contactsAvailable.add(contactAvailable); if (addYoGraphicForceVectorsForceVectors) { YoGraphicPosition yoGraphicPosition = new YoGraphicPosition(name + "Point" + i, contactPoint.getYoPosition(), 0.02, YoAppearance.Green()); YoGraphicVector yoGraphicVector = new YoGraphicVector(name + "Force" + i, contactPoint.getYoPosition(), contactPoint.getYoForce(), forceVectorScale, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicPosition); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicVector); } } if (addYoGraphicForceVectorsForceVectors) { robot.addYoGraphicsListRegistry(yoGraphicsListRegistry); } }