@Override public void addController(MultiThreadedRobotControlElement controller, int executionsPerControlTick, boolean skipFirstControlCycle) { controllers.add(new MultiThreadedRobotControllerExecutor(simulatedRobot, controller, executionsPerControlTick, skipFirstControlCycle, registry)); if(controller.getYoGraphicsListRegistry() != null) { scs.addYoGraphicsListRegistry(controller.getYoGraphicsListRegistry(), false); } }
@Override public void addController(MultiThreadedRobotControlElement controller, int executionsPerControlTick, boolean skipFirstControlCycle) { controllers.add(new SingleThreadedRobotControllerExecutor(simulatedRobot, controller, executionsPerControlTick, skipFirstControlCycle, registry)); if(scs != null && controller.getYoGraphicsListRegistry() != null) { scs.addYoGraphicsListRegistry(controller.getYoGraphicsListRegistry(), true); } }
public final void setLogDataProcessor(LogDataProcessorFunction logDataProcessor) { YoGraphicsListRegistry yoGraphicsListRegistry = logDataProcessor.getYoGraphicsListRegistry(); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.addArtifactListsToPlotter(plotter); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } logDataProcessorWrapper.addLogDataProcessor(logDataProcessor); }
public final void setLogDataProcessor(LogDataProcessorFunction logDataProcessor) { YoGraphicsListRegistry yoGraphicsListRegistry = logDataProcessor.getYoGraphicsListRegistry(); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.addArtifactListsToPlotter(plotter); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } logDataProcessorWrapper.addLogDataProcessor(logDataProcessor); }
public final void setLogDataProcessor(LogDataProcessorFunction logDataProcessor) { YoGraphicsListRegistry yoGraphicsListRegistry = logDataProcessor.getYoGraphicsListRegistry(); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.addArtifactListsToPlotter(plotter); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } logDataProcessorWrapper.addLogDataProcessor(logDataProcessor); }
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(); }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED , 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
public ActualCMPComputer(boolean createViz, SimulationConstructionSet scs, HumanoidFloatingRootJointRobot simulatedRobot) { this.simulatedRobot = simulatedRobot; simulateDT = scs.getDT(); gravity = simulatedRobot.getGravityZ(); momentumChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector("rateOfChangeLinearMomentum", "", alpha, simulateDT, registry, yoLinearMomentum); if (createViz) { yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoArtifactPosition cmpViz = new YoArtifactPosition("SimulationCMP", yoCmp.getYoX(), yoCmp.getYoY(), GraphicType.BALL_WITH_CROSS, Color.RED , 0.005); cmpViz.setVisible(visibleByDefault); yoGraphicsListRegistry.registerArtifact(getClass().getSimpleName(), cmpViz); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); } else { yoGraphicsListRegistry = null; } }
public static void main(String[] args) { YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); DRCDemoEnvironmentSimpleSteeringWheelContact env = new DRCDemoEnvironmentSimpleSteeringWheelContact(yoGraphicsListRegistry); List<Robot> robots = env.getEnvironmentRobots(); Robot[] robotArray = new Robot[robots.size()]; robots.toArray(robotArray); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(); parameters.setDataBufferSize(36000); SimulationConstructionSet scs = new SimulationConstructionSet(robotArray, parameters); scs.setDT(1e-4, 20); TerrainObject3D terrainObject = env.getTerrainObject3D(); scs.addStaticLinkGraphics(terrainObject.getLinkGraphics()); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); scs.setGroundVisible(false); scs.startOnAThread(); }
public static void visualizeFootsteps(Robot nullRobot, List<Footstep> footsteps, List<ContactablePlaneBody> contactablePlaneBodies) { SimulationConstructionSet scs = new SimulationConstructionSet(nullRobot); scs.setDT(0.25, 1); YoVariableRegistry rootRegistry = scs.getRootRegistry(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); int maxNumberOfContacts = 2; int maxPointsPerContact = 4; FootstepGeneratorVisualizer footstepGeneratorVisualizer = new FootstepGeneratorVisualizer(maxNumberOfContacts, maxPointsPerContact, rootRegistry, yoGraphicsListRegistry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); footstepGeneratorVisualizer.addFootstepsAndTickAndUpdate(scs, footsteps, contactablePlaneBodies); scs.startOnAThread(); deleteFirstDataPointAndCropData(scs); }
public static void main(String argv[]) throws FileNotFoundException, JAXBException, MalformedURLException { Robot robot = new Robot("robot"); SimulationConstructionSet scs = new SimulationConstructionSet(robot); DRCVehicleSDFLoader drcVehicleSDFLoader = new DRCVehicleSDFLoader(); scs.addStaticLinkGraphics(drcVehicleSDFLoader.loadDRCVehicle(false)); RigidBodyTransform vehicleToWorldTransform = new RigidBodyTransform(); ReferenceFrame vehicleFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("vehicle", ReferenceFrame.getWorldFrame(), vehicleToWorldTransform, false, true, true); VehicleModelObjects vehicleModelObjects = new VehicleModelObjects(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoVariableRegistry registry = scs.getRootRegistry(); VehicleModelObjectVisualizer vehicleModelObjectVisualizer = new VehicleModelObjectVisualizer(vehicleFrame, vehicleModelObjects, yoGraphicsListRegistry, registry); vehicleModelObjectVisualizer.setVisible(true); vehicleModelObjectVisualizer.update(); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); Thread thread = new Thread(scs); thread.start(); }
public static void main(String argv[]) throws FileNotFoundException, JAXBException, MalformedURLException { Robot robot = new Robot("robot"); SimulationConstructionSet scs = new SimulationConstructionSet(robot); DRCVehicleSDFLoader drcVehicleSDFLoader = new DRCVehicleSDFLoader(); scs.addStaticLinkGraphics(drcVehicleSDFLoader.loadDRCVehicle(false)); RigidBodyTransform vehicleToWorldTransform = new RigidBodyTransform(); ReferenceFrame vehicleFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("vehicle", ReferenceFrame.getWorldFrame(), vehicleToWorldTransform); VehicleModelObjects vehicleModelObjects = new VehicleModelObjects(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoVariableRegistry registry = scs.getRootRegistry(); VehicleModelObjectVisualizer vehicleModelObjectVisualizer = new VehicleModelObjectVisualizer(vehicleFrame, vehicleModelObjects, yoGraphicsListRegistry, registry); vehicleModelObjectVisualizer.setVisible(true); vehicleModelObjectVisualizer.update(); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); Thread thread = new Thread(scs); thread.start(); }
public static void main(String argv[]) throws FileNotFoundException, JAXBException, MalformedURLException { Robot robot = new Robot("robot"); SimulationConstructionSet scs = new SimulationConstructionSet(robot); DRCVehicleSDFLoader drcVehicleSDFLoader = new DRCVehicleSDFLoader(); scs.addStaticLinkGraphics(drcVehicleSDFLoader.loadDRCVehicle(false)); RigidBodyTransform vehicleToWorldTransform = new RigidBodyTransform(); ReferenceFrame vehicleFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("vehicle", ReferenceFrame.getWorldFrame(), vehicleToWorldTransform, false, true, true); VehicleModelObjects vehicleModelObjects = new VehicleModelObjects(); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); YoVariableRegistry registry = scs.getRootRegistry(); VehicleModelObjectVisualizer vehicleModelObjectVisualizer = new VehicleModelObjectVisualizer(vehicleFrame, vehicleModelObjects, yoGraphicsListRegistry, registry); vehicleModelObjectVisualizer.setVisible(true); vehicleModelObjectVisualizer.update(); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); Thread thread = new Thread(scs); thread.start(); }
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 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<>(); 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()); }
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()); }