public LogDataProcessorWrapper(SimulationConstructionSet scs) { scs.addYoVariableRegistry(logDataProcessorRegistry); scs.addScript(this); controllerTimerCount = (YoLong) scs.getVariable(DRCControllerThread.class.getSimpleName(), "controllerTimerCount"); haveFoundControllerTimerVariable = controllerTimerCount != null; if (!haveFoundControllerTimerVariable) { System.err.println("Could not find controller timer variable, running processors at log data rate"); } }
public LogDataProcessorWrapper(SimulationConstructionSet scs) { scs.addYoVariableRegistry(logDataProcessorRegistry); scs.addScript(this); controllerTimerCount = (LongYoVariable) scs.getVariable(DRCControllerThread.class.getSimpleName(), "controllerTimerCount"); haveFoundControllerTimerVariable = controllerTimerCount != null; if (!haveFoundControllerTimerVariable) { System.err.println("Could not find controller timer variable, running processors at log data rate"); } }
public LogDataProcessorWrapper(SimulationConstructionSet scs) { scs.addYoVariableRegistry(logDataProcessorRegistry); scs.addScript(this); controllerTimerCount = (LongYoVariable) scs.getVariable(DRCControllerThread.class.getSimpleName(), "controllerTimerCount"); haveFoundControllerTimerVariable = controllerTimerCount != null; if (!haveFoundControllerTimerVariable) { System.err.println("Could not find controller timer variable, running processors at log data rate"); } }
public void startVisualizer() { scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); scs.addYoVariableRegistry(registry); scs.setupGraphGroup("step times", new String[][] { {"t"} }); scs.startOnAThread(); scs.tickAndUpdate(); }
public void startVisualizer() { scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); scs.addYoVariableRegistry(registry); scs.setupGraphGroup("step times", new String[][] { {"t"} }); scs.startOnAThread(); scs.tickAndUpdate(); }
private void setupThreadDataSynchronizer() { if (scsInitialSetup.get().getRunMultiThreaded()) { threadDataSynchronizer = new ThreadDataSynchronizer(robotModel.get()); } else { YoVariableRegistry threadDataSynchronizerRegistry = new YoVariableRegistry("ThreadDataSynchronizerRegistry"); threadDataSynchronizer = new SingleThreadedThreadDataSynchronizer(simulationConstructionSet, robotModel.get(), threadDataSynchronizerRegistry); simulationConstructionSet.addYoVariableRegistry(threadDataSynchronizerRegistry); } }
private void setupThreadDataSynchronizer() { if (scsInitialSetup.get().getRunMultiThreaded()) { threadDataSynchronizer = new ThreadDataSynchronizer(robotModel.get()); } else { YoVariableRegistry threadDataSynchronizerRegistry = new YoVariableRegistry("ThreadDataSynchronizerRegistry"); threadDataSynchronizer = new SingleThreadedThreadDataSynchronizer(simulationConstructionSet, robotModel.get(), threadDataSynchronizerRegistry); simulationConstructionSet.addYoVariableRegistry(threadDataSynchronizerRegistry); } }
public PoseSequenceSelectorPanel(DRCRobotModel robotModel) { super(new GridLayout(1, 0)); registry = new YoVariableRegistry("PoseSequenceGUI"); fullRobotModel = robotModel.createFullRobotModel(); sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); sequence = new PlaybackPoseSequence(fullRobotModel); HumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullRobotModel); SDFPerfectSimulatedSensorReader reader = new SDFPerfectSimulatedSensorReader(sdfRobot, fullRobotModel, referenceFrames); ModularRobotController controller = new ModularRobotController("Reader"); controller.setRawSensorReader(reader); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.startOnAThread(); sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModel, null); String[] columnNames = new String[] { "#", "sy", "sp", "sr", "neck", "lhy", "lhr", "lhp", "lk", "lap", "lar", "rhy", "rhr", "rhp", "rk", "rap", "rar", "lsp", "lsr", "lep", "ler", "lwp", "lwr", "rsp", "rsr", "rep", "rer", "rwp", "rwr", "pause" }; tableModel = new DefaultTableModel(columnNames, 0); // new ScriptEditorTableModel(); table = new JTable(tableModel); tableInit(); }
public PoseSequenceSelectorPanel(DRCRobotModel robotModel) { super(new GridLayout(1, 0)); registry = new YoVariableRegistry("PoseSequenceGUI"); fullRobotModel = robotModel.createFullRobotModel(); sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); sequence = new PlaybackPoseSequence(fullRobotModel); HumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullRobotModel); SDFPerfectSimulatedSensorReader reader = new SDFPerfectSimulatedSensorReader(sdfRobot, fullRobotModel, referenceFrames); ModularRobotController controller = new ModularRobotController("Reader"); controller.setRawSensorReader(reader); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.startOnAThread(); sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModel, null); String[] columnNames = new String[] { "#", "sy", "sp", "sr", "neck", "lhy", "lhr", "lhp", "lk", "lap", "lar", "rhy", "rhr", "rhp", "rk", "rap", "rar", "lsp", "lsr", "lep", "ler", "lwp", "lwr", "rsp", "rsr", "rep", "rer", "rwp", "rwr", "pause" }; tableModel = new DefaultTableModel(columnNames, 0); // new ScriptEditorTableModel(); table = new JTable(tableModel); tableInit(); }
public PoseSequenceSelectorPanel(DRCRobotModel robotModel) { super(new GridLayout(1, 0)); registry = new YoVariableRegistry("PoseSequenceGUI"); fullRobotModel = robotModel.createFullRobotModel(); sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); sequence = new PlaybackPoseSequence(fullRobotModel); HumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullRobotModel); SDFPerfectSimulatedSensorReader reader = new SDFPerfectSimulatedSensorReader(sdfRobot, fullRobotModel, referenceFrames); ModularRobotController controller = new ModularRobotController("Reader"); controller.setRawSensorReader(reader); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.startOnAThread(); sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModel, null); String[] columnNames = new String[] { "#", "sy", "sp", "sr", "neck", "lhy", "lhr", "lhp", "lk", "lap", "lar", "rhy", "rhr", "rhp", "rk", "rap", "rar", "lsp", "lsr", "lep", "ler", "lwp", "lwr", "rsp", "rsr", "rep", "rer", "rwp", "rwr", "pause" }; tableModel = new DefaultTableModel(columnNames, 0); // new ScriptEditorTableModel(); table = new JTable(tableModel); tableInit(); }
public static PlanarRegionBipedalFootstepPlannerVisualizer createWithSimulationConstructionSet(double dtForViz, SideDependentList<ConvexPolygon2d> footPolygonsInSoleFrame) { YoVariableRegistry registry = new YoVariableRegistry(PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName()); YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); PlanarRegionBipedalFootstepPlannerVisualizer footstepPlannerVisualizer = new PlanarRegionBipedalFootstepPlannerVisualizer(10, footPolygonsInSoleFrame, registry, graphicsListRegistry); SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("Test")); footstepPlannerVisualizer.setTickAndUpdatable(scs); scs.changeBufferSize(32000); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(graphicsListRegistry); scs.setDT(dtForViz, 1); scs.setGroundVisible(false); scs.startOnAThread(); return footstepPlannerVisualizer; }
private void startSCS() { scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(graphicsListRegistry); scs.setPlaybackRealTimeRate(0.025); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(0.3); scs.addStaticLinkGraphics(linkGraphics); scs.setCameraFix(0.0, 0.0, 0.5); scs.setCameraPosition(-0.5, 0.0, 1.0); SimulationOverheadPlotterFactory simulationOverheadPlotterFactory = scs.createSimulationOverheadPlotterFactory(); simulationOverheadPlotterFactory.addYoGraphicsListRegistries(graphicsListRegistry); simulationOverheadPlotterFactory.createOverheadPlotter(); scs.startOnAThread(); }
public ReachabilitySphereMapCalculator(OneDoFJointBasics[] robotArmJoints, SimulationConstructionSet scs) { this.scs = scs; solver = new ReachabilityMapSolver(robotArmJoints, null, registry); FramePose3D gridFramePose = new FramePose3D(ReferenceFrame.getWorldFrame(), robotArmJoints[0].getFrameBeforeJoint().getTransformToWorldFrame()); gridFramePose.appendTranslation(getGridSizeInMeters() / 2.5, 0.0, 0.0); setGridFramePose(gridFramePose); scs.addStaticLinkGraphics(ReachabilityMapTools.createReachibilityColorScale()); scs.addYoGraphic(gridFrameViz); scs.addYoGraphic(currentEvaluationPose); scs.addYoGraphic(currentEvaluationPosition); scs.addYoVariableRegistry(registry); }
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
private void setupSupportViz() { SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); supportPolygons = new SideDependentList<YoFrameConvexPolygon2D>(); supportPolygons.set(RobotSide.LEFT, new YoFrameConvexPolygon2D("FootPolygonLeft", "", worldFrame, 4, registry)); supportPolygons.set(RobotSide.RIGHT, new YoFrameConvexPolygon2D("FootPolygonRight", "", worldFrame, 4, registry)); footContactsInAnkleFrame = new SideDependentList<>(); footContactsInAnkleFrame.set(RobotSide.LEFT, null); footContactsInAnkleFrame.set(RobotSide.RIGHT, null); yoGraphicsListRegistry.registerArtifact("SupportLeft", new YoArtifactPolygon("SupportLeft", supportPolygons.get(RobotSide.LEFT), Color.BLACK, false)); yoGraphicsListRegistry.registerArtifact("SupportRight", new YoArtifactPolygon("SupportRight", supportPolygons.get(RobotSide.RIGHT), Color.BLACK, false)); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); drcSimulationTestHelper.addRobotControllerOnControllerThread(new VizUpdater()); }
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
private void setupSupportViz() { SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); supportPolygons = new SideDependentList<YoFrameConvexPolygon2D>(); supportPolygons.set(RobotSide.LEFT, new YoFrameConvexPolygon2D("FootPolygonLeft", "", worldFrame, 4, registry)); supportPolygons.set(RobotSide.RIGHT, new YoFrameConvexPolygon2D("FootPolygonRight", "", worldFrame, 4, registry)); footContactsInAnkleFrame = new SideDependentList<ArrayList<Point2D>>(); footContactsInAnkleFrame.set(RobotSide.LEFT, null); footContactsInAnkleFrame.set(RobotSide.RIGHT, null); yoGraphicsListRegistry.registerArtifact("SupportLeft", new YoArtifactPolygon("SupportLeft", supportPolygons.get(RobotSide.LEFT), Color.BLACK, false)); yoGraphicsListRegistry.registerArtifact("SupportRight", new YoArtifactPolygon("SupportRight", supportPolygons.get(RobotSide.RIGHT), Color.BLACK, false)); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); drcSimulationTestHelper.addRobotControllerOnControllerThread(new VizUpdater()); }
public ReachabilitySphereMapCalculator(OneDoFJoint[] robotArmJoints, SimulationConstructionSet scs) { this.robotArmJoints = robotArmJoints; this.scs = scs; lastJoint = robotArmJoints[robotArmJoints.length - 1]; jacobian = new GeometricJacobian(robotArmJoints, lastJoint.getSuccessor().getBodyFixedFrame()); int maxIterations = 500; spatialInverseKinematicsCalculator = createNumericalInverseKinematicsCalculator(jacobian, maxIterations, true); linearInverseKinematicsCalculator = createNumericalInverseKinematicsCalculator(jacobian, maxIterations, false); ReferenceFrame frameBeforeRootJoint = robotArmJoints[0].getFrameBeforeJoint(); RigidBodyTransform gridTransformToParent = new RigidBodyTransform(new AxisAngle4d(), new Vector3d(gridSizeInNumberOfVoxels * voxelSize / 3.0, 0.0, 0.0)); ReferenceFrame gridFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("gridFrame", frameBeforeRootJoint, gridTransformToParent); Graphics3DObject gridFrameViz = new Graphics3DObject(); gridFrameViz.transform(gridFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame())); gridFrameViz.addCoordinateSystem(1.0, YoAppearance.Blue()); scs.addStaticLinkGraphics(gridFrameViz); sphereVoxelShape = new SphereVoxelShape(gridFrame, voxelSize, numberOfRays, numberOfRotationsAroundRay, SphereVoxelType.graspOrigin); voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, gridSizeInNumberOfVoxels, voxelSize); scs.addYoVariableRegistry(registry); }
public PolygonSnapperVisualizer(ConvexPolygon2D snappingPolygonShape) { Robot robot = new Robot("Robot"); scs = new SimulationConstructionSet(robot); scs.setDT(0.1, 1); polygonToSnap = new YoFrameConvexPolygon2D("polygonToSnap", ReferenceFrame.getWorldFrame(), 4, registry); snappedPolygon = new YoFrameConvexPolygon2D("snappedPolygon", ReferenceFrame.getWorldFrame(), 4, registry); polygonToSnap.set(snappingPolygonShape); snappedPolygon.set(snappingPolygonShape); polygonToSnapPose = new YoFramePoseUsingYawPitchRoll("polygonToSnapPose", ReferenceFrame.getWorldFrame(), registry); snappedPolygonPose = new YoFramePoseUsingYawPitchRoll("snappedPolygonPose", ReferenceFrame.getWorldFrame(), registry); polygonToSnapPose.setToNaN(); snappedPolygonPose.setToNaN(); polygonToSnapViz = new YoGraphicPolygon("polygonToSnapViz", polygonToSnap, polygonToSnapPose, 1.0, YoAppearance.Green()); snappedPolygonViz = new YoGraphicPolygon("snappedPolygonViz", polygonToSnap, snappedPolygonPose, 1.0, YoAppearance.Red()); polygonToSnapViz.update(); snappedPolygonViz.update(); scs.addYoGraphic(polygonToSnapViz); scs.addYoGraphic(snappedPolygonViz); scs.addYoVariableRegistry(registry); scs.setGroundVisible(false); scs.startOnAThread(); }