private void createHumanoidFloatingRootJointRobot() { humanoidFloatingRootJointRobot = robotModel.get().createHumanoidFloatingRootJointRobot(createCollisionMeshes.get()); }
private void createHumanoidFloatingRootJointRobot() { humanoidFloatingRootJointRobot = robotModel.get().createHumanoidFloatingRootJointRobot(createCollisionMeshes.get()); }
@ContinuousIntegrationTest(estimatedDuration = 4.6) @Test(timeout = 30000) public void testLoadingAndPlayingAnotherSequence() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); PlaybackPoseSequence sequence = new PlaybackPoseSequence(fullRobotModel); PlaybackPoseSequenceReader.appendFromFile(sequence, getClass().getClassLoader().getResourceAsStream("tenPoses.poseSequence")); System.out.println(sequence); PoseInterpolatorPlaybacker.playASequence(sdfRobot, sequence, SHOW_GUI, new PoseCheckerCallback() { @Override public void checkPose(PlaybackPose pose, PlaybackPose previousPose) { assertSmallPoseDifference(pose, previousPose); } }); }
public DRCInverseDynamicsCalculatorTestHelper createInverseDynamicsCalculatorTestHelper(boolean visualize, double gravityZ) { DRCRobotModel drcRobotModel = getRobotModel(); FullHumanoidRobotModel fullRobotModel = drcRobotModel.createFullRobotModel(); boolean createCollisionMeshes = false; boolean enableJointDamping = false; HumanoidFloatingRootJointRobot robot = drcRobotModel.createHumanoidFloatingRootJointRobot(createCollisionMeshes, enableJointDamping); robot.setGravity(gravityZ); return new DRCInverseDynamicsCalculatorTestHelper(fullRobotModel, robot, visualize, gravityZ); } }
@ContinuousIntegrationTest(estimatedDuration = 3.8) @Test(timeout = 30000) public void testLoadingAndPlayingASequence() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); PlaybackPoseSequence sequence = new PlaybackPoseSequence(fullRobotModel); PlaybackPoseSequenceReader.appendFromFile(sequence, getClass().getClassLoader().getResourceAsStream("testSequence2.poseSequence")); PoseInterpolatorPlaybacker.playASequence(sdfRobot, sequence, SHOW_GUI, new PoseCheckerCallback() { @Override public void checkPose(PlaybackPose pose, PlaybackPose previousPose) { assertSmallPoseDifference(pose, previousPose); } }); }
public static void main(String[] args) { final SimulationConstructionSet scs; // initialize SCS DRCRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.REAL_ROBOT, false); final FloatingRootJointRobot robot =robotModel.createHumanoidFloatingRootJointRobot(false); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(); parameters.setDataBufferSize(65536); scs = new SimulationConstructionSet(robot, parameters); // add sysid button JButton button = new JButton("findCOM"); button.addActionListener(new LinkComIDActionListener(scs.getDataBuffer(), robot)); scs.addButton(button); scs.startOnAThread(); } }
@ContinuousIntegrationTest(estimatedDuration = 1.2) @Test(timeout = 30000) public void testRandomExample() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); int numberOfPoses = 5; double delay = 0.3; double trajectoryTime = 1.0; Random random = new Random(1776L); PlaybackPoseSequence sequence = PosePlaybackExampleSequence.createRandomPlaybackPoseSequence(random, fullRobotModel, numberOfPoses, delay, trajectoryTime); //sequence.writeToOutputStream(fullRobotModel, System.out); PoseInterpolatorPlaybacker.playASequence(sdfRobot, sequence, SHOW_GUI, new PoseCheckerCallback() { @Override public void checkPose(PlaybackPose pose, PlaybackPose previousPose) { assertSmallPoseDifference(pose, previousPose); } }); }
public static void main(String[] args) throws IOException { DRCRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.SCS, false); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); FullHumanoidRobotModel fullRobotModelForSlider = robotModel.createFullRobotModel(); new PosePlaybackSCSBridge(sdfRobot, fullRobotModel, fullRobotModelForSlider, robotModel.getControllerDT()); } }
@ContinuousIntegrationTest(estimatedDuration = 4.0) @Test(timeout = 30000) public void testMoveElbowExample() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); double delay = 0.3; double trajectoryTime = 1.0; PlaybackPoseSequence sequence = PosePlaybackExampleSequence.createExamplePoseSequenceMoveArm(fullRobotModel, delay, trajectoryTime); PoseInterpolatorPlaybacker.playASequence(sdfRobot, sequence, SHOW_GUI, new PoseCheckerCallback() { @Override public void checkPose(PlaybackPose pose, PlaybackPose previousPose) { assertSmallPoseDifference(pose, previousPose); } }); }
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(); }
private FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
private FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
protected FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
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(); }
mainRegistry); robot = robotModel.createHumanoidFloatingRootJointRobot(false); toolboxUpdater = createToolboxUpdater(); robot.setController(toolboxUpdater); robotDescription.setName("Ghost"); KinematicsToolboxControllerTest.recursivelyModifyGraphics(robotDescription.getChildrenJoints().get(0), ghostApperance); ghost = ghostRobotModel.createHumanoidFloatingRootJointRobot(false); ghost.setDynamic(false); ghost.setGravity(0);
public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive) { simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false); DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName()); if (robotInitialSetup == null) robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap()); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); DoubleYoVariable yoTime = simulatedRobot.getYoTime(); double dt = robotModel.getEstimatorDT(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters(); if (diagnosticParameters == null) diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false); DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters); SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration); DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt, sensorProcessingConfiguration, simulationRegistry); automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream, simulationRegistry); automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive); automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController); outputWriter = new DRCSimulationOutputWriter(simulatedRobot); outputWriter.setFullRobotModel(fullRobotModel, null); int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT()); simulatedRobot.setController(this, simulationTicksPerControlTick); return automatedDiagnosticConfiguration; }
mainRegistry, yoGraphicsListRegistry, visualize); robot = robotModel.createHumanoidFloatingRootJointRobot(false); toolboxUpdater = createToolboxUpdater(); robot.setController(toolboxUpdater); robotDescription.setName("Ghost"); KinematicsToolboxControllerTest.recursivelyModifyGraphics(robotDescription.getChildrenJoints().get(0), ghostApperance); ghost = ghostRobotModel.createHumanoidFloatingRootJointRobot(false); ghost.setDynamic(false); ghost.setGravity(0);
public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive) { simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false); DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName()); if (robotInitialSetup == null) robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap()); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); YoDouble yoTime = simulatedRobot.getYoTime(); double dt = robotModel.getEstimatorDT(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters(); if (diagnosticParameters == null) diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false); JointDesiredOutputList lowLevelOutput = new JointDesiredOutputList(fullRobotModel.getOneDoFJoints()); DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters, lowLevelOutput); SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration); DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, lowLevelOutput, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt, sensorProcessingConfiguration, simulationRegistry); automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream, simulationRegistry); automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive); automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController); lowLevelOutputWriter = new SimulatedLowLevelOutputWriter(simulatedRobot, false); lowLevelOutputWriter.setJointDesiredOutputList(lowLevelOutput); int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT()); simulatedRobot.setController(this, simulationTicksPerControlTick); return automatedDiagnosticConfiguration; }