public Graphics3DObject getGraphicObject() { graphicObject.transform(transform); graphicObject.addCapsule(radius, height, YoAppearance.Yellow()); return graphicObject; }
private AppearanceDefinition getYoAppearance(RobotQuadrant robotQuadrant) { switch (robotQuadrant) { case FRONT_LEFT: return YoAppearance.White(); case FRONT_RIGHT: return YoAppearance.Yellow(); case HIND_LEFT: return YoAppearance.Blue(); case HIND_RIGHT: return YoAppearance.Black(); default: throw new RuntimeException("bad quad"); } }
private AppearanceDefinition getYoAppearance(RobotQuadrant robotQuadrant) { switch (robotQuadrant) { case FRONT_LEFT: return YoAppearance.White(); case FRONT_RIGHT: return YoAppearance.Yellow(); case HIND_LEFT: return YoAppearance.Blue(); case HIND_RIGHT: return YoAppearance.Black(); default: throw new RuntimeException("bad quad"); } }
@Override public Graphics3DObject getGraphicObject() { RigidBodyTransform rigidbodyTransform = transform; Point3D translationToCreate = new Point3D(0, 0, -sizeZ / 2); rigidbodyTransform.appendTranslation(translationToCreate); graphicObject.transform(rigidbodyTransform); AppearanceDefinition app; app = YoAppearance.Yellow(); graphicObject.addCube(sizeX, sizeY, sizeZ, app); return graphicObject; }
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 static AppearanceDefinition[] getStandardRoyGBivRainbow() { AppearanceDefinition[] rainbow = new AppearanceDefinition[] { YoAppearance.Red(), YoAppearance.OrangeRed(), YoAppearance.Yellow(), YoAppearance.Green(), YoAppearance.Blue(), YoAppearance.Indigo(), YoAppearance.Purple()}; return rainbow; }
public static AppearanceDefinition[] getStandardRoyGBivRainbow() { AppearanceDefinition[] rainbow = new AppearanceDefinition[] { YoAppearance.Red(), YoAppearance.OrangeRed(), YoAppearance.Yellow(), YoAppearance.Green(), YoAppearance.Blue(), YoAppearance.Indigo(), YoAppearance.Purple()}; return rainbow; }
private TerrainObject3D createBollard(int row, int column, Point2D positionInGrid, double height, double radius) { AppearanceDefinition bollardAppearance = YoAppearance.Yellow(); RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(getWorldCoordinate(row, column)); location.appendTranslation(positionInGrid.getX(), positionInGrid.getY(), height / 2); return new CylinderTerrainObject(location, height, radius, bollardAppearance); }
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 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 createWheelGraphics(FrameTorus3d wheelTorus) { linkGraphics.addCone(0.05, 0.05); linkGraphics.addArcTorus(FastMath.PI / 3, 2 * FastMath.PI / 3, wheelTorus.getRadius(), wheelTorus.getThickness(), YoAppearance.Yellow()); wheelGraphic = linkGraphics.addArcTorus(2 * FastMath.PI / 3, 7 * FastMath.PI / 3, wheelTorus.getRadius(), wheelTorus.getThickness(), YoAppearance.Black()); }
public TwoBollardEnvironment(double barrelSeparation) { // ground plane generator.addRectangle(1.0, barrelSeparation + 0.5); generator.translate(2.0, 0.0, 0.0); generator.addRectangle(3.0, 3.0); generator.translate(-4.0, 0.0, 0.0); generator.addRectangle(3.0, 3.0); addPlanarRegionsToTerrain(YoAppearance.RGBColor(110 / 256.0, 121 / 256.0, 121 / 256.0)); // first bollard generator.identity(); generator.translate(0.0, 0.5 * barrelSeparation, 0.0); generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight); addPlanarRegionsToTerrain(YoAppearance.Yellow()); // second bollard generator.identity(); generator.translate(0.0, -0.5 * barrelSeparation, 0.0); generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight); addPlanarRegionsToTerrain(YoAppearance.Yellow()); }
public FloatingObjectBoxRobot(String imageResourcePath) { super(FloatingObjectBoxRobot.class.getSimpleName()); qrCodeJoint = new FloatingJoint("object", "object", new Vector3D(), this); Link qrCodeLink = new Link("object"); qrCodeLink.setMassAndRadiiOfGyration(1.0, 0.1, 0.1, 0.1); Graphics3DObject qrCodeLinkGraphics = new Graphics3DObject(); // qrCodeLinkGraphics.addCoordinateSystem(2.0); double cubeLength = 1.0; qrCodeLinkGraphics.translate(0.0, 0.0, -0.99 * cubeLength); AppearanceDefinition cubeAppearance = YoAppearance.Texture(imageResourcePath); qrCodeLinkGraphics.addCube(cubeLength * 0.98, cubeLength * 1.01, cubeLength * 0.98, YoAppearance.Yellow()); boolean[] textureFaces = new boolean[] { true, true, false, false, false, false }; qrCodeLinkGraphics.translate(0.0, 0.0, -0.01 * cubeLength); qrCodeLinkGraphics.addCube(cubeLength, cubeLength, cubeLength, cubeAppearance, textureFaces); qrCodeLink.setLinkGraphics(qrCodeLinkGraphics); qrCodeJoint.setLink(qrCodeLink); addRootJoint(qrCodeJoint); setGravity(0.0); }
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 setupVisualizers(YoGraphicsListRegistry yoGraphicsListRegistry, boolean visualize) { YoGraphicsList yoGraphicsList = new YoGraphicsList(getClass().getSimpleName()); ArtifactList artifactList = new ArtifactList(getClass().getSimpleName()); YoGraphicPosition actualEndOfStateICP = new YoGraphicPosition("actualEndOfStateICP", this.actualEndOfStateICP, 0.005, YoAppearance.Aquamarine(), GraphicType.SOLID_BALL); YoGraphicPosition nominalReferenceICP = new YoGraphicPosition("nominalReferenceICP", this.nominalReferenceICP, 0.01, YoAppearance.LightYellow(), GraphicType.BALL); YoGraphicPosition nominalEndOfStateICP = new YoGraphicPosition("nominalEndOfStateICP", this.nominalEndOfStateICP, 0.01, YoAppearance.Green(), GraphicType.SOLID_BALL); yoGraphicsList.add(actualEndOfStateICP); yoGraphicsList.add(nominalReferenceICP); yoGraphicsList.add(nominalEndOfStateICP); artifactList.add(actualEndOfStateICP.createArtifact()); artifactList.add(nominalReferenceICP.createArtifact()); artifactList.add(nominalEndOfStateICP.createArtifact()); yoGraphicsList.setVisible(visualize); artifactList.setVisible(visualize); YoGraphicPosition referenceICP = new YoGraphicPosition("controllerReferenceICP", controllerReferenceICP, 0.005, YoAppearance.Yellow(), GraphicType.BALL_WITH_CROSS); YoGraphicPosition referenceCMP = new YoGraphicPosition("controllerReferenceCMP", controllerReferenceCMP, 0.005, YoAppearance.Beige(), GraphicType.BALL_WITH_CROSS); String name = "ICPOptimization"; yoGraphicsListRegistry.registerArtifact(name, referenceICP.createArtifact()); yoGraphicsListRegistry.registerArtifact(name, referenceCMP.createArtifact()); yoGraphicsListRegistry.registerYoGraphic(name, referenceICP); yoGraphicsListRegistry.registerYoGraphic(name, referenceCMP); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerArtifactList(artifactList); }
public FloatingFiducialBoxRobot(Fiducial fiducial) { super("FloatingFiducialBoxRobot"); qrCodeJoint = new FloatingJoint("qrCode", "qrCode", new Vector3D(), this); Link qrCodeLink = new Link("qrCode"); qrCodeLink.setMassAndRadiiOfGyration(1.0, 0.1, 0.1, 0.1); Graphics3DObject qrCodeLinkGraphics = new Graphics3DObject(); // qrCodeLinkGraphics.addCoordinateSystem(2.0); double cubeLength = 1.0; qrCodeLinkGraphics.translate(0.0, 0.0, -0.99 * cubeLength); AppearanceDefinition cubeAppearance = YoAppearance.Texture(fiducial.getPathString()); qrCodeLinkGraphics.addCube(cubeLength * 0.98, cubeLength * 1.01, cubeLength * 0.98, YoAppearance.Yellow()); boolean[] textureFaces = new boolean[] { true, true, false, false, false, false }; qrCodeLinkGraphics.translate(0.0, 0.0, -0.01 * cubeLength); qrCodeLinkGraphics.addCube(cubeLength, cubeLength, cubeLength, cubeAppearance, textureFaces); qrCodeLink.setLinkGraphics(qrCodeLinkGraphics); qrCodeJoint.setLink(qrCodeLink); addRootJoint(qrCodeJoint); setGravity(0.0); }
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 ProvidedMassMatrixToolRigidBody(RobotSide robotSide, final FullHumanoidRobotModel fullRobotModel, double gravity, ArmControllerParameters armControllerParameters, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { String name = robotSide.getCamelCaseNameForStartOfExpression() + "Tool"; this.registry = new YoVariableRegistry(name); this.fullRobotModel = fullRobotModel; this.gravity = gravity; this.handFixedFrame = fullRobotModel.getHand(robotSide).getBodyFixedFrame(); this.handControlFrame = fullRobotModel.getHandControlFrame(robotSide); this.elevatorFrame = fullRobotModel.getElevatorFrame(); toolFrame = new PoseReferenceFrame(name + "Frame", elevatorFrame); RigidBodyInertia inertia = new RigidBodyInertia(toolFrame, new Matrix3d(), 0.0); this.toolJoint = new SixDoFJoint(name + "Joint", fullRobotModel.getElevator(), fullRobotModel.getElevator().getBodyFixedFrame()); this.toolBody = new RigidBody(name + "Body", inertia, toolJoint); objectCenterOfMass = new YoFramePoint(name + "CoMOffset", handControlFrame, registry); objectMass = new DoubleYoVariable(name + "ObjectMass", registry); objectForceInWorld = new YoFrameVector(name + "Force", ReferenceFrame.getWorldFrame(), registry); this.objectCenterOfMassInWorld = new YoFramePoint(name + "CoMInWorld", ReferenceFrame.getWorldFrame(), registry); if (yoGraphicsListRegistry != null) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); YoGraphic comViz = new YoGraphicPosition(name + "CenterOfMassViz", objectCenterOfMassInWorld, 0.05, YoAppearance.Red()); yoGraphicsList.add(comViz); YoGraphic vectorViz = new YoGraphicVector(name + "ForceViz", objectCenterOfMassInWorld, objectForceInWorld, YoAppearance.Yellow()); yoGraphicsList.add(vectorViz); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); } parentRegistry.addChild(registry); }
yoHandControlFramePose = new YoFramePoseUsingYawPitchRoll("handControlFrame",HumanoidReferenceFrames.getWorldFrame(), registry); yoHandControlFramePose.set(handControlFramePose); YoGraphicCoordinateSystem yoToolTip = new YoGraphicCoordinateSystem("toolTipViz", yoHandControlFramePose, 0.1, YoAppearance.Yellow());