private CombinedTerrainObject3D setUpGround(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); combinedTerrainObject.addBox(-5.0, -5.0, 5.0, 5.0, -0.05, 0.0, YoAppearance.Blue()); return combinedTerrainObject; }
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"); } }
/** * Create a Bag of Balls with alternating ball color going through Red, White, and Blue. * * @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 createPatrioticBag(int numberOfBalls, double sizeInMeters, String name, YoVariableRegistry parentYoVariableRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { AppearanceDefinition[] redWhiteBlue = new AppearanceDefinition[] { YoAppearance.Red(), YoAppearance.White(), YoAppearance.Blue() }; ArrayList<AppearanceDefinition> appearances = new ArrayList<AppearanceDefinition>(); for (int i = 0; i < numberOfBalls; i++) { appearances.add(redWhiteBlue[i % redWhiteBlue.length]); } return new BagOfBalls(sizeInMeters, name, appearances, parentYoVariableRegistry, yoGraphicsListRegistry); }
public void addCoordinateSystem(double length, AppearanceDefinition arrowAppearance) { addCoordinateSystem(length, YoAppearance.Red(), YoAppearance.White(), YoAppearance.Blue(), arrowAppearance); }
public void addCoordinateSystem(double length, AppearanceDefinition arrowAppearance) { addCoordinateSystem(length, YoAppearance.Red(), YoAppearance.White(), YoAppearance.Blue(), arrowAppearance); }
/** * This is where the end-effectors needing a visualization are registered, if you need more, add * it there. * * @param rigidBodies all the rigid bodies for which the desired and actual pose will be * displayed using graphical coordinate systems. */ public void setupVisualization(RigidBodyBasics... rigidBodies) { AppearanceDefinition desiredAppearance = YoAppearance.Red(); AppearanceDefinition currentAppearance = YoAppearance.Blue(); for (RigidBodyBasics rigidBody : rigidBodies) { YoGraphicCoordinateSystem desiredCoodinateSystem = createCoodinateSystem(rigidBody, Type.DESIRED, desiredAppearance); YoGraphicCoordinateSystem currentCoodinateSystem = createCoodinateSystem(rigidBody, Type.CURRENT, currentAppearance); desiredCoodinateSystems.put(rigidBody, desiredCoodinateSystem); currentCoodinateSystems.put(rigidBody, currentCoodinateSystem); yoGraphicsListRegistry.registerYoGraphic("CoordinateSystems", desiredCoodinateSystem); yoGraphicsListRegistry.registerYoGraphic("CoordinateSystems", currentCoodinateSystem); } }
public SmartCMPProjector(YoGraphicsListRegistry graphicsListRegistry, YoVariableRegistry parentRegistry) { activeProjection.set(ProjectionMethod.NONE); if (parentRegistry != null) parentRegistry.addChild(registry); if (graphicsListRegistry != null) { YoArtifactPosition desiredCMPViz = new YoArtifactPosition("Desired CMP Position", yoDesiredCMP, GraphicType.SOLID_BALL, DarkRed().getAwtColor(), 0.008); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), desiredCMPViz); YoArtifactPosition projectedCMPViz = new YoArtifactPosition("Projected CMP Position", yoProjectedCMP, GraphicType.BALL_WITH_ROTATED_CROSS, DarkRed().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), projectedCMPViz); YoArtifactPolygon projectionAreaViz = new YoArtifactPolygon("CMP Projection Area", yoProjectionArea, Blue().getAwtColor(), false); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), projectionAreaViz); } }
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; }
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); }
private static Graphics3DObject createEndEffectorKeyFrameVisualization(Pose3D pose) { Graphics3DObject object = new Graphics3DObject(); object.transform(new RigidBodyTransform(pose.getOrientation(), pose.getPosition())); object.addSphere(0.01); object.addCylinder(0.1, 0.005, YoAppearance.Blue()); return object; } }
private static Link pelvis() { AppearanceDefinition pelvisAppearance = YoAppearance.Blue(); Link ret = new Link("pelvis"); ret.setMass(100.0); ret.setMomentOfInertia(1.0, 1.0, 1.0); ret.setComOffset(0.0, 0.0, 0.0); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(1.0); linkGraphics.addCylinder(PELVIS_HEIGHT, PELVIS_RAD, pelvisAppearance); ret.setLinkGraphics(linkGraphics); return ret; }
private void visualizeNormalVector(Plane3D plane) { this.linkGraphics.identity(); linkGraphics.translate(new Vector3D(plane.getPointCopy())); linkGraphics.addSphere(0.005, YoAppearance.Black()); Vector3D normalCopy = plane.getNormalCopy(); normalCopy.scale(0.01); linkGraphics.translate(normalCopy); linkGraphics.addSphere(0.005, YoAppearance.Blue()); }
/** * Creates a graphical representation of the x, y, and z axis of the current coordinate * system centered at its origin. In the image below red, white and blue represent the * x, y and z axies respectively.<br /><br /> * <img src="doc-files/LinkGraphics.addCoordinateSystem.jpg"> * * @param length the length in meters of each axis arrow. */ public void addCoordinateSystem(double length, AppearanceDefinition xAxisAppearance, AppearanceDefinition yAxisAppearance, AppearanceDefinition zAxisAppearance, AppearanceDefinition arrowAppearance) { rotate(Math.PI / 2.0, Axis.Y); addArrow(length, YoAppearance.Red(), arrowAppearance); rotate(-Math.PI / 2.0, Axis.Y); rotate(-Math.PI / 2.0, Axis.X); addArrow(length, YoAppearance.White(), arrowAppearance); rotate(Math.PI / 2.0, Axis.X); addArrow(length, YoAppearance.Blue(), arrowAppearance); }
/** * Creates a graphical representation of the x, y, and z axis of the current coordinate * system centered at its origin. In the image below red, white and blue represent the * x, y and z axies respectively.<br /><br /> * <img src="doc-files/LinkGraphics.addCoordinateSystem.jpg"> * * @param length the length in meters of each axis arrow. */ public void addCoordinateSystem(double length, AppearanceDefinition xAxisAppearance, AppearanceDefinition yAxisAppearance, AppearanceDefinition zAxisAppearance, AppearanceDefinition arrowAppearance) { rotate(Math.PI / 2.0, Axis.Y); addArrow(length, YoAppearance.Red(), arrowAppearance); rotate(-Math.PI / 2.0, Axis.Y); rotate(-Math.PI / 2.0, Axis.X); addArrow(length, YoAppearance.White(), arrowAppearance); rotate(Math.PI / 2.0, Axis.X); addArrow(length, YoAppearance.Blue(), arrowAppearance); }
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 QuadTreeTestHelper testOnAStaircase(Point3D center, Vector3D normal, double halfWidth, double resolution, double stairSeparation, double oneStairLandingHeight, boolean visualize) { normal.normalize(); BoundingBox2D boundingBox = new BoundingBox2D(center.getX() - halfWidth, center.getY() - halfWidth, center.getX() + halfWidth, center.getY() + halfWidth); Plane3D plane3d = new Plane3D(center, normal); ArrayList<Point3D> points = generatePointsForStairs(plane3d, halfWidth, resolution, stairSeparation, oneStairLandingHeight); // Collections.shuffle(points); int pointsPerBallUpdate = 1; QuadTreeTestHelper testHelper = testOnAListOfPoints(points, pointsPerBallUpdate, boundingBox, resolution, visualize); if (visualize) { testHelper.drawPoints(points, resolution/2.0, YoAppearance.Blue()); ArrayList<Point3D> allPointsInQuadTree = testHelper.getAllPointsInQuadTree(); testHelper. drawPoints(allPointsInQuadTree, resolution*0.6, YoAppearance.Chartreuse()); } // testHelper. drawPointsWithinAreaInSCS(pointsPerBallUpdate, YoAppearance.Chartreuse()); // testHelper.drawHeightOfOriginalPointsInPurple(points, pointsPerBallUpdate); // testHelper.drawHeightMap(boundingBox, resolution); return testHelper; }
private static Link link12(Link link11, PinJoint pin1) { Link ret = new Link("link12"); Vector3D comOffset12 = new Vector3D(link11.getComOffset()); Vector3D offset = new Vector3D(); pin1.getOffset(offset); comOffset12.sub(offset); ret.setComOffset(comOffset12); ret.setMass(link11.getMass()); Matrix3D link1moi = new Matrix3D(); link11.getMomentOfInertia(link1moi); ret.setMomentOfInertia(link1moi); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(COORDINATE_SYSTEM_LENGTH); createInertiaEllipsoid(ret, linkGraphics, YoAppearance.Blue()); ret.setLinkGraphics(linkGraphics); return ret; }
private CombinedTerrainObject3D setUpVessel(String name, Point2D doorLocation) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); combinedTerrainObject.addSphere(doorLocation.getX(), doorLocation.getY(), vesselCartWheelRadius + vesselCartBodyHeight + vesselRadius, vesselRadius, YoAppearance.Blue()); combinedTerrainObject.addBox(doorLocation.getX() - vesselCartLength / 2, doorLocation.getY() - vesselCartLength / 2, doorLocation.getX() + vesselCartLength / 2, doorLocation.getY() + vesselCartLength / 2, vesselCartWheelRadius, vesselCartWheelRadius + vesselCartBodyHeight, YoAppearance.Gray()); RigidBodyTransform frontWheel = new RigidBodyTransform(); RigidBodyTransform rearWheel = new RigidBodyTransform(); frontWheel.setTranslation(doorLocation.getX() + vesselCartLength * 0.25, doorLocation.getY(), vesselCartWheelRadius); rearWheel.setTranslation(doorLocation.getX() - vesselCartLength * 0.25, doorLocation.getY(), vesselCartWheelRadius); frontWheel.appendRollRotation(Math.PI / 2); rearWheel.appendRollRotation(Math.PI / 2); double offset = 0.05; combinedTerrainObject.addCylinder(frontWheel, vesselCartLength - offset, vesselCartWheelRadius, YoAppearance.DarkGray()); combinedTerrainObject.addCylinder(rearWheel, vesselCartLength - offset, vesselCartWheelRadius, YoAppearance.DarkGray()); return combinedTerrainObject; }