public YoGraphicPosition(String name, YoFramePointInMultipleFrames framePoint, double scale, AppearanceDefinition appearance, GraphicType type) { this(name, framePoint.buildUpdatedYoFramePointForVisualizationOnly(), scale, appearance, type); }
public YoGraphicPosition(String name, YoFramePointInMultipleFrames framePoint, double scale, AppearanceDefinition appearance) { this(name, framePoint.buildUpdatedYoFramePointForVisualizationOnly(), scale, appearance, GraphicType.BALL); }
private void setupVisualizers(YoGraphicsListRegistry yoGraphicsListRegistry) { YoGraphicsList yoGraphicsList = new YoGraphicsList(getClass().getSimpleName()); ArtifactList artifactList = new ArtifactList(getClass().getSimpleName()); referenceCMPsCalculator.createVisualizerForConstantCMPs(yoGraphicsList, artifactList); for (int i = 0; i < numberFootstepsToConsider.getIntegerValue() - 1; i++) { YoGraphicPosition icpEarlyCornerPointsViz = new YoGraphicPosition("EntryCornerPoints" + i, entryCornerPoints.get(i).buildUpdatedYoFramePointForVisualizationOnly(), ICP_CORNER_POINT_SIZE, YoAppearance.Blue(), GraphicType.SOLID_BALL); yoGraphicsList.add(icpEarlyCornerPointsViz); artifactList.add(icpEarlyCornerPointsViz.createArtifact()); YoGraphicPosition lateCornerPointsViz = new YoGraphicPosition("ExitCornerPoints" + i, exitCornerPoints.get(i).buildUpdatedYoFramePointForVisualizationOnly(), ICP_CORNER_POINT_SIZE, YoAppearance.Blue(), GraphicType.BALL); yoGraphicsList.add(lateCornerPointsViz); artifactList.add(lateCornerPointsViz.createArtifact()); } YoGraphicPosition singleSupportInitialICPViz = new YoGraphicPosition("singleSupportInitialICP", singleSupportInitialICP.buildUpdatedYoFramePointForVisualizationOnly(), 0.004, YoAppearance.Chocolate(), GraphicType.SOLID_BALL); yoGraphicsList.add(singleSupportInitialICPViz); artifactList.add(singleSupportInitialICPViz.createArtifact()); YoGraphicPosition singleSupportFinalICPViz = new YoGraphicPosition("singleSupportFinalICP", singleSupportFinalICP.buildUpdatedYoFramePointForVisualizationOnly(), 0.004, YoAppearance.Chocolate(), GraphicType.BALL); yoGraphicsList.add(singleSupportFinalICPViz); artifactList.add(singleSupportFinalICPViz.createArtifact()); icpSingleSupportTrajectoryGenerator.createVisualizers(yoGraphicsList, artifactList); artifactList.setVisible(VISUALIZE); yoGraphicsList.setVisible(VISUALIZE); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerArtifactList(artifactList); }
entryConstantCMP.setToNaN(); entryCMPs.add(entryConstantCMP); entryCMPsInWorldFrameReadOnly.add(entryConstantCMP.buildUpdatedYoFramePointForVisualizationOnly()); exitCMPsInWorldFrameReadOnly.add(exitConstantCMP.buildUpdatedYoFramePointForVisualizationOnly());
initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), initialOrientationForViz, 0.1); final YoGraphicCoordinateSystem finalPoseViz = new YoGraphicCoordinateSystem(namePrefix + "FinalPose", finalPosition.buildUpdatedYoFramePointForVisualizationOnly(), finalOrientationForViz, 0.1); final YoGraphicCoordinateSystem currentPoseViz = new YoGraphicCoordinateSystem(namePrefix + "CurrentPose", currentPosition.buildUpdatedYoFramePointForVisualizationOnly(), currentOrientationForViz, 0.25); yoGraphicsList = new YoGraphicsList(namePrefix + "StraightLineTrajectory"); yoGraphicsList.add(currentPositionViz);
final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicVector initialDirectionViz = new YoGraphicVector(namePrefix + "InitialDirection", initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), initialDirection.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.BlueViolet()); yoGraphicsList = new YoGraphicsList(namePrefix + "FinalApproachTraj");
final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicVector initialDirectionViz = new YoGraphicVector(namePrefix + "InitialDirection", initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), initialDirection.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.BlueViolet()); final YoGraphicVector finalDirectionViz = new YoGraphicVector(namePrefix + "FinalDirection", finalPosition.buildUpdatedYoFramePointForVisualizationOnly(), finalDirection.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.Red()); distortedPlanePose = new YoFramePose(namePrefix + "DistortedPlane", ReferenceFrame.getWorldFrame(), registry);
final YoGraphicPosition initialPositionViz = new YoGraphicPosition(namePrefix + "InitialPosition", initialPosition, 0.02, YoAppearance.BlueViolet()); final YoGraphicVector currentVelocityViz = new YoGraphicVector(namePrefix + "CurrentVelocity", currentPosition.buildUpdatedYoFramePointForVisualizationOnly(), currentVelocity.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.Chartreuse()); final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicVector currentAngularVelocityViz = new YoGraphicVector(namePrefix + "CurrentAngularVelocity", currentPosition.buildUpdatedYoFramePointForVisualizationOnly(), currentAngularVelocity.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.Green()); initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), interpolationFrameForViz, 0.3, YoAppearance.Black()); currentPosition.buildUpdatedYoFramePointForVisualizationOnly(), currentOrientationForViz, 0.3);
final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicVector finalDirectionViz = new YoGraphicVector(namePrefix + "FinalDirection", finalPosition.buildUpdatedYoFramePointForVisualizationOnly(), finalDirection.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.Red()); yoGraphicsList = new YoGraphicsList(namePrefix + "FinalApproachTraj");