public SingleFootstepVisualizer(RobotSide robotSide, int maxContactPoints, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { Integer index = indices.get(robotSide); String namePrefix = robotSide.getLowerCaseName() + "Foot" + index; YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix); this.robotSide = robotSide; ArrayList<Point2d> polyPoints = new ArrayList<Point2d>(); yoContactPoints = new YoFramePoint[maxContactPoints]; for (int i = 0; i < maxContactPoints; i++) { yoContactPoints[i] = new YoFramePoint(namePrefix + "ContactPoint" + i, ReferenceFrame.getWorldFrame(), registry); yoContactPoints[i].set(0.0, 0.0, -1.0); YoGraphicPosition baseControlPointViz = new YoGraphicPosition(namePrefix + "Point" + i, yoContactPoints[i], 0.01, YoAppearance.Blue()); yoGraphicsList.add(baseControlPointViz); polyPoints.add(new Point2d()); } footPolygon = new YoFrameConvexPolygon2d(namePrefix + "yoPolygon", "", ReferenceFrame.getWorldFrame(), maxContactPoints, registry); footPolygon.setConvexPolygon2d(new ConvexPolygon2d(polyPoints)); soleFramePose = new YoFramePose(namePrefix + "polygonPose", "", ReferenceFrame.getWorldFrame(), registry); soleFramePose.setXYZ(0.0, 0.0, -1.0); footPolygonViz = new YoGraphicPolygon(namePrefix + "graphicPolygon", footPolygon, soleFramePose, 1.0, footPolygonAppearances.get(robotSide)); yoGraphicsList.add(footPolygonViz); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(footPolygonViz); } index++; indices.set(robotSide, index); }
public SingleFootstepVisualizer(RobotSide robotSide, int maxContactPoints, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { Integer index = indices.get(robotSide); String namePrefix = robotSide.getLowerCaseName() + "Foot" + index; YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix); this.robotSide = robotSide; ArrayList<Point2D> polyPoints = new ArrayList<Point2D>(); yoContactPoints = new YoFramePoint3D[maxContactPoints]; for (int i = 0; i < maxContactPoints; i++) { yoContactPoints[i] = new YoFramePoint3D(namePrefix + "ContactPoint" + i, ReferenceFrame.getWorldFrame(), registry); yoContactPoints[i].set(0.0, 0.0, -1.0); YoGraphicPosition baseControlPointViz = new YoGraphicPosition(namePrefix + "Point" + i, yoContactPoints[i], 0.01, YoAppearance.Blue()); yoGraphicsList.add(baseControlPointViz); polyPoints.add(new Point2D()); } footPolygon = new YoFrameConvexPolygon2D(namePrefix + "yoPolygon", "", ReferenceFrame.getWorldFrame(), maxContactPoints, registry); footPolygon.set(new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(polyPoints))); soleFramePose = new YoFramePoseUsingYawPitchRoll(namePrefix + "polygonPose", "", ReferenceFrame.getWorldFrame(), registry); soleFramePose.setXYZ(0.0, 0.0, -1.0); footPolygonViz = new YoGraphicPolygon(namePrefix + "graphicPolygon", footPolygon, soleFramePose, 1.0, footPolygonAppearances.get(robotSide)); yoGraphicsList.add(footPolygonViz); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(footPolygonViz); } index++; indices.set(robotSide, index); }
polygonVisualizations.add(visualization); graphicsListRegistry.registerYoGraphic("viz", visualization); graphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(visualization);
polygonVisualizations.add(visualization); graphicsListRegistry.registerYoGraphic("viz", visualization); graphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(visualization);
yoGraphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(yoGraphicPolygon); yoGraphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(yoGraphicYoFramePolygon);