static YoGraphicReferenceFrame createAsRemoteYoGraphic(String name, YoVariable<?>[] yoVariables, double[] constants) { return new YoGraphicReferenceFrame(name, yoVariables, constants); }
private void setupDynamicGraphicObjects(YoGraphicsListRegistry yoGraphicsListRegistry, List<? extends IMUSensorReadOnly> imuProcessedOutputs) { for (int i = 0; i < imuProcessedOutputs.size(); i++) { YoGraphicReferenceFrame dynamicGraphicMeasurementFrame = new YoGraphicReferenceFrame(imuProcessedOutputs.get(i).getMeasurementFrame(), registry, 1.0); dynamicGraphicMeasurementFrames.add(dynamicGraphicMeasurementFrame); } yoGraphicsListRegistry.registerYoGraphics("imuFrame", dynamicGraphicMeasurementFrames); }
public VisualizeFramesController(ArrayList<ReferenceFrame> referenceFrames, YoGraphicsListRegistry yoGraphicsListRegistry, double coordinateSystemLength) { for (ReferenceFrame frame : referenceFrames) { YoGraphicReferenceFrame dynamicGraphicReferenceFrame = new YoGraphicReferenceFrame(frame, registry, coordinateSystemLength); dynamicGraphicReferenceFrames.add(dynamicGraphicReferenceFrame); yoGraphicsList.add(dynamicGraphicReferenceFrame); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
public CommonHumanoidReferenceFramesVisualizer(CommonHumanoidReferenceFrames referenceFrames, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { String vizName = referenceFrames.getClass().getSimpleName(); for (RobotSide robotSide : RobotSide.values) { YoGraphicReferenceFrame yoGraphic = new YoGraphicReferenceFrame(referenceFrames.getAnkleZUpFrame(robotSide), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, yoGraphic); referenceFramesVisualizers.add(yoGraphic); } YoGraphicReferenceFrame midFeetFrame = new YoGraphicReferenceFrame(referenceFrames.getMidFeetZUpFrame(), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, midFeetFrame); referenceFramesVisualizers.add(midFeetFrame); YoGraphicReferenceFrame comFrame = new YoGraphicReferenceFrame(referenceFrames.getCenterOfMassFrame(), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, comFrame); referenceFramesVisualizers.add(comFrame); YoGraphicReferenceFrame pelvisFrame = new YoGraphicReferenceFrame(referenceFrames.getPelvisFrame(), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, pelvisFrame); referenceFramesVisualizers.add(pelvisFrame); parentRegistry.addChild(registry); }
public InverseDynamicsMechanismReferenceFrameVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry, double length) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints()); while (!jointStack.isEmpty()) { JointBasics joint = jointStack.get(0); ReferenceFrame referenceFrame = joint.getSuccessor().getBodyFixedFrame(); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame, registry, false, length); yoGraphicsList.add(yoGraphicReferenceFrame); yoGraphicReferenceFrames.add(yoGraphicReferenceFrame); List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints(); jointStack.addAll(childrenJoints); jointStack.remove(joint); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
cameraGraphic = new YoGraphicReferenceFrame(cameraReferenceFrame, registry, true, 0.5); detectorGraphic = new YoGraphicReferenceFrame(detectorReferenceFrame, registry, true, 1.0); locatedFiducialGraphic = new YoGraphicReferenceFrame(locatedFiducialReferenceFrame, registry, true, 0.1); reportedFiducialGraphic = new YoGraphicReferenceFrame(reportedFiducialReferenceFrame, registry, true, 0.2);
private void createFrameVisualizers(YoGraphicsListRegistry yoGraphicsListRegistry, FullHumanoidRobotModel fullRobotModel, String listName, boolean enable) { YoGraphicsList list = new YoGraphicsList(listName); if (yoGraphicsListRegistry != null) { for (RobotSide robotSide : RobotSide.values) { ReferenceFrame handPositionControlFrame = fullRobotModel.getHandControlFrame(robotSide); if (handPositionControlFrame != null) { YoGraphicReferenceFrame dynamicGraphicReferenceFrame = new YoGraphicReferenceFrame(handPositionControlFrame, registry, 0.1); dynamicGraphicReferenceFrames.add(dynamicGraphicReferenceFrame); list.add(dynamicGraphicReferenceFrame); } } yoGraphicsListRegistry.registerYoGraphicsList(list); if (!enable) list.hideYoGraphics(); } }
cameraGraphic = new YoGraphicReferenceFrame(cameraReferenceFrame, registry, 0.5); detectorGraphic = new YoGraphicReferenceFrame(detectorReferenceFrame, registry, 1.0); locatedFiducialGraphic = new YoGraphicReferenceFrame(locatedFiducialReferenceFrame, registry, 0.1); reportedFiducialGraphic = new YoGraphicReferenceFrame(reportedFiducialReferenceFrame, registry, 0.2);
public JointAxisVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry, double length) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints()); while (!jointStack.isEmpty()) { JointBasics joint = jointStack.get(0); if(joint instanceof OneDoFJointBasics) { FrameVector3DReadOnly jAxis=((OneDoFJointBasics)joint).getJointAxis(); ReferenceFrame referenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(joint.getName()+"JointAxis", new FramePoint3D(jAxis.getReferenceFrame()), new FrameVector3D(jAxis.getReferenceFrame(),jAxis)); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame , registry, false, length, YoAppearance.Gold()); yoGraphicsList.add(yoGraphicReferenceFrame); yoGraphicReferenceFrames.add(yoGraphicReferenceFrame); } List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints(); jointStack.addAll(childrenJoints); jointStack.remove(joint); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
cameraGraphic = new YoGraphicReferenceFrame(cameraReferenceFrame, registry, true, 0.5); detectorGraphic = new YoGraphicReferenceFrame(detectorReferenceFrame, registry, true, 1.0); locatedFiducialGraphic = new YoGraphicReferenceFrame(locatedFiducialReferenceFrame, registry, true, 0.1); reportedFiducialGraphic = new YoGraphicReferenceFrame(reportedFiducialReferenceFrame, registry, true, 0.2);
cameraGraphic = new YoGraphicReferenceFrame(cameraReferenceFrame, registry, 0.5); detectorGraphic = new YoGraphicReferenceFrame(detectorReferenceFrame, registry, 1.0); locatedFiducialGraphic = new YoGraphicReferenceFrame(locatedFiducialReferenceFrame, registry, 0.1); reportedFiducialGraphic = new YoGraphicReferenceFrame(reportedFiducialReferenceFrame, registry, 0.2);
public VehicleModelObjectVisualizer(ReferenceFrame vehicleFrame, VehicleModelObjects vehicleModelObjects, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { yoGraphicsList = new YoGraphicsList("vehicleObjects"); for (VehicleObject vehicleObject : VehicleObject.values()) { FramePose3D framePose = vehicleModelObjects.getFramePose(vehicleFrame, vehicleObject); String objectName = FormattingTools.underscoredToCamelCase(vehicleObject.toString(), false); ReferenceFrame objectFrame = new PoseReferenceFrame(objectName, framePose); objectFrame.update(); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(objectFrame, registry, true, objectFrameScale); yoGraphicsList.add(yoGraphicReferenceFrame); } YoGraphicReferenceFrame vehicleFrameViz = new YoGraphicReferenceFrame(vehicleFrame, registry, true, vehicleFrameScale); yoGraphicsList.add(vehicleFrameViz); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); parentRegistry.addChild(registry); }
public VehicleModelObjectVisualizer(ReferenceFrame vehicleFrame, VehicleModelObjects vehicleModelObjects, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { yoGraphicsList = new YoGraphicsList("vehicleObjects"); for (VehicleObject vehicleObject : VehicleObject.values()) { FramePose framePose = vehicleModelObjects.getFramePose(vehicleFrame, vehicleObject); String objectName = FormattingTools.underscoredToCamelCase(vehicleObject.toString(), false); ReferenceFrame objectFrame = new PoseReferenceFrame(objectName, framePose); objectFrame.update(); YoGraphicReferenceFrame dynamicGraphicReferenceFrame = new YoGraphicReferenceFrame(objectFrame, registry, objectFrameScale); yoGraphicsList.add(dynamicGraphicReferenceFrame); } YoGraphicReferenceFrame vehicleFrameViz = new YoGraphicReferenceFrame(vehicleFrame, registry, vehicleFrameScale); yoGraphicsList.add(vehicleFrameViz); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); parentRegistry.addChild(registry); }
topFrameViz = new YoGraphicReferenceFrame(topFrame, registry, false, frameScale); afterTopJointFrameViz = new YoGraphicReferenceFrame(afterTopJointFrame, registry, false, frameScale * 0.8); beforeBottomJointFrameViz = new YoGraphicReferenceFrame(beforeBottomJointFrame, registry, false, frameScale * 0.6); bottomFrameViz = new YoGraphicReferenceFrame(bottomFrame, registry, false, frameScale * 0.4); actuator5SlideFrameViz = new YoGraphicReferenceFrame(actuator5SlideFrame, registry, false, frameScale * 0.6); actuator6SlideFrameViz = new YoGraphicReferenceFrame(actuator6SlideFrame, registry, false, frameScale * 0.6);
YoGraphicReferenceFrame worldViz = new YoGraphicReferenceFrame(worldFrame, registry, false, 0.5); graphicsListRegistry.registerYoGraphic(listName, worldViz);
frameViz = new YoGraphicReferenceFrame(controlFrame, registry, 0.2); if (graphicsListRegistry != null) graphicsListRegistry.registerYoGraphic(prefix + getClass().getSimpleName(), frameViz);
swingLeg.set(RobotQuadrant.FRONT_RIGHT); leftMidZUpFrameViz = new YoGraphicReferenceFrame(referenceFrames.getSideDependentMidFeetZUpFrame(RobotSide.LEFT), registry, 0.2); rightMidZUpFrameViz = new YoGraphicReferenceFrame(referenceFrames.getSideDependentMidFeetZUpFrame(RobotSide.RIGHT), registry, 0.2);
virtualLegTangentialFrameHipCenteredGraphics = new YoGraphicReferenceFrame(virtualLegTangentialFrameHipCentered, registry, 0.1); virtualLegTangentialFrameAnkleCenteredGraphics = new YoGraphicReferenceFrame(virtualLegTangentialFrameAnkleCentered, registry, 0.1); yoGraphicsListRegistry.registerYoGraphic("SingularityCollapseAvoidance", virtualLegTangentialFrameHipCenteredGraphics); yoGraphicsListRegistry.registerYoGraphic("SingularityCollapseAvoidance", virtualLegTangentialFrameAnkleCenteredGraphics);