static YoGraphicCoordinateSystem createAsRemoteYoGraphic(String name, YoVariable<?>[] yoVariables, double[] constants) { return new YoGraphicCoordinateSystem(name, yoVariables, constants); }
/** * Convenience method that should only be used for setting up the visualization. * <p> * Simply creates a graphic coordinate system visualizable in {@code SCSVisualizer}. * </p> * * @param endEffector used to create a name prefix required for creating a * {@link YoGraphicCoordinateSystem}. * @param type used to create a name prefix required for creating a * {@link YoGraphicCoordinateSystem}. * @param appearanceDefinition the appearance of the coordinate system's arrows. * @return the graphic with a good name for the given end-effector. */ private YoGraphicCoordinateSystem createCoodinateSystem(RigidBodyBasics endEffector, Type type, AppearanceDefinition appearanceDefinition) { String namePrefix = endEffector.getName() + type.getName(); return new YoGraphicCoordinateSystem(namePrefix, "", registry, false, 0.2, appearanceDefinition); }
@Override public YoGraphicCoordinateSystem duplicate(YoVariableRegistry newRegistry) { if (isUsingYawPitchRoll()) return new YoGraphicCoordinateSystem(getName(), position.duplicate(newRegistry), yawPitchRoll.duplicate(newRegistry), scale, arrowColor); else return new YoGraphicCoordinateSystem(getName(), position.duplicate(newRegistry), quaternion.duplicate(newRegistry), scale, arrowColor); }
private void setupVisualization(String namePrefix, YoGraphicsListRegistry yoGraphicsListRegistry) { YoGraphicsList list = new YoGraphicsList(name); YoGraphicCoordinateSystem desiredPoseViz = new YoGraphicCoordinateSystem(namePrefix + "DesiredPose", yoDesiredPose, 0.3); list.add(desiredPoseViz); yoGraphicsListRegistry.registerYoGraphicsList(list); list.hideYoGraphics(); }
public FootstepVisualizer(String name, String graphicListName, RobotSide robotSide, ContactablePlaneBody contactableFoot, AppearanceDefinition footstepColor, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry registry) { this.robotSide = robotSide; yoFootstepPose = new YoFramePose(name + "Pose", worldFrame, registry); yoFoothold = new YoFrameConvexPolygon2d(name + "Foothold", "", worldFrame, maxNumberOfContactPoints, registry); double coordinateSystemSize = 0.2; double footholdScale = 1.0; poseViz = new YoGraphicCoordinateSystem(name + "Pose", yoFootstepPose, coordinateSystemSize, footstepColor); footholdViz = new YoGraphicPolygon(name + "Foothold", yoFoothold, yoFootstepPose, footholdScale, footstepColor); yoGraphicsListRegistry.registerYoGraphic(graphicListName, poseViz); yoGraphicsListRegistry.registerYoGraphic(graphicListName, footholdViz); List<FramePoint2d> contactPoints2d = contactableFoot.getContactPoints2d(); for (int i = 0; i < contactPoints2d.size(); i++) defaultContactPointsInSoleFrame.add(contactPoints2d.get(i).getPointCopy()); }
private YoGraphicsListRegistry createStartAndGoalGraphics(FramePose3D initialStancePose, FramePose3D goalPose) { YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); YoGraphicsList graphicsList = new YoGraphicsList("testViz"); YoFramePoseUsingYawPitchRoll yoInitialStancePose = new YoFramePoseUsingYawPitchRoll("initialStancePose", initialStancePose.getReferenceFrame(), drcSimulationTestHelper.getYoVariableRegistry()); yoInitialStancePose.set(initialStancePose); YoFramePoseUsingYawPitchRoll yoGoalPose = new YoFramePoseUsingYawPitchRoll("goalStancePose", goalPose.getReferenceFrame(), drcSimulationTestHelper.getYoVariableRegistry()); yoGoalPose.set(goalPose); YoGraphicCoordinateSystem startPoseGraphics = new YoGraphicCoordinateSystem("startPose", yoInitialStancePose, 13.0); YoGraphicCoordinateSystem goalPoseGraphics = new YoGraphicCoordinateSystem("goalPose", yoGoalPose, 13.0); graphicsList.add(startPoseGraphics); graphicsList.add(goalPoseGraphics); return graphicsListRegistry; }
YoGraphicCoordinateSystem desiredHandPoseViz = new YoGraphicCoordinateSystem(sidePrefix + "DesiredHandPose", "", registry, 0.2); desiredHandPosesViz.put(robotSide, desiredHandPoseViz); YoGraphicCoordinateSystem desiredFootPoseViz = new YoGraphicCoordinateSystem(sidePrefix + "DesiredFootPose", "", registry, 0.2); desiredFootPosesViz.put(robotSide, desiredFootPoseViz); yoGraphicsListRegistry.registerYoGraphic("DesiredCoords", desiredHandPoseViz);
rightKneeCoMInZUpFrame = new YoFramePoint("rightKneeCoMInZUpFrame", rightKneeCenterOfMassCalculator.getDesiredFrame(), registry); spinePitchZUpFrameViz = new YoGraphicCoordinateSystem("spinePitchZUpFrameViz", "", registry, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", spinePitchZUpFrameViz); leftHipPitchZUpFrameViz = new YoGraphicCoordinateSystem("leftHipPitchZUpFrameViz", "", registry, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchZUpFrameViz); leftHipPitchFrameViz = new YoGraphicCoordinateSystem("leftHipPitchFrameViz", "", registry, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchFrameViz);
rightKneeCoMInZUpFrame = new YoFramePoint3D("rightKneeCoMInZUpFrame", rightKneeCenterOfMassCalculator.getReferenceFrame(), registry); spinePitchZUpFrameViz = new YoGraphicCoordinateSystem("spinePitchZUpFrameViz", "", registry, true, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", spinePitchZUpFrameViz); leftHipPitchZUpFrameViz = new YoGraphicCoordinateSystem("leftHipPitchZUpFrameViz", "", registry, true, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchZUpFrameViz); leftHipPitchFrameViz = new YoGraphicCoordinateSystem("leftHipPitchFrameViz", "", registry, true, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchFrameViz);
endeffectorFrame.put(robotSide, new YoGraphicCoordinateSystem("" + robotSide + "endeffectorPoseFrame", endeffectorPose.get(robotSide), 0.25)); endeffectorFrame.get(robotSide).setVisible(true); yoGraphicsListRegistry.registerYoGraphic("" + robotSide + "endeffectorPoseViz", endeffectorFrame.get(robotSide)); testFrameViz = new YoGraphicCoordinateSystem("testFrameViz", testFramePose, 0.25); yoGraphicsListRegistry.registerYoGraphic("testFrameYoGraphic", testFrameViz);
final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicCoordinateSystem initialPoseViz = new YoGraphicCoordinateSystem(namePrefix + "InitialPose", 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");
return new YoGraphicCoordinateSystem(name, (DoubleYoVariable) vars[0], (DoubleYoVariable) vars[1], (DoubleYoVariable) vars[2], (DoubleYoVariable) vars[3], (DoubleYoVariable) vars[4], (DoubleYoVariable) vars[5], consts[0]);
YoGraphicCoordinateSystem yoCorrectedPelvisPoseInWorldFrameGraphic = new YoGraphicCoordinateSystem("yoCorrectedPelvisPoseInWorldFrameGraphic", yoCorrectedPelvisPoseInWorldFrame, 0.5, YoAppearance.Yellow()); yoGraphicsListRegistry.registerYoGraphic("yoCorrectedPelvisPoseInWorldFrame", yoCorrectedPelvisPoseInWorldFrameGraphic); YoGraphicCoordinateSystem yoIterativeClosestPointPoseInWorldFrameGraphic = new YoGraphicCoordinateSystem("yoIterativeClosestPointPoseInWorldFrameGraphic", yoIterativeClosestPointPoseInWorldFrame, 0.5, YoAppearance.Red()); yoGraphicsListRegistry.registerYoGraphic("yoIterativeClosestPointPoseInWorldFrameGraphic", yoIterativeClosestPointPoseInWorldFrameGraphic); YoGraphicCoordinateSystem yoStateEstimatorInWorldFramePoseGraphic = new YoGraphicCoordinateSystem("yoStateEstimatorInWorldFramePoseGraphic", yoStateEstimatorInWorldFramePose, 0.5, YoAppearance.Gray()); yoGraphicsListRegistry.registerYoGraphic("yoCorrectedPelvisPoseInWorldFrame", yoStateEstimatorInWorldFramePoseGraphic);
viz.update(); YoGraphicCoordinateSystem coordinateSystem = new YoGraphicCoordinateSystem("coord", new YoFramePoseUsingYawPitchRoll("World", ReferenceFrame.getWorldFrame(), null), 1);
viz.update(); YoGraphicCoordinateSystem coordinateSystem = new YoGraphicCoordinateSystem("coord", new YoFramePose("World", ReferenceFrame.getWorldFrame(), null), 1);
yoGraphicsListRegistry.registerYoGraphic(listName, new YoGraphicPosition(sidePrefix + "WristViz", wristPositions.get(robotSide), 0.05, appearance.get(robotSide))); YoGraphicCoordinateSystem footCoordinateSystem = new YoGraphicCoordinateSystem(sidePrefix + "Foot", "", registry, true, 0.25); yoGraphicsListRegistry.registerYoGraphic(listName, footCoordinateSystem); feetCoordinateSystems.put(robotSide, footCoordinateSystem); YoGraphicCoordinateSystem handCoordinateSystem = new YoGraphicCoordinateSystem(sidePrefix + "Hand", "", registry, true, 0.25); yoGraphicsListRegistry.registerYoGraphic(listName, handCoordinateSystem); handCoordinateSystems.put(robotSide, handCoordinateSystem);
yoGraphicsListRegistry.registerYoGraphic(listName, new YoGraphicPosition(sidePrefix + "WristViz", wristPositions.get(robotSide), 0.05, appearance.get(robotSide))); YoGraphicCoordinateSystem footCoordinateSystem = new YoGraphicCoordinateSystem(sidePrefix + "Foot", "", registry, 0.25); yoGraphicsListRegistry.registerYoGraphic(listName, footCoordinateSystem); feetCoordinateSystems.put(robotSide, footCoordinateSystem); YoGraphicCoordinateSystem handCoordinateSystem = new YoGraphicCoordinateSystem(sidePrefix + "Hand", "", registry, 0.25); yoGraphicsListRegistry.registerYoGraphic(listName, handCoordinateSystem); handCoordinateSystems.put(robotSide, handCoordinateSystem);
YoDouble roll = new YoDouble("roll", registry); YoGraphicCoordinateSystem worldCoordinateSystem = new YoGraphicCoordinateSystem("Example", worldOrigin, new YoFrameYawPitchRoll(yaw, pitch, roll, ReferenceFrame.getWorldFrame()), 2.0);
yoWristJointPose = new YoFramePose("wristJointPose", HumanoidReferenceFrames.getWorldFrame(), registry); yoWristJointPose.set(wristJointPose); YoGraphicCoordinateSystem yoWristCoordinateSystem = new YoGraphicCoordinateSystem("wristCoordinateSystemViz", yoWristJointPose, 0.1, YoAppearance.Red()); yoHandControlFramePose = new YoFramePose("handControlFrame",HumanoidReferenceFrames.getWorldFrame(), registry); yoHandControlFramePose.set(handControlFramePose); YoGraphicCoordinateSystem yoToolTip = new YoGraphicCoordinateSystem("toolTipViz", yoHandControlFramePose, 0.1, YoAppearance.Yellow());
yoWristJointPose = new YoFramePoseUsingYawPitchRoll("wristJointPose", HumanoidReferenceFrames.getWorldFrame(), registry); yoWristJointPose.set(wristJointPose); YoGraphicCoordinateSystem yoWristCoordinateSystem = new YoGraphicCoordinateSystem("wristCoordinateSystemViz", yoWristJointPose, 0.1, YoAppearance.Red()); yoHandControlFramePose = new YoFramePoseUsingYawPitchRoll("handControlFrame",HumanoidReferenceFrames.getWorldFrame(), registry); yoHandControlFramePose.set(handControlFramePose); YoGraphicCoordinateSystem yoToolTip = new YoGraphicCoordinateSystem("toolTipViz", yoHandControlFramePose, 0.1, YoAppearance.Yellow());